orientation_aware Directory Reference

Directory dependency graph for orientation_aware:

Directories
	smoothing

Files
	AStarPlanner.cpp
	AStarPlanner.h
	collision_checking.cpp
	collision_checking.h
	Costmap3D.cpp
	Costmap3D.h
	Costmap3DBuilder.cpp
	Costmap3DBuilder.h
	io.cpp
	io.h
	Node.cpp
	Node.h
	TrajectoryChecker.cpp
	TrajectoryChecker.h
	util.h

Detailed Description

This folder contains the orientation-aware trajectory planner for mobile robot navigation, including A* path planning, 3D collision-aware costmap generation, and Ceres-based trajectory smoothing.

Architecture Overview

The trajectory generation pipeline follows a two-stage approach:

Stage 1: Pre-processing (Hill-climbing)

Interior waypoints from the raw A* path are pushed into the safe zone by hill-climbing on the signed-distance field (SDF). The SDF's local maxima correspond to corridor centres, so this naturally places waypoints in the middle of free space while respecting the full robot footprint (orientation-aware).

Key design points:

• Only interior waypoints are moved; start and goal remain fixed.
• Candidates are rejected if they fall outside the costmap bounds.
• The pre-processed safe positions are saved as tracking targets for Stage 2.

Stage 2: Ceres Smoothing

Ceres optimises the trajectory using a set of weighted residuals. The obstacle residuals query the costmap with the dynamically-optimised theta and, when useFuzzyOrientation is enabled, take the smooth minimum over a small angular window around that theta. This propagates a strong orientation gradient to the optimiser while keeping the residual differentiable across the 72-bin discrete orientation representation.

Residuals used:

• Pose smoothness (second-difference on x, y, theta)
• Velocity smoothness (second-difference on v)
• Spacing (keeps segment lengths close to average)
• Obstacle (waypoint + midpoint, with theta-aware fuzzy orientation window)
• Tracking (pulls toward pre-processed safe positions)
• Velocity limit (hinge penalty on v > vmax)
• Robot smoothness (optional, smooths actual robot base pose)

Key Design Decisions

True Euclidean SDF

The costmap stores true Euclidean obstacle distances for the full robot footprint at each of 72 discrete orientations. Collision cells (distance <= 0) are converted to negative Euclidean distances using OpenCV's cv::distanceTransform with DIST_L2 and DIST_MASK_PRECISE. This gives correct gradients that point directly toward the nearest free space, unlike the old Manhattan-style flood-fill pseudo-SDF.

Fuzzy Orientation Obstacle Queries

Because the costmap is discrete in orientation (5 deg bins), small changes in theta can cause the costmap value to jump discontinuously at bin boundaries. To give Ceres a strong but still differentiable orientation signal, the obstacle residuals sample the costmap at several orientations around the current theta (by default ±10°, i.e. 2 bins on each side) and use the smooth minimum of those samples. Each sample orientation is a differentiable function of theta, so the obstacle residual propagates a meaningful theta gradient while the smooth-min softens the discrete bin transitions.

Bilinear Position Interpolation

Costmap3DWrapper uses bilinear interpolation in (x, y) and nearest-neighbour in theta. When a query point lands exactly on a grid line, the bilinear weights can sum to zero. A degeneracy guard detects this and falls back to nearest-neighbour to avoid returning an incorrect zero cost.

Midpoint Obstacle Residual

A dedicated residual evaluates obstacle cost at the midpoint between consecutive waypoints. This is essential because two free waypoints can still have a straight segment that cuts through an obstacle.

File Structure

File	Purpose
Costmap3D.h/cpp	3D orientation-aware costmap grid, Euclidean SDF conversion
Costmap3DBuilder.h/cpp	Builds the costmap from scene geometry and robot footprint
AStarPlanner.cpp	Orientation-aware A* planner that calls the smoother
smoothing/residuals.h	All Ceres residuals and the main optimizeTrajectoryCeres function
smoothing/Smoothing.cpp	Pre-processing (hill-climbing), collision checking, orchestration
smoothing/Costmap3DWrapper.h	Differentiable wrapper around Costmap3D for Ceres AutoDiff
smoothing/io.h	Config parameter struct and parser
collision_checking.h/cpp	Collision checking utilities

Configuration

Parameters are read from data/armarx_navigation/algorithms/orientation-aware-smoothing.cfg:

Key	Default	Description
weightBoundary	1.0	Start/end rotation smoothness
weightPoseSmooth	20.0	Centre pose second-difference weight
weightPoseSmoothOrientation	200.0	Orientation smoothness weight
weightVelocitySmooth	0.05	Velocity smoothness weight
weightRobotSmooth	0.0	Robot-base smoothness weight (requires realistic offset)
weightObstacle	150.0	Obstacle penalty weight
weightTracking	5.0	Tracking-to-pre-processed-target weight
weightSpacing	0.2	Segment spacing weight
clearance	100.0	Desired safety margin in mm
obsMaxDistance	300.0	Obstacle residual cut-off distance in mm
vmax	1.0	Maximum velocity
useFuzzyOrientation	true	Enable smooth-min orientation window for obstacle queries
fuzzyOrientationWindowBins	2	Number of 5° bins to sample on each side of theta
fuzzyOrientationAlpha	5.0	Smooth-min sharpness over the orientation window
maxIterations	50	Ceres maximum iterations
numThreads	4	Ceres solver threads

Notes

• All distances are in millimetres (mm).
• The costmap already bakes in the full robot footprint (including obstacleSafetyMargin) during construction. Do not subtract additional artificial margins in the wrapper or residuals.
• If Ceres reports NO_CONVERGENCE, check that maxIterations is large enough and that weightObstacle is not so high that the problem becomes too stiff.
• If the optimizer ignores orientation near obstacles, increase fuzzyOrientationAlpha or reduce weightOrientationSmooth. If optimization becomes unstable, decrease fuzzyOrientationAlpha or reduce fuzzyOrientationWindowBins.