rapidly_exploring_random_tree.h

Go to the documentation of this file.

#ifndef RAPIDLY_EXPLORING_RANDOM_TREE_H
#define RAPIDLY_EXPLORING_RANDOM_TREE_H
 
#include "cubic_engine/base/cubic_engine_types.h"
#include "kernel/data_structs/boost_serial_graph.h"
#include "kernel/base/kernel_consts.h"
 
#include"boost/noncopyable.hpp"
#include <chrono>
#include <iostream>
 
 
namespace cengine {
namespace search {
 
template<typename NodeData, typename EdgeData>
class RRT: private boost::noncopyable
{
public:
 
    typedef NodeData vertex_data_t;
 
    typedef EdgeData edge_data_t;
 
    typedef typename kernel::BoostSerialGraph<vertex_data_t,
                                              edge_data_t>::vertex_t vertex_t;
 
    typedef typename kernel::BoostSerialGraph<vertex_data_t,
                                              edge_data_t>::edge_t edge_t;
 
    typedef typename kernel::BoostSerialGraph<vertex_data_t,
                                              edge_data_t>::edge_iterator edge_iterator;
 
    typedef typename kernel::BoostSerialGraph<vertex_data_t,
                                              edge_data_t>::adjacency_iterator adjacency_iterator;
 
    RRT();
 
    vertex_t& get_vertex(uint_t v){ return tree_.get_vertex(v);}
 
    const vertex_t& get_vertex(uint_t v)const{return tree_.get_vertex(v);}
 
    vertex_t& get_vertex(adjacency_iterator itr){return tree_.get_vertex(itr);}
 
    const vertex_t& get_vertex(adjacency_iterator itr)const{return  tree_.get_vertex(itr);}
 
    std::pair<adjacency_iterator, adjacency_iterator>
    get_vertex_neighbors(uint_t id)const{
        return tree_.get_vertex_neighbors(id);
    }
 
    std::pair<adjacency_iterator, adjacency_iterator>
    get_vertex_neighbors(const vertex_t& v)const{
        return tree_.get_vertex_neighbors(v);
    }
 
    vertex_t& add_vertex(const vertex_t& node){ return tree_.add_vertex(node.data);}
 
    vertex_t& add_vertex(const vertex_data_t& data);
 
    edge_t& get_edge(uint_t v1, uint_t v2){
        return tree_.get_edge(v1, v2);
    }
 
    const edge_t& get_edge(uint_t v1, uint_t v2)const{
        return tree_.get_edge(v1, v2);
    }
 
    std::pair<edge_iterator, edge_iterator> edges()const{
        return tree_.edges();
    }
 
    edge_t& add_edge(uint_t v1, uint_t v2){
        return tree_.add_edge(v1, v2);
    }
 
    template<typename MetricTp>
    const vertex_t& find_nearest_neighbor(const vertex_t& other,
                                          const MetricTp& metric)const;
 
    template<typename MetricTp>
    const vertex_t& find_nearest_neighbor(const vertex_data_t& other,
                                          const MetricTp& metric)const;
 
    void clear(){tree_.clear();}
 
    uint_t n_vertices()const{return tree_.n_vertices();}
 
    uint_t n_edges()const{return tree_.n_edges();}
 
    void set_show_iterations_flag(bool val){show_iterations_ = val;}
 
    template<typename StateSelector,
             typename MetricTp, typename DynamicsTp>
    void build(uint_t nitrs, const vertex_t& xinit,
               const  StateSelector& state_selector,
               const MetricTp& metric,
               DynamicsTp& dynamics);
 
    template<typename StateSelector, typename MetricTp, typename DynamicsTp>
    std::tuple<bool, uint_t, uint_t>
    build(uint_t nitrs, const vertex_t& xinit,
               const vertex_t& goal, const  StateSelector& state_selector,
               const MetricTp& metric, DynamicsTp& dynamics, real_t goal_radius);
 
private:
 
    kernel::BoostSerialGraph<vertex_data_t, edge_data_t> tree_;
 
    bool show_iterations_;
 
};
 
template<typename NodeData, typename EdgeData>
RRT<NodeData, EdgeData>::RRT()
    :
      tree_(),
      show_iterations_(false)
{}
 
template<typename NodeData, typename EdgeData>
typename RRT<NodeData, EdgeData>::vertex_t&
RRT<NodeData, EdgeData>::add_vertex(const vertex_data_t& data){
    return tree_.add_vertex(data);
}
 
template<typename NodeTp, typename EdgeTp>
template<typename StateSelector, typename MetricTp, typename DynamicsTp>
void
RRT<NodeTp, EdgeTp>::build(uint_t nitrs, const vertex_t& xinit,
                           const  StateSelector& state_selector,
                           const MetricTp& metric,
                           DynamicsTp& dynamics){
 
    std::chrono::time_point<std::chrono::system_clock> start, end;
    start = std::chrono::system_clock::now();
 
    // just in case clear what the tree has
    clear();
 
    // initialize the tree. This is the root node
    tree_.add_vertex(xinit.data);
 
    // loop over the states and create
    // the tree
    for(uint_t itr=0; itr<nitrs; ++itr){
 
        if(show_iterations_){
            std::cout<<"At iteration: "<<itr<<std::endl;
        }
 
        // select a new random state
        auto xrand = state_selector();
 
        // find the nearest neighbor between this tree
        // and the randomly selected state
        auto& xnear = find_nearest_neighbor(xrand, metric);
 
        // determine the new state. Move xnear
        // towards xrand according to the prescribed dynamics
        // also return the data required for the edge
        // connecting xnew and xnear
        auto [xnew, u] = dynamics(xnear, xrand);
 
        // add a new vertex out of xnew
        auto& new_v = add_vertex(xnew);
 
        // add a new edge
        auto new_e = add_edge(xnear.id, new_v.id);
        new_e.set_data(u);
    }
 
    end = std::chrono::system_clock::now();
 
    if(show_iterations_){
        std::chrono::duration<real_t> dur = end - start;
        std::cout<<"Total build time: "<<dur.count()<<std::endl;
    }
}
 
template<typename NodeData, typename EdgeData>
template<typename StateSelector, typename MetricTp, typename DynamicsTp>
std::tuple<bool, uint_t, uint_t>
RRT<NodeData, EdgeData>::build(uint_t nitrs, const vertex_t& xinit,
                               const vertex_t& goal, const  StateSelector& state_selector,
                               const MetricTp& metric, DynamicsTp& dynamics, real_t goal_radius){
 
 
    std::chrono::time_point<std::chrono::system_clock> start, end;
    start = std::chrono::system_clock::now();
 
    // just in case clear what the tree has
    clear();
 
    // start and goal are the same
    // then there is nothing to do
    if( metric(xinit, goal) < goal_radius ){
        return std::make_tuple(true, 0, 0);
    }
 
    // initialize the tree. This is the root node
    auto& root = tree_.add_vertex(xinit.data);
 
    // flag indicating that the goal is found
    bool goal_found = false;
 
    // the uid of the last vertex
    uint_t last_v_id = kernel::KernelConsts::invalid_size_type();
 
    // loop over the states and create
    // the tree
    for(uint_t itr=0; (itr<nitrs && !goal_found); ++itr){
 
        //if(show_iterations_){
        //    std::cout<<"At iteration: "<<itr<<std::endl;
        //}
 
        // select a new random state
        auto xrand = state_selector();
 
        // find the nearest neighbor between this tree
        // and the randomly selected state
        auto& xnear = find_nearest_neighbor(xrand, metric);
 
        // determine the new state. Move xnear
        // towards xrand according to the prescribed dynamics
        // also return the data required for the edge
        // connecting xnew and xnear
        auto [xnew, u] = dynamics(xnear, xrand);
 
        // add a new vertex out of xnew
        auto& new_v = add_vertex(xnew);
 
        // add a new edge
        auto new_e = add_edge(xnear.id, new_v.id);
        new_e.set_data(u);
 
        // if this new node is the goal then
        // exit the loop
        if(metric(new_v, goal) < goal_radius ){
            last_v_id =  new_v.id;
            goal_found = true;
            break;
        }
    }
 
    end = std::chrono::system_clock::now();
 
    //if(show_iterations_){
        std::chrono::duration<real_t> dur = end - start;
        std::cout<<"Total build time: "<<dur.count()<<std::endl;
    //}
 
    return std::make_tuple(goal_found, root.id, last_v_id);
}
 
template<typename NodeData, typename EdgeData>
template<typename MetricTp>
const typename RRT<NodeData, EdgeData>::vertex_t&
RRT<NodeData, EdgeData>::find_nearest_neighbor(const vertex_t& other,
                                               const MetricTp& metric)const{
 
    auto dist = metric(tree_.get_vertex(0), other);
    auto result = 0;
 
    for(uint_t v=1; v< tree_.n_vertices(); ++v){
        auto vertex_data = tree_.get_vertex(v);
 
        auto new_dist = metric(vertex_data, other);
 
        if(new_dist < dist){
            dist = new_dist;
            result = v;
        }
    }
 
    return tree_.get_vertex(result);
}
 
template<typename NodeData, typename EdgeData>
template<typename MetricTp>
const typename RRT<NodeData, EdgeData>::vertex_t&
RRT<NodeData, EdgeData>::find_nearest_neighbor(const vertex_data_t& other,
                                               const MetricTp& metric)const{
 
    typedef typename RRT<NodeData, EdgeData>::vertex_t vertex_t;
    vertex_t dummy;
    dummy.data = other;
 
    auto dist = metric(tree_.get_vertex(0), dummy);
    auto result = 0;
 
    for(uint_t v=1; v< tree_.n_vertices(); ++v){
        auto vertex_data = tree_.get_vertex(v);
 
        auto new_dist = metric(vertex_data, dummy);
 
        if(new_dist < dist){
            dist = new_dist;
            result = v;
        }
    }
 
    return tree_.get_vertex(result);
 
}
 
 
 
}
 
}
 
#endif // RAPIDLY_EXPLORING_RANDOM_TREE_H