RRT working

ccrrt/ConstrainedRRT.h

@@ -0,0 +1,31 @@
+#ifndef CONSTRAINEDRRT_H
+#define CONSTRAINEDRRT_H
+
+#include "../ccrrt/RRTManager.h"
+#include "../ccrrt/Constraint.h"
+
+namespace ccrrt {
+
+class ConstrainedRRT : public RRTManager
+{
+public:
+
+    ConstrainedRRT(int maxTreeSize=10000, 
+                   double maxStepSize=0.1, 
+                   double collisionCheckStepSize=0.1);
+
+    bool collisionChecker(JointConfig& config, const JointConfig& parentConfig);
+
+    void setConstraint(Constraint* constraint);
+
+protected:
+
+    Trajectory _col;
+    Constraint* _constraint;
+    Eigen::VectorXd _cost;
+
+};
+
+} // namespace ccrrt
+
+#endif // CONSTRAINEDRRT_H

ccrrt/Drawer.h

@@ -4,6 +4,7 @@
 #include "osgAkin/Line.h"
 #include "../ccrrt/Trajectory.h"
 #include "../ccrrt/CircleConstraint.h"
+#include "../ccrrt/RRTManager.h"
 
 namespace ccrrt {
 
@@ -12,13 +13,25 @@ class Drawer
 public:
 
     Drawer();
-//    ~Drawer();
 
-    void draw_trajectory(const Trajectory& traj);
-    void draw_trajectory(const Trajectory& traj, const osg::Vec4& color);
+    void draw_trajectory(const Trajectory& traj, 
+                         const osg::Vec4& color=osg::Vec4(0.1f,0.1f,0.1f,1.0f));
+
+    void draw_path(const ConfigPath& path, 
+                   const osg::Vec4& color=osg::Vec4(0.2,0.4,0.2,1.0));
+
+    void draw_rrts(const RRTManager& mgr,
+                   const osg::Vec4& tree_color=osg::Vec4(0.2,0.7,0.2,1.0),
+                   const osg::Vec4& path_color=osg::Vec4(0.2,0.4,0.2,1.0));
+
+    void draw_tree(const RRTNode& root_node,
+                   const osg::Vec4& color=osg::Vec4(0.2,0.7,0.2,1.0));
+
     void draw_circle(const CircleConstraint& circle);
+
     void draw_vector(const Eigen::Vector2d& vec, const Eigen::Vector2d& origin);
 
+
     void run();
 
 protected:

ccrrt/RRTManager.h

@@ -129,7 +129,7 @@ class RRTNode
 public:
 
     JointConfig config;
-    JointConfig getConfig() const;
+    const JointConfig& getConfig() const;
 
     RRTNode(JointConfig mConfig, double maxStepSize=0.3, RRTNode* mParent = NULL);
 
@@ -141,10 +141,10 @@ class RRTNode
     bool hasParent() const;
 
     // If the node does not have a child, this will return itself
-    const RRTNode* getChild(int child) const;
+    const RRTNode* getChild(size_t child) const;
     // Always check for hasChild() before calling getChild(~)
-    bool hasChild();
-    int numChildren();
+    bool hasChild() const;
+    size_t numChildren() const;
 
     // Identify the node that's closest and scale down newConfig to be
     // an acceptable child to that node
@@ -190,7 +190,9 @@ class RRTNode
 class RRTManager
 {
 public:
-    RRTManager(int maxTreeSize=10000, double maxStepSize=0.3, double collisionCheckStepSize=0.3);
+    RRTManager(int maxTreeSize=10000,
+               double maxStepSize=0.3,
+               double collisionCheckStepSize=0.3);
 
     void setDomain(const JointConfig& minJointConfig,
                    const JointConfig& maxJointConfig,
@@ -241,19 +243,22 @@ class RRTManager
     // Resolution that should be used for partitioning the domain
     int dResolution;
 
-    int getTreeSize(int tree);
-    RRT_Tree_t getTreeType(int tree);
-    int getTotalNodes();
-    int getNumIterations();
+
+    int getTreeSize(size_t tree) const;
+    RRT_Tree_t getTreeType(size_t tree) const;
+    int getTotalNodes() const;
+    int getNumIterations() const;
+    size_t getNumTrees() const;
+    const RRTNode* getTree(size_t tree) const;
 
     // Check if a particular tree is maxed out
-    bool checkIfMaxed(int tree);
+    bool checkIfMaxed(int tree) const;
     // Check if all the trees are maxed out
-    bool checkIfAllMaxed();
+    bool checkIfAllMaxed() const;
     // Check if any of the trees are maxed out
-    bool checkIfAnyMaxed();
+    bool checkIfAnyMaxed() const;
     // Check if the manager has identified a solution yet
-    bool checkIfSolved();
+    bool checkIfSolved() const;
 
     void randomizeConfig(JointConfig& config);
     void scaleConfig(JointConfig& config, const JointConfig& parentConfig);
@@ -271,18 +276,18 @@ class RRTManager
 
     RRTNode* lookForTreeConnection(const RRTNode* targetNode, RRT_Tree_t connectTargetType);
     void constructSolution(const RRTNode* beginTree, const RRTNode* endTree);
-    bool hasSolution_;
+    bool _hasSolution;
 
     JointConfig minConfig;
     JointConfig maxConfig;
-    int domainSize;
-    bool hasDomain_;
-    int iterations_;
+    int _domainSize;
+    bool _hasDomain;
+    int _iterations;
 
-    double numPrecThresh;
+    double _numPrecThresh;
 
     // Maximum step size through the jointspace
-    double maxStepSize_;
+    double _maxStepSize;
 
     std::vector<int> currentTreeSize;
 

src/ConstrainedRRT.cpp

@@ -0,0 +1,39 @@
+
+#include "../ccrrt/ConstrainedRRT.h"
+
+using namespace ccrrt;
+
+ConstrainedRRT::ConstrainedRRT(int maxTreeSize, 
+                               double maxStepSize, 
+                               double collisionCheckStepSize) :
+    RRTManager(maxTreeSize, maxStepSize, collisionCheckStepSize)
+{
+    _constraint = NULL;
+}
+
+bool ConstrainedRRT::collisionChecker(JointConfig& config, const JointConfig& parentConfig)
+{
+    if(_constraint==NULL)
+        return true;
+
+    double dist = (parentConfig-config).norm();
+    _col.state_space = config.size();
+    _col.waypoints = ceil(dist/collisionCheckStepSize_);
+    _col.start = parentConfig;
+    _col.end = config;
+    _col.xi.resize(_col.state_space*_col.waypoints);
+
+    for(size_t i=0; i<_col.waypoints; ++i)
+    {
+        _col.xi.block(i*_col.state_space,0,_col.state_space,1) =
+                (config-parentConfig)*(double)(i+1)/(double)(_col.waypoints)
+                + parentConfig;
+    }
+
+    return _constraint->getCost(_cost, _col) != Constraint::INVALID;
+}
+
+void ConstrainedRRT::setConstraint(Constraint *constraint)
+{
+    _constraint = constraint;
+}

src/Drawer.cpp

@@ -17,11 +17,6 @@ Drawer::Drawer()
     viewer.setSceneData(_root);
 }
 
-void Drawer::draw_trajectory(const Trajectory &traj)
-{
-    draw_trajectory(traj, osg::Vec4(0.1f,0.1f,0.1f,1.0f));
-}
-
 void Drawer::draw_trajectory(const Trajectory &traj, const osg::Vec4 &color)
 {
     osgAkin::Line* line = new osgAkin::Line(
@@ -39,7 +34,75 @@ void Drawer::draw_trajectory(const Trajectory &traj, const osg::Vec4 &color)
     _geode->addDrawable(line);
 }
 
-void Drawer::draw_circle(const CircleConstraint &circle)
+void Drawer::draw_path(const ConfigPath &path, const osg::Vec4 &color)
+{
+    osgAkin::Line* line = new osgAkin::Line;
+
+    line->setColor(color);
+
+    for(size_t i=0; i < path.size(); ++i)
+    {
+        line->addVertex(akin::Translation(path[i][0],0,path[i][1]));
+    }
+
+    line->updateVertices();
+    _geode->addDrawable(line);
+}
+
+void Drawer::draw_tree(const RRTNode &root_node, const osg::Vec4 &color)
+{
+    std::map<const RRTNode*,ushort> node_map;
+    osgAkin::LineTree* tree = new osgAkin::LineTree;
+    tree->setColor(color);
+    std::vector<size_t> tracker;
+    tracker.push_back(0);
+    size_t depth = 0;
+    const RRTNode* current_node = &root_node;
+
+    node_map[&root_node] = tree->addVertex(akin::Translation(
+                                               root_node.getConfig()[0],0,
+                                           root_node.getConfig()[1]),-1);
+
+    while(tracker.size() > 0)
+    {
+        if(tracker[depth] < current_node->numChildren())
+        {
+            current_node = current_node->getChild(tracker[depth]);
+            node_map[current_node] = tree->addVertex(akin::Translation(
+                                        current_node->getConfig()[0],0,
+                                        current_node->getConfig()[1]),
+                                     node_map[current_node->getParent()]);
+
+//            std::cout << "D:" << depth << " T:" << tracker[depth] 
+//                      << " C:" << current_node->numChildren() << " | " 
+//                      << current_node->getConfig().transpose() << std::endl;
+
+            ++tracker[depth];
+            tracker.push_back(0);
+            ++depth;
+        }
+        else
+        {
+            current_node = current_node->getParent();
+            tracker.pop_back();
+            --depth;
+        }
+    }
+
+    tree->updateVertices();
+    _geode->addDrawable(tree);
+}
+
+void Drawer::draw_rrts(const RRTManager& mgr,
+                       const osg::Vec4& tree_color,
+                       const osg::Vec4& path_color)
+{
+    draw_path(mgr.solvedPlan, path_color);
+    for(size_t i=0; i<mgr.getNumTrees(); ++i)
+        draw_tree(*mgr.getTree(i),tree_color);
+}
+
+void Drawer::draw_circle(const CircleConstraint& circle)
 {
     osgAkin::Line* line = new osgAkin::Line;