Represents a smooth trajectory through a set of waypoints. More...

#include <trajectory.hpp>

Public Member Functions
	~Trajectory () noexcept
	Destructor cleans up resources for trajectory. More...

size_t	getJointCount () const
	The number of independent position trajectories over the same time domain that are managed by this object. More...

size_t	getWaypointCount () const
	The number of fixed waypoints that each joint is moving through. More...

double	getStartTime () const
	Get the time (in seconds) at which the defined trajectory begins. More...

double	getEndTime () const
	Get the time (in seconds) at which the defined trajectory ends. More...

double	getDuration () const
	The time (in seconds) between the start and end of this trajectory. More...

bool	getState (double time, Eigen::VectorXd position, Eigen::VectorXd velocity, Eigen::VectorXd *acceleration) const
	Returns the position, velocity, and acceleration for a given point in time along the trajectory. More...

bool	getMinMaxPosition (Eigen::VectorXd &min_position, Eigen::VectorXd &max_position)
	Returns the minimum and maximum positions reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified bounds. More...

bool	getMaxVelocity (Eigen::VectorXd &max_velocity)
	Returns the maximum absolute velocity reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified velocity limits. More...

bool	getMaxAcceleration (Eigen::VectorXd &max_acceleration)
	Returns the maximum absolute acceleration reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified acceleration limits. More...

bool	rescale (double scale)
	Rescales a trajectory by a time factor. More...

Static Public Member Functions
static std::shared_ptr< Trajectory >	createUnconstrainedQp (const Eigen::VectorXd &time_vector, const Eigen::MatrixXd &positions, const Eigen::MatrixXd velocities=nullptr, const Eigen::MatrixXd accelerations=nullptr)
	Creates a smooth trajectory through a set of waypoints (position velocity and accelerations defined at particular times). This trajectory wrapper object can create multi-dimensional trajectories (i.e., multiple joints moving together using the same time reference). More...

static std::shared_ptr< Trajectory >	createJerkazoidal (const Eigen::VectorXd &start_position, const Eigen::VectorXd &end_position, const Eigen::VectorXd &const_speed, double start_ramp_time, double end_ramp_time, const Eigen::VectorXd start_velocities=nullptr, const Eigen::VectorXd end_velocities=nullptr, const Eigen::VectorXd start_accelerations=nullptr, const Eigen::VectorXd end_accelerations=nullptr)
	Creates a constant velocity trajectory with smooth ramp-up and ramp-down phases. More...

static std::shared_ptr< Trajectory >	createJerkazoidal (double start_position, double end_position, double const_speed, double start_ramp_time, double end_ramp_time, double start_velocity=0.0, double end_velocity=0.0, double start_acceleration=0.0, double end_acceleration=0.0)
	Overload of createJerkazoidal for singular variable. See above for implementation details. More...

static Eigen::VectorXd	segmentTimesToWaypointTimes (const Eigen::VectorXd &segment_times)
	Converts a vector of segment times to a vector of time values at each waypoint. The first element of the vector is the time at which the trajectory starts is always 0.0. More...

static Eigen::VectorXd	waypointTimesToSegmentTimes (const Eigen::VectorXd &waypoint_times)
	Converts a vector of time values at each waypoint to a vector of segment times. More...

static Eigen::VectorXd	estimateSegmentTimesNFabian (const Eigen::MatrixXd &positions, const Eigen::VectorXd &max_velocities, const Eigen::VectorXd &max_accelerations, double fabian_constant=6.5, double min_segment_time=0.01)
	Estimates the time required to move between waypoints using the N-Fabian method. It uses the both distance/v_max and v_max/a_max ratios with a heuristic to determine the segment times. More...

static Eigen::VectorXd	estimateSegmentTimesTrapezoidal (const Eigen::MatrixXd &positions, const Eigen::VectorXd &max_velocities, const Eigen::VectorXd &max_accelerations, double min_segment_time=0.01)
	Estimates the time required to move between waypoints based on the trapezoidal velocity profiles and the maximum velocities and accelerations provided. It calculates the velocity assuming an instantaneous constant acceleration (max_acceleration) and then estimates the time to reach the maximum velocity (v_max) and the time to decelerate to zero velocity. More...

Detailed Description

Represents a smooth trajectory through a set of waypoints.

Constructor & Destructor Documentation

◆ ~Trajectory()

