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 }
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 }
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 }
double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
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)
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...
#define ROS_WARN_NAMED(name,...)
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
double max_time_change_per_it_
maximum number of iterations to find solution
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
#define ROS_DEBUG_NAMED(name,...)
unsigned int index
const robot_model::JointModelGroup * getGroup() const
double findT1(const double d1, const double d2, double t1, const double t2, const double a_max) const
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
Maintain a sequence of waypoints and the time durations between these waypoints.
void setWayPointDurationFromPrevious(std::size_t index, double value)
std::size_t getWayPointCount() const
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
void applyAccelerationConstraints(robot_trajectory::RobotTrajectory &rob_trajectory, std::vector< double > &time_diff, const double max_acceleration_scaling_factor) const
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
#define ROS_ERROR_NAMED(name,...)
IterativeParabolicTimeParameterization(unsigned int max_iterations=100, double max_time_change_per_it=.01)
moveit::core::RobotModelConstPtr rmodel


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05