TrajectoryFollowingController.cpp

Go to the documentation of this file.

#include "TrajectoryFollowingController.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/core/MultiDimPIDController.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/trajectory_control/global/TrajectoryController.h>
#include <armarx/navigation/trajectory_control/global/aron/TrajectoryFollowingControllerParams.aron.generated.h>
#include <armarx/navigation/trajectory_control/global/aron_conversions.h>
#include <armarx/navigation/trajectory_control/global/core.h>
 
namespace armarx::navigation::traj_ctrl::global
{
    // TrajectoryFollowingControllerParams
 
    Algorithms
    TrajectoryFollowingControllerParams::algorithm() const
    {
        return Algorithms::TrajectoryFollowingController;
    }
 
    aron::data::DictPtr
    TrajectoryFollowingControllerParams::toAron() const
    {
        arondto::TrajectoryFollowingControllerParams dto;
 
        TrajectoryFollowingControllerParams bo;
        armarx::navigation::traj_ctrl::global::toAron(dto, bo);
 
        return dto.toAron();
    }
 
    TrajectoryFollowingControllerParams
    TrajectoryFollowingControllerParams::FromAron(const aron::data::DictPtr& dict)
    {
        arondto::TrajectoryFollowingControllerParams dto;
        dto.fromAron(dict);
 
        TrajectoryFollowingControllerParams bo;
        armarx::navigation::traj_ctrl::global::fromAron(dto, bo);
 
        return bo;
    }
 
    // TrajectoryFollowingController
 
    TrajectoryFollowingController::TrajectoryFollowingController(const Params& params) :
        params(params),
        pidPos(params.pidPos.Kp,
               params.pidPos.Ki,
               params.pidPos.Kd,
               std::numeric_limits<double>::max(),
               std::numeric_limits<double>::max(),
               false,
               std::vector<bool>{false, false, false}),
        pidPosTarget(params.pidPos.Kp,
                     params.pidPos.Ki,
                     params.pidPos.Kd,
                     std::numeric_limits<double>::max(),
                     std::numeric_limits<double>::max(),
                     false,
                     std::vector<bool>{false, false, false}),
        pidOri(params.pidOri.Kp,
               params.pidOri.Ki,
               params.pidOri.Kd,
               std::numeric_limits<double>::max(),
               std::numeric_limits<double>::max(),
               false,
               std::vector<bool>{true, true, true}),
        pidOriTarget(params.pidOri.Kp,
                     params.pidOri.Ki,
                     params.pidOri.Kd,
                     std::numeric_limits<double>::max(),
                     std::numeric_limits<double>::max(),
                     false,
                     std::vector<bool>{true, true, true})
    {
        ARMARX_IMPORTANT << "Trajectory following controller params: "
                         << VAROUT(params.limits.linear) << ", " << VAROUT(params.limits.angular);
    }
 
    core::Twist
    TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
    {
        // Zero is a legitimate limit -- the safety guard produces it -- and so is a non-finite
        // twist arriving from a non-finite pose. Neither may throw: this runs in the 100 Hz
        // `additionalTask()`, and one escaping exception ends that thread permanently while the
        // RT loop keeps executing the twist it was last handed.
        if (params.limits.linear <= 0 or params.limits.angular <= 0 or
            not std::isfinite(params.limits.linear) or not std::isfinite(params.limits.angular) or
            not twist.linear.allFinite() or not twist.angular.allFinite())
        {
            return core::Twist{.linear = Eigen::Vector3f::Zero(),
                               .angular = Eigen::Vector3f::Zero()};
        }
 
        const core::AngularVelocity angularLimit = Eigen::Vector3f::Ones() * params.limits.angular;
 
        // for all entries, scale should be less than 1
        // velocity limit is for total cartesian velocity, not a limit for each coordinate direction
        const float scalePos = twist.linear.norm() / params.limits.linear;
        const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(angularLimit.cwiseAbs());
 
        const float scaleMax = std::max(scalePos, scaleOri.maxCoeff());
 
        if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
        {
            return twist;
        }
 
        // scale such that no limit is violated
        if (params.coupleLinearAndAngularLimits)
        {
            twist.linear /= scaleMax;
            twist.angular /= scaleMax;
        }
        else
        {
            if (scalePos >= 1.0F)
            {
                twist.linear /= scalePos;
            }
            if (scaleOri.maxCoeff() >= 1.0F)
            {
                twist.angular /= scaleOri.maxCoeff();
            }
        }
 
        // constexpr float eps = 0.001;
 
        // pedantic checks
        // ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
 
        return twist;
    }
 
