Classes
class	ParabolicSmoother
	Class for performing parabolic smoothing on trajectories. More...

class	ParabolicTimer
	Class for performing parabolic retiming on trajectories. More...

Functions
std::unique_ptr< trajectory::Spline >	doShortcut (const trajectory::Spline &_inputTrajectory, aikido::constraint::TestablePtr _feasibilityCheck, const Eigen::VectorXd &_maxVelocity, const Eigen::VectorXd &_maxAcceleration, aikido::common::RNG &_rng, double _timelimit=DEFAULT_TIMELIMT, double _checkResolution=DEFAULT_CHECK_RESOLUTION, double _tolerance=DEFAULT_TOLERANCE)
	Shortcut waypoints in a trajectory using parabolic splines. More...

std::unique_ptr< trajectory::Spline >	doBlend (const trajectory::Spline &_inputTrajectory, aikido::constraint::TestablePtr _feasibilityCheck, const Eigen::VectorXd &_maxVelocity, const Eigen::VectorXd &_maxAcceleration, double _blendRadius=DEFAULT_BLEND_RADIUS, int _blendIterations=DEFAULT_BLEND_ITERATIONS, double _checkResolution=DEFAULT_CHECK_RESOLUTION, double _tolerance=DEFAULT_TOLERANCE)
	Blend around waypoints in a trajectory using parabolic splines. More...

std::unique_ptr< trajectory::Spline >	doShortcutAndBlend (const trajectory::Spline &_inputTrajectory, aikido::constraint::TestablePtr _feasibilityCheck, const Eigen::VectorXd &_maxVelocity, const Eigen::VectorXd &_maxAcceleration, aikido::common::RNG &_rng, double _timelimit=DEFAULT_TIMELIMT, double _blendRadius=DEFAULT_BLEND_RADIUS, int _blendIterations=DEFAULT_BLEND_ITERATIONS, double _checkResolution=DEFAULT_CHECK_RESOLUTION, double _tolerance=DEFAULT_TOLERANCE)
	Shortcut and blends waypoints in a trajectory using parabolic splines. More...

std::unique_ptr< aikido::trajectory::Spline >	computeParabolicTiming (const aikido::trajectory::Interpolated &_inputTrajectory, const Eigen::VectorXd &_maxVelocity, const Eigen::VectorXd &_maxAcceleration)
	Computes the time-optimal timing of a trajectory consisting of a sequence Geodesic interpolations between states under velocity and acceleration bounds. More...

std::unique_ptr< aikido::trajectory::Spline >	computeParabolicTiming (const aikido::trajectory::Spline &_inputTrajectory, const Eigen::VectorXd &_maxVelocity, const Eigen::VectorXd &_maxAcceleration)
	Computes the time-optimal timing of a trajectory consisting of a linear spline between states under velocity and acceleration bounds. More...

Variables
constexpr double	DEFAULT_TIMELIMT = 3.0

constexpr double	DEFAULT_BLEND_RADIUS = 0.5

constexpr int	DEFAULT_BLEND_ITERATIONS = 4

constexpr double	DEFAULT_CHECK_RESOLUTION = 1e-4

constexpr double	DEFAULT_TOLERANCE = 1e-3

Function Documentation

◆ computeParabolicTiming() [1/2]

std::unique_ptr<aikido::trajectory::Spline> aikido::planner::parabolic::computeParabolicTiming	(	const aikido::trajectory::Interpolated &	_inputTrajectory,
		const Eigen::VectorXd &	_maxVelocity,
		const Eigen::VectorXd &	_maxAcceleration
	)

Computes the time-optimal timing of a trajectory consisting of a sequence Geodesic interpolations between states under velocity and acceleration bounds.

The output is a parabolic spline, encoded in cubic polynomials, that exactly follows the input path.

The output trajectory consists of a sequence of trapezoidal velocity profiles that implement bang-bang control. This trajectory must stop at each waypoint that introduces a velocity discontinuity to satisfy finite acceleration bounds. You should consider using a blending or smoothing algorithm, which does not follow the exact input path, if this behavior is undesirable.

This function curently only supports RealVector, SO2, and compound state spaces of those types. Additionally, this function requires that _inputTrajectory to be interpolated using a GeodesicInterpolator.

Parameters

