Toppra.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 "Toppra.h"
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#if NAVIGATION_TOPPRA_ENABLED
#include <pybind11/eigen.h>
#include <pybind11/embed.h>
#include <pybind11/stl.h>
 
namespace py = pybind11;
#endif
 
namespace armarx::navigation::algorithms
{
 
    namespace
    {
        float
        yawOf(const core::Pose& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
 
#if NAVIGATION_TOPPRA_ENABLED
        /**
         * @brief Brings up the one interpreter this process gets.
         *
         * `py::scoped_interpreter` may not be constructed twice, and something else in the
         * process may have started Python already, so guard on `Py_IsInitialized()` and keep the
         * guard alive for the process lifetime. `site-packages` has to be added by hand: an
         * embedded interpreter does not pick up a virtual environment on its own.
         *
         * `site.addsitedir`, not `sys.path.append`: appending a directory to `sys.path` does not
         * process the `.pth` files in it, and an editable install (`pip install -e`, which is
         * what Axii does) puts *only* a `.pth` and a finder module in site-packages -- there is
         * no package directory to find. Appending therefore leaves the package invisible with
         * "No module named 'armarx_navigation'". It happens to work when the venv is activated,
         * because then the interpreter processes the `.pth` at startup, which is exactly why
         * this hid from every test run through an activated shell and only appeared in the
         * navigator.
         */
        void
        ensureInterpreter()
        {
            static const bool initialized = []
            {
                if (Py_IsInitialized() == 0)
                {
                    // Leaked deliberately: finalizing while other holders still exist crashes.
                    new py::scoped_interpreter{};
 
                    // `scoped_interpreter` leaves the GIL held by whichever thread got here
                    // first and never gives it back. The analysis application never notices --
                    // it is the only thread there is -- but the navigator answers each request
                    // on an Ice dispatch thread, so the second request is served by a different
                    // thread, whose `gil_scoped_acquire` then waits on a lock nobody will ever
                    // release. Hand the GIL back here; every use site acquires it explicitly.
                    //
                    // Leaked for the same reason as the interpreter: this must outlive every
                    // use, and re-acquiring at process exit is exactly the finalization we are
                    // avoiding.
                    new py::gil_scoped_release{};
                }
 
                const py::gil_scoped_acquire gil;
 
                py::module_::import("site").attr("addsitedir")(
                    NAVIGATION_TOPPRA_SITE_PACKAGES);
 
                ARMARX_INFO << "Embedded Python for TOPP-RA, site-packages "
                            << NAVIGATION_TOPPRA_SITE_PACKAGES;
 
                return true;
            }();
 
            (void)initialized;
        }
#endif
    } // namespace
 
    std::filesystem::path
    DriveParamsPath(const std::string& robot)
    {
        return armarx::PackagePath("armarx_navigation",
                                   "config/platform/PlatformDynamics" + robot + ".json")
            .toSystemPath();
    }
 
