コード例 #1
0
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)
コード例 #2
0
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)
コード例 #3
0
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)