def moveEffectorToPlacement(fb,
v,
cs,
eeName,
placement,
initStateCenterSupportPolygon=True,
projectCOM=False):
print "## Move effector " + eeName + " to placement : " + str(
placement.translation.T)
prev_phase = cs.contact_phases[-1]
if isContactActive(prev_phase, eeName, fb):
print "#Contact repositionning, add two new phases."
removeContact(fb, cs, eeName)
print "+ Contact creation : ", eeName
phase = ContactPhaseHumanoid(cs.contact_phases[-1])
patch = contactPatchForEffector(phase, eeName, fb)
patch.placement = placement
patch.active = True
q = generateConfigFromPhase(fb, phase, projectCOM)
fb.setCurrentConfig(q)
state = np.matrix(np.zeros(9)).T
if initStateCenterSupportPolygon:
state[0:3] = computeCenterOfSupportPolygonFromPhase(phase, fb)
else:
state[0:3] = np.matrix(fb.getCenterOfMass()).T
phase.init_state = state.copy()
prev_phase.final_state = state.copy()
cs.contact_phases.append(phase)
def moveEffectorOf(fb, v, cs, eeName, translation):
print "## Move effector " + eeName + " of : " + str(translation)
prev_phase = cs.contact_phases[-1]
assert isContactActive(
prev_phase, eeName, fb
), "Cannot use 'moveEffectorOf' if the effector is not in contact in the last phase."
placement = getContactPlacement(prev_phase, eeName, fb).copy()
if isinstance(translation, types.ListType):
translation = np.matrix(translation).T
assert translation.shape[0] == 3, "translation must be a 3D vector"
placement.translation += translation
moveEffectorToPlacement(fb, v, cs, eeName, placement)
def walk(fb, v, cs, distance, stepLength, gait):
fb.usePosturalTaskContactCreation(True)
prev_phase = cs.contact_phases[-1]
for limb in gait:
eeName = fb.dict_limb_joint[limb]
assert isContactActive(
prev_phase, eeName,
fb), "All limbs in gait should be in contact in the first phase"
isFirst = True
reached = False
firstContactReachedGoal = False
remainingDistance = distance
while remainingDistance >= 0:
for k, limb in enumerate(gait):
if isFirst:
length = stepLength / 2.
isFirst = False
else:
length = stepLength
if k == 0:
if length > (remainingDistance + stepLength / 2.):
length = remainingDistance + stepLength / 2.
firstContactReachedGoal = True
else:
if length > remainingDistance:
length = remainingDistance
translation = [length, 0, 0]
moveEffectorOf(fb, v, cs, fb.dict_limb_joint[limb], translation)
remainingDistance -= stepLength
if not firstContactReachedGoal:
translation = [stepLength / 2., 0, 0]
moveEffectorOf(fb, v, cs, fb.dict_limb_joint[gait[0]], translation)
q_end = fb.referenceConfig[::] + [0] * 6
q_end[0] += distance
setFinalState(cs, q=q_end)
fb.usePosturalTaskContactCreation(False)