    Toppra::DriveParams
    LoadDriveParams(const std::filesystem::path& configFile)
    {
        if (not std::filesystem::exists(configFile))
        {
            throw std::runtime_error("No platform dynamics config at " + configFile.string());
        }
 
        std::ifstream stream{configFile};
        if (not stream.good())
        {
            throw std::runtime_error("Cannot read platform dynamics config at " +
                                     configFile.string());
        }
 
        const nlohmann::json config = nlohmann::json::parse(stream);
 
        Toppra::DriveParams params;
 
        // Which drive the platform has is a decision, not an inference: a mecanum robot has no
        // omni geometry in its config at all, and the zero-initialised omni defaults would reach
        // the solver as a degenerate Jacobian and an "infeasible" unrelated to the path.
        const bool mecanum = config.value("model", std::string{}) == "mecanum_torque";
 
        const nlohmann::json& drive =
            mecanum ? config.at("mecanumTorque") : config.at("omniWheelTorque");
 
        if (mecanum)
        {
            const nlohmann::json& geometry = config.at("mecanum");
 
            params.driveType = Toppra::DriveType::Mecanum;
            params.mecanum.gauge = geometry.at("gauge").get<float>();
            params.mecanum.wheelbase = geometry.at("wheelbase").get<float>();
            params.mecanum.wheelRadius = geometry.at("wheelRadius").get<float>();
        }
        else
        {
            const nlohmann::json& geometry = config.at("omniWheel");
 
            // The JSON carries degrees; `OmniWheelPlatformKinematicsParams` wants radians.
            constexpr float degToRad = static_cast<float>(M_PI) / 180.F;
 
            params.driveType = Toppra::DriveType::OmniWheel;
            params.omniWheel.bodyRadius = geometry.at("bodyRadius").get<float>();
            params.omniWheel.wheelRadius = geometry.at("wheelRadius").get<float>();
            params.omniWheel.delta =
                geometry.at("angularPositionFirstWheelDegrees").get<float>() * degToRad;
            params.omniWheel.relativeAngle =
                geometry.at("relativeAngleDegrees").get<float>() * degToRad;
            params.omniWheel.gearRatio = geometry.value("gearRatio", 1.F);
 
            const std::vector<bool> invert =
                geometry.value("invertWheel", std::vector<bool>{false, false, false});
            ARMARX_CHECK_EQUAL(invert.size(), 3U);
            params.omniWheel.invertWheel =
                Eigen::Vector3i{invert[0] ? 1 : 0, invert[1] ? 1 : 0, invert[2] ? 1 : 0};
        }
 
        const nlohmann::json& robot = drive.at("robot");
        params.robotFile =
            armarx::PackagePath(robot.at("package").get<std::string>(),
                                robot.at("file").get<std::string>())
                .toSystemPath();
        params.nodeSet = robot.at("nodeSet").get<std::string>();
        params.configuration = robot.at("configuration").get<std::string>();
 
        params.motorMaxTorque = drive.at("motorMaxTorque").get<float>();
        params.gearRatio = drive.at("gearRatio").get<float>();
        params.gearboxEfficiency = drive.value("gearboxEfficiency", 1.F);
        params.torqueFraction = drive.value("torqueFraction", 1.F);
 
        // Motor/gearbox speed rating referred to the wheel, always in rad/s regardless of the
        // drive's own unit convention.
        const float inputSpeedRpm = std::min(drive.at("motorNominalSpeedRpm").get<float>(),
                                             drive.at("gearboxMaxInputSpeedRpm").get<float>());
        params.maxWheelVelocity =
            inputSpeedRpm / 60.F / params.gearRatio * 2.F * static_cast<float>(M_PI);
 
        // A measured wheel speed limit [rad/s], where the platform has one, supersedes the rating:
        // ratings can be estimates (ARMAR-DE's are, its drive datasheet is unavailable), a
        // measurement is what the wheels actually reach.
        if (drive.contains("maxWheelVelocity"))
        {
            params.maxWheelVelocity = drive.at("maxWheelVelocity").get<float>();
        }
 
        // The motors are not the only bottleneck: the device command ramp is stricter, and it is
        // what actually rate-limits the base. Without it the profile plans a stop the ramp
        // cannot execute and the robot sails past the goal.
        const nlohmann::json& ramp = config.at("commandRateLimit");
        params.maxAcceleration = ramp.at("maxAcceleration").get<float>();
        params.maxDeceleration = ramp.at("maxDeceleration").get<float>();
        params.maxAngularAcceleration = ramp.at("maxAngularAcceleration").get<float>();
        params.limitAngularAcceleration = ramp.value("limitAngularAcceleration", false);
 
        return params;
    }
 
    bool
    Toppra::available()
    {
#if NAVIGATION_TOPPRA_ENABLED
        return true;
#else
        return false;
#endif
    }
 
    std::string
    Toppra::unavailableReason()
    {
#if NAVIGATION_TOPPRA_ENABLED
        return {};
#else
        return NAVIGATION_TOPPRA_DISABLED_REASON;
#endif
    }
 
