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 namespace
54 {
55 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, std::size_t i)
56 {
57  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " time [%zu]= %f", i,
58  point.time_from_start.toSec());
59  if (point.positions.size() >= 7)
60  {
61  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " pos_ [%zu]= %f %f %f %f %f %f %f", i,
62  point.positions[0], point.positions[1], point.positions[2], point.positions[3], point.positions[4],
63  point.positions[5], point.positions[6]);
64  }
65  if (point.velocities.size() >= 7)
66  {
67  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " vel_ [%zu]= %f %f %f %f %f %f %f", i,
68  point.velocities[0], point.velocities[1], point.velocities[2], point.velocities[3],
69  point.velocities[4], point.velocities[5], point.velocities[6]);
70  }
71  if (point.accelerations.size() >= 7)
72  {
73  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", " acc_ [%zu]= %f %f %f %f %f %f %f", i,
74  point.accelerations[0], point.accelerations[1], point.accelerations[2], point.accelerations[3],
75  point.accelerations[4], point.accelerations[5], point.accelerations[6]);
76  }
77 }
78 
79 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
80 {
81  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "jointNames= %s %s %s %s %s %s %s",
82  limits[0].joint_name.c_str(), limits[1].joint_name.c_str(), limits[2].joint_name.c_str(),
83  limits[3].joint_name.c_str(), limits[4].joint_name.c_str(), limits[5].joint_name.c_str(),
84  limits[6].joint_name.c_str());
85  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxVelocities= %f %f %f %f %f %f %f",
86  limits[0].max_velocity, limits[1].max_velocity, limits[2].max_velocity, limits[3].max_velocity,
87  limits[4].max_velocity, limits[5].max_velocity, limits[6].max_velocity);
88  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization", "maxAccelerations= %f %f %f %f %f %f %f",
89  limits[0].max_acceleration, limits[1].max_acceleration, limits[2].max_acceleration,
90  limits[3].max_acceleration, limits[4].max_acceleration, limits[5].max_acceleration,
91  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 } // namespace
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  ROS_DEBUG_NAMED("trajectory_processing.iterative_time_parameterization",
115  "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
116  velocity_scaling_factor);
117  else
118  ROS_WARN_NAMED("trajectory_processing.iterative_time_parameterization",
119  "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
120  max_velocity_scaling_factor, velocity_scaling_factor);
121 
122  for (int i = 0; i < num_points - 1; ++i)
123  {
124  const robot_state::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
125  const robot_state::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
126 
127  for (std::size_t j = 0; j < vars.size(); ++j)
128  {
129  double v_max = DEFAULT_VEL_MAX;
130  const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
131  if (b.velocity_bounded_)
132  v_max =
133  std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor));
134  const double dq1 = curr_waypoint->getVariablePosition(idx[j]);
135  const double dq2 = next_waypoint->getVariablePosition(idx[j]);
136  const double t_min = std::abs(dq2 - dq1) / v_max;
137  if (t_min > time_diff[i])
138  time_diff[i] = t_min;
139  }
140  }
141 }
142 
143 // Iteratively expand dt1 interval by a constant factor until within acceleration constraint
144 // In the future we may want to solve to quadratic equation to get the exact timing interval.
145 // To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h
146 double IterativeParabolicTimeParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2,
147  const double a_max) const
148 {
149  const double mult_factor = 1.01;
150  double v1 = (dq1) / dt1;
151  double v2 = (dq2) / dt2;
152  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
153 
154  while (std::abs(a) > a_max)
155  {
156  v1 = (dq1) / dt1;
157  v2 = (dq2) / dt2;
158  a = 2.0 * (v2 - v1) / (dt1 + dt2);
159  dt1 *= mult_factor;
160  }
161 
162  return dt1;
163 }
164 
165 double IterativeParabolicTimeParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2,
166  const double a_max) const
167 {
168  const double mult_factor = 1.01;
169  double v1 = (dq1) / dt1;
170  double v2 = (dq2) / dt2;
171  double a = 2.0 * (v2 - v1) / (dt1 + dt2);
172 
173  while (std::abs(a) > a_max)
174  {
175  v1 = (dq1) / dt1;
176  v2 = (dq2) / dt2;
177  a = 2.0 * (v2 - v1) / (dt1 + dt2);
178  dt2 *= mult_factor;
179  }
180 
181  return dt2;
182 }
183 
184 namespace
185 {
186 // Takes the time differences, and updates the timestamps, velocities and accelerations
187 // in the trajectory.
188 void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector<double>& time_diff)
189 {
190  // Error check
191  if (time_diff.empty())
192  return;
193 
194  double time_sum = 0.0;
195 
196  robot_state::RobotStatePtr prev_waypoint;
197  robot_state::RobotStatePtr curr_waypoint;
198  robot_state::RobotStatePtr next_waypoint;
199 
200  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
201  const std::vector<std::string>& vars = group->getVariableNames();
202  const std::vector<int>& idx = group->getVariableIndexList();
203 
204  int num_points = rob_trajectory.getWayPointCount();
205 
206  rob_trajectory.setWayPointDurationFromPrevious(0, time_sum);
207 
208  // Times
209  for (int i = 1; i < num_points; ++i)
210  // Update the time between the waypoints in the robot_trajectory.
211  rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
212 
213  // Return if there is only one point in the trajectory!
214  if (num_points <= 1)
215  return;
216 
217  // Accelerations
218  for (int i = 0; i < num_points; ++i)
219  {
220  curr_waypoint = rob_trajectory.getWayPointPtr(i);
221 
222  if (i > 0)
223  prev_waypoint = rob_trajectory.getWayPointPtr(i - 1);
224 
225  if (i < num_points - 1)
226  next_waypoint = rob_trajectory.getWayPointPtr(i + 1);
227 
228  for (std::size_t j = 0; j < vars.size(); ++j)
229  {
230  double q1;
231  double q2;
232  double q3;
233  double dt1;
234  double dt2;
235 
236  if (i == 0)
237  {
238  // First point
239  q1 = next_waypoint->getVariablePosition(idx[j]);
240  q2 = curr_waypoint->getVariablePosition(idx[j]);
241  q3 = q1;
242 
243  dt1 = dt2 = time_diff[i];
244  }
245  else if (i < num_points - 1)
246  {
247  // middle points
248  q1 = prev_waypoint->getVariablePosition(idx[j]);
249  q2 = curr_waypoint->getVariablePosition(idx[j]);
250  q3 = next_waypoint->getVariablePosition(idx[j]);
251 
252  dt1 = time_diff[i - 1];
253  dt2 = time_diff[i];
254  }
255  else
256  {
257  // last point
258  q1 = prev_waypoint->getVariablePosition(idx[j]);
259  q2 = curr_waypoint->getVariablePosition(idx[j]);
260  q3 = q1;
261 
262  dt1 = dt2 = time_diff[i - 1];
263  }
264 
265  double v1, v2, a;
266 
267  bool start_velocity = false;
268  if (dt1 == 0.0 || dt2 == 0.0)
269  {
270  v1 = 0.0;
271  v2 = 0.0;
272  a = 0.0;
273  }
274  else
275  {
276  if (i == 0)
277  {
278  if (curr_waypoint->hasVelocities())
279  {
280  start_velocity = true;
281  v1 = curr_waypoint->getVariableVelocity(idx[j]);
282  }
283  }
284  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
285  // v2 = (q3-q2)/dt2;
286  v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point
287  a = 2.0 * (v2 - v1) / (dt1 + dt2);
288  }
289 
290  curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0);
291  curr_waypoint->setVariableAcceleration(idx[j], a);
292  }
293  }
294 }
295 } // namespace
296 
297 // Applies Acceleration constraints
299  robot_trajectory::RobotTrajectory& rob_trajectory, std::vector<double>& time_diff,
300  const double max_acceleration_scaling_factor) const
301 {
302  robot_state::RobotStatePtr prev_waypoint;
303  robot_state::RobotStatePtr curr_waypoint;
304  robot_state::RobotStatePtr next_waypoint;
305 
306  const robot_model::JointModelGroup* group = rob_trajectory.getGroup();
307  const std::vector<std::string>& vars = group->getVariableNames();
308  const std::vector<int>& idx = group->getVariableIndexList();
309  const robot_model::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 robot_model::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 
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 robot_model::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  updateTrajectory(trajectory, time_diff);
483  return true;
484 }
485 } // 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,...)
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
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:76
#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
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:432


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04