time_optimal_trajectory_generation.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2012, 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 #ifndef MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H
40 #define MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H
41 
42 #include <Eigen/Core>
43 #include <list>
46 
47 namespace trajectory_processing
48 {
50 {
51 public:
52  PathSegment(double length = 0.0) : length_(length)
53  {
54  }
55  virtual ~PathSegment() // is required for destructing derived classes
56  {
57  }
58  double getLength() const
59  {
60  return length_;
61  }
62  virtual Eigen::VectorXd getConfig(double s) const = 0;
63  virtual Eigen::VectorXd getTangent(double s) const = 0;
64  virtual Eigen::VectorXd getCurvature(double s) const = 0;
65  virtual std::list<double> getSwitchingPoints() const = 0;
66  virtual PathSegment* clone() const = 0;
67 
68  double position_;
69 
70 protected:
71  double length_;
72 };
73 
74 class Path
75 {
76 public:
77  Path(const std::list<Eigen::VectorXd>& path, double max_deviation = 0.0);
78  Path(const Path& path);
79  double getLength() const;
80  Eigen::VectorXd getConfig(double s) const;
81  Eigen::VectorXd getTangent(double s) const;
82  Eigen::VectorXd getCurvature(double s) const;
83  double getNextSwitchingPoint(double s, bool& discontinuity) const;
84  std::list<std::pair<double, bool>> getSwitchingPoints() const;
85 
86 private:
87  PathSegment* getPathSegment(double& s) const;
88  double length_;
89  std::list<std::pair<double, bool>> switching_points_;
90  std::list<std::unique_ptr<PathSegment>> path_segments_;
91 };
92 
94 {
95 public:
97  Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
98  double time_step = 0.001);
99 
100  ~Trajectory();
101 
105  bool isValid() const;
106 
108  double getDuration() const;
109 
112  Eigen::VectorXd getPosition(double time) const;
114  Eigen::VectorXd getVelocity(double time) const;
116  Eigen::VectorXd getAcceleration(double time) const;
117 
118 private:
120  {
122  {
123  }
124  TrajectoryStep(double path_pos, double path_vel) : path_pos_(path_pos), path_vel_(path_vel)
125  {
126  }
127  double path_pos_;
128  double path_vel_;
129  double time_;
130  };
131 
132  bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
133  double& after_acceleration);
134  bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
135  double& before_acceleration, double& after_acceleration);
136  bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
137  double& after_acceleration);
138  bool integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration);
139  void integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
140  double acceleration);
141  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max);
142  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max);
143  double getAccelerationMaxPathVelocity(double path_pos) const;
144  double getVelocityMaxPathVelocity(double path_pos) const;
145  double getAccelerationMaxPathVelocityDeriv(double path_pos);
146  double getVelocityMaxPathVelocityDeriv(double path_pos);
147 
148  std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;
149 
151  Eigen::VectorXd max_velocity_;
152  Eigen::VectorXd max_acceleration_;
153  unsigned int joint_num_;
154  bool valid_;
155  std::list<TrajectoryStep> trajectory_;
156  std::list<TrajectoryStep> end_trajectory_; // non-empty only if the trajectory generation failed.
157 
158  const double time_step_;
159 
160  mutable double cached_time_;
161  mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
162 };
163 
166 {
167 public:
168  TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1,
169  const double min_angle_change = 0.001);
170 
171  bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
172  const double max_acceleration_scaling_factor = 1.0) const override;
173 
174 private:
175  const double path_tolerance_;
176  const double resample_dt_;
177  const double min_angle_change_;
178 };
179 } // namespace trajectory_processing
180 
181 #endif // MOVEIT_TRAJECTORY_PROCESSING_TIME_OPTIMAL_TRAJECTORY_GENERATION_H
virtual Eigen::VectorXd getTangent(double s) const =0
XmlRpcServer s
std::list< std::unique_ptr< PathSegment > > path_segments_
virtual Eigen::VectorXd getConfig(double s) const =0
std::list< std::pair< double, bool > > switching_points_
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
Maintain a sequence of waypoints and the time durations between these waypoints.
std::list< TrajectoryStep >::const_iterator cached_trajectory_segment_
virtual std::list< double > getSwitchingPoints() const =0
MOVEIT_CLASS_FORWARD(IterativeParabolicTimeParameterization)
This class modifies the timestamps of a trajectory to respect velocity and acceleration constraints...
virtual Eigen::VectorXd getCurvature(double s) const =0
virtual PathSegment * clone() const =0


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04