Scene.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 <filesystem>
#include <vector>
 
#include <Eigen/Core>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/simulation/TrajectoryFollowingSimulation.h>
 
namespace armarx::navigation::analysis
{
 
    /// A box obstacle in the ground plane, optionally rotated about its own centre.
    struct Box
    {
        Eigen::Vector2f min;
 
        Eigen::Vector2f max;
 
        /// Rotation about the box centre [rad]. Zero leaves the box axis-aligned, which is
        /// what a scene description without a `yaw` field means.
        ///
        /// Real furniture is rarely axis-aligned, and enclosing a rotated table in its
        /// axis-aligned bounding box inflates it by up to 41% on the diagonal -- which would
        /// show up directly in any clearance the evaluation reports.
        float yaw{0.F};
 
        /// Distance from `p` to the box surface. Zero if `p` is inside the box.
        float distanceTo(const Eigen::Vector2f& p) const;
    };
 
    /// The synthetic scene: what the world looks like, independent of the planner.
    struct Scene
    {
        algorithms::SceneBounds bounds;
 
        /// Edge length of a costmap cell [mm].
        float cellSize{100.F};
 
        std::vector<Box> obstacles;
    };
 
    /// Everything the application needs, as read from the scene description file.
    struct Config
    {
        Scene scene;
 
        core::Pose start;
 
        core::Pose goal;
 
        algorithms::Costmap::Parameters costmapParams;
 
        /// Obstacle distances are clipped to this value [mm].
        float costmapMaxDistance{2000.F};
 
        global_planning::SPFAParams plannerParams;
 
        core::GeneralConfig generalConfig;
 
        /// Simulate the low-level controller following the planned trajectory.
        bool simulate{true};
 
        simulation::TrajectoryFollowingSimulation::Parameters simulationParams;
 
        /// Platform name selecting `PlatformDynamics<robot>.json`, spelled as the rest of the
        /// codebase does: `Armar7`, `Armar6`, `ArmarDE`. Kept so the reparametrization loads the
        /// same file the simulation did.
        std::string robot{"Armar7"};
 
        /// How the velocities along the planned path are (re-)assigned before simulating.
        ///
        /// Defaults to `None` rather than the stack's `Ramping`: this application exists to
        /// compare the modes, so it should apply nothing unless asked.
        core::TrajectoryParametrization parametrization{core::TrajectoryParametrization::None};
 
        /// Waypoints the reparametrization should return. 0 keeps the input count.
        ///
        /// This matters more than it looks. TOPPRA bounds the accelerations along a *smooth*
        /// path, but a GlobalTrajectory is a polyline: the heading changes discontinuously at
        /// every vertex, and the controller executes that turn in a single control cycle. At the
        /// planner's ~195 mm spacing a 7 deg vertex at 1000 mm/s alone demands 12500 mm/s^2,
        /// which swamps everything TOPPRA achieved. Resampling so the spacing is on the order of
        /// the distance travelled per cycle spreads the turn out and the guarantee transfers.
        int parametrizationSamples{500};
 
        /// Fraction of the motor torque the reparametrization may use.
        float parametrizationTorqueFraction{1.F};
    };
 
    /// Read the scene description. Throws on malformed input.
    Config readConfig(const std::filesystem::path& filename);
 
    /**
     * @brief Build a costmap holding, per cell, the distance to the closest obstacle
     *        reduced by the robot radius (clipped to [0, costmapMaxDistance]).
     *
     * A value of zero marks a cell in collision, which is what `Costmap::isInCollision`
     * tests for. Distances are computed analytically from the box obstacles, so the grid
     * is exact up to the cell discretization.
     */
    algorithms::Costmap buildCostmap(const Config& config);
 
} // namespace armarx::navigation::analysis