TrajectoryFollowingSimulation.cpp

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
 */
 
#include "TrajectoryFollowingSimulation.h"
 
#include <algorithm>
#include <cmath>
#include <filesystem>
#include <fstream>
#include <stdexcept>
#include <string>
#include <optional>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::navigation::simulation
{
 
    PlatformDynamics
    PlatformDynamics::FromPackagePath(const std::string& robot)
    {
        const armarx::PackagePath packagePath(
            "armarx_navigation", "config/platform/PlatformDynamics" + robot + ".json");
        const std::string filename = packagePath.toSystemPath();
 
        ARMARX_CHECK(std::filesystem::exists(filename))
            << "PlatformDynamics config file does not exist: " << filename;
 
        std::ifstream ifs{filename};
        const nlohmann::json j = nlohmann::json::parse(ifs);
 
        PlatformDynamics dynamics;
        dynamics.maxLinearAcceleration =
            j.at("maxLinearAcceleration").get<float>(); // [mm/s^2]
        dynamics.maxAngularAcceleration =
            j.at("maxAngularAcceleration").get<float>(); // [rad/s^2]
 
        ARMARX_CHECK_GREATER(dynamics.maxLinearAcceleration, 0.F);
        ARMARX_CHECK_GREATER(dynamics.maxAngularAcceleration, 0.F);
 
        const std::string model = j.value("model", std::string{"cartesian"});
        if (model == "cartesian")
        {
            dynamics.model = PlatformModel::Cartesian;
        }
        else if (model == "omni_wheel_velocity")
        {
            dynamics.model = PlatformModel::OmniWheelVelocity;
        }
        else if (model == "omni_wheel_torque")
        {
            dynamics.model = PlatformModel::OmniWheelTorque;
        }
        else if (model == "mecanum_torque")
        {
            dynamics.model = PlatformModel::MecanumTorque;
        }
        else
        {
            throw std::invalid_argument("Unknown platform model `" + model +
                                        "`. Expected `cartesian`, `omni_wheel_velocity`, "
                                        "`omni_wheel_torque` or `mecanum_torque`.");
        }
 
        if (j.contains("omniWheel"))
        {
            const nlohmann::json& jWheel = j.at("omniWheel");
            OmniWheelDrive& drive = dynamics.omniWheel;
 
            const std::vector<bool> invert = jWheel.at("invertWheel");
            ARMARX_CHECK_EQUAL(invert.size(), 3);
 
            drive.kinematics.L = jWheel.at("bodyRadius").get<float>();
            drive.kinematics.R = jWheel.at("wheelRadius").get<float>();
            drive.kinematics.delta = VirtualRobot::MathTools::deg2rad(
                jWheel.at("angularPositionFirstWheelDegrees").get<float>());
            drive.kinematics.n = jWheel.at("gearRatio").get<float>();
            drive.kinematics.relativeAngle = VirtualRobot::MathTools::deg2rad(
                jWheel.at("relativeAngleDegrees").get<float>());
            drive.kinematics.wheelFactor = Eigen::Vector3f{invert[0] ? -1.F : 1.F,
                                                           invert[1] ? -1.F : 1.F,
                                                           invert[2] ? -1.F : 1.F};
 
            drive.maxWheelVelocity = jWheel.at("maxWheelVelocity").get<float>();
            drive.maxWheelAcceleration = jWheel.at("maxWheelAcceleration").get<float>();
            drive.maxWheelDeceleration = jWheel.at("maxWheelDeceleration").get<float>();
 
            ARMARX_CHECK_GREATER(drive.maxWheelAcceleration, 0.F);
            ARMARX_CHECK_GREATER(drive.maxWheelDeceleration, 0.F);
        }
 
        if (j.contains("mecanum"))
        {
            const nlohmann::json& jWheel = j.at("mecanum");
            MecanumDrive& drive = dynamics.mecanum;
 
            // l1/l2 are the *half* spacings, matching Doroftei et al. and ARMAR-6's
            // platformHalfWidth / platformHalfHeight.
            drive.kinematics.l1 = jWheel.at("gauge").get<float>();
            drive.kinematics.l2 = jWheel.at("wheelbase").get<float>();
            drive.kinematics.R = jWheel.at("wheelRadius").get<float>();
 
            drive.maxWheelVelocity = jWheel.value("maxWheelVelocity", drive.maxWheelVelocity);
            drive.maxWheelAcceleration =
                jWheel.value("maxWheelAcceleration", drive.maxWheelAcceleration);
            drive.maxWheelDeceleration =
                jWheel.value("maxWheelDeceleration", drive.maxWheelDeceleration);
 
            ARMARX_CHECK_GREATER(drive.kinematics.R, 0.F);
            ARMARX_CHECK_GREATER(drive.maxWheelAcceleration, 0.F);
            ARMARX_CHECK_GREATER(drive.maxWheelDeceleration, 0.F);
        }
 
        // The drive-train block is wheel-count agnostic, so both platforms share the struct.
        // Mecanum robots name it `mecanumTorque` for symmetry with their geometry block.
        const std::string torqueKey =
            j.contains("mecanumTorque") ? "mecanumTorque" : "omniWheelTorque";
 
        if (j.contains(torqueKey))
        {
            const nlohmann::json& jTorque = j.at(torqueKey);
            OmniWheelTorqueDrive& drive = dynamics.omniWheelTorque;
 
            drive.gearRatio = jTorque.value("gearRatio", drive.gearRatio);
            drive.motorMaxTorque = jTorque.value("motorMaxTorque", drive.motorMaxTorque);
            drive.motorRatedTorque = jTorque.value("motorRatedTorque", drive.motorRatedTorque);
            drive.motorNominalSpeedRpm =
                jTorque.value("motorNominalSpeedRpm", drive.motorNominalSpeedRpm);
            drive.gearboxMaxInputSpeedRpm =
                jTorque.value("gearboxMaxInputSpeedRpm", drive.gearboxMaxInputSpeedRpm);
            drive.gearboxEfficiency =
                jTorque.value("gearboxEfficiency", drive.gearboxEfficiency);
            drive.maxWheelVelocity = jTorque.value("maxWheelVelocity", drive.maxWheelVelocity);
 
            // A derating rather than a hardware property: it exists so a platform whose real
            // torque is unknown or implausible can be made to behave like a known one. See
            // PlatformDynamicsArmarDE.json.
            drive.motorMaxTorque *= jTorque.value("torqueFraction", 1.F);
 
            ARMARX_CHECK_GREATER(drive.gearRatio, 0.F);
            ARMARX_CHECK_GREATER(drive.motorMaxTorque, 0.F);
            ARMARX_CHECK_GREATER(drive.gearboxEfficiency, 0.F);
 
            if (jTorque.contains("robot"))
            {
                const nlohmann::json& jRobot = jTorque.at("robot");
                drive.robot.robotPackage = jRobot.value("package", drive.robot.robotPackage);
                drive.robot.robotFile = jRobot.value("file", drive.robot.robotFile);
                drive.robot.nodeSet = jRobot.value("nodeSet", drive.robot.nodeSet);
                drive.robot.configuration =
                    jRobot.value("configuration", drive.robot.configuration);
            }
        }
 
        if (dynamics.model == PlatformModel::OmniWheelTorque)
        {
            ARMARX_INFO << "Simulated platform: omni-wheel drive with per-motor torque limit "
                        << dynamics.omniWheelTorque.motorMaxTorque << " N m, gear "
                        << dynamics.omniWheelTorque.gearRatio << ":1.";
        }
        else if (dynamics.model == PlatformModel::OmniWheelVelocity)
        {
            ARMARX_INFO << "Simulated platform: omni-wheel drive, per-wheel limits "
                        << dynamics.omniWheel.maxWheelAcceleration << " / "
                        << dynamics.omniWheel.maxWheelDeceleration << " rev/s^2.";
        }
        else
        {
            ARMARX_INFO << "Simulated platform: Cartesian limits "
                        << dynamics.maxLinearAcceleration << " mm/s^2, "
                        << dynamics.maxAngularAcceleration << " rad/s^2.";
        }
 
        return dynamics;
    }
 
    namespace
    {
 
        float
        yawOf(const core::Pose& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
 
        /**
         * @brief One axis of the platform device's command ramp.
         *
         * Ported from `LinearLimitedAccelerationController::update` in
         * `armar7_omni/joint_controller/Velocity.cpp`, including its quirks: acceleration and
         * deceleration are distinguished by *magnitude* (the sign flip mirrors the problem for
         * negative velocities), and standstill takes the deceleration branch because the test
         * is `currentValue <= 0`.
         */
        class AxisRateLimit
        {
        public:
            AxisRateLimit(const float maxVelocity,
                          const float maxAcceleration,
                          const float maxDeceleration) :
                maxVelocity_(maxVelocity),
                maxAcceleration_(maxAcceleration),
                maxDeceleration_(maxDeceleration)
            {
            }
 
            float
            update(const float value, const float dt)
            {
                float delta = value - current_;
                float sign = 1.F;
 
                if (current_ <= 0.F)
                {
                    delta = -delta;
                    sign = -1.F;
                }
 
                delta = delta < 0.F ? -std::min(maxDeceleration_ * dt, -delta)
                                    : std::min(maxAcceleration_ * dt, delta);
 
                current_ = std::clamp(current_ + delta * sign, -maxVelocity_, maxVelocity_);
 
                return current_;
            }
 
        private:
            float maxVelocity_;
 
            float maxAcceleration_;
 
            float maxDeceleration_;
 
            /// Persistent, exactly as on the device: the ramp resumes from the last command.
            float current_{0.F};
        };
 
        /// Port of `PlanarLimitedAccelerationController` from the `armar7_omni` device.
        class PlanarRateLimit
        {
        public:
            PlanarRateLimit(const float maxVelocity,
                            const float maxAcceleration,
                            const float maxDeceleration) :
                maxVelocity_(maxVelocity),
                maxAcceleration_(maxAcceleration),
                maxDeceleration_(maxDeceleration)
            {
            }
 
            Eigen::Vector2f
            update(const Eigen::Vector2f& value, const float dt)
            {
                const Eigen::Vector2f target = limitNorm(value);
                const Eigen::Vector2f delta = target - current_;
                const float distance = delta.norm();
 
                if (distance > 0.F)
                {
                    const float bound =
                        (target.norm() >= current_.norm() ? maxAcceleration_ : maxDeceleration_) *
                        dt;
 
                    current_ += delta * std::min(1.F, bound / distance);
                }
 
                current_ = limitNorm(current_);
 
                return current_;
            }
 
        private:
            Eigen::Vector2f
            limitNorm(const Eigen::Vector2f& v) const
            {
                const float norm = v.norm();
 
                return norm > maxVelocity_ ? Eigen::Vector2f{v * (maxVelocity_ / norm)} : v;
            }
 
            float maxVelocity_;
 
            float maxAcceleration_;
 
            float maxDeceleration_;
 
            Eigen::Vector2f current_{Eigen::Vector2f::Zero()};
        };
 
        core::Pose
        poseFrom(const Eigen::Vector2f& position, const float yaw)
        {
            core::Pose pose = core::Pose::Identity();
            pose.translation() << position.x(), position.y(), 0.F;
            pose.linear() = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
            return pose;
        }
 
    } // namespace
 
    TrajectoryFollowingSimulation::TrajectoryFollowingSimulation(const Parameters& params) :
        params_(params)
    {
        ARMARX_CHECK_GREATER(params_.dt, 0.F);
        ARMARX_CHECK_GREATER(params_.maxDuration, 0.F);
    }
 
    TrajectoryFollowingSimulation::Result
    TrajectoryFollowingSimulation::run(const core::GlobalTrajectory& trajectory,
                                       const core::Pose& start) const
    {
        ARMARX_CHECK(not trajectory.points().empty());
 
        // The controller is stateful (PID terms and the trajectory projection index), so a single
        // instance has to be used for the whole run, exactly as the real controller does.
        traj_ctrl::global::TrajectoryFollowingController controller(params_.controller);
 
        const Eigen::Vector2f goalPosition =
            trajectory.points().back().waypoint.pose.translation().head<2>();
 
        // Direction of the last segment, used to tell "still approaching" from "past the end".
        const Eigen::Vector2f goalDirection =
            (goalPosition -
             trajectory.points().at(trajectory.points().size() - 2).waypoint.pose.translation()
                 .head<2>())
                .normalized();
 
        std::size_t settledSteps = 0;
 
        Eigen::Vector2f position = start.translation().head<2>();
        float yaw = yawOf(start);
 
        // Mirrors `Twist2D filteredTwist` of the platform controller, including its zero
        // initialization in `rtPreActivateController()`.
        Eigen::Vector2f filteredLinear = Eigen::Vector2f::Zero();
        float filteredAngular = 0.F;
 
        Eigen::Vector2f previousVelocityGlobal = Eigen::Vector2f::Zero();
        float previousAngularVelocity = 0.F;
 
        // The device ramp runs in the RT loop (1 kHz), the controller in its additional task
        // (10 ms). Step the ramp at its own rate, otherwise it is an order of magnitude coarser
        // than the real one.
        const CommandRateLimit& rateLimit = params_.rateLimit;
        AxisRateLimit rampX(
            rateLimit.maxVelocity, rateLimit.maxAcceleration, rateLimit.maxDeceleration);
        AxisRateLimit rampY(
            rateLimit.maxVelocity, rateLimit.maxAcceleration, rateLimit.maxDeceleration);
        PlanarRateLimit rampLinear(
            rateLimit.maxVelocity, rateLimit.maxAcceleration, rateLimit.maxDeceleration);
        AxisRateLimit rampYaw(rateLimit.maxAngularVelocity,
                              rateLimit.maxAngularAcceleration,
                              rateLimit.maxAngularDeceleration);
 
        const auto rampSubSteps =
            std::max<std::size_t>(1, static_cast<std::size_t>(std::lround(
                                         params_.dt / std::max(rateLimit.dt, 1e-6F))));
 
        if (rateLimit.enabled)
        {
            ARMARX_INFO << "Modelling the device command ramp: " << rateLimit.maxAcceleration
                        << " / " << rateLimit.maxDeceleration << " mm/s^2, "
                        << rateLimit.maxAngularAcceleration << " / "
                        << rateLimit.maxAngularDeceleration << " rad/s^2, " << rampSubSteps
                        << " sub-steps of " << rateLimit.dt << " s per control cycle, "
                        << (rateLimit.directionPreserving ? "direction-preserving"
                                                          : "per-axis")
                        << ".";
        }
        else
        {
            ARMARX_INFO << "Device command ramp disabled: the raw controller twist goes "
                          "straight to the platform.";
        }
 
        // What the mass is actually doing, as opposed to what it was told to do.
        Eigen::Vector2f velocityGlobal = Eigen::Vector2f::Zero();
        float angularVelocity = 0.F;
 
        const float maxDeltaV = params_.dynamics.maxLinearAcceleration * params_.dt;
        const float maxDeltaOmega = params_.dynamics.maxAngularAcceleration * params_.dt;
 
        const bool usesWheels = params_.dynamics.model != PlatformModel::Cartesian;
        const bool usesMecanum = params_.dynamics.model == PlatformModel::MecanumTorque;
 
        // Both drives reduce to the same pair of matrices, so nothing below this point needs to
        // know how many wheels there are or which platform it is.
        //
        //   jacobianMM        (3, N)  wheel -> [mm/s, mm/s, rad/s]
        //   inverseJacobianMM (N, 3)  [mm/s, mm/s, rad/s] -> wheel
        //
        // Simox supplies both directions explicitly for the mecanum drive (J and J_inv, which
        // satisfy J * J_inv = I), so no pseudo-inverse is involved even though the matrices are
        // not square. The omni drive's C() is square and its inverse is the inverse model.
        //
        // Unit conventions differ between the two and are absorbed here: C() takes wheel
        // **rev/s**, J() takes **rad/s**.
        Eigen::MatrixXf jacobianMM;
        Eigen::MatrixXf inverseJacobianMM;
 
        if (usesMecanum)
        {
            jacobianMM = params_.dynamics.mecanum.kinematics.J();
            inverseJacobianMM = params_.dynamics.mecanum.kinematics.J_inv();
        }
        else
        {
            jacobianMM = params_.dynamics.omniWheel.kinematics.C();
            inverseJacobianMM = jacobianMM.inverse();
        }
 
        const Eigen::Index wheelCount = jacobianMM.cols();
 
        const float maxWheelVelocity = usesMecanum
                                           ? params_.dynamics.mecanum.maxWheelVelocity
                                           : params_.dynamics.omniWheel.maxWheelVelocity;
        Eigen::VectorXf wheelVelocities = Eigen::VectorXf::Zero(wheelCount);
 
        // --- Torque model setup -------------------------------------------------------------
        const OmniWheelTorqueDrive& drive = params_.dynamics.omniWheelTorque;
        const bool usesTorque = params_.dynamics.model == PlatformModel::OmniWheelTorque or
                                params_.dynamics.model == PlatformModel::MecanumTorque;
 
        std::optional<PlatformInertia> inertia;
        Eigen::MatrixXd wheelJacobianT = Eigen::MatrixXd::Identity(3, 3);
        Eigen::MatrixXd inverseWheelJacobianT = Eigen::MatrixXd::Identity(3, 3);
        float maxWheelSpeedFromDrive = 0.F;
 
        if (usesTorque)
        {
            inertia.emplace(drive.robot);
 
            // Promote both matrices to SI: wheel rate -> body twist [m/s, m/s, rad/s]. Only the
            // two linear rows carry the mm->m factor. The omni drive additionally needs the
            // 2*pi that turns its rev/s into rad/s; the mecanum drive is already in rad/s.
            const Eigen::Matrix3d mmToM = Eigen::Vector3d(1e-3, 1e-3, 1.0).asDiagonal();
            const double toRadians = usesMecanum ? 1.0 : (2.0 * M_PI);
 
            const Eigen::MatrixXd jacobianSI =
                mmToM * jacobianMM.cast<double>() / toRadians;
            const Eigen::MatrixXd inverseJacobianSI =
                inverseJacobianMM.cast<double>() * mmToM.inverse() * toRadians;
 
            // Virtual work: tau_wheel = J^T F_body, and dually F_body = J_inv^T tau_wheel.
            // Taking the second from `inverseJacobianSI` rather than inverting the first is what
            // makes this work for a non-square (four-wheel) drive.
            wheelJacobianT = jacobianSI.transpose();
            inverseWheelJacobianT = inverseJacobianSI.transpose();
 
            // The gearbox input rating binds before the motor on ARMAR-7, so take the min
            // rather than assuming which one is lower.
            const float inputSpeedRpm =
                std::min(drive.motorNominalSpeedRpm, drive.gearboxMaxInputSpeedRpm);
            // rpm at the gearbox input -> wheel speed in the drive's *native* unit. The
            // division yields rev/s, which is already what the omni model wants; the mecanum
            // model works in rad/s and needs the 2*pi. Note this is the reciprocal of the
            // `toRadians` used for the Jacobian above -- that one converts the wheel unit *into*
            // rad/s, this one converts *out of* rev/s.
            const double toNativeWheelUnit = usesMecanum ? (2.0 * M_PI) : 1.0;
            maxWheelSpeedFromDrive = inputSpeedRpm / 60.F / drive.gearRatio *
                                     static_cast<float>(toNativeWheelUnit);
 
            ARMARX_IMPORTANT << (usesMecanum ? "Mecanum" : "Omni-wheel") << " torque model: "
                             << wheelCount << " wheels, gear " << drive.gearRatio << ":1, "
                             << drive.motorMaxTorque << " N m per motor, wheel speed limit "
                             << maxWheelSpeedFromDrive << (usesMecanum ? " rad/s (" : " rev/s (")
                             << inputSpeedRpm << " rpm at the gearbox input).";
        }
 
        Result result;
 
        if (inertia.has_value())
        {
            result.massMatrix = inertia->massMatrix(Eigen::Vector3d::Zero());
            result.platformMass = static_cast<float>(result.massMatrix(0, 0));
            result.platformYawInertia = static_cast<float>(result.massMatrix(2, 2));
        }
 
        const auto steps = static_cast<std::size_t>(params_.maxDuration / params_.dt);
        result.samples.reserve(steps);
 
        for (std::size_t step = 0; step < steps; step++)
        {
            const float time = static_cast<float>(step) * params_.dt;
            const core::Pose global_T_robot = poseFrom(position, yaw);
 
            const traj_ctrl::global::TrajectoryControllerResult controlResult =
                controller.control(trajectory, global_T_robot);
 
            // Low-pass filter, as in `Controller::additionalTask()`.
            const float alpha = params_.controller.alpha;
            filteredLinear =
                alpha * filteredLinear + (1.F - alpha) * controlResult.twist.linear.head<2>();
            filteredAngular =
                alpha * filteredAngular + (1.F - alpha) * controlResult.twist.angular.z();
 
            // The device ramps the command per Cartesian axis before the inverse kinematics.
            // The controller holds its output for a whole control cycle, so the ramp sees the
            // same target for every sub-step.
            if (rateLimit.enabled)
            {
                const Eigen::Vector2f targetLinear = filteredLinear;
                const float targetAngular = filteredAngular;
 
                for (std::size_t sub = 0; sub < rampSubSteps; sub++)
                {
                    if (rateLimit.directionPreserving)
                    {
                        filteredLinear = rampLinear.update(targetLinear, rateLimit.dt);
                    }
                    else
                    {
                        filteredLinear.x() = rampX.update(targetLinear.x(), rateLimit.dt);
                        filteredLinear.y() = rampY.update(targetLinear.y(), rateLimit.dt);
                    }
 
                    filteredAngular = rampYaw.update(targetAngular, rateLimit.dt);
                }
            }
 
            // The controller returns a twist in the robot frame; the holonomic platform executes
            // it there. Rotate it into the global frame to integrate the position.
            const Eigen::Rotation2Df global_R_robot(yaw);
            const Eigen::Vector2f commandedVelocityGlobal = global_R_robot * filteredLinear;
 
            // What matching the command within one cycle would take, ignoring any limit.
            const Eigen::Vector2f deltaV = commandedVelocityGlobal - velocityGlobal;
            const float deltaOmega = filteredAngular - angularVelocity;
 
            Sample sample;
 
            // Sized up front: the Cartesian model never touches these, and an unsized VectorXf
            // would make every consumer's element access an out-of-range Eigen assertion.
            sample.commandedWheelVelocities = Eigen::VectorXf::Zero(wheelCount);
            sample.wheelVelocities = Eigen::VectorXf::Zero(wheelCount);
            sample.requiredWheelAccelerations = Eigen::VectorXf::Zero(wheelCount);
            sample.requiredMotorTorques = Eigen::VectorXf::Zero(wheelCount);
            sample.appliedMotorTorques = Eigen::VectorXf::Zero(wheelCount);
            sample.wheelSaturated = Eigen::ArrayXi::Zero(wheelCount);
            sample.motorSaturated = Eigen::ArrayXi::Zero(wheelCount);
            sample.wheelSpeedSaturated = Eigen::ArrayXi::Zero(wheelCount);
            sample.requiredLinearAcceleration = deltaV.norm() / params_.dt;
            sample.requiredAngularAcceleration = std::abs(deltaOmega) / params_.dt;
 
            if (usesTorque)
            {
                // Everything here is SI; the rest of the simulation works in mm.
                constexpr double mmToM = 1e-3;
 
                const Eigen::Vector3d q{position.x() * mmToM, position.y() * mmToM, yaw};
                const Eigen::Vector3d qdot{velocityGlobal.x() * mmToM,
                                           velocityGlobal.y() * mmToM,
                                           angularVelocity};
 
                const Eigen::Vector3d desiredAcceleration{deltaV.x() * mmToM / params_.dt,
                                                          deltaV.y() * mmToM / params_.dt,
                                                          deltaOmega / params_.dt};
 
                const Eigen::Matrix3d massMatrix = inertia->massMatrix(q);
                const Eigen::Vector3d biasForce = inertia->bias(q, qdot);
 
                // Generalized force needed to follow the command exactly, in the world frame.
                const Eigen::Vector3d requiredWrenchWorld =
                    massMatrix * desiredAcceleration + biasForce;
 
                const Eigen::Rotation2Dd worldFromRobot(yaw);
                Eigen::Vector3d requiredWrenchBody;
                requiredWrenchBody.head<2>() =
                    worldFromRobot.inverse() * requiredWrenchWorld.head<2>();
                requiredWrenchBody.z() = requiredWrenchWorld.z();
 
                const Eigen::VectorXd requiredWheelTorque = wheelJacobianT * requiredWrenchBody;
                const Eigen::VectorXd requiredMotorTorque =
                    requiredWheelTorque / (drive.gearRatio * drive.gearboxEfficiency);
 
                // Each motor saturates on its own. Clipping them independently is what rotates
                // the achieved acceleration away from the commanded direction.
                Eigen::VectorXd appliedMotorTorque = requiredMotorTorque;
                for (Eigen::Index motor = 0; motor < wheelCount; motor++)
                {
                    if (std::abs(appliedMotorTorque[motor]) > drive.motorMaxTorque)
                    {
                        appliedMotorTorque[motor] =
                            std::copysign(drive.motorMaxTorque, appliedMotorTorque[motor]);
                        sample.motorSaturated[motor] = 1;
                    }
                }
 
                const Eigen::Vector3d appliedWrenchBody =
                    inverseWheelJacobianT *
                    (appliedMotorTorque * drive.gearRatio * drive.gearboxEfficiency);
 
                Eigen::Vector3d appliedWrenchWorld;
                appliedWrenchWorld.head<2>() = worldFromRobot * appliedWrenchBody.head<2>();
                appliedWrenchWorld.z() = appliedWrenchBody.z();
 
                const Eigen::Vector3d appliedAcceleration =
                    massMatrix.ldlt().solve(appliedWrenchWorld - biasForce);
 
                velocityGlobal.x() +=
                    static_cast<float>(appliedAcceleration.x() / mmToM * params_.dt);
                velocityGlobal.y() +=
                    static_cast<float>(appliedAcceleration.y() / mmToM * params_.dt);
                angularVelocity += static_cast<float>(appliedAcceleration.z() * params_.dt);
 
                // Wheel speed bound, applied in wheel space so that one slow wheel also bends
                // the achieved twist rather than merely scaling it.
                Eigen::Vector3f bodyTwist;
                bodyTwist.head<2>() = Eigen::Rotation2Df(yaw).inverse() * velocityGlobal;
                bodyTwist.z() = angularVelocity;
 
                Eigen::VectorXf wheels = inverseJacobianMM * bodyTwist;
                bool speedClamped = false;
                for (Eigen::Index wheel = 0; wheel < wheelCount; wheel++)
                {
                    if (std::abs(wheels[wheel]) > maxWheelSpeedFromDrive)
                    {
                        wheels[wheel] =
                            std::copysign(maxWheelSpeedFromDrive, wheels[wheel]);
                        sample.wheelSpeedSaturated[wheel] = 1;
                        speedClamped = true;
                    }
                }
 
                if (speedClamped)
                {
                    const Eigen::Vector3f clampedTwist =
                        jacobianMM * wheels;
                    velocityGlobal = Eigen::Rotation2Df(yaw) * clampedTwist.head<2>();
                    angularVelocity = clampedTwist.z();
                }
 
                wheelVelocities = wheels;
 
                sample.requiredMotorTorques = requiredMotorTorque.cast<float>();
                sample.appliedMotorTorques = appliedMotorTorque.cast<float>();
                sample.commandedWheelVelocities =
                    inverseJacobianMM * (Eigen::Vector3f{
                        filteredLinear.x(), filteredLinear.y(), filteredAngular});
                sample.wheelVelocities = wheels;
                sample.accelerationSaturated =
                    sample.motorSaturated.any() or sample.wheelSpeedSaturated.any();
 
                result.maxRequiredMotorTorque =
                    std::max(result.maxRequiredMotorTorque,
                             static_cast<float>(requiredMotorTorque.cwiseAbs().maxCoeff()));
                result.motorTorqueSaturatedCycles += sample.motorSaturated.any() ? 1 : 0;
                result.wheelSpeedSaturatedCycles += sample.wheelSpeedSaturated.any() ? 1 : 0;
            }
            else if (params_.dynamics.model == PlatformModel::OmniWheelVelocity)
            {
                // Work in wheel space: a single wheel that cannot keep up changes the direction
                // of the achieved motion, not only its magnitude.
                const Eigen::Vector3f commandedTwistLocal{
                    filteredLinear.x(), filteredLinear.y(), filteredAngular};
                const Eigen::VectorXf commandedWheels =
                    inverseJacobianMM * commandedTwistLocal;
 
                Eigen::VectorXf appliedDeltaW = commandedWheels - wheelVelocities;
                sample.requiredWheelAccelerations = appliedDeltaW / params_.dt;
 
                for (Eigen::Index wheel = 0; wheel < wheelCount; wheel++)
                {
                    // Slowing a wheel down is limited by the deceleration bound, speeding it up
                    // by the acceleration bound.
                    const bool decelerating = std::abs(commandedWheels[wheel]) <
                                              std::abs(wheelVelocities[wheel]);
                    const float bound =
                        (decelerating ? (usesMecanum
                                             ? params_.dynamics.mecanum.maxWheelDeceleration
                                             : params_.dynamics.omniWheel.maxWheelDeceleration)
                                      : (usesMecanum
                                             ? params_.dynamics.mecanum.maxWheelAcceleration
                                             : params_.dynamics.omniWheel.maxWheelAcceleration)) *
                        params_.dt;
 
                    if (std::abs(appliedDeltaW[wheel]) > bound)
                    {
                        appliedDeltaW[wheel] = std::copysign(bound, appliedDeltaW[wheel]);
                        sample.wheelSaturated[wheel] = 1;
                    }
                }
 
                wheelVelocities += appliedDeltaW;
                wheelVelocities = wheelVelocities.cwiseMax(-maxWheelVelocity)
                                      .cwiseMin(maxWheelVelocity);
 
                const Eigen::Vector3f achievedTwistLocal =
                    jacobianMM * wheelVelocities;
 
                velocityGlobal = global_R_robot * achievedTwistLocal.head<2>();
                angularVelocity = achievedTwistLocal.z();
 
                sample.commandedWheelVelocities = commandedWheels;
                sample.wheelVelocities = wheelVelocities;
                sample.accelerationSaturated = sample.wheelSaturated.any();
            }
            else
            {
                const bool linearSaturated = deltaV.norm() > maxDeltaV;
                const bool angularSaturated = std::abs(deltaOmega) > maxDeltaOmega;
 
                // Follow the command only as fast as the mass allows.
                const Eigen::Vector2f appliedDeltaV =
                    linearSaturated ? (deltaV.normalized() * maxDeltaV).eval() : deltaV;
                const float appliedDeltaOmega =
                    std::clamp(deltaOmega, -maxDeltaOmega, maxDeltaOmega);
 
                velocityGlobal += appliedDeltaV;
                angularVelocity += appliedDeltaOmega;
 
                sample.accelerationSaturated = linearSaturated or angularSaturated;
            }
 
            sample.time = time;
            sample.global_T_robot = global_T_robot;
            sample.commandedLinearLocal = filteredLinear;
            sample.commandedAngular = filteredAngular;
            sample.commandedVelocityGlobal = commandedVelocityGlobal;
            sample.velocityGlobal = velocityGlobal;
            sample.speed = velocityGlobal.norm();
            // What the base actually delivered this cycle, however it was limited.
            sample.linearAcceleration =
                (velocityGlobal - previousVelocityGlobal).norm() / params_.dt;
            sample.angularAcceleration =
                std::abs(angularVelocity - previousAngularVelocity) / params_.dt;
            sample.dropPointVelocity = controlResult.dropPoint.velocity;
            sample.trackingError =
                (controlResult.dropPoint.waypoint.pose.translation().head<2>() - position).norm();
            sample.distanceToGoal = controlResult.positionError;
            sample.orientationError = controlResult.orientationError;
 
            result.maxRequiredLinearAcceleration = std::max(
                result.maxRequiredLinearAcceleration, sample.requiredLinearAcceleration);
            result.maxRequiredAngularAcceleration = std::max(
                result.maxRequiredAngularAcceleration, sample.requiredAngularAcceleration);
            result.maxTrackingError = std::max(result.maxTrackingError, sample.trackingError);
            result.saturatedCycles += sample.accelerationSaturated ? 1 : 0;
 
            result.samples.push_back(sample);
 
            previousVelocityGlobal = velocityGlobal;
            previousAngularVelocity = angularVelocity;
 
            // Integrate the achieved velocity, not the commanded one.
            position += velocityGlobal * params_.dt;
            yaw += angularVelocity * params_.dt;
 
            const float distanceToGoal = (position - goalPosition).norm();
            result.finalDistanceToGoal = distanceToGoal;
 
            // Only count it as overshoot once the robot is past the end of the path, not while
            // it is still approaching from the start.
            if ((position - goalPosition).dot(goalDirection) > 0.F)
            {
                result.maxGoalOvershoot = std::max(result.maxGoalOvershoot, distanceToGoal);
            }
 
            if (distanceToGoal < params_.goalDistanceThreshold)
            {
                result.reachedGoal = true;
                break;
            }
 
            // The final segment is a pure P law, so the robot can converge to a standstill just
            // outside the threshold and creep there for the rest of maxDuration.
            settledSteps = (velocityGlobal.norm() < params_.settledSpeedThreshold) ? settledSteps + 1 : 0;
 
            if (static_cast<float>(settledSteps) * params_.dt > params_.settledDuration)
            {
                result.settledShortOfGoal = true;
                break;
            }
        }
 
        if (result.settledShortOfGoal)
        {
            ARMARX_WARNING << "Simulation stopped: the robot came to rest "
                           << result.finalDistanceToGoal << " mm from the goal, outside the "
                           << params_.goalDistanceThreshold << " mm threshold.";
        }
        else if (not result.reachedGoal)
        {
            ARMARX_WARNING << "Simulation did not reach the goal within " << params_.maxDuration
                           << " s. Final distance to goal: " << result.finalDistanceToGoal
                           << " mm.";
        }
 
        if (result.maxGoalOvershoot > params_.goalDistanceThreshold)
        {
            ARMARX_WARNING << "Overshot the end of the path by up to " << result.maxGoalOvershoot
                           << " mm.";
        }
 
        ARMARX_INFO << "Simulated " << result.samples.size() << " control cycles ("
                    << (static_cast<float>(result.samples.size()) * params_.dt)
                    << " s). Peak required acceleration: "
                    << result.maxRequiredLinearAcceleration << " mm/s^2 (limit "
                    << params_.dynamics.maxLinearAcceleration << "), "
                    << result.maxRequiredAngularAcceleration << " rad/s^2 (limit "
                    << params_.dynamics.maxAngularAcceleration << "). Saturated in "
                    << result.saturatedCycles << " of " << result.samples.size()
                    << " cycles. Peak tracking error: " << result.maxTrackingError << " mm.";
 
        return result;
    }
 
} // namespace armarx::navigation::simulation