TrajectoryFollowingController.cpp

Go to the documentation of this file.

#include "TrajectoryFollowingController.h"
 
#include <algorithm>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.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/local/TrajectoryController.h>
#include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
#include <armarx/navigation/trajectory_control/local/core.h>
 
namespace armarx::navigation::traj_ctrl::local
{
    // TrajectoryFollowingControllerParams
 
    Algorithms
    TrajectoryFollowingControllerParams::algorithm() const
    {
        return Algorithms::TrajectoryFollowingController;
    }
 
    aron::data::DictPtr
    TrajectoryFollowingControllerParams::toAron() const
    {
        arondto::TrajectoryFollowingControllerParams dto;
 
        TrajectoryFollowingControllerParams bo;
        armarx::navigation::traj_ctrl::local::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::local::fromAron(dto, bo);
 
        return bo;
    }
 
    // TrajectoryFollowingController
 
    TrajectoryFollowingController::Params
    applyLimitFromGeneralConfig(const TrajectoryFollowingController::Params& params,
                                const core::GeneralConfig& generalConfig)
    {
        TrajectoryFollowingController::Params p = params;
 
        p.limits = generalConfig.maxVel;
 
        return p;
    }
 
    TrajectoryFollowingController::TrajectoryFollowingController(
        const Params& i_params,
        const core::GeneralConfig& generalConfig) :
        params(applyLimitFromGeneralConfig(i_params, generalConfig)),
        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 / 2,
                     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);
    }
 
    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 / 2,
                     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)
    {
        const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
                                 .angular = Eigen::Vector3f::Ones() * params.limits.angular};
 
        ARMARX_CHECK((limits.linear.array() > 0).all());
        ARMARX_CHECK((limits.angular.array() > 0).all());
        ARMARX_CHECK(limits.linear.allFinite());
        ARMARX_CHECK(limits.angular.allFinite());
 
        // for all entries, scale should be less than 1
        const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
        const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.cwiseAbs());
 
        ARMARX_CHECK(scalePos.allFinite());
        ARMARX_CHECK(scaleOri.allFinite());
 
        const float scaleMax = std::max(scalePos.maxCoeff(), 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.maxCoeff() >= 1.0F)
            {
                twist.linear /= scalePos.maxCoeff();
            }
            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.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
 
    TrajectoryControllerResult
    TrajectoryFollowingController::control(const core::LocalTrajectory& trajectory,
                                           const Eigen::Isometry3f& global_T_robot)
    {
        if (trajectory.points().empty())
        {
            ARMARX_INFO << "Trajectory is empty.";
            return TrajectoryControllerResult{.twist = core::Twist::Zero(),
                                              .target = {}}; // target not applicable
        }
 
        // FIXME
        const DateTime timestamp = Clock::Now() + Duration::MilliSeconds(100);
 
        const core::Evaluation target = trajectory.evaluate(timestamp);
 
        using simox::math::mat4f_to_pos;
        using simox::math::mat4f_to_rpy;
 
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(global_T_robot.translation().head<2>());
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(target.pose.translation().head<2>());
        ARMARX_VERBOSE << deactivateSpam(1)
                       << VAROUT(target.pose.translation().head<2>() -
                                  global_T_robot.translation().head<2>());
 
        pidPos.update(global_T_robot.translation(), target.pose.translation());
        pidOri.update(mat4f_to_rpy(global_T_robot.matrix()), mat4f_to_rpy(target.pose.matrix()));
 
        // Feed forward is already in global frame
        core::Twist twistFeedForward = target.feedforwardTwist;
        float ffAngular = twistFeedForward.angular.z();
        float cappedFfVel = twistFeedForward.linear.norm();
 
        // If local planner didn't provide angular feedforward, compute from segment
        if (params.enableAngularFeedforward)
        {
            if (std::abs(ffAngular) < 0.001f && target.idx + 1 < trajectory.points().size())
            {
                const auto& ptBefore = trajectory.points().at(target.idx);
                const auto& ptAfter = trajectory.points().at(target.idx + 1);
 
                const float yawBefore = mat4f_to_rpy(ptBefore.pose.matrix()).z();
                const float yawAfter = mat4f_to_rpy(ptAfter.pose.matrix()).z();
                const float angularDelta = signedShortestAngularDiff(yawAfter, yawBefore);
 
                const float distance =
                    (ptAfter.pose.translation() - ptBefore.pose.translation()).norm();
                const float dt = (ptAfter.timestamp - ptBefore.timestamp).toSecondsDouble();
 
                if (distance > 1.0f && dt > 0.001f)
                {
                    const float requiredRate = angularDelta / dt;
                    ffAngular = requiredRate;
 
                    // Scale feedforward if turn is too sharp for the robot
                    if (std::abs(requiredRate) > params.limits.angular)
                    {
                        const float scale = params.limits.angular / std::abs(requiredRate);
                        twistFeedForward.linear *= scale;
                        twistFeedForward.angular.z() = requiredRate * scale;
                        cappedFfVel = twistFeedForward.linear.norm();
                    }
                }
            }
            else if (std::abs(ffAngular) > params.limits.angular)
            {
                // Local planner provided angular feedforward but it exceeds limits
                const float scale = params.limits.angular / std::abs(ffAngular);
                twistFeedForward.linear *= scale;
                twistFeedForward.angular *= scale;
                ffAngular *= scale;
                cappedFfVel = twistFeedForward.linear.norm();
            }
        }
 
        const core::Twist twistError{.linear = pidPos.getControlValue(),
                                     .angular = pidOri.getControlValue()};
 
        // const core::Twist twistError = core::Twist::Zero();
 
        const core::Twist twist{.linear = twistFeedForward.linear + twistError.linear,
                                .angular = twistFeedForward.angular + twistError.angular};
 
        // const core::Twist twist = twistError;
 
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twistFeedForward.linear.transpose());
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twistFeedForward.angular.transpose());
 
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twistError.linear.transpose());
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twistError.angular.transpose());
 
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twist.linear.transpose());
        ARMARX_VERBOSE << deactivateSpam(1) << VAROUT(twist.angular.transpose());
 
 
        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;
 
        const core::Twist twistLocal{.linear =
                                         global_T_robot.linear().inverse() * twistGlobal.linear,
                                     .angular = twistGlobal.angular};
 
        return TrajectoryControllerResult{.twist = twistLocal,
                                            .target = target,
                                            .ffAngular = ffAngular,
                                            .cappedFfVel = cappedFfVel};
    }
 
} // namespace armarx::navigation::traj_ctrl::local