PathGeometry.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/>.
 */
 
#pragma once
 
#include <cstddef>
#include <vector>
 
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    /**
     * @brief Thresholds for what counts as a geometrically usable path.
     *
     * Separate from the smoothing weights: these describe the *result* the optimizer has to
     * produce, not how it gets there.
     */
    struct GeometryLimits
    {
        /// Consecutive travel directions whose dot product is below this have reversed. Slightly
        /// above zero so a numerically flat vertex is not reported as a fold.
        double minDirectionDot = 0.0;
 
        /// Segments shorter than this [mm] carry no reliable direction, and the heading a
        /// downstream spline reads off them is numerical noise.
        double minSegmentLength = 1.0;
 
        /**
         * @brief Most the commanded heading may turn per millimetre travelled [rad/mm].
         *
         * A path-length bound, not a rate: `yaw'(s) * v` is the yaw rate the base is asked for,
         * so a profile that turns faster than this per millimetre can only be executed by
         * slowing down, and past some point by not moving at all.
         *
         * The default is what a base holding `limits.angular` (0.8 rad/s on ARMAR-7) can turn
         * while still making the `boundaryVelocity` the profile is floored at (150 mm/s):
         * 0.8 / 150 = 0.00533 rad/mm. Healthy ARMAR-7 runs measure 0.0016 rad/mm at their worst;
         * the run this bound was derived from reached 0.0259.
         */
        double maxTurnRate = 0.8 / 150.0; // [rad/mm]
    };
 
    /**
     * @brief Indices of waypoints that make the path double back on itself.
     *
     * Nothing in the smoother's cost function can see a fold: `SpacingResidual` compares *squared*
     * distances and so is blind to direction, and `PositionSmoothResidual` -- the discrete second
     * difference -- is zero for any point on the line through its neighbours, including one placed
     * behind its predecessor. The only acceptance gate the caller had was a collision check, which
     * a fold passes happily.
     *
     * A fold matters downstream because the reparametrization fits a spline through the path with
     * the *polyline* arc length as its parameter. That parameter keeps increasing through a
     * reversal, so nothing rejects it, but the spline tangent collapses at the cusp -- measured at
     * ~1/60 of its nominal unit norm for a single waypoint displaced 30 mm backwards. The velocity
     * bound is `v(s) * |p'(s)|` and is only equivalent to a speed limit while `|p'| == 1`, so the
     * solver is driven to a near-zero path speed exactly there: the robot stops mid-path.
     *
     * Start and goal are never reported -- they are commanded, not optimized.
     */
    std::vector<std::size_t> findGeometryViolations(const core::GlobalTrajectory& trajectory,
                                                    const GeometryLimits& limits = {});
 
    /**
     * @brief Drop the offending waypoints, repeating until the path is monotone.
     *
     * Removing a waypoint is the right repair rather than rejecting the whole result: a fold is
     * normally one point that overshot, and `BoundedTrackingResidual` keeps its neighbours near
     * the reference path, so the remainder is still the path the optimizer intended. Removing one
     * point does create a new segment between its neighbours, which can cut a corner -- the caller
     * must re-run the collision check on the result.
     *
     * @param removed Receives the indices removed, in terms of the *original* trajectory.
     * @return The repaired trajectory. Unchanged if there was nothing to repair.
     */
    core::GlobalTrajectory repairGeometry(const core::GlobalTrajectory& trajectory,
                                          std::vector<std::size_t>& removed,
                                          const GeometryLimits& limits = {});
 
    /**
     * @brief Segments whose commanded heading turns faster than the base could follow.
     *
     * A separate failure from a fold, and one the fold check cannot see: the *position* path can
     * be perfectly smooth -- on the run this was derived from its tangent held to within 0.08 deg
     * per segment -- while the orientation assigned to it sweeps 116 deg over 300 mm. Both
     * checks pass such a path today, and the reparametrization then has to choose between
     * violating the angular limit and stopping.
     *
     * Reported per segment, as the index of the waypoint the segment starts at.
     *
     * Unlike a fold this is not repairable by dropping waypoints: the rotation is spread over
     * many of them and removing any subset only makes the remaining ones turn faster. The caller
     * has to discard the profile and fall back to one it can execute.
     */
    std::vector<std::size_t> findTurnRateViolations(const core::GlobalTrajectory& trajectory,
                                                    const GeometryLimits& limits = {});
 
    /// The largest `|dyaw| / segment length` in the trajectory [rad/mm]. Zero for fewer than two
    /// points. Reported so a caller can log how far out a rejected profile was.
    double maxTurnRate(const core::GlobalTrajectory& trajectory, const GeometryLimits& limits = {});
 
} // namespace armarx::navigation::algorithms::spfa::smoothing