Merge pull request #66 from pFernbach/topic/fix_3d_ctc

fix setStartState/setEndState for 3D contacts

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -322,10 +322,10 @@ class FullBody(Robot):
             return self.clientRbprm.rbprm.setStartState(configuration, contacts)
         cl = self.clientRbprm.rbprm
         sId = cl.createState(configuration, contacts)
-        num_max_sample = 1
+        num_max_sample = 0
         for (limbName, normal) in zip(contacts, normals):
             p = cl.getEffectorPosition(limbName, configuration)[0]
-            cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
+            sId = cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
         return cl.setStartStateId(sId)
 
     # # Initialize the goal configuration of the path interpolation
@@ -339,10 +339,10 @@ class FullBody(Robot):
             return self.clientRbprm.rbprm.setEndState(configuration, contacts)
         cl = self.clientRbprm.rbprm
         sId = cl.createState(configuration, contacts)
-        num_max_sample = 1
+        num_max_sample = 0
         for (limbName, normal) in zip(contacts, normals):
             p = cl.getEffectorPosition(limbName, configuration)[0]
-            cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
+            sId = cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
         return cl.setEndStateId(sId)
 
     # # Initialize the first state of the path interpolation

tests/python/test_rbprm_state_3d.py

@@ -49,6 +49,8 @@ class TestRBPRMstate3D(unittest.TestCase):
         self.assertTrue(state.isLimbInContact(fullbody.rArmId))
         self.assertTrue(state.isLimbInContact(fullbody.lArmId))
         self.assertTrue(state2.isBalanced())
+        p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
+        self.assertEqual(n, n_real)
 
         process.kill()