DART  6.7.3
RRT.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  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above
13  * copyright notice, this list of conditions and the following
14  * disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials
18  * provided with the distribution.
19  * * Neither the name of the Georgia Tech Research Corporation nor
20  * the names of its contributors may be used to endorse or
21  * promote products derived from this software without specific
22  * prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS
25  * IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL GEORGIA
28  * TECH RESEARCH CORPORATION BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
30  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
31  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
32  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
33  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
35  * OF THE POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
47 #ifndef DART_PLANNING_RRT_HPP_
48 #define DART_PLANNING_RRT_HPP_
49 
50 #include <vector>
51 #include <list>
52 #include <Eigen/Core>
53 
56 
57 namespace flann {
58  template <class A> class L2;
59  template <class A> class Index;
60 }
61 
62 namespace dart {
63 
64 namespace simulation { class World; }
65 namespace dynamics { class Skeleton; }
66 
67 namespace planning {
68 
70 class RRT {
71 public:
72 
74  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 
77  typedef enum {
78  STEP_COLLISION, // Collided with obstacle. No node added.
79  STEP_REACHED, // The target node is closer than step size (so reached). No node added.
80  STEP_PROGRESS // One node added.
82 
83 public:
84  // Initialization constants and search variables
85 
86  const int ndim;
87  const double stepSize;
88 
89  int activeNode;
90  std::vector<int> parentVector;
91 
93  // NOTE We are using pointers for the VectorXd's because flann copies the pointers for the
94  // data points and we give it the copies made in the heap
95  std::vector<const Eigen::VectorXd*> configVector;
96 
97 public:
98 
100  RRT(dart::simulation::WorldPtr world, dart::dynamics::SkeletonPtr robot, const std::vector<std::size_t> &dofs, const Eigen::VectorXd &root,
101  double stepSize = 0.02);
102 
104  RRT(simulation::WorldPtr world, dynamics::SkeletonPtr robot, const std::vector<std::size_t> &dofs,
105  const std::vector<Eigen::VectorXd> &roots, double stepSize = 0.02);
106 
108  virtual ~RRT() {}
109 
112  bool connect();
113 
115  bool connect(const Eigen::VectorXd &target);
116 
119 
121  StepResult tryStep(const Eigen::VectorXd &qtry);
122 
124  virtual StepResult tryStepFromNode(const Eigen::VectorXd &qtry, int NNidx);
125 
130  virtual bool newConfig(std::list<Eigen::VectorXd> &intermediatePoints, Eigen::VectorXd &qnew,
131  const Eigen::VectorXd &qnear, const Eigen::VectorXd &qtarget);
132 
136  double getGap(const Eigen::VectorXd &target);
137 
140  void tracePath(int node, std::list<Eigen::VectorXd> &path, bool reverse = false);
141 
143  std::size_t getSize();
144 
146  virtual bool checkCollisions(const Eigen::VectorXd &c);
147 
149  virtual Eigen::VectorXd getRandomConfig();
150 
151 protected:
152 
155  std::vector<std::size_t> dofs;
156 
159 
161  double randomInRange(double min, double max);
162 
164  virtual int getNearestNeighbor(const Eigen::VectorXd &qsamp);
165 
167  virtual int addNode(const Eigen::VectorXd &qnew, int parentId);
168 };
169 
170 } // namespace planning
171 } // namespace dart
172 
173 #endif // DART_PLANNING_RRT_HPP_
The rapidly-expanding random tree implementation.
Definition: RRT.hpp:70
const double stepSize
Step size at each node creation.
Definition: RRT.hpp:87
virtual int addNode(const Eigen::VectorXd &qnew, int parentId)
Adds a new node to the tree.
Definition: RRT.cpp:159
std::vector< int > parentVector
The ith node in configVector has parent with index pV[i].
Definition: RRT.hpp:90
const int ndim
Number of dof we can manipulate (may be less than robot's)
Definition: RRT.hpp:86
StepResult tryStep()
Try a single step with the given "stepSize" to a random configuration. Fail if collide.
Definition: RRT.cpp:116
StepResult
To get byte-aligned Eigen vectors.
Definition: RRT.hpp:77
@ STEP_PROGRESS
Definition: RRT.hpp:80
@ STEP_REACHED
Definition: RRT.hpp:79
@ STEP_COLLISION
Definition: RRT.hpp:78
flann::Index< flann::L2< double > > * index
The underlying flann data structure for fast nearest neighbor searches.
Definition: RRT.hpp:158
virtual bool newConfig(std::list< Eigen::VectorXd > &intermediatePoints, Eigen::VectorXd &qnew, const Eigen::VectorXd &qnear, const Eigen::VectorXd &qtarget)
Checks if the given new configuration is in collision with an obstacle.
Definition: RRT.cpp:154
int activeNode
Last added node or the nearest node found after a search.
Definition: RRT.hpp:89
RRT(dart::simulation::WorldPtr world, dart::dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const Eigen::VectorXd &root, double stepSize=0.02)
virtual Eigen::VectorXd getRandomConfig()
Returns a random configuration with the specified node IDs.
Definition: RRT.cpp:201
RRT(simulation::WorldPtr world, dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const std::vector< Eigen::VectorXd > &roots, double stepSize=0.02)
Constructor with multiple roots (so, multiple trees)
dynamics::SkeletonPtr robot
The ID of the robot for which a plan is generated.
Definition: RRT.hpp:154
double getGap(const Eigen::VectorXd &target)
Returns the distance between the current active node and the given node.
Definition: RRT.cpp:212
StepResult tryStep(const Eigen::VectorXd &qtry)
Try a single step with the given "stepSize" to the given configuration. Fail if collide.
simulation::WorldPtr world
The world that the robot is in.
Definition: RRT.hpp:153
virtual int getNearestNeighbor(const Eigen::VectorXd &qsamp)
Returns the nearest neighbor to query point.
Definition: RRT.cpp:178
std::size_t getSize()
Returns the number of nodes in the tree.
Definition: RRT.cpp:235
virtual bool checkCollisions(const Eigen::VectorXd &c)
Implementation-specific function for checking collisions.
Definition: RRT.cpp:229
bool connect()
Reach for a random node by repeatedly extending nodes from the nearest neighbor in the tree.
Definition: RRT.cpp:95
std::vector< std::size_t > dofs
The dofs of the robot the planner can manipulate.
Definition: RRT.hpp:155
bool connect(const Eigen::VectorXd &target)
Reach for a target by repeatedly extending nodes from the nearest neighbor. Stop if collide.
std::vector< const Eigen::VectorXd * > configVector
All visited configs.
Definition: RRT.hpp:95
void tracePath(int node, std::list< Eigen::VectorXd > &path, bool reverse=false)
Traces the path from some node to the initConfig node - useful in creating the full path after the go...
Definition: RRT.cpp:217
virtual ~RRT()
Destructor.
Definition: RRT.hpp:108
double randomInRange(double min, double max)
Returns a random value between the given minimum and maximum value.
Definition: RRT.cpp:192
virtual StepResult tryStepFromNode(const Eigen::VectorXd &qtry, int NNidx)
Tries to extend tree towards provided sample.
Definition: RRT.cpp:128
Definition: RRT.hpp:59
Definition: RRT.hpp:58
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
std::shared_ptr< World > WorldPtr
Definition: SmartPointer.hpp:41
Definition: BulletCollisionDetector.cpp:63
Definition: RRT.hpp:57