io.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 "io.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <fstream>
#include <vector>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
namespace armarx::navigation::analysis
{
 
    namespace
    {
 
        nlohmann::json
        toJson(const Eigen::Vector2f& v)
        {
            return nlohmann::json::array({v.x(), v.y()});
        }
 
        /// Wheel-count agnostic: the array is as long as the drive has wheels.
        nlohmann::json
        toJson(const Eigen::VectorXf& v)
        {
            nlohmann::json a = nlohmann::json::array();
            for (Eigen::Index i = 0; i < v.size(); i++)
            {
                a.push_back(v[i]);
            }
            return a;
        }
 
        /// Same, as booleans -- the saturation flags are stored as 0/1 ints.
        nlohmann::json
        toJsonFlags(const Eigen::ArrayXi& v)
        {
            nlohmann::json a = nlohmann::json::array();
            for (Eigen::Index i = 0; i < v.size(); i++)
            {
                a.push_back(v[i] != 0);
            }
            return a;
        }
 
        float
        yawOf(const core::Pose& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
 
        nlohmann::json
        toJson(const core::Pose& pose)
        {
            return nlohmann::json{{"x", pose.translation().x()},
                                  {"y", pose.translation().y()},
                                  {"yaw", yawOf(pose)}};
        }
 
        nlohmann::json
        toJson(const Scene& scene)
        {
            nlohmann::json jObstacles = nlohmann::json::array();
            for (const Box& obstacle : scene.obstacles)
            {
                jObstacles.push_back(nlohmann::json{
                    {"type", "box"}, {"min", toJson(obstacle.min)}, {"max", toJson(obstacle.max)}});
            }
 
            return nlohmann::json{
                {"bounds",
                 nlohmann::json{{"min", toJson(scene.bounds.min)},
                                {"max", toJson(scene.bounds.max)}}},
                {"cell_size", scene.cellSize},
                {"obstacles", jObstacles}};
        }
 
        nlohmann::json
        toJson(const algorithms::Costmap& costmap)
        {
            const algorithms::Costmap::Grid& grid = costmap.getGrid();
 
            nlohmann::json jGrid = nlohmann::json::array();
            for (int ix = 0; ix < grid.rows(); ix++)
            {
                std::vector<float> row(static_cast<std::size_t>(grid.cols()));
                for (int iy = 0; iy < grid.cols(); iy++)
                {
                    row[static_cast<std::size_t>(iy)] = grid(ix, iy);
                }
 
                jGrid.push_back(row);
            }
 
            return nlohmann::json{
                {"cell_size", costmap.params().cellSize},
                {"robot_radius", costmap.params().robotRadius},
                {"bounds",
                 nlohmann::json{{"min", toJson(costmap.getLocalSceneBounds().min)},
                                {"max", toJson(costmap.getLocalSceneBounds().max)}}},
                {"rows", grid.rows()},
                {"cols", grid.cols()},
                {"grid", jGrid}};
        }
 
        /// A velocity counts as violating the limit only beyond this relative slack.
        constexpr float violationTolerance = 1e-3F;
 
        /// ... and beyond this absolute slack [mm/s].
        ///
        /// The limit is sampled from a costmap with finite cells, so moving a waypoint by a
        /// fraction of a cell changes the limit it is compared against. A resampled trajectory
        /// therefore reports a handful of violations of one or two mm/s that say nothing about
        /// the planner -- they are the difference between two lookups, not a real excess.
        constexpr float violationToleranceAbsolute = 5.F;
 
        nlohmann::json
        toJson(const core::GlobalTrajectory& trajectory,
               const algorithms::ObstacleAwareVelocityLimit& limit)
        {
            nlohmann::json jPoints = nlohmann::json::array();
 
            std::size_t violations = 0;
            float maxViolation = 0.F;
 
            for (const core::GlobalTrajectoryPoint& point : trajectory.points())
            {
                const core::Position& position = point.waypoint.pose.translation();
                const float permissible = limit.at(Eigen::Vector2f{position.head<2>()});
                const float excess = point.velocity - permissible;
 
                if (excess > std::max(violationTolerance * permissible,
                                      violationToleranceAbsolute))
                {
                    violations++;
                    maxViolation = std::max(maxViolation, excess);
                }
 
                jPoints.push_back(nlohmann::json{{"x", position.x()},
                                                 {"y", position.y()},
                                                 {"yaw", yawOf(point.waypoint.pose)},
                                                 {"velocity", point.velocity},
                                                 {"velocity_limit", permissible}});
            }
 
            return nlohmann::json{
                {"points", jPoints},
                {"length", trajectory.length()},
                {"duration",
                 trajectory.duration(core::VelocityInterpolation::LinearInterpolation)},
                {"velocity_limit_violations", violations},
                {"max_velocity_limit_violation", maxViolation}};
        }
 
        nlohmann::json
        toJson(const simulation::TrajectoryFollowingSimulation::Result& simulated,
               const simulation::PlatformDynamics& dynamics)
        {
            nlohmann::json jSamples = nlohmann::json::array();
 
            for (const auto& sample : simulated.samples)
            {
                const core::Position& position = sample.global_T_robot.translation();
 
                jSamples.push_back(
                    nlohmann::json{{"t", sample.time},
                                   {"x", position.x()},
                                   {"y", position.y()},
                                   {"yaw", yawOf(sample.global_T_robot)},
                                   {"speed", sample.speed},
                                   {"vx", sample.velocityGlobal.x()},
                                   {"vy", sample.velocityGlobal.y()},
                                   {"commanded_speed", sample.commandedVelocityGlobal.norm()},
                                   {"angular_velocity", sample.commandedAngular},
                                   {"required_linear_acceleration",
                                    sample.requiredLinearAcceleration},
                                   {"linear_acceleration", sample.linearAcceleration},
                                   {"required_angular_acceleration",
                                    sample.requiredAngularAcceleration},
                                   {"angular_acceleration", sample.angularAcceleration},
                                   {"acceleration_saturated", sample.accelerationSaturated},
                                   {"required_wheel_accelerations", toJson(sample.requiredWheelAccelerations)},
                                   {"wheel_velocities", toJson(sample.wheelVelocities)},
                                   {"wheel_saturated", toJsonFlags(sample.wheelSaturated)},
                                   {"required_motor_torques", toJson(sample.requiredMotorTorques)},
                                   {"applied_motor_torques", toJson(sample.appliedMotorTorques)},
                                   {"motor_saturated", toJsonFlags(sample.motorSaturated)},
                                   {"wheel_speed_saturated", toJsonFlags(sample.wheelSpeedSaturated)},
                                   {"drop_point_velocity", sample.dropPointVelocity},
                                   {"tracking_error", sample.trackingError},
                                   {"distance_to_goal", sample.distanceToGoal},
                                   {"orientation_error", sample.orientationError}});
            }
 
            const std::string modelName = [&]() -> std::string
            {
                switch (dynamics.model)
                {
                    case simulation::PlatformModel::OmniWheelTorque:
                        return "omni_wheel_torque";
                    case simulation::PlatformModel::MecanumTorque:
                        return "mecanum_torque";
                    case simulation::PlatformModel::OmniWheelVelocity:
                        return "omni_wheel_velocity";
                    case simulation::PlatformModel::Cartesian:
                        return "cartesian";
                }
 
                return "unknown";
            }();
 
            return nlohmann::json{
                {"samples", jSamples},
                {"reached_goal", simulated.reachedGoal},
                {"settled_short_of_goal", simulated.settledShortOfGoal},
                {"final_distance_to_goal", simulated.finalDistanceToGoal},
                {"max_goal_overshoot", simulated.maxGoalOvershoot},
                {"max_required_linear_acceleration", simulated.maxRequiredLinearAcceleration},
                {"max_required_angular_acceleration", simulated.maxRequiredAngularAcceleration},
                {"saturated_cycles", simulated.saturatedCycles},
                {"motor_torque_saturated_cycles", simulated.motorTorqueSaturatedCycles},
                {"wheel_speed_saturated_cycles", simulated.wheelSpeedSaturatedCycles},
                {"max_required_motor_torque", simulated.maxRequiredMotorTorque},
                {"platform_mass", simulated.platformMass},
                {"platform_yaw_inertia", simulated.platformYawInertia},
                {"mass_matrix",
                 {{simulated.massMatrix(0, 0),
                   simulated.massMatrix(0, 1),
                   simulated.massMatrix(0, 2)},
                  {simulated.massMatrix(1, 0),
                   simulated.massMatrix(1, 1),
                   simulated.massMatrix(1, 2)},
                  {simulated.massMatrix(2, 0),
                   simulated.massMatrix(2, 1),
                   simulated.massMatrix(2, 2)}}},
                {"max_tracking_error", simulated.maxTrackingError},
                {"max_linear_acceleration", dynamics.maxLinearAcceleration},
                {"max_angular_acceleration", dynamics.maxAngularAcceleration},
                {"model", modelName},
                {"max_wheel_acceleration", dynamics.omniWheel.maxWheelAcceleration},
                {"max_wheel_deceleration", dynamics.omniWheel.maxWheelDeceleration},
                {"motor_max_torque", dynamics.omniWheelTorque.motorMaxTorque},
                {"gear_ratio", dynamics.omniWheelTorque.gearRatio}};
        }
 
    } // namespace
 
