#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) 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 197 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 1370 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 1197 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 566 of file Trajectory.cpp.

Here is the call graph for this function:

◆ calcSubsampledTrajectory()

GlobalTrajectory calcSubsampledTrajectory ( int subsamplingFactor ) const

Definition at line 693 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 664 of file Trajectory.cpp.

Here is the call graph for this function:

◆ duration()

float duration ( core::VelocityInterpolation interpolation ) const

Definition at line 1153 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 774 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 768 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 835 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 780 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 1376 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 1431 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 ) 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.

Definition at line 395 of file Trajectory.cpp.

◆ getProjection() [2/2]

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

Definition at line 283 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 510 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 533 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 448 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 492 of file Trajectory.cpp.

Here is the call graph for this function:

◆ hasMaxDistanceBetweenWaypoints()

bool hasMaxDistanceBetweenWaypoints ( float maxDistance ) const

Definition at line 1123 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ isValid()

bool isValid ( ) const

noexcept

Definition at line 1188 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 1091 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 1147 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ points()

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

Definition at line 1141 of file Trajectory.cpp.

Here is the caller graph for this function:

◆ poses()

std::vector< Pose > poses ( ) const

nodiscardnoexcept

Definition at line 753 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 739 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 990 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 1112 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()