DART  6.10.1
PathFollowingTrajectory.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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:
49  const Path& path,
50  const Eigen::VectorXd& maxVelocity,
51  const Eigen::VectorXd& maxAcceleration);
53 
54  bool isValid() const;
55  double getDuration() const;
56  Eigen::VectorXd getPosition(double time) const;
57  Eigen::VectorXd getVelocity(double time) const;
58  double getMaxAccelerationError();
59 
60 private:
62  {
64  {
65  }
66  TrajectoryStep(double pathPos, double pathVel)
68  {
69  }
70  double pathPos;
71  double pathVel;
72  double time;
73  };
74 
76  double pathPos,
77  TrajectoryStep& nextSwitchingPoint,
78  double& beforeAcceleration,
79  double& afterAcceleration);
81  double pathPos,
82  TrajectoryStep& nextSwitchingPoint,
83  double& beforeAcceleration,
84  double& afterAcceleration);
86  double pathPos,
87  TrajectoryStep& nextSwitchingPoint,
88  double& beforeAcceleration,
89  double& afterAcceleration);
90  bool integrateForward(
91  std::list<TrajectoryStep>& trajectory, double acceleration);
92  void integrateBackward(
93  std::list<TrajectoryStep>& trajectory,
94  std::list<TrajectoryStep>& startTrajectory,
95  double acceleration);
97  double pathPosition, double pathVelocity, bool max);
98  double getMinMaxPhaseSlope(
99  double pathPosition, double pathVelocity, bool max);
100  double getAccelerationMaxPathVelocity(double pathPos);
101  double getVelocityMaxPathVelocity(double pathPos);
102  double getAccelerationMaxPathVelocityDeriv(double pathPos);
103  double getVelocityMaxPathVelocityDeriv(double pathPos);
104 
106  const std::list<TrajectoryStep>& trajectory,
108  const TrajectoryStep& linePoint1,
109  const TrajectoryStep& linePoint2);
110  inline double getSlope(
111  const TrajectoryStep& point1, const TrajectoryStep& point2);
112  inline double getSlope(std::list<TrajectoryStep>::const_iterator lineEnd);
113 
114  std::list<TrajectoryStep>::const_iterator getTrajectorySegment(
115  double time) const;
116 
118  Eigen::VectorXd maxVelocity;
119  Eigen::VectorXd maxAcceleration;
120  unsigned int n;
121  bool valid;
122  std::list<TrajectoryStep> trajectory;
123 
124  static const double eps;
125  static const double timeStep;
126 
127  mutable double cachedTime;
128  mutable std::list<TrajectoryStep>::const_iterator cachedTrajectorySegment;
129 };
130 
131 } // namespace planning
132 } // namespace dart
133 
134 #endif // DART_PLANNING_PATHFOLLOWINGTRAJECTORY_HPP_
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:110
Definition: PathFollowingTrajectory.hpp:46
Path path
Definition: PathFollowingTrajectory.hpp:117
void integrateBackward(std::list< TrajectoryStep > &trajectory, std::list< TrajectoryStep > &startTrajectory, double acceleration)
Definition: PathFollowingTrajectory.cpp:455
Eigen::VectorXd maxVelocity
Definition: PathFollowingTrajectory.hpp:118
unsigned int n
Definition: PathFollowingTrajectory.hpp:120
double getSlope(const TrajectoryStep &point1, const TrajectoryStep &point2)
Definition: PathFollowingTrajectory.cpp:588
Eigen::VectorXd getVelocity(double time) const
Definition: PathFollowingTrajectory.cpp:803
bool valid
Definition: PathFollowingTrajectory.hpp:121
bool getNextSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:137
~PathFollowingTrajectory(void)
Definition: PathFollowingTrajectory.cpp:132
double getVelocityMaxPathVelocity(double pathPos)
Definition: PathFollowingTrajectory.cpp:709
double getMinMaxPhaseSlope(double pathPosition, double pathVelocity, bool max)
Definition: PathFollowingTrajectory.cpp:666
std::list< TrajectoryStep > trajectory
Definition: PathFollowingTrajectory.hpp:122
bool isValid() const
Definition: PathFollowingTrajectory.cpp:748
double getMaxAccelerationError()
Definition: PathFollowingTrajectory.cpp:825
std::list< TrajectoryStep >::const_iterator cachedTrajectorySegment
Definition: PathFollowingTrajectory.hpp:128
Eigen::VectorXd getPosition(double time) const
Definition: PathFollowingTrajectory.cpp:782
double cachedTime
Definition: PathFollowingTrajectory.hpp:127
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:323
double getAccelerationMaxPathVelocity(double pathPos)
Definition: PathFollowingTrajectory.cpp:672
double getMinMaxPathAcceleration(double pathPosition, double pathVelocity, bool max)
Definition: PathFollowingTrajectory.cpp:646
Eigen::VectorXd maxAcceleration
Definition: PathFollowingTrajectory.hpp:119
bool getNextAccelerationSwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:204
double getVelocityMaxPathVelocityDeriv(double pathPos)
Definition: PathFollowingTrajectory.cpp:729
TrajectoryStep getIntersection(const std::list< TrajectoryStep > &trajectory, std::list< TrajectoryStep >::iterator &it, const TrajectoryStep &linePoint1, const TrajectoryStep &linePoint2)
Definition: PathFollowingTrajectory.cpp:603
bool getNextVelocitySwitchingPoint(double pathPos, TrajectoryStep &nextSwitchingPoint, double &beforeAcceleration, double &afterAcceleration)
Definition: PathFollowingTrajectory.cpp:267
std::list< TrajectoryStep >::const_iterator getTrajectorySegment(double time) const
Definition: PathFollowingTrajectory.cpp:759
static const double eps
Definition: PathFollowingTrajectory.hpp:124
static const double timeStep
Definition: PathFollowingTrajectory.hpp:125
double getDuration() const
Definition: PathFollowingTrajectory.cpp:753
double getAccelerationMaxPathVelocityDeriv(double pathPos)
Definition: PathFollowingTrajectory.cpp:721
Definition: Path.hpp:72
Definition: Trajectory.hpp:44
std::multimap< dart::dynamics::Shape *, SimpleFrameShapeDnD * >::iterator iterator
Definition: Viewer.cpp:620
Definition: BulletCollisionDetector.cpp:65
Definition: PathFollowingTrajectory.hpp:62
double pathPos
Definition: PathFollowingTrajectory.hpp:70
TrajectoryStep(double pathPos, double pathVel)
Definition: PathFollowingTrajectory.hpp:66
TrajectoryStep()
Definition: PathFollowingTrajectory.hpp:63
double time
Definition: PathFollowingTrajectory.hpp:72
double pathVel
Definition: PathFollowingTrajectory.hpp:71