Scene.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 "Scene.h"
 
#include "reparametrization.h"
 
#include <algorithm>
#include <fstream>
#include <limits>
#include <stdexcept>
#include <string>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/CostmapBuilder.h>
 
namespace armarx::navigation::analysis
{
 
    namespace
    {
 
        Eigen::Vector2f
        toVector2f(const nlohmann::json& j)
        {
            ARMARX_CHECK_EQUAL(j.size(), 2) << "Expected a two-element array, got " << j.dump();
            return Eigen::Vector2f{j.at(0).get<float>(), j.at(1).get<float>()};
        }
 
        core::Pose
        toPose(const nlohmann::json& j)
        {
            const float yaw = j.value("yaw", 0.F);
 
            core::Pose pose = core::Pose::Identity();
            pose.translation() << j.at("x").get<float>(), j.at("y").get<float>(), 0.F;
            pose.linear() = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
            return pose;
        }
 
    } // namespace
 
    float
    Box::distanceTo(const Eigen::Vector2f& p) const
    {
        // Rotate the query point into the box's own frame, about the box centre, so the
        // axis-aligned test below applies to the rotated box too. Exactly the old behaviour
        // when yaw is zero.
        Eigen::Vector2f q = p;
        if (yaw != 0.F)
        {
            const Eigen::Vector2f centre = 0.5F * (min + max);
            q = Eigen::Rotation2Df{-yaw} * (p - centre) + centre;
        }
 
        // Component-wise overshoot beyond the box on either side; zero inside.
        const Eigen::Vector2f d = (min - q).cwiseMax(q - max).cwiseMax(Eigen::Vector2f::Zero());
        return d.norm();
    }
 
