Class Trajectory

Defined in File trajectory.hpp

Class Documentation

class Trajectory

Represents a smooth trajectory through a set of waypoints.

Public Functions

~Trajectory() noexcept: Destructor cleans up resources for trajectory.

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

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

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

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

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

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.

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.

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.

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.

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.

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.

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.

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.

bool 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).

Public Static 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).

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.

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.

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.

Throws:

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.

Returns:

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

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.

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.

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.

static Eigen::VectorXd waypointTimesToSegmentTimes(const Eigen::VectorXd &waypoint_times)

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.

static inline 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.

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.

static inline 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.

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.