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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56