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