PathPlanner.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2021, 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 <cstdio>
#include <iostream>
#include <limits>
#include <list>
#include <vector>
#include <Eigen/Core>
#include "dart/dynamics/Skeleton.hpp"
#include "dart/planning/RRT.hpp"
#include "dart/simulation/World.hpp"
 
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_