Toppra.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 <filesystem>
#include <memory>
#include <string>
 
#include <Eigen/Core>
 
#include <armarx/navigation/algorithms/parametrization/TrajectoryParametrization.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
 
namespace armarx::navigation::algorithms
{
 
    /**
     * @brief Time-optimal reparametrization under per-motor torque and command-ramp limits.
     *
     * Delegates to `armarx_navigation.reparametrization` through an embedded Python
     * interpreter. The robot model and the wheel Jacobian are loaded once, when this object is
     * constructed; each `apply()` then exchanges two arrays with it and nothing else.
     *
     * The header is deliberately not guarded by the build-time availability macro, so the class
     * exists whether or not the python side was found. When it was not, the constructor
     * succeeds and `apply()` throws with the reason recorded at configure time -- that keeps
     * the factory free of preprocessor branches.
     *
     * @note Called once per global path segment, not per control cycle. The solve takes on the
     *       order of a second and holds the GIL; do not move it onto a control path.
     */
    class Toppra : public TrajectoryParametrization
    {
    public:
        /// Column layout of `lastSamples()`. Mirrors `SampleColumn` in the python package,
        /// which is the single source of truth for the array boundary.
        enum SampleColumn
        {
            T = 0,
            X = 1,
            Y = 2,
            YAW = 3,
            VELOCITY = 4,
            ANGULAR_VELOCITY = 5,
            TANGENTIAL_ACCELERATION = 6,
            ANGULAR_ACCELERATION = 7,
            SAMPLE_COLUMN_COUNT = 8
        };
 
        /// Which drive the platform has. Selects how the geometry below is read, and is not
        /// inferable from the geometry itself -- a mecanum robot simply leaves the omni-wheel
        /// fields at zero, which would otherwise reach the solver as a degenerate Jacobian.
        enum class DriveType
        {
            OmniWheel,
            Mecanum
        };
 
        /// ARMAR-7's three-wheel drive, in the units `VirtualRobot::OmniWheelPlatformKinematics`
        /// uses: mm for the radii, rad for the angles.
        struct OmniWheelGeometry
        {
            float bodyRadius{0.F};
            float wheelRadius{0.F};
            float delta{0.F};
            float relativeAngle{0.F};
            float gearRatio{1.F};
            Eigen::Vector3i invertWheel{Eigen::Vector3i::Zero()};
        };
 
        /// ARMAR-6's and ARMAR-DE's four-wheel drive, following
        /// `VirtualRobot::MecanumPlatformKinematics`. All lengths in mm.
        struct MecanumGeometry
        {
            /// Half the lateral wheel spacing (`l1`).
            float gauge{0.F};
 
            /// Half the longitudinal wheel spacing (`l2`).
            float wheelbase{0.F};
 
            float wheelRadius{0.F};
        };
 
        /// Everything the python side needs that does not change between paths.
        struct DriveParams
        {
            /// Robot model the inverse dynamics is built from.
            std::string robotFile;
            std::string nodeSet;
            std::string configuration;
 
            /// Selects which of the two geometries below is used.
            DriveType driveType{DriveType::OmniWheel};
            OmniWheelGeometry omniWheel;
            MecanumGeometry mecanum;
 
            /// Drive train.
            float motorMaxTorque{0.F};
            float gearRatio{1.F};
            float gearboxEfficiency{1.F};
            float torqueFraction{1.F};
 
            /// Motor/gearbox speed rating referred to the wheel [rad/s], always in rad/s
            /// regardless of the drive's own unit convention. Bounds each wheel individually,
            /// which the Cartesian cap does not imply: wheel rate depends on the direction of
            /// travel, so a mecanum diagonal needs up to sqrt(2) times a straight run's.
            float maxWheelVelocity{std::numeric_limits<float>::infinity()};
 
            /// Device command ramp. Infinite leaves the profile bounded by torque alone.
            float maxAcceleration{std::numeric_limits<float>::infinity()};
            float maxDeceleration{std::numeric_limits<float>::infinity()};
            float maxAngularAcceleration{std::numeric_limits<float>::infinity()};
 
            /// Waypoints the result should have. 0 keeps the input count.
            int numSamples{500};
        };
 
        Toppra(const core::GeneralConfig& config, const DriveParams& drive);
        ~Toppra() override;
 
        void apply(core::GlobalTrajectory& trajectory, float startVelocity) const override;
 
        /// Whether the python side was found at configure time. False means `apply()` throws.
        static bool available();
 
        /// Why it is unavailable, as recorded at configure time. Empty when available.
        static std::string unavailableReason();
 
        /**
         * @brief The time-parametrized profile from the most recent `apply()`, `(M, 8)`.
         *
         * Introspection only -- for plots and offline analysis, never for control. `apply()`
         * hands back a `GlobalTrajectory`, which stores a speed per *position*, so the time
         * axis TOPP-RA computed is lost the moment the result is converted. This keeps it.
         *
         * Empty until `apply()` has run; overwritten by each call. Columns are `SampleColumn`.
         */
        const Eigen::MatrixXd& lastSamples() const;
 
        /// Duration [s] of the profile behind `lastSamples()`. Zero until `apply()` has run.
        double lastDuration() const;
 
    private:
        class Impl;
        std::unique_ptr<Impl> impl_;
    };
 
    /**
     * @brief Read `Toppra::DriveParams` from a `PlatformDynamics<Robot>.json`.
     *
     * The same files the analysis application and the python benchmark read, so all three agree
     * on a platform by construction rather than by three hand-maintained copies.
     *
     * @note The ramp limits come from that file's `commandRateLimit` block, which mirrors the
     *       robot's `HardwareConfig/.../Platform.xml`. The two are not linked, so a change to the
     *       hardware config has to be reflected here by hand.
     *
     * @throws std::runtime_error if the file is missing, unreadable or lacks a required key.
     */
    Toppra::DriveParams LoadDriveParams(const std::filesystem::path& configFile);
 
    /// Resolve `config/platform/PlatformDynamics<robot>.json` inside the `armarx_navigation`
    /// package. `robot` is the platform name as the rest of the codebase spells it -- `Armar7`,
    /// `Armar6`, `ArmarDE` -- matching the analysis application's `--robot`.
    std::filesystem::path DriveParamsPath(const std::string& robot = "Armar7");
 
} // namespace armarx::navigation::algorithms