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;
    }
 
    namespace
    {
        /**
         * @brief Passes of the smoothing filter below.
         *
         * A binomial filter is variation-diminishing, so each pass can only remove local extrema
         * from `dyaw/ds`, never add them. What matters is that derivative, not the heading
         * itself: the achievable speed is `maxVel.angular / max|dyaw/ds|`. Measured on
         * `wide_passage_diagonal`, whose path tangent has 3 extrema of its own:
         *
         *     passes   dyaw/ds extrema   speed cap at 0.5 rad/s   max heading shift
         *          0                14           378 mm/s                  --
         *          8                 6           787 mm/s               8.0 deg
         *         32                 3           996 mm/s               9.6 deg
         *         64                 3          1060 mm/s               9.9 deg
         *
         * 32 is where the profile reaches the path's own 3 extrema and the turn rate stops
         * capping the speed below the 1000 mm/s linear limit -- i.e. where the angular and
         * linear limits are matched rather than one throttling the other. Beyond that the gain
         * flattens while the headings keep drifting from the optimized ones.
         */
        constexpr std::size_t orientationSmoothingPasses = 32;
 
        /**
         * @brief Remove the oscillation the optimizer leaves in the heading profile.
         *
         * The solve minimises a sum of pairwise smoothness and prior terms, and stops at a loose
         * `function_tolerance`, which leaves a heading whose *rate* reverses far more often than
         * the path bends: 14 reversals of `dyaw/ds` against the path tangent's 3 on
         * `wide_passage_diagonal`.
         *
         * That is not cosmetic. A `GlobalTrajectory` stores one *linear* speed per waypoint, so
         * the turn rate it implies is `v * dyaw/ds`; wherever that reaches the platform's
         * angular limit, it and not the obstacle-aware limit caps the speed. A few wiggly
         * waypoints therefore slow the whole traverse.
         *
         * Ideally the optimizer would not produce this and the pass would not exist. It does, so
         * this is a post-processing step: a binomial [1/4, 1/2, 1/4] filter on the interior
         * headings with start and goal pinned, applied in the *increment* domain so a wrap
         * across +-pi averages correctly.
         */
        /// Largest |dyaw| per millimetre over an already-unwrapped profile.
        double
        turnRateOf(const std::vector<double>& unwrapped, const std::vector<double>& segmentLengths)
        {
            double worst = 0.0;
 
            for (std::size_t i = 0; i + 1 < unwrapped.size(); i++)
            {
                const double length = segmentLengths.at(i);
 
                if (length > 1e-3)
                {
                    worst =
                        std::max(worst, std::abs(unwrapped.at(i + 1) - unwrapped.at(i)) / length);
                }
            }
 
            return worst;
        }
 
        void
        smoothOrientations(std::vector<double>& orientations,
                           const std::vector<double>& segmentLengths,
                           const double maxTurnRate)
        {
            if (orientations.size() < 3)
            {
                return;
            }
 
            // Accumulate the shortest-path increments, so the values are continuous even where
            // the raw angles wrap.
            std::vector<double> unwrapped(orientations.size());
            unwrapped.front() = orientations.front();
            for (std::size_t i = 1; i < orientations.size(); i++)
            {
                unwrapped.at(i) =
                    unwrapped.at(i - 1) + angleDiff(orientations.at(i), orientations.at(i - 1));
            }
 
            const double start = unwrapped.front();
            const double goal = unwrapped.back();
 
            const auto runPasses = [&unwrapped](const std::size_t passes)
            {
                for (std::size_t pass = 0; pass < passes; pass++)
                {
                    const std::vector<double> previous = unwrapped;
 
                    for (std::size_t i = 1; i + 1 < unwrapped.size(); i++)
                    {
                        unwrapped.at(i) = 0.25 * previous.at(i - 1) + 0.5 * previous.at(i) +
                                          0.25 * previous.at(i + 1);
                    }
                }
            };
 
            runPasses(orientationSmoothingPasses);
 
            // Then keep going until the profile is one the base could actually hold, rather than
            // stopping at a fixed pass count that has no idea how much rotation it is spreading
            // or how much path it has to spread it over.
            //
            // Diffusion with the endpoints pinned converges towards a constant increment per
            // sample, i.e. the rotation spread over the whole path -- so this terminates whenever
            // the path is long enough to absorb the turn at all, and the cap catches the case
            // where it is not. Spreading costs heading accuracy against the movement direction,
            // which is why it stops at the limit instead of smoothing to convergence.
            //
            // Checked in blocks: the rate moves slowly and evaluating it every pass would cost
            // more than the passes.
            constexpr std::size_t passBlock = 64;
            constexpr std::size_t maxAdditionalPasses = 200000;
 
            std::size_t additional = 0;
            while (maxTurnRate > 0.0 and turnRateOf(unwrapped, segmentLengths) > maxTurnRate and
                   additional < maxAdditionalPasses)
            {
                runPasses(passBlock);
                additional += passBlock;
            }
 
            const double achieved = turnRateOf(unwrapped, segmentLengths);
 
            if (maxTurnRate > 0.0 and achieved > maxTurnRate)
            {
                // The path is too short for the rotation it is being asked to carry. Reported
                // rather than hidden: downstream this becomes a speed cap, and on the robot it
                // looked like the controller misbehaving.
                ARMARX_WARNING << "[nav-guard] orientation-unturnable: the heading profile still "
                               << "turns " << achieved << " rad/mm after " << additional
                               << " smoothing passes, against a limit of " << maxTurnRate
                               << ". The path is too short for the rotation it has to carry; the "
                               << "reparametrization will have to slow down for it.";
            }
            else if (additional > 0)
            {
                ARMARX_VERBOSE << "Orientation profile needed " << additional
                               << " extra smoothing passes to reach " << achieved << " rad/mm.";
            }
 
            // The endpoints are commanded, not optimized: they must survive untouched.
            unwrapped.front() = start;
            unwrapped.back() = goal;
 
            orientations = unwrapped;
        }
    } // namespace
 
    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;
 
            // There used to be a +-0.05 rad perturbation of every interior orientation here, to
            // break the symmetry of a perfectly aligned set of waypoints "which the minimizer
            // would otherwise not move off".
            //
            // It was not the minimizer. `periodicDiff` used to be `acos` of a dot product, whose
            // derivative is infinite where the two angles agree, so the analytic jacobian guarded
            // the singularity by returning *zero* there -- suppressing the gradient at exactly
            // the aligned configurations the noise was added to escape. With the `atan2`
            // formulation the derivative is a clean +-1 everywhere and there is nothing to escape.
            //
            // Measured on a straight 3184 mm run, with and without the perturbation: identical
            // results to six decimal places for every goal orientation tried, and the aligned
            // case now converges at iteration 0 with a cost of exactly zero instead of spending
            // three iterations grinding the injected noise back out of a correct answer.
            //
            // Removing it also removes the last source of run-to-run variation: the solve is now
            // deterministic because there is no randomness in it, rather than because a PRNG was
            // pinned to a fixed seed.
 
            //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());
                //
                // With the perturbation gone this loop has no effect: `vCombined` is computed and
                // discarded, and the line above is the only thing that ever used it. Left in
                // place rather than deleted because it is someone's unfinished intent, not my
                // dead code -- but it is dead, and it is the reason `movementDirWeight` and
                // `walkingDirAlpha` are computed for nothing.
                (void)vCombined;
            }
 
            /* //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;
            // Ceres 2.2 already defaults to 1, but planning has to be reproducible, so state it
            // rather than inherit it from whichever Ceres happens to be linked.
            options.num_threads = 1;
            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;
 
        // The smoothing needs the geometry, not just the angles: how far a heading may turn is
        // a question about distance travelled, and a profile that is fine over six metres is
        // unexecutable over one.
        const std::vector<double> segmentLengths = [this]
        {
            std::vector<double> lengths;
            const auto& pts = trajectory.points();
            lengths.reserve(pts.empty() ? 0 : pts.size() - 1);
 
            for (std::size_t i = 0; i + 1 < pts.size(); i++)
            {
                lengths.push_back((pts.at(i + 1).waypoint.pose.translation() -
                                   pts.at(i).waypoint.pose.translation())
                                      .norm());
            }
 
            return lengths;
        }();
 
        smoothOrientations(orientations, segmentLengths, params.maxTurnRate);
 
        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