NavigationStackGeneralConfig.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 )
 * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::core
{
    /**
     * @brief How the velocities along a planned path are assigned.
     *
     * The planner produces a *spatial* trajectory -- one linear speed per waypoint, taken from
     * the obstacle-aware limit -- and nothing in it knows what the base can dynamically achieve.
     * This selects what happens to that profile before it is executed.
     */
    enum class TrajectoryParametrization
    {
        /// Leave the planner's velocities alone.
        None,
 
        /// Ramp down at the start, the goal and every corner. The stack's historical behaviour.
        Ramping,
 
        /// Time-optimal under per-motor torque and command-ramp limits. Needs the python
        /// package; throws at runtime when it was not available at build time.
        Toppra
    };
 
    /// The lowercase name used in configs, logs and the analysis application's results.
    std::string ToString(TrajectoryParametrization parametrization);
 
    struct GeneralConfig
    {
        // if the desired velocity is below this threshold, the platform will not be moved
        core::TwistLimits minVel{.linear = 0.F, .angular = 0.F};
 
        //! max velocity
        core::TwistLimits maxVel{.linear = 200.F, .angular = 0.2F};
 
        //! How the planned velocity profile is (re-)assigned before execution.
        TrajectoryParametrization parametrization{TrajectoryParametrization::Ramping};
 
        // Ramping at start and end of trajectory. Read by TrajectoryParametrization::Ramping.
        bool enableRampingStart = true;
        bool enableRampingEnd = true;
        bool enableRampingCorners = true;
        float rampLength{1000}; // [mm]
        float cornerVelocity = 200; // [mm/s]
        /**
         * @brief Speed the profile is floored at [mm/s].
         *
         * Needed at the *start*: a `GlobalTrajectory` assigns a velocity to a position, so
         * `v(s = 0) = 0` is a fixed point -- the controller looks up zero at the pose it is
         * standing in, commands zero, and the robot never leaves the start.
         */
        float boundaryVelocity = 150; // [mm/s]
 
        /**
         * @brief Speed the profile is allowed to fall to at the *goal* [mm/s].
         *
         * The start's argument does not apply here: nothing has to get the robot moving again
         * once it is at the goal, and a floor of `boundaryVelocity` there is precisely what it
         * overshoots on. The feed-forward tracks the profile up to the last waypoint, so the
         * robot arrives at the goal still commanded at that speed; the controller's
         * final-segment law then steps the demand down in a single cycle and the command slew
         * needs `v^2 / (2 a)` of run-out to absorb it -- measured at 45-59 mm on ARMAR-7 with
         * the historical 150 mm/s.
         *
         * Kept above zero rather than at it, so the fixed-point argument cannot bite here
         * either if the robot is nudged back onto the tail of the profile.
         */
        float goalVelocity = 10; // [mm/s]
        // the minimum angle between two trajectory segments to be considered a corner (regarding ramping)
        float cornerLimit = 35; // [deg]
 
        //! Maximum distance [mm] to search for a valid recovery position when robot starts in collision
        float inCollisionDistanceThresholdForRecovery{100.0F};
 
        //! If true and the goal is not reachable, navigate to the closest reachable position instead of failing
        bool navigateCloseAsPossible{false};
 
        virtual ~GeneralConfig() = default;
 
        virtual aron::data::DictPtr toAron() const;
 
        static GeneralConfig FromAron(const aron::data::DictPtr& dict);
    };
 
} // namespace armarx::navigation::core