added setNormalFilter method to python bindings

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -96,6 +96,16 @@ module hpp
     void setFilter (in Names_t roms)
       raises (Error);
 
+    /// Set Rom surface constraints for the configuration shooter
+    /// a Rom configuration will only be valid it collides with a surface
+    /// the normal of which is colinear to the indicated normal,
+    /// modulated by a range value.
+    /// \param normal prefered contact surface normal direction
+    /// \param range tolerance between surface normal and desired surface normal. If the dot
+    /// product of the normal is greater than range then the surface will be accepted.
+    ///
+    void setNormalFilter(in string romName, in floatSeq normal, in double range) raises (Error);
+
     /// Get Sample configuration by its id
     /// \param sampleName name of the limb from which to retrieve a sample
     /// \param sampleId id of the desired samples

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -48,7 +48,16 @@ class Builder (object):
         self.client = CorbaClient ()
         self.load = load
         
-    ## Virtual function to load the robot model
+    ## Virtual function to load the robot model.
+	#
+    # \param urdfName urdf description of the robot trunk,
+    # \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
+    # \param rootJointType type of root joint among ("freeflyer", "planar",
+    #        "anchor"),
+    # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
+    # \param packageName name of the package from where the robot will be loaded
+    # \param urdfSuffix optional suffix for the urdf of the robot package
+    # \param srdfSuffix optional suffix for the srdf of the robot package
     def loadModel (self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix):
 		if(isinstance(urdfNameroms, list)):		
 			for urdfNamerom in urdfNameroms:
@@ -81,19 +90,28 @@ class Builder (object):
 
 	## Init RbprmShooter
     #
-    # \param jointName name of the joint,
-    # \return name of the link.
     def initshooter (self):
         return self.client.rbprm.rbprm.initshooter ()
-    ## \}
 
-	## Init RbprmShooter
-    #
-    # \param jointName name of the joint,
-    # \return name of the link.
+	## 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.
+	#
+    # \param rom name of the rome,
+    # \param normal 3d vector specifying the normal,
+    # \param tolerance expressed as the dot product between the considered obstacle and the ideal normal.
+    # if the dot product is greater than the tolerance the surface will be considered valid.
+    def setnormalfilter (self, rom, normal, tolerance):
+        return self.client.rbprm.rbprm.setNormalFilter (rom, normal, tolerance)
+
+	## Specifies a rom constraint for the planner.
+	# A configuration will be valid if and only if the considered rom collides
+	# with the environment.
+	#
+    # \param romFilter array of roms indicated by name, which determine the constraint.
     def setFilter (self, romFilter):
         return self.client.rbprm.rbprm.setFilter (romFilter)
-    ## \}
+
 
    ## \name Degrees of freedom
     #  \{

src/rbprmbuilder.impl.cc

@@ -226,6 +226,20 @@ namespace hpp {
         bindShooter_.romFilter_ = stringConversion(roms);
     }
 
+
+    void RbprmBuilder::setNormalFilter(const char* romName, const hpp::floatSeq& normal, double range) throw (hpp::Error)
+    {
+        std::string name(romName);
+        bindShooter_.normalFilter_.erase(name);
+        fcl::Vec3f dir;
+        for(std::size_t i =0; i <3; ++i)
+        {
+            dir[i] = normal[i];
+        }
+        NormalFilter filter(dir,range);
+        bindShooter_.normalFilter_.insert(std::make_pair(name,filter));
+    }
+
     hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration, const hpp::floatSeq& direction) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)
@@ -455,17 +469,6 @@ namespace hpp {
         }
     }
 
-    /*namespace
-    {
-        hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
-        {
-            hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
-            hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast));
-            hpp::core::CollisionPathValidationReport defaultValidation;
-            return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
-        }
-    }*/
-
     void RbprmBuilder::SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver)
     {
         problemSolver_ = problemSolver;

src/rbprmbuilder.impl.hh

@@ -45,19 +45,20 @@ namespace hpp {
         {
             hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
             return hpp::rbprm::RbPrmShooter::create
-                    (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,shootLimit_,displacementLimit_);
+                    (robotcast,problemSolver_->problem ()->collisionObstacles(),romFilter_,normalFilter_,shootLimit_,displacementLimit_);
         }
 
         hpp::core::PathValidationPtr_t createPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
         {
             hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
-            hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_));
+            hpp::rbprm::RbPrmValidationPtr_t validation(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, normalFilter_));
             hpp::core::CollisionPathValidationReport defaultValidation;
             return hpp::core::DiscretizedCollisionChecking::createWithValidation(robot,val ,defaultValidation, validation);
         }
 
         hpp::core::ProblemSolverPtr_t problemSolver_;
         std::vector<std::string> romFilter_;
+        std::map<std::string, NormalFilter> normalFilter_;
         std::size_t shootLimit_;
         std::size_t displacementLimit_;
     };
@@ -90,6 +91,7 @@ namespace hpp {
                  const char* srdfSuffix) throw (hpp::Error);
 
         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 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);