Skip to content

Instantly share code, notes, and snippets.

@giogadi

giogadi/RRG.h

Last active December 19, 2015 15:19
Download ZIP

Revisions

  1. giogadi revised this gist Jul 31, 2013. 1 changed file with 6 additions and 53 deletions.
    59 changes: 6 additions & 53 deletions RRG.h
    View file
    Original file line number Diff line number Diff line change
    @@ -104,26 +104,10 @@ namespace ompl

    protected:

    class CostAux
    {
    public:
    CostAux(); // UGH
    CostAux(const CostAux& rhs);
    CostAux(const base::Cost* cost);
    ~CostAux();

    void operator=(const CostAux& rhs);
    bool operator<(const CostAux& rhs) const;
    CostAux operator+(const CostAux& rhs) const;

    base::Cost* c;
    static const base::OptimizationObjective* opt;
    };

    typedef boost::adjacency_list<boost::vecS,
    boost::vecS,
    boost::directedS,
    base::State*, CostAux> Graph;
    base::State*, base::Cost> Graph;

    typedef boost::graph_traits<Graph>::vertex_descriptor Node;
    typedef boost::graph_traits<Graph>::edge_descriptor Edge;
    @@ -161,7 +145,7 @@ namespace ompl
    base::PathPtr getBestPath(Node startNode, const base::GoalPtr& goal);

    /** \brief Free the memory allocated by this planner */
    void freeMemory(void);
    void freeMemory(void);

    /** \brief Compute distance between nodes (actually distance between contained states) */
    double distanceFunction(DistItem a, DistItem b) const
    @@ -193,43 +177,13 @@ namespace ompl
    /** \brief Objective we're optimizing */
    base::OptimizationObjectivePtr opt_;

    class CostCompareFctr
    {
    public:
    bool operator()(const CostAux& c1, const CostAux& c2) const;
    // bool operator()(const base::Cost* c1, const CostAux& c2) const;
    };

    class CostCombineFctr
    {
    public:
    CostAux operator()(const CostAux& c1, const CostAux& c2) const;
    // CostAux operator()(const CostAux& c1, const base::Cost* c2) const;
    // CostAux operator()(const base::Cost* c1, const CostAux& c2) const;
    };

    // NOTE: THIS IS SUPER NOT-GENERAL AT ALL because goal
    // might not be a GoalRegion
    class CostToGoFctr
    {
    public:
    CostToGoFctr(const Graph& g,
    const base::PathIntegralOptimizationObjective* opt,
    const base::GoalRegion* goal) :
    g_(g), opt_(opt), goal_(goal)
    {}
    CostAux operator() (Node v);

    const Graph& g_;
    const base::PathIntegralOptimizationObjective* opt_;
    const base::GoalRegion* goal_;
    };
    base::Cost costToGo(Node v, const base::Goal* goal) const;

    struct found_goal
    {
    found_goal(Node u) : goalNode(u) {}
    Node goalNode;
    };
    };

    // TOOOOTALLY ASSUMES THAT WE'RE WORKING WITH A GOAL REGION
    class astar_goal_visitor : public boost::default_astar_visitor
    @@ -267,10 +221,9 @@ namespace ompl

    // persistent vectors for use in A*
    std::vector<Node> predMap_;
    std::vector<CostAux> vertexCostMap_;

    std::vector<base::Cost> vertexCostMap_;
    };
    }
    }

    #endif
    #endif
  2. giogadi revised this gist Jul 11, 2013. 1 changed file with 0 additions and 34 deletions.
    34 changes: 0 additions & 34 deletions RRG.h
    View file
    Original file line number Diff line number Diff line change
    @@ -1,37 +1,3 @@
    /*********************************************************************
    * Software License Agreement (BSD License)
    *
    * Copyright (c) 2011, Rice University
    * All rights reserved.
    *
    * 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 Rice University 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 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 OWNER 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.
    *********************************************************************/

    /* Authors: Luis G. Torres */

    #ifndef OMPL_CONTRIB_RRT_STAR_RRG_
  3. giogadi created this gist Jul 11, 2013.
    310 changes: 310 additions & 0 deletions RRG.h
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,310 @@
    /*********************************************************************
    * Software License Agreement (BSD License)
    *
    * Copyright (c) 2011, Rice University
    * All rights reserved.
    *
    * 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 Rice University 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 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 OWNER 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.
    *********************************************************************/

    /* Authors: Luis G. Torres */

    #ifndef OMPL_CONTRIB_RRT_STAR_RRG_
    #define OMPL_CONTRIB_RRT_STAR_RRG_

    #include "ompl/geometric/planners/PlannerIncludes.h"
    #include "ompl/base/OptimizationObjective.h"
    #include "ompl/datastructures/NearestNeighbors.h"
    #include "ompl/base/goals/GoalRegion.h"

    #include <boost/graph/adjacency_list.hpp>
    #include <boost/graph/astar_search.hpp>

    namespace ompl
    {
    namespace geometric
    {
    class RRG : public base::Planner
    {
    public:
    RRG(const base::SpaceInformationPtr &si);

    virtual ~RRG(void);

    void preprocess(unsigned numIterations);

    virtual void getPlannerData(base::PlannerData &data) const;

    virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);

    virtual void clear(void);

    /** \brief Set the range the planner is supposed to use.
    This parameter greatly influences the runtime of the
    algorithm. It represents the maximum length of a
    motion to be added in the tree of motions. */
    void setRange(double distance)
    {
    maxDistance_ = distance;
    }

    /** \brief Get the range the planner is using */
    double getRange(void) const
    {
    return maxDistance_;
    }

    /** \brief When the planner attempts to refine the graph,
    it does so by looking at some of the neighbors within
    a computed radius. The computation of that radius
    depends on the multiplicative factor set here.
    Set this parameter should be set at least to the side
    length of the (bounded) state space. E.g., if the state
    space is a box with side length L, then this parameter
    should be set to at least L for rapid and efficient
    convergence in trajectory space. */
    void setBallRadiusConstant(double ballRadiusConstant)
    {
    ballRadiusConst_ = ballRadiusConstant;
    }

    /** \brief Get the multiplicative factor used in the
    computation of the radius whithin which tree rewiring
    is done. */
    double getBallRadiusConstant(void) const
    {
    return ballRadiusConst_;
    }

    /** \brief When the planner attempts to refine the graph,
    it does so by looking at some of the neighbors within
    a computed radius. That radius is bounded by the value
    set here. This parameter should ideally be equal longest
    straight line from the initial state to anywhere in the
    state space. In other words, this parameter should be
    "sqrt(d) L", where d is the dimensionality of space
    and L is the side length of a box containing the obstacle free space. */
    void setMaxBallRadius(double maxBallRadius)
    {
    ballRadiusMax_ = maxBallRadius;
    }

    /** \brief Get the maximum radius the planner uses in the
    tree rewiring step */
    double getMaxBallRadius(void) const
    {
    return ballRadiusMax_;
    }

    /** \brief Set a different nearest neighbors datastructure */
    template<template<typename T> class NN>
    void setNearestNeighbors(void)
    {
    nn_.reset(new NN<DistItem>());
    }

    virtual void setup(void);

    unsigned getNumCollisionChecks(void) const
    {
    return numCollisionChecks_;
    }

    protected:

    class CostAux
    {
    public:
    CostAux(); // UGH
    CostAux(const CostAux& rhs);
    CostAux(const base::Cost* cost);
    ~CostAux();

    void operator=(const CostAux& rhs);
    bool operator<(const CostAux& rhs) const;
    CostAux operator+(const CostAux& rhs) const;

    base::Cost* c;
    static const base::OptimizationObjective* opt;
    };

    typedef boost::adjacency_list<boost::vecS,
    boost::vecS,
    boost::directedS,
    base::State*, CostAux> Graph;

    typedef boost::graph_traits<Graph>::vertex_descriptor Node;
    typedef boost::graph_traits<Graph>::edge_descriptor Edge;

    struct DistItem
    {
    const base::State *state;
    Node node;
    bool operator==(const DistItem& rhs) const
    {
    return (this->state == rhs.state) && (this->node == rhs.node);
    }
    bool operator!=(const DistItem& rhs) const
    {
    return !(*this == rhs);
    }
    };

    void initPreprocess(void);

    void initSolve(void);

    bool extend(const base::State *s, Node* newNode, Node* nearNode);

    void connectNear(Node node, Node nearNode);

    double getNeighborhoodRadius() const
    {
    return std::min(ballRadiusConst_ *
    pow(log((double)(1 + nn_->size())) / (double)(nn_->size()),
    1.0 / (double)si_->getStateSpace()->getDimension()),
    ballRadiusMax_);
    }

    base::PathPtr getBestPath(Node startNode, const base::GoalPtr& goal);

    /** \brief Free the memory allocated by this planner */
    void freeMemory(void);

    /** \brief Compute distance between nodes (actually distance between contained states) */
    double distanceFunction(DistItem a, DistItem b) const
    {
    return si_->distance(a.state, b.state);
    }

    /** \brief Graph representing the roadmap */
    Graph graph_;

    /** \brief State sampler */
    base::StateSamplerPtr sampler_;

    /** \brief A nearest-neighbors datastructure containing the graph nodes */
    boost::shared_ptr< NearestNeighbors<DistItem> > nn_;

    /** \brief The maximum length of a motion to be added to the graph */
    double maxDistance_;

    /** \brief Shrink rate of radius the planner uses to find near neighbors */
    double ballRadiusConst_;

    /** \brief Maximum radius the planner uses to find near neighbors */
    double ballRadiusMax_;

    /** \brief Total number of calls to checkMotion() during execution */
    unsigned numCollisionChecks_;

    /** \brief Objective we're optimizing */
    base::OptimizationObjectivePtr opt_;

    class CostCompareFctr
    {
    public:
    bool operator()(const CostAux& c1, const CostAux& c2) const;
    // bool operator()(const base::Cost* c1, const CostAux& c2) const;
    };

    class CostCombineFctr
    {
    public:
    CostAux operator()(const CostAux& c1, const CostAux& c2) const;
    // CostAux operator()(const CostAux& c1, const base::Cost* c2) const;
    // CostAux operator()(const base::Cost* c1, const CostAux& c2) const;
    };

    // NOTE: THIS IS SUPER NOT-GENERAL AT ALL because goal
    // might not be a GoalRegion
    class CostToGoFctr
    {
    public:
    CostToGoFctr(const Graph& g,
    const base::PathIntegralOptimizationObjective* opt,
    const base::GoalRegion* goal) :
    g_(g), opt_(opt), goal_(goal)
    {}
    CostAux operator() (Node v);

    const Graph& g_;
    const base::PathIntegralOptimizationObjective* opt_;
    const base::GoalRegion* goal_;
    };

    struct found_goal
    {
    found_goal(Node u) : goalNode(u) {}
    Node goalNode;
    };

    // TOOOOTALLY ASSUMES THAT WE'RE WORKING WITH A GOAL REGION
    class astar_goal_visitor : public boost::default_astar_visitor
    {
    public:
    astar_goal_visitor(const base::GoalRegion* goal,
    double* nearestGoalDist,
    Node* nearestToGoal) :
    goal(goal),
    nearestGoalDist(nearestGoalDist),
    nearestToGoal(nearestToGoal)
    {
    *nearestGoalDist = std::numeric_limits<double>::infinity();
    *nearestToGoal = boost::graph_traits<Graph>::null_vertex();
    }

    void examine_vertex(Node u, const Graph& g)
    {
    double dist;
    if (goal->isSatisfied(g[u], &dist))
    {
    throw found_goal(u);
    }
    else if (dist < *nearestGoalDist)
    {
    *nearestGoalDist = dist;
    *nearestToGoal = u;
    }
    }

    const base::GoalRegion* goal;
    double* nearestGoalDist;
    Node* nearestToGoal;
    };

    // persistent vectors for use in A*
    std::vector<Node> predMap_;
    std::vector<CostAux> vertexCostMap_;

    };
    }
    }

    #endif