zmp support works

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -94,7 +94,8 @@ 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 floatSeq offset, in unsigned short samples, in double resolution) raises (Error);
+    void addLimb(in string limb, in floatSeq offset, in floatSeq normal,
+                 in double x, in double y, in unsigned short samples, in double resolution) raises (Error);
     void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
     void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
     floatSeqSeq interpolate(in double timestep) raises (Error);

script/loadhrp2.py

@@ -22,14 +22,19 @@ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 ps = ProblemSolver( fullBody )
 r = Viewer (ps)
 r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
-#~ 
+
+#~ AFTER loading obstacles
 rLeg = 'RLEG_JOINT0'
 rLegOffset = [0,0,-0.105]
-fullBody.addLimb(rLeg,rLegOffset, 5000, 0.001)
+rLegNormal = [0,0,1]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLeg,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.001)
 
 lLeg = 'LLEG_JOINT0'
 lLegOffset = [0,0,-0.105]
-fullBody.addLimb(lLeg,lLegOffset, 10000, 0.001)
+lLegNormal = [0,0,1]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLeg,lLegOffset,rLegNormal, lLegx, lLegy, 10000, 0.001)
 
 fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
 fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
@@ -42,12 +47,23 @@ q_init [0:3] = [0, -0.5, 0.6]
 r (q_init)
 
 q_goal = q_init [::]
-q_goal [0:3] = [0.5, -0.5, 0.7]
+q_goal [0:3] = [1, -0.5, 0.6]
 
-fullBody.setCurrentConfig (q_init)
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+r(q_goal)
+ps.solve ()
 
+fullBody.setCurrentConfig (q_init)
 q_init = fullBody.generateContacts(q_init, [0,0,1])
 r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ 
+#~ 
+#~ configs = fullBody.interpolate(0.1)
+
 #~ fullBody.getContactSamplesIds(rLeg, q_init, [0,0,1])
 #~ q_init = fullBody.getSample(rLeg, 1)
 
@@ -58,15 +74,16 @@ r (q_init)
 #~ fullBody.setCurrentConfig (q_goal)
 #~ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
 
-#~ ps.setInitialConfig (q_init)
-#~ ps.addGoalConfig (q_goal)
-#~ ps.solve ()
-
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
 #~ 
-#~ fullBody.setStartState(q_init,[rLeg,lLeg])
-#~ fullBody.setEndState(q_goal,[rLeg])
+pp (0)
 
-#~ configs = fullBody.interpolate(0.1)
-#~ i = 0; 
-#~ r (configs[i]); i=i+1; i
+fullBody.setStartState(q_init,[rLeg,lLeg])
+fullBody.setEndState(q_goal,[rLeg])
+
+configs = fullBody.interpolate(0.1)
+i = 0; 
+r (configs[i]); i=i+1; i
+#~ pp.toFile(0, "/home/stonneau/test.txt")
 

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, offset, samples, resolution):
-		self.client.rbprm.rbprm.addLimb(name, offset, samples, resolution)
+    def addLimb(self, name, offset, normal, x, y, samples, resolution):
+		self.client.rbprm.rbprm.addLimb(name, offset, normal, x, y, samples, resolution)
 
     def getSample(self, name, idsample):
 		return self.client.rbprm.rbprm.getSampleConfig(name,idsample)

src/rbprmbuilder.impl.cc

@@ -267,18 +267,19 @@ namespace hpp {
         }
     }
 
-    void RbprmBuilder::addLimb(const char* limb, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error)
+    void RbprmBuilder::addLimb(const char* limb, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,  unsigned short samples, double resolution) throw (hpp::Error)
     {
         if(!fullBodyLoaded_)
             throw Error ("No full body robot was loaded");
         try
         {
-            fcl::Vec3f off;
+            fcl::Vec3f off, norm;
             for(std::size_t i =0; i <3; ++i)
             {
                 off[i] = offset[i];
+                norm[i] = normal[i];
             }
-            fullBody_->AddLimb(std::string(limb), off, problemSolver_->collisionObstacles(), samples,resolution);
+            fullBody_->AddLimb(std::string(limb), off, norm, x, y, 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, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error);
+        virtual void addLimb(const char* limb, const hpp::floatSeq& offset,  const hpp::floatSeq& normal, double x, double y, unsigned short samples, double resolution) throw (hpp::Error);
 
         virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);