    Config
    readConfig(const std::filesystem::path& filename)
    {
        ARMARX_CHECK(std::filesystem::exists(filename))
            << "Scene description " << QUOTED(filename.string()) << " does not exist.";
 
        std::ifstream ifs{filename};
        const nlohmann::json j = nlohmann::json::parse(ifs);
 
        Config config;
 
        // Scene.
        {
            const nlohmann::json& jScene = j.at("scene");
 
            config.scene.bounds.min = toVector2f(jScene.at("bounds").at("min"));
            config.scene.bounds.max = toVector2f(jScene.at("bounds").at("max"));
            config.scene.cellSize = jScene.at("cell_size").get<float>();
 
            ARMARX_CHECK_GREATER(config.scene.cellSize, 0.F);
            ARMARX_CHECK_GREATER(config.scene.bounds.max.x(), config.scene.bounds.min.x());
            ARMARX_CHECK_GREATER(config.scene.bounds.max.y(), config.scene.bounds.min.y());
 
            for (const nlohmann::json& jObstacle : jScene.at("obstacles"))
            {
                const std::string type = jObstacle.value("type", std::string{"box"});
                if (type != "box")
                {
                    throw std::invalid_argument("Unsupported obstacle type `" + type +
                                                "`. Only `box` is supported.");
                }
 
                const Box box{.min = toVector2f(jObstacle.at("min")),
                              .max = toVector2f(jObstacle.at("max")),
                              .yaw = jObstacle.value("yaw", 0.F)};
 
                ARMARX_CHECK_GREATER_EQUAL(box.max.x(), box.min.x());
                ARMARX_CHECK_GREATER_EQUAL(box.max.y(), box.min.y());
 
                config.scene.obstacles.push_back(box);
            }
        }
 
        config.start = toPose(j.at("start"));
        config.goal = toPose(j.at("goal"));
 
        // Costmap.
        {
            const nlohmann::json jCostmap = j.value("costmap", nlohmann::json::object());
 
            config.costmapParams.cellSize = config.scene.cellSize;
            config.costmapParams.robotRadius = jCostmap.value("robot_radius", 0.F);
            config.costmapParams.binaryGrid = false;
 
            config.costmapMaxDistance = jCostmap.value("max_distance", config.costmapMaxDistance);
 
            ARMARX_CHECK_GREATER(config.costmapMaxDistance, 0.F);
        }
 
        // Planner.
        {
            const nlohmann::json jSpfa = j.value("spfa", nlohmann::json::object());
            auto& algo = config.plannerParams.algo;
 
            algo.obstacleDistanceCosts =
                jSpfa.value("obstacle_distance_costs", algo.obstacleDistanceCosts);
            algo.obstacleCostExponent =
                jSpfa.value("obstacle_cost_exponent", algo.obstacleCostExponent);
            algo.obstacleMaxDistance =
                jSpfa.value("obstacle_max_distance", algo.obstacleMaxDistance);
            algo.obstacleDistanceWeight =
                jSpfa.value("obstacle_distance_weight", algo.obstacleDistanceWeight);
 
            config.plannerParams.enablePositionSmoothing =
                jSpfa.value("enable_position_smoothing",
                            config.plannerParams.enablePositionSmoothing);
            config.plannerParams.enableFinalVelocityClamp =
                jSpfa.value("enable_final_velocity_clamp",
                            config.plannerParams.enableFinalVelocityClamp);
        }
 
        // General navigation stack config.
        {
            const nlohmann::json jGeneral = j.value("general", nlohmann::json::object());
            core::GeneralConfig& general = config.generalConfig;
 
            general.maxVel.linear = jGeneral.value("max_linear_velocity", general.maxVel.linear);
            general.maxVel.angular = jGeneral.value("max_angular_velocity", general.maxVel.angular);
            general.enableRampingStart =
                jGeneral.value("enable_ramping_start", general.enableRampingStart);
            general.enableRampingEnd =
                jGeneral.value("enable_ramping_end", general.enableRampingEnd);
            general.enableRampingCorners =
                jGeneral.value("enable_ramping_corners", general.enableRampingCorners);
            general.rampLength = jGeneral.value("ramp_length", general.rampLength);
            general.cornerVelocity = jGeneral.value("corner_velocity", general.cornerVelocity);
            general.boundaryVelocity =
                jGeneral.value("boundary_velocity", general.boundaryVelocity);
            general.cornerLimit = jGeneral.value("corner_limit", general.cornerLimit);
        }
 
        // Low-level controller simulation.
        {
            const nlohmann::json jSim = j.value("simulation", nlohmann::json::object());
            auto& sim = config.simulationParams;
            auto& ctrl = sim.controller;
 
            config.simulate = jSim.value("enabled", config.simulate);
 
            sim.dt = jSim.value("dt", sim.dt);
            sim.maxDuration = jSim.value("max_duration", sim.maxDuration);
            sim.goalDistanceThreshold =
                jSim.value("goal_distance_threshold", sim.goalDistanceThreshold);
 
            ctrl.alpha = jSim.value("alpha", ctrl.alpha);
            ctrl.velocityFactor = jSim.value("velocity_factor", ctrl.velocityFactor);
            ctrl.maxSegmentsAhead = jSim.value("max_segments_ahead", ctrl.maxSegmentsAhead);
            ctrl.orientationWeight = jSim.value("orientation_weight", ctrl.orientationWeight);
            ctrl.coupleLinearAndAngularLimits =
                jSim.value("couple_linear_and_angular_limits",
                           ctrl.coupleLinearAndAngularLimits);
            ctrl.enableAngularFeedforward =
                jSim.value("enable_angular_feedforward", ctrl.enableAngularFeedforward);
 
            ctrl.pidPos.Kp = jSim.value("pid_pos_kp", ctrl.pidPos.Kp);
            ctrl.pidPos.Ki = jSim.value("pid_pos_ki", ctrl.pidPos.Ki);
            ctrl.pidPos.Kd = jSim.value("pid_pos_kd", ctrl.pidPos.Kd);
            ctrl.pidOri.Kp = jSim.value("pid_ori_kp", ctrl.pidOri.Kp);
            ctrl.pidOri.Ki = jSim.value("pid_ori_ki", ctrl.pidOri.Ki);
            ctrl.pidOri.Kd = jSim.value("pid_ori_kd", ctrl.pidOri.Kd);
 
            // The controller's own limits default to the navigation stack's.
            ctrl.limits.linear = jSim.value("limit_linear", config.generalConfig.maxVel.linear);
            ctrl.limits.angular = jSim.value("limit_angular", config.generalConfig.maxVel.angular);
 
            // What the simulated mass can deliver comes from the package data directory; the
            // scene may override it to compare against an unconstrained base.
            // Selects config/platform/PlatformDynamics<Robot>.json, spelled as the rest of
            // the codebase does: `Armar7`, `Armar6`, `ArmarDE`.
            config.robot = jSim.value("robot", std::string{"Armar7"});
            sim.dynamics = simulation::PlatformDynamics::FromPackagePath(config.robot);
            sim.dynamics.maxLinearAcceleration =
                jSim.value("max_linear_acceleration", sim.dynamics.maxLinearAcceleration);
            sim.dynamics.maxAngularAcceleration =
                jSim.value("max_angular_acceleration", sim.dynamics.maxAngularAcceleration);
 
            // Device-side command ramp. Defaults mirror Armar7a/parts/Platform.xml.
            {
                const nlohmann::json jRamp =
                    jSim.value("rate_limit", nlohmann::json::object());
                auto& ramp = sim.rateLimit;
 
                ramp.enabled = jRamp.value("enabled", ramp.enabled);
                ramp.dt = jRamp.value("dt", ramp.dt);
                ramp.maxVelocity = jRamp.value("max_velocity", ramp.maxVelocity);
                ramp.maxAcceleration = jRamp.value("max_acceleration", ramp.maxAcceleration);
                ramp.maxDeceleration = jRamp.value("max_deceleration", ramp.maxDeceleration);
                ramp.maxAngularVelocity =
                    jRamp.value("max_angular_velocity", ramp.maxAngularVelocity);
                ramp.maxAngularAcceleration =
                    jRamp.value("max_angular_acceleration", ramp.maxAngularAcceleration);
                ramp.maxAngularDeceleration =
                    jRamp.value("max_angular_deceleration", ramp.maxAngularDeceleration);
                ramp.directionPreserving =
                    jRamp.value("direction_preserving", ramp.directionPreserving);
            }
 
            if (jSim.contains("model"))
            {
                const std::string model = jSim.at("model");
                if (model == "omni_wheel_torque")
                {
                    sim.dynamics.model = simulation::PlatformModel::OmniWheelTorque;
                }
                else if (model == "omni_wheel_velocity")
                {
                    sim.dynamics.model = simulation::PlatformModel::OmniWheelVelocity;
                }
                else if (model == "mecanum_torque")
                {
                    sim.dynamics.model = simulation::PlatformModel::MecanumTorque;
                }
                else if (model == "cartesian")
                {
                    sim.dynamics.model = simulation::PlatformModel::Cartesian;
                }
                else
                {
                    // Previously an unrecognised name silently became Cartesian, which looks
                    // like a working run against the wrong model -- exactly how `mecanum_torque`
                    // went unnoticed here after being added to the config parser.
                    throw std::invalid_argument(
                        "Unknown platform model `" + model +
                        "`. Expected `cartesian`, `omni_wheel_velocity`, `omni_wheel_torque` "
                        "or `mecanum_torque`.");
                }
            }
        }
 
        // Velocity parametrization.
        {
            const nlohmann::json jParam =
                j.value("reparametrization", nlohmann::json::object());
 
            config.parametrization =
                parametrizationModeFromString(jParam.value("mode", std::string{"none"}));
            config.parametrizationSamples =
                jParam.value("num_samples", config.parametrizationSamples);
            config.parametrizationTorqueFraction =
                jParam.value("torque_fraction", config.parametrizationTorqueFraction);
        }
 
        return config;
    }
 
