residuals.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 <algorithm>
#include <cmath>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/spfa/smoothing/Costmap2DWrapper.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
 
#include <ceres/ceres.h>
 
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1
#  include <ceres/manifold.h>
    using SubsetConstraint = ceres::SubsetManifold;
#  define SET_CONSTRAINT(problem, param, constraint) \
        (problem).SetManifold((param), (constraint))
#else
#  include <ceres/local_parameterization.h>
    using SubsetConstraint = ceres::SubsetParameterization;
#  define SET_CONSTRAINT(problem, param, constraint) \
        (problem).SetParameterization((param), (constraint))
#endif
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    struct CenterPoint
    {
        double x, y, theta, v;
    };
 
    inline double normalizeAngle(double a)
    {
        return std::atan2(std::sin(a), std::cos(a));
    }
 
    static void normalizeTrajectoryAngles(std::vector<CenterPoint>& traj)
    {
        for (auto& p : traj)
            p.theta = normalizeAngle(p.theta);
    }
 
    // -----------------------------
    // Position-only smoothness residual
    // -----------------------------
    struct PositionSmoothResidual
    {
        explicit PositionSmoothResidual(double weight = 1.0) : w(weight) {}
 
        template <typename T>
        bool operator()(const T* const c_prev,
                        const T* const c_i,
                        const T* const c_next,
                        T* residual) const
        {
            residual[0] = T(w) * (c_prev[0] - T(2.0) * c_i[0] + c_next[0]); // x
            residual[1] = T(w) * (c_prev[1] - T(2.0) * c_i[1] + c_next[1]); // y
            return true;
        }
 
        double w;
    };
 
    /// Hermite smoothstep: 0 below edge0, 1 above edge1, C1 at both ends.
    inline double smoothstep(double edge0, double edge1, double x)
    {
        if (x <= edge0)
        {
            return 0.0;
        }
        if (x >= edge1)
        {
            return 1.0;
        }
 
        const double t = (x - edge0) / (edge1 - edge0);
        return t * t * (3.0 - 2.0 * t);
    }
 
    /**
     * Scale for the tracking weight, from the clearance at a waypoint's reference position.
     *
     * 1 where clearance is scarce, fading to 0 where it is plentiful, so the leash to the
     * planner's route tightens exactly where wandering is expensive and lets go where the
     * smoothness term should be free to straighten.
     *
     * This is the soft form of pinning a tight waypoint outright. Pinning makes both adjoining
     * segments half-constrained, which is the structure behind this smoother's rejections at
     * its commanded endpoints; a weight cannot do that, and it degrades gracefully instead of
     * removing a degree of freedom.
     */
    inline double trackingScaleFromClearance(double clearance,
                                             double fadeStartClearance,
                                             double fadeEndClearance)
    {
        return 1.0 - smoothstep(fadeStartClearance, fadeEndClearance, clearance);
    }
 
    // -----------------------------
    // Obstacle cost
    // -----------------------------
 
    /**
     * Obstacle cost: a single parabola in the distance to the nearest obstacle.
     *
     *     t = (d - clearance) / max_dist
     *     r = (t >= 1) ? 0 : weight * (1 - t)^2
     *
     * Three properties matter, and the form this replaced broke the first two.
     *
     * *No step.* The previous form branched at the clearance boundary into two pieces that
     * did not meet: the inside piece tended to 0 there while the outside piece tended to
     * `weight`. A trust-region solver compares predicted against actual reduction, which is
     * meaningless across a discontinuity, so steps were rejected at a boundary the optimizer
     * crosses constantly.
     *
     * *Monotone in distance.* Because of that step the collision side of the boundary was
     * strictly cheaper than the safe side, so a waypoint descending out of an obstacle was
     * rewarded for stopping just inside it. Here the cost never increases as an obstacle gets
     * further away, so there is never anything to gain by moving toward one.
     *
     * *A standoff preference that ends cleanly.* The cost reaches exactly zero at
     * `clearance + max_dist`, with zero slope, so obstacles beyond the band exert no pull and
     * cannot couple waypoints that are already comfortably clear. Within the band there is a
     * mild preference for more room, which is what keeps paths off the furniture rather than
     * merely legal.
     *
     * That last property is why this is a band and not a hinge at the clearance. An earlier
     * revision made the cost zero everywhere outside `clearance`, which removed the step but
     * also removed the only term expressing "prefer to stay away". Measured over 600 requests
     * in R007 that cost 88 mm of median clearance and put 81 more paths within 200 mm of
     * furniture, for no improvement in the acceptance rate.
     *
     * Inside the clearance the same parabola keeps growing, so `max_dist` also sets how
     * sharply penetration is punished. It is not a distance the optimizer is pushed to reach.
     */
    template <typename T>
    T obstacleBarrier(const T& d_raw,
                      const double weight,
                      const double max_dist,
                      const double clearance)
    {
        // Defensive guard: if the wrapper ever returns non-finite, treat as deep obstacle so
        // the residual stays finite and Ceres does not abort.
        if (!ceres::isfinite(d_raw))
        {
            return T(weight) * T(100.0);
        }
 
        // Keep deeply negative distances bounded, so a waypoint far inside an obstacle cannot
        // produce an arbitrarily large residual.
        T d = d_raw;
        const T min_allowed = T(-3.0 * max_dist);
        if (d < min_allowed)
        {
            d = min_allowed;
        }
 
        const T t = (d - T(clearance)) / T(max_dist);
 
        if (t >= T(1.0))
        {
            return T(0.0);
        }
 
        const T u = T(1.0) - t;
        return T(weight) * u * u;
    }
 
    // -----------------------------
    // Obstacle residuals (2-D)
    // -----------------------------
    struct ObstacleResidual2D
    {
        ObstacleResidual2D(const Costmap2DWrapper& wrapper,
                           double weight,
                           double max_dist,
                           double clearance) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance)
        {
        }
 
        template <typename T>
        bool operator()(const T* const node, T* residual) const
        {
            T d_raw;
            wrapper_(&node[0], &node[1], &d_raw);
 
            residual[0] = obstacleBarrier(d_raw, w_, max_dist_, clearance_);
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
    };
 
    struct MidpointObstacleResidual2D
    {
        MidpointObstacleResidual2D(const Costmap2DWrapper& wrapper,
                                   double weight,
                                   double max_dist,
                                   double clearance) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T mx = (c_i[0] + c_ip1[0]) / T(2.0);
            T my = (c_i[1] + c_ip1[1]) / T(2.0);
 
            T d_raw;
            wrapper_(&mx, &my, &d_raw);
 
            residual[0] = obstacleBarrier(d_raw, w_, max_dist_, clearance_);
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
    };
 
    // Obstacle residual sampled at an arbitrary position along the segment.
    // t=0 evaluates at c_i, t=1 at c_ip1; midpoints use t=0.5.
    struct SegmentObstacleResidual2D
    {
        SegmentObstacleResidual2D(const Costmap2DWrapper& wrapper,
                                  double weight,
                                  double max_dist,
                                  double clearance,
                                  double t) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance),
            t_(t)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T sx = (T(1.0) - T(t_)) * c_i[0] + T(t_) * c_ip1[0];
            T sy = (T(1.0) - T(t_)) * c_i[1] + T(t_) * c_ip1[1];
 
            T d_raw;
            wrapper_(&sx, &sy, &d_raw);
 
            residual[0] = obstacleBarrier(d_raw, w_, max_dist_, clearance_);
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
        double t_;
    };
 
    // -----------------------------
    // Tracking residuals
    // -----------------------------
 
    /**
     * Bounded tracking residual.
     *
     * - Deviation <= deadzone: 0 cost.
     * - deadzone < deviation <= max_deviation: quadratic growth.
     * - deviation > max_deviation: very steep quadratic growth (hard limit).
     */
    struct BoundedTrackingResidual
    {
        BoundedTrackingResidual(double xr,
                                double yr,
                                double deadzone,
                                double max_deviation,
                                double w_in_bounds,
                                double w_hard) :
            xr_(xr),
            yr_(yr),
            deadzone_(deadzone),
            max_deviation_(max_deviation),
            w_in_bounds_(w_in_bounds),
            w_hard_(w_hard)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, T* residual) const
        {
            T dx = c_i[0] - T(xr_);
            T dy = c_i[1] - T(yr_);
            T d = ceres::sqrt(dx * dx + dy * dy + T(1e-6));
 
            T excess = d - T(deadzone_);
            if (excess <= T(0))
            {
                residual[0] = T(0);
            }
            else if (excess <= T(max_deviation_ - deadzone_))
            {
                residual[0] = T(w_in_bounds_) * excess;
            }
            else
            {
                T over = excess - T(max_deviation_ - deadzone_);
                residual[0] = T(w_in_bounds_) * T(max_deviation_ - deadzone_) + T(w_hard_) * over;
            }
            return true;
        }
 
        double xr_, yr_;
        double deadzone_;
        double max_deviation_;
        double w_in_bounds_;
        double w_hard_;
    };
 
    // Soft tracking to the hill-climbed safe path (weak weight).
    struct SoftTrackingResidual
    {
        SoftTrackingResidual(double xr, double yr, double weight) :
            xr_(xr), yr_(yr), w_(weight)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, T* residual) const
        {
            residual[0] = T(w_) * (c_i[0] - T(xr_));
            residual[1] = T(w_) * (c_i[1] - T(yr_));
            return true;
        }
 
        double xr_, yr_;
        double w_;
    };
 
    // -----------------------------
    // Spacing residual
    // -----------------------------
    struct SpacingResidual
    {
        SpacingResidual(double d_avg, double weight) : d_avg_(d_avg), w(weight) {}
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T dx = c_ip1[0] - c_i[0];
            T dy = c_ip1[1] - c_i[1];
            T dist_sq = dx * dx + dy * dy;
            T target_sq = T(d_avg_ * d_avg_);
            residual[0] = T(w) * (dist_sq - target_sq);
            return true;
        }
 
        double d_avg_;
        double w;
    };
 
    // -----------------------------
    // Helpers
    // -----------------------------
    static double segment_length(const CenterPoint& a, const CenterPoint& b)
    {
        return std::hypot(b.x - a.x, b.y - a.y);
    }
 
    static double computeAverageSpacing(const std::vector<CenterPoint>& traj)
    {
        if (traj.size() < 2)
            return 0.0;
        double sum = 0.0;
        for (size_t i = 0; i + 1 < traj.size(); ++i)
            sum += segment_length(traj[i], traj[i + 1]);
        return sum / double(traj.size() - 1);
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing