RRT.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
 *
 * 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 <vector>
#include <list>
#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 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_