Merge pull request #45 from stonneau/python_get_colliding_obstacles

Get colliding obstacles

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -754,6 +754,17 @@ module hpp
 	/// \param stateIdTo : index of the second state
     Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
 
+
+    /// 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

script/tools/display_tools.py

@@ -37,4 +37,16 @@ def addVector(viewer,rbprmBuilder,color,v,name=None):
   gui.applyConfiguration(name,v)
   gui.refresh()
 
+def displaySurfaceFromPoints(viewer,p_list,color=[0,0,0,1],name=None):
+  gui = viewer.client.gui
+  if name==None:
+    i=0
+    name='surface_'+str(i)
+    while name in gui.getNodeList():
+      i=i+1
+      name='surface_'+str(i)
+  gui.addCurve(name,p_list,color)
+  gui.addToGroup(name,viewer.sceneName)
+  gui.refresh()  
+
 

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -103,3 +103,14 @@ def exportPath (self, viewer, problem, pathId, stepsize, filename):
   # \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
@@ -1553,6 +1555,156 @@ namespace hpp {
       }
     }
 
+    Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& configuration,const char* limbName)throw (hpp::Error){
+      try
+      {
+          std::vector<std::string> res;
+          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);
+          hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(romDevice);
+          RbPrmPathValidationPtr_t rbprmPathValidation_ (boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
+          rbprmPathValidation_->getValidator()->computeAllContacts(true);
+          core::ValidationReportPtr_t report;
+          problemSolver()->problem()->configValidations()->validate(q,report);
+          core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport> (report);
+          for(std::map<std::string,core::CollisionValidationReportPtr_t>::const_iterator it = rbReport->ROMReports.begin() ; it != rbReport->ROMReports.end() ; ++it){
+              if (name == it->first)
+              //if (true)
+              {
+                    core::AllCollisionsValidationReportPtr_t romReports = boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(it->second);
+                    if(!romReports){
+                      hppDout(warning,"For rom : "<<it->first<<" unable to cast in a AllCollisionsValidationReport, did you correctly call computeAllContacts(true) before generating the report ? ");
+                      //return;
+                        }
+                  if(romReports->collisionReports.size()> 1){
+
+                      for(std::vector<CollisionValidationReportPtr_t>::const_iterator itAff = romReports->collisionReports.begin() ; itAff != romReports->collisionReports.end() ; ++itAff){
+                         res.push_back((*itAff)->object2->name ());
+                      }
+                  }
+              }
+          }
+          CORBA::ULong size = (CORBA::ULong) res.size ();
+          char** nameList = Names_t::allocbuf(size);
+          Names_t *variations = new Names_t (size,size,nameList);
+          for (std::size_t i = 0 ; i < res.size() ; ++i){
+            nameList[i] = (char*) malloc (sizeof(char)*(res[i].length ()+1));
+            strcpy (nameList [i], res[i].c_str ());
+          }
+          return variations;
+        }
+      catch(std::runtime_error& e)
+      {
+          throw Error(e.what());
+      }
+    }
+
+      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.");
+          // Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface:
+          hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
+          fcl::Vec3f reference = rbprmDevice->getEffectorReference(name);
+          hppDout(notice,"Reference position for rom"<<name<<" = "<<reference);
+          //apply transform from currernt config :
+          fcl::Transform3f tRoot;
+          tRoot.setTranslation(fcl::Vec3f(q[0],q[1],q[2]));
+          fcl::Quaternion3f quat(q[6],q[3],q[4],q[5]);
+          tRoot.setRotation(quat.matrix());
+          reference = (tRoot*reference).getTranslation();
+          geom::Point refPoint(reference);
+          hppDout(notice,"Reference after root transform = "<<reference);
+          geom::Point normal,proj;
+          double minDistance = std::numeric_limits<double>::max();
+          double distance;
+          _CORBA_ULong bestSurface(0);
+
+          // init floatSeqSeq to store results
+          hpp::floatSeqSeq *res;
+          res = new hpp::floatSeqSeq ();
+          res->length ((_CORBA_ULong)romReports->collisionReports.size());
+          _CORBA_ULong 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());
+                // compute heuristic score :
+                distance = geom::projectPointInsidePlan(inter,refPoint,normal,inter.front(),proj);
+                hppDout(notice,"Distance found : "<<distance);
+                if(distance < minDistance){
+                    minDistance = distance;
+                    bestSurface = idSurface;
+                }
+                // 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) [idSurface] = floats;
+                ++idSurface;
+              }
+            }
+          }
+          // swap res[0] and res[bestSurface]:
+          if(bestSurface>0){
+            hpp::floatSeq tmp = (*res)[0];
+            (*res)[0] = (*res)[bestSurface];
+            (*res)[bestSurface] = tmp;
+          }
+          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

@@ -357,7 +357,10 @@ namespace hpp {
         virtual hpp::floatSeq* evaluateConfig(const hpp::floatSeq& configuration, const hpp::floatSeq &direction) throw (hpp::Error);
         virtual void dumpProfile(const char* logFile) throw (hpp::Error);
         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* 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);