_inputTrajectory	input piecewise Geodesic trajectory
_maxVelocity	maximum velocity for each dimension
_maxAcceleration	maximum acceleration for each dimension

Returns: time optimal trajectory that satisfies acceleration constraints

◆ computeParabolicTiming() [2/2]

std::unique_ptr<aikido::trajectory::Spline> aikido::planner::parabolic::computeParabolicTiming	(	const aikido::trajectory::Spline &	_inputTrajectory,
		const Eigen::VectorXd &	_maxVelocity,
		const Eigen::VectorXd &	_maxAcceleration
	)

Computes the time-optimal timing of a trajectory consisting of a linear spline between states under velocity and acceleration bounds.

The output is a parabolic spline, encoded in cubic polynomials, that exactly follows the input path.

The output trajectory consists of a sequence of trapezoidal velocity profiles that implement bang-bang control. This trajectory must stop at each waypoint that introduces a velocity discontinuity to satisfy finite acceleration bounds. You should consider using a blending or smoothing algorithm, which does not follow the exact input path, if this behavior is undesirable.

This function curently only supports RealVector, SO2, and compound state spaces of those types. Additionally, this function requires that _inputTrajectory to be interpolated using a GeodesicInterpolator.

Parameters

_inputTrajectory	linear spline trajectory
_maxVelocity	maximum velocity for each dimension
_maxAcceleration	maximum acceleration for each dimension

Returns: time optimal trajectory that satisfies acceleration constraints

◆ doBlend()

std::unique_ptr<trajectory::Spline> aikido::planner::parabolic::doBlend	(	const trajectory::Spline &	_inputTrajectory,
		aikido::constraint::TestablePtr	_feasibilityCheck,
		const Eigen::VectorXd &	_maxVelocity,
		const Eigen::VectorXd &	_maxAcceleration,
		double	_blendRadius = `DEFAULT_BLEND_RADIUS`,
		int	_blendIterations = `DEFAULT_BLEND_ITERATIONS`,
		double	_checkResolution = `DEFAULT_CHECK_RESOLUTION`,
		double	_tolerance = `DEFAULT_TOLERANCE`
	)

Blend around waypoints in a trajectory using parabolic splines.

This function smooths _inputTrajectory by blending around waypoints that have zero velocity using the following algorithm:

while _inputTrajectory has a waypoint with zero velocity:

Construct the time-optimal parabolic spline from the state of the trajectory at time t - _blendRadius to the state of the trajectory at time t + _blendRadius.
Check the validity of the segment against _feasabilityCheck.
If the segment is valid, replace the portion of the trajectory between times t - _blendRadius and t + _blendRadius with the segment.

If blending with _blendRadius fails to remove all waypoints with zero velocity, the process described above repeats with half the initial _blendRadius. Halving occurs _blendIteration times, with the final iteration using a blend radius of _blendRadius / std::pow(2, _blendIterations - 1).

Parameters

_inputTrajectory	input piecewise Geodesic trajectory
_feasibilityCheck	Check whether a position is feasible
_maxVelocity	maximum velocity for each dimension
_maxAcceleration	maximum acceleration for each dimension
_blendRadius	the radius used in doing blend
_blendIterations	the maximum iteration number in doing blend
_checkResolution	the resolution in discretizing a segment in checking the feasibility of the segment
_tolerance	this tolerance is used in a piecewise linear discretization that deviates no more than `_tolerance` from the parabolic ramp along any axis, and then checks for configuration and segment feasibility along that piecewise linear path.

Returns: smoothed trajectory that satisfies acceleration constraints

◆ doShortcut()

std::unique_ptr<trajectory::Spline> aikido::planner::parabolic::doShortcut	(	const trajectory::Spline &	_inputTrajectory,
		aikido::constraint::TestablePtr	_feasibilityCheck,
		const Eigen::VectorXd &	_maxVelocity,
		const Eigen::VectorXd &	_maxAcceleration,
		aikido::common::RNG &	_rng,
		double	_timelimit = `DEFAULT_TIMELIMT`,
		double	_checkResolution = `DEFAULT_CHECK_RESOLUTION`,
		double	_tolerance = `DEFAULT_TOLERANCE`
	)

Shortcut waypoints in a trajectory using parabolic splines.