hebi::trajectory::Trajectory::~Trajectory ( )

noexcept

Destructor cleans up resources for trajectory.

Member Function Documentation

◆ createUnconstrainedQp()

std::shared_ptr< Trajectory > hebi::trajectory::Trajectory::createUnconstrainedQp	(	const Eigen::VectorXd &	time_vector,
		const Eigen::MatrixXd &	positions,
		const Eigen::MatrixXd *	velocities = `nullptr`,
		const Eigen::MatrixXd *	accelerations = `nullptr`
	)

static

Creates a smooth trajectory through a set of waypoints (position velocity and accelerations defined at particular times). This trajectory wrapper object can create multi-dimensional trajectories (i.e., multiple joints moving together using the same time reference).

Parameters

time_vector	A vector of desired times at which to reach each waypoint; this must be defined (and not NAN for any element).
positions	A matrix of waypoint joint positions (in SI units). The number of rows should be equal to the number of joints, and the number of columns equal to the number of waypoints. Any elements that are NAN will be considered free parameters when solving for a trajectory. Values of +/-infinity are not allowed.
velocities	An optional matrix of velocity constraints at the corresponding waypoints; should either be nullptr or matching the size of the positions matrix. Any elements that are NAN will be considered free parameters when solving for a trajectory. Values of +/-infinity are not allowed.
accelerations	An optional matrix of acceleration constraints at the corresponding waypoints; should either be nullptr or matching the size of the positions matrix. Any elements that are NAN will be considered free parameters when solving for a trajectory. Values of +/-infinity are not allowed.

Returns: A HebiTrajectory object if there were no errors, and the trajectory has been created. An empty shared_ptr indicates that there was an error in creating the trajectory.

◆ createJerkazoidal() [1/2]

std::shared_ptr< Trajectory > hebi::trajectory::Trajectory::createJerkazoidal	(	const Eigen::VectorXd &	start_position,
		const Eigen::VectorXd &	end_position,
		const Eigen::VectorXd &	const_speed,
		double	start_ramp_time,
		double	end_ramp_time,
		const Eigen::VectorXd *	start_velocities = `nullptr`,
		const Eigen::VectorXd *	end_velocities = `nullptr`,
		const Eigen::VectorXd *	start_accelerations = `nullptr`,
		const Eigen::VectorXd *	end_accelerations = `nullptr`
	)

static

Creates a constant velocity trajectory with smooth ramp-up and ramp-down phases.

This function generates a multi-joint trajectory that follows a "jerkazoidal" velocity profile - similar to a trapezoidal velocity profile but with minimum-jerk ramp-up and ramp-down phases instead of constant acceleration. This produces smoother motion by maintaining a constant velocity between start and end positions, with smooth acceleration and deceleration phases at the beginning and end.

The trajectory consists of 2 to 4 waypoints depending on the ramp times:

Start position (with specified initial velocity and acceleration)
End of ramp-up phase (included only if start_ramp_time is sufficiently large)
Start of ramp-down phase (included only if constant velocity segment exists)
End position (with specified final velocity and acceleration)

All joints are synchronized to finish at the same time. The function computes the maximum constant velocity segment time required across all joints and uses this value for all joints to ensure coordination.

Parameters

start_position	Initial position for each joint (in SI units). Must have the same size as all other vector parameters.
end_position	Final position for each joint (in SI units). Must have the same size as all other vector parameters.
const_speed	Desired constant speed for each joint during the constant velocity phase (in SI units per second). All values must be positive and finite.
start_ramp_time	Duration of the initial acceleration phase (in seconds). If very small or zero, the trajectory starts at constant velocity with no ramp-up.
end_ramp_time	Duration of the final deceleration phase (in seconds). Must be positive and finite.
start_velocities	Optional initial velocity for each joint at the start of the trajectory (in SI units per second). If nullptr, defaults to zero velocity. Must be finite if provided.
end_velocities	Optional final velocity for each joint at the end of the trajectory (in SI units per second). If nullptr, defaults to zero velocity. Must be finite if provided.
start_accelerations	Optional initial acceleration for each joint at the start of the trajectory (in SI units per second squared). If nullptr, defaults to zero acceleration. Must be finite if provided.
end_accelerations	Optional final acceleration for each joint at the end of the trajectory (in SI units per second squared). If nullptr, defaults to zero acceleration. Must be finite if provided.

