TrajectoryFollowingSimulation.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstddef>
#include <limits>
#include <vector>
 
#include <Eigen/Core>
 
#include <VirtualRobot/IK/platform/MecanumPlatformKinematics.h>
#include <VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h>
 
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/simulation/PlatformInertia.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/trajectory_control/global/TrajectoryFollowingController.h>
 
namespace armarx::navigation::simulation
{
 
    /// How the simulated base turns a commanded twist into an achieved one.
    enum class PlatformModel
    {
        /// Bound the magnitude of the twist change. Direction of the change is preserved.
        Cartesian,
 
        /// Bound each wheel's angular acceleration separately, so one saturating wheel also
        /// distorts the direction. Kinematic: the per-wheel bounds are placeholders.
        OmniWheelVelocity,
 
        /// Bound each *motor's torque* and speed, using the robot's real inertia. The physical
        /// model: required wrench -> wheel torques -> per-motor clip -> achieved acceleration.
        OmniWheelTorque,
 
        /// As `OmniWheelTorque`, but for a four-wheel mecanum drive.
        MecanumTorque
    };
 
    /**
     * @brief Limits of the omni-directional wheel drive, as on ARMAR-7.
     *
     * Wheel velocities follow `VirtualRobot::OmniWheelPlatformKinematics`, whose forward model
     * carries a factor `2 pi R / n`. Wheel speeds are therefore in **revolutions per second** at
     * the wheel, not rad/s.
     */
    struct OmniWheelDrive
    {
        VirtualRobot::OmniWheelPlatformKinematicsParams kinematics{};
 
        float maxWheelVelocity{3.82F}; // [rev/s]
 
        float maxWheelAcceleration{0.64F}; // [rev/s^2]
 
        float maxWheelDeceleration{1.02F}; // [rev/s^2]
    };
 
    /**
     * @brief Limits of the four-wheel mecanum drive, as on ARMAR-6 and ARMAR-DE.
     *
     * Wheel velocities follow `VirtualRobot::MecanumPlatformKinematics`, whose `J()` is
     * `R/4 * ...` and therefore takes **rad/s** at the wheel -- unlike the omni drive's `C()`,
     * which takes rev/s. Note also that Simox has y pointing forwards, not x as in the paper,
     * and orders the wheels `[left front, right front, rear left, rear right]`.
     */
    struct MecanumDrive
    {
        VirtualRobot::MecanumPlatformKinematicsParams kinematics{};
 
        float maxWheelVelocity{21.1F}; // [rad/s]
 
        float maxWheelAcceleration{8.5F}; // [rad/s^2]
 
        float maxWheelDeceleration{8.5F}; // [rad/s^2]
    };
 
    /**
     * @brief What the ARMAR-7 drive train can actually deliver.
     *
     * Motor: Maxon IDX 56 M, 48 V winding. Gearbox: Maxon GPX 52, 16:1. See
     * `data/armarx_navigation/config/platform/README.md` for the full derivation of every
     * number here.
     *
     * Geometry is *not* repeated: the torque model reuses `OmniWheelDrive::kinematics`.
     */
    struct OmniWheelTorqueDrive
    {
        /// Motor revolutions per wheel revolution. 65536 counts/wheel-rev / 4096 counts/motor-rev.
        float gearRatio{16.F};
 
        /// Torque bound per motor [N m]. Default is the 7.0 A `MaxCurrent` cap x 62.4 mNm/A.
        float motorMaxTorque{0.437F};
 
        /// Torque at `RatedCurrent` (7.34 A x 62.4 mNm/A) [N m]. Recorded for reference.
        float motorRatedTorque{0.458F};
 
        /// Continuous motor speed [rpm], IDX 56 M at 48 V.
        float motorNominalSpeedRpm{6260.F};
 
        /// Gearbox continuous input speed [rpm]. Binds before the motor on ARMAR-7.
        float gearboxMaxInputSpeedRpm{6000.F};
 
