PathPlanner.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef DART_PLANNING_PATHPLANNER_HPP_
#define DART_PLANNING_PATHPLANNER_HPP_
 
#include <Eigen/Core>
#include <iostream>
#include <limits>
#include <list>
#include <vector>
#include "dart/dynamics/Skeleton.hpp"
#include "dart/simulation/World.hpp"
#include "dart/planning/RRT.hpp"
#include <cstdio>
 
namespace dart {
namespace planning {
 
/* ********************************************************************************************* */
template <class R = RRT>
class PathPlanner {
public:
 
  bool connect;            
  bool bidirectional;      
  double stepSize;        
  double goalBias;        
  std::size_t maxNodes;        
  simulation::WorldPtr world;  
 
  // NOTE: It is useful to keep the rrts around after planning for reuse, analysis, and etc.
  R* start_rrt;            
  R* goal_rrt;              
 
public:
 
  PathPlanner() : world(nullptr) {}
 
  PathPlanner(simulation::World& world, bool bidirectional_ = true, bool connect_ = true, double stepSize_ = 0.1,
    std::size_t maxNodes_ = 1e6, double goalBias_ = 0.3) : world(&world), bidirectional(bidirectional_),
    connect(connect_), stepSize(stepSize_), maxNodes(maxNodes_), goalBias(goalBias_) {
  }
 
  virtual ~PathPlanner() {}
 
  bool planPath(dynamics::Skeleton* robot, const std::vector<int> &dofs, const Eigen::VectorXd &start,
      const Eigen::VectorXd &goal, std::list<Eigen::VectorXd> &path) {
    std::vector<Eigen::VectorXd> startVector, goalVector;
    startVector.push_back(start);
    goalVector.push_back(goal);
    return planPath(robot, dofs, startVector, goalVector, path);
  }
 
  bool planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs, const std::vector<Eigen::VectorXd> &start,
    const std::vector<Eigen::VectorXd> &goal, std::list<Eigen::VectorXd> &path);
 
private:
 
  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);
 
  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);
};
 
/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::planPath(dynamics::Skeleton* robot, const std::vector<std::size_t> &dofs,
    const std::vector<Eigen::VectorXd> &start, const std::vector<Eigen::VectorXd> &goal,
    std::list<Eigen::VectorXd> &path) {
 
  Eigen::VectorXd savedConfiguration = robot->getPositions(dofs);
 
  // ====================================================================
  // Check for collisions in the start and goal configurations
 
  // Sift through the possible start configurations and eliminate those that are in collision
  std::vector<Eigen::VectorXd> feasibleStart;
  for(unsigned int i = 0; i < start.size(); i++) {
    robot->setPositions(dofs, start[i]);
    if(!world->checkCollision()) feasibleStart.push_back(start[i]);
  }
 
  // Return false if there are no feasible start configurations
  if(feasibleStart.empty()) {
    printf("WARNING: PathPlanner: Feasible start points are empty!\n");
    return false;
  }
 
  // Sift through the possible goal configurations and eliminate those that are in collision
  std::vector<Eigen::VectorXd> feasibleGoal;
  for(unsigned int i = 0; i < goal.size(); i++) {
    robot->setPositions(dofs, goal[i]);
    if(!world->checkCollision()) feasibleGoal.push_back(goal[i]);
  }
 
  // Return false if there are no feasible goal configurations
  if(feasibleGoal.empty()) {
    printf("WARNING: PathPlanner: Feasible goal points are empty!\n");
    return false;
  }
 
  // ====================================================================
  // Make the correct RRT algorithm for the given method
 
  // Direct the search towards single or bidirectional
  bool result = false;
  if(bidirectional)
    result = planBidirectionalRrt(robot, dofs, feasibleStart, feasibleGoal, path);
  else {
    if(feasibleGoal.size() > 1) fprintf(stderr, "WARNING: planPath is using ONLY the first goal!\n");
    result = planSingleTreeRrt(robot, dofs, feasibleStart, feasibleGoal.front(), path);
  }
 
  // Restore previous robot configuration
  robot->setPositions(dofs, savedConfiguration);
 
  return result;
}
 
