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/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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::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
        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;
    }
 
    TrajectoryControllerResult
    TrajectoryFollowingController::control(const core::LocalTrajectory& trajectory,
                                           const Eigen::Isometry3f& global_T_robot)
    {
        ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
 
        if (trajectory.points().empty())
        {
            ARMARX_INFO << "Trajectory is empty.";
            return TrajectoryControllerResult{.twist = core::Twist::Zero()};
        }
 
        // TOOD maybe set via arg?
        const DateTime timestamp = Clock::Now();
 
        const core::Evaluation target = trajectory.evaluate(timestamp);
 
        using simox::math::mat4f_to_pos;
        using simox::math::mat4f_to_rpy;
 
        pidPos.update(mat4f_to_pos(global_T_robot.matrix()), mat4f_to_pos(target.pose.matrix()));
        pidOri.update(mat4f_to_rpy(global_T_robot.matrix()), mat4f_to_rpy(target.pose.matrix()));
 
 
        const core::Twist& twistFeedForward = target.feedforwardTwist;
 
        const core::Twist twistError{.linear = pidPos.getControlValue(),
                                     .angular = pidOri.getControlValue()};
 
        const core::Twist twist{.linear = twistFeedForward.linear + twistError.linear,
                                .angular = twistFeedForward.angular + twistError.angular};
 
        const auto twistLimited = applyTwistLimits(twist);
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
        ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
 
        // convert to the robot's base frame
        const auto& twistGlobal = twistLimited;
 
        const core::Twist twistLocal
        {
            .linear =  global_T_robot.linear().inverse() * twistGlobal.linear,
            .angular = twistGlobal.angular
        };
 
        return TrajectoryControllerResult{.twist = twistLocal};
    }
 
} // namespace armarx::navigation::traj_ctrl::local