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>
45 
46 namespace trajectory_processing
47 {
49 {
50 public:
51  PathSegment(double length = 0.0) : length_(length)
52  {
53  }
54  virtual ~PathSegment() // is required for destructing derived classes
55  {
56  }
57  double getLength() const
58  {
59  return length_;
60  }
61  virtual Eigen::VectorXd getConfig(double s) const = 0;
62  virtual Eigen::VectorXd getTangent(double s) const = 0;
63  virtual Eigen::VectorXd getCurvature(double s) const = 0;
64  virtual std::list<double> getSwitchingPoints() const = 0;
65  virtual PathSegment* clone() const = 0;
66 
67  double position_;
68 
69 protected:
70  double length_;
71 };
72 
73 class Path
74 {
75 public:
76  Path(const std::list<Eigen::VectorXd>& path, double max_deviation = 0.0);
77  Path(const Path& path);
78  double getLength() const;
79  Eigen::VectorXd getConfig(double s) const;
80  Eigen::VectorXd getTangent(double s) const;
81  Eigen::VectorXd getCurvature(double s) const;
82  double getNextSwitchingPoint(double s, bool& discontinuity) const;
83  std::list<std::pair<double, bool>> getSwitchingPoints() const;
84 
85 private:
86  PathSegment* getPathSegment(double& s) const;
87  double length_;
88  std::list<std::pair<double, bool>> switching_points_;
89  std::list<std::unique_ptr<PathSegment>> path_segments_;
90 };
91 
93 {
94 public:
96  Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
97  double time_step = 0.001);
98 
99  ~Trajectory(void);
100 
104  bool isValid() const;
105 
107  double getDuration() const;
108 
111  Eigen::VectorXd getPosition(double time) const;
113  Eigen::VectorXd getVelocity(double time) const;
115  Eigen::VectorXd getAcceleration(double time) const;
116 
117 private:
119  {
121  {
122  }
123  TrajectoryStep(double path_pos, double path_vel) : path_pos_(path_pos), path_vel_(path_vel)
124  {
125  }
126  double path_pos_;
127  double path_vel_;
128  double time_;
129  };
130 
131  bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
132  double& after_acceleration);
133  bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
134  double& before_acceleration, double& after_acceleration);
135  bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
136  double& after_acceleration);
137  bool integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration);
138  void integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
139  double acceleration);
140  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max);
141  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max);
142  double getAccelerationMaxPathVelocity(double path_pos) const;
143  double getVelocityMaxPathVelocity(double path_pos) const;
144  double getAccelerationMaxPathVelocityDeriv(double path_pos);
145  double getVelocityMaxPathVelocityDeriv(double path_pos);
146 
147  std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;
148 
150  Eigen::VectorXd max_velocity_;
151  Eigen::VectorXd max_acceleration_;
152  unsigned int joint_num_;
153  bool valid_;
154  std::list<TrajectoryStep> trajectory_;
155  std::list<TrajectoryStep> end_trajectory_; // non-empty only if the trajectory generation failed.
156 
157  const double time_step_;
158 
159  mutable double cached_time_;
160  mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
161 };
162 
164 {
165 public:
166  TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1,
167  const double min_angle_change = 0.001);
169 
170  bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
171  const double max_acceleration_scaling_factor = 1.0) const;
172 
173 private:
174  const double path_tolerance_;
175  const double resample_dt_;
176  const double min_angle_change_;
177 };
178 } // namespace trajectory_processing
179 
180 #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
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 Tue Sep 8 2020 04:12:46