    class Toppra::Impl
    {
    public:
        Impl(const core::GeneralConfig& config, const DriveParams& drive) :
            config(config), drive(drive)
        {
        }
 
#if NAVIGATION_TOPPRA_ENABLED
        ~Impl()
        {
            if (not reparametrizer_)
            {
                return;
            }
 
            // Dropping a `py::object` decrements a Python refcount, which needs the GIL just as
            // much as calling into Python does. A navigator that is torn down and rebuilt --
            // which happens between navigation requests -- would otherwise corrupt the
            // interpreter's bookkeeping from whatever thread did the teardown, and the crash
            // would surface much later, in an unrelated call.
            const py::gil_scoped_acquire gil;
            reparametrizer_ = py::object{};
        }
#endif
 
        core::GeneralConfig config;
        DriveParams drive;
 
#if NAVIGATION_TOPPRA_ENABLED
        /// Constructed lazily: building it loads the RBDL model, which should not happen while
        /// the navigation stack is still being assembled.
        py::object&
        reparametrizer()
        {
            if (reparametrizer_)
            {
                return reparametrizer_;
            }
 
            const py::module_ module =
                py::module_::import("armarx_navigation.reparametrization");
 
            reparametrizer_ = module.attr("Reparametrizer")(
                py::arg("robot") = module.attr("RobotSpec")(
                    py::arg("file") = drive.robotFile,
                    py::arg("node_set") = drive.nodeSet,
                    py::arg("configuration") = drive.configuration),
                py::arg("kinematics") = kinematics(module),
                py::arg("limits") = module.attr("DriveLimits")(
                    py::arg("max_velocity") = config.maxVel.linear,
                    py::arg("max_angular_velocity") = config.maxVel.angular,
                    py::arg("motor_max_torque") = drive.motorMaxTorque,
                    py::arg("gear_ratio") = drive.gearRatio,
                    py::arg("gearbox_efficiency") = drive.gearboxEfficiency,
                    py::arg("torque_fraction") = drive.torqueFraction,
                    py::arg("max_wheel_velocity") = drive.maxWheelVelocity,
                    py::arg("max_acceleration") = drive.maxAcceleration,
                    py::arg("max_deceleration") = drive.maxDeceleration,
                    py::arg("max_angular_acceleration") = drive.maxAngularAcceleration,
                    py::arg("limit_angular_acceleration") = drive.limitAngularAcceleration));
 
            return reparametrizer_;
        }
 
        /// The time-parametrized profile of the most recent call, kept for introspection.
        Eigen::MatrixXd lastSamples;
        double lastDuration{0.0};
 
    private:
        /// Construct the drive-specific kinematics dataclass.
        ///
        /// Which one is a decision, not an inference: a mecanum robot has no omni geometry
        /// configured at all, and passing the zero-initialised omni defaults gives the solver a
        /// degenerate Jacobian and an "infeasible" that has nothing to do with the path.
        py::object
        kinematics(const py::module_& module) const
        {
            switch (drive.driveType)
            {
                case DriveType::Mecanum:
                    return module.attr("MecanumKinematics")(
                        py::arg("gauge") = drive.mecanum.gauge,
                        py::arg("wheelbase") = drive.mecanum.wheelbase,
                        py::arg("wheel_radius") = drive.mecanum.wheelRadius);
 
                case DriveType::OmniWheel:
                    return module.attr("OmniWheelKinematics")(
                        py::arg("body_radius") = drive.omniWheel.bodyRadius,
                        py::arg("wheel_radius") = drive.omniWheel.wheelRadius,
                        py::arg("delta") = drive.omniWheel.delta,
                        py::arg("relative_angle") = drive.omniWheel.relativeAngle,
                        py::arg("gear_ratio") = drive.omniWheel.gearRatio,
                        py::arg("invert") =
                            std::vector<bool>{drive.omniWheel.invertWheel.x() != 0,
                                              drive.omniWheel.invertWheel.y() != 0,
                                              drive.omniWheel.invertWheel.z() != 0});
 
                default:
                    throw std::runtime_error("Unknown platform drive type.");
            }
        }
 
