iterative_spline_parameterization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Ken Anderson
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 
39 #include <vector>
40 
41 static const double VLIMIT = 1.0; // default if not specified in model
42 static const double ALIMIT = 1.0; // default if not specified in model
43 
44 namespace trajectory_processing
45 {
46 static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]);
47 static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[],
48  const double x2_i, const double x2_f);
49 static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity);
50 static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[],
51  const double max_velocity, const double min_velocity,
52  const double max_acceleration, const double min_acceleration,
53  const double tfactor);
54 static double global_adjustment_factor(const int n, double dt[], const double x[], double x1[], double x2[],
55  const double max_velocity, const double min_velocity,
56  const double max_acceleration, const double min_acceleration);
57 
58 // The path of a single joint: positions, velocities, and accelerations
60 {
61  std::vector<double> positions_; // joint's position at time[x]
62  std::vector<double> velocities_;
63  std::vector<double> accelerations_;
66  double min_velocity_;
67  double max_velocity_;
70 };
71 
72 void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
73  std::vector<double>& time_diff);
74 
75 IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points)
76 {
77 }
78 
80 
82  const double max_velocity_scaling_factor,
83  const double max_acceleration_scaling_factor) const
84 {
85  if (trajectory.empty())
86  return true;
87 
88  const robot_model::JointModelGroup* group = trajectory.getGroup();
89  if (!group)
90  {
91  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "It looks like the planner did not set "
92  "the group the plan was computed for");
93  return false;
94  }
95  const robot_model::RobotModel& rmodel = group->getParentModel();
96  const std::vector<int>& idx = group->getVariableIndexList();
97  const std::vector<std::string>& vars = group->getVariableNames();
98  double velocity_scaling_factor = 1.0;
99  double acceleration_scaling_factor = 1.0;
100  unsigned int num_points = trajectory.getWayPointCount();
101  unsigned int num_joints = group->getVariableCount();
102 
103  // Set scaling factors
104  if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
105  velocity_scaling_factor = max_velocity_scaling_factor;
106  else if (max_velocity_scaling_factor == 0.0)
107  ROS_DEBUG_NAMED("trajectory_processing.iterative_spline_parameterization",
108  "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
109  velocity_scaling_factor);
110  else
111  ROS_WARN_NAMED("trajectory_processing.iterative_spline_parameterization",
112  "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
113  max_velocity_scaling_factor, velocity_scaling_factor);
114 
115  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
116  acceleration_scaling_factor = max_acceleration_scaling_factor;
117  else if (max_acceleration_scaling_factor == 0.0)
118  ROS_DEBUG_NAMED("trajectory_processing.iterative_spline_parameterization",
119  "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
120  acceleration_scaling_factor);
121  else
122  ROS_WARN_NAMED("trajectory_processing.iterative_spline_parameterization",
123  "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
124  max_acceleration_scaling_factor, acceleration_scaling_factor);
125 
126  // No wrapped angles.
127  trajectory.unwind();
128 
129  if (add_points_)
130  {
131  // Insert 2nd and 2nd-last points
132  // (required to force acceleration to specified values at endpoints)
133  if (trajectory.getWayPointCount() >= 2)
134  {
135  robot_state::RobotState point = trajectory.getWayPoint(1);
136  robot_state::RobotStatePtr p0, p1;
137 
138  // 2nd point is 90% of p0, and 10% of p1
139  p0 = trajectory.getWayPointPtr(0);
140  p1 = trajectory.getWayPointPtr(1);
141  for (unsigned int j = 0; j < num_joints; j++)
142  {
143  point.setVariablePosition(idx[j],
144  (9 * p0->getVariablePosition(idx[j]) + 1 * p1->getVariablePosition(idx[j])) / 10);
145  }
146  trajectory.insertWayPoint(1, point, 0.0);
147  num_points++;
148 
149  // 2nd-last point is 10% of p0, and 90% of p1
150  p0 = trajectory.getWayPointPtr(num_points - 2);
151  p1 = trajectory.getWayPointPtr(num_points - 1);
152  for (unsigned int j = 0; j < num_joints; j++)
153  {
154  point.setVariablePosition(idx[j],
155  (1 * p0->getVariablePosition(idx[j]) + 9 * p1->getVariablePosition(idx[j])) / 10);
156  }
157  trajectory.insertWayPoint(num_points - 1, point, 0.0);
158  num_points++;
159  }
160  }
161 
162  // JointTrajectory indexes in [point][joint] order.
163  // We need [joint][point] order to solve efficiently,
164  // so convert form here.
165 
166  std::vector<SingleJointTrajectory> t2(num_joints);
167 
168  for (unsigned int j = 0; j < num_joints; j++)
169  {
170  // Copy positions
171  t2[j].positions_.resize(num_points, 0.0);
172  for (unsigned int i = 0; i < num_points; i++)
173  {
174  t2[j].positions_[i] = trajectory.getWayPointPtr(i)->getVariablePosition(idx[j]);
175  }
176 
177  // Initialize velocities
178  t2[j].velocities_.resize(num_points, 0.0);
179  // Copy initial/final velocities if specified
180  if (trajectory.getWayPointPtr(0)->hasVelocities())
181  t2[j].velocities_[0] = trajectory.getWayPointPtr(0)->getVariableVelocity(idx[j]);
182  if (trajectory.getWayPointPtr(num_points - 1)->hasVelocities())
183  t2[j].velocities_[num_points - 1] = trajectory.getWayPointPtr(num_points - 1)->getVariableVelocity(idx[j]);
184 
185  // Initialize accelerations
186  t2[j].accelerations_.resize(num_points, 0.0);
187  t2[j].initial_acceleration_ = 0.0;
188  t2[j].final_acceleration_ = 0.0;
189  // Copy initial/final accelerations if specified
190  if (trajectory.getWayPointPtr(0)->hasAccelerations())
191  t2[j].initial_acceleration_ = trajectory.getWayPointPtr(0)->getVariableAcceleration(idx[j]);
192  t2[j].accelerations_[0] = t2[j].initial_acceleration_;
193  if (trajectory.getWayPointPtr(num_points - 1)->hasAccelerations())
194  t2[j].final_acceleration_ = trajectory.getWayPointPtr(num_points - 1)->getVariableAcceleration(idx[j]);
195  t2[j].accelerations_[num_points - 1] = t2[j].final_acceleration_;
196 
197  // Set bounds based on model, or default limits
198  const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);
199  t2[j].max_velocity_ = VLIMIT;
200  t2[j].min_velocity_ = -VLIMIT;
201  if (bounds.velocity_bounded_)
202  {
203  t2[j].max_velocity_ = bounds.max_velocity_;
204  t2[j].min_velocity_ = bounds.min_velocity_;
205  if (t2[j].min_velocity_ == 0.0)
206  t2[j].min_velocity_ = -t2[j].max_velocity_;
207  }
208  t2[j].max_velocity_ *= velocity_scaling_factor;
209  t2[j].min_velocity_ *= velocity_scaling_factor;
210 
211  t2[j].max_acceleration_ = ALIMIT;
212  t2[j].min_acceleration_ = -ALIMIT;
213  if (bounds.acceleration_bounded_)
214  {
215  t2[j].max_acceleration_ = bounds.max_acceleration_;
216  t2[j].min_acceleration_ = bounds.min_acceleration_;
217  if (t2[j].min_acceleration_ == 0.0)
218  t2[j].min_acceleration_ = -t2[j].max_acceleration_;
219  }
220  t2[j].max_acceleration_ *= acceleration_scaling_factor;
221  t2[j].min_acceleration_ *= acceleration_scaling_factor;
222 
223  // Error out if bounds don't make sense
224  if (t2[j].max_velocity_ <= 0.0 || t2[j].max_acceleration_ <= 0.0)
225  {
226  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
227  "Joint %d max velocity %f and max acceleration %f must be greater than zero "
228  "or a solution won't be found.\n",
229  j, t2[j].max_velocity_, t2[j].max_acceleration_);
230  return false;
231  }
232  if (t2[j].min_velocity_ >= 0.0 || t2[j].min_acceleration_ >= 0.0)
233  {
234  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
235  "Joint %d min velocity %f and min acceleration %f must be less than zero "
236  "or a solution won't be found.\n",
237  j, t2[j].min_velocity_, t2[j].min_acceleration_);
238  return false;
239  }
240  }
241 
242  // Error check
243  if (num_points < 4)
244  {
245  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
246  "number of waypoints %d, needs to be greater than 3.\n", num_points);
247  return false;
248  }
249  for (unsigned int j = 0; j < num_joints; j++)
250  {
251  if (t2[j].velocities_[0] > t2[j].max_velocity_ || t2[j].velocities_[0] < t2[j].min_velocity_)
252  {
253  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "Initial velocity %f out of bounds\n",
254  t2[j].velocities_[0]);
255  return false;
256  }
257  else if (t2[j].velocities_[num_points - 1] > t2[j].max_velocity_ ||
258  t2[j].velocities_[num_points - 1] < t2[j].min_velocity_)
259  {
260  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "Final velocity %f out of bounds\n",
261  t2[j].velocities_[num_points - 1]);
262  return false;
263  }
264  else if (t2[j].accelerations_[0] > t2[j].max_acceleration_ || t2[j].accelerations_[0] < t2[j].min_acceleration_)
265  {
266  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
267  "Initial acceleration %f out of bounds\n", t2[j].accelerations_[0]);
268  return false;
269  }
270  else if (t2[j].accelerations_[num_points - 1] > t2[j].max_acceleration_ ||
271  t2[j].accelerations_[num_points - 1] < t2[j].min_acceleration_)
272  {
273  ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
274  "Final acceleration %f out of bounds\n", t2[j].accelerations_[num_points - 1]);
275  return false;
276  }
277  }
278 
279  // Initialize times
280  // start with valid velocities, then expand intervals
281  // epsilon to prevent divide-by-zero
282  std::vector<double> time_diff(trajectory.getWayPointCount() - 1, std::numeric_limits<double>::epsilon());
283  for (unsigned int j = 0; j < num_joints; j++)
284  init_times(num_points, &time_diff[0], &t2[j].positions_[0], t2[j].max_velocity_, t2[j].min_velocity_);
285 
286  // Stretch intervals until close to the bounds
287  while (1)
288  {
289  int loop = 0;
290 
291  // Calculate the interval stretches due to acceleration
292  std::vector<double> time_factor(num_points - 1, 1.00);
293  for (unsigned j = 0; j < num_joints; j++)
294  {
295  // Move points to satisfy initial/final acceleration
296  if (add_points_)
297  {
298  adjust_two_positions(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0],
299  &t2[j].accelerations_[0], t2[j].initial_acceleration_, t2[j].final_acceleration_);
300  }
301 
302  fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]);
303  for (unsigned i = 0; i < num_points; i++)
304  {
305  const double acc = t2[j].accelerations_[i];
306  double atfactor = 1.0;
307  if (acc > t2[j].max_acceleration_)
308  atfactor = sqrt(acc / t2[j].max_acceleration_);
309  if (acc < t2[j].min_acceleration_)
310  atfactor = sqrt(acc / t2[j].min_acceleration_);
311  if (atfactor > 1.01) // within 1%
312  loop = 1;
313  atfactor = (atfactor - 1.0) / 16.0 + 1.0; // 1/16th
314  if (i > 0)
315  time_factor[i - 1] = std::max(time_factor[i - 1], atfactor);
316  if (i < num_points - 1)
317  time_factor[i] = std::max(time_factor[i], atfactor);
318  }
319  }
320 
321  if (loop == 0)
322  break; // finished
323 
324  // Stretch
325  for (unsigned i = 0; i < num_points - 1; i++)
326  time_diff[i] *= time_factor[i];
327  }
328 
329  // Final adjustment forces the trajectory within bounds
330  globalAdjustment(t2, num_joints, num_points, time_diff);
331 
332  // Convert back to JointTrajectory form
333  for (unsigned int i = 1; i < num_points; i++)
334  trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]);
335  for (unsigned int i = 0; i < num_points; i++)
336  {
337  for (unsigned int j = 0; j < num_joints; j++)
338  {
339  trajectory.getWayPointPtr(i)->setVariableVelocity(idx[j], t2[j].velocities_[i]);
340  trajectory.getWayPointPtr(i)->setVariableAcceleration(idx[j], t2[j].accelerations_[i]);
341  }
342 
343  // Only update position of additionally inserted points (at second and next-to-last position)
344  if (add_points_ && (i == 1 || i == num_points - 2))
345  {
346  for (unsigned int j = 0; j < num_joints; j++)
347  trajectory.getWayPointPtr(i)->setVariablePosition(idx[j], t2[j].positions_[i]);
348  trajectory.getWayPointPtr(i)->update();
349  }
350  }
351 
352  return true;
353 }
354 
356 
357 /*
358  Fit a 'clamped' cubic spline over a series of points.
359  A cubic spline ensures continuous function across positions,
360  1st derivative (velocities), and 2nd derivative (accelerations).
361  'Clamped' means the first derivative at the endpoints is specified.
362 
363  Fitting a cubic spline involves solving a series of linear equations.
364  The general form for each segment is:
365  (tj-t_(j-1))*x"_(j-1) + 2*(t_(j+1)-t_(j-1))*x"j + (t_(j+1)-tj)*x"_j+1) =
366  (x_(j+1)-xj)/(t_(j+1)-tj) - (xj-x_(j-1))/(tj-t_(j-1))
367  And the first and last segment equations are clamped to specified values: x1_i and x1_f.
368 
369  Represented in matrix form:
370  [ 2*(t1-t0) (t1-t0) 0 ][x0"] [(x1-x0)/(t1-t0) - t1_i ]
371  [ t1-t0 2*(t2-t0) t2-t1 ][x1"] [(x2-x1)/(t2-t1) - (x1-x0)/(t1-t0)]
372  [ t2-t1 2*(t3-t1) t3-t2 ][x2"] = 6 * [(x3-x2)/(t3/t2) - (x2-x1)/(t2-t1)]
373  [ ... ... ... ][...] [... ]
374  [ 0 tN-t_(N-1) 2*(tN-t_(N-1)) ][xN"] [t1_f - (xN-x_(N-1))/(tN-t_(N-1)) ]
375 
376  This matrix is tridiagonal, which can be solved solved in O(N) time
377  using the tridiagonal algorithm.
378  There is a forward propogation pass followed by a backsubstitution pass.
379 
380  n is the number of points
381  dt contains the time difference between each point (size=n-1)
382  x contains the positions (size=n)
383  x1 contains the 1st derivative (velocities) (size=n)
384  x1[0] and x1[n-1] MUST be specified.
385  x2 contains the 2nd derivative (accelerations) (size=n)
386  x1 and x2 are filled in by the algorithm.
387 */
388 
389 static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[])
390 {
391  int i;
392  const double x1_i = x1[0], x1_f = x1[n - 1];
393 
394  // Tridiagonal alg - forward sweep
395  // x1 and x2 used to store the temporary coefficients c and d
396  // (will get overwritten during backsubstitution)
397  double *c = x1, *d = x2;
398  c[0] = 0.5;
399  d[0] = 3.0 * ((x[1] - x[0]) / dt[0] - x1_i) / dt[0];
400  for (i = 1; i <= n - 2; i++)
401  {
402  const double dt2 = dt[i - 1] + dt[i];
403  const double a = dt[i - 1] / dt2;
404  const double denom = 2.0 - a * c[i - 1];
405  c[i] = (1.0 - a) / denom;
406  d[i] = 6.0 * ((x[i + 1] - x[i]) / dt[i] - (x[i] - x[i - 1]) / dt[i - 1]) / dt2;
407  d[i] = (d[i] - a * d[i - 1]) / denom;
408  }
409  const double denom = dt[n - 2] * (2.0 - c[n - 2]);
410  d[n - 1] = 6.0 * (x1_f - (x[n - 1] - x[n - 2]) / dt[n - 2]);
411  d[n - 1] = (d[n - 1] - dt[n - 2] * d[n - 2]) / denom;
412 
413  // Tridiagonal alg - backsubstitution sweep
414  // 2nd derivative
415  x2[n - 1] = d[n - 1];
416  for (i = n - 2; i >= 0; i--)
417  x2[i] = d[i] - c[i] * x2[i + 1];
418 
419  // 1st derivative
420  x1[0] = x1_i;
421  for (i = 1; i < n - 1; i++)
422  x1[i] = (x[i + 1] - x[i]) / dt[i] - (2 * x2[i] + x2[i + 1]) * dt[i] / 6.0;
423  x1[n - 1] = x1_f;
424 }
425 
426 /*
427  Modify the value of x[1] and x[N-2]
428  so that 2nd derivative starts and ends at specified value.
429  This involves fitting the spline twice,
430  then solving for the specified value.
431 
432  x2_i and x2_f are the (initial and final) 2nd derivative at 0 and N-1
433 */
434 
435 static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[],
436  const double x2_i, const double x2_f)
437 {
438  x[1] = x[0];
439  x[n - 2] = x[n - 3];
440  fit_cubic_spline(n, dt, x, x1, x2);
441  double a0 = x2[0];
442  double b0 = x2[n - 1];
443 
444  x[1] = x[2];
445  x[n - 2] = x[n - 1];
446  fit_cubic_spline(n, dt, x, x1, x2);
447  double a2 = x2[0];
448  double b2 = x2[n - 1];
449 
450  // we can solve this with linear equation (use two-point form)
451  if (a2 != a0)
452  x[1] = x[0] + ((x[2] - x[0]) / (a2 - a0)) * (x2_i - a0);
453  if (b2 != b0)
454  x[n - 2] = x[n - 3] + ((x[n - 1] - x[n - 3]) / (b2 - b0)) * (x2_f - b0);
455 }
456 
457 /*
458  Find time required to go max velocity on each segment.
459  Increase a segment's time interval if the current time isn't long enough.
460 */
461 
462 static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity)
463 {
464  int i;
465  for (i = 0; i < n - 1; i++)
466  {
467  double time;
468  double dx = x[i + 1] - x[i];
469  if (dx >= 0.0)
470  time = (dx / max_velocity);
471  else
472  time = (dx / min_velocity);
473  time += std::numeric_limits<double>::epsilon(); // prevent divide-by-zero
474 
475  if (dt[i] < time)
476  dt[i] = time;
477  }
478 }
479 
480 /*
481  Fit a spline, then check each interval to see if bounds are met.
482  If all bounds met (no time adjustments made), return 0.
483  If bounds not met (time adjustments made), slightly increase the
484  surrounding time intervals and return 1.
485 
486  n is the number of points
487  dt contains the time difference between each point (size=n-1)
488  x contains the positions (size=n)
489  x1 contains the 1st derivative (velocities) (size=n)
490  x1[0] and x1[n-1] MUST be specified.
491  x2 contains the 2nd derivative (accelerations) (size=n)
492  max_velocity is the max velocity for this joint.
493  min_velocity is the min velocity for this joint.
494  max_acceleration is the max acceleration for this joint.
495  min_acceleration is the min acceleration for this joint.
496  tfactor is the time adjustment (multiplication) factor.
497  x1 and x2 are filled in by the algorithm.
498 */
499 
500 static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[],
501  const double max_velocity, const double min_velocity,
502  const double max_acceleration, const double min_acceleration,
503  const double tfactor)
504 {
505  int i, ret = 0;
506 
507  fit_cubic_spline(n, dt, x, x1, x2);
508 
509  // Instantaneous velocity is calculated at each point
510  for (i = 0; i < n - 1; i++)
511  {
512  const double vel = x1[i];
513  const double vel2 = x1[i + 1];
514  if (vel > max_velocity || vel < min_velocity || vel2 > max_velocity || vel2 < min_velocity)
515  {
516  dt[i] *= tfactor;
517  ret = 1;
518  }
519  }
520  // Instantaneous acceleration is calculated at each point
521  if (ret == 0)
522  {
523  for (i = 0; i < n - 1; i++)
524  {
525  const double acc = x2[i];
526  const double acc2 = x2[i + 1];
527  if (acc > max_acceleration || acc < min_acceleration || acc2 > max_acceleration || acc2 < min_acceleration)
528  {
529  dt[i] *= tfactor;
530  ret = 1;
531  }
532  }
533  }
534 
535  return ret;
536 }
537 
538 // return global expansion multiplicative factor required
539 // to force within bounds.
540 // Assumes that the spline is already fit
541 // (fit_cubic_spline must have been called before this).
542 static double global_adjustment_factor(const int n, double dt[], const double x[], double x1[], double x2[],
543  const double max_velocity, const double min_velocity,
544  const double max_acceleration, const double min_acceleration)
545 {
546  int i;
547  double tfactor2 = 1.00;
548 
549  // fit_cubic_spline(n, dt, x, x1, x2);
550 
551  for (i = 0; i < n; i++)
552  {
553  double tfactor;
554  tfactor = x1[i] / max_velocity;
555  if (tfactor2 < tfactor)
556  tfactor2 = tfactor;
557  tfactor = x1[i] / min_velocity;
558  if (tfactor2 < tfactor)
559  tfactor2 = tfactor;
560 
561  if (x2[i] >= 0)
562  {
563  tfactor = sqrt(fabs(x2[i] / max_acceleration));
564  if (tfactor2 < tfactor)
565  tfactor2 = tfactor;
566  }
567  else
568  {
569  tfactor = sqrt(fabs(x2[i] / min_acceleration));
570  if (tfactor2 < tfactor)
571  tfactor2 = tfactor;
572  }
573  }
574 
575  return tfactor2;
576 }
577 
578 // Expands the entire trajectory to fit exactly within bounds
579 void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
580  std::vector<double>& time_diff)
581 {
582  double gtfactor = 1.0;
583  for (int j = 0; j < num_joints; j++)
584  {
585  double tfactor;
586  tfactor = global_adjustment_factor(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0],
587  &t2[j].accelerations_[0], t2[j].max_velocity_, t2[j].min_velocity_,
588  t2[j].max_acceleration_, t2[j].min_acceleration_);
589  if (tfactor > gtfactor)
590  gtfactor = tfactor;
591  }
592 
593  // printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0));
594  for (int i = 0; i < num_points - 1; i++)
595  time_diff[i] *= gtfactor;
596 
597  for (int j = 0; j < num_joints; j++)
598  {
599  fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]);
600  }
601 }
602 } // namespace trajectory_processing
d
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
#define ROS_WARN_NAMED(name,...)
static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], const double x2_i, const double x2_f)
void globalAdjustment(std::vector< SingleJointTrajectory > &t2, int num_joints, const int num_points, std::vector< double > &time_diff)
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...
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:67
static double global_adjustment_factor(const int n, double dt[], const double x[], double x1[], double x2[], const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration)
#define ROS_DEBUG_NAMED(name,...)
static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[], const double max_velocity, const double min_velocity, const double max_acceleration, const double min_acceleration, const double tfactor)
static const double ALIMIT
const robot_model::JointModelGroup * getGroup() const
Maintain a sequence of waypoints and the time durations between these waypoints.
void insertWayPoint(std::size_t index, const robot_state::RobotState &state, double dt)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static const double VLIMIT
void setWayPointDurationFromPrevious(std::size_t index, double value)
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[])
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
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,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known...
Definition: robot_state.h:221
double x
const robot_state::RobotState & getWayPoint(std::size_t index) 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:407
static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Oct 23 2020 03:57:08