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