iterative_time_parameterization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
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
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ken Anderson */
36 
38 #include <moveit_msgs/JointLimits.h>
40 
41 namespace trajectory_processing
42 {
43 static const double DEFAULT_VEL_MAX = 1.0;
44 static const double DEFAULT_ACCEL_MAX = 1.0;
45 static const double ROUNDING_THRESHOLD = 0.01;
46 
48  double max_time_change_per_it)
49  : max_iterations_(max_iterations), max_time_change_per_it_(max_time_change_per_it)
50 {
51 }
52 
53 #if 0 // unused functions
54 namespace
55 {
56 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
57 {
58  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " time [%zu]= %f", i,
59  point.time_from_start.toSec());
60  if (point.positions.size() >= 7)
61  {
62  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " pos_ [%zu]= %f %f %f %f %f %f %f", i,
63  point.positions[0], point.positions[1], point.positions[2], point.positions[3], point.positions[4],
64  point.positions[5], point.positions[6]);
65  }
66  if (point.velocities.size() >= 7)
67  {
68  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " vel_ [%zu]= %f %f %f %f %f %f %f", i,
69  point.velocities[0], point.velocities[1], point.velocities[2], point.velocities[3],
70  point.velocities[4], point.velocities[5], point.velocities[6]);
71  }
72  if (point.accelerations.size() >= 7)
73  {
74  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " acc_ [%zu]= %f %f %f %f %f %f %f", i,
75  point.accelerations[0], point.accelerations[1], point.accelerations[2], point.accelerations[3],
76  point.accelerations[4], point.accelerations[5], point.accelerations[6]);
77  }
78 }
79 
80 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
81 {
82  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "jointNames= %s %s %s %s %s %s %s",
83  limits[0].joint_name.c_str(), limits[1].joint_name.c_str(), limits[2].joint_name.c_str(),
84  limits[3].joint_name.c_str(), limits[4].joint_name.c_str(), limits[5].joint_name.c_str(),
85  limits[6].joint_name.c_str());
86  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxVelocities= %f %f %f %f %f %f %f",
87  limits[0].max_velocity, limits[1].max_velocity, limits[2].max_velocity, limits[3].max_velocity,
88  limits[4].max_velocity, limits[5].max_velocity, limits[6].max_velocity);
89  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxAccelerations= %f %f %f %f %f %f %f",
90  limits[0].max_acceleration, limits[1].max_acceleration, limits[2].max_acceleration,
91  limits[3].max_acceleration, limits[4].max_acceleration, limits[5].max_acceleration,
92  limits[6].max_acceleration);
93  // for every point in time:
94  for (std::size_t i = 0; i < trajectory.points.size(); ++i)
95  printPoint(trajectory.points[i], i);
96 }
97 } // namespace
98 #endif
99 
100 // Applies velocity
101 void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
102  std::vector<double>& time_diff,
103  const double max_velocity_scaling_factor) const
104 {
105  const moveit::core::JointModelGroup* group = rob_trajectory.getGroup();
106  const std::vector<std::string>& vars = group->getVariableNames();
107  const std::vector<int>& idx = group->getVariableIndexList();
108  const moveit::core::RobotModel& rmodel = group->getParentModel();
109  const int num_points = rob_trajectory.getWayPointCount();
110 
111  double velocity_scaling_factor = 1.0;
112 
113  if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
114  velocity_scaling_factor = max_velocity_scaling_factor;
115  else if (max_velocity_scaling_factor == 0.0)
116  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization",
117  "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
118  velocity_scaling_factor);
119  else
120  ROS_WARN_NAMED("trajectory_processing.iterative_time_parameterization",
121  "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
122  max_velocity_scaling_factor, velocity_scaling_factor);
123 
124  for (int i = 0; i < num_points - 1; ++i)
125  {
126  const moveit::core::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
127  const moveit::core::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
128 
129  for (std::size_t j = 0; j < vars.size(); ++j)
130  {
131  double v_max = DEFAULT_VEL_MAX;
132  const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
134  v_max =
135  std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor));
136  const double dq1 = curr_waypoint->getVariablePosition(idx[j]);
137  const double dq2 = next_waypoint->getVariablePosition(idx[j]);
138  const double t_min = std::abs(dq2 - dq1) / v_max;
139  if (t_min > time_diff[i])
140  time_diff[i] = t_min;
141  }
142  }
143 }
144 
145 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
146 // In the future we may want to solve to quadratic equation to get the exact timing interval.
147 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
148 double IterativeParabolicTimeParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2,
149  const double a_max) const
150 {
151  const double mult_factor = 1.01;
152  double v1 = (dq1) / dt1;
153  double v2 = (dq2) / dt2;
154  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
155 
156  while (std::abs(a) > a_max)
157  {
158  v1 = (dq1) / dt1;
159  v2 = (dq2) / dt2;
160  a = 2.0 * (v2 - v1) / (dt1 + dt2);
161  dt1 *= mult_factor;
162  }
163 
164  return dt1;
165 }
166 
167 double IterativeParabolicTimeParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2,
168  const double a_max) const
169 {
170  const double mult_factor = 1.01;
171  double v1 = (dq1) / dt1;
172  double v2 = (dq2) / dt2;
173  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
174 
175  while (std::abs(a) > a_max)
176  {
177  v1 = (dq1) / dt1;
178  v2 = (dq2) / dt2;
179  a = 2.0 * (v2 - v1) / (dt1 + dt2);
180  dt2 *= mult_factor;
181  }
182 
183  return dt2;
184 }
185 
186 // Takes the time differences, and updates the timestamps, velocities and accelerations
187 // in the trajectory.
188 void IterativeParabolicTimeParameterization::updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory,
189  const std::vector<double>& time_diff)
190 {
191  // Error check
192  if (time_diff.empty())
193  return;
194 
195  double time_sum = 0.0;
196 
197  moveit::core::RobotStatePtr prev_waypoint;
198  moveit::core::RobotStatePtr curr_waypoint;
199  moveit::core::RobotStatePtr next_waypoint;
200 
201  const moveit::core::JointModelGroup* group = rob_trajectory.getGroup();
202  const std::vector<std::string>& vars = group->getVariableNames();
203  const std::vector<int>& idx = group->getVariableIndexList();
204 
205  int num_points = rob_trajectory.getWayPointCount();
206 
207  rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);
208 
209  // Times
210  for (int i = 1; i < num_points; ++i)
211  // Update the time between the waypoints in the robot_trajectory.
212  rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
213 
214  // Return if there is only one point in the trajectory!
215  if (num_points <= 1)
216  return;
217 
218  // Accelerations
219  for (int i = 0; i < num_points; ++i)
220  {
221  curr_waypoint = rob_trajectory.getWayPointPtr(i);
222 
223  if (i > 0)
224  prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);
225 
226  if (i < num_points - 1)
227  next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
228 
229  for (std::size_t j = 0; j < vars.size(); ++j)
230  {
231  double q1;
232  double q2;
233  double q3;
234  double dt1;
235  double dt2;
236 
237  if (i == 0)
238  {
239  // First point
240  q1 = next_waypoint->getVariablePosition(idx[j]);
241  q2 = curr_waypoint->getVariablePosition(idx[j]);
242  q3 = q1;
243 
244  dt1 = dt2 = time_diff[i];
245  }
246  else if (i < num_points - 1)
247  {
248  // middle points
249  q1 = prev_waypoint->getVariablePosition(idx[j]);
250  q2 = curr_waypoint->getVariablePosition(idx[j]);
251  q3 = next_waypoint->getVariablePosition(idx[j]);
252 
253  dt1 = time_diff[i - 1];
254  dt2 = time_diff[i];
255  }
256  else
257  {
258  // last point
259  q1 = prev_waypoint->getVariablePosition(idx[j]);
260  q2 = curr_waypoint->getVariablePosition(idx[j]);
261  q3 = q1;
262 
263  dt1 = dt2 = time_diff[i - 1];
264  }
265 
266  double v1, v2, a;
267 
268  bool start_velocity = false;
269  if (dt1 == 0.0 || dt2 == 0.0)
270  {
271  v1 = 0.0;
272  v2 = 0.0;
273  a = 0.0;
274  }
275  else
276  {
277  if (i == 0)
278  {
279  if (curr_waypoint->hasVelocities())
280  {
281  start_velocity = true;
282  v1 = curr_waypoint->getVariableVelocity(idx[j]);
283  }
284  }
285  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
286  // v2 = (q3-q2)/dt2;
287  v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point
288  a = 2.0 * (v2 - v1) / (dt1 + dt2);
289  }
290 
291  curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
292  curr_waypoint->setVariableAcceleration(idx[j], a);
293  }
294  }
295 }
296 
297 // Applies Acceleration constraints
298 void IterativeParabolicTimeParameterization::applyAccelerationConstraints(
299  robot_trajectory::RobotTrajectory& rob_trajectory, std::vector<double>& time_diff,
300  const double max_acceleration_scaling_factor) const
301 {
302  moveit::core::RobotStatePtr prev_waypoint;
303  moveit::core::RobotStatePtr curr_waypoint;
304  moveit::core::RobotStatePtr next_waypoint;
305 
306  const moveit::core::JointModelGroup* group = rob_trajectory.getGroup();
307  const std::vector<std::string>& vars = group->getVariableNames();
308  const std::vector<int>& idx = group->getVariableIndexList();
309  const moveit::core::RobotModel& rmodel = group->getParentModel();
310 
311  const int num_points = rob_trajectory.getWayPointCount();
312  const unsigned int num_joints = group->getVariableCount();
313  int num_updates = 0;
314  int iteration = 0;
315  bool backwards = false;
316  double q1;
317  double q2;
318  double q3;
319  double dt1;
320  double dt2;
321  double v1;
322  double v2;
323  double a;
324 
325  double acceleration_scaling_factor = 1.0;
326 
327  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
328  acceleration_scaling_factor = max_acceleration_scaling_factor;
329  else if (max_acceleration_scaling_factor == 0.0)
330  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization",
331  "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
332  acceleration_scaling_factor);
333  else
334  ROS_WARN_NAMED("trajectory_processing.iterative_time_parameterization",
335  "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
336  max_acceleration_scaling_factor, acceleration_scaling_factor);
337 
338  do
339  {
340  num_updates = 0;
341  iteration++;
342 
343  // In this case we iterate through the joints on the outer loop.
344  // This is so that any time interval increases have a chance to get propogated through the trajectory
345  for (unsigned int j = 0; j < num_joints; ++j)
346  {
347  // Loop forwards, then backwards
348  for (int count = 0; count < 2; ++count)
349  {
350  for (int i = 0; i < num_points - 1; ++i)
351  {
352  int index = backwards ? (num_points - 1) - i : i;
353 
354  curr_waypoint = rob_trajectory.getWayPointPtr(index);
355 
356  if (index > 0)
357  prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
358 
359  if (index < num_points - 1)
360  next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
361 
362  // Get acceleration limits
363  double a_max = DEFAULT_ACCEL_MAX;
364  const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
365  if (b.acceleration_bounded_)
366  a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
367  fabs(b.min_acceleration_ * acceleration_scaling_factor));
368 
369  if (index == 0)
370  {
371  // First point
372  q1 = next_waypoint->getVariablePosition(idx[j]);
373  q2 = curr_waypoint->getVariablePosition(idx[j]);
374  q3 = next_waypoint->getVariablePosition(idx[j]);
375 
376  dt1 = dt2 = time_diff[index];
377  assert(!backwards);
378  }
379  else if (index < num_points - 1)
380  {
381  // middle points
382  q1 = prev_waypoint->getVariablePosition(idx[j]);
383  q2 = curr_waypoint->getVariablePosition(idx[j]);
384  q3 = next_waypoint->getVariablePosition(idx[j]);
385 
386  dt1 = time_diff[index - 1];
387  dt2 = time_diff[index];
388  }
389  else
390  {
391  // last point - careful, there are only numpoints-1 time intervals
392  q1 = prev_waypoint->getVariablePosition(idx[j]);
393  q2 = curr_waypoint->getVariablePosition(idx[j]);
394  q3 = prev_waypoint->getVariablePosition(idx[j]);
395 
396  dt1 = dt2 = time_diff[index - 1];
397  assert(backwards);
398  }
399 
400  if (dt1 == 0.0 || dt2 == 0.0)
401  {
402  v1 = 0.0;
403  v2 = 0.0;
404  a = 0.0;
405  }
406  else
407  {
408  bool start_velocity = false;
409  if (index == 0)
410  {
411  if (curr_waypoint->hasVelocities())
412  {
413  start_velocity = true;
414  v1 = curr_waypoint->getVariableVelocity(idx[j]);
415  }
416  }
417  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
418  v2 = (q3 - q2) / dt2;
419  a = 2.0 * (v2 - v1) / (dt1 + dt2);
420  }
421 
422  if (fabs(a) > a_max + ROUNDING_THRESHOLD)
423  {
424  if (!backwards)
425  {
426  dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
427  time_diff[index] = dt2;
428  }
429  else
430  {
431  dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
432  time_diff[index - 1] = dt1;
433  }
434  num_updates++;
435 
436  if (dt1 == 0.0 || dt2 == 0.0)
437  {
438  v1 = 0.0;
439  v2 = 0.0;
440  a = 0.0;
441  }
442  else
443  {
444  v1 = (q2 - q1) / dt1;
445  v2 = (q3 - q2) / dt2;
446  a = 2 * (v2 - v1) / (dt1 + dt2);
447  }
448  }
449  }
450  backwards = !backwards;
451  }
452  }
453  // ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "applyAcceleration: num_updates=%i",
454  // num_updates);
455  } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
456 }
457 
458 bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
459  const double max_velocity_scaling_factor,
460  const double max_acceleration_scaling_factor) const
461 {
462  if (trajectory.empty())
463  return true;
464 
465  const moveit::core::JointModelGroup* group = trajectory.getGroup();
466  if (!group)
467  {
468  ROS_ERROR_NAMED("trajectory_processing.iterative_time_parameterization", "It looks like the planner did not set "
469  "the group the plan was computed for");
470  return false;
471  }
472 
473  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
474  trajectory.unwind();
475 
476  const int num_points = trajectory.getWayPointCount();
477  std::vector<double> time_diff(num_points - 1, 0.0); // the time difference between adjacent points
478 
479  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
480  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
481 
482  IterativeParabolicTimeParameterization::updateTrajectory(trajectory, time_diff);
483  return true;
484 }
485 } // 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::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
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:141
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:499
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,...)
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
iterative_time_parameterization.h
conversions.h
trajectory_processing::IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization
IterativeParabolicTimeParameterization(unsigned int max_iterations=100, double max_time_change_per_it=.01)
Definition: iterative_time_parameterization.cpp:79
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
moveit::core::VariableBounds::max_velocity_
double max_velocity_
Definition: joint_model.h:171
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
robot_trajectory::RobotTrajectory::empty
bool empty() const
Definition: robot_trajectory.h:195
trajectory_processing
Definition: iterative_spline_parameterization.h:43
moveit::core::VariableBounds::min_velocity_
double min_velocity_
Definition: joint_model.h:170
trajectory_processing::DEFAULT_ACCEL_MAX
static const double DEFAULT_ACCEL_MAX
Definition: iterative_time_parameterization.cpp:76
trajectory_processing::DEFAULT_VEL_MAX
static const double DEFAULT_VEL_MAX
Definition: iterative_time_parameterization.cpp:75
point
std::chrono::system_clock::time_point point
moveit::core::VariableBounds::acceleration_bounded_
bool acceleration_bounded_
Definition: joint_model.h:176
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
index
unsigned int index
trajectory_processing::ROUNDING_THRESHOLD
static const double ROUNDING_THRESHOLD
Definition: iterative_time_parameterization.cpp:77
moveit::core::VariableBounds::velocity_bounded_
bool velocity_bounded_
Definition: joint_model.h:172
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:151
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
robot_trajectory::RobotTrajectory::unwind
RobotTrajectory & unwind()
Definition: robot_trajectory.cpp:170
moveit::core::VariableBounds::max_acceleration_
double max_acceleration_
Definition: joint_model.h:175
moveit::core::VariableBounds::min_acceleration_
double min_acceleration_
Definition: joint_model.h:174


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15