This function smooths ‘_inputTrajectory’ by iteratively sampling two waypoints in a trajectory and trying to find a shortcut using the following algorithm:

while _timelimit is not out:

Randomly sample two times uniformly at random.
Find two points according two times in _inputTrajectory.
Attempt to connect the two points with a time-optimal parabolic spline. The feasibility of the spline is evaluated using _feasibilityCheck .

Parameters

_inputTrajectory	input piecewise Geodesic trajectory
_feasibilityCheck	Check whether a position is feasible
_maxVelocity	maximum velocity for each dimension
_maxAcceleration	maximum acceleration for each dimension
_rng	A random generator for sampling time in shortcut.
_timelimit	The maximum time to allow for doing shortcut
_checkResolution	the resolution in discretizing a segment in checking the feasibility of the segment
_tolerance	this tolerance is used in a piecewise linear discretization that deviates no more than `_tolerance` from the parabolic ramp along any axis, and then checks for configuration and segment feasibility along that piecewise linear path.

Returns: smoothed trajectory that satisfies acceleration constraints

◆ doShortcutAndBlend()

std::unique_ptr<trajectory::Spline> aikido::planner::parabolic::doShortcutAndBlend	(	const trajectory::Spline &	_inputTrajectory,
		aikido::constraint::TestablePtr	_feasibilityCheck,
		const Eigen::VectorXd &	_maxVelocity,
		const Eigen::VectorXd &	_maxAcceleration,
		aikido::common::RNG &	_rng,
		double	_timelimit = `DEFAULT_TIMELIMT`,
		double	_blendRadius = `DEFAULT_BLEND_RADIUS`,
		int	_blendIterations = `DEFAULT_BLEND_ITERATIONS`,
		double	_checkResolution = `DEFAULT_CHECK_RESOLUTION`,
		double	_tolerance = `DEFAULT_TOLERANCE`
	)

Shortcut and blends waypoints in a trajectory using parabolic splines.

This function smooths ‘_inputTrajectory’ by firstly applying shortcut to _inputTrajectory and then using blend to remove segments that have zero velocities. Calling this function is more efficienct than calling those two functions in sequence because it avoids duplicated effort.

Parameters

_inputTrajectory	input piecewise Geodesic trajectory
_feasibilityCheck	Check whether a position is feasible
_maxVelocity	maximum velocity for each dimension
_maxAcceleration	maximum acceleration for each dimension
_rng	A random generator for sampling time in shortcut.
_timelimit	The maximum time to allow for doing shortcut (unit in second)
_blendRadius	the radius used in doing blend
_blendIterations	the maximum iteration number in doing blend
_checkResolution	the resolution in discretizing a segment in checking the feasibility of the segment
_tolerance	this tolerance is used in a piecewise linear discretization that deviates no more than `_tolerance` from the parabolic ramp along any axis, and then checks for configuration and segment feasibility along that piecewise linear path.

Returns: smoothed trajectory that satisfies acceleration constraints

Variable Documentation

◆ DEFAULT_BLEND_ITERATIONS

constexpr int aikido::planner::parabolic::DEFAULT_BLEND_ITERATIONS = 4

constexpr

◆ DEFAULT_BLEND_RADIUS

constexpr double aikido::planner::parabolic::DEFAULT_BLEND_RADIUS = 0.5

constexpr

◆ DEFAULT_CHECK_RESOLUTION

constexpr double aikido::planner::parabolic::DEFAULT_CHECK_RESOLUTION = 1e-4

constexpr

◆ DEFAULT_TIMELIMT

constexpr double aikido::planner::parabolic::DEFAULT_TIMELIMT = 3.0

constexpr

◆ DEFAULT_TOLERANCE

constexpr double aikido::planner::parabolic::DEFAULT_TOLERANCE = 1e-3

constexpr

Classes

Functions

Variables

Function Documentation

◆ computeParabolicTiming() [1/2]

◆ computeParabolicTiming() [2/2]

◆ doBlend()

◆ doShortcut()

◆ doShortcutAndBlend()

Variable Documentation

◆ DEFAULT_BLEND_ITERATIONS

◆ DEFAULT_BLEND_RADIUS

◆ DEFAULT_CHECK_RESOLUTION

◆ DEFAULT_TIMELIMT

◆ DEFAULT_TOLERANCE