        /// Deliberately left null rather than initialised to `py::none()`: the member
        /// initializer runs before `ensureInterpreter()`, so `py::none()` would touch Python
        /// before the interpreter exists and without the GIL. A null `py::object` is just a
        /// null pointer, and is the "not built yet" state `reparametrizer()` tests for.
        py::object reparametrizer_;
#else
    public:
        Eigen::MatrixXd lastSamples;
        double lastDuration{0.0};
#endif
    };
 
    const Eigen::MatrixXd&
    Toppra::lastSamples() const
    {
        return impl_->lastSamples;
    }
 
    double
    Toppra::lastDuration() const
    {
        return impl_->lastDuration;
    }
 
    Toppra::Toppra(const core::GeneralConfig& config, const DriveParams& drive) :
        impl_(std::make_unique<Impl>(config, drive))
    {
#if NAVIGATION_TOPPRA_ENABLED
        // Warm up here rather than on the first `apply()`. Bringing up the interpreter,
        // importing toppra/scipy and loading the RBDL model costs ~0.5 s and does not depend on
        // the path, so leaving it lazy would put all of it on the first navigation request --
        // exactly when the robot is meant to start moving. Paid once while the navigation stack
        // is being constructed it is invisible, and every request then costs ~20 ms.
        const auto started = std::chrono::steady_clock::now();
 
        ensureInterpreter();
 
        {
            const py::gil_scoped_acquire gil;
 
            try
            {
                impl_->reparametrizer();
            }
            catch (const py::error_already_set& error)
            {
                throw std::runtime_error(
                    std::string{"TOPP-RA reparametrization could not be initialised: "} +
                    error.what());
            }
        }
 
        ARMARX_INFO << "TOPP-RA ready in "
                    << std::chrono::duration<double, std::milli>(
                           std::chrono::steady_clock::now() - started)
                           .count()
                    << " ms (interpreter, imports and robot model).";
#endif
    }
 
    Toppra::~Toppra() = default;
 