    algorithms::Costmap
    buildCostmap(const Config& config)
    {
        // Reuse the builder's sizing so the grid dimensions cannot drift from what the
        // costmap's index <-> position math expects.
        algorithms::Costmap::Grid grid = algorithms::CostmapBuilder::createUniformGrid(
            config.scene.bounds, config.costmapParams);
 
        ARMARX_INFO << "Costmap grid: " << grid.rows() << " rows (x) x " << grid.cols()
                    << " cols (y), cell size " << config.costmapParams.cellSize << " mm.";
 
        const float cellSize = config.costmapParams.cellSize;
        const Eigen::Vector2f& boundsMin = config.scene.bounds.min;
 
        for (int ix = 0; ix < grid.rows(); ix++)
        {
            for (int iy = 0; iy < grid.cols(); iy++)
            {
                // Rows correspond to x, columns to y (see Costmap::toPositionLocal).
                const Eigen::Vector2f cellCenter{
                    boundsMin.x() + static_cast<float>(ix) * cellSize + cellSize / 2,
                    boundsMin.y() + static_cast<float>(iy) * cellSize + cellSize / 2};
 
                float distance = std::numeric_limits<float>::max();
                for (const Box& obstacle : config.scene.obstacles)
                {
                    distance = std::min(distance, obstacle.distanceTo(cellCenter));
                }
 
                const float clearance = distance - config.costmapParams.robotRadius;
                grid(ix, iy) = std::clamp(clearance, 0.F, config.costmapMaxDistance);
            }
        }
 
        return algorithms::Costmap(grid, config.costmapParams, config.scene.bounds);
    }
 
} // namespace armarx::navigation::analysis