get all sample candidates in python

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -92,6 +92,7 @@ module hpp
     floatSeq getSampleConfig(in string sampleName, in unsigned short sampleId) raises (Error);
 
     floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
+    floatSeq getContactSamplesIds(in string name, in floatSeq dofArray, in floatSeq direction) raises (Error);
 
     void addLimb(in string limb, in unsigned short samples, in double resolution) raises (Error);
 

script/loadfullbody.py

@@ -16,7 +16,7 @@ fullBody = FullBody ()
 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 fullBody.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
-fullBody.addLimb("r_shoulder_pan_joint", 1000, 0.001)
+fullBody.addLimb("r_shoulder_pan_joint", 10000, 0.001)
 
 from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 
@@ -33,6 +33,8 @@ fullBody.setCurrentConfig (q_init)
 q_init = fullBody.generateContacts(q_init, [0,1,0])
 r (q_init)
 
+fullBody.getContactSamplesIds("r_shoulder_pan_joint", q_init, [0,1,0])
 
 #~ q_init = fullBody.getSample('r_shoulder_pan_joint', 1)
 #~ r (q_init)
+q_init [0:3] = [-0.6, 0, -0.4]

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -79,6 +79,9 @@ class FullBody (object):
 
     def generateContacts(self, configuration, direction):
 		return self.client.rbprm.rbprm.generateContacts(configuration, direction)
+		
+    def getContactSamplesIds(self, name, configuration, direction):
+		return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
    ## \name Degrees of freedom
     #  \{
 

src/rbprmbuilder.impl.cc

@@ -205,6 +205,56 @@ namespace hpp {
         }
     }
 
+    hpp::floatSeq* RbprmBuilder::getContactSamplesIds(const char* limbname,
+                                        const hpp::floatSeq& configuration,
+                                        const hpp::floatSeq& direction) throw (hpp::Error)
+    {
+        if(!fullBodyLoaded_)
+            throw Error ("No full body robot was loaded");
+        try
+        {
+            Eigen::Vector3d dir;
+            for(std::size_t i =0; i <3; ++i)
+            {
+                dir[i] = direction[i];
+            }
+            model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+
+            sampling::T_OctreeReport finalSet;
+            rbprm::T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
+            if(lit == fullBody_->GetLimbs().end())
+            {
+                throw std::runtime_error ("Impossible to find limb for joint "
+                                          + std::string(limbname) + " to robot; limb not defined exists");
+            }
+            const RbPrmLimbPtr_t& limb = lit->second;
+            fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->currentTransformation (); // get root transform from configuration
+            std::vector<sampling::T_OctreeReport> reports(problemSolver_->collisionObstacles().size());
+            std::size_t i (0);
+            //#pragma omp parallel for
+            for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
+                oit != problemSolver_->collisionObstacles().end(); ++oit, ++i)
+            {
+                sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i]);
+            }
+            for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
+                cit != reports.end(); ++cit)
+            {
+                finalSet.insert(cit->begin(), cit->end());
+            }
+            hpp::floatSeq* dofArray = new hpp::floatSeq();
+            dofArray->length(finalSet.size());
+            sampling::T_OctreeReport::const_iterator candCit = finalSet.begin();
+            for(std::size_t i=0; i< _CORBA_ULong(finalSet.size()); ++i, ++candCit)
+            {
+              (*dofArray)[(_CORBA_ULong)i] = candCit->sample_->id_;
+            }
+            return dofArray;
+        } catch (const std::exception& exc) {
+        throw hpp::Error (exc.what ());
+        }
+    }
+
     void RbprmBuilder::addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)

src/rbprmbuilder.impl.hh

@@ -80,6 +80,10 @@ namespace hpp {
         virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
                                                 const hpp::floatSeq& direction) throw (hpp::Error);
 
+        virtual hpp::floatSeq* getContactSamplesIds(const char* limb,
+                                                   const hpp::floatSeq& configuration,
+                                                   const hpp::floatSeq& direction) throw (hpp::Error);
+
         virtual void addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error);
 
         public: