GlobalTrajectory Class Reference

#include <armarx/navigation/core/Trajectory.h>

Public Types
using	Indices = std::vector<std::size_t>

Public Member Functions
Indices	allConnectedPointsInRange (std::size_t idx, float radius) const
	get all points within a certain radius that are directly connected to idx
void	applyRamping (const std::vector< std::pair< std::size_t, float > > &fixedVelocities, float rampLength, float rampAcceleration)
	Set the velocity at the given indices to their respective value and ramp down over the specified distance.
GlobalTrajectory	calcSubsampledTrajectory (int subsamplingFactor) const
std::vector< std::pair< std::size_t, float > >	calculateCorners (float cornerLimit) const
	Return a list of the indices of all corneres in the trajectory with their respective angle difference.
float	duration (core::VelocityInterpolation interpolation) const
std::optional< GlobalTrajectoryPoint >	getPointAtSpatialDistanceFrom (const Position &point, float distance) const
std::optional< GlobalTrajectoryPoint >	getPointAtTemporalDistanceFrom (const Position &point, float distance) const
Projection	getProjection (const core::Pose &robotPose, const VelocityInterpolation &velocityInterpolation, std::size_t startSegmentIndex, std::size_t maxSegmentsAhead, float orientationWeight, float lookaheadDistance=0.F) const
	Project the robot pose onto the trajectory, searching only within a window of segments ahead of the given start index, and optionally penalizing orientation differences.
Projection	getProjection (const Position &point, const VelocityInterpolation &velocityInterpolation) const
float	getRemainingDistance (const Position &point) const
	Project point onto the trajectory and evaluate the remaining distance from there on.
float	getRemainingDuration (const Position &point, float linearVelocity=400, float angularVelocity=0.5) const
	Project point onto the trajectory and evaluate the remaining duration from there on.
std::pair< GlobalTrajectory, bool >	getSubTrajectory (const Position &point, float distance) const
	Project point onto the trajectory and return a new trajectory along the old one from that point for the specified distance.
GlobalTrajectory	getSubTrajectory (std::size_t s, std::size_t t) const
	Return a new trajectory along the old one from the given start index (inclusive) to the given end index (exclusive).
	GlobalTrajectory ()=default
	GlobalTrajectory (const std::vector< GlobalTrajectoryPoint > &points)
bool	hasMaxDistanceBetweenWaypoints (float maxDistance) const
bool	isValid () const noexcept
float	length () const
std::vector< GlobalTrajectoryPoint > &	mutablePoints ()
const std::vector< GlobalTrajectoryPoint > &	points () const
std::vector< Pose >	poses () const noexcept
std::vector< Position >	positions () const noexcept
GlobalTrajectory	resample (float eps) const
void	setMaxVelocity (float maxVelocity)

Static Public Member Functions
static GlobalTrajectory	FromPath (const Path &path, const std::vector< float > &velocity)
static GlobalTrajectory	FromPath (const Path &path, float velocity)
	Note: the velocity will not be set!
static GlobalTrajectory	FromPath (const Pose &start, const Positions &waypoints, const Pose &goal, const std::vector< float > &velocity)
static GlobalTrajectory	FromPath (const Pose &start, const Positions &waypoints, const Pose &goal, float velocity)
	Note: the velocity will not be set!

Member Typedef Documentation

Indices

using Indices = std::vector<std::size_t>

Constructor & Destructor Documentation

GlobalTrajectory() [1/2]

GlobalTrajectory ( )

default

GlobalTrajectory() [2/2]

GlobalTrajectory

(

const std::vector< GlobalTrajectoryPoint > &

points

)

Member Function Documentation

allConnectedPointsInRange()

GlobalTrajectory::Indices allConnectedPointsInRange	(	std::size_t	idx,
		float	radius ) const

get all points within a certain radius that are directly connected to idx

Returns

the list without idx

applyRamping()

void applyRamping	(	const std::vector< std::pair< std::size_t, float > > &	fixedVelocities,
		float	rampLength,
		float	rampAcceleration )

Set the velocity at the given indices to their respective value and ramp down over the specified distance.

Will only ever decrease the velocity at a given point.

Parameters

fixedVelocities	the list of indices and their velocity [mm/s] to adjust
rampLength	[mm] the maximum distance over which to ramp the velocity
rampAcceleration	[mm/s^2] the deceleration the ramp is allowed to demand. The ramp follows v(s) = sqrt(v_fixed^2 + 2 a s), so it stays well defined on a trajectory shorter than rampLength instead of holding it near v_fixed.

calcSubsampledTrajectory()

GlobalTrajectory calcSubsampledTrajectory

(

int

subsamplingFactor

)

const

calculateCorners()

std::vector< std::pair< std::size_t, float > > calculateCorners ( float cornerLimit ) const

