def sampleRandomStateFlatFloor(fullBody, limbsInContact, z):
success = False
it = 0
while not success and it < 10000:
it += 1
s0 = createRandomState(fullBody, limbsInContact)
# try to project feet in contact (N = [0,0,1] and p[2] = z)
n = [0, 0, 1]
for limb in limbsInContact:
p = fullBody.getJointPosition(fullBody.dict_limb_joint[limb])[0:3]
p[2] = z
s0, success = StateHelper.addNewContact(s0,
limb,
p,
n,
lockOtherJoints=True)
if not success:
break
if success:
# check stability
success = fullBody.isStateBalanced(s0.sId, 5)
if not success:
print "Timeout for generation of static configuration with ground contact"
sys.exit(1)
return s0
def sampleRandomStateStairs(fullBody, limbsInContact, zInterval, movingLimb,
z_moving):
success = False
it = 0
while not success and it < 10000:
it += 1
s0 = createRandomState(fullBody, limbsInContact)
# try to project feet in contact (N = [0,0,1] and p[2] = z)
n = [0, 0, 1]
for limb in limbsInContact:
p = fullBody.getJointPosition(fullBody.dict_limb_joint[limb])[0:3]
if limb == movingLimb:
p[2] = z_moving
else:
p[2] = zInterval[random.randint(0, len(zInterval) - 1)]
s0, success = StateHelper.addNewContact(s0,
limb,
p,
n,
lockOtherJoints=True)
if not success:
break
if success:
# check stability
success = fullBody.isStateBalanced(s0.sId, 5)
if not success:
print(
"Timeout for generation of static configuration with ground contact"
)
sys.exit(1)
return s0
def tryCreateContactAround(s, eff_id, pos, normal, num_max_sample= 0,rotation = Quaternion.Identity(),num_try = 1000):
sres, succ = StateHelper.addNewContact(s, eff_id, pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation))
i_try = 0
x_bounds = [-0.1,0.1]
y_bounds = [-0.05,0.05]
print("try create contact around, first try success : ",succ)
print("result = ",sres.q())
while not succ and i_try < num_try:
print("try create contact around, try : ",i_try)
x = random.uniform(x_bounds[0],x_bounds[1])
y = random.uniform(y_bounds[0],y_bounds[1])
offset = np.matrix([x,y,0]).T
offset = rotation.matrix() * offset
new_pos = pos[::]
for i in range(3):
new_pos[i] += offset[i,0]
sres, succ = StateHelper.addNewContact(s, eff_id, new_pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation))
i_try += 1
return sres,succ
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
effectorsInContact = phase.effectorsInContact()
contacts = [
] # contacts should contains the limb names, not the effector names
list_effector = list(fb.dict_limb_joint.values())
for eeName in effectorsInContact:
contacts += [
list(fb.dict_limb_joint.keys())[list_effector.index(eeName)]
]
#q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(
phase, fb.DEFAULT_COM_HEIGHT).tolist()
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = phase.contactPatch(eeName).placement
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = phase.contactPatch(eeName).placement
p = placement.translation.tolist()
n = computeContactNormal(placement).tolist()
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print(
"Cannot project the configuration to contact, for effector : ",
eeName)
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print(
"cannot project com to the middle of the support polygon."
)
phase.q_init = np.array(state.q())
return state.q()
def perturbateContactNormal(fb, state_id, epsilon=1e-2):
"""
Add a small variation (+- epsilon) to the contact normals of the given state
:param fb:
:param state_id:
:param epsilon:
:return: the new state ID, -1 if fail
"""
state = State(fb, state_id)
for name in state.getLimbsInContact():
p, n = state.getCenterOfContactForLimb(name)
n[2] += uniform(-epsilon, epsilon)
n = np.array(n)
state, success = StateHelper.addNewContact(state, name, p, n.tolist())
if not success:
return -1
return state.sId
def sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] = random.uniform(eff_x_range[0], eff_x_range[1])
p[1] = random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
if success:
"""
# force root orientation : (align current z axis with vertical)
quat_1 = Quaternion(s1.q()[6],s1.q()[3],s1.q()[4],s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1,vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
"""
root = SE3.Identity()
root.translation = np.matrix(s1.q()[0:3]).T
root = sampleRotationAlongZ(root)
success = s1.projectToRoot(se3ToXYZQUATtuple(root))
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
if success:
success = projectInKinConstraints(fullBody, s1)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
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 = State(fullBody, q=q, limbsIncontact=limbIds)
succ = True
for effId, pos in zip(limbIds, positions):
s, succ = StateHelper.addNewContact(s,
effId,
pos, [0., 0., 1.],
num_max_sample=0)
if not succ:
break
# ~ posrf = fullBody.getJointPosition(rfoot)[:3]
# ~ poslf = fullBody.getJointPosition(lfoot)[:3]
# ~ print ("limbsIds ", limbIds)
# ~ s = State(fullBody, q = q, limbsIncontact = limbIds)
# ~ s, succ = StateHelper.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
# ~ if succ:
# ~ s, succ = StateHelper.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
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 test_contacts_3d(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, fullbody.rArmId, fullbody.lArmId]
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.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
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.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.45, -0.2, 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(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertTrue(state2.isBalanced())
p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n_real)
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
contacts = getActiveContactLimbs(phase, fb)
#q = phase.reference_configurations[0].T.tolist()[0] # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(phase, fb).T.tolist()[0]
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(
rootOrientationFromFeetPlacement(phase, None)[0].rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = JointPlacementForEffector(phase, eeName, fb)
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = getContactPlacement(phase, eeName, fb)
p = placement.translation.T.tolist()[0]
n = computeContactNormal(placement).T.tolist()[0]
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print "Cannot project the configuration to contact, for effector : ", eeName
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print "cannot project com to the middle of the support polygon."
phase.reference_configurations[0] = np.matrix(state.q()).T
return state.q()
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])
pRH = SE3.Identity()
pRH = rotatePlacement(pRH, 'y', -0.8115781021773631)
n = computeContactNormal(pRH).T.tolist()[0]
state = State(fb, q=q_ref, limbsIncontact=[fb.rLegId, fb.lLegId])
pLHand = [-2.75, -1.235, 1.02]
pRHand = [-2.75, -2.28, 1.02]
"""
from tools.display_tools import *
createSphere('s',v)
moveSphere('s',v,pLHand)
"""
state, success = StateHelper.addNewContact(state,
fb.rArmId,
pRHand,
n,
lockOtherJoints=True)
assert success, "Unable to create contact for right hand"
state, success = StateHelper.addNewContact(state,
fb.lArmId,
pLHand,
n,
lockOtherJoints=True)
assert success, "Unable to create contact for right hand"
q_ref = state.q()
fb.setReferenceConfig(q_ref)
fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6)
v(q_ref)
q_init[0] += 0.05
createSphere('s',r)
n = [0,0,1]
p = [0,0.1,0]
q_init[-6:-3] = [0.2,0,0]
q_goal[-6:-3] = [0.1,0,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.22, -0.019]
moveSphere('s',r,p)
smid,success = StateHelper.addNewContact(si,lLegId,p,n,100)
assert(success)
smid2,success = StateHelper.addNewContact(sf,lLegId,p,n,100)
assert(success)
r(smid.q())
sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
"""
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,timings=[0.4,0.2,0.4])
fullBody.isDynamicallyReachableFromState(si.sId,smid.sId,timings=[0.8,0.6,0.8])
fullBody.isDynamicallyReachableFromState(smid2.sId,sf.sId,timings=[0.8,0.6,0.8])
import disp_bezier
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
# create contact :
fullBody.setStartState(q_init, [fullBody.lLegId, fullBody.rLegId])
n = [0, 0, 1]
s0 = State(fullBody,
q=q_init,
limbsIncontact=[fullBody.lLegId, fullBody.rLegId])
### move left foot of 30cm in front
pLLeg = s0.getCenterOfContactForLimb(fullBody.lLegId)[0]
pLLeg[0] += 0.3
pLLeg[2] -= 0.001 # required because this delta is added in rbprm ...
s1, success = StateHelper.addNewContact(s0,
fullBody.lLegId,
pLLeg,
n,
lockOtherJoints=False)
assert success, "Unable to project contact position for left foot"
### move hand to the handle
pHand = [0.1, 0.5, 0.75]
createSphere('s', v)
moveSphere('s', v, pHand)
s2, success = StateHelper.addNewContact(s1,
fullBody.rArmId,
pHand,
n,
lockOtherJoints=False)
assert success, "Unable to project contact position for right hand"
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)
def genStateWithOneStep(fullbody, limbs, num_samples=100, coplanar=True):
q_0 = fullBody.getCurrentConfig()
#~ fullBody.getSampleConfig()
qs = []
states = []
assert (len(limbs) == 2
and "only implemented for 2 limbs in contact for now")
it = 0
while len(states) < num_samples and it < 10000:
i = len(states)
if (coplanar):
q0 = fullBody.generateGroundContact(limbs)
else:
q0 = _genbalance(fullBody, limbs)
if len(q0) > 1:
s0 = projectMidFeet(fullBody, q0, limbs)
if s0 is not None:
success = False
iter = 0
# try to make a step
while not success and iter < 100:
if (coplanar):
q2 = fullBody.generateGroundContact(limbs)
else:
q2 = _genbalance(fullBody, limbs)
if len(q2) > 1:
s2 = State(fullbody, q=q2, limbsIncontact=limbs)
moving_limb = limbs[i % 2]
[p, n] = s2.getCenterOfContactForLimb(
moving_limb
) # get position of the moving limb in the next state
s1, success = StateHelper.addNewContact(
s0, moving_limb, p, n
) # try to move the limb for s0 to it's position in s2
if success and __loosely_z_aligned(
limbs[0], s1.q()) and __loosely_z_aligned(
limbs[1], s1.q()):
fullBody.setCurrentConfig(s1.q())
com = np.array(
fullBody.getJointPosition(rfoot)[0:3])
com += np.array(
fullBody.getJointPosition(lfoot)[0:3])
com /= 2.
com[2] = fullBody.getCenterOfMass()[2]
successProj = s1.projectToCOM(com.tolist())
success = successProj and fullBody.isConfigValid(
s1.q())[0] and fullBody.isConfigBalanced(
s1.q(), limbs, 5)
else:
success = False
iter += 1
if success:
# recreate the states to assure the continuity of the index in fullBody :
state0 = State(fullBody, q=s0.q(), limbsIncontact=limbs)
states += [[s1, state0]]
print "Step " + str(i) + " done."
else:
print "cannot make the step."
else:
print "cannot project between feet"
else:
print "unable to generate first configuration."
it += 1
print "Done generating pairs of states, found : " + str(
len(states)) + " pairs."
return states
createSphere('s', r, color=r.color.red)
q_init[2] = -1.57
s0 = State(fullBody, q=q_init, limbsIncontact=[rLegId, lLegId])
#s1,success = StateHelper.addNewContact(s0,rLegId,p,[0,0,1])
#assert(success)
#r.addLandmark('talos/arm_left_7_link',0.2)
p = [-12.3, -3.26, -1.2]
moveSphere('s', r, p)
s1 = s0
p = [-12.55, -3.26, -1.65]
s2, success = StateHelper.addNewContact(s1, lArmId, p, [0, 0, 1], 10000)
r(s2.q())
p = [-12.55, -4.32, -1.65]
s3, success = StateHelper.addNewContact(s2, rArmId, p, [0, 0, 1], 10000)
r(s3.q())
s3.projectToCOM(s0.getCenterOfMass(), 10000)
r(s3.q())
p = [-12.5, -3.65, -2.39] # l foot first step
s4, success = StateHelper.addNewContact(s3, lLegId, p, [0, 0, 1], 10000)
r(s4.q())
p = [-12.5, -3.85, -2.39] # r foot first step
s5, success = StateHelper.addNewContact(s4, rLegId, p, [0, 0, 1], 10000)