Diagnostics.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 "Diagnostics.h"
 
#include <cmath>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <ostream>
#include <sstream>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::platform_controller::diagnostics
{
 
    namespace
    {
 
        float
        yawOf(const core::Pose& pose)
        {
            return std::atan2(pose.linear()(1, 0), pose.linear()(0, 0));
        }
 
        /// The point schema `server/ParametrizationDump.cpp` writes, so the same reader takes both.
        nlohmann::json
        toJson(const core::GlobalTrajectory& trajectory)
        {
            nlohmann::json points = nlohmann::json::array();
 
            for (const core::GlobalTrajectoryPoint& point : trajectory.points())
            {
                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}});
            }
 
            if (trajectory.points().empty())
            {
                return nlohmann::json{{"points", points}, {"length", 0.F}, {"duration", 0.F}};
            }
 
            return nlohmann::json{
                {"points", points},
                {"length", trajectory.length()},
                {"duration",
                 trajectory.duration(core::VelocityInterpolation::LinearInterpolation)}};
        }
 
        nlohmann::json
        toJson(const traj_ctrl::global::TrajectoryFollowingControllerParams& params)
        {
            // Same key names as `controller_config/GlobalTrajectory/*.json`, so a dump can be
            // compared against the file it came from without a translation step.
            return nlohmann::json{
                {"pidPos",
                 {{"Kp", params.pidPos.Kp}, {"Ki", params.pidPos.Ki}, {"Kd", params.pidPos.Kd}}},
                {"pidOri",
                 {{"Kp", params.pidOri.Kp}, {"Ki", params.pidOri.Ki}, {"Kd", params.pidOri.Kd}}},
                {"limits", {{"linear", params.limits.linear}, {"angular", params.limits.angular}}},
                {"velocityFactor", params.velocityFactor},
                {"alpha", params.alpha},
                {"orientationWeight", params.orientationWeight},
                {"maxSegmentsAhead", params.maxSegmentsAhead},
                {"lookaheadDistance", params.lookaheadDistance},
                {"angularFeedforwardFraction", params.angularFeedforwardFraction},
                {"minFeedforwardVelocityFraction", params.minFeedforwardVelocityFraction},
                {"coupleLinearAndAngularLimits", params.coupleLinearAndAngularLimits},
                {"enableAngularFeedforward", params.enableAngularFeedforward},
                {"maxLinearAcceleration", params.maxLinearAcceleration},
                {"maxAngularAcceleration", params.maxAngularAcceleration}};
        }
 
        /// Seconds spanned by a sample range, 0 for fewer than two samples.
        template <class Sample>
        double
        span(const std::vector<Sample>& samples)
        {
            if (samples.size() < 2)
            {
                return 0.0;
            }
 
            return static_cast<double>(samples.back().timestampUs - samples.front().timestampUs) *
                   1e-6;
        }
 
        void
        writeRtCsv(const std::filesystem::path& file, const std::vector<RtSample>& samples)
        {
            std::ofstream stream{file};
            if (not stream.good())
            {
                ARMARX_WARNING << "Cannot write " << file;
                return;
            }
 
            stream << "t_us,episode,seq,x,y,yaw,v_meas_x,v_meas_y,w_meas,v_tgt_x,v_tgt_y,w_tgt,"
                      "v_cmd_x,v_cmd_y,w_cmd,dt,stale,slew_limited\n";
 
            stream << std::setprecision(9);
 
            for (const RtSample& s : samples)
            {
                stream << s.timestampUs << ',' << s.episode << ',' << s.sequence << ',' << s.x
                       << ',' << s.y << ',' << s.yaw << ',' << s.vMeasX << ',' << s.vMeasY << ','
                       << s.wMeas << ',' << s.vTgtX << ',' << s.vTgtY << ',' << s.wTgt << ','
                       << s.vCmdX << ',' << s.vCmdY << ',' << s.wCmd << ',' << s.dt << ','
                       << (s.stale ? 1 : 0) << ',' << (s.slewLimited ? 1 : 0) << '\n';
            }
        }
 
        void
        writeControlCsv(const std::filesystem::path& file,
                        const std::vector<ControlSample>& samples)
        {
            std::ofstream stream{file};
            if (not stream.good())
            {
                ARMARX_WARNING << "Cannot write " << file;
                return;
            }
 
            stream << "t_us,episode,seq,traj_rev,x,y,yaw,ref_x,ref_y,ref_yaw,ref_v,proj_idx,"
                      "final_seg,pos_err,ori_err,cur_ori,des_ori,ff_ang,capped_ff_v,ff_sat,guard,"
                      "v_raw_x,v_raw_y,w_raw,v_filt_x,v_filt_y,w_filt,"
                      "limit_linear,limit_angular,velocity_factor\n";
 
            stream << std::setprecision(9);
 
            for (const ControlSample& s : samples)
            {
                stream << s.timestampUs << ',' << s.episode << ',' << s.sequence << ','
                       << s.trajectoryRevision << ',' << s.x << ',' << s.y << ',' << s.yaw << ','
                       << s.refX << ',' << s.refY << ',' << s.refYaw << ',' << s.refVelocity << ','
                       << s.projectionIndex << ',' << (s.finalSegment ? 1 : 0) << ','
                       << s.positionError << ',' << s.orientationError << ','
                       << s.currentOrientation << ',' << s.desiredOrientation << ',' << s.ffAngular
                       << ',' << s.cappedFfVelocity << ',' << (s.ffSaturated ? 1 : 0) << ','
                       << s.guard << ',' << s.vRawX << ',' << s.vRawY << ',' << s.wRaw << ','
                       << s.vFiltX << ',' << s.vFiltY << ',' << s.wFilt << ',' << s.limitLinear
                       << ',' << s.limitAngular << ',' << s.velocityFactor << '\n';
            }
        }
 
    } // namespace
 
    std::string
    writeDump(const Episode& episode, const Params& params)
    try
    {
        std::ostringstream name;
        name << armarx::core::time::DateTime::Now().toDateTimeString() << "-ep" << std::setw(3)
             << std::setfill('0') << episode.episode;
 
        const std::filesystem::path directory =
            std::filesystem::path{params.outputDirectory} / name.str();
 
        std::filesystem::create_directories(directory);
 
        nlohmann::json meta;
        meta["timestamp"] = armarx::core::time::DateTime::Now().toDateTimeString();
        meta["episode"] = episode.episode;
        meta["controller"] = episode.controllerInstance;
        meta["platform"] = episode.platform;
        meta["params"] = toJson(episode.params);
 
        meta["rt"] = nlohmann::json{{"recorded", episode.rtSamples.size()},
                                    {"truncated", episode.rtTruncated},
                                    {"overwritten_lifetime", episode.rtOverwritten},
                                    {"duration", span(episode.rtSamples)}};
        meta["control"] = nlohmann::json{{"recorded", episode.controlSamples.size()},
                                         {"truncated", episode.controlTruncated},
                                         {"overwritten_lifetime", episode.controlOverwritten},
                                         {"duration", span(episode.controlSamples)}};
 
        meta["simulation"] = episode.simulation;
        meta["slew_dt_bound"] = episode.slewDtBound;
        meta["control_task_exceptions"] = episode.controlTaskExceptions;
        meta["control_target_stale_cycles"] = episode.controlTargetStaleCycles;
 
        {
            std::ofstream stream{directory / "meta.json"};
            if (not stream.good())
            {
                ARMARX_WARNING << "Cannot write " << (directory / "meta.json");
                return {};
            }
            stream << meta.dump(2) << std::endl;
        }
 
        {
            nlohmann::json revisions = nlohmann::json::array();
 
            for (const TrajectoryRevision& revision : episode.trajectories)
            {
                nlohmann::json entry = toJson(revision.trajectory);
                entry["revision"] = revision.revision;
                revisions.push_back(entry);
            }
 
            const nlohmann::json trajectories{
                {"revisions", revisions},
                {"dropped_revisions", episode.trajectoryRevisionsDropped}};
 
            std::ofstream stream{directory / "trajectory.json"};
            if (not stream.good())
            {
                ARMARX_WARNING << "Cannot write " << (directory / "trajectory.json");
                return {};
            }
            stream << trajectories.dump(2) << std::endl;
        }
 
        writeRtCsv(directory / "rt.csv", episode.rtSamples);
        writeControlCsv(directory / "control.csv", episode.controlSamples);
 
        ARMARX_INFO << "Wrote the execution diagnostics of episode " << episode.episode << " to "
                    << directory << " (" << episode.rtSamples.size() << " rt samples, "
                    << episode.controlSamples.size() << " control samples, "
                    << episode.trajectories.size() << " trajectory revisions).";
 
        return directory.string();
    }
    catch (const std::exception& e)
    {
        // A debugging aid must never be able to disturb the controller that produced it.
        ARMARX_WARNING << "Could not write the execution diagnostics: " << e.what();
        return {};
    }
 
} // namespace armarx::navigation::platform_controller::diagnostics