Returns: A shared pointer to a Trajectory object representing the constant velocity trajectory.

Exceptions

std::invalid_argument	If any of the following conditions are met: Input vectors have mismatched sizes Any input value is non-finite (NaN or infinity) Any constant speed is not positive end_ramp_time is not positive The computed constant velocity segment time is negative (indicates invalid parameters)
std::runtime_error	If the underlying C trajectory creation fails.

◆ createJerkazoidal() [2/2]

std::shared_ptr< Trajectory > hebi::trajectory::Trajectory::createJerkazoidal	(	double	start_position,
		double	end_position,
		double	const_speed,
		double	start_ramp_time,
		double	end_ramp_time,
		double	start_velocity = `0.0`,
		double	end_velocity = `0.0`,
		double	start_acceleration = `0.0`,
		double	end_acceleration = `0.0`
	)

static

Overload of createJerkazoidal for singular variable. See above for implementation details.

◆ getJointCount()

size_t hebi::trajectory::Trajectory::getJointCount ( ) const

inline

The number of independent position trajectories over the same time domain that are managed by this object.

◆ getWaypointCount()

size_t hebi::trajectory::Trajectory::getWaypointCount ( ) const

inline

The number of fixed waypoints that each joint is moving through.

◆ getStartTime()

double hebi::trajectory::Trajectory::getStartTime ( ) const

inline

Get the time (in seconds) at which the defined trajectory begins.

◆ getEndTime()

double hebi::trajectory::Trajectory::getEndTime ( ) const

inline

Get the time (in seconds) at which the defined trajectory ends.

◆ getDuration()

double hebi::trajectory::Trajectory::getDuration ( ) const

The time (in seconds) between the start and end of this trajectory.

◆ getState()

bool hebi::trajectory::Trajectory::getState	(	double	time,
		Eigen::VectorXd *	position,
		Eigen::VectorXd *	velocity,
		Eigen::VectorXd *	acceleration
	)		const

Returns the position, velocity, and acceleration for a given point in time along the trajectory.

Parameters

time	The time for which the trajectory state is being queried. This should be between the start and end of the trajectory.
position	If not nullptr, this vector is filled in with the position along the trajectory for each joint at the given time.
velocity	If not nullptr, this vector is filled in with the velocity along the trajectory for each joint at the given time.
acceleration	If not nullptr, this vector is filled in with the acceleration along the trajectory for each joint at the given time.

Returns: True if the state was successfully retrieved for all joints, false if there was an error.

◆ getMinMaxPosition()

bool hebi::trajectory::Trajectory::getMinMaxPosition	(	Eigen::VectorXd &	min_position,
		Eigen::VectorXd &	max_position
	)

Returns the minimum and maximum positions reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified bounds.

Parameters

min_position	This vector is filled in with the minimum position reached by each joint over the entire trajectory.
max_position	This vector is filled in with the maximum position reached by each joint over the entire trajectory.

Returns: True if the min and max positions were successfully retrieved for all joints, false if there was an error.

◆ getMaxVelocity()

bool hebi::trajectory::Trajectory::getMaxVelocity ( Eigen::VectorXd & max_velocity )

Returns the maximum absolute velocity reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified velocity limits.

Parameters

max_velocity This vector is filled in with the maximum velocity reached by each joint over the entire trajectory.

Returns: True if the max velocities were successfully retrieved for all joints, false if there was an error.

◆ getMaxAcceleration()

bool hebi::trajectory::Trajectory::getMaxAcceleration ( Eigen::VectorXd & max_acceleration )

Returns the maximum absolute acceleration reached by each joint over the entire trajectory. This can be useful for checking that a trajectory stays within specified acceleration limits.

Parameters

max_acceleration This vector is filled in with the maximum acceleration reached by each joint over the entire trajectory.

Returns: True if the max accelerations were successfully retrieved for all joints, false if there was an error.

◆ rescale()

bool hebi::trajectory::Trajectory::rescale ( double scale )

Rescales a trajectory by a time factor.

Values > 1 slow down the trajectory (increase duration), values < 1 speed up the trajectory (decrease duration). The position waypoints remain unchanged, but velocities and accelerations are scaled inversely.

Parameters

scale Time scale factor. Must be positive (> 0).

Returns: True if rescaling was successful, false if the scale factor is invalid (<= 0).

◆ segmentTimesToWaypointTimes()