    void
    writeResult(const std::filesystem::path& filename,
                const Config& config,
                const algorithms::Costmap& costmap,
                const std::optional<global_planning::GlobalPlannerResult>& result,
                const algorithms::ObstacleAwareVelocityLimit& limit,
                const std::optional<simulation::TrajectoryFollowingSimulation::Result>& simulated,
                const nlohmann::json& reference,
                const CommandRampCheck& rampCheck,
                const double parametrizationSeconds)
    {
        nlohmann::json j{
            {"success", result.has_value()},
            {"scene", toJson(config.scene)},
            {"start", toJson(config.start)},
            {"goal", toJson(config.goal)},
            {"costmap", toJson(costmap)},
            {"planner",
             nlohmann::json{
                 {"obstacle_distance_costs", config.plannerParams.algo.obstacleDistanceCosts},
                 {"obstacle_cost_exponent", config.plannerParams.algo.obstacleCostExponent},
                 {"obstacle_max_distance", config.plannerParams.algo.obstacleMaxDistance},
                 {"obstacle_distance_weight", config.plannerParams.algo.obstacleDistanceWeight},
                 {"max_linear_velocity", config.generalConfig.maxVel.linear},
                 {"max_angular_velocity", config.generalConfig.maxVel.angular},
                 {"enable_position_smoothing", config.plannerParams.enablePositionSmoothing},
                 {"enable_final_velocity_clamp",
                  config.plannerParams.enableFinalVelocityClamp},
                 {"parametrization", toString(config.parametrization)}}}};
 
        if (result.has_value())
        {
            j["trajectory"] = toJson(result->trajectory, limit);
            j["timings"] = result->timings;
 
            // Deliberately not folded into `timings`: that is the planner's own stage
            // breakdown, which the reader sums, and the parametrization runs after plan().
            j["parametrization_seconds"] = parametrizationSeconds;
 
            // The bounds the reparametrization actually solved against, as opposed to the
            // configured ones they are derived from. They still differ enough to mislead: the
            // acceleration bound is min(accel, decel) -- deceleration is deliberately not spent
            // here, it is reserved for the reactive safety guard -- against the 500 mm/s^2 the
            // simulation reports as its own limit, and the wheel bound is referred to the wheel
            // rather than the motor. Without these a reader cannot tell which constraint shaped
            // the profile.
            const auto& ramp = config.simulationParams.rateLimit;
            const auto& drive = config.simulationParams.dynamics.omniWheelTorque;
 
            const float inputSpeedRpm =
                std::min(drive.motorNominalSpeedRpm, drive.gearboxMaxInputSpeedRpm);
 
            j["parametrization_limits"] = nlohmann::json{
                {"max_velocity", config.generalConfig.maxVel.linear},
                {"max_angular_velocity", config.generalConfig.maxVel.angular},
                {"max_acceleration", ramp.maxAcceleration},
                {"max_deceleration", ramp.maxDeceleration},
                // Radius of the disc the acceleration is held inside. Approximated by an
                // inscribed polygon of `acceleration_polygon_sides`; keep that in sync with
                // `N_DIRECTIONS` in the python package's `reparametrization/acceleration.py`.
                {"acceleration_radius", std::min(ramp.maxAcceleration, ramp.maxDeceleration)},
                {"acceleration_polygon_sides", 32},
                // Which acceleration the radius bounds. The ramp acts on the commanded *body*
                // twist, so bounding the world frame would leave the real limit exceeded.
                {"acceleration_frame", "body"},
                {"max_wheel_velocity",
                 inputSpeedRpm / 60.F / drive.gearRatio * 2.F * static_cast<float>(M_PI)},
                {"motor_max_torque",
                 drive.motorMaxTorque * config.parametrizationTorqueFraction}};
 
            nlohmann::json jGridPath = nlohmann::json::array();
            for (const core::Position& position : result->gridPath)
            {
                jGridPath.push_back(
                    nlohmann::json{{"x", position.x()}, {"y", position.y()}});
            }
            j["grid_path"] = jGridPath;
            j["position_smoothing_applied"] = result->positionSmoothingApplied;
        }
 
        if (simulated.has_value())
        {
            j["simulation"] = toJson(simulated.value(), config.simulationParams.dynamics);
        }
 
        {
            nlohmann::json jSpans = nlohmann::json::array();
            for (const auto& [from, to] : rampCheck.spans)
            {
                jSpans.push_back(nlohmann::json::array({from, to}));
            }
 
            j["velocity_profile_exceeds_command_ramp"] =
                nlohmann::json{{"violations", rampCheck.violations},
                               {"worst_demand", rampCheck.worstDemand},
                               {"worst_limit", rampCheck.worstLimit},
                               {"worst_arc_length", rampCheck.worstArcLength},
                               {"terminal_stopping_distance",
                                rampCheck.terminalStoppingDistance},
                               {"terminal_velocity_exceeds_boundary",
                                rampCheck.terminalVelocityExceedsBoundary},
                               {"spans", jSpans}};
        }
 
        if (not reference.is_null())
        {
            j["reparametrization_reference"] = reference;
        }
 
        std::ofstream ofs{filename};
        ARMARX_CHECK(ofs.good()) << "Cannot write to " << QUOTED(filename.string()) << ".";
 
        ofs << j.dump(2) << std::endl;
    }
 
} // namespace armarx::navigation::analysis