def gen_state(s, pId, com, num_max_sample=1, first=False, normal=z, newContact=True): #~ pId = 6 phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fullBody.lLegId if moving == RF: movingID = fullBody.rLegId pos = allfeetpos[pId] pos[2] += 0.01 if not first: com[2] += 0.5 #~ com[2] += 0.4 #~ print "com" , com #~ print "pos" , pos.tolist() #~ q = fullBody.getCurrentConfig() #~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) if newContact: sres, succ = state_alg.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample=200) else: sres, succ = StateHelper.cloneState(s) if not succ: print("succes ?", succ) succCom = False ite = 0 #~ if first: #~ print "FIRST " #~ com[2] -= 0.25 #~ while(not succCom and ite < 11): while (not succCom and ite < 30): succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) com[2] -= 0.05 ite += 1 if succCom: q = sres.q() q[3:7] = [0., 0., 0., 1.] q[3:] = fullBody.referenceConfig[3:] sres.setQ(q) succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) if not succCom: print("refail") v(sres.q()) return sres
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 False and pId < len(pb["phaseData"])-1: quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"]) qrot = (quat0.slerp(0.5,quat1)).matrix() else: qrot = Quaternion(phase["rootOrientation"]) # rotation of the root, from the guide #q_n = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T) #rot = quatToConfig(qrot * q_n) if not isclose(normal,Z_AXIS).all(): qrot = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T) # ignore guide orientation when normal is not z ... #rot = quatToConfig(qrot) pos = allfeetpos[pId]; pos[2] += 0.002 pose = pos.tolist()+quatToConfig(qrot) print("Try to add contact for "+movingID+" pos = "+str(pose)) disp.moveSphere('c',v,pose) if newContact: sres, succ = tryCreateContactAround(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample,rotation = qrot) #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(pose)+" ; n = "+str(normal.tolist())) raise RuntimeError("Cannot project feet to new contact position") # try something else ?? if projectCOM : #sfeet, _ = StateHelper.cloneState(sres) #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
def projCom(state, c, qref): s, succ = StateHelper.cloneState(state) succCom = succ and fullBody.projectStateToCOM(s.sId, c, 0) if succCom: return s #~ else: #~ c[2] += 0.2 #~ v(qref) for i in range(10): s.setQ(qref) succCom = succ and fullBody.projectStateToCOM(s.sId, c, 0) c[2] -= .05 if succCom: return s #~ else: print("fail to project com") #~ return state.q() return state
def gen_state(s, pId, com, num_max_sample=0, first=False, normal=z, newContact=True, projectCOM=True): #~ pId = 6 phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fullBody.lLegId if moving == RF: movingID = fullBody.rLegId print("# gen state for phase Id = ", pId) #print "current config q=",s.q() #print "move limb ",movingID pos = allfeetpos[pId + 2] # +2 because it contains also the 2 feet pos at the init config pos[2] += 0.01 if newContact: 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()) + " : 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
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