Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use binary search in Trajectory::sample() #1294

Closed
wants to merge 3 commits into from
Closed

Use binary search in Trajectory::sample() #1294

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0669566
Use binary search instead of linear scanning for trajectory sampling
RobertWilbrandt Sep 23, 2024
6d7a6d3
Handle sample time before trajectory using binary search result
RobertWilbrandt Sep 23, 2024
c0149a6
Remove duplication for interpolation
RobertWilbrandt Sep 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
102 changes: 46 additions & 56 deletions joint_trajectory_controller/src/trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,80 +119,70 @@ 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)
// Find the first trajectory point later than the current sample_time
const auto next_traj_point_it = std::upper_bound(
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())
{
// If interpolation is disabled, just forward the next waypoint
auto & next_traj_point = *next_traj_point_it;
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)
{
output_state = state_before_traj_msg_;
output_state = before_first_point ? last_traj_point : next_traj_point;
}
// Do interpolation
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());
last_traj_point, next_traj_point, state_before_traj_msg_.positions.size(),
(t1 - t0).seconds());

interpolate_between_points(
time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg,
sample_time, output_state);
t0, last_traj_point, t1, next_traj_point, sample_time, output_state);
}
start_segment_itr = begin(); // no segments before the first
end_segment_itr = begin();

// 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;
}

// 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)
// whole animation has played out
else
{
auto & point = trajectory_msg_->points[i];
auto & next_point = trajectory_msg_->points[i + 1];

const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start;
const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start;

if (sample_time >= t0 && sample_time < t1)
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())
{
// 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.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;
}

// 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;
}

void Trajectory::interpolate_between_points(
Expand Down
Loading