Class Trajectory

Class Documentation

class Trajectory

Public Functions

Trajectory()
explicit Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
explicit Trajectory(const rclcpp::Time &current_time, const trajectory_msgs::msg::JointTrajectoryPoint &current_point, std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
void set_point_before_trajectory_msg(const rclcpp::Time &current_time, const trajectory_msgs::msg::JointTrajectoryPoint &current_point, const std::vector<bool> &joints_angle_wraparound = std::vector<bool>())

Set the point before the trajectory message is replaced/appended Example: if we receive a new trajectory message and it’s first point is 0.5 seconds from the current one, we call this function to log the current state, then append/replace the current trajectory

Parameters:

joints_angle_wraparound – Vector of boolean where true value corresponds to a joint that wrap around (ie. is continuous).

void update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
bool sample(const rclcpp::Time &sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint &output_state, TrajectoryPointConstIter &start_segment_itr, TrajectoryPointConstIter &end_segment_itr, const bool search_monotonically_increasing = true)

Find the segment (made up of 2 points) and its expected state from the containing trajectory. Sampling trajectory at given sample_time. If position in the end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively. Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment.

This function by default assumes that sampling is only done at monotonically increasing sample_time for any trajectory. That means, it will only search for a point matching the sample time after the point it has been called before. If this isn’t desired, set search_monotonically_increasing to false.

Specific case returns for start_segment_itr and end_segment_itr:

  • Sampling before the trajectory start: start_segment_itr = begin(), end_segment_itr = begin()

  • Sampling exactly on a point of the trajectory: start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr

  • Sampling between points: start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr

  • Sampling after entire trajectory: start_segment_itr = &#8212;end(), end_segment_itr = end()

  • Sampling empty msg or before the time given in set_point_before_trajectory_msg() return false

Parameters:

  • sample_time – [in] Time at which trajectory will be sampled.

  • interpolation_method – [in] Specify whether splines, another method, or no interpolation at all.

  • output_state – [out] Calculated new at sample_time.

  • start_segment_itr – [out] Iterator to the start segment for given sample_time. See description above.

  • end_segment_itr – [out] Iterator to the end segment for given sample_time. See description above.

  • search_monotonically_increasing – [in] If set to true, the next sample call will start searching in the trajectory at the index of this call’s result.

void interpolate_between_points(const rclcpp::Time &time_a, const trajectory_msgs::msg::JointTrajectoryPoint &state_a, const rclcpp::Time &time_b, const trajectory_msgs::msg::JointTrajectoryPoint &state_b, const rclcpp::Time &sample_time, trajectory_msgs::msg::JointTrajectoryPoint &output)

Do interpolation between 2 states given a time in between their respective timestamps

The start and end states need not necessarily be specified all the way to the acceleration level:

  • If only positions are specified, linear interpolation will be used.

  • If positions and velocities are specified, a cubic spline will be used.

  • If positions, velocities and accelerations are specified, a quintic spline will be used.

If start and end states have different specifications (eg. start is position-only, end is position-velocity), the lowest common specification will be used (position-only in the example).

Parameters:

  • time_a – [in] Time at which the segment state equals state_a.

  • state_a – [in] State at time_a.

  • time_b – [in] Time at which the segment state equals state_b.

  • state_b – [in] State at time time_b.

  • sample_time – [in] The time to sample, between time_a and time_b.

  • output – [out] The state at sample_time.

TrajectoryPointConstIter begin() const
TrajectoryPointConstIter end() const
rclcpp::Time time_from_start() const
bool has_trajectory_msg() const
bool has_nontrivial_msg() const
inline std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg() const
inline bool is_sampled_already() const
inline size_t last_sample_index() const

Get the index of the segment start returned by the last sample() operation.

As the trajectory is only accessed at monotonically increasing sampling times, this index is used to speed up the selection of relevant trajectory points.