- Generated by
1.13.2
|
|
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) |
| template<typename T> | |
| T | obstacleBarrier (const T &d_raw, const double weight, const double max_dist, const double clearance) |
| Obstacle cost: a single parabola in the distance to the nearest obstacle. | |
| 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. | |
| double | smoothstep (double edge0, double edge1, double x) |
| Hermite smoothstep: 0 below edge0, 1 above edge1, C1 at both ends. | |
| double | trackingScaleFromClearance (double clearance, double fadeStartClearance, double fadeEndClearance) |
| Scale for the tracking weight, from the clearance at a waypoint's reference position. | |
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/.
| 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.
| 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.
| 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.
|
inline |
| T obstacleBarrier | ( | const T & | d_raw, |
| const double | weight, | ||
| const double | max_dist, | ||
| const double | clearance ) |
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.
| 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.
| removed | Receives the indices removed, in terms of the original trajectory. |
|
inline |
Hermite smoothstep: 0 below edge0, 1 above edge1, C1 at both ends.
|
inline |
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.
1.13.2