From 066956622a3971aefe55f4e6fcdb434f86c0e1b9 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Mon, 23 Sep 2024 20:19:53 +0200 Subject: [PATCH 1/3] Use binary search instead of linear scanning for trajectory sampling --- .../src/trajectory.cpp | 83 ++++++++++--------- 1 file changed, 46 insertions(+), 37 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 54f785a070..c596c0ba01 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -147,52 +147,61 @@ bool Trajectory::sample( return true; } - // time_from_start + trajectory time is the expected arrival time of trajectory - const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + // Find the first trajectory point later than the current sample_time + const auto next_traj_point_it = std::upper_bound( + std::next(trajectory_msg_->points.begin()), trajectory_msg_->points.end(), sample_time, + [this](const rclcpp::Time & t, const auto & point) + { + // time_from_start + trajectory time is the expected arrival time of trajectory + return t < (trajectory_start_time_ + point.time_from_start); + }); + + if (next_traj_point_it != trajectory_msg_->points.end()) { - auto & point = trajectory_msg_->points[i]; - auto & next_point = trajectory_msg_->points[i + 1]; + auto & next_traj_point = *next_traj_point_it; + const auto t1 = trajectory_start_time_ + next_traj_point.time_from_start; - const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start; - const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start; + auto last_traj_point_it = std::prev(next_traj_point_it); + auto & last_traj_point = *last_traj_point_it; + const auto t0 = trajectory_start_time_ + last_traj_point.time_from_start; - if (sample_time >= t0 && sample_time < t1) + // If interpolation is disabled, just forward the next waypoint + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) { - // If interpolation is disabled, just forward the next waypoint - if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) - { - output_state = next_point; - } - // Do interpolation - else - { - // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); - - interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); - } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); - return true; + output_state = next_traj_point; } - } + // Do interpolation + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(), + (t1 - t0).seconds()); - // whole animation has played out - start_segment_itr = --end(); - end_segment_itr = end(); - output_state = (*start_segment_itr); - // the trajectories in msg may have empty velocities/accel, so resize them - if (output_state.velocities.empty()) - { - output_state.velocities.resize(output_state.positions.size(), 0.0); + interpolate_between_points( + t0, last_traj_point, t1, next_traj_point, sample_time, output_state); + } + start_segment_itr = last_traj_point_it; + end_segment_itr = next_traj_point_it; + return true; } - if (output_state.accelerations.empty()) + else { - output_state.accelerations.resize(output_state.positions.size(), 0.0); + // whole animation has played out + start_segment_itr = --end(); + end_segment_itr = end(); + output_state = (*start_segment_itr); + // the trajectories in msg may have empty velocities/accel, so resize them + if (output_state.velocities.empty()) + { + output_state.velocities.resize(output_state.positions.size(), 0.0); + } + if (output_state.accelerations.empty()) + { + output_state.accelerations.resize(output_state.positions.size(), 0.0); + } + return true; } - return true; } void Trajectory::interpolate_between_points( From 6d7a6d32e8a49691b3f914fc68c72fb8ed1ca12e Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Mon, 23 Sep 2024 21:09:01 +0200 Subject: [PATCH 2/3] Handle sample time before trajectory using binary search result --- .../src/trajectory.cpp | 95 ++++++++++--------- 1 file changed, 48 insertions(+), 47 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index c596c0ba01..6c02fd4aae 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -119,37 +119,10 @@ bool Trajectory::sample( } output_state = trajectory_msgs::msg::JointTrajectoryPoint(); - auto & first_point_in_msg = trajectory_msg_->points[0]; - const rclcpp::Time first_point_timestamp = - trajectory_start_time_ + first_point_in_msg.time_from_start; - - // current time hasn't reached traj time of the first point in the msg yet - if (sample_time < first_point_timestamp) - { - // If interpolation is disabled, just forward the next waypoint - if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) - { - output_state = state_before_traj_msg_; - } - else - { - // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), - (first_point_timestamp - time_before_traj_msg_).seconds()); - - interpolate_between_points( - time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg, - sample_time, output_state); - } - start_segment_itr = begin(); // no segments before the first - end_segment_itr = begin(); - return true; - } // Find the first trajectory point later than the current sample_time const auto next_traj_point_it = std::upper_bound( - std::next(trajectory_msg_->points.begin()), trajectory_msg_->points.end(), sample_time, + trajectory_msg_->points.begin(), trajectory_msg_->points.end(), sample_time, [this](const rclcpp::Time & t, const auto & point) { // time_from_start + trajectory time is the expected arrival time of trajectory @@ -159,35 +132,63 @@ bool Trajectory::sample( if (next_traj_point_it != trajectory_msg_->points.end()) { auto & next_traj_point = *next_traj_point_it; - const auto t1 = trajectory_start_time_ + next_traj_point.time_from_start; - - auto last_traj_point_it = std::prev(next_traj_point_it); - auto & last_traj_point = *last_traj_point_it; - const auto t0 = trajectory_start_time_ + last_traj_point.time_from_start; + const auto next_traj_point_timestamp = trajectory_start_time_ + next_traj_point.time_from_start; - // If interpolation is disabled, just forward the next waypoint - if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) + // current time hasn't reached traj time of the first point in the msg yet + if (next_traj_point_it == trajectory_msg_->points.begin()) { - output_state = next_traj_point; + // If interpolation is disabled, just forward the next waypoint + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) + { + output_state = state_before_traj_msg_; + } + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + state_before_traj_msg_, next_traj_point, state_before_traj_msg_.positions.size(), + (next_traj_point_timestamp - time_before_traj_msg_).seconds()); + + interpolate_between_points( + time_before_traj_msg_, state_before_traj_msg_, next_traj_point_timestamp, next_traj_point, + sample_time, output_state); + } + start_segment_itr = begin(); // no segments before the first + end_segment_itr = begin(); + return true; } - // Do interpolation + // Current time is between two trajectory points and interpolation needs to be performed else { - // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(), - (t1 - t0).seconds()); + auto last_traj_point_it = std::prev(next_traj_point_it); + auto & last_traj_point = *last_traj_point_it; + const auto t0 = trajectory_start_time_ + last_traj_point.time_from_start; - interpolate_between_points( - t0, last_traj_point, t1, next_traj_point, sample_time, output_state); + // If interpolation is disabled, just forward the next waypoint + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) + { + output_state = next_traj_point; + } + // Do interpolation + else + { + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(), + (next_traj_point_timestamp - t0).seconds()); + + interpolate_between_points( + t0, last_traj_point, next_traj_point_timestamp, next_traj_point, sample_time, + output_state); + } + start_segment_itr = last_traj_point_it; + end_segment_itr = next_traj_point_it; + return true; } - start_segment_itr = last_traj_point_it; - end_segment_itr = next_traj_point_it; - return true; } + // whole animation has played out else { - // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); output_state = (*start_segment_itr); From c0149a6da7e59e885e05b2df9480ad0137262058 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Mon, 23 Sep 2024 21:23:58 +0200 Subject: [PATCH 3/3] Remove duplication for interpolation --- .../src/trajectory.cpp | 74 +++++++------------ 1 file changed, 27 insertions(+), 47 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 6c02fd4aae..d9ba3f0b4c 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -132,59 +132,39 @@ bool Trajectory::sample( if (next_traj_point_it != trajectory_msg_->points.end()) { auto & next_traj_point = *next_traj_point_it; - const auto next_traj_point_timestamp = trajectory_start_time_ + next_traj_point.time_from_start; - - // current time hasn't reached traj time of the first point in the msg yet - if (next_traj_point_it == trajectory_msg_->points.begin()) + const auto t1 = trajectory_start_time_ + next_traj_point.time_from_start; + + // Sample by interpolating between last_- and next_traj_point: + // - If there is no point before sample_time in the trajectory, use before_first_point as + // last_traj_point + // - Otherwise, use the last point before sample_time instead + const bool before_first_point = next_traj_point_it == trajectory_msg_->points.begin(); + auto & last_traj_point = + before_first_point ? state_before_traj_msg_ : *std::prev(next_traj_point_it); + const auto t0 = before_first_point ? time_before_traj_msg_ + : trajectory_start_time_ + last_traj_point.time_from_start; + + if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) { - // If interpolation is disabled, just forward the next waypoint - if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) - { - output_state = state_before_traj_msg_; - } - else - { - // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - state_before_traj_msg_, next_traj_point, state_before_traj_msg_.positions.size(), - (next_traj_point_timestamp - time_before_traj_msg_).seconds()); - - interpolate_between_points( - time_before_traj_msg_, state_before_traj_msg_, next_traj_point_timestamp, next_traj_point, - sample_time, output_state); - } - start_segment_itr = begin(); // no segments before the first - end_segment_itr = begin(); - return true; + output_state = before_first_point ? last_traj_point : next_traj_point; } - // Current time is between two trajectory points and interpolation needs to be performed + // Do interpolation else { - auto last_traj_point_it = std::prev(next_traj_point_it); - auto & last_traj_point = *last_traj_point_it; - const auto t0 = trajectory_start_time_ + last_traj_point.time_from_start; + // it changes points only if position and velocity do not exist, but their derivatives + deduce_from_derivatives( + last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(), + (t1 - t0).seconds()); - // If interpolation is disabled, just forward the next waypoint - if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) - { - output_state = next_traj_point; - } - // Do interpolation - else - { - // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(), - (next_traj_point_timestamp - t0).seconds()); - - interpolate_between_points( - t0, last_traj_point, next_traj_point_timestamp, next_traj_point, sample_time, - output_state); - } - start_segment_itr = last_traj_point_it; - end_segment_itr = next_traj_point_it; - return true; + interpolate_between_points( + t0, last_traj_point, t1, next_traj_point, sample_time, output_state); } + + // If sample_time is before the first trajectory point, there is no segment and + // start_segment_itr = end_segment_itr + start_segment_itr = before_first_point ? begin() : std::prev(next_traj_point_it); + end_segment_itr = next_traj_point_it; + return true; } // whole animation has played out else