def genFlat():
q = fullBody.shootRandomConfig()
q[0:7] = zeroConf
fullBody.setCurrentConfig(q)
# v(q)
posrf = fullBody.getJointPosition(rfoot)[:3]
poslf = fullBody.getJointPosition(lfoot)[:3]
s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds)
s, succ = state_alg.addNewContact(s,
rLegId,
posrf, [0., 0., 1.],
num_max_sample=0)
if succ:
s, succ = state_alg.addNewContact(s,
lLegId,
poslf, [0., 0., 1.],
num_max_sample=0)
if succ:
succ = fullBody.isConfigValid(
q)[0] and norm(array(posrf[:2]) - array(poslf[:2])) >= 0.3
# print("sid = ", s.sId)
# if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
if succ and norm(array(posrf) - array(poslf)) <= 0.1:
v(s.q())
return s.q(), succ, s, [posrf, poslf]
def gen_state(s, pId, com , num_max_sample = 1, first = False ):
#~ 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
com[2] += 0.5
print("com" , com)
print("pos" , pos.tolist())
#~ q = fullBody.getCurrentConfig()
#~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist())
sres, succ = state_alg.addNewContact(s, movingID, pos.tolist(), z.tolist(), num_max_sample= 0)
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 < 17):
succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample)
com[2] -= 0.05
ite += 1
print("COM?", succCom)
v(sres.q())
return sres
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 genFlat(init=False):
q = fullBody.shootRandomConfig()
if init:
q = fullBody.referenceConfig[::]
q[0:7] = zeroConf
fullBody.setCurrentConfig(q)
# v(q)
positions = [fullBody.getJointPosition(foot)[:3] for foot in effectors]
s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
s, succ = state_alg.addNewContact(s,
effId,
pos, [0.0, 0.0, 1.0],
num_max_sample=0)
if not succ:
break
# posrf = fullBody.getJointPosition(rfoot)[:3]
# poslf = fullBody.getJointPosition(lfoot)[:3]
# print ("limbsIds ", limbIds)
# s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds)
# s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample= 0)
# if succ:
# s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample= 0)
if succ:
# ~ succ = fullBody.isConfigValid(q)[0]
# and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
succ = fullBody.isConfigValid(q)[0]
# assert that in static equilibrium
if succ:
fullBody.setCurrentConfig(q)
succ = staticEq(positions, fullBody.getCenterOfMass())
if not succ:
v(q)
if succ:
succ = not pointInsideHull(positions)
if not succ:
print("************* contacts crossing", not succ)
v(q)
# if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
# if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
v(s.q())
return s.q(), succ, s, positions
