moveit2
The MoveIt Motion Planning Framework for ROS 2.
ruckig_traj_smoothing.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, PickNik Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 /* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */
35 
36 #include <rclcpp/logger.hpp>
37 #include <rclcpp/logging.hpp>
38 #include <algorithm>
39 #include <cmath>
40 #include <Eigen/Geometry>
41 #include <limits>
43 #include <vector>
44 
45 namespace trajectory_processing
46 {
47 namespace
48 {
49 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
50 constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s
51 constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2
52 constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3
53 constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
54 constexpr double DURATION_EXTENSION_FRACTION = 1.1;
55 // If "mitigate_overshoot" is enabled, overshoot is checked with this timestep
56 constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec
57 } // namespace
58 
60  const double max_velocity_scaling_factor,
61  const double max_acceleration_scaling_factor, const bool mitigate_overshoot,
62  const double overshoot_threshold)
63 {
64  if (!validateGroup(trajectory))
65  {
66  return false;
67  }
68 
69  const size_t num_waypoints = trajectory.getWayPointCount();
70  if (num_waypoints < 2)
71  {
72  RCLCPP_WARN(LOGGER,
73  "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
74  return true;
75  }
76 
77  // Kinematic limits (vels/accels/jerks) from RobotModel
78  const moveit::core::JointModelGroup* const group = trajectory.getGroup();
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))
82  {
83  RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
84  return false;
85  }
86 
87  return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
88 }
89 
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)
97 {
98  if (!validateGroup(trajectory))
99  {
100  return false;
101  }
102 
103  const size_t num_waypoints = trajectory.getWayPointCount();
104  if (num_waypoints < 2)
105  {
106  RCLCPP_WARN(LOGGER,
107  "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
108  return true;
109  }
110 
111  // Set default kinematic limits (vels/accels/jerks)
112  const moveit::core::JointModelGroup* const group = trajectory.getGroup();
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))
116  {
117  RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
118  return false;
119  }
120 
121  // Check if custom limits were supplied as arguments to overwrite the defaults
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)
125  {
126  // Velocity
127  auto it = velocity_limits.find(vars[j]);
128  if (it != velocity_limits.end())
129  {
130  ruckig_input.max_velocity.at(j) = it->second * max_velocity_scaling_factor;
131  }
132  // Acceleration
133  it = acceleration_limits.find(vars[j]);
134  if (it != acceleration_limits.end())
135  {
136  ruckig_input.max_acceleration.at(j) = it->second * max_acceleration_scaling_factor;
137  }
138  // Jerk
139  it = jerk_limits.find(vars[j]);
140  if (it != jerk_limits.end())
141  {
142  ruckig_input.max_jerk.at(j) = it->second;
143  }
144  }
145 
146  return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
147 }
148 
150  const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
151  const double max_velocity_scaling_factor,
152  const double max_acceleration_scaling_factor)
153 {
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;
157  for (const auto& limit : joint_limits)
158  {
159  // If custom limits are not defined here, they will be supplied from getRobotModelBounds() later
160  if (limit.has_velocity_limits)
161  {
162  velocity_limits[limit.joint_name] = limit.max_velocity;
163  }
164  if (limit.has_acceleration_limits)
165  {
166  acceleration_limits[limit.joint_name] = limit.max_acceleration;
167  }
168  if (limit.has_jerk_limits)
169  {
170  jerk_limits[limit.joint_name] = limit.max_jerk;
171  }
172  }
173  return applySmoothing(trajectory, velocity_limits, acceleration_limits, jerk_limits, max_velocity_scaling_factor,
174  max_acceleration_scaling_factor);
175 }
176 
177 bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory)
178 {
179  const moveit::core::JointModelGroup* const group = trajectory.getGroup();
180  if (!group)
181  {
182  RCLCPP_ERROR(LOGGER, "The planner did not set the group the plan was computed for");
183  return false;
184  }
185  return true;
186 }
187 
188 bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_factor,
189  const double max_acceleration_scaling_factor,
191  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
192 {
193  const size_t num_dof = group->getVariableCount();
194  const std::vector<std::string>& vars = group->getVariableNames();
195  const moveit::core::RobotModel& rmodel = group->getParentModel();
196  for (size_t i = 0; i < num_dof; ++i)
197  {
198  const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
199 
200  // This assumes min/max bounds are symmetric
201  if (bounds.velocity_bounded_)
202  {
203  ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.max_velocity_;
204  }
205  else
206  {
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;
212  }
213  if (bounds.acceleration_bounded_)
214  {
215  ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.max_acceleration_;
216  }
217  else
218  {
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;
224  }
225  ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK;
226  if (bounds.jerk_bounded_)
227  {
228  ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
229  }
230  else
231  {
232  RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default "
233  << DEFAULT_MAX_JERK
234  << " rad/s^3. You can define jerk limits in joint_limits.yaml.");
235  ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
236  }
237  }
238 
239  return true;
240 }
241 
242 bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
243  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
244  const bool mitigate_overshoot, const double overshoot_threshold)
245 {
246  const size_t num_waypoints = trajectory.getWayPointCount();
247  const moveit::core::JointModelGroup* const group = trajectory.getGroup();
248  const size_t num_dof = group->getVariableCount();
249  ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
250 
251  // This lib does not work properly when angles wrap, so we need to unwind the path first
252  trajectory.unwind();
253 
254  // Initialize the smoother
255  ruckig::Ruckig<ruckig::DynamicDOFs> ruckig(num_dof, trajectory.getAverageSegmentDuration());
256  initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input);
257 
258  // Cache the trajectory in case we need to reset it
259  robot_trajectory::RobotTrajectory original_trajectory =
260  robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);
261 
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)
267  {
268  while (waypoint_idx < num_waypoints - 1)
269  {
270  moveit::core::RobotStatePtr curr_waypoint = trajectory.getWayPointPtr(waypoint_idx);
271  moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);
272 
273  getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input);
274 
275  // Run Ruckig
276  ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_trajectory(num_dof);
277  ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory);
278 
279  // Step through the trajectory at the given OVERSHOOT_CHECK_PERIOD and check for overshoot.
280  // We will extend the duration to mitigate it.
281  bool overshoots = false;
282  if (mitigate_overshoot)
283  {
284  overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold);
285  }
286 
287  // The difference between Result::Working and Result::Finished is that Finished can be reached in one
288  // Ruckig timestep (constructor parameter). Both are acceptable for trajectories.
289  // (The difference is only relevant for streaming mode.)
290 
291  // If successful and at the last trajectory segment
292  if (!overshoots && (waypoint_idx == num_waypoints - 2) &&
293  (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
294  {
295  trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_trajectory.get_duration());
296  smoothing_complete = true;
297  break;
298  }
299 
300  // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
301  if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished))
302  {
303  duration_extension_factor *= DURATION_EXTENSION_FRACTION;
304 
305  const std::vector<int>& move_group_idx = group->getVariableIndexList();
306  extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory,
307  trajectory);
308 
309  initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input);
310  // Continue the loop from failed segment, but with increased duration extension factor
311  break;
312  }
313  ++waypoint_idx;
314  }
315  }
316 
317  if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
318  {
319  RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
320  return false;
321  }
322 
323  return true;
324 }
325 
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,
328  const robot_trajectory::RobotTrajectory& original_trajectory,
330 {
331  trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1,
332  duration_extension_factor *
333  original_trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1));
334  // re-calculate waypoint velocity and acceleration
335  auto target_state = trajectory.getWayPointPtr(waypoint_idx + 1);
336  const auto prev_state = trajectory.getWayPointPtr(waypoint_idx);
337 
338  double timestep = trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1);
339 
340  for (size_t joint = 0; joint < num_dof; ++joint)
341  {
342  target_state->setVariableVelocity(move_group_idx.at(joint),
343  (1 / duration_extension_factor) *
344  target_state->getVariableVelocity(move_group_idx.at(joint)));
345 
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);
349  }
350 }
351 
352 void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& first_waypoint,
353  const moveit::core::JointModelGroup* joint_group,
354  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
355 {
356  const size_t num_dof = joint_group->getVariableCount();
357  const std::vector<int>& idx = joint_group->getVariableIndexList();
358 
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);
362 
363  for (size_t i = 0; i < num_dof; ++i)
364  {
365  current_positions_vector.at(i) = first_waypoint.getVariablePosition(idx.at(i));
366  current_velocities_vector.at(i) = first_waypoint.getVariableVelocity(idx.at(i));
367  current_accelerations_vector.at(i) = first_waypoint.getVariableAcceleration(idx.at(i));
368  // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
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));
373  }
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());
377 }
378 
379 void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint,
380  const moveit::core::RobotStateConstPtr& next_waypoint,
381  const moveit::core::JointModelGroup* joint_group,
382  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
383 {
384  const size_t num_dof = joint_group->getVariableCount();
385  const std::vector<int>& idx = joint_group->getVariableIndexList();
386 
387  for (size_t joint = 0; joint < num_dof; ++joint)
388  {
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));
392 
393  // Target state is the next waypoint
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));
397 
398  // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
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));
411  }
412 }
413 
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)
417 {
418  // For every timestep
419  for (double time_from_start = OVERSHOOT_CHECK_PERIOD; time_from_start < ruckig_trajectory.get_duration();
420  time_from_start += OVERSHOOT_CHECK_PERIOD)
421  {
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);
426  // For every joint
427  for (size_t joint = 0; joint < num_dof; ++joint)
428  {
429  // If the sign of the error changed and the threshold difference was exceeded
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)
433  {
434  return true;
435  }
436  }
437  }
438  return false;
439 }
440 } // namespace trajectory_processing
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...
Definition: robot_model.h:76
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
Definition: robot_model.h:433
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:395
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:296
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:207
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & setWayPointDurationFromPrevious(std::size_t index, double value)
const moveit::core::JointModelGroup * getGroup() const
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
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.