def test_contacts_3d(self):
with ServerManager('hpp-rbprm-server'):
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.45, -0.2, 0.002]
state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
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)
init_bezier_traj(fullBody, r, pp, configs, limbsCOMConstraints)
all_paths = [[], []]
from hpp.corbaserver.rbprm.state_alg import *
#~ d(0.07);e(0.01)
i = 0
#~ d(0.09); e(0.01);
configs = d(0.1)
e()
states = planToStates(fullBody, configs)[:-1]
s = State(fullBody,
q=states[-1].q(),
limbsIncontact=states[-1].getLimbsInContact())
x = s.getCenterOfMass()
x[0] -= 0.05
x[2] -= 0.1
fullBody.projectStateToCOM(s.sId, x, 10)
r(s.q())
states += [s]
#~ lc()
#~ le = min(38, len(states)-10)
lc()
#~ path = []
#~ all_paths = [[],[]]
#~ states = list(reversed(states))
#~ for i in range(0,4):
#~ print i
#~ onepath(i,0,nopt=1,effector=False,mu=1);
#s1,success = StateHelper.addNewContact(s0,rLegId,p,[0,0,1])
#assert(success)
#r.addLandmark('talos/arm_left_7_link',0.2)
p = [-12.3, -3.26, -1.2]
moveSphere('s', r, p)
s1 = s0
p = [-12.55, -3.26, -1.65]
s2, success = StateHelper.addNewContact(s1, lArmId, p, [0, 0, 1], 10000)
r(s2.q())
p = [-12.55, -4.32, -1.65]
s3, success = StateHelper.addNewContact(s2, rArmId, p, [0, 0, 1], 10000)
r(s3.q())
s3.projectToCOM(s0.getCenterOfMass(), 10000)
r(s3.q())
p = [-12.5, -3.65, -2.39] # l foot first step
s4, success = StateHelper.addNewContact(s3, lLegId, p, [0, 0, 1], 10000)
r(s4.q())
p = [-12.5, -3.85, -2.39] # r foot first step
s5, success = StateHelper.addNewContact(s4, rLegId, p, [0, 0, 1], 10000)
r(s5.q())
s5.projectToCOM(s0.getCenterOfMass(), 10000)
r(s5.q())
def test_contacts_6d(self):
with ServerManager('hpp-rbprm-server'):
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds(
[-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertFalse(state1.isBalanced())
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.15, -0.085, 0.002]
state2, success = StateHelper.addNewContact(
state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state2.isBalanced())
p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n2)
for i in range(3):
self.assertAlmostEqual(p[i], p2[i], 2)
# try to replace a contact :
p = [0.2, 0.085, 0.002]
state3, success = StateHelper.addNewContact(
state2, fullbody.lLegId, p, n)
self.assertTrue(success)
self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
self.assertTrue(state3.isBalanced())
# try com projection:
com_target = com[::]
com_target[2] -= 0.08
success = state.projectToCOM(com_target)
self.assertTrue(success)
fullbody.setCurrentConfig(state.q())
com_state = fullbody.getCenterOfMass()
self.assertEqual(com_state, state.getCenterOfMass())
for i in range(3):
self.assertAlmostEqual(com_state[i], com_target[i], 4)
# try lockOtherJoints:
p = [0.1, -0.085, 0.002]
state4, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
lockOtherJoints=True)
self.assertTrue(success)
self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
self.assertTrue(state4.isBalanced())
for i in range(7, 13):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
for i in range(19, len(q)):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
# try with a rotation
rot = [0.259, 0, 0, 0.966]
state5, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
rotation=rot)
self.assertTrue(success)
self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
self.assertTrue(state5.isBalanced())
fullbody.setCurrentConfig(state5.q())
rf_pose = fullbody.getJointPosition(fullbody.rfoot)
for i in range(4):
self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
