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

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): ...@@ -322,10 +322,10 @@ class FullBody(Robot):
return self.clientRbprm.rbprm.setStartState(configuration, contacts) return self.clientRbprm.rbprm.setStartState(configuration, contacts)
cl = self.clientRbprm.rbprm cl = self.clientRbprm.rbprm
sId = cl.createState(configuration, contacts) sId = cl.createState(configuration, contacts)
num_max_sample = 1 num_max_sample = 0
for (limbName, normal) in zip(contacts, normals): for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName, configuration)[0] 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) return cl.setStartStateId(sId)
# # Initialize the goal configuration of the path interpolation # # Initialize the goal configuration of the path interpolation
...@@ -339,10 +339,10 @@ class FullBody(Robot): ...@@ -339,10 +339,10 @@ class FullBody(Robot):
return self.clientRbprm.rbprm.setEndState(configuration, contacts) return self.clientRbprm.rbprm.setEndState(configuration, contacts)
cl = self.clientRbprm.rbprm cl = self.clientRbprm.rbprm
sId = cl.createState(configuration, contacts) sId = cl.createState(configuration, contacts)
num_max_sample = 1 num_max_sample = 0
for (limbName, normal) in zip(contacts, normals): for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName, configuration)[0] 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) return cl.setEndStateId(sId)
# # Initialize the first state of the path interpolation # # Initialize the first state of the path interpolation
......
...@@ -49,6 +49,8 @@ class TestRBPRMstate3D(unittest.TestCase): ...@@ -49,6 +49,8 @@ class TestRBPRMstate3D(unittest.TestCase):
self.assertTrue(state.isLimbInContact(fullbody.rArmId)) self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId)) self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertTrue(state2.isBalanced()) self.assertTrue(state2.isBalanced())
p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n_real)
process.kill() 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