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  CONSOLE_BRIDGE_logDebug(" time [%i]= %f", i, point.time_from_start.toSec());
63  if (point.positions.size() >= 7)
64  {
65  CONSOLE_BRIDGE_logDebug(" pos_ [%i]= %f %f %f %f %f %f %f", i, point.positions[0], point.positions[1],
66  point.positions[2], point.positions[3], point.positions[4], point.positions[5],
67  point.positions[6]);
68  }
69  if (point.velocities.size() >= 7)
70  {
71  CONSOLE_BRIDGE_logDebug(" vel_ [%i]= %f %f %f %f %f %f %f", i, point.velocities[0], point.velocities[1],
72  point.velocities[2], point.velocities[3], point.velocities[4], point.velocities[5],
73  point.velocities[6]);
74  }
75  if (point.accelerations.size() >= 7)
76  {
77  CONSOLE_BRIDGE_logDebug(" acc_ [%i]= %f %f %f %f %f %f %f", i, point.accelerations[0], point.accelerations[1],
78  point.accelerations[2], point.accelerations[3], point.accelerations[4],
79  point.accelerations[5], point.accelerations[6]);
80  }
81 }
82 
83 void printStats(const trajectory_msgs::JointTrajectory& trajectory, const std::vector<moveit_msgs::JointLimits>& limits)
84 {
85  CONSOLE_BRIDGE_logDebug("jointNames= %s %s %s %s %s %s %s", limits[0].joint_name.c_str(),
86  limits[1].joint_name.c_str(), limits[2].joint_name.c_str(), limits[3].joint_name.c_str(),
87  limits[4].joint_name.c_str(), limits[5].joint_name.c_str(), limits[6].joint_name.c_str());
88  CONSOLE_BRIDGE_logDebug("maxVelocities= %f %f %f %f %f %f %f", limits[0].max_velocity, limits[1].max_velocity,
89  limits[2].max_velocity, limits[3].max_velocity, limits[4].max_velocity,
90  limits[5].max_velocity, limits[6].max_velocity);
91  CONSOLE_BRIDGE_logDebug("maxAccelerations= %f %f %f %f %f %f %f", limits[0].max_acceleration,
92  limits[1].max_acceleration, limits[2].max_acceleration, limits[3].max_acceleration,
93  limits[4].max_acceleration, limits[5].max_acceleration, 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  CONSOLE_BRIDGE_logDebug("A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
117  velocity_scaling_factor);
118  else
119  CONSOLE_BRIDGE_logWarn("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 }
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();
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  CONSOLE_BRIDGE_logDebug("A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
331  acceleration_scaling_factor);
332  else
333  CONSOLE_BRIDGE_logWarn("Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
334  max_acceleration_scaling_factor, acceleration_scaling_factor);
335 
336  do
337  {
338  num_updates = 0;
339  iteration++;
340 
341  // In this case we iterate through the joints on the outer loop.
342  // This is so that any time interval increases have a chance to get propogated through the trajectory
343  for (unsigned int j = 0; j < num_joints; ++j)
344  {
345  // Loop forwards, then backwards
346  for (int count = 0; count < 2; ++count)
347  {
348  for (int i = 0; i < num_points - 1; ++i)
349  {
350  int index = backwards ? (num_points - 1) - i : i;
351 
352  curr_waypoint = rob_trajectory.getWayPointPtr(index);
353 
354  if (index > 0)
355  prev_waypoint = rob_trajectory.getWayPointPtr(index - 1);
356 
357  if (index < num_points - 1)
358  next_waypoint = rob_trajectory.getWayPointPtr(index + 1);
359 
360  // Get acceleration limits
361  double a_max = DEFAULT_ACCEL_MAX;
362  const robot_model::VariableBounds& b = rmodel.getVariableBounds(vars[j]);
363  if (b.acceleration_bounded_)
364  a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor),
365  fabs(b.min_acceleration_ * acceleration_scaling_factor));
366 
367  if (index == 0)
368  {
369  // First point
370  q1 = next_waypoint->getVariablePosition(idx[j]);
371  q2 = curr_waypoint->getVariablePosition(idx[j]);
372  q3 = next_waypoint->getVariablePosition(idx[j]);
373 
374  dt1 = dt2 = time_diff[index];
375  assert(!backwards);
376  }
377  else if (index < num_points - 1)
378  {
379  // middle points
380  q1 = prev_waypoint->getVariablePosition(idx[j]);
381  q2 = curr_waypoint->getVariablePosition(idx[j]);
382  q3 = next_waypoint->getVariablePosition(idx[j]);
383 
384  dt1 = time_diff[index - 1];
385  dt2 = time_diff[index];
386  }
387  else
388  {
389  // last point - careful, there are only numpoints-1 time intervals
390  q1 = prev_waypoint->getVariablePosition(idx[j]);
391  q2 = curr_waypoint->getVariablePosition(idx[j]);
392  q3 = prev_waypoint->getVariablePosition(idx[j]);
393 
394  dt1 = dt2 = time_diff[index - 1];
395  assert(backwards);
396  }
397 
398  if (dt1 == 0.0 || dt2 == 0.0)
399  {
400  v1 = 0.0;
401  v2 = 0.0;
402  a = 0.0;
403  }
404  else
405  {
406  bool start_velocity = false;
407  if (index == 0)
408  {
409  if (curr_waypoint->hasVelocities())
410  {
411  start_velocity = true;
412  v1 = curr_waypoint->getVariableVelocity(idx[j]);
413  }
414  }
415  v1 = start_velocity ? v1 : (q2 - q1) / dt1;
416  v2 = (q3 - q2) / dt2;
417  a = 2.0 * (v2 - v1) / (dt1 + dt2);
418  }
419 
420  if (fabs(a) > a_max + ROUNDING_THRESHOLD)
421  {
422  if (!backwards)
423  {
424  dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max));
425  time_diff[index] = dt2;
426  }
427  else
428  {
429  dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max));
430  time_diff[index - 1] = dt1;
431  }
432  num_updates++;
433 
434  if (dt1 == 0.0 || dt2 == 0.0)
435  {
436  v1 = 0.0;
437  v2 = 0.0;
438  a = 0.0;
439  }
440  else
441  {
442  v1 = (q2 - q1) / dt1;
443  v2 = (q3 - q2) / dt2;
444  a = 2 * (v2 - v1) / (dt1 + dt2);
445  }
446  }
447  }
448  backwards = !backwards;
449  }
450  }
451  // CONSOLE_BRIDGE_logDebug("applyAcceleration: num_updates=%i", num_updates);
452  } while (num_updates > 0 && iteration < static_cast<int>(max_iterations_));
453 }
454 
456  const double max_velocity_scaling_factor,
457  const double max_acceleration_scaling_factor) const
458 {
459  if (trajectory.empty())
460  return true;
461 
462  const robot_model::JointModelGroup* group = trajectory.getGroup();
463  if (!group)
464  {
465  CONSOLE_BRIDGE_logError("It looks like the planner did not set the group the plan was computed for");
466  return false;
467  }
468 
469  // this lib does not actually work properly when angles wrap around, so we need to unwind the path first
470  trajectory.unwind();
471 
472  const int num_points = trajectory.getWayPointCount();
473  std::vector<double> time_diff(num_points - 1, 0.0); // the time difference between adjacent points
474 
475  applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor);
476  applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor);
477 
478  updateTrajectory(trajectory, time_diff);
479  return true;
480 }
481 }
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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:408
#define CONSOLE_BRIDGE_logDebug(fmt,...)
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
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)
#define CONSOLE_BRIDGE_logError(fmt,...)
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::RobotModelConstPtr rmodel


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Apr 21 2018 02:54:51