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 <algorithm>
37 #include <cmath>
38 #include <Eigen/Geometry>
39 #include <limits>
41 #include <ros/ros.h>
42 #include <vector>
43 
44 namespace trajectory_processing
45 {
46 namespace
47 {
48 const std::string LOGNAME = "moveit_trajectory_processing.ruckig_traj_smoothing";
49 constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s
50 constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2
51 constexpr double DEFAULT_MAX_JERK = 100; // rad/s^3
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; // sec
55 } // namespace
56 
58  const double max_velocity_scaling_factor,
59  const double max_acceleration_scaling_factor, const bool mitigate_overshoot,
60  const double overshoot_threshold)
61 {
62  if (!validateGroup(trajectory))
63  {
64  return false;
65  }
66 
67  const size_t num_waypoints = trajectory.getWayPointCount();
68  if (num_waypoints < 2)
69  {
71  "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
72  return true;
73  }
74 
75  // Kinematic limits (vels/accels/jerks) from RobotModel
76  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
77  const size_t num_dof = group->getVariableCount();
78  ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
79  if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input))
80  {
81  ROS_ERROR_NAMED(LOGNAME, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
82  return false;
83  }
84 
85  return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
86 }
87 
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)
93 {
94  if (!validateGroup(trajectory))
95  {
96  return false;
97  }
98 
99  const size_t num_waypoints = trajectory.getWayPointCount();
100  if (num_waypoints < 2)
101  {
103  "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
104  return true;
105  }
106 
107  // Set default kinematic limits (vels/accels/jerks)
108  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
109  const size_t num_dof = group->getVariableCount();
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))
114  {
115  ROS_ERROR_NAMED(LOGNAME, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
116  return false;
117  }
118 
119  // Check if custom limits were supplied as arguments to overwrite the defaults
120  const std::vector<std::string>& vars = group->getVariableNames();
121  const unsigned num_joints = group->getVariableCount();
122  for (size_t j = 0; j < num_joints; ++j)
123  {
124  // Velocity
125  auto it = velocity_limits.find(vars[j]);
126  if (it != velocity_limits.end())
127  {
128  ruckig_input.max_velocity.at(j) = it->second;
129  }
130  // Acceleration
131  it = acceleration_limits.find(vars[j]);
132  if (it != acceleration_limits.end())
133  {
134  ruckig_input.max_acceleration.at(j) = it->second;
135  }
136  // Jerk
137  it = jerk_limits.find(vars[j]);
138  if (it != jerk_limits.end())
139  {
140  ruckig_input.max_jerk.at(j) = it->second;
141  }
142  }
143 
144  return runRuckig(trajectory, ruckig_input, mitigate_overshoot, overshoot_threshold);
145 }
146 
147 std::optional<robot_trajectory::RobotTrajectory>
149  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input, size_t batch_size)
150 {
151  // We take the batch size as the lesser of 0.1*num_waypoints or batch_size,
152  // to keep a balance between run time and time-optimality.
153  // TODO(andyz): parameterize as MIN_BATCH_SIZE and BATCH_SCALING_FACTOR or something like that
154  const size_t num_waypoints = trajectory.getWayPointCount();
155  const size_t temp_batch_size = std::min(size_t(0.1 * num_waypoints), size_t(100));
156  // We need at least 2 waypoints
157  batch_size = std::max(size_t(2), temp_batch_size);
158 
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;
162  // A deep copy is not needed since the waypoints are cleared immediately
163  robot_trajectory::RobotTrajectory sub_trajectory =
164  robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */);
165  robot_trajectory::RobotTrajectory output_trajectory =
166  robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */);
167  output_trajectory.clear();
168 
169  while (batch_end_idx <= full_traj_final_idx)
170  {
171  sub_trajectory.clear();
172  for (size_t waypoint_idx = batch_start_idx; waypoint_idx <= batch_end_idx; ++waypoint_idx)
173  {
174  sub_trajectory.addSuffixWayPoint(trajectory.getWayPoint(waypoint_idx),
175  trajectory.getWayPointDurationFromPrevious(waypoint_idx));
176  }
177 
178  // When starting a new batch, set the last Ruckig output equal to the new, starting robot state
179  bool first_point_previously_smoothed = false;
180  if (output_trajectory.getWayPointCount() > 0)
181  {
182  sub_trajectory.addPrefixWayPoint(output_trajectory.getLastWayPoint(), 0);
183  first_point_previously_smoothed = true;
184  }
185 
186  if (!runRuckig(sub_trajectory, ruckig_input))
187  {
188  return std::nullopt;
189  }
190 
191  // Skip appending the first waypoint in sub_trajectory if it was smoothed in
192  // the previous iteration
193  size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0;
194 
195  // Add smoothed waypoints to the output
196  for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount(); ++waypoint_idx)
197  {
198  output_trajectory.addSuffixWayPoint(sub_trajectory.getWayPoint(waypoint_idx),
199  sub_trajectory.getWayPointDurationFromPrevious(waypoint_idx));
200  }
201 
202  batch_start_idx += batch_size;
203  batch_end_idx += batch_size;
204  }
205 
206  return std::make_optional<robot_trajectory::RobotTrajectory>(output_trajectory, true /* deep copy */);
207 }
208 
210 {
211  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
212  if (!group)
213  {
214  ROS_ERROR_NAMED(LOGNAME, "The planner did not set the group the plan was computed for");
215  return false;
216  }
217  return true;
218 }
219 
220 bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_factor,
221  const double max_acceleration_scaling_factor,
222  moveit::core::JointModelGroup const* const group,
223  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
224 {
225  const size_t num_dof = group->getVariableCount();
226  const std::vector<std::string>& vars = group->getVariableNames();
227  const moveit::core::RobotModel& rmodel = group->getParentModel();
228  for (size_t i = 0; i < num_dof; ++i)
229  {
230  const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
231 
232  // This assumes min/max bounds are symmetric
233  if (bounds.velocity_bounded_)
234  {
235  ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * bounds.max_velocity_;
236  }
237  else
238  {
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;
244  }
245  if (bounds.acceleration_bounded_)
246  {
247  ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * bounds.max_acceleration_;
248  }
249  else
250  {
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;
256  }
257  // Read jerk limits from parameters since bounds.jerk_bounded_ was never implemented for MoveIt1
258  double jerk_limit = DEFAULT_MAX_JERK;
259  std::string jerk_param = "ruckig/" + vars.at(i) + "/jerk_limit";
260  if (ros::param::get(jerk_param, jerk_limit))
261  {
262  ruckig_input.max_jerk.at(i) = jerk_limit;
263  }
264  else
265  {
266  ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
267  ROS_WARN_STREAM_NAMED(LOGNAME, "Joint jerk limit for joint " + vars.at(i) + " was not defined. Using the default "
268  << DEFAULT_MAX_JERK
269  << " rad/s^3. You can define a jerk limit with parameter " + jerk_param);
270  }
271  }
272 
273  return true;
274 }
275 
277  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
278  const bool mitigate_overshoot, const double overshoot_threshold)
279 {
280  const size_t num_waypoints = trajectory.getWayPointCount();
281  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
282  const size_t num_dof = group->getVariableCount();
283  ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
284 
285  // This lib does not work properly when angles wrap, so we need to unwind the path first
286  trajectory.unwind();
287 
288  // Initialize the smoother
289  ruckig::Ruckig<ruckig::DynamicDOFs> ruckig(num_dof, trajectory.getAverageSegmentDuration());
290  initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input);
291 
292  // Cache the trajectory in case we need to reset it
293  robot_trajectory::RobotTrajectory original_trajectory =
294  robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);
295 
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)
301  {
302  while (waypoint_idx < num_waypoints - 1)
303  {
304  moveit::core::RobotStatePtr curr_waypoint = trajectory.getWayPointPtr(waypoint_idx);
305  moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);
306 
307  getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input);
308 
309  // Run Ruckig
310  ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_trajectory(num_dof);
311  ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory);
312 
313  // Step through the trajectory at the given OVERSHOOT_CHECK_PERIOD and check for overshoot.
314  // We will extend the duration to mitigate it.
315  bool overshoots = false;
316  if (mitigate_overshoot)
317  {
318  overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold);
319  }
320 
321  // The difference between Result::Working and Result::Finished is that Finished can be reached in one
322  // Ruckig timestep (constructor parameter). Both are acceptable for trajectories.
323  // (The difference is only relevant for streaming mode.)
324 
325  // If successful and at the last trajectory segment
326  if (!overshoots && (waypoint_idx == num_waypoints - 2) &&
327  (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
328  {
329  trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_trajectory.get_duration());
330  smoothing_complete = true;
331  break;
332  }
333 
334  // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
335  if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished))
336  {
337  duration_extension_factor *= DURATION_EXTENSION_FRACTION;
338 
339  const std::vector<int>& move_group_idx = group->getVariableIndexList();
340  extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory,
341  trajectory);
342 
343  initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input);
344  // Continue the loop from failed segment, but with increased duration extension factor
345  break;
346  }
347  ++waypoint_idx;
348  }
349  }
350 
351  if (duration_extension_factor > MAX_DURATION_EXTENSION_FACTOR)
352  {
354  "Ruckig extended the trajectory duration to its maximum and still did not find a solution");
355  }
356 
357  if (ruckig_result != ruckig::Result::Finished && ruckig_result != ruckig::Result::Working)
358  {
359  ROS_ERROR_STREAM_NAMED(LOGNAME, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
360  return false;
361  }
362 
363  return true;
364 }
365 
366 void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t waypoint_idx,
367  const size_t num_dof, const std::vector<int>& move_group_idx,
368  const robot_trajectory::RobotTrajectory& original_trajectory,
370 {
371  trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1,
372  duration_extension_factor *
373  original_trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1));
374  // re-calculate waypoint velocity and acceleration
375  auto target_state = trajectory.getWayPointPtr(waypoint_idx + 1);
376  const auto prev_state = trajectory.getWayPointPtr(waypoint_idx);
377 
378  double timestep = trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1);
379 
380  for (size_t joint = 0; joint < num_dof; ++joint)
381  {
382  target_state->setVariableVelocity(move_group_idx.at(joint),
383  (1 / duration_extension_factor) *
384  target_state->getVariableVelocity(move_group_idx.at(joint)));
385 
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);
389  }
390 }
391 
393  const moveit::core::JointModelGroup* joint_group,
394  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
395 {
396  const size_t num_dof = joint_group->getVariableCount();
397  const std::vector<int>& idx = joint_group->getVariableIndexList();
398 
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);
402 
403  for (size_t i = 0; i < num_dof; ++i)
404  {
405  current_positions_vector.at(i) = first_waypoint.getVariablePosition(idx.at(i));
406  current_velocities_vector.at(i) = first_waypoint.getVariableVelocity(idx.at(i));
407  current_accelerations_vector.at(i) = first_waypoint.getVariableAcceleration(idx.at(i));
408  // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
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));
413  }
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());
417 }
418 
419 void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint,
420  const moveit::core::RobotStateConstPtr& next_waypoint,
421  const moveit::core::JointModelGroup* joint_group,
422  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
423 {
424  const size_t num_dof = joint_group->getVariableCount();
425  const std::vector<int>& idx = joint_group->getVariableIndexList();
426 
427  for (size_t joint = 0; joint < num_dof; ++joint)
428  {
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));
432 
433  // Target state is the next waypoint
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));
437 
438  // Clamp velocities/accelerations in case they exceed the limit due to small numerical errors
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));
451  }
452 }
453 
454 bool RuckigSmoothing::checkOvershoot(ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector>& ruckig_trajectory,
455  const size_t num_dof, ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
456  const double overshoot_threshold)
457 {
458  // For every timestep
459  for (double time_from_start = OVERSHOOT_CHECK_PERIOD; time_from_start < ruckig_trajectory.get_duration();
460  time_from_start += OVERSHOOT_CHECK_PERIOD)
461  {
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);
466  // For every joint
467  for (size_t joint = 0; joint < num_dof; ++joint)
468  {
469  // If the sign of the error changed and the threshold difference was exceeded
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)
473  {
474  return true;
475  }
476  }
477  }
478  return false;
479 }
480 } // namespace trajectory_processing
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:131
robot_trajectory::RobotTrajectory::getGroup
const moveit::core::JointModelGroup * getGroup() const
Definition: robot_trajectory.h:118
moveit::core::RobotState::getVariableVelocity
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:362
moveit::core::JointModelGroup::getVariableIndexList
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
Definition: joint_model_group.h:351
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::VariableBounds
Definition: joint_model.h:118
ros::param::get
ROSCPP_DECL bool get(const std::string &key, bool &b)
ros.h
robot_trajectory::RobotTrajectory::getFirstWayPointPtr
moveit::core::RobotStatePtr & getFirstWayPointPtr()
Definition: robot_trajectory.h:161
robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious
RobotTrajectory & setWayPointDurationFromPrevious(std::size_t index, double value)
Definition: robot_trajectory.h:187
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:143
moveit::core::RobotModel::getVariableBounds
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:512
robot_trajectory::RobotTrajectory::addSuffixWayPoint
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
Definition: robot_trajectory.h:205
moveit::core::JointModelGroup::getParentModel
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
Definition: joint_model_group.h:179
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
trajectory_processing::RuckigSmoothing::extendTrajectoryDuration
static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints, const size_t num_dof, const std::vector< int > &move_group_idx, const robot_trajectory::RobotTrajectory &original_trajectory, robot_trajectory::RobotTrajectory &trajectory)
Extend the duration of every trajectory segment.
Definition: ruckig_traj_smoothing.cpp:397
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
moveit::core::JointModelGroup::getVariableNames
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
Definition: joint_model_group.h:254
trajectory_processing::RuckigSmoothing::runRuckigInBatches
static std::optional< robot_trajectory::RobotTrajectory > runRuckigInBatches(const robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, size_t batch_size=100)
Break the trajectory parameter into batches of reasonable size (~100), run Ruckig on each of them,...
Definition: ruckig_traj_smoothing.cpp:179
moveit::core::VariableBounds::max_velocity_
double max_velocity_
Definition: joint_model.h:171
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
robot_trajectory::RobotTrajectory::clear
RobotTrajectory & clear()
Definition: robot_trajectory.h:264
moveit::core::RobotState::getVariablePosition
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:273
trajectory_processing::RuckigSmoothing::initializeRuckigState
static void initializeRuckigState(const moveit::core::RobotState &first_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same val...
Definition: ruckig_traj_smoothing.cpp:423
trajectory_processing
Definition: iterative_spline_parameterization.h:43
robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious
double getWayPointDurationFromPrevious(std::size_t index) const
Definition: robot_trajectory.h:179
ROS_WARN_STREAM_ONCE_NAMED
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
robot_trajectory::RobotTrajectory::getLastWayPoint
const moveit::core::RobotState & getLastWayPoint() const
Definition: robot_trajectory.h:141
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
ruckig_traj_smoothing.h
robot_trajectory::RobotTrajectory::getAverageSegmentDuration
double getAverageSegmentDuration() const
Definition: robot_trajectory.cpp:119
trajectory_processing::RuckigSmoothing::applySmoothing
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 jerk-limited smoothing to a trajectory.
Definition: ruckig_traj_smoothing.cpp:88
moveit::core::VariableBounds::acceleration_bounded_
bool acceleration_bounded_
Definition: joint_model.h:176
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
moveit::core::RobotState::getVariableAcceleration
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:461
moveit::core::VariableBounds::velocity_bounded_
bool velocity_bounded_
Definition: joint_model.h:172
trajectory_processing::RuckigSmoothing::getNextRuckigInput
static void getNextRuckigInput(const moveit::core::RobotStateConstPtr &current_waypoint, const moveit::core::RobotStateConstPtr &next_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
Feed previous output back as input for next iteration. Get next target state from the next waypoint.
Definition: ruckig_traj_smoothing.cpp:450
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:151
trajectory_processing::RuckigSmoothing::validateGroup
static bool validateGroup(const robot_trajectory::RobotTrajectory &trajectory)
A utility function to check if the group is defined.
Definition: ruckig_traj_smoothing.cpp:240
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
trajectory_processing::RuckigSmoothing::getRobotModelBounds
static bool getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, moveit::core::JointModelGroup const *const group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
A utility function to get bounds from a JointModelGroup and save them for Ruckig.
Definition: ruckig_traj_smoothing.cpp:251
robot_trajectory::RobotTrajectory::unwind
RobotTrajectory & unwind()
Definition: robot_trajectory.cpp:170
robot_trajectory::RobotTrajectory::addPrefixWayPoint
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:223
moveit::core::VariableBounds::max_acceleration_
double max_acceleration_
Definition: joint_model.h:175
trajectory_processing::RuckigSmoothing::runRuckig
static bool runRuckig(robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01)
A utility function to instantiate and run Ruckig for a series of waypoints.
Definition: ruckig_traj_smoothing.cpp:307
trajectory_processing::LOGNAME
const std::string LOGNAME
Definition: time_optimal_trajectory_generation.cpp:49
trajectory_processing::RuckigSmoothing::checkOvershoot
static bool checkOvershoot(ruckig::Trajectory< ruckig::DynamicDOFs, ruckig::StandardVector > &ruckig_trajectory, const size_t num_dof, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const double overshoot_threshold)
Check if a trajectory out of Ruckig overshoots the target state.
Definition: ruckig_traj_smoothing.cpp:485
robot_trajectory::RobotTrajectory::getWayPoint
const moveit::core::RobotState & getWayPoint(std::size_t index) const
Definition: robot_trajectory.h:136


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 21 2024 03:23:42