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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jan 18 2022 03:23:45