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 getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]):
s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs)
rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId)
lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId)
return {
Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]),
Robot.lLegId: (lLegPn[0][0], lLegPn[1][0])
}
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
# ~ if rp[1] > -MARGIN_FEET_SIDE:
# ~ compoints[j].append(rp)
else:
global fails
fails += 1
# print(fullBody.isConfigValid(q)[1])
# for j in range(0,len(limbIds)):
# f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
# for p in points[j]:
# f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
# f1.close()
s = rbprmstate.State(fullBody,
q=fullBody.getCurrentConfig(),
limbsIncontact=[fullBody.limbs_names[0]])
# printRootPosition(rLegId, rfoot, nbSamples)
# printRootPosition(lLegId, lfoot, nbSamples)
# printRootPosition(rarmId, rHand, nbSamples)
# printRootPosition(larmId, lHand, nbSamples)
printFootPositionRelativeToOther(6000)
print("successes ", success)
print("fails ", fails)
# ~ for effector, comData, pointsData in zip(effectors, compoints, points):
# ~ for effector, limbId, comData, pointsData in zip(effectors[:1],limbIds[1:],
# compoints[:1], points[:1]):
for effector, limbId, comData, pointsData in zip(effectors, limbIds, compoints,
points):
#~ 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
q = fullBody.getCurrentConfig()
q[:2] = allfeetpos[0][:2].tolist()
v(q)
s = rbprmstate.State(fullBody, q = q, limbsIncontact = [])
all_states = [s]
sprev = s
idfirst = 2
for i in range(1, len(pb["phaseData"])):
#~ com = (coms[i-i] ).tolist()
com = (coms[i-1] ).tolist()
#~ if i > 2:
#~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist()
snew = gen_state(sprev, i , com, num_max_sample = 20, first = i == idfirst )
all_states += [snew]
sprev = snew
com2 = coms[i].tolist()
snew2 = gen_state(snew, i , com2, num_max_sample = 20 )
all_states += [snew2]
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
q = fullBody.getCurrentConfig()
q[:3] = [-2.69, 0.21, 0.649702]
v(q)
s = rbprmstate.State(fullBody,
q=q,
limbsIncontact=[fullBody.lLegId, fullBody.rLegId])
idfirst = 2
coms[0] = array(s.getCenterOfMass())
#~ coms[0] = coms[1]
#~ coms[1] = array(s.getCenterOfMass())
def normal(phase):
s = phase["S"][0]
n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0])
n /= norm(n)
if n[2] < 0.:
for i in range(3):
n[i] = -n[i]
