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
99 void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
100  std::vector<double>& time_diff,
101  const double max_velocity_scaling_factor) const
102 {
103  const moveit::core::JointModelGroup* group = rob_trajectory.getGroup();
104  const std::vector<std::string>& vars = group->getVariableNames();
105  const std::vector<int>& idx = group->getVariableIndexList();
106  const moveit::core::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 moveit::core::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i);
125  const moveit::core::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 moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
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  moveit::core::RobotStatePtr prev_waypoint;
197  moveit::core::RobotStatePtr curr_waypoint;
198  moveit::core::RobotStatePtr next_waypoint;
199 
200  const moveit::core::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
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  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:113
robot_trajectory::RobotTrajectory::getGroup
const moveit::core::JointModelGroup * getGroup() const
Definition: robot_trajectory.h:104
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
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:132
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:474
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:83
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
robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious
void setWayPointDurationFromPrevious(std::size_t index, double value)
Definition: robot_trajectory.h:169
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:176
trajectory_processing
Definition: iterative_spline_parameterization.h:42
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:133
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
void unwind()
Definition: robot_trajectory.cpp:166
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 Sat Dec 12 2020 03:25:44