print "number of configs :", len(configsFull) """ q_init[0] += 0.05 createSphere('s', r) n = [0, 0, 1] p = [0, 0.1, 0] sf = State(fullBody, q=q_goal, limbsIncontact=[lLegId, rLegId]) si = State(fullBody, q=q_init, limbsIncontact=[lLegId, rLegId]) n = [0.0, -0.42261828000211843, 0.9063077785212101] p = [0.775, 0.23, -0.02] moveSphere('s', r, p) smid, success = StateHelper.addNewContact(si, lLegId, p, n) assert (success) smid2, success = StateHelper.addNewContact(sf, lLegId, p, n) assert (success) r(smid.q()) sf2 = State(fullBody, q=q_goal, limbsIncontact=[lLegId, rLegId]) """ com = fullBody.getCenterOfMass() com[1] = 0 """ import disp_bezier pp.dt = 0.0001 pids = [] curves = [] timings = []
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)
q_0 = fullBody.getCurrentConfig(); q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(0,tp.ps.pathLength(0))[0:7] dir_init = tp.ps.configAtParam(0,0.01)[7:10] acc_init = tp.ps.configAtParam(0,0.01)[10:13] dir_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[7:10] acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[10:13] configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace() fullBody.setStaticStability(True) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) ; r(q_init) s_init = StateHelper.generateStateInContact(fullBody,q_init,dir_init,acc_init) q_init = s_init.q() r(q_init) # Randomly generating a contact configuration at q_end fullBody.setCurrentConfig (q_goal) s_goal = StateHelper.generateStateInContact(fullBody,q_goal, dir_goal,acc_goal) q_goal = s_goal.q() # copy extraconfig for start and init configurations q_init[configSize:configSize+3] = dir_init[::] q_init[configSize+3:configSize+6] = acc_init[::] q_goal[configSize:configSize+3] = dir_goal[::] q_goal[configSize+3:configSize+6] = acc_goal[::] # specifying the full body configurations as start and goal state of the problem fullBody.setStartStateId(s_init.sId)
def gen_state(s, pId, num_max_sample=0, first=False, normal=lp.Z_AXIS, newContact=True, projectCOM=True): #~ pId = 6 phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fullBody.lLegId if moving == lp.RF: movingID = fullBody.rLegId print("# gen state for phase Id = ", pId) if USE_ORIENTATION: if pId < len(pb["phaseData"]) - 1: if phase["moving"] == lp.RF: rot = quatConfigFromMatrix( pb["phaseData"][pId + 1]["rootOrientation"] ) # rotation of the root, from the guide else: rot = quatConfigFromMatrix( pb["phaseData"][pId]["rootOrientation"] ) # rotation of the root, from the guide #quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) #quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"]) #rot = quatConfigFromMatrix((quat0.slerp(0.5,quat1)).matrix()) else: rot = quatConfigFromMatrix( phase["rootOrientation"] ) # rotation of the root, from the guide else: rot = [0, 0, 0, 1] #rot = quatConfigFromMatrix(phase["rootOrientation"]) # rotation of the root, from the guide #print "current config q=",s.q() #print "move limb ",movingID pos = allfeetpos[pId] print("Try to add contact for " + movingID + " pos = " + str(pos.tolist() + rot)) disp.moveSphere('c', v, pos.tolist() + rot) if newContact: sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample=num_max_sample, rotation=rot) #sres, succ = StateHelper.removeContact(s,movingID) #assert succ #succ = sres.projectToRoot(s.q()[0:3]+rot) #sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample) else: sres, succ = StateHelper.cloneState(s) if not succ: print("Cannot project config q = ", sres.q()) print("To new contact position for " + movingID + " = " + str(pos.tolist() + rot) + " ; n = " + str(normal.tolist())) raise RuntimeError("Cannot project feet to new contact position" ) # try something else ?? if projectCOM: #print "config before projecting to com q1=",sres.q() successCOM = projectCoMInSupportPolygon(sres) if not successCOM: # is it really an issue ? print( "Unable to project CoM in the center of the support polygone") v(sres.q()) return sres
from display_tools import * createSphere('target', v, size=0.05, color=v.color.red) v.client.gui.setVisibility('target', 'ON') moveSphere('target', v, [0, 0, 0.5]) # create contact : fullBody.setStartState(q_init, [lLegId, rLegId]) q_ref[0:3] = q_init[0:3] sref = State(fullBody, q=q_ref, limbsIncontact=[lLegId, rLegId]) s0 = State(fullBody, q=q_init, limbsIncontact=[lLegId, rLegId]) createSphere('s', v) p0 = [-0.25, 0.5, 0.75] #p1 = [-0,0.45,0.8] p = [0.1, 0.5, 0.75] moveSphere('s', v, p) s0_bis, success = StateHelper.addNewContact(sref, rArmId, p0, [0, 0, 1]) #s0_bis2,success = StateHelper.addNewContact(s0_bis,rArmId,p1,[0,0,1]) s1, success = StateHelper.addNewContact(s0_bis, rArmId, p, [0, 0, 1]) assert (success) v(s1.q()) #project com v(q_init) com_i = fullBody.getCenterOfMass() com_i[2] -= 0.03 com_i[0] += 0.06 createSphere("com", v) moveSphere("com", v, com_i) s1.projectToCOM(com_i) v(s1.q()) s1_feet = State(fullBody, q=s1.q(), limbsIncontact=[lLegId, rLegId])