#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)
	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, const 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 202 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 1424 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 1251 of file Trajectory.cpp.

◆ applyRamping()

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

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 distance over which to ramp the velocity

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 747 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 718 of file Trajectory.cpp.

Here is the call graph for this function:

◆ duration()

float duration ( core::VelocityInterpolation interpolation ) const

Definition at line 1207 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 828 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 822 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 889 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 834 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 1430 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 1485 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,
		const 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 1177 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ isValid()

bool isValid ( ) const

noexcept

Definition at line 1242 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 1145 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 1201 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ points()

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

Definition at line 1195 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ poses()

std::vector< Pose > poses ( ) const

nodiscardnoexcept

Definition at line 807 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 793 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 1044 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 1166 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()