Class Trajectory

Class Documentation

class Trajectory

Public Functions

JOINT_TRAJECTORY_CONTROLLER_PUBLIC Trajectory()
explicit JOINT_TRAJECTORY_CONTROLLER_PUBLIC Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
explicit JOINT_TRAJECTORY_CONTROLLER_PUBLIC Trajectory(const rclcpp::Time &current_time, const trajectory_msgs::msg::JointTrajectoryPoint &current_point, std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory)
JOINT_TRAJECTORY_CONTROLLER_PUBLIC 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).

JOINT_TRAJECTORY_CONTROLLER_PUBLIC void update (std::shared_ptr< trajectory_msgs::msg::JointTrajectory > joint_trajectory)
JOINT_TRAJECTORY_CONTROLLER_PUBLIC 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)

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 assumes that sampling is only done at monotonically increasing sample_time for any trajectory.

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.

JOINT_TRAJECTORY_CONTROLLER_PUBLIC 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.

JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter begin () const
JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter end () const
JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp::Time time_from_start () const
JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_trajectory_msg () const
JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_nontrivial_msg () const
inline JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr< trajectory_msgs::msg::JointTrajectory > get_trajectory_msg () const
inline JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already () const
inline JOINT_TRAJECTORY_CONTROLLER_PUBLIC 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.