        /// Measured wheel speed limit [rad/s]. Where finite, it supersedes the rating above
        /// (`min(motorNominalSpeedRpm, gearboxMaxInputSpeedRpm) / gearRatio`), as in
        /// `LoadDriveParams`.
        float maxWheelVelocity{std::numeric_limits<float>::infinity()};
 
        /// Ignored losses are modelled as 1.0, i.e. optimistic.
        float gearboxEfficiency{1.F};
 
        PlatformInertia::Parameters robot;
    };
 
    /**
     * @brief The per-axis command ramp the platform device applies below the navigation stack.
     *
     * Faithful model of `LinearLimitedAccelerationController` in
     * `devices/ethercat/platform/armar7_omni/joint_controller/Velocity.cpp`, which rate-limits
     * `vx`, `vy` and the platform yaw rate **independently** before the inverse kinematics.
     *
     * This is the only acceleration limit in the real signal chain: the navigation stack does
     * not bound the time derivative of the commanded twist (its `alpha` low-pass is configured
     * to 0 on ARMAR-7), and the simulated platform device in `RobotUnitSimulation` does not
     * either. Modelling it here is what lets the analysis answer whether the values in
     * `Armar7a/parts/Platform.xml` actually keep the motors out of saturation.
     *
     * Defaults mirror that file.
     */
    struct CommandRateLimit
    {
        bool enabled{true};
 
        /// Control cycle of the RT loop [s]. `RTUnit` runs at 1 kHz and clamps dt to <= 2 ms.
        float dt{0.001F};
 
        float maxVelocity{1500.F}; // [mm/s]
 
        float maxAcceleration{250.F}; // [mm/s^2]
 
        float maxDeceleration{400.F}; // [mm/s^2]
 
        float maxAngularVelocity{1.0F}; // [rad/s]
 
        float maxAngularAcceleration{0.4F}; // [rad/s^2]
 
        float maxAngularDeceleration{0.45F}; // [rad/s^2]
 
        /**
         * @brief Ramp the two linear axes together instead of independently.
         *
         * Mirrors `directionPreservingRamp` in the platform's hardware config. Independent
         * per-axis ramps let the commanded direction rotate whenever the two targets differ in
         * magnitude; coupling them bounds `|dv/dt|` and keeps the direction. Default matches the
         * device default so an unconfigured simulation reproduces the historical behaviour.
         */
        bool directionPreserving{false};
    };
 
    /**
     * @brief Dynamic limits of the simulated platform.
     *
     * Nothing in the real stack enforces a bound on how fast the commanded twist may change:
     * `applyTwistLimits()` bounds the velocity only, and the RT layer forwards the twist
     * unchanged. (The `armar7_omni` device does rate-limit, but per Cartesian axis, downstream
     * of the navigation stack.) These limits exist so the simulation can show the consequence —
     * a base that cannot follow a velocity step lags behind the command and leaves the path.
     */
    struct PlatformDynamics
    {
        PlatformModel model{PlatformModel::Cartesian};
 
        float maxLinearAcceleration{500.F}; // [mm/s^2]
 
        float maxAngularAcceleration{0.5F}; // [rad/s^2]
 
        OmniWheelDrive omniWheel;
 
        MecanumDrive mecanum;
 
        OmniWheelTorqueDrive omniWheelTorque;
 
        /**
         * @brief Read the dynamics from `config/platform/PlatformDynamics<Robot>.json`.
         *
         * @param robot  Platform name, spelled as the rest of the codebase does:
         *               `Armar7`, `Armar6`, `ArmarDE`.
         */
        static PlatformDynamics FromPackagePath(const std::string& robot = "Armar7");
    };
 
