reparametrization.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 "reparametrization.h"
 
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <vector>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/parametrization/Toppra.h>
#include <armarx/navigation/factories/TrajectoryParametrizationFactory.h>
 
namespace armarx::navigation::analysis
{
 
    namespace
    {
 
        /// Drive parameters for the Toppra variant, from the same platform config the
        /// simulation was built from.
        ///
        /// The loader lives in the algorithms library so that this application, the navigator and
        /// the documentation all read one file with one parser and cannot drift apart.
        algorithms::Toppra::DriveParams
        buildDriveParams(const Config& config)
        {
            algorithms::Toppra::DriveParams params =
                algorithms::LoadDriveParams(algorithms::DriveParamsPath(config.robot));
 
            // Multiplied, not assigned: the platform config may already carry a derating (0.084
            // on ARMAR-DE) and the scene's own fraction is a further reduction on top of it.
            // Assigning here would silently restore the full motor torque.
            params.torqueFraction *= config.parametrizationTorqueFraction;
 
            params.numSamples = config.parametrizationSamples;
 
            return params;
        }
 
        /// Re-expose the time parametrization for the plots, in the shape they already read.
        nlohmann::json
        toReference(const algorithms::Toppra& toppra)
        {
            using Column = algorithms::Toppra::SampleColumn;
 
            const Eigen::MatrixXd& samples = toppra.lastSamples();
 
            nlohmann::json waypoints = nlohmann::json::array();
 
            for (Eigen::Index i = 0; i < samples.rows(); i++)
            {
                waypoints.push_back(nlohmann::json{
                    {"t", samples(i, Column::T)},
                    {"x", samples(i, Column::X)},
                    {"y", samples(i, Column::Y)},
                    {"yaw", samples(i, Column::YAW)},
                    {"velocity", samples(i, Column::VELOCITY)},
                    {"angular_velocity", samples(i, Column::ANGULAR_VELOCITY)},
                    {"tangential_acceleration", samples(i, Column::TANGENTIAL_ACCELERATION)},
                    {"angular_acceleration", samples(i, Column::ANGULAR_ACCELERATION)}});
            }
 
            nlohmann::json reference;
            reference["success"] = true;
            reference["duration"] = toppra.lastDuration();
            reference["waypoints"] = waypoints;
 
            return reference;
        }
 
    } // namespace
 
    CommandRampCheck
    checkAgainstCommandRamp(
        const core::GlobalTrajectory& trajectory,
        const simulation::CommandRateLimit& rateLimit,
        const float boundaryVelocity)
    {
        CommandRampCheck check;
 
        const std::vector<core::GlobalTrajectoryPoint>& points = trajectory.points();
 
        float arcLength = 0.F;
        bool inSpan = false;
        float spanStart = 0.F;
 
        for (std::size_t i = 0; i + 1 < points.size(); i++)
        {
            const float segment = (points[i + 1].waypoint.pose.translation() -
                                   points[i].waypoint.pose.translation())
                                      .norm();
            arcLength += segment;
 
            if (segment < 1e-6F)
            {
                continue;
            }
 
            const float from = points[i].velocity;
            const float to = points[i + 1].velocity;
 
            // v dv/ds, exact for a profile that is linear in v^2 over the segment.
            const float demand = std::abs(to * to - from * from) / (2.F * segment);
            const float limit =
                to >= from ? rateLimit.maxAcceleration : rateLimit.maxDeceleration;
 
            if (demand > limit)
            {
                check.violations++;
 
                if (demand - limit > check.worstDemand - check.worstLimit)
                {
                    check.worstDemand = demand;
                    check.worstLimit = limit;
                    check.worstArcLength = arcLength;
                }
 
                if (not inSpan)
                {
                    inSpan = true;
                    spanStart = arcLength - segment;
                }
            }
            else if (inSpan)
            {
                inSpan = false;
                check.spans.emplace_back(spanStart, arcLength);
            }
        }
 
        if (inSpan)
        {
            check.spans.emplace_back(spanStart, arcLength);
        }
 
        // A profile that never decelerates passes every segment test above and still overshoots.
        const float terminalVelocity = points.back().velocity;
        check.terminalStoppingDistance =
            terminalVelocity * terminalVelocity / (2.F * rateLimit.maxDeceleration);
 
        check.terminalVelocityExceedsBoundary = terminalVelocity > boundaryVelocity;
 
        if (check.terminalVelocityExceedsBoundary)
        {
            ARMARX_WARNING << "The velocity profile ends at " << terminalVelocity
                           << " mm/s. Stopping from there at " << rateLimit.maxDeceleration
                           << " mm/s^2 needs " << check.terminalStoppingDistance
                           << " mm of path that does not exist -- the base will overshoot the "
                              "goal by roughly that much.";
        }
 
        if (check.violations > 0)
        {
            ARMARX_WARNING
                << "The velocity profile demands more than the device command ramp can "
                   "deliver at "
                << check.violations << " of " << (points.size() - 1)
                << " segments. Worst: " << check.worstDemand << " mm/s^2 against a limit of "
                << check.worstLimit << " mm/s^2, at arc length " << check.worstArcLength
                << " mm. The base will not track the profile there -- expect it to lag on the "
                   "way up and to overshoot on the way down.";
        }
 
        return check;
    }
 
