38 #include <Eigen/Geometry>
48 const std::string
LOGNAME =
"moveit_trajectory_processing.ruckig_traj_smoothing";
49 constexpr
double DEFAULT_MAX_VELOCITY = 5;
50 constexpr
double DEFAULT_MAX_ACCELERATION = 10;
51 constexpr
double DEFAULT_MAX_JERK = 100;
52 constexpr
double MAX_DURATION_EXTENSION_FACTOR = 50.0;
53 constexpr
double DURATION_EXTENSION_FRACTION = 1.1;
54 constexpr
double OVERSHOOT_CHECK_PERIOD = 0.01;
58 const double max_velocity_scaling_factor,
59 const double max_acceleration_scaling_factor,
const bool mitigate_overshoot,
60 const double overshoot_threshold)
68 if (num_waypoints < 2)
71 "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
78 ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
79 if (!
getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input))
85 return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
89 const std::unordered_map<std::string, double>& velocity_limits,
90 const std::unordered_map<std::string, double>& acceleration_limits,
91 const std::unordered_map<std::string, double>& jerk_limits,
92 const bool mitigate_overshoot,
const double overshoot_threshold)
100 if (num_waypoints < 2)
103 "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
110 ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
111 double max_velocity_scaling_factor = 1.0;
112 double max_acceleration_scaling_factor = 1.0;
113 if (!
getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input))
122 for (
size_t j = 0; j < num_joints; ++j)
125 auto it = velocity_limits.find(vars[j]);
126 if (it != velocity_limits.end())
128 ruckig_input.max_velocity.at(j) = it->second;
131 it = acceleration_limits.find(vars[j]);
132 if (it != acceleration_limits.end())
134 ruckig_input.max_acceleration.at(j) = it->second;
137 it = jerk_limits.find(vars[j]);
138 if (it != jerk_limits.end())
140 ruckig_input.max_jerk.at(j) = it->second;
144 return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
147 std::optional<robot_trajectory::RobotTrajectory>
149 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
size_t batch_size)
155 const size_t temp_batch_size = std::min(
size_t(0.1 * num_waypoints),
size_t(100));
157 batch_size = std::max(
size_t(2), temp_batch_size);
159 size_t batch_start_idx = 0;
160 size_t batch_end_idx = batch_size - 1;
161 const size_t full_traj_final_idx = num_waypoints - 1;
167 output_trajectory.
clear();
169 while (batch_end_idx <= full_traj_final_idx)
171 sub_trajectory.
clear();
172 for (
size_t waypoint_idx = batch_start_idx; waypoint_idx <= batch_end_idx; ++waypoint_idx)
179 bool first_point_previously_smoothed =
false;
183 first_point_previously_smoothed =
true;
186 if (!
runRuckig(sub_trajectory, ruckig_input))
193 size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0;
196 for (
size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.
getWayPointCount(); ++waypoint_idx)
202 batch_start_idx += batch_size;
203 batch_end_idx += batch_size;
206 return std::make_optional<robot_trajectory::RobotTrajectory>(output_trajectory,
true );
221 const double max_acceleration_scaling_factor,
223 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
228 for (
size_t i = 0; i < num_dof; ++i)
235 ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.
max_velocity_;
240 "Joint velocity limits are not defined. Using the default "
241 << DEFAULT_MAX_VELOCITY
242 <<
" rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
243 ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
247 ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.
max_acceleration_;
252 LOGNAME,
"Joint acceleration limits are not defined. Using the default "
253 << DEFAULT_MAX_ACCELERATION
254 <<
" rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
255 ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
258 double jerk_limit = DEFAULT_MAX_JERK;
259 std::string jerk_param =
"ruckig/" + vars.at(i) +
"/jerk_limit";
262 ruckig_input.max_jerk.at(i) = jerk_limit;
266 ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
269 <<
" rad/s^3. You can define a jerk limit with parameter " + jerk_param);
277 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
278 const bool mitigate_overshoot,
const double overshoot_threshold)
283 ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
296 ruckig::Result ruckig_result;
297 double duration_extension_factor = 1;
298 bool smoothing_complete =
false;
299 size_t waypoint_idx = 0;
300 while ((duration_extension_factor <= MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
302 while (waypoint_idx < num_waypoints - 1)
304 moveit::core::RobotStatePtr curr_waypoint = trajectory.
getWayPointPtr(waypoint_idx);
305 moveit::core::RobotStatePtr next_waypoint = trajectory.
getWayPointPtr(waypoint_idx + 1);
310 ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_trajectory(num_dof);
311 ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory);
315 bool overshoots =
false;
316 if (mitigate_overshoot)
318 overshoots =
checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold);
326 if (!overshoots && (waypoint_idx == num_waypoints - 2) &&
327 (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
330 smoothing_complete =
true;
335 if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished))
337 duration_extension_factor *= DURATION_EXTENSION_FRACTION;
351 if (duration_extension_factor > MAX_DURATION_EXTENSION_FACTOR)
354 "Ruckig extended the trajectory duration to its maximum and still did not find a solution");
357 if (ruckig_result != ruckig::Result::Finished && ruckig_result != ruckig::Result::Working)
367 const size_t num_dof,
const std::vector<int>& move_group_idx,
372 duration_extension_factor *
380 for (
size_t joint = 0; joint < num_dof; ++joint)
382 target_state->setVariableVelocity(move_group_idx.at(joint),
383 (1 / duration_extension_factor) *
384 target_state->getVariableVelocity(move_group_idx.at(joint)));
386 double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
387 double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
388 target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
394 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
399 std::vector<double> current_positions_vector(num_dof);
400 std::vector<double> current_velocities_vector(num_dof);
401 std::vector<double> current_accelerations_vector(num_dof);
403 for (
size_t i = 0; i < num_dof; ++i)
409 current_velocities_vector.at(i) =
410 std::clamp(current_velocities_vector.at(i), -ruckig_input.max_velocity.at(i), ruckig_input.max_velocity.at(i));
411 current_accelerations_vector.at(i) = std::clamp(
412 current_accelerations_vector.at(i), -ruckig_input.max_acceleration.at(i), ruckig_input.max_acceleration.at(i));
414 std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
415 std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
416 std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
420 const moveit::core::RobotStateConstPtr& next_waypoint,
422 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
427 for (
size_t joint = 0; joint < num_dof; ++joint)
429 ruckig_input.current_position.at(joint) = current_waypoint->getVariablePosition(idx.at(joint));
430 ruckig_input.current_velocity.at(joint) = current_waypoint->getVariableVelocity(idx.at(joint));
431 ruckig_input.current_acceleration.at(joint) = current_waypoint->getVariableAcceleration(idx.at(joint));
434 ruckig_input.target_position.at(joint) = next_waypoint->getVariablePosition(idx.at(joint));
435 ruckig_input.target_velocity.at(joint) = next_waypoint->getVariableVelocity(idx.at(joint));
436 ruckig_input.target_acceleration.at(joint) = next_waypoint->getVariableAcceleration(idx.at(joint));
439 ruckig_input.current_velocity.at(joint) =
440 std::clamp(ruckig_input.current_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
441 ruckig_input.max_velocity.at(joint));
442 ruckig_input.current_acceleration.at(joint) =
443 std::clamp(ruckig_input.current_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
444 ruckig_input.max_acceleration.at(joint));
445 ruckig_input.target_velocity.at(joint) =
446 std::clamp(ruckig_input.target_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
447 ruckig_input.max_velocity.at(joint));
448 ruckig_input.target_acceleration.at(joint) =
449 std::clamp(ruckig_input.target_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
450 ruckig_input.max_acceleration.at(joint));
455 const size_t num_dof, ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
456 const double overshoot_threshold)
459 for (
double time_from_start = OVERSHOOT_CHECK_PERIOD; time_from_start < ruckig_trajectory.get_duration();
460 time_from_start += OVERSHOOT_CHECK_PERIOD)
462 std::vector<double> new_position(num_dof);
463 std::vector<double> new_velocity(num_dof);
464 std::vector<double> new_acceleration(num_dof);
465 ruckig_trajectory.at_time(time_from_start, new_position, new_velocity, new_acceleration);
467 for (
size_t joint = 0; joint < num_dof; ++joint)
470 double error = new_position[joint] - ruckig_input.target_position.at(joint);
471 if (((error / (ruckig_input.current_position.at(joint) - ruckig_input.target_position.at(joint))) < 0) &&
472 std::fabs(error) > overshoot_threshold)