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 
54 
55 namespace
56 {
57 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
58 {
59  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " time [%zu]= %f", i,
60  point.time_from_start.toSec());
61  if (point.positions.size() >= 7)
62  {
63  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " pos_ [%zu]= %f %f %f %f %f %f %f", i,
64  point.positions[0], point.positions[1], point.positions[2], point.positions[3], point.positions[4],
65  point.positions[5], point.positions[6]);
66  }
67  if (point.velocities.size() >= 7)
68  {
69  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " vel_ [%zu]= %f %f %f %f %f %f %f", i,
70  point.velocities[0], point.velocities[1], point.velocities[2], point.velocities[3],
71  point.velocities[4], point.velocities[5], point.velocities[6]);
72  }
73  if (point.accelerations.size() >= 7)
74  {
75  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " acc_ [%zu]= %f %f %f %f %f %f %f", i,
76  point.accelerations[0], point.accelerations[1], point.accelerations[2], point.accelerations[3],
77  point.accelerations[4], point.accelerations[5], point.accelerations[6]);
78  }
79 }
80 
81 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
82 {
83  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "jointNames= %s %s %s %s %s %s %s",
84  limits[0].joint_name.c_str(), limits[1].joint_name.c_str(), limits[2].joint_name.c_str(),
85  limits[3].joint_name.c_str(), limits[4].joint_name.c_str(), limits[5].joint_name.c_str(),
86  limits[6].joint_name.c_str());
87  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxVelocities= %f %f %f %f %f %f %f",
88  limits[0].max_velocity, limits[1].max_velocity, limits[2].max_velocity, limits[3].max_velocity,
89  limits[4].max_velocity, limits[5].max_velocity, limits[6].max_velocity);
90  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxAccelerations= %f %f %f %f %f %f %f",
91  limits[0].max_acceleration, limits[1].max_acceleration, limits[2].max_acceleration,
92  limits[3].max_acceleration, limits[4].max_acceleration, limits[5].max_acceleration,
93  limits[6].max_acceleration);
94  // for every point in time:
95  for (std::size_t i = 0; i < trajectory.points.size(); ++i)
96  printPoint(trajectory.points[i], i);
97 }
98 } // namespace
99 
100 // Applies velocity
102  std::vector<double>& time_diff,
103  const double max_velocity_scaling_factor) const
104 {
105  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
106  const std::vector<std::string>& vars = group->getVariableNames();
107  const std::vector<int>& idx = group->getVariableIndexList();
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 robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
127  const robot_state::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 robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
133  if (b.velocity_bounded_)
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 namespace
187 {
188 // Takes the time differences, and updates the timestamps, velocities and accelerations
189 // in the trajectory.
190 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
191 {
192  // Error check
193  if (time_diff.empty())
194  return;
195 
196  double time_sum = 0.0;
197 
198  robot_state::RobotStatePtr prev_waypoint;
199  robot_state::RobotStatePtr curr_waypoint;
200  robot_state::RobotStatePtr next_waypoint;
201 
202  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
203  const std::vector<std::string>& vars = group->getVariableNames();
204  const std::vector<int>& idx = group->getVariableIndexList();
205 
206  int num_points = rob_trajectory.getWayPointCount();
207 
208  rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);
209 
210  // Times
211  for (int i = 1; i < num_points; ++i)
212  // Update the time between the waypoints in the robot_trajectory.
213  rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
214 
215  // Return if there is only one point in the trajectory!
216  if (num_points <= 1)
217  return;
218 
219  // Accelerations
220  for (int i = 0; i < num_points; ++i)
221  {
222  curr_waypoint = rob_trajectory.getWayPointPtr(i);
223 
224  if (i > 0)
225  prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);
226 
227  if (i < num_points - 1)
228  next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
229 
230  for (std::size_t j = 0; j < vars.size(); ++j)
231  {
232  double q1;
233  double q2;
234  double q3;
235  double dt1;
236  double dt2;
237 
238  if (i == 0)
239  {
240  // First point
241  q1 = next_waypoint->getVariablePosition(idx[j]);
242  q2 = curr_waypoint->getVariablePosition(idx[j]);
243  q3 = q1;
244 
245  dt1 = dt2 = time_diff[i];
246  }
247  else if (i < num_points - 1)
248  {
249  // middle points
250  q1 = prev_waypoint->getVariablePosition(idx[j]);
251  q2 = curr_waypoint->getVariablePosition(idx[j]);
252  q3 = next_waypoint->getVariablePosition(idx[j]);
253 
254  dt1 = time_diff[i - 1];
255  dt2 = time_diff[i];
256  }
257  else
258  {
259  // last point
260  q1 = prev_waypoint->getVariablePosition(idx[j]);
261  q2 = curr_waypoint->getVariablePosition(idx[j]);
262  q3 = q1;
263 
264  dt1 = dt2 = time_diff[i - 1];
265  }
266 
267  double v1, v2, a;
268 
269  bool start_velocity = false;
270  if (dt1 == 0.0 || dt2 == 0.0)
271  {
272  v1 = 0.0;
273  v2 = 0.0;
274  a = 0.0;
275  }
276  else
277  {
278  if (i == 0)
279  {
280  if (curr_waypoint->hasVelocities())
281  {
282  start_velocity = true;
283  v1 = curr_waypoint->getVariableVelocity(idx[j]);
284  }
285  }
286  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
287  // v2 = (q3-q2)/dt2;
288  v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point
289  a = 2.0 * (v2 - v1) / (dt1 + dt2);
290  }
291 
292  curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
293  curr_waypoint->setVariableAcceleration(idx[j], a);
294  }
295  }
296 }
297 } // namespace
298 
299 // Applies Acceleration constraints
301  robot_trajectory::RobotTrajectory& rob_trajectory, std::vector<double>& time_diff,
302  const double max_acceleration_scaling_factor) const
303 {
304  robot_state::RobotStatePtr prev_waypoint;
305  robot_state::RobotStatePtr curr_waypoint;
306  robot_state::RobotStatePtr next_waypoint;
307 
308  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
309  const std::vector<std::string>& vars = group->getVariableNames();
310  const std::vector<int>& idx = group->getVariableIndexList();
312 
313  const int num_points = rob_trajectory.getWayPointCount();
314  const unsigned int num_joints = group->getVariableCount();
315  int num_updates = 0;
316  int iteration = 0;
317  bool backwards = false;
318  double q1;
319  double q2;
320  double q3;
321  double dt1;
322  double dt2;
323  double v1;
324  double v2;
325  double a;
326 
327  double acceleration_scaling_factor = 1.0;
328 
329  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
330  acceleration_scaling_factor = max_acceleration_scaling_factor;
331  else if (max_acceleration_scaling_factor == 0.0)
332  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization",
333  "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
334  acceleration_scaling_factor);
335  else
336  ROS_WARN_NAMED("trajectory_processing.iterative_time_parameterization",
337  "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
338  max_acceleration_scaling_factor, acceleration_scaling_factor);
339 
340  do
341  {
342  num_updates = 0;
343  iteration++;
344 
345  // In this case we iterate through the joints on the outer loop.
346  // This is so that any time interval increases have a chance to get propogated through the trajectory
347  for (unsigned int j = 0; j < num_joints; ++j)
348  {
349  // Loop forwards, then backwards
350  for (int count = 0; count < 2; ++count)
351  {
352  for (int i = 0; i < num_points - 1; ++i)
353  {
354  int index = backwards ? (num_points - 1) - i : i;
355 
356  curr_waypoint = rob_trajectory.getWayPointPtr(index);
357 
358  if (index > 0)
359  prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
360 
361  if (index < num_points - 1)
362  next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
363 
364  // Get acceleration limits
365  double a_max = DEFAULT_ACCEL_MAX;
366  const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
367  if (b.acceleration_bounded_)
368  a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
369  fabs(b.min_acceleration_ * acceleration_scaling_factor));
370 
371  if (index == 0)
372  {
373  // First point
374  q1 = next_waypoint->getVariablePosition(idx[j]);
375  q2 = curr_waypoint->getVariablePosition(idx[j]);
376  q3 = next_waypoint->getVariablePosition(idx[j]);
377 
378  dt1 = dt2 = time_diff[index];
379  assert(!backwards);
380  }
381  else if (index < num_points - 1)
382  {
383  // middle points
384  q1 = prev_waypoint->getVariablePosition(idx[j]);
385  q2 = curr_waypoint->getVariablePosition(idx[j]);
386  q3 = next_waypoint->getVariablePosition(idx[j]);
387 
388  dt1 = time_diff[index - 1];
389  dt2 = time_diff[index];
390  }
391  else
392  {
393  // last point - careful, there are only numpoints-1 time intervals
394  q1 = prev_waypoint->getVariablePosition(idx[j]);
395  q2 = curr_waypoint->getVariablePosition(idx[j]);
396  q3 = prev_waypoint->getVariablePosition(idx[j]);
397 
398  dt1 = dt2 = time_diff[index - 1];
399  assert(backwards);
400  }
401 
402  if (dt1 == 0.0 || dt2 == 0.0)
403  {
404  v1 = 0.0;
405  v2 = 0.0;
406  a = 0.0;
407  }
408  else
409  {
410  bool start_velocity = false;
411  if (index == 0)
412  {
413  if (curr_waypoint->hasVelocities())
414  {
415  start_velocity = true;
416  v1 = curr_waypoint->getVariableVelocity(idx[j]);
417  }
418  }
419  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
420  v2 = (q3 - q2) / dt2;
421  a = 2.0 * (v2 - v1) / (dt1 + dt2);
422  }
423 
424  if (fabs(a) > a_max + ROUNDING_THRESHOLD)
425  {
426  if (!backwards)
427  {
428  dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
429  time_diff[index] = dt2;
430  }
431  else
432  {
433  dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
434  time_diff[index - 1] = dt1;
435  }
436  num_updates++;
437 
438  if (dt1 == 0.0 || dt2 == 0.0)
439  {
440  v1 = 0.0;
441  v2 = 0.0;
442  a = 0.0;
443  }
444  else
445  {
446  v1 = (q2 - q1) / dt1;
447  v2 = (q3 - q2) / dt2;
448  a = 2 * (v2 - v1) / (dt1 + dt2);
449  }
450  }
451  }
452  backwards = !backwards;
453  }
454  }
455  // ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "applyAcceleration: num_updates=%i",
456  // num_updates);
457  } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
458 }
459 
461  const double max_velocity_scaling_factor,
462  const double max_acceleration_scaling_factor) const
463 {
464  if (trajectory.empty())
465  return true;
466 
467  const robot_model::JointModelGroup* group = trajectory.getGroup();
468  if (!group)
469  {
470  ROS_ERROR_NAMED("trajectory_processing.iterative_time_parameterization", "It looks like the planner did not set "
471  "the group the plan was computed for");
472  return false;
473  }
474 
475  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
476  trajectory.unwind();
477 
478  const int num_points = trajectory.getWayPointCount();
479  std::vector<double> time_diff(num_points - 1, 0.0); // the time difference between adjacent points
480 
481  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
482  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
483 
484  updateTrajectory(trajectory, time_diff);
485  return true;
486 }
487 } // namespace trajectory_processing
void applyVelocityConstraints(robot_trajectory::RobotTrajectory &rob_trajectory, std::vector< double > &time_diff, const double max_velocity_scaling_factor) const
maximum allowed time change per iteration in seconds
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const
#define ROS_WARN_NAMED(name,...)
void applyAccelerationConstraints(robot_trajectory::RobotTrajectory &rob_trajectory, std::vector< double > &time_diff, const double max_acceleration_scaling_factor) const
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...
double max_time_change_per_it_
maximum number of iterations to find solution
std::size_t getWayPointCount() const
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
#define ROS_DEBUG_NAMED(name,...)
unsigned int index
const robot_model::JointModelGroup * getGroup() const
Maintain a sequence of waypoints and the time durations between these waypoints.
void setWayPointDurationFromPrevious(std::size_t index, double value)
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
#define ROS_ERROR_NAMED(name,...)
IterativeParabolicTimeParameterization(unsigned int max_iterations=100, double max_time_change_per_it=.01)
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
double findT1(const double d1, const double d2, double t1, const double t2, const double a_max) const
moveit::core::RobotModelConstPtr rmodel
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:407
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri May 17 2019 02:51:53