time_optimal_trajectory_generation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Georgia Tech Research Corporation
3  * All rights reserved.
4  *
5  * Author: Tobias Kunz <tobias@gatech.edu>
6  * Date: 05/2012
7  *
8  * Humanoid Robotics Lab Georgia Institute of Technology
9  * Director: Mike Stilman http://www.golems.org
10  *
11  * Algorithm details and publications:
12  * http://www.golems.org/node/1570
13  *
14  * This file is provided under the following "BSD-style" License:
15  * Redistribution and use in source and binary forms, with or
16  * without modification, are permitted provided that the following
17  * conditions are met:
18  * * Redistributions of source code must retain the above copyright
19  * notice, this list of conditions and the following disclaimer.
20  * * Redistributions in binary form must reproduce the above
21  * copyright notice, this list of conditions and the following
22  * disclaimer in the documentation and/or other materials provided
23  * with the distribution.
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
25  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
26  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
27  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
29  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
30  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
31  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
32  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
33  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  */
38 
39 #include <limits>
40 #include <Eigen/Geometry>
41 #include <algorithm>
42 #include <angles/angles.h>
43 #include <cmath>
45 #include <ros/console.h>
46 #include <vector>
47 
48 #include <iostream>
49 
50 namespace trajectory_processing
51 {
52 const std::string LOGNAME = "trajectory_processing.time_optimal_trajectory_generation";
53 constexpr double EPS = 0.000001;
55 {
56 public:
57  LinearPathSegment(const Eigen::VectorXd& start, const Eigen::VectorXd& end)
58  : PathSegment((end - start).norm()), end_(end), start_(start)
59  {
60  }
61 
62  Eigen::VectorXd getConfig(double s) const override
63  {
64  s /= length_;
65  s = std::max(0.0, std::min(1.0, s));
66  return (1.0 - s) * start_ + s * end_;
67  }
68 
69  Eigen::VectorXd getTangent(double /* s */) const override
70  {
71  return (end_ - start_) / length_;
72  }
73 
74  Eigen::VectorXd getCurvature(double /* s */) const override
75  {
76  return Eigen::VectorXd::Zero(start_.size());
77  }
78 
79  std::list<double> getSwitchingPoints() const override
80  {
81  return std::list<double>();
82  }
83 
84  LinearPathSegment* clone() const override
85  {
86  return new LinearPathSegment(*this);
87  }
88 
89 private:
90  Eigen::VectorXd end_;
91  Eigen::VectorXd start_;
92 };
93 
95 {
96 public:
97  CircularPathSegment(const Eigen::VectorXd& start, const Eigen::VectorXd& intersection, const Eigen::VectorXd& end,
98  double max_deviation)
99  {
100  if ((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001)
101  {
102  length_ = 0.0;
103  radius = 1.0;
104  center = intersection;
105  x = Eigen::VectorXd::Zero(start.size());
106  y = Eigen::VectorXd::Zero(start.size());
107  return;
108  }
109 
110  const Eigen::VectorXd start_direction = (intersection - start).normalized();
111  const Eigen::VectorXd end_direction = (end - intersection).normalized();
112 
113  if ((start_direction - end_direction).norm() < 0.000001)
114  {
115  length_ = 0.0;
116  radius = 1.0;
117  center = intersection;
118  x = Eigen::VectorXd::Zero(start.size());
119  y = Eigen::VectorXd::Zero(start.size());
120  return;
121  }
122 
123  // directions must be different at this point so angle is always non-zero
124  const double angle = acos(std::max(-1.0, start_direction.dot(end_direction)));
125  const double start_distance = (start - intersection).norm();
126  const double end_distance = (end - intersection).norm();
127 
128  // enforce max deviation
129  double distance = std::min(start_distance, end_distance);
130  distance = std::min(distance, max_deviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle)));
131 
132  radius = distance / tan(0.5 * angle);
133  length_ = angle * radius;
134 
135  center = intersection + (end_direction - start_direction).normalized() * radius / cos(0.5 * angle);
136  x = (intersection - distance * start_direction - center).normalized();
137  y = start_direction;
138  }
139 
140  Eigen::VectorXd getConfig(double s) const override
141  {
142  const double angle = s / radius;
143  return center + radius * (x * cos(angle) + y * sin(angle));
144  }
145 
146  Eigen::VectorXd getTangent(double s) const override
147  {
148  const double angle = s / radius;
149  return -x * sin(angle) + y * cos(angle);
150  }
151 
152  Eigen::VectorXd getCurvature(double s) const override
153  {
154  const double angle = s / radius;
155  return -1.0 / radius * (x * cos(angle) + y * sin(angle));
156  }
157 
158  std::list<double> getSwitchingPoints() const override
159  {
160  std::list<double> switching_points;
161  const double dim = x.size();
162  for (unsigned int i = 0; i < dim; ++i)
163  {
164  double switching_angle = atan2(y[i], x[i]);
165  if (switching_angle < 0.0)
166  {
167  switching_angle += M_PI;
168  }
169  const double switching_point = switching_angle * radius;
170  if (switching_point < length_)
171  {
172  switching_points.push_back(switching_point);
173  }
174  }
175  switching_points.sort();
176  return switching_points;
177  }
178 
179  CircularPathSegment* clone() const override
180  {
181  return new CircularPathSegment(*this);
182  }
183 
184 private:
185  double radius;
186  Eigen::VectorXd center;
187  Eigen::VectorXd x;
188  Eigen::VectorXd y;
189 };
190 
191 Path::Path(const std::list<Eigen::VectorXd>& path, double max_deviation) : length_(0.0)
192 {
193  if (path.size() < 2)
194  return;
195  std::list<Eigen::VectorXd>::const_iterator path_iterator1 = path.begin();
196  std::list<Eigen::VectorXd>::const_iterator path_iterator2 = path_iterator1;
197  ++path_iterator2;
198  std::list<Eigen::VectorXd>::const_iterator path_iterator3;
199  Eigen::VectorXd start_config = *path_iterator1;
200  while (path_iterator2 != path.end())
201  {
202  path_iterator3 = path_iterator2;
203  ++path_iterator3;
204  if (max_deviation > 0.0 && path_iterator3 != path.end())
205  {
206  CircularPathSegment* blend_segment =
207  new CircularPathSegment(0.5 * (*path_iterator1 + *path_iterator2), *path_iterator2,
208  0.5 * (*path_iterator2 + *path_iterator3), max_deviation);
209  Eigen::VectorXd end_config = blend_segment->getConfig(0.0);
210  if ((end_config - start_config).norm() > 0.000001)
211  {
212  path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, end_config));
213  }
214  path_segments_.emplace_back(blend_segment);
215 
216  start_config = blend_segment->getConfig(blend_segment->getLength());
217  }
218  else
219  {
220  path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *path_iterator2));
221  start_config = *path_iterator2;
222  }
223  path_iterator1 = path_iterator2;
224  ++path_iterator2;
225  }
226 
227  // Create list of switching point candidates, calculate total path length and
228  // absolute positions of path segments
229  for (std::list<std::unique_ptr<PathSegment>>::iterator segment = path_segments_.begin();
230  segment != path_segments_.end(); ++segment)
231  {
232  (*segment)->position_ = length_;
233  std::list<double> local_switching_points = (*segment)->getSwitchingPoints();
234  for (std::list<double>::const_iterator point = local_switching_points.begin();
235  point != local_switching_points.end(); ++point)
236  {
237  switching_points_.push_back(std::make_pair(length_ + *point, false));
238  }
239  length_ += (*segment)->getLength();
240  while (!switching_points_.empty() && switching_points_.back().first >= length_)
241  switching_points_.pop_back();
242  switching_points_.push_back(std::make_pair(length_, true));
243  }
244  switching_points_.pop_back();
245 }
246 
248 {
249  for (std::list<std::unique_ptr<PathSegment>>::const_iterator it = path.path_segments_.begin();
250  it != path.path_segments_.end(); ++it)
251  {
252  path_segments_.emplace_back((*it)->clone());
253  }
254 }
255 
256 double Path::getLength() const
257 {
258  return length_;
259 }
260 
262 {
263  std::list<std::unique_ptr<PathSegment>>::const_iterator it = path_segments_.begin();
264  std::list<std::unique_ptr<PathSegment>>::const_iterator next = it;
265  ++next;
266  while (next != path_segments_.end() && s >= (*next)->position_)
267  {
268  it = next;
269  ++next;
270  }
271  s -= (*it)->position_;
272  return (*it).get();
273 }
274 
275 Eigen::VectorXd Path::getConfig(double s) const
276 {
277  const PathSegment* path_segment = getPathSegment(s);
278  return path_segment->getConfig(s);
279 }
280 
281 Eigen::VectorXd Path::getTangent(double s) const
282 {
283  const PathSegment* path_segment = getPathSegment(s);
284  return path_segment->getTangent(s);
285 }
286 
287 Eigen::VectorXd Path::getCurvature(double s) const
288 {
289  const PathSegment* path_segment = getPathSegment(s);
290  return path_segment->getCurvature(s);
291 }
292 
293 double Path::getNextSwitchingPoint(double s, bool& discontinuity) const
294 {
295  std::list<std::pair<double, bool>>::const_iterator it = switching_points_.begin();
296  while (it != switching_points_.end() && it->first <= s)
297  {
298  ++it;
299  }
300  if (it == switching_points_.end())
301  {
302  discontinuity = true;
303  return length_;
304  }
305  discontinuity = it->second;
306  return it->first;
307 }
308 
309 std::list<std::pair<double, bool>> Path::getSwitchingPoints() const
310 {
311  return switching_points_;
312 }
313 
314 static double squared(double d)
315 {
316  return d * d;
317 }
318 
319 Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
320  double time_step)
321  : path_(path)
322  , max_velocity_(max_velocity)
323  , max_acceleration_(max_acceleration)
324  , joint_num_(max_velocity.size())
325  , valid_(true)
326  , time_step_(time_step)
327  , cached_time_(std::numeric_limits<double>::max())
328 {
329  trajectory_.push_back(TrajectoryStep(0.0, 0.0));
330  double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true);
331  while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_)
332  {
333  double before_acceleration;
334  TrajectoryStep switching_point;
335  if (getNextSwitchingPoint(trajectory_.back().path_pos_, switching_point, before_acceleration, after_acceleration))
336  {
337  break;
338  }
339  integrateBackward(trajectory_, switching_point.path_pos_, switching_point.path_vel_, before_acceleration);
340  }
341 
342  if (valid_)
343  {
344  double before_acceleration = getMinMaxPathAcceleration(path_.getLength(), 0.0, false);
345  integrateBackward(trajectory_, path_.getLength(), 0.0, before_acceleration);
346  }
347 
348  if (valid_)
349  {
350  // Calculate timing
351  std::list<TrajectoryStep>::iterator previous = trajectory_.begin();
352  std::list<TrajectoryStep>::iterator it = previous;
353  it->time_ = 0.0;
354  ++it;
355  while (it != trajectory_.end())
356  {
357  it->time_ =
358  previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0);
359  previous = it;
360  ++it;
361  }
362  }
363 }
364 
366 {
367 }
368 
369 // Returns true if end of path is reached.
370 bool Trajectory::getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
371  double& before_acceleration, double& after_acceleration)
372 {
373  TrajectoryStep acceleration_switching_point(path_pos, 0.0);
374  double acceleration_before_acceleration, acceleration_after_acceleration;
375  bool acceleration_reached_end;
376  do
377  {
378  acceleration_reached_end =
379  getNextAccelerationSwitchingPoint(acceleration_switching_point.path_pos_, acceleration_switching_point,
380  acceleration_before_acceleration, acceleration_after_acceleration);
381  } while (!acceleration_reached_end &&
382  acceleration_switching_point.path_vel_ > getVelocityMaxPathVelocity(acceleration_switching_point.path_pos_));
383 
384  TrajectoryStep velocity_switching_point(path_pos, 0.0);
385  double velocity_before_acceleration, velocity_after_acceleration;
386  bool velocity_reached_end;
387  do
388  {
389  velocity_reached_end = getNextVelocitySwitchingPoint(velocity_switching_point.path_pos_, velocity_switching_point,
390  velocity_before_acceleration, velocity_after_acceleration);
391  } while (
392  !velocity_reached_end && velocity_switching_point.path_pos_ <= acceleration_switching_point.path_pos_ &&
393  (velocity_switching_point.path_vel_ > getAccelerationMaxPathVelocity(velocity_switching_point.path_pos_ - EPS) ||
394  velocity_switching_point.path_vel_ > getAccelerationMaxPathVelocity(velocity_switching_point.path_pos_ + EPS)));
395 
396  if (acceleration_reached_end && velocity_reached_end)
397  {
398  return true;
399  }
400  else if (!acceleration_reached_end &&
401  (velocity_reached_end || acceleration_switching_point.path_pos_ <= velocity_switching_point.path_pos_))
402  {
403  next_switching_point = acceleration_switching_point;
404  before_acceleration = acceleration_before_acceleration;
405  after_acceleration = acceleration_after_acceleration;
406  return false;
407  }
408  else
409  {
410  next_switching_point = velocity_switching_point;
411  before_acceleration = velocity_before_acceleration;
412  after_acceleration = velocity_after_acceleration;
413  return false;
414  }
415 }
416 
417 bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
418  double& before_acceleration, double& after_acceleration)
419 {
420  double switching_path_pos = path_pos;
421  double switching_path_vel;
422  while (true)
423  {
424  bool discontinuity;
425  switching_path_pos = path_.getNextSwitchingPoint(switching_path_pos, discontinuity);
426 
427  if (switching_path_pos > path_.getLength() - EPS)
428  {
429  return true;
430  }
431 
432  if (discontinuity)
433  {
434  const double before_path_vel = getAccelerationMaxPathVelocity(switching_path_pos - EPS);
435  const double after_path_vel = getAccelerationMaxPathVelocity(switching_path_pos + EPS);
436  switching_path_vel = std::min(before_path_vel, after_path_vel);
437  before_acceleration = getMinMaxPathAcceleration(switching_path_pos - EPS, switching_path_vel, false);
438  after_acceleration = getMinMaxPathAcceleration(switching_path_pos + EPS, switching_path_vel, true);
439 
440  if ((before_path_vel > after_path_vel ||
441  getMinMaxPhaseSlope(switching_path_pos - EPS, switching_path_vel, false) >
442  getAccelerationMaxPathVelocityDeriv(switching_path_pos - 2.0 * EPS)) &&
443  (before_path_vel < after_path_vel ||
444  getMinMaxPhaseSlope(switching_path_pos + EPS, switching_path_vel, true) <
445  getAccelerationMaxPathVelocityDeriv(switching_path_pos + 2.0 * EPS)))
446  {
447  break;
448  }
449  }
450  else
451  {
452  switching_path_vel = getAccelerationMaxPathVelocity(switching_path_pos);
453  before_acceleration = 0.0;
454  after_acceleration = 0.0;
455 
456  if (getAccelerationMaxPathVelocityDeriv(switching_path_pos - EPS) < 0.0 &&
457  getAccelerationMaxPathVelocityDeriv(switching_path_pos + EPS) > 0.0)
458  {
459  break;
460  }
461  }
462  }
463 
464  next_switching_point = TrajectoryStep(switching_path_pos, switching_path_vel);
465  return false;
466 }
467 
468 bool Trajectory::getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
469  double& before_acceleration, double& after_acceleration)
470 {
471  const double step_size = 0.001;
472  const double accuracy = 0.000001;
473 
474  bool start = false;
475  path_pos -= step_size;
476  do
477  {
478  path_pos += step_size;
479 
480  if (getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >=
482  {
483  start = true;
484  }
485  } while ((!start ||
486  getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >
487  getVelocityMaxPathVelocityDeriv(path_pos)) &&
488  path_pos < path_.getLength());
489 
490  if (path_pos >= path_.getLength())
491  {
492  return true; // end of trajectory reached
493  }
494 
495  double before_path_pos = path_pos - step_size;
496  double after_path_pos = path_pos;
497  while (after_path_pos - before_path_pos > accuracy)
498  {
499  path_pos = (before_path_pos + after_path_pos) / 2.0;
500  if (getMinMaxPhaseSlope(path_pos, getVelocityMaxPathVelocity(path_pos), false) >
502  {
503  before_path_pos = path_pos;
504  }
505  else
506  {
507  after_path_pos = path_pos;
508  }
509  }
510 
511  before_acceleration = getMinMaxPathAcceleration(before_path_pos, getVelocityMaxPathVelocity(before_path_pos), false);
512  after_acceleration = getMinMaxPathAcceleration(after_path_pos, getVelocityMaxPathVelocity(after_path_pos), true);
513  next_switching_point = TrajectoryStep(after_path_pos, getVelocityMaxPathVelocity(after_path_pos));
514  return false;
515 }
516 
517 // Returns true if end of path is reached
518 bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration)
519 {
520  double path_pos = trajectory.back().path_pos_;
521  double path_vel = trajectory.back().path_vel_;
522 
523  std::list<std::pair<double, bool>> switching_points = path_.getSwitchingPoints();
524  std::list<std::pair<double, bool>>::iterator next_discontinuity = switching_points.begin();
525 
526  while (true)
527  {
528  while ((next_discontinuity != switching_points.end()) &&
529  (next_discontinuity->first <= path_pos || !next_discontinuity->second))
530  {
531  ++next_discontinuity;
532  }
533 
534  double old_path_pos = path_pos;
535  double old_path_vel = path_vel;
536 
537  path_vel += time_step_ * acceleration;
538  path_pos += time_step_ * 0.5 * (old_path_vel + path_vel);
539 
540  if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first)
541  {
542  // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical
543  // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665)
544  if (path_pos - next_discontinuity->first < EPS)
545  {
546  continue;
547  }
548  path_vel = old_path_vel +
549  (next_discontinuity->first - old_path_pos) * (path_vel - old_path_vel) / (path_pos - old_path_pos);
550  path_pos = next_discontinuity->first;
551  }
552 
553  if (path_pos > path_.getLength())
554  {
555  trajectory.push_back(TrajectoryStep(path_pos, path_vel));
556  return true;
557  }
558  else if (path_vel < 0.0)
559  {
560  valid_ = false;
561  ROS_ERROR_NAMED(LOGNAME, "Error while integrating forward: Negative path velocity");
562  return true;
563  }
564 
565  if (path_vel > getVelocityMaxPathVelocity(path_pos) &&
566  getMinMaxPhaseSlope(old_path_pos, getVelocityMaxPathVelocity(old_path_pos), false) <=
567  getVelocityMaxPathVelocityDeriv(old_path_pos))
568  {
569  path_vel = getVelocityMaxPathVelocity(path_pos);
570  }
571 
572  trajectory.push_back(TrajectoryStep(path_pos, path_vel));
573  acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true);
574 
575  if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos))
576  {
577  // Find more accurate intersection with max-velocity curve using bisection
578  TrajectoryStep overshoot = trajectory.back();
579  trajectory.pop_back();
580  double before = trajectory.back().path_pos_;
581  double before_path_vel = trajectory.back().path_vel_;
582  double after = overshoot.path_pos_;
583  double after_path_vel = overshoot.path_vel_;
584  while (after - before > EPS)
585  {
586  const double midpoint = 0.5 * (before + after);
587  double midpoint_path_vel = 0.5 * (before_path_vel + after_path_vel);
588 
589  if (midpoint_path_vel > getVelocityMaxPathVelocity(midpoint) &&
590  getMinMaxPhaseSlope(before, getVelocityMaxPathVelocity(before), false) <=
592  {
593  midpoint_path_vel = getVelocityMaxPathVelocity(midpoint);
594  }
595 
596  if (midpoint_path_vel > getAccelerationMaxPathVelocity(midpoint) ||
597  midpoint_path_vel > getVelocityMaxPathVelocity(midpoint))
598  {
599  after = midpoint;
600  after_path_vel = midpoint_path_vel;
601  }
602  else
603  {
604  before = midpoint;
605  before_path_vel = midpoint_path_vel;
606  }
607  }
608  trajectory.push_back(TrajectoryStep(before, before_path_vel));
609 
611  {
612  if (after > next_discontinuity->first)
613  {
614  return false;
615  }
616  else if (getMinMaxPhaseSlope(trajectory.back().path_pos_, trajectory.back().path_vel_, true) >
617  getAccelerationMaxPathVelocityDeriv(trajectory.back().path_pos_))
618  {
619  return false;
620  }
621  }
622  else
623  {
624  if (getMinMaxPhaseSlope(trajectory.back().path_pos_, trajectory_.back().path_vel_, false) >
626  {
627  return false;
628  }
629  }
630  }
631  }
632 }
633 
634 void Trajectory::integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
635  double acceleration)
636 {
637  std::list<TrajectoryStep>::iterator start2 = start_trajectory.end();
638  --start2;
639  std::list<TrajectoryStep>::iterator start1 = start2;
640  --start1;
641  std::list<TrajectoryStep> trajectory;
642  double slope;
643  assert(start1->path_pos_ <= path_pos);
644 
645  while (start1 != start_trajectory.begin() || path_pos >= 0.0)
646  {
647  if (start1->path_pos_ <= path_pos)
648  {
649  trajectory.push_front(TrajectoryStep(path_pos, path_vel));
650  path_vel -= time_step_ * acceleration;
651  path_pos -= time_step_ * 0.5 * (path_vel + trajectory.front().path_vel_);
652  acceleration = getMinMaxPathAcceleration(path_pos, path_vel, false);
653  slope = (trajectory.front().path_vel_ - path_vel) / (trajectory.front().path_pos_ - path_pos);
654 
655  if (path_vel < 0.0)
656  {
657  valid_ = false;
658  ROS_ERROR_NAMED(LOGNAME, "Error while integrating backward: Negative path velocity");
659  end_trajectory_ = trajectory;
660  return;
661  }
662  }
663  else
664  {
665  --start1;
666  --start2;
667  }
668 
669  // Check for intersection between current start trajectory and backward
670  // trajectory segments
671  const double start_slope = (start2->path_vel_ - start1->path_vel_) / (start2->path_pos_ - start1->path_pos_);
672  const double intersection_path_pos =
673  (start1->path_vel_ - path_vel + slope * path_pos - start_slope * start1->path_pos_) / (slope - start_slope);
674  if (std::max(start1->path_pos_, path_pos) - EPS <= intersection_path_pos &&
675  intersection_path_pos <= EPS + std::min(start2->path_pos_, trajectory.front().path_pos_))
676  {
677  const double intersection_path_vel =
678  start1->path_vel_ + start_slope * (intersection_path_pos - start1->path_pos_);
679  start_trajectory.erase(start2, start_trajectory.end());
680  start_trajectory.push_back(TrajectoryStep(intersection_path_pos, intersection_path_vel));
681  start_trajectory.splice(start_trajectory.end(), trajectory);
682  return;
683  }
684  }
685 
686  valid_ = false;
687  ROS_ERROR_NAMED(LOGNAME, "Error while integrating backward: Did not hit start trajectory");
688  end_trajectory_ = trajectory;
689 }
690 
691 double Trajectory::getMinMaxPathAcceleration(double path_pos, double path_vel, bool max)
692 {
693  Eigen::VectorXd config_deriv = path_.getTangent(path_pos);
694  Eigen::VectorXd config_deriv2 = path_.getCurvature(path_pos);
695  double factor = max ? 1.0 : -1.0;
696  double max_path_acceleration = std::numeric_limits<double>::max();
697  for (unsigned int i = 0; i < joint_num_; ++i)
698  {
699  if (config_deriv[i] != 0.0)
700  {
701  max_path_acceleration =
702  std::min(max_path_acceleration, max_acceleration_[i] / std::abs(config_deriv[i]) -
703  factor * config_deriv2[i] * path_vel * path_vel / config_deriv[i]);
704  }
705  }
706  return factor * max_path_acceleration;
707 }
708 
709 double Trajectory::getMinMaxPhaseSlope(double path_pos, double path_vel, bool max)
710 {
711  return getMinMaxPathAcceleration(path_pos, path_vel, max) / path_vel;
712 }
713 
714 double Trajectory::getAccelerationMaxPathVelocity(double path_pos) const
715 {
716  double max_path_velocity = std::numeric_limits<double>::infinity();
717  const Eigen::VectorXd config_deriv = path_.getTangent(path_pos);
718  const Eigen::VectorXd config_deriv2 = path_.getCurvature(path_pos);
719  for (unsigned int i = 0; i < joint_num_; ++i)
720  {
721  if (config_deriv[i] != 0.0)
722  {
723  for (unsigned int j = i + 1; j < joint_num_; ++j)
724  {
725  if (config_deriv[j] != 0.0)
726  {
727  double a_ij = config_deriv2[i] / config_deriv[i] - config_deriv2[j] / config_deriv[j];
728  if (a_ij != 0.0)
729  {
730  max_path_velocity = std::min(max_path_velocity, sqrt((max_acceleration_[i] / std::abs(config_deriv[i]) +
731  max_acceleration_[j] / std::abs(config_deriv[j])) /
732  std::abs(a_ij)));
733  }
734  }
735  }
736  }
737  else if (config_deriv2[i] != 0.0)
738  {
739  max_path_velocity = std::min(max_path_velocity, sqrt(max_acceleration_[i] / std::abs(config_deriv2[i])));
740  }
741  }
742  return max_path_velocity;
743 }
744 
745 double Trajectory::getVelocityMaxPathVelocity(double path_pos) const
746 {
747  const Eigen::VectorXd tangent = path_.getTangent(path_pos);
748  double max_path_velocity = std::numeric_limits<double>::max();
749  for (unsigned int i = 0; i < joint_num_; ++i)
750  {
751  max_path_velocity = std::min(max_path_velocity, max_velocity_[i] / std::abs(tangent[i]));
752  }
753  return max_path_velocity;
754 }
755 
757 {
758  return (getAccelerationMaxPathVelocity(path_pos + EPS) - getAccelerationMaxPathVelocity(path_pos - EPS)) /
759  (2.0 * EPS);
760 }
761 
763 {
764  const Eigen::VectorXd tangent = path_.getTangent(path_pos);
765  double max_path_velocity = std::numeric_limits<double>::max();
766  unsigned int active_constraint;
767  for (unsigned int i = 0; i < joint_num_; ++i)
768  {
769  const double this_max_path_velocity = max_velocity_[i] / std::abs(tangent[i]);
770  if (this_max_path_velocity < max_path_velocity)
771  {
772  max_path_velocity = this_max_path_velocity;
773  active_constraint = i;
774  }
775  }
776  return -(max_velocity_[active_constraint] * path_.getCurvature(path_pos)[active_constraint]) /
777  (tangent[active_constraint] * std::abs(tangent[active_constraint]));
778 }
779 
781 {
782  return valid_;
783 }
784 
786 {
787  return trajectory_.back().time_;
788 }
789 
790 std::list<Trajectory::TrajectoryStep>::const_iterator Trajectory::getTrajectorySegment(double time) const
791 {
792  if (time >= trajectory_.back().time_)
793  {
794  std::list<TrajectoryStep>::const_iterator last = trajectory_.end();
795  last--;
796  return last;
797  }
798  else
799  {
800  if (time < cached_time_)
801  {
803  }
804  while (time >= cached_trajectory_segment_->time_)
805  {
807  }
808  cached_time_ = time;
810  }
811 }
812 
813 Eigen::VectorXd Trajectory::getPosition(double time) const
814 {
815  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
816  std::list<TrajectoryStep>::const_iterator previous = it;
817  previous--;
818 
819  double time_step = it->time_ - previous->time_;
820  const double acceleration =
821  2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
822 
823  time_step = time - previous->time_;
824  const double path_pos =
825  previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
826 
827  return path_.getConfig(path_pos);
828 }
829 
830 Eigen::VectorXd Trajectory::getVelocity(double time) const
831 {
832  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
833  std::list<TrajectoryStep>::const_iterator previous = it;
834  previous--;
835 
836  double time_step = it->time_ - previous->time_;
837  const double acceleration =
838  2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
839 
840  time_step = time - previous->time_;
841  const double path_pos =
842  previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
843  const double path_vel = previous->path_vel_ + time_step * acceleration;
844 
845  return path_.getTangent(path_pos) * path_vel;
846 }
847 
848 Eigen::VectorXd Trajectory::getAcceleration(double time) const
849 {
850  std::list<TrajectoryStep>::const_iterator it = getTrajectorySegment(time);
851  std::list<TrajectoryStep>::const_iterator previous = it;
852  previous--;
853 
854  double time_step = it->time_ - previous->time_;
855  const double acceleration =
856  2.0 * (it->path_pos_ - previous->path_pos_ - time_step * previous->path_vel_) / (time_step * time_step);
857 
858  time_step = time - previous->time_;
859  const double path_pos =
860  previous->path_pos_ + time_step * previous->path_vel_ + 0.5 * time_step * time_step * acceleration;
861  const double path_vel = previous->path_vel_ + time_step * acceleration;
862  Eigen::VectorXd path_acc =
863  (path_.getTangent(path_pos) * path_vel - path_.getTangent(previous->path_pos_) * previous->path_vel_);
864  if (time_step > 0.0)
865  path_acc /= time_step;
866  return path_acc;
867 }
868 
869 TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double path_tolerance, const double resample_dt)
870  : path_tolerance_(path_tolerance), resample_dt_(resample_dt)
871 {
872 }
873 
875 {
876 }
877 
879  const double max_velocity_scaling_factor,
880  const double max_acceleration_scaling_factor) const
881 {
882  if (trajectory.empty())
883  return true;
884 
885  const robot_model::JointModelGroup* group = trajectory.getGroup();
886  if (!group)
887  {
888  ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for");
889  return false;
890  }
891 
892  // Validate scaling
893  double velocity_scaling_factor = 1.0;
894  if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
895  {
896  velocity_scaling_factor = max_velocity_scaling_factor;
897  }
898  else if (max_velocity_scaling_factor == 0.0)
899  {
900  ROS_DEBUG_NAMED(LOGNAME, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
901  velocity_scaling_factor);
902  }
903  else
904  {
905  ROS_WARN_NAMED(LOGNAME, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
906  max_velocity_scaling_factor, velocity_scaling_factor);
907  }
908 
909  double acceleration_scaling_factor = 1.0;
910  if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
911  {
912  acceleration_scaling_factor = max_acceleration_scaling_factor;
913  }
914  else if (max_acceleration_scaling_factor == 0.0)
915  {
916  ROS_DEBUG_NAMED(LOGNAME, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
917  acceleration_scaling_factor);
918  }
919  else
920  {
921  ROS_WARN_NAMED(LOGNAME, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
922  max_acceleration_scaling_factor, acceleration_scaling_factor);
923  }
924 
925  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
926  trajectory.unwind();
927 
928  // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints
929  const std::vector<std::string>& vars = group->getVariableNames();
930  const std::vector<int>& idx = group->getVariableIndexList();
931  const robot_model::RobotModel& rmodel = group->getParentModel();
932  const unsigned num_joints = group->getVariableCount();
933  const unsigned num_points = trajectory.getWayPointCount();
934 
935  // Get the limits (we do this at same time, unlike IterativeParabolicTimeParameterization)
936  Eigen::VectorXd max_velocity(num_joints);
937  Eigen::VectorXd max_acceleration(num_joints);
938  for (size_t j = 0; j < num_joints; ++j)
939  {
940  const robot_model::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);
941 
942  // Limits need to be non-zero, otherwise we never exit
943  max_velocity[j] = 1.0;
944  if (bounds.velocity_bounded_)
945  {
946  max_velocity[j] = std::min(fabs(bounds.max_velocity_), fabs(bounds.min_velocity_)) * velocity_scaling_factor;
947  max_velocity[j] = std::max(0.01, max_velocity[j]);
948  }
949 
950  max_acceleration[j] = 1.0;
951  if (bounds.acceleration_bounded_)
952  {
953  max_acceleration[j] =
954  std::min(fabs(bounds.max_acceleration_), fabs(bounds.min_acceleration_)) * acceleration_scaling_factor;
955  max_acceleration[j] = std::max(0.01, max_acceleration[j]);
956  }
957  }
958 
959  // Have to convert into Eigen data structs and remove repeated points
960  // (https://github.com/tobiaskunz/trajectories/issues/3)
961  std::list<Eigen::VectorXd> points;
962  for (size_t p = 0; p < num_points; ++p)
963  {
964  robot_state::RobotStatePtr waypoint = trajectory.getWayPointPtr(p);
965  Eigen::VectorXd new_point(num_joints);
966  bool diverse_point = (p == 0);
967 
968  for (size_t j = 0; j < num_joints; j++)
969  {
970  new_point[j] = waypoint->getVariablePosition(idx[j]);
971  if (p > 0 && std::abs(new_point[j] - points.back()[j]) > 0.001)
972  diverse_point = true;
973  }
974 
975  if (diverse_point)
976  points.push_back(new_point);
977  }
978 
979  // Return trajectory with only the first waypoint if there are not multiple diverse points
980  if (points.size() == 1)
981  {
982  ROS_WARN_NAMED(LOGNAME, "Trajectory is not being parameterized since it only contains a single distinct waypoint.");
984  trajectory.clear();
985  trajectory.addSuffixWayPoint(waypoint, 0.0);
986  return true;
987  }
988 
989  // Now actually call the algorithm
990  Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, 0.001);
991  if (!parameterized.isValid())
992  {
993  ROS_ERROR_NAMED(LOGNAME, "Unable to parameterize trajectory.");
994  return false;
995  }
996 
997  // Compute sample count
998  size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_);
999 
1000  // Resample and fill in trajectory
1002  trajectory.clear();
1003  double last_t = 0;
1004  for (size_t sample = 0; sample <= sample_count; ++sample)
1005  {
1006  // always sample the end of the trajectory as well
1007  double t = std::min(parameterized.getDuration(), sample * resample_dt_);
1008  Eigen::VectorXd position = parameterized.getPosition(t);
1009  Eigen::VectorXd velocity = parameterized.getVelocity(t);
1010  Eigen::VectorXd acceleration = parameterized.getAcceleration(t);
1011 
1012  for (size_t j = 0; j < num_joints; ++j)
1013  {
1014  waypoint.setVariablePosition(idx[j], position[j]);
1015  waypoint.setVariableVelocity(idx[j], velocity[j]);
1016  waypoint.setVariableAcceleration(idx[j], acceleration[j]);
1017  }
1018 
1019  trajectory.addSuffixWayPoint(waypoint, t - last_t);
1020  last_t = t;
1021  }
1022 
1023  return true;
1024 }
1025 } // namespace trajectory_processing
double getDuration() const
Returns the optimal duration of the trajectory.
Eigen::VectorXd getCurvature(double s) const
double getNextSwitchingPoint(double s, bool &discontinuity) const
std::list< std::pair< double, bool > > getSwitchingPoints() const
Eigen::VectorXd getTangent(double s) const
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
bool getNextSwitchingPoint(double path_pos, TrajectoryStep &next_switching_point, double &before_acceleration, double &after_acceleration)
ROSCPP_DECL void start()
#define ROS_WARN_NAMED(name,...)
virtual Eigen::VectorXd getTangent(double s) const =0
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Eigen::VectorXd getConfig(double s) const override
std::list< std::unique_ptr< PathSegment > > path_segments_
Eigen::VectorXd getVelocity(double time) const
Return the velocity vector for a given point in time.
void integrateBackward(std::list< TrajectoryStep > &start_trajectory, double path_pos, double path_vel, double acceleration)
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...
CircularPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &intersection, const Eigen::VectorXd &end, double max_deviation)
#define M_PI
geometry_msgs::TransformStamped t
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
double y
virtual Eigen::VectorXd getConfig(double s) const =0
std::list< std::pair< double, bool > > switching_points_
#define ROS_DEBUG_NAMED(name,...)
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const
PathSegment * getPathSegment(double &s) const
Trajectory(const Path &path, const Eigen::VectorXd &max_velocity, const Eigen::VectorXd &max_acceleration, double time_step=0.001)
Generates a time-optimal trajectory.
const robot_model::JointModelGroup * getGroup() const
Maintain a sequence of waypoints and the time durations between these waypoints.
std::list< TrajectoryStep >::const_iterator cached_trajectory_segment_
double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double getAccelerationMaxPathVelocity(double path_pos) const
double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max)
virtual Eigen::VectorXd getCurvature(double s) const =0
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
LinearPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &end)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep &next_switching_point, double &before_acceleration, double &after_acceleration)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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,...)
Eigen::VectorXd getAcceleration(double time) const
Return the acceleration vector for a given point in time.
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...
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep &next_switching_point, double &before_acceleration, double &after_acceleration)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Eigen::VectorXd getPosition(double time) const
Return the position/configuration vector for a given point in time.
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
Add a point to the trajectory.
double x
double max(double a, double b)
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::list< TrajectoryStep >::const_iterator getTrajectorySegment(double time) const
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
Path(const std::list< Eigen::VectorXd > &path, double max_deviation=0.0)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
bool isValid() const
Call this method after constructing the object to make sure the trajectory generation succeeded witho...
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
TimeOptimalTrajectoryGeneration(const double path_tolerance=0.1, const double resample_dt=0.1)
bool integrateForward(std::list< TrajectoryStep > &trajectory, double acceleration)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Apr 2 2020 03:50:31