DART  6.7.3
PathPlanner.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 
42 #ifndef DART_PLANNING_PATHPLANNER_HPP_
43 #define DART_PLANNING_PATHPLANNER_HPP_
44 
45 #include <Eigen/Core>
46 #include <iostream>
47 #include <limits>
48 #include <list>
49 #include <vector>
52 #include "dart/planning/RRT.hpp"
53 #include <cstdio>
54 
55 namespace dart {
56 namespace planning {
57 
58 /* ********************************************************************************************* */
60 template <class R = RRT>
61 class PathPlanner {
62 public:
63 
64  bool connect;
66  double stepSize;
67  double goalBias;
68  std::size_t maxNodes;
70 
71  // NOTE: It is useful to keep the rrts around after planning for reuse, analysis, and etc.
72  R* start_rrt;
73  R* goal_rrt;
74 
75 public:
76 
78  PathPlanner() : world(nullptr) {}
79 
81  PathPlanner(simulation::World& world, bool bidirectional_ = true, bool connect_ = true, double stepSize_ = 0.1,
82  std::size_t maxNodes_ = 1e6, double goalBias_ = 0.3) : world(&world), bidirectional(bidirectional_),
83  connect(connect_), stepSize(stepSize_), maxNodes(maxNodes_), goalBias(goalBias_) {
84  }
85 
87  virtual ~PathPlanner() {}
88 
90  bool planPath(dynamics::Skeleton* robot, const std::vector<int> &dofs, const Eigen::VectorXd &start,
91  const Eigen::VectorXd &goal, std::list<Eigen::VectorXd> &path) {
92  std::vector<Eigen::VectorXd> startVector, goalVector;
93  startVector.push_back(start);
94  goalVector.push_back(goal);
95  return planPath(robot, dofs, startVector, goalVector, path);
96  }
97 
99  bool planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs, const std::vector<Eigen::VectorXd> &start,
100  const std::vector<Eigen::VectorXd> &goal, std::list<Eigen::VectorXd> &path);
101 
102 private:
103 
105  bool planSingleTreeRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
106  const std::vector<Eigen::VectorXd> &start, const Eigen::VectorXd &goal,
107  std::list<Eigen::VectorXd> &path);
108 
115  bool planBidirectionalRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
116  const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
117  std::list<Eigen::VectorXd> &path);
118 };
119 
120 /* ********************************************************************************************* */
121 template <class R>
122 bool PathPlanner<R>::planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs,
123  const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
124  std::list<Eigen::VectorXd> &path) {
125 
126  Eigen::VectorXd savedConfiguration = robot->getPositions(dofs);
127 
128  // ====================================================================
129  // Check for collisions in the start and goal configurations
130 
131  // Sift through the possible start configurations and eliminate those that are in collision
132  std::vector<Eigen::VectorXd> feasibleStart;
133  for(unsigned int i = 0; i < start.size(); i++) {
134  robot->setPositions(dofs, start[i]);
135  if(!world->checkCollision()) feasibleStart.push_back(start[i]);
136  }
137 
138  // Return false if there are no feasible start configurations
139  if(feasibleStart.empty()) {
140  printf("WARNING: PathPlanner: Feasible start points are empty!\n");
141  return false;
142  }
143 
144  // Sift through the possible goal configurations and eliminate those that are in collision
145  std::vector<Eigen::VectorXd> feasibleGoal;
146  for(unsigned int i = 0; i < goal.size(); i++) {
147  robot->setPositions(dofs, goal[i]);
148  if(!world->checkCollision()) feasibleGoal.push_back(goal[i]);
149  }
150 
151  // Return false if there are no feasible goal configurations
152  if(feasibleGoal.empty()) {
153  printf("WARNING: PathPlanner: Feasible goal points are empty!\n");
154  return false;
155  }
156 
157  // ====================================================================
158  // Make the correct RRT algorithm for the given method
159 
160  // Direct the search towards single or bidirectional
161  bool result = false;
162  if(bidirectional)
163  result = planBidirectionalRrt(robot, dofs, feasibleStart, feasibleGoal, path);
164  else {
165  if(feasibleGoal.size() > 1) fprintf(stderr, "WARNING: planPath is using ONLY the first goal!\n");
166  result = planSingleTreeRrt(robot, dofs, feasibleStart, feasibleGoal.front(), path);
167  }
168 
169  // Restore previous robot configuration
170  robot->setPositions(dofs, savedConfiguration);
171 
172  return result;
173 }
174 
175 /* ********************************************************************************************* */
176 template <class R>
177 bool PathPlanner<R>::planSingleTreeRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
178  const std::vector<Eigen::VectorXd> &start, const Eigen::VectorXd &goal,
179  std::list<Eigen::VectorXd> &path) {
180 
181  const bool debug = false;
182 
183  // Initialize the RRT
184  start_rrt = new R (world, robot, dofs, start, stepSize);
185 
186  // Expand the tree until the goal is reached or the max # nodes is passed
187  typename R::StepResult result = R::STEP_PROGRESS;
188  double smallestGap = std::numeric_limits<double>::infinity();
189  std::size_t numNodes = start_rrt->getSize();
190  while(numNodes <= maxNodes) {
191 
192  // Get the target node based on the bias
193  Eigen::VectorXd target;
194  double randomValue = ((double) rand()) / RAND_MAX;
195  if(randomValue < goalBias) target = goal;
196  else target = start_rrt->getRandomConfig();
197 
198  // Based on the method, either attempt to connect to the target directly or take a small step
199  if(connect) start_rrt->connect(target);
200  else start_rrt->tryStep(target);
201 
202  // Check if the goal is reached and create the path, if so
203  double gap = start_rrt->getGap(goal);
204  if(gap < stepSize) {
205  if(debug) std::cout << "Returning true, reached the goal" << std::endl;
206  start_rrt->tracePath(start_rrt->activeNode, path);
207  return true;
208  }
209 
210  // Update the number of nodes
211  numNodes = start_rrt->getSize();
212  }
213 
214  if(debug) printf("numNodes: %lu\n", numNodes);
215 
216  // Maximum # of iterations are reached and path is not found - failed.
217  return false;
218 }
219 
220 /* ********************************************************************************************* */
221 template <class R>
222 bool PathPlanner<R>::planBidirectionalRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
223  const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
224  std::list<Eigen::VectorXd> &path) {
225 
226  const bool debug = false;
227 
228  // Initialize both the start and goal RRTs.
229  // NOTE: We use the pointers for the RRTs to swap their roles in extending towards a target
230  // (random or goal) node.
231  start_rrt = new R(world, robot, dofs, start, stepSize);
232  goal_rrt = new R(world, robot, dofs, goal, stepSize);
233  R* rrt1 = start_rrt;
234  R* rrt2 = goal_rrt;
235 
236  // Expand the tree until the trees meet or the max # nodes is passed
237  double smallestGap = std::numeric_limits<double>::infinity();
238  std::size_t numNodes = rrt1->getSize() + rrt2->getSize();
239  while(numNodes < maxNodes) {
240 
241  // Swap the roles of the two RRTs. Remember, the first rrt reaches out to a target node and
242  // creates a new node and the second rrt reaches to _the new node_.
243  R* temp = rrt1;
244  rrt1 = rrt2;
245  rrt2 = temp;
246 
247  // Get the target node based on the bias
248  Eigen::VectorXd target;
249  double randomValue = ((double) rand()) / RAND_MAX;
250  if(randomValue < goalBias) target = goal[0];
251  else target = rrt1->getRandomConfig();
252 
253  // Based on the method, rrt1 either attempt to connect to the target directly or takes a step
254  if(connect) rrt1->connect(target);
255  else rrt1->tryStep(target);
256 
257  // rrt2 uses the last added node of rrt1 as a target and reaches out to it (connect or step)
258  // NOTE: If a node was not added, the nearest neighbor to the random node in rrt1 is used.
259  // NOTE: connect(x) and tryStep(x) functions return true if rrt2 can add the given node
260  // in the tree. In this case, this would imply that the two trees meet.
261  bool treesMet = false;
262  const Eigen::VectorXd& rrt2target = *(rrt1->configVector[rrt1->activeNode]);
263  if(connect) treesMet = rrt2->connect(rrt2target);
264  else treesMet = (rrt2->tryStep(rrt2target) == R::STEP_REACHED);
265 
266  // Check if the trees have met and create the path, if so.
267  if(treesMet) {
268  start_rrt->tracePath(start_rrt->activeNode, path);
269  goal_rrt->tracePath(goal_rrt->activeNode, path, true);
270  return true;
271  }
272 
273  // Update the number of nodes in the two trees
274  numNodes = rrt1->getSize() + rrt2->getSize();
275 
276  // Print the gap between the trees in debug mode
277  if(debug) {
278  double gap = rrt2->getGap(*(rrt1->configVector[rrt1->activeNode]));
279  if(gap < smallestGap) {
280  smallestGap = gap;
281  std::cout << "Gap: " << smallestGap << " Sizes: " << start_rrt->configVector.size()
282  << "/" << goal_rrt->configVector.size() << std::endl;
283  }
284  }
285  }
286 
287  // Maximum # of iterations are reached and path is not found - failed.
288  return false;
289 }
290 
291 } // namespace planning
292 } // namespace dart
293 
294 #endif // DART_PLANNING_PATHPLANNER_HPP_
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:157
void setPositions(const Eigen::VectorXd &_positions)
Set the positions for all generalized coordinates.
Definition: MetaSkeleton.cpp:387
Eigen::VectorXd getPositions() const
Get the positions for all generalized coordinates.
Definition: MetaSkeleton.cpp:402
class Skeleton
Definition: Skeleton.hpp:59
The path planner class - a common interface to motion planners.
Definition: PathPlanner.hpp:61
virtual ~PathPlanner()
The destructor.
Definition: PathPlanner.hpp:87
PathPlanner(simulation::World &world, bool bidirectional_=true, bool connect_=true, double stepSize_=0.1, std::size_t maxNodes_=1e6, double goalBias_=0.3)
The desired constructor - you should use this one.
Definition: PathPlanner.hpp:81
bool planBidirectionalRrt(dynamics::Skeleton *robot, const std::vector< int > &dofs, const std::vector< Eigen::VectorXd > &start, const std::vector< Eigen::VectorXd > &goal, std::list< Eigen::VectorXd > &path)
Performs bidirectional RRT with the given options.
Definition: PathPlanner.hpp:222
std::size_t maxNodes
Maximum number of iterations the sampling would continue.
Definition: PathPlanner.hpp:68
R * goal_rrt
The second rrt if bidirectional search is executed.
Definition: PathPlanner.hpp:73
bool planPath(dynamics::Skeleton *robot, const std::vector< int > &dofs, const Eigen::VectorXd &start, const Eigen::VectorXd &goal, std::list< Eigen::VectorXd > &path)
Plan a path from a single start configuration to a single goal.
Definition: PathPlanner.hpp:90
simulation::WorldPtr world
The world that the robot is in (for obstacles and etc.)
Definition: PathPlanner.hpp:69
bool bidirectional
Whether two trees try to meet each other or one goes for the goal.
Definition: PathPlanner.hpp:65
double goalBias
Choose btw goal and random value (for goal-biased search)
Definition: PathPlanner.hpp:67
PathPlanner()
The default constructor.
Definition: PathPlanner.hpp:78
double stepSize
Step size from a node in the tree to the random/goal node.
Definition: PathPlanner.hpp:66
bool planSingleTreeRrt(dynamics::Skeleton *robot, const std::vector< int > &dofs, const std::vector< Eigen::VectorXd > &start, const Eigen::VectorXd &goal, std::list< Eigen::VectorXd > &path)
Performs a unidirectional RRT with the given options.
Definition: PathPlanner.hpp:177
bool connect
Whether we take a small step or shoot for the target node.
Definition: PathPlanner.hpp:64
R * start_rrt
The rrt for unidirectional search.
Definition: PathPlanner.hpp:72
class World
Definition: World.hpp:83
std::shared_ptr< World > WorldPtr
Definition: SmartPointer.hpp:41
Definition: BulletCollisionDetector.cpp:63
const bool debug
Definition: urdf_world_parser.cpp:52