From d12ebb2221d41b948428c77c9fa38fb3f352d3ce Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Thu, 9 Jul 2015 11:57:09 +0200
Subject: [PATCH] get all sample candidates in python

---
 idl/hpp/corbaserver/rbprm/rbprmbuilder.idl |  1 +
 script/loadfullbody.py                     |  4 +-
 src/hpp/corbaserver/rbprm/rbprmfullbody.py |  3 ++
 src/rbprmbuilder.impl.cc                   | 50 ++++++++++++++++++++++
 src/rbprmbuilder.impl.hh                   |  4 ++
 5 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index c7c8840..577ae30 100644
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/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);
 
diff --git a/script/loadfullbody.py b/script/loadfullbody.py
index d889949..e2cbe43 100644
--- a/script/loadfullbody.py
+++ b/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]
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index ec04f7c..43dc0d1 100644
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/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
     #  \{
 
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index ae89e89..5e549e8 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/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_)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 53b33a1..c8e6e08 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/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:
-- 
GitLab