    core::TrajectoryParametrization
    parametrizationModeFromString(const std::string& name)
    {
        if (name == "none")
        {
            return core::TrajectoryParametrization::None;
        }
        if (name == "ramping")
        {
            return core::TrajectoryParametrization::Ramping;
        }
        if (name == "toppra")
        {
            return core::TrajectoryParametrization::Toppra;
        }
 
        throw std::invalid_argument("Unknown parametrization mode `" + name +
                                    "`. Expected `none`, `ramping` or `toppra`.");
    }
 
    std::string
    toString(const core::TrajectoryParametrization mode)
    {
        switch (mode)
        {
            case core::TrajectoryParametrization::None:
                return "none";
            case core::TrajectoryParametrization::Ramping:
                return "ramping";
            case core::TrajectoryParametrization::Toppra:
                return "toppra";
        }
 
        return "unknown";
    }
 
    ReparametrizationResult
    reparametrize(const core::GlobalTrajectory& trajectory, const Config& config)
    {
        // The same objects the Navigator runs, built the same way, so this measures the robot's
        // behaviour rather than a second implementation of it.
        core::GeneralConfig general = config.generalConfig;
        general.parametrization = config.parametrization;
 
        // Timed separately from `apply()` below: construction warms up the interpreter, the
        // imports and the robot model, which the navigation stack pays once at startup and not
        // per request. Only the `apply()` figure is representative of a navigation request.
        const auto constructionStarted = std::chrono::steady_clock::now();
 
        const algorithms::TrajectoryParametrizationPtr parametrization =
            fac::TrajectoryParametrizationFactory::create(general, buildDriveParams(config));
 
        const double constructionSeconds =
            std::chrono::duration<double>(std::chrono::steady_clock::now() -
                                          constructionStarted)
                .count();
 
        ReparametrizationResult result{
            .trajectory = trajectory, .reference = nullptr, .seconds = 0.0};
 
        const auto started = std::chrono::steady_clock::now();
 
        // `boundaryVelocity` is what Navigator passes when starting from rest, which is the
        // situation this application simulates.
        parametrization->apply(result.trajectory, config.generalConfig.boundaryVelocity);
 
        result.seconds = std::chrono::duration<double>(
                             std::chrono::steady_clock::now() - started)
                             .count();
 
        // Introspection: a GlobalTrajectory has no time axis, and the plots want one.
        if (const auto* toppra =
                dynamic_cast<const algorithms::Toppra*>(parametrization.get());
            toppra != nullptr)
        {
            result.reference = toReference(*toppra);
        }
 
        std::cout << "  [reparametrization] " << std::left << std::setw(32)
                  << "one-time setup (startup)" << std::right << std::setw(8) << std::fixed
                  << std::setprecision(1) << constructionSeconds * 1e3 << " ms" << std::endl;
 
        ARMARX_IMPORTANT << "Parametrization `" << toString(config.parametrization)
                         << "`: " << constructionSeconds << " s setup + " << result.seconds
                         << " s per request, " << result.trajectory.points().size()
                         << " waypoints.";
 
        return result;
    }
 
} // namespace armarx::navigation::analysis