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>
39 #include <console_bridge/console.h>
41 
42 namespace trajectory_processing
43 {
44 static const double DEFAULT_VEL_MAX = 1.0;
45 static const double DEFAULT_ACCEL_MAX = 1.0;
46 static const double ROUNDING_THRESHOLD = 0.01;
47 
49  double max_time_change_per_it)
50  : max_iterations_(max_iterations), max_time_change_per_it_(max_time_change_per_it)
51 {
52 }
53 
55 {
56 }
57 
58 namespace
59 {
60 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
61 {
62  logDebug(" time [%i]= %f", i, point.time_from_start.toSec());
63  if (point.positions.size() >= 7)
64  {
65  logDebug(" pos_ [%i]= %f %f %f %f %f %f %f", i, point.positions[0], point.positions[1], point.positions[2],
66  point.positions[3], point.positions[4], point.positions[5], point.positions[6]);
67  }
68  if (point.velocities.size() >= 7)
69  {
70  logDebug(" vel_ [%i]= %f %f %f %f %f %f %f", i, point.velocities[0], point.velocities[1], point.velocities[2],
71  point.velocities[3], point.velocities[4], point.velocities[5], point.velocities[6]);
72  }
73  if (point.accelerations.size() >= 7)
74  {
75  logDebug(" acc_ [%i]= %f %f %f %f %f %f %f", i, point.accelerations[0], point.accelerations[1],
76  point.accelerations[2], point.accelerations[3], point.accelerations[4], point.accelerations[5],
77  point.accelerations[6]);
78  }
79 }
80 
81 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
82 {
83  logDebug("jointNames= %s %s %s %s %s %s %s", limits[0].joint_name.c_str(), limits[1].joint_name.c_str(),
84  limits[2].joint_name.c_str(), limits[3].joint_name.c_str(), limits[4].joint_name.c_str(),
85  limits[5].joint_name.c_str(), limits[6].joint_name.c_str());
86  logDebug("maxVelocities= %f %f %f %f %f %f %f", limits[0].max_velocity, limits[1].max_velocity,
87  limits[2].max_velocity, limits[3].max_velocity, limits[4].max_velocity, limits[5].max_velocity,
88  limits[6].max_velocity);
89  logDebug("maxAccelerations= %f %f %f %f %f %f %f", limits[0].max_acceleration, limits[1].max_acceleration,
90  limits[2].max_acceleration, limits[3].max_acceleration, limits[4].max_acceleration,
91  limits[5].max_acceleration, limits[6].max_acceleration);
92  // for every point in time:
93  for (std::size_t i = 0; i < trajectory.points.size(); ++i)
94  printPoint(trajectory.points[i], i);
95 }
96 }
97 
98 // Applies velocity
100  std::vector<double>& time_diff,
101  const double max_velocity_scaling_factor) const
102 {
103  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
104  const std::vector<std::string>& vars = group->getVariableNames();
105  const std::vector<int>& idx = group->getVariableIndexList();
106  const robot_model::RobotModel& rmodel = group->getParentModel();
107  const int num_points = rob_trajectory.getWayPointCount();
108 
109  double velocity_scaling_factor = 1.0;
110 
111  if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
112  velocity_scaling_factor = max_velocity_scaling_factor;
113  else if (max_velocity_scaling_factor == 0.0)
114  logDebug("A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", velocity_scaling_factor);
115  else
116  logWarn("Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", max_velocity_scaling_factor,
117  velocity_scaling_factor);
118 
119  for (int i = 0; i < num_points - 1; ++i)
120  {
121  const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
122  const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
123 
124  for (std::size_t j = 0; j < vars.size(); ++j)
125  {
126  double v_max = DEFAULT_VEL_MAX;
127  const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
128  if (b.velocity_bounded_)
129  v_max =
130  std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor));
131  const double dq1 = curr_waypoint->getVariablePosition(idx[j]);
132  const double dq2 = next_waypoint->getVariablePosition(idx[j]);
133  const double t_min = std::abs(dq2 - dq1) / v_max;
134  if (t_min > time_diff[i])
135  time_diff[i] = t_min;
136  }
137  }
138 }
139 
140 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
141 // In the future we may want to solve to quadratic equation to get the exact timing interval.
142 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
143 double IterativeParabolicTimeParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2,
144  const double a_max) const
145 {
146  const double mult_factor = 1.01;
147  double v1 = (dq1) / dt1;
148  double v2 = (dq2) / dt2;
149  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
150 
151  while (std::abs(a) > a_max)
152  {
153  v1 = (dq1) / dt1;
154  v2 = (dq2) / dt2;
155  a = 2.0 * (v2 - v1) / (dt1 + dt2);
156  dt1 *= mult_factor;
157  }
158 
159  return dt1;
160 }
161 
162 double IterativeParabolicTimeParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2,
163  const double a_max) const
164 {
165  const double mult_factor = 1.01;
166  double v1 = (dq1) / dt1;
167  double v2 = (dq2) / dt2;
168  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
169 
170  while (std::abs(a) > a_max)
171  {
172  v1 = (dq1) / dt1;
173  v2 = (dq2) / dt2;
174  a = 2.0 * (v2 - v1) / (dt1 + dt2);
175  dt2 *= mult_factor;
176  }
177 
178  return dt2;
179 }
180 
181 namespace
182 {
183 // Takes the time differences, and updates the timestamps, velocities and accelerations
184 // in the trajectory.
185 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
186 {
187  // Error check
188  if (time_diff.empty())
189  return;
190 
191  double time_sum = 0.0;
192 
193  robot_state::RobotStatePtr prev_waypoint;
194  robot_state::RobotStatePtr curr_waypoint;
195  robot_state::RobotStatePtr next_waypoint;
196 
197  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
198  const std::vector<std::string>& vars = group->getVariableNames();
199  const std::vector<int>& idx = group->getVariableIndexList();
200 
201  int num_points = rob_trajectory.getWayPointCount();
202 
203  rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);
204 
205  // Times
206  for (int i = 1; i < num_points; ++i)
207  // Update the time between the waypoints in the robot_trajectory.
208  rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
209 
210  // Return if there is only one point in the trajectory!
211  if (num_points <= 1)
212  return;
213 
214  // Accelerations
215  for (int i = 0; i < num_points; ++i)
216  {
217  curr_waypoint = rob_trajectory.getWayPointPtr(i);
218 
219  if (i > 0)
220  prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);
221 
222  if (i < num_points - 1)
223  next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
224 
225  for (std::size_t j = 0; j < vars.size(); ++j)
226  {
227  double q1;
228  double q2;
229  double q3;
230  double dt1;
231  double dt2;
232 
233  if (i == 0)
234  {
235  // First point
236  q1 = next_waypoint->getVariablePosition(idx[j]);
237  q2 = curr_waypoint->getVariablePosition(idx[j]);
238  q3 = q1;
239 
240  dt1 = dt2 = time_diff[i];
241  }
242  else if (i < num_points - 1)
243  {
244  // middle points
245  q1 = prev_waypoint->getVariablePosition(idx[j]);
246  q2 = curr_waypoint->getVariablePosition(idx[j]);
247  q3 = next_waypoint->getVariablePosition(idx[j]);
248 
249  dt1 = time_diff[i - 1];
250  dt2 = time_diff[i];
251  }
252  else
253  {
254  // last point
255  q1 = prev_waypoint->getVariablePosition(idx[j]);
256  q2 = curr_waypoint->getVariablePosition(idx[j]);
257  q3 = q1;
258 
259  dt1 = dt2 = time_diff[i - 1];
260  }
261 
262  double v1, v2, a;
263 
264  bool start_velocity = false;
265  if (dt1 == 0.0 || dt2 == 0.0)
266  {
267  v1 = 0.0;
268  v2 = 0.0;
269  a = 0.0;
270  }
271  else
272  {
273  if (i == 0)
274  {
275  if (curr_waypoint->hasVelocities())
276  {
277  start_velocity = true;
278  v1 = curr_waypoint->getVariableVelocity(idx[j]);
279  }
280  }
281  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
282  // v2 = (q3-q2)/dt2;
283  v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point
284  a = 2.0 * (v2 - v1) / (dt1 + dt2);
285  }
286 
287  curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
288  curr_waypoint->setVariableAcceleration(idx[j], a);
289  }
290  }
291 }
292 }
293 
294 // Applies Acceleration constraints
296  robot_trajectory::RobotTrajectory& rob_trajectory, std::vector<double>& time_diff,
297  const double max_acceleration_scaling_factor) const
298 {
299  robot_state::RobotStatePtr prev_waypoint;
300  robot_state::RobotStatePtr curr_waypoint;
301  robot_state::RobotStatePtr next_waypoint;
302 
303  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
304  const std::vector<std::string>& vars = group->getVariableNames();
305  const std::vector<int>& idx = group->getVariableIndexList();
306  const robot_model::RobotModel& rmodel = group->getParentModel();
307 
308  const int num_points = rob_trajectory.getWayPointCount();
309  const unsigned int num_joints = group->getVariableCount();
310  int num_updates = 0;
311  int iteration = 0;
312  bool backwards = false;
313  double q1;
314  double q2;
315  double q3;
316  double dt1;
317  double dt2;
318  double v1;
319  double v2;
320  double a;
321 
322  double acceleration_scaling_factor = 1.0;
323 
324  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
325  acceleration_scaling_factor = max_acceleration_scaling_factor;
326  else if (max_acceleration_scaling_factor == 0.0)
327  logDebug("A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
328  acceleration_scaling_factor);
329  else
330  logWarn("Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
331  max_acceleration_scaling_factor, acceleration_scaling_factor);
332 
333  do
334  {
335  num_updates = 0;
336  iteration++;
337 
338  // In this case we iterate through the joints on the outer loop.
339  // This is so that any time interval increases have a chance to get propogated through the trajectory
340  for (unsigned int j = 0; j < num_joints; ++j)
341  {
342  // Loop forwards, then backwards
343  for (int count = 0; count < 2; ++count)
344  {
345  for (int i = 0; i < num_points - 1; ++i)
346  {
347  int index = backwards ? (num_points - 1) - i : i;
348 
349  curr_waypoint = rob_trajectory.getWayPointPtr(index);
350 
351  if (index > 0)
352  prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
353 
354  if (index < num_points - 1)
355  next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
356 
357  // Get acceleration limits
358  double a_max = DEFAULT_ACCEL_MAX;
359  const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
360  if (b.acceleration_bounded_)
361  a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
362  fabs(b.min_acceleration_ * acceleration_scaling_factor));
363 
364  if (index == 0)
365  {
366  // First point
367  q1 = next_waypoint->getVariablePosition(idx[j]);
368  q2 = curr_waypoint->getVariablePosition(idx[j]);
369  q3 = next_waypoint->getVariablePosition(idx[j]);
370 
371  dt1 = dt2 = time_diff[index];
372  assert(!backwards);
373  }
374  else if (index < num_points - 1)
375  {
376  // middle points
377  q1 = prev_waypoint->getVariablePosition(idx[j]);
378  q2 = curr_waypoint->getVariablePosition(idx[j]);
379  q3 = next_waypoint->getVariablePosition(idx[j]);
380 
381  dt1 = time_diff[index - 1];
382  dt2 = time_diff[index];
383  }
384  else
385  {
386  // last point - careful, there are only numpoints-1 time intervals
387  q1 = prev_waypoint->getVariablePosition(idx[j]);
388  q2 = curr_waypoint->getVariablePosition(idx[j]);
389  q3 = prev_waypoint->getVariablePosition(idx[j]);
390 
391  dt1 = dt2 = time_diff[index - 1];
392  assert(backwards);
393  }
394 
395  if (dt1 == 0.0 || dt2 == 0.0)
396  {
397  v1 = 0.0;
398  v2 = 0.0;
399  a = 0.0;
400  }
401  else
402  {
403  bool start_velocity = false;
404  if (index == 0)
405  {
406  if (curr_waypoint->hasVelocities())
407  {
408  start_velocity = true;
409  v1 = curr_waypoint->getVariableVelocity(idx[j]);
410  }
411  }
412  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
413  v2 = (q3 - q2) / dt2;
414  a = 2.0 * (v2 - v1) / (dt1 + dt2);
415  }
416 
417  if (fabs(a) > a_max + ROUNDING_THRESHOLD)
418  {
419  if (!backwards)
420  {
421  dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
422  time_diff[index] = dt2;
423  }
424  else
425  {
426  dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
427  time_diff[index - 1] = dt1;
428  }
429  num_updates++;
430 
431  if (dt1 == 0.0 || dt2 == 0.0)
432  {
433  v1 = 0.0;
434  v2 = 0.0;
435  a = 0.0;
436  }
437  else
438  {
439  v1 = (q2 - q1) / dt1;
440  v2 = (q3 - q2) / dt2;
441  a = 2 * (v2 - v1) / (dt1 + dt2);
442  }
443  }
444  }
445  backwards = !backwards;
446  }
447  }
448  // logDebug("applyAcceleration: num_updates=%i", num_updates);
449  } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
450 }
451 
453  const double max_velocity_scaling_factor,
454  const double max_acceleration_scaling_factor) const
455 {
456  if (trajectory.empty())
457  return true;
458 
459  const robot_model::JointModelGroup* group = trajectory.getGroup();
460  if (!group)
461  {
462  logError("It looks like the planner did not set the group the plan was computed for");
463  return false;
464  }
465 
466  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
467  trajectory.unwind();
468 
469  const int num_points = trajectory.getWayPointCount();
470  std::vector<double> time_diff(num_points - 1, 0.0); // the time difference between adjacent points
471 
472  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
473  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
474 
475  updateTrajectory(trajectory, time_diff);
476  return true;
477 }
478 }
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...
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:395
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:68
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
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
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
IterativeParabolicTimeParameterization(unsigned int max_iterations=100, double max_time_change_per_it=.01)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44