/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::planSingleTreeRrt(dynamics::Skeleton* robot, const std::vector<int> &dofs,
    const std::vector<Eigen::VectorXd> &start, const Eigen::VectorXd &goal,
    std::list<Eigen::VectorXd> &path) {
 
  const bool debug = false;
 
  // Initialize the RRT
  start_rrt = new R (world, robot, dofs, start, stepSize);
 
  // Expand the tree until the goal is reached or the max # nodes is passed
  typename R::StepResult result = R::STEP_PROGRESS;
  double smallestGap = std::numeric_limits<double>::infinity();
  std::size_t numNodes = start_rrt->getSize();
  while(numNodes <= maxNodes) {
 
    // Get the target node based on the bias
    Eigen::VectorXd target;
    double randomValue = ((double) rand()) / RAND_MAX;
    if(randomValue < goalBias) target = goal;
    else target = start_rrt->getRandomConfig();
 
    // Based on the method, either attempt to connect to the target directly or take a small step
    if(connect) start_rrt->connect(target);
    else start_rrt->tryStep(target);
 
    // Check if the goal is reached and create the path, if so
    double gap = start_rrt->getGap(goal);
    if(gap < stepSize) {
      if(debug) std::cout << "Returning true, reached the goal" << std::endl;
      start_rrt->tracePath(start_rrt->activeNode, path);
      return true;
    }
 
    // Update the number of nodes
    numNodes = start_rrt->getSize();
  }
 
  if(debug) printf("numNodes: %lu\n", numNodes);
 
  // Maximum # of iterations are reached and path is not found - failed.
  return false;
}
 
/* ********************************************************************************************* */
template <class R>
bool PathPlanner<R>::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) {
 
  const bool debug = false;
 
  // Initialize both the start and goal RRTs.
  // NOTE: We use the pointers for the RRTs to swap their roles in extending towards a target
  // (random or goal) node.
  start_rrt = new R(world, robot, dofs, start, stepSize);
  goal_rrt = new R(world, robot, dofs, goal, stepSize);
  R* rrt1 = start_rrt;
  R* rrt2 = goal_rrt;
 
  // Expand the tree until the trees meet or the max # nodes is passed
  double smallestGap = std::numeric_limits<double>::infinity();
  std::size_t numNodes = rrt1->getSize() + rrt2->getSize();
  while(numNodes < maxNodes) {
 
    // Swap the roles of the two RRTs. Remember, the first rrt reaches out to a target node and
    // creates a new node and the second rrt reaches to _the new node_.
    R* temp = rrt1;
    rrt1 = rrt2;
    rrt2 = temp;
 
     // Get the target node based on the bias
    Eigen::VectorXd target;
    double randomValue = ((double) rand()) / RAND_MAX;
    if(randomValue < goalBias) target = goal[0];
    else target = rrt1->getRandomConfig();
 
    // Based on the method, rrt1 either attempt to connect to the target directly or takes a step
    if(connect) rrt1->connect(target);
    else rrt1->tryStep(target);
 
    // rrt2 uses the last added node of rrt1 as a target and reaches out to it (connect or step)
    // NOTE: If a node was not added, the nearest neighbor to the random node in rrt1 is used.
    // NOTE: connect(x) and tryStep(x) functions return true if rrt2 can add the given node
    // in the tree. In this case, this would imply that the two trees meet.
    bool treesMet = false;
    const Eigen::VectorXd& rrt2target = *(rrt1->configVector[rrt1->activeNode]);
    if(connect) treesMet = rrt2->connect(rrt2target);
    else treesMet = (rrt2->tryStep(rrt2target) == R::STEP_REACHED);
 
    // Check if the trees have met and create the path, if so.
    if(treesMet) {
      start_rrt->tracePath(start_rrt->activeNode, path);
      goal_rrt->tracePath(goal_rrt->activeNode, path, true);
      return true;
    }
 
    // Update the number of nodes in the two trees
    numNodes = rrt1->getSize() + rrt2->getSize();
 
    // Print the gap between the trees in debug mode
    if(debug) {
      double gap = rrt2->getGap(*(rrt1->configVector[rrt1->activeNode]));
      if(gap < smallestGap) {
        smallestGap = gap;
        std::cout << "Gap: " << smallestGap << "  Sizes: " << start_rrt->configVector.size()
          << "/" << goal_rrt->configVector.size() << std::endl;
      }
    }
  }
 
  // Maximum # of iterations are reached and path is not found - failed.
  return false;
}
 
} // namespace planning
} // namespace dart
 
#endif  // DART_PLANNING_PATHPLANNER_HPP_