    void
    Toppra::apply([[maybe_unused]] core::GlobalTrajectory& trajectory,
                  [[maybe_unused]] float startVelocity) const
    {
#if not NAVIGATION_TOPPRA_ENABLED
        throw std::runtime_error("TOPP-RA reparametrization not available because " +
                                 unavailableReason());
#else
        const std::vector<core::GlobalTrajectoryPoint>& input = trajectory.points();
 
        // [x, y, yaw, velocity_limit] -- the planner's obstacle-aware bound is carried through
        // so the reparametrization cannot speed up next to an obstacle.
        Eigen::MatrixXd waypoints(static_cast<Eigen::Index>(input.size()), 4);
        for (Eigen::Index i = 0; i < waypoints.rows(); i++)
        {
            const core::GlobalTrajectoryPoint& point = input[static_cast<std::size_t>(i)];
            const core::Position& position = point.waypoint.pose.translation();
 
            waypoints(i, 0) = position.x();
            waypoints(i, 1) = position.y();
            waypoints(i, 2) = yawOf(point.waypoint.pose);
            waypoints(i, 3) = point.velocity;
        }
 
        // Printed on stdout with the same marker the python side uses, so a caller filtering
        // for it gets one continuous breakdown. Both are one-time costs in a long-lived
        // process -- the interpreter and the imported modules outlive any single call -- so
        // they are reported separately from the solve rather than folded into it.
        const auto timed = [](const char* label, auto&& work)
        {
            const auto started = std::chrono::steady_clock::now();
            work();
            const double ms = std::chrono::duration<double, std::milli>(
                                  std::chrono::steady_clock::now() - started)
                                  .count();
 
            std::cout << "  [reparametrization] " << std::left << std::setw(32) << label
                      << std::right << std::setw(8) << std::fixed << std::setprecision(1) << ms
                      << " ms" << std::endl;
        };
 
        // Already warmed up in the constructor; this is a no-op unless something finalized the
        // interpreter behind our back.
        ensureInterpreter();
 
        const py::gil_scoped_acquire gil;
 
        Eigen::MatrixXd samples;
        double duration = 0.0;
 
        try
        {
            timed("solve + sample (total)",
                  [&]
                  {
                      const py::object result =
                          impl_->reparametrizer()(waypoints, impl_->drive.numSamples);
 
                      samples = result.attr("samples").cast<Eigen::MatrixXd>();
                      duration = result.attr("duration").cast<double>();
                  });
        }
        catch (const py::error_already_set& error)
        {
            throw std::runtime_error(std::string{"TOPP-RA reparametrization failed: "} +
                                     error.what());
        }
 
        ARMARX_CHECK_EQUAL(samples.cols(), SAMPLE_COLUMN_COUNT);
 
        if (samples.rows() < 2)
        {
            throw std::runtime_error("TOPP-RA returned fewer than 2 waypoints.");
        }
 
        // Keep the time axis before it is dropped -- see `lastSamples()`.
        impl_->lastSamples = samples;
        impl_->lastDuration = duration;
 
        std::vector<core::GlobalTrajectoryPoint> points;
        points.reserve(static_cast<std::size_t>(samples.rows()));
 
        // The velocity floor exists for one reason: a GlobalTrajectory assigns a velocity to a
        // *position*, so `v(s = 0) = 0` is a fixed point -- the controller reads zero at the pose
        // it is standing in, commands zero, and the robot never leaves the start.
        //
        // That argument is about the start and nothing else, but the floor used to be applied to
        // the whole profile, which had two consequences, both seen on ARMAR-7:
        //
        //  * at the goal the robot arrived still commanded at `boundaryVelocity` and overshot by
        //    `v^2 / 2a` once the controller switched to its final-segment law -- 45-59 mm on
        //    every recorded run;
        //  * mid-path it *overrode this solver's own answer*. Where the orientation profile turns
        //    sharply, TOPP-RA slows down so that `yaw'(s) * v` stays inside the angular limit;
        //    `max(v, boundaryVelocity)` threw that away. One recorded run had 47 consecutive
        //    samples pinned at exactly 150 mm/s across a stretch whose orientation profile then
        //    demanded 3.9 rad/s against a 0.8 rad/s limit, and the robot spent seven seconds
        //    saturated, alternating direction and crawling at the feed-forward's floor.
        //
        // So the floor is applied only to the leading stretch -- up to the first sample this
        // solver's own profile reaches it -- and the rest of the profile stands as computed. The
        // transition is continuous by construction: at that sample the two are equal. The tail
        // keeps a much smaller floor (`goalVelocity`) so the fixed-point argument still cannot
        // bite anywhere, while being far too low to override a turn-limited speed.
        const double boundaryFloor = impl_->config.boundaryVelocity;
        const double goalFloor =
            std::min<double>(impl_->config.goalVelocity, impl_->config.boundaryVelocity);
 
        Eigen::Index leadingEnd = 0;
        while (leadingEnd < samples.rows() and samples(leadingEnd, VELOCITY) < boundaryFloor)
        {
            leadingEnd++;
        }
 
        for (Eigen::Index i = 0; i < samples.rows(); i++)
        {
            core::Pose pose = core::Pose::Identity();
            pose.translation() << static_cast<float>(samples(i, X)),
                static_cast<float>(samples(i, Y)), 0.F;
            pose.linear() = Eigen::AngleAxisf(static_cast<float>(samples(i, YAW)),
                                              Eigen::Vector3f::UnitZ())
                                .toRotationMatrix();
 
            // See the floor's derivation above.
            const double floorVelocity = i <= leadingEnd ? boundaryFloor : goalFloor;
 
            points.push_back(core::GlobalTrajectoryPoint{
                .waypoint = {.pose = pose},
                .velocity = std::max(static_cast<float>(samples(i, VELOCITY)),
                                     static_cast<float>(floorVelocity))});
        }
 
        ARMARX_INFO << "TOPP-RA: " << points.size() << " waypoints, " << duration << " s.";
 
        // Rebuilt point by point rather than with `FromPath`, which derives intermediate
        // headings from the movement direction and keeps only the given start and goal poses --
        // that would discard the orientation profile the planner optimized and this carried
        // through, and put a step at each end wherever the two disagree.
        trajectory = core::GlobalTrajectory{points};
#endif
    }
 
} // namespace armarx::navigation::algorithms