36 #include <rclcpp/logger.hpp>
37 #include <rclcpp/logging.hpp>
40 #include <Eigen/Geometry>
49 const rclcpp::Logger LOGGER =
rclcpp::get_logger(
"moveit_trajectory_processing.ruckig_traj_smoothing");
50 constexpr
double DEFAULT_MAX_VELOCITY = 5;
51 constexpr
double DEFAULT_MAX_ACCELERATION = 10;
52 constexpr
double DEFAULT_MAX_JERK = 1000;
53 constexpr
double MAX_DURATION_EXTENSION_FACTOR = 10.0;
54 constexpr
double DURATION_EXTENSION_FRACTION = 1.1;
56 constexpr
double OVERSHOOT_CHECK_PERIOD = 0.01;
60 const double max_velocity_scaling_factor,
61 const double max_acceleration_scaling_factor,
const bool mitigate_overshoot,
62 const double overshoot_threshold)
64 if (!validateGroup(trajectory))
70 if (num_waypoints < 2)
73 "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
79 const size_t num_dof =
group->getVariableCount();
80 ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
81 if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor,
group, ruckig_input))
83 RCLCPP_ERROR(LOGGER,
"Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
87 return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
91 const std::unordered_map<std::string, double>& velocity_limits,
92 const std::unordered_map<std::string, double>& acceleration_limits,
93 const std::unordered_map<std::string, double>& jerk_limits,
94 const double max_velocity_scaling_factor,
95 const double max_acceleration_scaling_factor,
const bool mitigate_overshoot,
96 const double overshoot_threshold)
98 if (!validateGroup(trajectory))
104 if (num_waypoints < 2)
107 "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
113 const size_t num_dof =
group->getVariableCount();
114 ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
115 if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor,
group, ruckig_input))
117 RCLCPP_ERROR(LOGGER,
"Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
122 const std::vector<std::string>& vars =
group->getVariableNames();
123 const unsigned num_joints =
group->getVariableCount();
124 for (
size_t j = 0; j < num_joints; ++j)
127 auto it = velocity_limits.find(vars[j]);
128 if (it != velocity_limits.end())
130 ruckig_input.max_velocity.at(j) = it->second * max_velocity_scaling_factor;
133 it = acceleration_limits.find(vars[j]);
134 if (it != acceleration_limits.end())
136 ruckig_input.max_acceleration.at(j) = it->second * max_acceleration_scaling_factor;
139 it = jerk_limits.find(vars[j]);
140 if (it != jerk_limits.end())
142 ruckig_input.max_jerk.at(j) = it->second;
146 return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
150 const std::vector<moveit_msgs::msg::JointLimits>&
joint_limits,
151 const double max_velocity_scaling_factor,
152 const double max_acceleration_scaling_factor)
154 std::unordered_map<std::string, double> velocity_limits;
155 std::unordered_map<std::string, double> acceleration_limits;
156 std::unordered_map<std::string, double> jerk_limits;
160 if (limit.has_velocity_limits)
162 velocity_limits[limit.joint_name] = limit.max_velocity;
164 if (limit.has_acceleration_limits)
166 acceleration_limits[limit.joint_name] = limit.max_acceleration;
168 if (limit.has_jerk_limits)
170 jerk_limits[limit.joint_name] = limit.max_jerk;
173 return applySmoothing(trajectory, velocity_limits, acceleration_limits, jerk_limits, max_velocity_scaling_factor,
174 max_acceleration_scaling_factor);
182 RCLCPP_ERROR(LOGGER,
"The planner did not set the group the plan was computed for");
188 bool RuckigSmoothing::getRobotModelBounds(
const double max_velocity_scaling_factor,
189 const double max_acceleration_scaling_factor,
191 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
193 const size_t num_dof =
group->getVariableCount();
194 const std::vector<std::string>& vars =
group->getVariableNames();
196 for (
size_t i = 0; i < num_dof; ++i)
203 ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.
max_velocity_;
207 RCLCPP_WARN_STREAM_ONCE(LOGGER,
208 "Joint velocity limits are not defined. Using the default "
209 << DEFAULT_MAX_VELOCITY
210 <<
" rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
211 ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
215 ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.
max_acceleration_;
219 RCLCPP_WARN_STREAM_ONCE(LOGGER,
220 "Joint acceleration limits are not defined. Using the default "
221 << DEFAULT_MAX_ACCELERATION
222 <<
" rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
223 ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
228 ruckig_input.max_jerk.at(i) = bounds.
max_jerk_;
232 RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint jerk limits are not defined. Using the default "
234 <<
" rad/s^3. You can define jerk limits in joint_limits.yaml.");
235 ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
243 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
244 const bool mitigate_overshoot,
const double overshoot_threshold)
248 const size_t num_dof =
group->getVariableCount();
249 ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
262 ruckig::Result ruckig_result;
263 double duration_extension_factor = 1;
264 bool smoothing_complete =
false;
265 size_t waypoint_idx = 0;
266 while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
268 while (waypoint_idx < num_waypoints - 1)
270 moveit::core::RobotStatePtr curr_waypoint = trajectory.
getWayPointPtr(waypoint_idx);
271 moveit::core::RobotStatePtr next_waypoint = trajectory.
getWayPointPtr(waypoint_idx + 1);
273 getNextRuckigInput(curr_waypoint, next_waypoint,
group, ruckig_input);
276 ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_trajectory(num_dof);
277 ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory);
281 bool overshoots =
false;
282 if (mitigate_overshoot)
284 overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold);
292 if (!overshoots && (waypoint_idx == num_waypoints - 2) &&
293 (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
296 smoothing_complete =
true;
301 if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished))
303 duration_extension_factor *= DURATION_EXTENSION_FRACTION;
305 const std::vector<int>& move_group_idx =
group->getVariableIndexList();
306 extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory,
317 if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
319 RCLCPP_ERROR_STREAM(LOGGER,
"Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
326 void RuckigSmoothing::extendTrajectoryDuration(
const double duration_extension_factor,
size_t waypoint_idx,
327 const size_t num_dof,
const std::vector<int>& move_group_idx,
332 duration_extension_factor *
340 for (
size_t joint = 0; joint < num_dof; ++joint)
342 target_state->setVariableVelocity(move_group_idx.at(joint),
343 (1 / duration_extension_factor) *
344 target_state->getVariableVelocity(move_group_idx.at(joint)));
346 double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
347 double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
348 target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
354 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
359 std::vector<double> current_positions_vector(num_dof);
360 std::vector<double> current_velocities_vector(num_dof);
361 std::vector<double> current_accelerations_vector(num_dof);
363 for (
size_t i = 0; i < num_dof; ++i)
369 current_velocities_vector.at(i) =
370 std::clamp(current_velocities_vector.at(i), -ruckig_input.max_velocity.at(i), ruckig_input.max_velocity.at(i));
371 current_accelerations_vector.at(i) = std::clamp(
372 current_accelerations_vector.at(i), -ruckig_input.max_acceleration.at(i), ruckig_input.max_acceleration.at(i));
374 std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
375 std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
376 std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
379 void RuckigSmoothing::getNextRuckigInput(
const moveit::core::RobotStateConstPtr& current_waypoint,
380 const moveit::core::RobotStateConstPtr& next_waypoint,
382 ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
387 for (
size_t joint = 0; joint < num_dof; ++joint)
389 ruckig_input.current_position.at(joint) = current_waypoint->getVariablePosition(idx.at(joint));
390 ruckig_input.current_velocity.at(joint) = current_waypoint->getVariableVelocity(idx.at(joint));
391 ruckig_input.current_acceleration.at(joint) = current_waypoint->getVariableAcceleration(idx.at(joint));
394 ruckig_input.target_position.at(joint) = next_waypoint->getVariablePosition(idx.at(joint));
395 ruckig_input.target_velocity.at(joint) = next_waypoint->getVariableVelocity(idx.at(joint));
396 ruckig_input.target_acceleration.at(joint) = next_waypoint->getVariableAcceleration(idx.at(joint));
399 ruckig_input.current_velocity.at(joint) =
400 std::clamp(ruckig_input.current_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
401 ruckig_input.max_velocity.at(joint));
402 ruckig_input.current_acceleration.at(joint) =
403 std::clamp(ruckig_input.current_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
404 ruckig_input.max_acceleration.at(joint));
405 ruckig_input.target_velocity.at(joint) =
406 std::clamp(ruckig_input.target_velocity.at(joint), -ruckig_input.max_velocity.at(joint),
407 ruckig_input.max_velocity.at(joint));
408 ruckig_input.target_acceleration.at(joint) =
409 std::clamp(ruckig_input.target_acceleration.at(joint), -ruckig_input.max_acceleration.at(joint),
410 ruckig_input.max_acceleration.at(joint));
414 bool RuckigSmoothing::checkOvershoot(ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector>& ruckig_trajectory,
415 const size_t num_dof, ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
416 const double overshoot_threshold)
419 for (
double time_from_start = OVERSHOOT_CHECK_PERIOD; time_from_start < ruckig_trajectory.get_duration();
420 time_from_start += OVERSHOOT_CHECK_PERIOD)
422 std::vector<double> new_position(num_dof);
423 std::vector<double> new_velocity(num_dof);
424 std::vector<double> new_acceleration(num_dof);
425 ruckig_trajectory.at_time(time_from_start, new_position, new_velocity, new_acceleration);
427 for (
size_t joint = 0; joint < num_dof; ++joint)
430 double error = new_position[joint] - ruckig_input.target_position.at(joint);
431 if (((error / (ruckig_input.current_position.at(joint) - ruckig_input.target_position.at(joint))) < 0.0) &&
432 std::fabs(error) > overshoot_threshold)
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & setWayPointDurationFromPrevious(std::size_t index, double value)
double getAverageSegmentDuration() const
const moveit::core::JointModelGroup * getGroup() const
RobotTrajectory & unwind()
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
std::size_t getWayPointCount() const
double getWayPointDurationFromPrevious(std::size_t index) const
moveit::core::RobotStatePtr & getFirstWayPointPtr()
static bool applySmoothing(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01)
Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated.
rclcpp::Logger get_logger()
bool acceleration_bounded_