nodiscard

Return a list of the indices of all corneres in the trajectory with their respective angle difference.

Parameters

cornerLimit

the angle above which two segments are considered a corner [deg]

Returns

std::vector<std::pair<std::size_t, float>> a list of the index and its angle difference for each corner in the trajectory

duration()

float duration

(

core::VelocityInterpolation

interpolation

)

const

FromPath() [1/4]

GlobalTrajectory FromPath ( const Path & path, const std::vector< float > & velocity )

static

FromPath() [2/4]

GlobalTrajectory FromPath ( const Path & path, float velocity )

static

Note: the velocity will not be set!

FromPath() [3/4]

GlobalTrajectory FromPath ( const Pose & start, const Positions & waypoints, const Pose & goal, const std::vector< float > & velocity )

static

FromPath() [4/4]

GlobalTrajectory FromPath ( const Pose & start, const Positions & waypoints, const Pose & goal, float velocity )

static

Note: the velocity will not be set!

getPointAtSpatialDistanceFrom()

std::optional< GlobalTrajectoryPoint > getPointAtSpatialDistanceFrom	(	const Position &	point,
		float	distance ) const

getPointAtTemporalDistanceFrom()

std::optional< GlobalTrajectoryPoint > getPointAtTemporalDistanceFrom	(	const Position &	point,
		float	distance ) const

getProjection() [1/2]

Projection getProjection	(	const core::Pose &	robotPose,
		const VelocityInterpolation &	velocityInterpolation,
		std::size_t	startSegmentIndex,
		std::size_t	maxSegmentsAhead,
		float	orientationWeight,
		float	lookaheadDistance = 0.F ) const

Project the robot pose onto the trajectory, searching only within a window of segments ahead of the given start index, and optionally penalizing orientation differences.

Parameters

robotPose	The current robot pose (translation + orientation).
velocityInterpolation	How to interpolate velocity along the segment.
startSegmentIndex	Index of the first segment to consider (inclusive).
maxSegmentsAhead	Maximum number of segments to search ahead from startSegmentIndex.
orientationWeight	Dimensionless weight for orientation cost. Cost = translationDist * (1.0 + orientationWeight * angularDifference). 0 means translation-only.
lookaheadDistance	Extends the window until this much path length [mm] has been covered, so the search range is a distance and not a waypoint count. A count alone shrinks in metres as the trajectory gets denser, which leaves the reference unable to reach the robot in one cycle. 0 keeps the count-only window.

getProjection() [2/2]

Projection getProjection	(	const Position &	point,
		const VelocityInterpolation &	velocityInterpolation ) const

getRemainingDistance()

float getRemainingDistance

(

const Position &

point

)

const

Project point onto the trajectory and evaluate the remaining distance from there on.

Parameters

point

current location, from which on to measure

Returns

Remaining translation along the trajectory in mm (ignoring rotations!)

getRemainingDuration()

float getRemainingDuration	(	const Position &	point,
		float	linearVelocity = 400,
		float	angularVelocity = 0.5 ) const

Project point onto the trajectory and evaluate the remaining duration from there on.

Parameters

point	current location, from which on to measure
linearVelocity	velocity for translations in mm/s
angularVelocity	velocity for rotations in rad/s

Returns

Remaining translation along the trajectory in s

getSubTrajectory() [1/2]

std::pair< GlobalTrajectory, bool > getSubTrajectory	(	const Position &	point,
		float	distance ) const

Project point onto the trajectory and return a new trajectory along the old one from that point for the specified distance.

Returns

The subtrajectory and whether the subtrajectory ends at the same point, as this trajectory

getSubTrajectory() [2/2]

GlobalTrajectory getSubTrajectory	(	std::size_t	s,
		std::size_t	t ) const

Return a new trajectory along the old one from the given start index (inclusive) to the given end index (exclusive).

Returns

A subtrajectory in the range [s,t)

hasMaxDistanceBetweenWaypoints()

bool hasMaxDistanceBetweenWaypoints

(

float

maxDistance

)

const

isValid()

bool isValid ( ) const

noexcept

length()

float length

(

)

const

mutablePoints()

std::vector< GlobalTrajectoryPoint > & mutablePoints

(

)

points()

const std::vector< GlobalTrajectoryPoint > & points

(

)

const

poses()

std::vector< Pose > poses ( ) const

nodiscardnoexcept

positions()

std::vector< Eigen::Vector3f > positions ( ) const

nodiscardnoexcept

resample()

GlobalTrajectory resample

(

float

eps

)

const

setMaxVelocity()

void setMaxVelocity

(

float

maxVelocity

)

---

The documentation for this class was generated from the following files:

• armarx/navigation/core/Trajectory.h
• armarx/navigation/core/Trajectory.cpp