    /**
     * @brief Point-mass simulation of the platform following a global trajectory.
     *
     * Reproduces the loop that `platform_controller::platform_global_trajectory::Controller`
     * runs on the robot:
     *
     *   1. `TrajectoryFollowingController::control(trajectory, global_T_robot)` produces a twist
     *      in the *robot* frame (`additionalTask()`),
     *   2. the twist is low-pass filtered with `alpha` (`additionalTask()`),
     *   3. the filtered twist is handed to the holonomic platform as a velocity target
     *      (`rtRun()`).
     *
     * The robot is modelled as a point mass that reaches the commanded velocity instantly, i.e.
     * **no acceleration limit is imposed**. That is deliberate: the resulting acceleration is an
     * output of the simulation, so it can be compared against what the base can actually deliver.
     * Where the required acceleration exceeds that, the real robot lags behind and overshoots —
     * which the trajectory's velocity profile alone does not reveal.
     *
     * The controller and the real-time loop run at different rates on the robot (the additional
     * task at ~100 Hz, the RT layer faster). Here both run at `dt`, which is the controller rate:
     * the RT layer only forwards the twist, so it does not affect the trajectory.
     */
    class TrajectoryFollowingSimulation
    {
    public:
        struct Parameters
        {
            /// Control cycle [s]. `CycleUtil(10)` in the controller's additional task -> 10 ms.
            float dt{0.01F};
 
            /// Give up after this much simulated time [s].
            float maxDuration{120.F};
 
            /// Consider the goal reached within this distance to the last trajectory point [mm].
            float goalDistanceThreshold{20.F};
 
            /// Speed below which the robot counts as standing still [mm/s].
            ///
            /// The controller's final segment is a pure P law, so it approaches the goal
            /// asymptotically and can settle just outside `goalDistanceThreshold` -- creeping at
            /// a few hundredths of a mm/s for the rest of `maxDuration`. Without this the
            /// simulation would spend most of its samples on a robot that has stopped.
            float settledSpeedThreshold{1.F};
 
            /// How long the robot has to stay below `settledSpeedThreshold` before giving up [s].
            float settledDuration{1.F};
 
            /// Parameters of the trajectory following controller, including `alpha` and limits.
            traj_ctrl::global::TrajectoryFollowingControllerParams controller;
 
            /// What the simulated mass can actually deliver.
            PlatformDynamics dynamics;
 
            /// The device-side command ramp between the controller and the platform.
            CommandRateLimit rateLimit;
        };
 
        struct Sample
        {
            float time{0.F}; // [s]
 
            core::Pose global_T_robot{core::Pose::Identity()};
 
            /// Filtered twist as commanded to the platform, in the robot frame.
            Eigen::Vector2f commandedLinearLocal{Eigen::Vector2f::Zero()}; // [mm/s]
 
            float commandedAngular{0.F}; // [rad/s]
 
            /// Commanded linear velocity in the global frame, i.e. what the mass is asked for.
            Eigen::Vector2f commandedVelocityGlobal{Eigen::Vector2f::Zero()}; // [mm/s]
 
            /// Achieved linear velocity in the global frame, after the acceleration limit.
            Eigen::Vector2f velocityGlobal{Eigen::Vector2f::Zero()}; // [mm/s]
 
            float speed{0.F}; // [mm/s] magnitude of the achieved velocity
 
            /// Acceleration needed to match the command within one cycle, ignoring the limit.
            float requiredLinearAcceleration{0.F}; // [mm/s^2]
 
            /// Acceleration actually applied, bounded by `PlatformDynamics`.
            float linearAcceleration{0.F}; // [mm/s^2]
 
            float requiredAngularAcceleration{0.F}; // [rad/s^2]
 
            float angularAcceleration{0.F}; // [rad/s^2]
 
            /// True when a limit was binding, i.e. the base could not follow the command.
            bool accelerationSaturated{false};
 
            /// Wheel speeds the command asks for [rev/s]. Only set for the omni-wheel model.
            Eigen::VectorXf commandedWheelVelocities;
 