VectorXd hebi::trajectory::Trajectory::segmentTimesToWaypointTimes ( const Eigen::VectorXd & segment_times )

static

Converts a vector of segment times to a vector of time values at each waypoint. The first element of the vector is the time at which the trajectory starts is always 0.0.

Parameters

segment_times A vector of segment times, where each element represents the duration of a segment between waypoints. Values must be finite, i.e., not NaN or +/- infinity.

Returns: A vector of time values at each waypoint, or throws an exception if the input vector is empty or contains invalid values.

◆ waypointTimesToSegmentTimes()

VectorXd hebi::trajectory::Trajectory::waypointTimesToSegmentTimes ( const Eigen::VectorXd & waypoint_times )

static

Converts a vector of time values at each waypoint to a vector of segment times.

Parameters

time_vector A vector of time values at each waypoint. Values must be finite, i.e., not NaN or +/- infinity.

Returns: A vector of segment times, or throws an exception if the input vector is empty or contains invalid values.

◆ estimateSegmentTimesNFabian()

static Eigen::VectorXd hebi::trajectory::Trajectory::estimateSegmentTimesNFabian	(	const Eigen::MatrixXd &	positions,
		const Eigen::VectorXd &	max_velocities,
		const Eigen::VectorXd &	max_accelerations,
		double	fabian_constant = `6.5`,
		double	min_segment_time = `0.01`
	)

inlinestatic

Estimates the time required to move between waypoints using the N-Fabian method. It uses the both distance/v_max and v_max/a_max ratios with a heuristic to determine the segment times.

t_est = 2 * distance/v_max * (1 + magic_fabian_constant * v_max/a_max

exp(-distance/v_max * 2);

The method assumes that the segment starts and ends with zero velocity and acceleration.

Parameters

positions	A matrix of waypoint joint positions. The number of rows should be equal to the number of joints, and the number of columns equal to the number of waypoints. Values of +/-infinity are not allowed.
max_velocities	A vector of maximum velocities for each joint. The size of this vector should match the number of joints.
max_accelerations	A vector of maximum accelerations for each joint. The size of this vector should match the number of joints.
fabian_constant	A constant used in the N-Fabian heuristic. Defaults to 6.5.
min_segment_time	Minimum allowed segment duration. Any estimated time below this value will be clipped. Must be a finite non-negative value. Defaults to 0.01.

Returns: A vector of estimated segment times, or throws an exception if the input matrix is empty or contains invalid values.

◆ estimateSegmentTimesTrapezoidal()

static Eigen::VectorXd hebi::trajectory::Trajectory::estimateSegmentTimesTrapezoidal	(	const Eigen::MatrixXd &	positions,
		const Eigen::VectorXd &	max_velocities,
		const Eigen::VectorXd &	max_accelerations,
		double	min_segment_time = `0.01`
	)

inlinestatic

Estimates the time required to move between waypoints based on the trapezoidal velocity profiles and the maximum velocities and accelerations provided. It calculates the velocity assuming an instantaneous constant acceleration (max_acceleration) and then estimates the time to reach the maximum velocity (v_max) and the time to decelerate to zero velocity.

Parameters

positions	A matrix of waypoint joint positions. The number of rows should be equal to the number of joints, and the number of columns equal to the number of waypoints. Values of +/-infinity are not allowed.
max_velocities	A vector of maximum velocities for each joint. The size of this vector should match the number of joints.
max_accelerations	A vector of maximum accelerations for each joint. The size of this vector should match the number of joints.
min_segment_time	Minimum allowed segment duration. Any estimated time below this value will be clipped. Must be a finite non-negative value. Defaults to 0.01.

Returns: A vector of estimated segment times, or throws an exception if the input matrix is empty or contains invalid values.

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

Public Member Functions

Static Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ ~Trajectory()

Member Function Documentation

◆ createUnconstrainedQp()

◆ createJerkazoidal() [1/2]

◆ createJerkazoidal() [2/2]

◆ getJointCount()

◆ getWaypointCount()

◆ getStartTime()

◆ getEndTime()

◆ getDuration()

◆ getState()

◆ getMinMaxPosition()

◆ getMaxVelocity()

◆ getMaxAcceleration()

◆ rescale()

◆ segmentTimesToWaypointTimes()

◆ waypointTimesToSegmentTimes()

◆ estimateSegmentTimesNFabian()

◆ estimateSegmentTimesTrapezoidal()