    core::Twist
    TrajectoryFollowingController::applyVelocityFactor(core::Twist twist)
    {
        twist.linear *= params.velocityFactor;
        twist.angular *= params.velocityFactor;
 
        return twist;
    }
 
    void
    TrajectoryFollowingController::updateParams(const Params& p)
    {
        ARMARX_CHECK(p.velocityFactor >= 0)
            << "Scaling factor for velocity may not be negative, but is " << p.velocityFactor;
        ARMARX_CHECK(p.velocityFactor <= 1)
            << "Scaling factor for velocity may not be > 1, but is " << p.velocityFactor;
 
        // only changes to these to parameters are actually relevant for this class
        params.velocityFactor = p.velocityFactor;
        params.limits = p.limits;
        params.orientationWeight = p.orientationWeight;
        params.maxSegmentsAhead = p.maxSegmentsAhead;
        params.coupleLinearAndAngularLimits = p.coupleLinearAndAngularLimits;
        params.enableAngularFeedforward = p.enableAngularFeedforward;
        params.lookaheadDistance = p.lookaheadDistance;
        params.angularFeedforwardFraction = p.angularFeedforwardFraction;
        params.minFeedforwardVelocityFraction = p.minFeedforwardVelocityFraction;
    }
 
    namespace
    {
        /**
         * Shared prefix for every guard warning, so one `grep '[nav-guard]'` over a robot log
         * shows which guards fired during a run. The token after it names the guard.
         */
        constexpr const char* guardTag = "[nav-guard] ";
 
        float
        signedShortestAngularDiff(const float target, const float current)
        {
            float diff = target - current;
            while (diff > M_PIf32)
            {
                diff -= 2.0f * M_PIf32;
            }
            while (diff < -M_PIf32)
            {
                diff += 2.0f * M_PIf32;
            }
            return diff;
        }
    } // namespace
 
    void
    TrajectoryFollowingController::resetProgress()
    {
        lastTrajectoryId_ = std::nullopt;
        lastProjectionIndex_ = 0;
    }
 
    TrajectoryControllerResult
    TrajectoryFollowingController::control(const core::GlobalTrajectory& trajectory,
                                           const core::Pose& global_T_robot)
    {
        using simox::math::mat4f_to_pos;
        using simox::math::mat4f_to_rpy;
 
        const core::Pose currentPose(global_T_robot);
        const float currentOrientation = mat4f_to_rpy(currentPose.matrix()).z();
 
        if (trajectory.points().empty())
        {
            ARMARX_INFO << "Trajectory is empty.";
            return TrajectoryControllerResult{
                .twist = core::Twist::Zero(),
                .dropPoint = {.waypoint = {.pose = core::Pose::Identity()}, .velocity = 0},
                .isFinalSegment = true,
                .currentOrientation = currentOrientation,
                .desiredOrientation = currentOrientation,
                .orientationError = 0,
                .positionError = 0};
        }
 
        // Detect new trajectory and reset progress if needed
        const TrajectoryId currentTrajectoryId{
            .numPoints = trajectory.points().size(),
            .firstTranslation = trajectory.points().front().waypoint.pose.translation(),
            .lastTranslation = trajectory.points().back().waypoint.pose.translation()};
 
        if (not lastTrajectoryId_.has_value() or lastTrajectoryId_.value() != currentTrajectoryId)
        {
            lastTrajectoryId_ = currentTrajectoryId;
            lastProjectionIndex_ = 0;
            guards_ = GuardState{};
            ARMARX_VERBOSE << "New trajectory detected, resetting progress.";
        }
 
        const std::size_t startSegment =
            (lastProjectionIndex_ > 0) ? lastProjectionIndex_ - 1 : 0;
 
        const auto projectedPose = trajectory.getProjection(
            currentPose,
            core::VelocityInterpolation::LinearInterpolation,
            startSegment,
            static_cast<std::size_t>(params.maxSegmentsAhead),
            params.orientationWeight,
            params.lookaheadDistance);
 
        lastProjectionIndex_ = projectedPose.indexBefore;
 
 
        pidPos.update(mat4f_to_pos(currentPose.matrix()),
                      mat4f_to_pos(projectedPose.projection.waypoint.pose.matrix()));
        pidOri.update(mat4f_to_rpy(currentPose.matrix()),
                      mat4f_to_rpy(projectedPose.projection.waypoint.pose.matrix()));
 
        const float desiredOrientation =
            mat4f_to_rpy(projectedPose.projection.waypoint.pose.matrix()).z();
 
        float ffAngular = 0.0f;
        float cappedFfVel = 0.0f;
        bool angularFeedforwardSaturated = false;
        GuardVerdict guard = GuardVerdict::None;
 
        const core::Twist twist = [&]() -> core::Twist
        {
            // on the final segment, bahavior differs
            if (projectedPose.segment == core::Projection::Segment::FINAL)
            {
 
                ARMARX_VERBOSE << deactivateSpam(1) << "final segment";
                // TODO fairly inefficient to do this every time
                pidPos.reset();
                pidOri.reset();
 
                pidPosTarget.update(currentPose.translation(),
                                    trajectory.points().back().waypoint.pose.translation());
 
                pidOriTarget.update(
                    mat4f_to_rpy(currentPose.matrix()),
                    mat4f_to_rpy(trajectory.points().back().waypoint.pose.matrix()));
 
                return core::Twist{.linear = pidPosTarget.getControlValue(),
                                   .angular = pidOriTarget.getControlValue()};
            }
 
            // pidPosTarget not used yet
            // TODO fairly inefficient to do this every time
            pidPosTarget.reset();
 
            // the "standard" case following the trajectory
            const Eigen::Vector3f segmentDelta =
                projectedPose.wayPointAfter.waypoint.pose.translation() -
                projectedPose.wayPointBefore.waypoint.pose.translation();
 
            // `Eigen::normalized()` is `v / v.norm()`, i.e. 0/0 for a degenerate segment, and the
            // NaN would travel all the way to the platform's velocity target. Coincident
            // waypoints reach here from the planner, from `getSubTrajectory` after an emergency
            // stop, and from any reparametrization that emits a repeated sample.
            const bool degenerateSegment = not(segmentDelta.norm() > 0.F);
 
            if (degenerateSegment)
            {
                guard = GuardVerdict::ZeroLengthSegment;
 
                if (not guards_.zeroLengthSegment)
                {
                    guards_.zeroLengthSegment = true;
                    ARMARX_WARNING << guardTag << "zero-length-segment: trajectory segment "
                                   << QUOTED(projectedPose.indexBefore)
                                   << " has zero length. Dropping the feed-forward for it and "
                                      "following on the orientation feedback alone.";
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(1) << guardTag
                                   << "zero-length-segment: still at segment "
                                   << QUOTED(projectedPose.indexBefore) << ".";
                }
 
                return core::Twist{.linear = pidPos.getControlValue(),
                                   .angular = pidOri.getControlValue()};
            }
 
            const Eigen::Vector3f desiredMovementDirection = segmentDelta.normalized();
 
            const float ffVel = projectedPose.projection.velocity;
 
            // Was `ARMARX_CHECK_FINITE` / `ARMARX_CHECK_LESS(ffVel, 3000)`. Both diagnose a real
            // defect upstream, but throwing here kills the control thread, so they report and
            // fall back instead. 3 m/s is still the sanity bound the original check used.
            constexpr float implausibleVelocity = 3000.F;
 
            if (not std::isfinite(ffVel) or ffVel >= implausibleVelocity)
            {
                guard = GuardVerdict::NonFiniteTwist;
 
                if (not guards_.nonFiniteTwist)
                {
                    guards_.nonFiniteTwist = true;
                    ARMARX_WARNING << guardTag << "nonfinite-twist: implausible feed-forward "
                                   << "velocity " << QUOTED(ffVel) << " at segment "
                                   << QUOTED(projectedPose.indexBefore)
                                   << ". Dropping the feed-forward for this cycle.";
                }
 
                return core::Twist{.linear = pidPos.getControlValue(),
                                   .angular = pidOri.getControlValue()};
            }
 
            // --- Angular feedforward based on segment orientation rate ---
            cappedFfVel = ffVel;
            if (params.enableAngularFeedforward)
            {
                const float yawBefore =
                    mat4f_to_rpy(projectedPose.wayPointBefore.waypoint.pose.matrix()).z();
                const float yawAfter =
                    mat4f_to_rpy(projectedPose.wayPointAfter.waypoint.pose.matrix()).z();
                const float angularDelta = signedShortestAngularDiff(yawAfter, yawBefore);
 
                const float segmentLength =
                    (projectedPose.wayPointAfter.waypoint.pose.translation() -
                     projectedPose.wayPointBefore.waypoint.pose.translation())
                        .norm();
 
                // The old gate was `segmentLength > 1.0f`, in millimetres. TOPP-RA emits a fixed
                // 500 samples regardless of path length, so any path shorter than ~500 mm fell
                // below it and the whole angular feed-forward silently vanished -- and did so
                // discontinuously at that threshold. Only a genuinely degenerate segment needs
                // excluding, and the zero-length guard above already handles that.
                constexpr float minSegmentLength = 1e-3F; // [mm]
 
                const float turnRatePerLength =
                    (segmentLength > minSegmentLength) ? (angularDelta / segmentLength) : 0.0f;
 
                // How much of the angular budget the feed-forward may spend. Leaving headroom is
                // what lets `pidOri` still correct a heading error: at the full limit the
                // feed-forward saturates on every capped turn and the loop settles at a standing
                // error of `limits.angular / pidOri.Kp` -- 57 deg with the shipped gain of 1.
                const float ffAngularLimit =
                    std::clamp(params.angularFeedforwardFraction, 0.F, 1.F) * params.limits.angular;
 
                const float requiredAngularRate = turnRatePerLength * ffVel;
 
                // Cap linear velocity if the turn is too sharp for the robot
                if (std::abs(requiredAngularRate) > ffAngularLimit and ffAngularLimit > 0.F)
                {
                    // Substituting, this is `ffAngularLimit / |turnRatePerLength|` -- `ffVel`
                    // cancels out. Without a floor it goes to zero as the turn sharpens, which is
                    // the robot creeping while it spins.
                    const float minFfVel =
                        std::clamp(params.minFeedforwardVelocityFraction, 0.F, 1.F) * ffVel;
 
                    cappedFfVel = std::max(
                        minFfVel, ffVel * (ffAngularLimit / std::abs(requiredAngularRate)));
                }
 
                // Actual angular feedforward at the (possibly capped) linear speed, never above
                // the share of the budget it is allowed -- the floor above can otherwise put it
                // back over the limit.
                ffAngular = std::clamp(
                    turnRatePerLength * cappedFfVel, -ffAngularLimit, ffAngularLimit);
 
                // Substituting the cap gives `cappedFfVel = omega_max * L / |dyaw|` -- `ffVel`
                // cancels -- and therefore `ffAngular = sign(dyaw) * omega_max` exactly. The
                // feed-forward then owns the entire angular budget and `pidOri` has none left to
                // correct a heading error, so the robot settles at a standing offset of
                // `omega_max / Kp` while its linear command collapses to `omega_max / |yaw'|`.
                angularFeedforwardSaturated =
                    ffAngularLimit > 0.F and std::abs(requiredAngularRate) > ffAngularLimit;
 
                ARMARX_VERBOSE << deactivateSpam(1) << "FF angular " << ffAngular;
            }
 
            const auto feedforwardVelocity = desiredMovementDirection * cappedFfVel;
 
            ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward direction "
                           << feedforwardVelocity.normalized();
            ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward velocity " << feedforwardVelocity;
            ARMARX_VERBOSE << deactivateSpam(1) << "Control value " << pidPos.getControlValue();
 
            core::AngularVelocity angularFF = Eigen::Vector3f::Zero();
            angularFF.z() = ffAngular;
            return core::Twist{.linear = pidPos.getControlValue() + feedforwardVelocity,
                               .angular = pidOri.getControlValue() + angularFF};
        }();
 
        // The cap firing is not by itself an anomaly -- it fires on any sharp turn -- so warning
        // per cycle would flood the log and prove nothing. Only the pathological signature is
        // reported: the yaw rate pinned *and* the linear command collapsed, held for long enough
        // that it is not a corner being negotiated.
        if (angularFeedforwardSaturated and cappedFfVel < 0.25F * projectedPose.projection.velocity)
        {
            guards_.angularSaturatedCycles++;
 
            constexpr std::size_t sustainedCycles = 50; // ~0.5 s at the 100 Hz task rate
 
            if (guards_.angularSaturatedCycles >= sustainedCycles and
                not guards_.angularFeedforwardSaturated)
            {
                guards_.angularFeedforwardSaturated = true;
                ARMARX_WARNING
                    << guardTag << "ff-angular-saturated: the angular feed-forward has been "
                    << "pinned at the limit for " << guards_.angularSaturatedCycles
                    << " cycles while the linear command collapsed. segment="
                    << QUOTED(projectedPose.indexBefore) << " ffVel="
                    << QUOTED(projectedPose.projection.velocity)
                    << " cappedFfVel=" << QUOTED(cappedFfVel) << " ffAngular="
                    << QUOTED(ffAngular) << " limitAngular=" << QUOTED(params.limits.angular)
                    << ". The robot is turning on the spot rather than following the path.";
            }
        }
        else
        {
            guards_.angularSaturatedCycles = 0;
        }
 
        // Ahead of `applyTwistLimits`, not after it: a NaN reaching the limiter used to trip an
        // `ARMARX_CHECK` there, which is the throw this whole guard exists to prevent.
        core::Twist twistChecked = twist;
 
        if (not twistChecked.linear.allFinite() or not twistChecked.angular.allFinite())
        {
            guard = GuardVerdict::NonFiniteTwist;
            twistChecked = core::Twist::Zero();
 
            if (not guards_.nonFiniteTwist)
            {
                guards_.nonFiniteTwist = true;
                ARMARX_WARNING << guardTag
                               << "nonfinite-twist: the computed twist was not finite at segment "
                               << QUOTED(projectedPose.indexBefore)
                               << ". Commanding zero. A non-finite pose or velocity reached the "
                                  "controller from the trajectory or the localization.";
            }
            else
            {
                ARMARX_WARNING << deactivateSpam(1) << guardTag << "nonfinite-twist: still zero.";
            }
        }
 
        const auto twistLimited = applyTwistLimits(twistChecked);
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited linear "
                       << twistLimited.linear.transpose();
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited angular "
                       << twistLimited.angular.transpose();
 
        const auto twistScaled = applyVelocityFactor(twistLimited);
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist scaled linear "
                       << twistScaled.linear.transpose();
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist scaled angular "
                       << twistScaled.angular.transpose();
 
        // convert to the robot's base frame
        const auto& twistGlobal = twistScaled;
 
        core::Twist twistLocal; // NOLINT: reassigned by the non-finite guard below
        twistLocal.linear = global_T_robot.linear().inverse() * twistGlobal.linear;
        // TODO if not in 2D, then this must be changed!
        twistLocal.angular = twistGlobal.angular;
 
 
        const bool isFinalSegment = projectedPose.segment == core::Projection::Segment::FINAL;
 
        // Last line of defence. Everything above guards its own inputs, but this runs in the
        // 100 Hz `additionalTask()`, whose `runTask` callback `RunningTaskBase::run()` wraps in a
        // single try/catch -- one escaping throw ends that thread for good while the RT loop keeps
        // executing the last twist it was handed. Commanding zero is always safe; throwing is not.
        if (not twistLocal.linear.allFinite() or not twistLocal.angular.allFinite())
        {
            guard = GuardVerdict::NonFiniteTwist;
            twistLocal = core::Twist::Zero();
 
            if (not guards_.nonFiniteTwist)
            {
                guards_.nonFiniteTwist = true;
                ARMARX_WARNING << guardTag
                               << "nonfinite-twist: the computed twist was not finite at segment "
                               << QUOTED(projectedPose.indexBefore)
                               << ". Commanding zero. This means a non-finite pose or velocity "
                                  "reached the controller from the trajectory or the localization.";
            }
            else
            {
                ARMARX_WARNING << deactivateSpam(1) << guardTag << "nonfinite-twist: still zero.";
            }
        }
 
        return TrajectoryControllerResult{
            .twist = twistLocal,
            .dropPoint = projectedPose.projection,
            .isFinalSegment = isFinalSegment,
            .currentOrientation = currentOrientation,
            .desiredOrientation = desiredOrientation,
            // Wrapped: the raw difference spikes by up to 2*pi on every crossing of the +-pi
            // branch cut, which is exactly where a heading-tracking plot is read most carefully.
            .orientationError =
                std::abs(signedShortestAngularDiff(desiredOrientation, currentOrientation)),
            .positionError = (global_T_robot.translation() -
                              trajectory.points().back().waypoint.pose.translation())
                                 .head<2>()
                                 .norm(),
            .ffAngular = ffAngular,
            .cappedFfVel = cappedFfVel,
            .projectionIndex = projectedPose.indexBefore,
            .angularFeedforwardSaturated = angularFeedforwardSaturated,
            .guard = guard};
    }
 
} // namespace armarx::navigation::traj_ctrl::global