python binding to bound so3

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -86,6 +86,12 @@ class Builder (object):
     def initshooter (self):
         return self.client.rbprm.rbprm.initshooter ()
 
+	## Sets limits on robot orientation, described according to Euler's ZYX rotation order
+	#
+    # \param bounds 6D vector with the lower and upperBound for each rotation axis in sequence
+    def boundSO3 (self,  bounds):
+        return self.client.rbprm.rbprm.boundSO3 (bounds)
+
 	## Specifies a preferred normal direction for a given rom.
 	# This constrains the planner to accept a rom configuration only if
 	# it collides with a surface the normal of which has these properties.

src/rbprmbuilder.impl.cc

@@ -221,6 +221,17 @@ namespace hpp {
         return res;
     }
 
+    std::vector<double> doubleConversion(const hpp::floatSeq& dofArray)
+    {
+        std::vector<double> res;
+        std::size_t dim = (std::size_t)dofArray.length();
+        for (std::size_t iDof = 0; iDof < dim; iDof++)
+        {
+            res.push_back(dofArray[iDof]);
+        }
+        return res;
+    }
+
 
     void RbprmBuilder::setFilter(const hpp::Names_t& roms) throw (hpp::Error)
     {
@@ -241,6 +252,17 @@ namespace hpp {
         bindShooter_.normalFilter_.insert(std::make_pair(name,filter));
     }
 
+    void RbprmBuilder::boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error)
+    {
+        std::vector<double> limits = doubleConversion(limitszyx);
+        if(limits.size() !=6)
+        {
+            throw Error ("Can not bound SO3, array of 6 double required");
+        }
+        bindShooter_.so3Bounds_ = limits;
+
+    }
+
     hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)

src/rbprmbuilder.impl.hh

@@ -44,8 +44,11 @@ namespace hpp {
         hpp::rbprm::RbPrmShooterPtr_t create (const hpp::model::DevicePtr_t& robot)
         {
             hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
-            return hpp::rbprm::RbPrmShooter::create
+            rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create
                     (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_);
+            if(!so3Bounds_.empty())
+                shooter->BoundSO3(so3Bounds_);
+            return shooter;
         }
 
         hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
@@ -61,6 +64,7 @@ namespace hpp {
         std::map<std::string, NormalFilter> normalFilter_;
         std::size_t shootLimit_;
         std::size_t displacementLimit_;
+        std::vector<double> so3Bounds_;
     };
 
       class RbprmBuilder : public virtual POA_hpp::corbaserver::rbprm::RbprmBuilder
@@ -92,6 +96,8 @@ namespace hpp {
 
         virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
         virtual void setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error);
+        virtual void boundSO3(const hpp::floatSeq& limitszyx) throw (hpp::Error);
+
 
         virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
         virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);