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);
 
        const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
                                 .angular = Eigen::Vector3f::Ones() * params.limits.angular};
 
        ARMARX_CHECK(limits.linear.allFinite());
        ARMARX_CHECK(limits.angular.allFinite());
 
        // for all entries, scale should be less than 1
        const float scalePos = twist.linear.norm() / limits.linear.norm();
        const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.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
        twist.linear /= scaleMax;
        twist.angular /= scaleMax;
 
        // 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;
    }
 
    void
    TrajectoryFollowingController::updateTwistLimits(const core::TwistLimits& limits)
    {
        params.limits = limits;
    }
 
    core::Twist TrajectoryFollowingController::applyVelocityFactor(core::Twist twist)
    {
        twist.linear *= params.velocityFactor;
        twist.angular *= params.velocityFactor;
 
        return twist;
    }
 
    void TrajectoryFollowingController::updateVelocityFactor(const float velocityFactor)
    {
        ARMARX_CHECK(velocityFactor >= 0)
            << "Scaling factor for velocity may not be negative, but is " << velocityFactor;
        ARMARX_CHECK(velocityFactor <= 1)
            << "Scaling factor for velocity may not be > 1, but is " << velocityFactor;
 
        params.velocityFactor = velocityFactor;
    }
 
    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};
        }
 
 
        const auto projectedPose = trajectory.getProjection(
            currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
 
 
        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();
 
        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?
 
            const auto feedforwardVelocity = desiredMovementDirection * ffVel;
 
            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();
 
            return core::Twist{.linear = pidPos.getControlValue() + feedforwardVelocity,
                               .angular = pidOri.getControlValue()};
        }();
 
        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()};
    }
 
} // namespace armarx::navigation::traj_ctrl::global