This file is part of ArmarX. More...

Namespaces
namespace	io
	This file is part of ArmarX.

Classes
struct	BoundedTrackingResidual
	Bounded tracking residual. More...

struct	CenterPoint

class	Costmap2DWrapper
	Differentiable wrapper around the standard 2-D distance-to-obstacle costmap. More...

struct	GeometryLimits
	Thresholds for what counts as a geometrically usable path. More...

struct	MidpointObstacleResidual2D

struct	ObstacleResidual2D

struct	PositionSmoothResidual

struct	SegmentObstacleResidual2D

struct	SoftTrackingResidual

struct	SpacingResidual

class	SPFASmoothing

class	TrajectoryChecker2D

Functions
std::vector< std::size_t >	findGeometryViolations (const core::GlobalTrajectory &trajectory, const GeometryLimits &limits={})
	Indices of waypoints that make the path double back on itself.

std::vector< std::size_t >	findTurnRateViolations (const core::GlobalTrajectory &trajectory, const GeometryLimits &limits={})
	Segments whose commanded heading turns faster than the base could follow.

double	maxTurnRate (const core::GlobalTrajectory &trajectory, const GeometryLimits &limits={})
	The largest `\|dyaw\| / segment length` in the trajectory [rad/mm].

double	normalizeAngle (double a)

core::GlobalTrajectory	repairGeometry (const core::GlobalTrajectory &trajectory, std::vector< std::size_t > &removed, const GeometryLimits &limits={})
	Drop the offending waypoints, repeating until the path is monotone.

Detailed Description

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/.

Function Documentation

◆ findGeometryViolations()

std::vector< std::size_t > findGeometryViolations	(	const core::GlobalTrajectory &	trajectory,
		const GeometryLimits &	limits = {} )

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.

Definition at line 41 of file PathGeometry.cpp.

Here is the caller graph for this function:

◆ findTurnRateViolations()

std::vector< std::size_t > findTurnRateViolations	(	const core::GlobalTrajectory &	trajectory,
		const GeometryLimits &	limits = {} )

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.

Definition at line 291 of file PathGeometry.cpp.

◆ maxTurnRate()

double maxTurnRate	(	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.

Definition at line 312 of file PathGeometry.cpp.

◆ normalizeAngle()

double normalizeAngle ( double a )

inline

Definition at line 49 of file residuals.h.

◆ repairGeometry()

core::GlobalTrajectory repairGeometry	(	const core::GlobalTrajectory &	trajectory,
		std::vector< std::size_t > &	removed,
		const GeometryLimits &	limits = {} )

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.

Parameters

removed Receives the indices removed, in terms of the original trajectory.

Returns: The repaired trajectory. Unchanged if there was nothing to repair.

Definition at line 81 of file PathGeometry.cpp.

Here is the call graph for this function: