ObstacleAwareVelocityLimit.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 <vector>
 
#include <Eigen/Core>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms
{
 
    /**
     * @brief The maximum permissible linear velocity as a function of the distance to the
     *        closest obstacle.
     *
     * The limit decays from `maxVelocity` in free space to `maxVelocity / (1 + weight)` at zero
     * clearance:
     *
     * \f[
     *   v_{max}(d) = \frac{v_{max}}{1 + w \left(1 - \frac{\min(d, d_{max})}{d_{max}}\right)^{e}}
     * \f]
     *
     * This used to live as a private member function of `global_planning::SPFAImpl`, duplicated
     * once more in its final velocity clamp. It is a standalone concept, so it lives here and can
     * be used to *verify* a trajectory produced by any planner: for a path that respects the
     * limit, `at(position) >= velocity` holds at every trajectory point.
     *
     * @note The referenced costmap must outlive this object.
     */
    class ObstacleAwareVelocityLimit
    {
    public:
        struct Parameters
        {
            /// Permissible velocity far away from any obstacle [mm/s].
            float maxVelocity{200.F};
 
            /// Distances are clipped to this value; beyond it the limit is `maxVelocity` [mm].
            float obstacleMaxDistance{500.F};
 
            /// Strength of the reduction. At zero clearance the limit is `maxVelocity / (1 + w)`.
            float obstacleDistanceWeight{2.F};
 
            /// Exponent of the proximity term. Larger values keep full speed closer to obstacles.
            float obstacleCostExponent{4.F};
        };
 
        ObstacleAwareVelocityLimit(const Costmap& costmap, const Parameters& params);
 
        /// The limit for a given distance to the closest obstacle [mm].
        float atDistance(float obstacleDistance) const;
 
        /**
         * @brief The limit at a position in the costmap's global frame.
         *
         * Cells without costmap information (masked out) are treated as zero clearance, i.e. the
         * most restrictive limit. Not knowing how far away the obstacles are is not a reason to
         * drive fast.
         */
        float at(const Eigen::Vector2f& position) const;
 
        std::vector<float> at(const core::Positions& positions) const;
 
        const Parameters& params() const noexcept;
 
    private:
        const Costmap& costmap_;
 
        const Parameters params_;
    };
 
} // namespace armarx::navigation::algorithms