DART  6.7.3
PathFollowingTrajectory.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 // Algorithm details and publications: http://www.golems.org/node/1570
34 
35 #ifndef DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
36 #define DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
37 
38 #include <Eigen/Core>
39 #include "dart/planning/Path.hpp"
41 
42 namespace dart {
43 namespace planning {
44 
46 {
47 public:
48  PathFollowingTrajectory(const Path &path, const Eigen::VectorXd &maxVelocity, const Eigen::VectorXd &maxAcceleration);
50 
51  bool isValid() const;
52  double getDuration() const;
53  Eigen::VectorXd getPosition(double time) const;
54  Eigen::VectorXd getVelocity(double time) const;
55  double getMaxAccelerationError();
56 
57 private:
58  struct TrajectoryStep {
60  TrajectoryStep(double pathPos, double pathVel) :
63  time(0.0)
64  {}
65  double pathPos;
66  double pathVel;
67  double time;
68  };
69 
70  bool getNextSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
71  bool getNextAccelerationSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
72  bool getNextVelocitySwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration);
73  bool integrateForward(std::list<TrajectoryStep> &trajectory, double acceleration);
74  void integrateBackward(std::list<TrajectoryStep> &trajectory, std::list<TrajectoryStep> &startTrajectory, double acceleration);
75  double getMinMaxPathAcceleration(double pathPosition, double pathVelocity, bool max);
76  double getMinMaxPhaseSlope(double pathPosition, double pathVelocity, bool max);
77  double getAccelerationMaxPathVelocity(double pathPos);
78  double getVelocityMaxPathVelocity(double pathPos);
79  double getAccelerationMaxPathVelocityDeriv(double pathPos);
80  double getVelocityMaxPathVelocityDeriv(double pathPos);
81 
82  TrajectoryStep getIntersection(const std::list<TrajectoryStep> &trajectory, std::list<TrajectoryStep>::iterator &it, const TrajectoryStep &linePoint1, const TrajectoryStep &linePoint2);
83  inline double getSlope(const TrajectoryStep &point1, const TrajectoryStep &point2);
84  inline double getSlope(std::list<TrajectoryStep>::const_iterator lineEnd);
85 
86  std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;
87 
89  Eigen::VectorXd maxVelocity;
90  Eigen::VectorXd maxAcceleration;
91  unsigned int n;
92  bool valid;
93  std::list<TrajectoryStep> trajectory;
94 
95  static const double eps;
96  static const double timeStep;
97 
98  mutable double cachedTime;
99  mutable std::list<TrajectoryStep>::const_iterator cachedTrajectorySegment;
100 };
101 
102 } // namespace planning
103 } // namespace dart
104 
105 #endif // DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:109
Definition: PathFollowingTrajectory.hpp:46
Path path
Definition: PathFollowingTrajectory.hpp:88
void integrateBackward(std::list< TrajectoryStep > &trajectory, std::list< TrajectoryStep > &startTrajectory, double acceleration)
Definition: PathFollowingTrajectory.cpp:331
Eigen::VectorXd maxVelocity
Definition: PathFollowingTrajectory.hpp:89
unsigned int n
Definition: PathFollowingTrajectory.hpp:91
double getSlope(const TrajectoryStep &point1, const TrajectoryStep &point2)
Definition: PathFollowingTrajectory.cpp:425
Eigen::VectorXd getVelocity(double time) const
Definition: PathFollowingTrajectory.cpp:575
bool valid
Definition: PathFollowingTrajectory.hpp:92
bool getNextSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:120
~PathFollowingTrajectory(void)
Definition: PathFollowingTrajectory.cpp:115
double getVelocityMaxPathVelocity(double pathPos)
Definition: PathFollowingTrajectory.cpp:505
double getMinMaxPhaseSlope(double pathPosition, double pathVelocity, bool max)
Definition: PathFollowingTrajectory.cpp:476
std::list< TrajectoryStep > trajectory
Definition: PathFollowingTrajectory.hpp:93
bool isValid() const
Definition: PathFollowingTrajectory.cpp:533
double getMaxAccelerationError()
Definition: PathFollowingTrajectory.cpp:592
std::list< TrajectoryStep >::const_iterator cachedTrajectorySegment
Definition: PathFollowingTrajectory.hpp:99
Eigen::VectorXd getPosition(double time) const
Definition: PathFollowingTrajectory.cpp:559
double cachedTime
Definition: PathFollowingTrajectory.hpp:98
double getSlope(std::list< TrajectoryStep >::const_iterator lineEnd)
PathFollowingTrajectory(const Path &path, const Eigen::VectorXd &maxVelocity, const Eigen::VectorXd &maxAcceleration)
Definition: PathFollowingTrajectory.cpp:54
bool integrateForward(std::list< TrajectoryStep > &trajectory, double acceleration)
Definition: PathFollowingTrajectory.cpp:234
double getAccelerationMaxPathVelocity(double pathPos)
Definition: PathFollowingTrajectory.cpp:480
double getMinMaxPathAcceleration(double pathPosition, double pathVelocity, bool max)
Definition: PathFollowingTrajectory.cpp:462
Eigen::VectorXd maxAcceleration
Definition: PathFollowingTrajectory.hpp:90
bool getNextAccelerationSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:155
double getVelocityMaxPathVelocityDeriv(double pathPos)
Definition: PathFollowingTrajectory.cpp:518
TrajectoryStep getIntersection(const std::list< TrajectoryStep > &trajectory, std::list< TrajectoryStep >::iterator &it, const TrajectoryStep &linePoint1, const TrajectoryStep &linePoint2)
Definition: PathFollowingTrajectory.cpp:435
bool getNextVelocitySwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:196
std::list< TrajectoryStep >::const_iterator getTrajectorySegment(double time) const
Definition: PathFollowingTrajectory.cpp:541
static const double eps
Definition: PathFollowingTrajectory.hpp:95
static const double timeStep
Definition: PathFollowingTrajectory.hpp:96
double getDuration() const
Definition: PathFollowingTrajectory.cpp:537
double getAccelerationMaxPathVelocityDeriv(double pathPos)
Definition: PathFollowingTrajectory.cpp:514
Definition: Path.hpp:71
Definition: Trajectory.hpp:44
std::multimap< dart::dynamics::Shape *, SimpleFrameShapeDnD * >::iterator iterator
Definition: Viewer.cpp:622
Definition: BulletCollisionDetector.cpp:63
Definition: PathFollowingTrajectory.hpp:58
double pathPos
Definition: PathFollowingTrajectory.hpp:65
TrajectoryStep(double pathPos, double pathVel)
Definition: PathFollowingTrajectory.hpp:60
TrajectoryStep()
Definition: PathFollowingTrajectory.hpp:59
double time
Definition: PathFollowingTrajectory.hpp:67
double pathVel
Definition: PathFollowingTrajectory.hpp:66