reparametrization.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 <string>
#include <utility>
#include <vector>
 
#include <SimoxUtility/json/json.hpp>
 
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/Trajectory.h>
 
#include "Scene.h"
 
namespace armarx::navigation::analysis
{
 
    /**
     * @brief How far a velocity profile asks for more than the device command ramp can deliver.
     *
     * A `GlobalTrajectory` stores a speed per position, so the tangential acceleration it
     * implies is `v * dv/ds`. Nothing in the planning stack compares that against the ramp in
     * `Velocity.cpp`, which is the actual bottleneck: TOPPRA is bounded by motor torque and
     * happily plans an 873 mm/s^2 stop that the configured 400 mm/s^2 ramp cannot execute, and
     * the base sails past the goal.
     */
    struct CommandRampCheck
    {
        /// Waypoints demanding more than the ramp can deliver.
        std::size_t violations{0};
 
        /// Largest demanded tangential acceleration [mm/s^2], and where along the path it is.
        float worstDemand{0.F};
        float worstArcLength{0.F};
 
        /// The bound that was exceeded there [mm/s^2].
        float worstLimit{0.F};
 
        /// Arc-length spans in which the demand exceeds the ramp, for the plot.
        std::vector<std::pair<float, float>> spans;
 
        /// Distance the ramp needs to stop from the profile's final velocity [mm].
        ///
        /// A flat profile passes the `v * dv/ds` test everywhere and still overshoots, because
        /// it simply ends at speed: nothing in a `GlobalTrajectory` says the robot has to be
        /// able to stop. This is the number that predicts the overshoot.
        float terminalStoppingDistance{0.F};
 
        /// Whether the profile ends above `boundaryVelocity`, i.e. faster than by design.
        bool terminalVelocityExceedsBoundary{false};
    };
 
    /// Compare the profile's `v * dv/ds` against `rateLimit`. Logs a warning if it exceeds it.
    ///
    /// `boundaryVelocity` is the speed the stack deliberately ends at, so a terminal stopping
    /// distance implied by it is by design and not worth warning about.
    CommandRampCheck checkAgainstCommandRamp(const core::GlobalTrajectory& trajectory,
                                             const simulation::CommandRateLimit& rateLimit,
                                             float boundaryVelocity);
 
    /// Parse the `--parametrization` choice. Throws on an unknown name.
    core::TrajectoryParametrization parametrizationModeFromString(const std::string& name);
 
    std::string toString(core::TrajectoryParametrization mode);
 
    struct ReparametrizationResult
    {
        core::GlobalTrajectory trajectory;
 
        /**
         * @brief The time-parametrized reference, as `{duration, waypoints: [...]}`.
         *
         * A GlobalTrajectory only stores a speed per *position*, so the time information TOPPRA
         * computed is lost as soon as the result is converted. Reading it back out of the
         * parametrization lets the plots put the promised profile next to the executed one.
         * Null unless mode is `toppra`.
         */
        nlohmann::json reference;
 
        /// Wall-clock time the parametrization took [s]. The planner's own stage timings do not
        /// cover it, and for TOPPRA it dwarfs them.
        double seconds{0.0};
    };
 
    /**
     * @brief Re-assign the velocities along `trajectory` according to `config.parametrization`.
     *
     * A thin wrapper over `fac::TrajectoryParametrizationFactory` -- the modes themselves live
     * in the algorithms library and are the same objects the `Navigator` runs, so what this
     * application measures is what the robot would do. All that is added here is assembling
     * `Toppra::DriveParams` from the scene's simulation config and reading back the time
     * parametrization for the plots.
     *
     * @throws std::runtime_error if the parametrization is unavailable or fails.
     */
    ReparametrizationResult reparametrize(const core::GlobalTrajectory& trajectory,
                                          const Config& config);
 
} // namespace armarx::navigation::analysis