#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!

Detailed Description

Definition at line 74 of file Trajectory.h.

Member Typedef Documentation

◆ Indices

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

Definition at line 206 of file Trajectory.h.

Constructor & Destructor Documentation

◆ GlobalTrajectory() [1/2]

GlobalTrajectory ( )

default

Here is the call graph for this function:

Here is the caller graph for this function:

◆ GlobalTrajectory() [2/2]

GlobalTrajectory ( const std::vector< GlobalTrajectoryPoint > & points )

Definition at line 1467 of file Trajectory.cpp.

Here is the call graph for this function:

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

Definition at line 1294 of file Trajectory.cpp.

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

Definition at line 620 of file Trajectory.cpp.

Here is the call graph for this function:

◆ calcSubsampledTrajectory()

GlobalTrajectory calcSubsampledTrajectory ( int subsamplingFactor ) const

Definition at line 738 of file Trajectory.cpp.

Here is the call graph for this function:

◆ 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

Definition at line 709 of file Trajectory.cpp.

Here is the call graph for this function:

◆ duration()

float duration ( core::VelocityInterpolation interpolation ) const

Definition at line 1250 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ FromPath() [1/4]

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

static

Definition at line 819 of file Trajectory.cpp.

Here is the call graph for this function:

◆ FromPath() [2/4]

GlobalTrajectory FromPath	(	const Path &	path,
		float	velocity )

static

Note: the velocity will not be set!

Definition at line 813 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ FromPath() [3/4]

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

static

Definition at line 880 of file Trajectory.cpp.

Here is the call graph for this function:

◆ FromPath() [4/4]

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

static

Note: the velocity will not be set!

Definition at line 825 of file Trajectory.cpp.

Here is the call graph for this function:

◆ getPointAtSpatialDistanceFrom()

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

Definition at line 1473 of file Trajectory.cpp.

Here is the call graph for this function:

◆ getPointAtTemporalDistanceFrom()

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

Definition at line 1528 of file Trajectory.cpp.

Here is the call graph for this function:

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

Definition at line 438 of file Trajectory.cpp.

◆ getProjection() [2/2]

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

Definition at line 286 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ 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!)

Definition at line 564 of file Trajectory.cpp.

◆ 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

Definition at line 587 of file Trajectory.cpp.

◆ 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

Definition at line 502 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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)

Definition at line 546 of file Trajectory.cpp.

Here is the call graph for this function:

◆ hasMaxDistanceBetweenWaypoints()

bool hasMaxDistanceBetweenWaypoints ( float maxDistance ) const

Definition at line 1220 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ isValid()

bool isValid ( ) const

noexcept

Definition at line 1285 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ length()

float length ( ) const

Definition at line 1188 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mutablePoints()

std::vector< GlobalTrajectoryPoint > & mutablePoints ( )

Definition at line 1244 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ points()

const std::vector< GlobalTrajectoryPoint > & points ( ) const

Definition at line 1238 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ poses()

std::vector< Pose > poses ( ) const

nodiscardnoexcept

Definition at line 798 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ positions()

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

nodiscardnoexcept

Definition at line 784 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resample()

GlobalTrajectory resample ( float eps ) const

Definition at line 1035 of file Trajectory.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setMaxVelocity()

void setMaxVelocity ( float maxVelocity )

Definition at line 1209 of file Trajectory.cpp.

Here is the caller graph for this function:

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

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

Public Types

Public Member Functions

Static Public Member Functions

Detailed Description

Member Typedef Documentation

◆ Indices

Constructor & Destructor Documentation

◆ GlobalTrajectory() [1/2]

◆ GlobalTrajectory() [2/2]

Member Function Documentation

◆ allConnectedPointsInRange()

◆ applyRamping()

◆ calcSubsampledTrajectory()

◆ calculateCorners()

◆ duration()

◆ FromPath() [1/4]

◆ FromPath() [2/4]

◆ FromPath() [3/4]

◆ FromPath() [4/4]

◆ getPointAtSpatialDistanceFrom()

◆ getPointAtTemporalDistanceFrom()

◆ getProjection() [1/2]

◆ getProjection() [2/2]

◆ getRemainingDistance()

◆ getRemainingDuration()

◆ getSubTrajectory() [1/2]

◆ getSubTrajectory() [2/2]

◆ hasMaxDistanceBetweenWaypoints()

◆ isValid()

◆ length()

◆ mutablePoints()

◆ points()

◆ poses()

◆ positions()

◆ resample()

◆ setMaxVelocity()