            /// Wheel speeds actually reached [rev/s]. Only set for the omni-wheel model.
            Eigen::VectorXf wheelVelocities;
 
            /// Per-wheel acceleration the command asks for [rev/s^2], ignoring the limit.
            Eigen::VectorXf requiredWheelAccelerations;
 
            /// Which wheels hit their acceleration bound. Only set for `OmniWheelVelocity`.
            Eigen::ArrayXi wheelSaturated;
 
            /// Motor torque the command asks for [N m]. Only set for `OmniWheelTorque`.
            Eigen::VectorXf requiredMotorTorques;
 
            /// Motor torque after clipping [N m]. Only set for `OmniWheelTorque`.
            Eigen::VectorXf appliedMotorTorques;
 
            /// Which motors hit their torque bound. Only set for `OmniWheelTorque`.
            Eigen::ArrayXi motorSaturated;
 
            /// Which wheels hit their speed bound. Only set for `OmniWheelTorque`.
            Eigen::ArrayXi wheelSpeedSaturated;
 
            /// Velocity of the trajectory point the controller is currently tracking.
            float dropPointVelocity{0.F}; // [mm/s]
 
            /// Distance from the robot to the trajectory point it is tracking [mm].
            /// This is the cross-track error, i.e. how far the robot has drifted off the path.
            float trackingError{0.F};
 
            /// Distance to the last trajectory point [mm]. This is what the controller itself
            /// reports as `positionError`, despite the name.
            float distanceToGoal{0.F};
 
            float orientationError{0.F}; // [rad]
        };
 
        struct Result
        {
            std::vector<Sample> samples;
 
            bool reachedGoal{false};
 
            /// Set when the run ended because the robot stopped moving short of the goal.
            bool settledShortOfGoal{false};
 
            /// Distance to the last trajectory point when the run ended [mm].
            float finalDistanceToGoal{0.F};
 
            /// Furthest the robot ever got *past* the last trajectory point [mm].
            ///
            /// The velocity profile is indexed by position, so nothing in it bounds the stopping
            /// distance the device command ramp actually needs. Overshoot is the visible symptom.
            float maxGoalOvershoot{0.F};
 
            /// Largest `requiredLinearAcceleration` over the run [mm/s^2].
            float maxRequiredLinearAcceleration{0.F};
 
            /// Largest `requiredAngularAcceleration` over the run [rad/s^2].
            float maxRequiredAngularAcceleration{0.F};
 
            /// Number of control cycles in which any limit was binding.
            std::size_t saturatedCycles{0};
 
            /// Cycles in which a motor torque bound was binding. `OmniWheelTorque` only.
            std::size_t motorTorqueSaturatedCycles{0};
 
            /// Cycles in which a wheel speed bound was binding. `OmniWheelTorque` only.
            std::size_t wheelSpeedSaturatedCycles{0};
 
            /// Largest motor torque demanded over the run [N m]. `OmniWheelTorque` only.
            float maxRequiredMotorTorque{0.F};
 
            /// Platform mass and yaw inertia from the robot model. `OmniWheelTorque` only.
            float platformMass{0.F}; // [kg]
 
            float platformYawInertia{0.F}; // [kg m^2]
 
            /// Full mass matrix at the `Home` configuration, `[x, y, yaw]`. The off-diagonal
            /// terms are the CoM offset from the yaw axis and are not negligible on ARMAR-7.
            Eigen::Matrix3d massMatrix{Eigen::Matrix3d::Zero()};
 
            /// Largest cross-track error over the run [mm].
            float maxTrackingError{0.F};
        };
 
        explicit TrajectoryFollowingSimulation(const Parameters& params);
 
        /// Follow `trajectory` starting from `start`, which need not be on the trajectory.
        Result run(const core::GlobalTrajectory& trajectory, const core::Pose& start) const;
 
    private:
        Parameters params_;
    };
 
} // namespace armarx::navigation::simulation