OrientationOptimizer.cpp

Go to the documentation of this file.

#include "OrientationOptimizer.h"
 
#include <math.h>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <vector>
 
#include <range/v3/algorithm/for_each.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip.hpp>
 
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <VirtualRobot/Random.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/global_planning/optimization/cost_functions.h>
 
#include <ceres/ceres.h>
#include <ceres/cost_function.h>
 
namespace armarx::navigation::global_planning::optimization
{
    OrientationOptimizer::OrientationOptimizer(const core::GlobalTrajectory& trajectory,
                                               const Params& params) :
        trajectory(trajectory), params(params)
    {
    }
 
    inline double
    angleDiff(const double a, const double b)
    {
        double d = a - b;
 
        // Wrap into (−π, π]
        while (d <= -M_PI)
        {
            d += 2 * M_PI;
        }
        while (d > M_PI)
        {
            d -= 2 * M_PI;
        }
 
        return d;
    }
 
    OrientationOptimizer::OptimizationResult
    OrientationOptimizer::optimize()
    {
        namespace r = ::ranges;
        namespace rv = ::ranges::views;
 
        ARMARX_IMPORTANT << VAROUT(params.startGoalDistanceThreshold);
 
        const auto toYaw = [](const core::GlobalTrajectoryPoint& pt) -> double
        { return simox::math::mat4f_to_xyyaw(pt.waypoint.pose.matrix()).z(); };
 
        //orientations vector contains variable values that are changed during optimization. these
        //   values are changed to fit the given optimization criteria as good as possible
        std::vector<double> orientations =
            trajectory.points() | ranges::views::transform(toYaw) | ranges::to_vector;
 
        //the optimization tries to stay close to the movement direction, thus the original
        //   orientation is copied (it is expected that original orientation values are oriented
        //   so that they face along the trajectory)
        const std::vector<double> inMovementDir = orientations;
 
        //calculate angle increment necessary for initial equidistant scattering of orientations
        const double startOrientation = orientations.front();
        const double goalOrientation = orientations.back();
 
        ARMARX_DEBUG << VAROUT(startOrientation);
        ARMARX_DEBUG << VAROUT(goalOrientation);
 
        const std::size_t nOrientationStartGoalInfluence = std::ceil(
            (orientations.size() - 1) * params.startGoalDistanceThreshold / trajectory.length());
        ARMARX_DEBUG << VAROUT(nOrientationStartGoalInfluence);
 
        const double signedAngleDiff = angleDiff(goalOrientation, startOrientation);
 
        ARMARX_DEBUG << VAROUT(signedAngleDiff);
 
        const double signedAngleDiffDesired = [&]() -> double
        {
            switch (params.predefinedRotationDirection)
            {
                case RotationDirection::Clockwise:
                    ARMARX_VERBOSE << "Clockwise";
                    if (signedAngleDiff <= 0)
                    {
                        return signedAngleDiff;
                    }
                    return -2 * M_PI + signedAngleDiff;
 
                case RotationDirection::CounterClockwise:
                    ARMARX_VERBOSE << "CounterClockwise";
                    if (signedAngleDiff >= 0)
                    {
                        return signedAngleDiff;
                    }
                    return 2 * M_PI + signedAngleDiff;
                default:
                    ARMARX_VERBOSE << "Unspecified";
                    return signedAngleDiff;
            }
        }();
 
 
        if (orientations.size() < 2 * nOrientationStartGoalInfluence)
        {
            // we just interpolate between start and end
 
            const double angleIncrement = signedAngleDiffDesired / (orientations.size() - 1);
 
            //equidistant orientations are used as starting value for the optimization of the
            //   orientations in the first iteration
            const std::vector<double> equidistantOrientations = [&]
            {
                std::vector<double> vec{orientations.front()};
                for (std::size_t i = 1; i < orientations.size() - 1; i++)
                {
                    vec.push_back(startOrientation + angleIncrement * i);
                }
                vec.push_back(orientations.back());
                return vec;
            }();
            orientations = equidistantOrientations;
        }
        else
        {
            // here, we ignore the rotation direction
 
            // we obtain the first part in which we interpolate between the start and the inMovementDir(nOrientationStartGoalInfluence)
            {
                const double signedAngleDiff = angleDiff(
                    inMovementDir.at(nOrientationStartGoalInfluence - 1), startOrientation);
 
                const double angleIncrement = signedAngleDiff / (nOrientationStartGoalInfluence);
 
                for (std::size_t i = 1; i < nOrientationStartGoalInfluence; i++)
                {
                    orientations.at(i) = startOrientation + angleIncrement * i;
                }
            }
 
            // the intermediate points are initialized with inMovementDir
            {
                for (std::size_t i = nOrientationStartGoalInfluence;
                     i < orientations.size() - nOrientationStartGoalInfluence;
                     i++)
                {
                    orientations.at(i) = inMovementDir.at(i);
                }
            }
 
            // we obtain the last part in which we interpolate between the end and the inMovementDir(n-nOrientationStartGoalInfluence)
            {
                const double signedAngleDiff = angleDiff(
                    goalOrientation,
                    inMovementDir.at(orientations.size() - nOrientationStartGoalInfluence));
 
                const double angleIncrement = signedAngleDiff / (nOrientationStartGoalInfluence);
 
                for (std::size_t i = 1; i < nOrientationStartGoalInfluence; i++)
                {
                    orientations.at(orientations.size() - nOrientationStartGoalInfluence + i) =
                        goalOrientation - angleIncrement * (nOrientationStartGoalInfluence - i);
                }
            }
        }
 
 
        ARMARX_DEBUG << "Equidistant";
        for (const auto& ori : orientations)
        {
            ARMARX_DEBUG << ori;
        }
 
        /* //COMMENT IN FOR TEST CASE PLOTTING
        std::vector<double> initial = orientations;
        std::vector<double> prior = inMovementDir; */
 
        const float movementDirWeightStep =
            (params.movementDirWeightEnd - params.movementDirWeightStart) / (params.iterations - 1);
 
        //iteratively optimize the orientations with increasing movementDirWeight, thus at first ignoring
        //   the criteria to look in the movement direction and slowly considering this more and more
        //   with each iteration
        // for (int it = 0; it < params.iterations; it++)
        {
            const float movementDirWeight = params.movementDirWeightEnd;
            // params.iterations == 1 ? params.movementDirWeightEnd
            //    : params.movementDirWeightStart + it * movementDirWeightStep;
 
            //interpolate orientation in walking direction and current orientation at walkingDirAlpha.
            //   interpolated values are used as starting values for next iteration
            for (std::size_t i = 1; i < orientations.size() - 1; i++)
            {
                const float walkingDirAlpha = 0.2;
 
                const Eigen::Vector2f vMovementDir =
                    walkingDirAlpha *
                    (Eigen::Rotation2Df(inMovementDir.at(i)) * Eigen::Vector2f::UnitX());
 
                const Eigen::Vector2f vStartGoal =
                    (1.0 - walkingDirAlpha) *
                    (Eigen::Rotation2Df(orientations.at(i)) * Eigen::Vector2f::UnitX());
 
                const Eigen::Vector2f vCombined = [&]() -> Eigen::Vector2f
                {
                    if (movementDirWeight > 0) // should rotate into movement direction?
                    {
                        return vMovementDir + vStartGoal;
                    }
 
                    return vStartGoal;
                }();
 
                // FIXME reconsider this
                // orientations.at(i) = std::atan2(vCombined.y(), vCombined.x());
 
                orientations.at(i) += VirtualRobot::RandomFloat(-0.05F, 0.05F);
            }
 
            /* //COMMENT IN FOR TEST CASE PLOTTING
            initial = orientations;
            prior = inMovementDir; */
 
            const std::size_t nPoints = orientations.size();
 
            ceres::Problem problem;
 
            ARMARX_VERBOSE << orientations.size() - 2 << " orientations to optimize";
 
 
            // Define Optimization Criteria
            {
                // TODO https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc
                //     ceres::LocalParameterization* angle_local_parameterization =
                //   ceres::AngleLocalParameterization::Create();
 
                // keeps the new orientations somewhat close to the orientation in movement direction
                if (movementDirWeight > 0.F)
                {
                    ARMARX_VERBOSE << "Optimize: movement dir enabled.";
                    for (size_t i = 1; i < (orientations.size() - 1); i++)
                    {
                        float actualMovementDirWeight = movementDirWeight;
                        //the first and last four points should not consider it too much to look in the
                        //  direction of the trajectory as the robot should not move too much when
                        //  close to start or goal
                        if (i < 5)
                        {
                            actualMovementDirWeight *= i / 5.;
                        }
                        else if ((orientations.size() - i - 1) < 5)
                        {
                            actualMovementDirWeight *= (orientations.size() - i - 1) / 5.;
                        }
 
                        ceres::CostFunction* movementDirCostFunction =
                            new OrientationPriorCostFunctor(inMovementDir.at(i),
                                                            actualMovementDirWeight);
 
                        ARMARX_DEBUG << "Adding OrientationPriorCostFunctor to optimize " << i;
                        problem.AddResidualBlock(
                            movementDirCostFunction, nullptr, &orientations.at(i));
                    }
                }
 
                // smooth waypoint orientation, no start and goal nodes involved!
                if (params.smoothnessWeight > 0.F)
                {
                    ARMARX_VERBOSE << "Enabled SmoothOrientationCost";
                    for (size_t i = 2; i < (orientations.size() - 2); i++)
                    {
                        ceres::CostFunction* smoothCostFunction =
                            new SmoothOrientationCostFunctor(params.smoothnessWeight);
 
                        ARMARX_DEBUG << "Addding SmoothOrientationCostFunctor to optimize " << i - 1
                                     << ", " << i << " and " << i + 1;
                        problem.AddResidualBlock(smoothCostFunction,
                                                 nullptr,
                                                 &orientations.at(i - 1),
                                                 &orientations.at(i),
                                                 &orientations.at(i + 1));
                    }
                }
 
                // within a certain range close to the start, the robot shouldn't change its orientation
                if (params.priorStartWeight > 0.F)
                {
                    ARMARX_DEBUG << "prior start enabled";
                    const auto connectedPointsInRangeStart =
                        trajectory.allConnectedPointsInRange(0, params.startGoalDistanceThreshold);
 
                    ARMARX_DEBUG << VAROUT(connectedPointsInRangeStart.size());
 
                    for (const size_t i : connectedPointsInRangeStart)
                    {
                        // skip the points that belong to the second half of the trajectory
                        if (i >= orientations.size() / 2)
                        {
                            continue;
                        }
 
                        ceres::CostFunction* priorCostFunction = new OrientationPriorCostFunctor(
                            inMovementDir.front(), params.priorStartWeight);
 
                        ARMARX_DEBUG << "Addding OrientationPriorCostFunctor(start) to optimize "
                                     << i;
                        problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
                    }
                }
 
                // within a certain range close to the end, the robot shouldn't change its orientation
                if (params.priorEndWeight > 0.F)
                {
                    ARMARX_VERBOSE << "prior end enabled";
 
                    const auto connectedPointsInRangeGoal = trajectory.allConnectedPointsInRange(
                        trajectory.poses().size() - 1, params.startGoalDistanceThreshold);
 
                    ARMARX_DEBUG << VAROUT(connectedPointsInRangeGoal.size());
 
                    for (const size_t i : connectedPointsInRangeGoal)
                    {
                        // skip the points that belong to the first half of the trajectory
                        if (i < orientations.size() / 2)
                        {
                            continue;
                        }
 
                        ceres::CostFunction* priorCostFunction = new OrientationPriorCostFunctor(
                            inMovementDir.back(), params.priorEndWeight);
 
                        ARMARX_DEBUG << "Addding OrientationPriorCostFunctor to optimize " << i;
                        problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
                    }
                }
 
 
                // smooth waypoint orientation, involving start
                if (params.smoothnessWeightStartGoal > 0.F and nPoints > 3)
                {
                    ARMARX_VERBOSE << "Enabled SmoothOrientationFixedPreCost";
 
                    ceres::CostFunction* smoothCostFunction =
                        new SmoothOrientationFixedPreCostFunctor(orientations.front(),
                                                                 params.smoothnessWeightStartGoal);
 
                    ARMARX_DEBUG
                        << "Addding SmoothOrientationFixedPreCostFunctor to optimize 1 and 2";
                    problem.AddResidualBlock(
                        smoothCostFunction, nullptr, &orientations.at(1), &orientations.at(2));
                }
 
                // smooth waypoint orientation, involving goal
                if (params.smoothnessWeightStartGoal > 0.F and nPoints > 3)
                {
                    ARMARX_VERBOSE << "Enabled SmoothOrientationFixedNextCost";
 
                    ceres::CostFunction* smoothCostFunction =
                        new SmoothOrientationFixedNextCostFunctor(orientations.back(),
                                                                  params.smoothnessWeightStartGoal);
 
                    ARMARX_DEBUG << "Addding SmoothOrientationFixedPreCostFunctor to optimize "
                                 << orientations.size() - 3 << " and " << orientations.size() - 2;
                    problem.AddResidualBlock(smoothCostFunction,
                                             nullptr,
                                             &orientations.at(orientations.size() - 3),
                                             &orientations.at(orientations.size() - 2));
                }
            }
 
 
            // Run the solver!
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_QR;
            options.minimizer_progress_to_stdout = true;
            options.max_num_iterations = 100;
            options.function_tolerance = 0.01;
            // options.check_gradients = false;
            // options.trust_region_minimizer_iterations_to_dump = {0, 1, 2, 5, 10, 20};
            // options.max_trust_region_radius = 0.05;
            // options.initial_trust_region_radius = 0.01;
 
            ceres::Solver::Summary summary;
            Solve(options, &problem, &summary);
 
            // orientations = inMovementDir;
 
            ARMARX_VERBOSE << summary.FullReport() << "\n";
 
            if (not summary.IsSolutionUsable())
            {
                ARMARX_ERROR << "Orientation optimization failed!";
                // TODO write to file
            }
        }
 
        // no matter what, we have to ensure that the start and goal are still as they were
        // FIXME ARMARX_CHECK
 
        // temporary fix: overwrite the values
        orientations.front() = startOrientation;
        orientations.back() = goalOrientation;
 
        ARMARX_DEBUG << "Final";
        for (const auto& ori : orientations)
        {
            ARMARX_DEBUG << ori;
        }
 
        const auto applyOrientation = [](const auto& p) -> core::GlobalTrajectoryPoint
        {
            core::GlobalTrajectoryPoint tp = p.first;
            const float yaw = p.second;
 
            tp.waypoint.pose.linear() =
                Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
            return tp;
        };
 
        // TODO(fabian.reister): could also be in-place
        const auto modifiedTrajectory = rv::zip(trajectory.points(), orientations) |
                                        rv::transform(applyOrientation) | r::to_vector;
 
        return OrientationOptimizer::OptimizationResult{
            .trajectory = modifiedTrajectory,
 
            /* //COMMENT IN FOR TEST CASE PLOTTING
                                                        .initial = initial,
                                                        .prior = prior */
 
        };
    }
 
 
} // namespace armarx::navigation::global_planning::optimization