OrientationOptimizer.cpp

Go to the documentation of this file.

#include "OrientationOptimizer.h"
 
#include <algorithm>
#include <cmath>
 
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <SimoxUtility/math/periodic/periodic_diff.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Random.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/components/units/RobotUnit/BasicControllers.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>
#include <ceres/jet.h>
#include <ceres/rotation.h>
#include <ceres/sized_cost_function.h>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/drop_last.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip.hpp>
 
namespace armarx::navigation::global_planning::optimization
{
    OrientationOptimizer::OrientationOptimizer(const core::GlobalTrajectory& trajectory,
                                               const Params& params) :
        trajectory(trajectory), params(params)
    {
    }
 
    OrientationOptimizer::OptimizationResult
    OrientationOptimizer::optimize()
    {
        namespace r = ::ranges;
        namespace rv = ::ranges::views;
 
        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();
        const double signedAngleDiff =
            std::fmod<double>(goalOrientation - startOrientation + 3 * M_PI, 2 * M_PI) - M_PI;
        const double signedAngleDiffDesired = [&]() -> double
        {
            if (params.predefinedRotationDirection)
            {
                switch (params.predefinedRotationDirection.value())
                {
                    case RotationDirection::Clockwise:
                        if (signedAngleDiff <= 0)
                        {
                            return signedAngleDiff;
                        }
                        return -2 * M_PI + signedAngleDiff;
 
                    case RotationDirection::CounterClockwise:
                        if (signedAngleDiff >= 0)
                        {
                            return signedAngleDiff;
                        }
                        return 2 * M_PI + signedAngleDiff;
                }
            }
            return signedAngleDiff;
        }();
        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;
 
        /* //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.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;
                }();
                orientations.at(i) = std::atan2(vCombined.y(), vCombined.x());
            }
 
            /* //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";
 
 
            //--------------------------------------------------------------------------------------
            // Start of 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_VERBOSE << "prior start enabled";
                const auto connectedPointsInRangeStart =
                    trajectory.allConnectedPointsInRange(0, params.startGoalDistanceThreshold);
 
                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);
 
                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));
            }
 
            // End of Optimization Criteria
            //--------------------------------------------------------------------------------------
 
            // Run the solver!
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_QR;
            options.minimizer_progress_to_stdout = false;
            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);
 
            ARMARX_VERBOSE << summary.FullReport() << "\n";
 
            const auto clampInPlace = [](auto& val)
            { val = simox::math::periodic_clamp(val, -M_PI, M_PI); };
 
            std::for_each(orientations.begin(), orientations.end(), clampInPlace);
 
            if (not summary.IsSolutionUsable())
            {
                ARMARX_ERROR << "Orientation optimization failed!";
                // TODO write to file
            }
        }
 
        const auto applyOrientation = [](const auto& p) -> core::GlobalTrajectoryPoint
        {
            core::GlobalTrajectoryPoint tp = p.first;
            const float yaw = p.second;
 
            tp.waypoint.pose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, yaw);
 
            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