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)