RRT.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
 *
 * 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.
 *     * Neither the name of the Georgia Tech Research Corporation nor
 *       the names of its contributors may be used to endorse or
 *       promote products derived from this software without specific
 *       prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''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 GEORGIA
 * TECH RESEARCH CORPORATION 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_RRT_HPP_
#define DART_PLANNING_RRT_HPP_
 
#include <list>
#include <vector>
#include <Eigen/Core>
 
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/simulation/World.hpp"
 
namespace flann {
template <class A>
class L2;
template <class A>
class Index;
} // namespace flann
 
namespace dart {
 
namespace simulation {
class World;
}
namespace dynamics {
class Skeleton;
}
 
namespace planning {
 
class RRT
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef enum
  {
    STEP_COLLISION, // Collided with obstacle. No node added.
    STEP_REACHED,   // The target node is closer than step size (so reached). No
                    // node added.
    STEP_PROGRESS   // One node added.
  } StepResult;
 
public:
  // Initialization constants and search variables
 
  const int
      ndim; 
  const double stepSize; 
 
  int activeNode; 
  std::vector<int> parentVector; 
 
  // NOTE We are using pointers for the VectorXd's because flann copies the
  // pointers for the data points and we give it the copies made in the heap
  std::vector<const Eigen::VectorXd*> configVector;
 
public:
  RRT(dart::simulation::WorldPtr world,
      dart::dynamics::SkeletonPtr robot,
      const std::vector<std::size_t>& dofs,
      const Eigen::VectorXd& root,
      double stepSize = 0.02);
 
  RRT(simulation::WorldPtr world,
      dynamics::SkeletonPtr robot,
      const std::vector<std::size_t>& dofs,
      const std::vector<Eigen::VectorXd>& roots,
      double stepSize = 0.02);
 
  virtual ~RRT()
  {
  }
 
  bool connect();
 
  bool connect(const Eigen::VectorXd& target);
 
  StepResult tryStep();
 
  StepResult tryStep(const Eigen::VectorXd& qtry);
 
  virtual StepResult tryStepFromNode(const Eigen::VectorXd& qtry, int NNidx);
 
  virtual bool newConfig(
      std::list<Eigen::VectorXd>& intermediatePoints,
      Eigen::VectorXd& qnew,
      const Eigen::VectorXd& qnear,
      const Eigen::VectorXd& qtarget);
 
  double getGap(const Eigen::VectorXd& target);
 
  void tracePath(
      int node, std::list<Eigen::VectorXd>& path, bool reverse = false);
 
  std::size_t getSize();
 
  virtual bool checkCollisions(const Eigen::VectorXd& c);
 
  virtual Eigen::VectorXd getRandomConfig();
 
protected:
  simulation::WorldPtr world; 
  dynamics::SkeletonPtr
      robot; 
  std::vector<std::size_t>
      dofs; 
 
  flann::Index<flann::L2<double> >* index;
 
  double randomInRange(double min, double max);
 
  virtual int getNearestNeighbor(const Eigen::VectorXd& qsamp);
 
  virtual int addNode(const Eigen::VectorXd& qnew, int parentId);
};
 
} // namespace planning
} // namespace dart
 
#endif // DART_PLANNING_RRT_HPP_