PathGeometry.cpp

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/>.
 */
 
#include "PathGeometry.h"
 
#include <algorithm>
#include <cmath>
#include <limits>
 
#include <Eigen/Core>
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    namespace
    {
        Eigen::Vector2d
        step(const core::GlobalTrajectory& trajectory, const std::size_t from)
        {
            const auto& a = trajectory.points().at(from).waypoint.pose.translation();
            const auto& b = trajectory.points().at(from + 1).waypoint.pose.translation();
 
            return Eigen::Vector2d{static_cast<double>(b.x() - a.x()),
                                   static_cast<double>(b.y() - a.y())};
        }
    } // namespace
 
    std::vector<std::size_t>
    findGeometryViolations(const core::GlobalTrajectory& trajectory, const GeometryLimits& limits)
    {
        std::vector<std::size_t> violations;
 
        const std::size_t count = trajectory.points().size();
 
        if (count < 3)
        {
            return violations;
        }
 
        for (std::size_t i = 1; i + 1 < count; i++)
        {
            const Eigen::Vector2d incoming = step(trajectory, i - 1);
            const Eigen::Vector2d outgoing = step(trajectory, i);
 
            const double incomingLength = incoming.norm();
            const double outgoingLength = outgoing.norm();
 
            // A vertex with no length on either side has no direction to compare, and the
            // heading read off it downstream is noise either way.
            if (incomingLength < limits.minSegmentLength or
                outgoingLength < limits.minSegmentLength)
            {
                violations.push_back(i);
                continue;
            }
 
            const double alignment = incoming.dot(outgoing) / (incomingLength * outgoingLength);
 
            if (alignment <= limits.minDirectionDot)
            {
                violations.push_back(i);
            }
        }
 
        return violations;
    }
 
    core::GlobalTrajectory
    repairGeometry(const core::GlobalTrajectory& trajectory,
                   std::vector<std::size_t>& removed,
                   const GeometryLimits& limits)
    {
        removed.clear();
 
        // Indices are reported against the trajectory as it stands, so they are mapped back to
        // the original numbering after each pass -- the caller logs them, and an index that
        // shifted with every removal would not name the waypoint the planner produced.
        std::vector<std::size_t> originalIndices(trajectory.points().size());
        for (std::size_t i = 0; i < originalIndices.size(); i++)
        {
            originalIndices.at(i) = i;
        }
 
        core::GlobalTrajectory repaired = trajectory;
 
        // One fold can span several waypoints, and removing one can expose the next. Bounded so a
        // pathological path cannot spin here: this runs inside a planning request.
        constexpr int maxPasses = 8;
 
        for (int pass = 0; pass < maxPasses; pass++)
        {
            const std::vector<std::size_t> violations = findGeometryViolations(repaired, limits);
 
            if (violations.empty())
            {
                break;
            }
 
            // A single displaced waypoint reverses *two* consecutive direction pairs -- the step
            // into it and the step out of it -- so it shows up as a run of adjacent violations
            // while only one waypoint is actually at fault. Removing the whole run would discard
            // an innocent neighbour and pull the path off the route the optimizer chose.
            //
            // The culprit is identified by length: a fold makes the path travel backwards and
            // then forwards again, so removing the displaced point shortens the local path more
            // than removing any of its neighbours does.
            std::vector<std::size_t> toRemove;
 
            for (std::size_t runStart = 0; runStart < violations.size();)
            {
                std::size_t runEnd = runStart;
                while (runEnd + 1 < violations.size() and
                       violations.at(runEnd + 1) == violations.at(runEnd) + 1)
                {
                    runEnd++;
                }
 
                const std::size_t first = violations.at(runStart);
                const std::size_t last = violations.at(runEnd);
 
                // Span one waypoint either side of the run, which is what any single removal
                // inside it can affect.
                const std::size_t from = (first > 0) ? first - 1 : 0;
                const std::size_t to = std::min(last + 1, repaired.points().size() - 1);
 
                std::size_t bestCandidate = last;
                double bestLength = std::numeric_limits<double>::max();
                double bestSpacingSpread = std::numeric_limits<double>::max();
 
                for (std::size_t candidate = first; candidate <= last; candidate++)
                {
                    std::vector<double> segments;
                    std::size_t previous = from;
 
                    for (std::size_t i = from + 1; i <= to; i++)
                    {
                        if (i == candidate)
                        {
                            continue;
                        }
 
                        const auto& a = repaired.points().at(previous).waypoint.pose.translation();
                        const auto& b = repaired.points().at(i).waypoint.pose.translation();
                        segments.push_back(std::hypot(static_cast<double>(b.x() - a.x()),
                                                      static_cast<double>(b.y() - a.y())));
                        previous = i;
                    }
 
                    double length = 0.0;
                    for (const double segment : segments)
                    {
                        length += segment;
                    }
 
                    // On a straight run every candidate removal yields the same length, so length
                    // alone cannot say which waypoint is the displaced one. Spacing does: the
                    // optimizer drives the path towards uniform spacing (`SpacingResidual`), so
                    // the removal that leaves the segments most even is the one that took out the
                    // outlier rather than one of its innocent neighbours.
                    double spacingSpread = 0.0;
                    if (not segments.empty())
                    {
                        const double mean = length / static_cast<double>(segments.size());
                        for (const double segment : segments)
                        {
                            spacingSpread += (segment - mean) * (segment - mean);
                        }
                    }
 
                    const double lengthTolerance = 1e-6 * std::max(1.0, bestLength);
                    const bool shorter = length < bestLength - lengthTolerance;
                    const bool sameLengthButMoreEven =
                        std::abs(length - bestLength) <= lengthTolerance and
                        spacingSpread < bestSpacingSpread;
 
                    if (shorter or sameLengthButMoreEven)
                    {
                        bestLength = length;
                        bestSpacingSpread = spacingSpread;
                        bestCandidate = candidate;
                    }
                }
 
                toRemove.push_back(bestCandidate);
                runStart = runEnd + 1;
            }
 
            std::vector<core::GlobalTrajectoryPoint> kept;
            std::vector<std::size_t> keptIndices;
            kept.reserve(repaired.points().size());
            keptIndices.reserve(originalIndices.size());
 
            for (std::size_t i = 0; i < repaired.points().size(); i++)
            {
                const bool drop = std::find(toRemove.begin(), toRemove.end(), i) != toRemove.end();
 
                if (drop)
                {
                    removed.push_back(originalIndices.at(i));
                    continue;
                }
 
                kept.push_back(repaired.points().at(i));
                keptIndices.push_back(originalIndices.at(i));
            }
 
            // Never strip the path down to something that is no longer a path.
            if (kept.size() < 3)
            {
                break;
            }
 
            repaired = core::GlobalTrajectory{kept};
            originalIndices = keptIndices;
        }
 
        std::sort(removed.begin(), removed.end());
 
        return repaired;
    }
 
    namespace
    {
        /// Yaw of a planar pose. `atan2` on the rotation's first column rather than an rpy
        /// decomposition, which is unstable when the matrix carries numerical pitch.
        double
        yawOf(const core::Pose& pose)
        {
            return std::atan2(static_cast<double>(pose.linear()(1, 0)),
                              static_cast<double>(pose.linear()(0, 0)));
        }
 
        /// Signed difference wrapped to (-pi, pi], i.e. the way the base would actually turn.
        /// Without the wrap a profile crossing the +-pi branch cut reads as a full revolution.
        double
        shortestAngularDiff(const double after, const double before)
        {
            double diff = after - before;
 
            while (diff > M_PI)
            {
                diff -= 2.0 * M_PI;
            }
            while (diff <= -M_PI)
            {
                diff += 2.0 * M_PI;
            }
 
            return diff;
        }
 
        /// Turn rate of the segment starting at `index`, or 0 for a segment too short to carry a
        /// meaningful one -- those are the fold check's business, and dividing by them here would
        /// report every one of them as a turn-rate violation too.
        double
        segmentTurnRate(const core::GlobalTrajectory& trajectory,
                        const std::size_t index,
                        const GeometryLimits& limits)
        {
            const auto& points = trajectory.points();
 
            const double length = (points.at(index + 1).waypoint.pose.translation() -
                                   points.at(index).waypoint.pose.translation())
                                      .norm();
 
            if (not(length > limits.minSegmentLength))
            {
                return 0.0;
            }
 
            const double delta = shortestAngularDiff(yawOf(points.at(index + 1).waypoint.pose),
                                                     yawOf(points.at(index).waypoint.pose));
 
            return std::abs(delta) / length;
        }
    } // namespace
 
    std::vector<std::size_t>
    findTurnRateViolations(const core::GlobalTrajectory& trajectory, const GeometryLimits& limits)
    {
        std::vector<std::size_t> violations;
 
        if (trajectory.points().size() < 2 or not(limits.maxTurnRate > 0.0))
        {
            return violations;
        }
 
        for (std::size_t i = 0; i + 1 < trajectory.points().size(); i++)
        {
            if (segmentTurnRate(trajectory, i, limits) > limits.maxTurnRate)
            {
                violations.push_back(i);
            }
        }
 
        return violations;
    }
 
    double
    maxTurnRate(const core::GlobalTrajectory& trajectory, const GeometryLimits& limits)
    {
        double worst = 0.0;
 
        for (std::size_t i = 0; i + 1 < trajectory.points().size(); i++)
        {
            worst = std::max(worst, segmentTurnRate(trajectory, i, limits));
        }
 
        return worst;
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing