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)
    {
        if (params.limits.linear == 0 or params.limits.angular == 0)
        {
            return core::Twist{.linear = Eigen::Vector3f::Zero(),
                               .angular = Eigen::Vector3f::Zero()};
        }
 
        ARMARX_CHECK_POSITIVE(params.limits.linear);
        ARMARX_CHECK_POSITIVE(params.limits.angular);
 
        core::AngularVelocity angularLimit = Eigen::Vector3f::Ones() * params.limits.angular;
 
        ARMARX_CHECK(std::isfinite(params.limits.linear));
        ARMARX_CHECK(angularLimit.allFinite());
 
        // 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());
 
        // ARMARX_CHECK(scalePos.allFinite());
        ARMARX_CHECK(scaleOri.allFinite());
 
        const float scaleMax = std::max(scalePos, scaleOri.maxCoeff());
 
        if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
        {
            return twist;
        }
 
        ARMARX_CHECK(not std::isnan(scaleMax));
 
        // 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;
    }
 
    namespace
    {
        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;
            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);
 
        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;
 
        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 desiredMovementDirection =
                (projectedPose.wayPointAfter.waypoint.pose.translation() -
                 projectedPose.wayPointBefore.waypoint.pose.translation())
                    .normalized();
 
            const float ffVel = projectedPose.projection.velocity;
            ARMARX_CHECK_FINITE(ffVel);
            ARMARX_CHECK_LESS(ffVel, 3000); // 3 m/s should be sufficient, right?
 
            // --- 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();
 
                // Required angular rate to track this segment at current ffVel
                const float requiredAngularRate =
                    (segmentLength > 1.0f) ? (angularDelta / segmentLength * ffVel) : 0.0f;
 
                // Cap linear velocity if the turn is too sharp for the robot
                if (std::abs(requiredAngularRate) > params.limits.angular)
                {
                    cappedFfVel = ffVel * (params.limits.angular / std::abs(requiredAngularRate));
                }
 
                // Actual angular feedforward at the (possibly capped) linear speed
                ffAngular =
                    (segmentLength > 1.0f) ? (angularDelta / segmentLength * cappedFfVel) : 0.0f;
 
                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};
        }();
 
        const auto twistLimited = applyTwistLimits(twist);
        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;
        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;
 
        return TrajectoryControllerResult{
            .twist = twistLocal,
            .dropPoint = projectedPose.projection,
            .isFinalSegment = isFinalSegment,
            .currentOrientation = currentOrientation,
            .desiredOrientation = desiredOrientation,
            .orientationError = std::abs(currentOrientation - desiredOrientation),
            .positionError = (global_T_robot.translation() -
                              trajectory.points().back().waypoint.pose.translation())
                                 .head<2>()
                                 .norm(),
            .ffAngular = ffAngular,
            .cappedFfVel = cappedFfVel};
    }
 
} // namespace armarx::navigation::traj_ctrl::global