contact accurately created along surface

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -94,7 +94,7 @@ module hpp
     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);
+    void addLimb(in string limb, in floatSeq offset, in unsigned short samples, in double resolution) raises (Error);
 
   }; // interface Robot
   }; // module rbprm

script/loadhrp2.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-1, 2, -2, 0, -1, 1.5])
+
+
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( fullBody )
+r = Viewer (ps)
+r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
+#~ 
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+fullBody.addLimb(rLeg,rLegOffset, 5000, 0.001)
+
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,0,-0.105]
+fullBody.addLimb(lLeg,lLegOffset, 5000, 0.001)
+
+q_init = fullBody.getCurrentConfig ()
+q_init [0:3] = [0, -0.5, 0.7]
+r (q_init)
+fullBody.setCurrentConfig (q_init)
+#~ 
+#~ 
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+r (q_init)
+#~ 
+fullBody.getContactSamplesIds(rLeg, q_init, [0,0,1])
+
+#~ q_init = fullBody.getSample(rLeg, 1)
+#~ r (q_init)
+
+ids = fullBody.getContactSamplesIds(rLeg, q_init, [0,0,1])
+i =-1
+#~ i=i+1; q_init = fullBody.getSample(rLeg, int(ids[i])); r(q_init); ids[i]
+

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -71,8 +71,8 @@ class FullBody (object):
 			self.rankInVelocity [j] = rankInVelocity
 			rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
 
-    def addLimb(self, name, samples, resolution):
-		self.client.rbprm.rbprm.addLimb(name, samples, resolution)
+    def addLimb(self, name, offset, samples, resolution):
+		self.client.rbprm.rbprm.addLimb(name, offset, samples, resolution)
 
     def getSample(self, name, idsample):
 		return self.client.rbprm.rbprm.getSampleConfig(name,idsample)

src/rbprmbuilder.impl.cc

@@ -187,14 +187,16 @@ namespace hpp {
             throw Error ("No full body robot was loaded");
         try
         {
-            Eigen::Vector3d dir;
+            fcl::Vec3f dir;
             for(std::size_t i =0; i <3; ++i)
             {
                 dir[i] = direction[i];
             }
             model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
+            std::cout << "configuration in " << config << std::endl;
             rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
                                             problemSolver_->collisionObstacles(), dir);
+    std::cout << "configuration out" << state.configuration_ << std::endl;
             hpp::floatSeq* dofArray = new hpp::floatSeq();
             dofArray->length(_CORBA_ULong(config.rows()));
             for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
@@ -213,7 +215,7 @@ namespace hpp {
             throw Error ("No full body robot was loaded");
         try
         {
-            Eigen::Vector3d dir;
+            fcl::Vec3f dir;
             for(std::size_t i =0; i <3; ++i)
             {
                 dir[i] = direction[i];
@@ -255,13 +257,18 @@ namespace hpp {
         }
     }
 
-    void RbprmBuilder::addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error)
+    void RbprmBuilder::addLimb(const char* limb, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)
             throw Error ("No full body robot was loaded");
         try
         {
-            fullBody_->AddLimb(std::string(limb),samples,resolution);
+            fcl::Vec3f off;
+            for(std::size_t i =0; i <3; ++i)
+            {
+                off[i] = offset[i];
+            }
+            fullBody_->AddLimb(std::string(limb), off, problemSolver_->collisionObstacles(), samples,resolution);
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -84,7 +84,7 @@ namespace hpp {
                                                    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);
+        virtual void addLimb(const char* limb, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error);
 
         public:
         void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);