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


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