ParametrizationDump.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 "ParametrizationDump.h"
 
#include <algorithm>
#include <cmath>
#include <filesystem>
#include <fstream>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
 
namespace armarx::navigation::server
{
 
    namespace
    {
 
        float
        yawOf(const core::Pose& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
 
        /// `trajectory.points[]` in the shape `evaluation/plot.py` already reads.
        ///
        /// `velocity_limit` comes from the *planner's* profile rather than from the
        /// `ObstacleAwareVelocityLimit`, which the navigator cannot reach: it holds only the
        /// `GlobalPlanner` interface. The planner's velocities are that limit, so taking them
        /// from the pre-parametrization copy is exact, not an approximation.
        nlohmann::json
        toJson(const core::GlobalTrajectory& parametrized, const core::GlobalTrajectory& planned)
        {
            nlohmann::json points = nlohmann::json::array();
 
            const std::size_t count = parametrized.points().size();
            const bool limitsUsable = planned.points().size() == count;
 
            for (std::size_t i = 0; i < count; i++)
            {
                const core::GlobalTrajectoryPoint& point = parametrized.points()[i];
                const core::Position& position = point.waypoint.pose.translation();
 
                points.push_back(nlohmann::json{
                    {"x", position.x()},
                    {"y", position.y()},
                    {"yaw", yawOf(point.waypoint.pose)},
                    {"velocity", point.velocity},
                    // TOPP-RA resamples, so the planner's profile may have a different length.
                    // Fall back to the executed velocity rather than mislabelling a limit.
                    {"velocity_limit",
                     limitsUsable ? planned.points()[i].velocity : point.velocity}});
            }
 
            // Same interpolation the analysis application reports, so the two are comparable.
            return nlohmann::json{
                {"points", points},
                {"length", parametrized.length()},
                {"duration",
                 parametrized.duration(core::VelocityInterpolation::LinearInterpolation)}};
        }
 
        nlohmann::json
        toJson(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)}});
            }
 
            return nlohmann::json{
                {"success", true}, {"duration", toppra.lastDuration()}, {"waypoints", waypoints}};
        }
 
    } // namespace
 
    void
    writeParametrizationDump(const ParametrizationRecord& record, const std::string& path)
    try
    {
        nlohmann::json dump;
 
        dump["success"] = true;
        dump["requested_parametrization"] = core::ToString(record.requested);
        dump["applied_parametrization"] = core::ToString(record.applied);
        dump["degraded"] = record.requested != record.applied;
 
        // `plot.py` reads the mode from here; it must be what actually ran.
        dump["planner"] = nlohmann::json{{"parametrization", core::ToString(record.applied)}};
 
        dump["trajectory"] = toJson(record.parametrized, record.planned);
        dump["planned_trajectory"] = toJson(record.planned, record.planned);
 
        if (record.toppra != nullptr and record.toppra->lastSamples().rows() > 0)
        {
            dump["reparametrization_reference"] = toJson(*record.toppra);
        }
 
        // Same key names the analysis application emits, so `plot.py` reads both unchanged.
        if (record.driveParams.has_value())
        {
            const algorithms::Toppra::DriveParams& drive = *record.driveParams;
 
            dump["parametrization_limits"] = nlohmann::json{
                {"max_velocity", record.maxVelocity},
                {"max_angular_velocity", record.maxAngularVelocity},
                {"max_acceleration", drive.maxAcceleration},
                {"max_deceleration", drive.maxDeceleration},
                {"acceleration_radius", std::min(drive.maxAcceleration, drive.maxDeceleration)},
                {"acceleration_polygon_sides", 32},
                {"acceleration_frame", "body"},
                {"max_wheel_velocity", drive.maxWheelVelocity},
                {"motor_max_torque", drive.motorMaxTorque * drive.torqueFraction}};
        }
 
        dump["timestamp"] = armarx::core::time::DateTime::Now().toDateTimeString();
 
        const std::filesystem::path latest{path};
        if (latest.has_parent_path())
        {
            std::filesystem::create_directories(latest.parent_path());
        }
 
        const auto write = [&dump](const std::filesystem::path& to)
        {
            std::ofstream stream{to};
            if (not stream.good())
            {
                ARMARX_WARNING << "Cannot write the parametrization record to " << to;
                return;
            }
            stream << dump.dump(2) << std::endl;
        };
 
        write(latest);
 
        // A timestamped sibling, so a sequence of requests can be compared afterwards while the
        // fixed path always holds the most recent one.
        std::filesystem::path stamped = latest;
        stamped.replace_filename(latest.stem().string() + "-" +
                                 armarx::core::time::DateTime::Now().toDateTimeString() +
                                 latest.extension().string());
        write(stamped);
 
        ARMARX_INFO << "Wrote the parametrization record to " << latest;
    }
    catch (const std::exception& e)
    {
        // A debugging aid must never be able to abort a navigation request.
        ARMARX_WARNING << "Could not write the parametrization record: " << e.what();
    }
 
} // namespace armarx::navigation::server