cbirrt seems good

mxgrey · Jun 9, 2014 · 6031ff4 · 6031ff4
1 parent ac52d03
commit 6031ff4
Show file tree

Hide file tree

Showing 7 changed files with 102 additions and 61 deletions.
diff --git a/ccrrt/CBiRRT.h b/ccrrt/CBiRRT.h
@@ -22,6 +22,8 @@ class CBiRRT : public RRTManager
 
     bool constraintProjector(JointConfig &config, const JointConfig &parentConfig);
     bool collisionChecker(const JointConfig &config, const JointConfig &parentConfig);
+    bool rejectionChecker(const JointConfig& config);
+    bool projectionChecker(const JointConfig& config);
 
     void setProjectionConstraints(Constraint* constraints);
     void setRejectionConstraints(Constraint* constraints);

diff --git a/ccrrt/Constraint.h b/ccrrt/Constraint.h
@@ -12,10 +12,10 @@ class Constraint
 
     typedef enum {
 
-        INVALID = 0,
+        STUCK = 0,
+        INVALID,
         AT_RISK,
-        VALID,
-        STUCK
+        VALID
 
     } validity_t;
 
@@ -26,10 +26,6 @@ class Constraint
 
     virtual validity_t getValidity(const Eigen::VectorXd& config) = 0;
 
-    virtual size_t constraintDimension() const = 0;
-
-    std::vector<size_t> elevatorDimensions;
-
 };
 
 } // namespace ccrrt

diff --git a/ccrrt/ConstraintGroup.h b/ccrrt/ConstraintGroup.h
@@ -10,21 +10,17 @@ class ConstraintGroup : public Constraint
 {
 public:
 
-    ConstraintGroup();
-
     validity_t getCostGradient(Eigen::VectorXd& gradient,
                                const Eigen::VectorXd& parent,
                                const Eigen::VectorXd& config,
                                const Eigen::VectorXd& target);
     validity_t getValidity(const Eigen::VectorXd &config);
-    size_t constraintDimension() const;
 
     void addConstraint(Constraint* constraint);
     size_t numConstraints() const;
 
 protected:
 
-    size_t _dimension;
     std::vector<Constraint*> _constraints;
 };
 

diff --git a/ccrrt/RRTManager.h b/ccrrt/RRTManager.h
@@ -295,6 +295,12 @@ class RRTManager
     bool checkIfInRange(const JointConfig& configA, const JointConfig& configB);
     bool checkIfInDomain(const JointConfig& config, bool verbose=true);
     bool checkDimensionality(const JointConfig& config, bool verbose=true);
+
+    double getPathLength(RRTNode* start, RRTNode* end);
+    static double getPathLength(const ConfigPath& path, size_t start, size_t end);
+
+    // Maximum step size through the jointspace
+    double maxStepSize_;
 
 protected:
 
@@ -318,8 +324,6 @@ class RRTManager
 
     double _numPrecThresh;
 
-    // Maximum step size through the jointspace
-    double _maxStepSize;
 
     std::vector<int> currentTreeSize;
 

diff --git a/src/CBiRRT.cpp b/src/CBiRRT.cpp
@@ -24,24 +24,37 @@ void CBiRRT::setProjectionConstraints(Constraint *constraints)
     _projection_constraints = constraints;
 }
 
+bool CBiRRT::rejectionChecker(const JointConfig &config)
+{
+    if(NULL == _rejection_constraints)
+        return true;
+
+    return _rejection_constraints->getValidity(config) != Constraint::INVALID;
+}
+
+bool CBiRRT::projectionChecker(const JointConfig &config)
+{
+    if(NULL == _projection_constraints)
+        return true;
+
+    return _projection_constraints->getValidity(config) != Constraint::INVALID;
+}
+
 bool CBiRRT::collisionChecker(const JointConfig &config, const JointConfig &parentConfig)
 {
-    if(_rejection_constraints==NULL)
+    if(NULL==_rejection_constraints && NULL==_projection_constraints)
         return true;
 
     size_t c = ceil((parentConfig-config).norm()/collisionCheckStepSize_);
     if( c == 0 )
-        return (_rejection_constraints->getValidity(config) != Constraint::INVALID)
-            && (_projection_constraints->getValidity(config) != Constraint::INVALID);
+        return rejectionChecker(config) && projectionChecker(config);
 
     for(size_t i=0; i<c; ++i)
     {
-        if(_rejection_constraints->getValidity(config + 
-                    (parentConfig-config).normalized()*i/c) == Constraint::INVALID)
+        if(!rejectionChecker(config + (parentConfig-config).normalized()*i/c))
             return false;
 
-        if(_projection_constraints->getValidity(config +
-                    (parentConfig-config).normalized()*i/c) == Constraint::INVALID)
+        if(!projectionChecker(config + (parentConfig-config).normalized()*i/c))
             return false;
     }
 
@@ -51,7 +64,12 @@ bool CBiRRT::collisionChecker(const JointConfig &config, const JointConfig &pare
 bool CBiRRT::constraintProjector(JointConfig &config, const JointConfig &parentConfig)
 {
     if(_projection_constraints==NULL)
+    {
+        JointConfig target = config;
+        config = parentConfig;
+        stepConfigTowards(config, target);
         return true;
+    }
 
     JointConfig target = config;
     config = parentConfig;
@@ -64,6 +82,9 @@ bool CBiRRT::constraintProjector(JointConfig &config, const JointConfig &parentC
 
     while(result == Constraint::INVALID)
     {
+        if(result == Constraint::STUCK)
+            return false;
+
         if(!checkIfInRange(config, parentConfig))
             return false;
 

diff --git a/src/ConstraintGroup.cpp b/src/ConstraintGroup.cpp
@@ -4,11 +4,6 @@
 
 using namespace ccrrt;
 
-ConstraintGroup::ConstraintGroup()
-{
-    _dimension = 0;
-}
-
 Constraint::validity_t ConstraintGroup::getCostGradient(Eigen::VectorXd& gradient,
                                                         const Eigen::VectorXd& parent,
                                                         const Eigen::VectorXd& config,
@@ -50,14 +45,8 @@ Constraint::validity_t ConstraintGroup::getValidity(const Eigen::VectorXd& confi
     return result;
 }
 
-size_t ConstraintGroup::constraintDimension() const
-{
-    return _dimension;
-}
-
 void ConstraintGroup::addConstraint(Constraint *constraint)
 {
-    _dimension += constraint->constraintDimension();
     _constraints.push_back(constraint);
 }