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
# sphere = target
from hpp.corbaserver.rbprm.tools.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, [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,
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
