add method getContactSurfacesAtConfig to python API

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -737,11 +737,16 @@ module hpp
     Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
 
 
-    /// For a given limb, returns all the contact surfaces in collision with the limb's reachable workspace
+    /// For a given limb, returns the names of all the contact surfaces in collisions with the limb's reachable workspace
     /// \param configuration : root configuration
     /// \param limbName : name of the considered limb
     Names_t getCollidingObstacleAtConfig(in floatSeq configuration,in string limbName) raises (Error);
 
+    /// For a given limb, return all the intersections between the limb reachable workspace and a contact surface
+    /// \param configuration : root configuration
+    /// \param limbName : name of the considered limb
+    floatSeqSeq getContactSurfacesAtConfig(in floatSeq configuration,in string limbName) raises (Error);
+
     /// return a list of all the limbs names
     Names_t getAllLimbsNames() raises (Error);
     /// tries to add a new contact to the state

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -103,3 +103,14 @@ class Builder (Robot):
   # \param ref the 3D reference position of the end effector, expressed in the root frame
   def setReferenceEndEffector(self,romName,ref):
     return self.clientRbprm.rbprm.setReferenceEndEffector(romName,ref)
+
+  ## For a given limb, return all the intersections between the limb reachable workspace and a contact surface
+  # \param configuration the root configuration
+  # \param limbName name of the considered limb
+  # \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id is the 3 coordinate of each vertex
+  def getContactSurfacesAtConfig(self,configuration,limbName):
+    surfaces = self.clientRbprm.rbprm.getContactSurfacesAtConfig(configuration,limbName)
+    res = []
+    for surface in surfaces:
+        res += [[surface[i:i+3] for i in range(0, len(surface),3)]] # split list of coordinate every 3 points (3D points)
+    return res

src/rbprmbuilder.impl.cc

@@ -50,6 +50,8 @@
 #include <hpp/rbprm/sampling/heuristic-tools.hh>
 #include <hpp/rbprm/contact_generation/reachability.hh>
 #include <hpp/pinocchio/urdf/util.hh>
+#include "hpp/rbprm/utils/algorithms.h"
+
 #ifdef PROFILE
     #include "hpp/rbprm/rbprm-profiler.hh"
 #endif
@@ -1508,6 +1510,78 @@ namespace hpp {
       }
     }
 
+      floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error){
+      try
+      {
+          hppDout(notice,"begin getContactSurfacesAtConfig");
+          std::string name (limbName);
+          //hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
+          const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name];
+          pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration);
+          romDevice->currentConfiguration(q);
+          RbPrmPathValidationPtr_t rbprmPathValidation_ (boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
+          rbprmPathValidation_->getValidator()->computeAllContacts(true);
+          core::ValidationReportPtr_t report;
+          hppDout(notice,"begin collision check");
+          problemSolver()->problem()->configValidations()->validate(q,report);
+          hppDout(notice,"done.");
+          core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport> (report);
+          hppDout(notice,"try to find rom name");
+          if(rbReport->ROMReports.find(name) == rbReport->ROMReports.end()){
+            throw std::runtime_error ("The given ROM name is not in collision in the given configuration.");
+          }
+          hppDout(notice,"try to cast report");
+          core::AllCollisionsValidationReportPtr_t romReports = boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(rbReport->ROMReports.at(name));
+          if(!romReports){
+            throw std::runtime_error ("Error while retrieving collision reports.");
+          }
+          hppDout(notice,"try deviceSync");
+          pinocchio::DeviceSync deviceSync (romDevice);
+          hppDout(notice,"done.");
+          hpp::floatSeqSeq *res;
+          res = new hpp::floatSeqSeq ();
+          res->length ((_CORBA_ULong)romReports->collisionReports.size());
+          std::size_t idSurface=0;
+          geom::Point pn;
+          hppDout(notice,"Number of collision reports for the rom : "<<romReports->collisionReports.size());
+          // for all collision report of the given ROM, compute the intersection surface between the affordance object and the rom :
+          for(std::vector<CollisionValidationReportPtr_t>::const_iterator itReport = romReports->collisionReports.begin() ; itReport != romReports->collisionReports.end() ; ++itReport){
+            // compute the intersection for itReport :
+            core::CollisionObjectConstPtr_t obj_rom = (*itReport)->object1;
+            core::CollisionObjectConstPtr_t obj_env = (*itReport)->object2;
+            // convert the two objects  :
+            geom::BVHModelOBConst_Ptr_t model_rom =  geom::GetModel(obj_rom,deviceSync.d());
+            geom::BVHModelOBConst_Ptr_t model_env =  geom::GetModel(obj_env,deviceSync.d());
+            geom::T_Point plane = geom::intersectPolygonePlane(model_rom,model_env,pn);
+            // plane contains a list of points : the intersections between model_rom and the infinite plane defined by model_env.
+            // but they may not be contained inside the shape defined by model_env
+            if(plane.size() > 0){
+              geom::T_Point inter = geom::compute3DIntersection(plane,geom::convertBVH(model_env)); // hull contain only points inside the model_env shape
+              if(inter.size() > 0){
+                hppDout(notice,"Number of points for the intersection rom/surface : "<<inter.size());
+                // add inter points to res list:
+                _CORBA_ULong size = (_CORBA_ULong) (inter.size()*3);
+                double* dofArray = hpp::floatSeq::allocbuf(size);
+                hpp::floatSeq floats (size, size, dofArray, true);
+                //convert the config in dofseq
+                for (pinocchio::size_type j=0 ; j < (pinocchio::size_type)inter.size() ; ++j) {
+                  dofArray[3*j] = inter[j][0];
+                  dofArray[3*j+1] = inter[j][1];
+                  dofArray[3*j+2] = inter[j][2];
+                }
+                (*res) [(_CORBA_ULong)idSurface] = floats;
+                ++idSurface;
+              }
+            }
+          }
+          return res;
+      }
+      catch(std::runtime_error& e)
+      {
+          throw Error(e.what());
+      }
+    }
+
     std::vector<State> TimeStatesToStates(const T_StateFrame& ref)
     {
         std::vector<State> res;

src/rbprmbuilder.impl.hh

@@ -348,6 +348,8 @@ namespace hpp {
         virtual double getTimeAtState(unsigned short stateId)throw (hpp::Error);
         virtual Names_t* getContactsVariations(unsigned short stateIdFrom,unsigned short stateIdTo )throw (hpp::Error);        
         virtual Names_t* getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);
+        virtual floatSeqSeq* getContactSurfacesAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error);
+
         virtual Names_t* getAllLimbsNames()throw (hpp::Error);
         virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
                                             const hpp::floatSeq& position, const hpp::floatSeq& normal, unsigned short max_num_sample, bool lockOtherJoints) throw (hpp::Error);