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