Unverified Commit d5d72ee8 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub

Merge pull request #66 from pFernbach/topic/fix_3d_ctc

fix setStartState/setEndState for 3D contacts
parents 7c4f4837 f0656cf8
......@@ -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
......
......@@ -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()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment