Exemplo n.º 1
0
    ddp.setCallbacks(
        [crocoddyl.CallbackVerbose(),
         crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
    ddp.setCallbacks([
        crocoddyl.CallbackLogger(),
        crocoddyl.CallbackVerbose(),
    ])
else:
    ddp.setCallbacks([crocoddyl.CallbackVerbose()])

# Solving it with the DDP algorithm
ddp.solve()

# Plotting the solution and the DDP convergence
if WITHPLOT:
    log = ddp.getCallbacks()[0]
    crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
    crocoddyl.plotConvergence(log.costs,
                              log.control_regs,
                              log.state_regs,
                              log.gm_stops,
                              log.th_stops,
                              log.steps,
                              figIndex=2)

# Visualizing the solution in gepetto-viewer
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
    display.display(talos_arm, ddp.xs, None, runningModel.dt)
Exemplo n.º 2
0
    # Solving the problem with the DDP solver
    xs = [talos_legs.model.defaultState] * (ddp[i].problem.T + 1)
    us = ddp[i].problem.quasiStatic([talos_legs.model.defaultState] * ddp[i].problem.T)
    ddp[i].solve(xs, us, 100, False, 0.1)

    # Defining the final state as initial one for the next phase
    x0 = ddp[i].xs[-1]

# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(talos_legs, frameNames=[rightFoot, leftFoot])
    for i, phase in enumerate(GAITPHASES):
        display.displayFromSolver(ddp[i])

# Plotting the entire motion
if WITHPLOT:
    plotSolution(ddp, bounds=False, figIndex=1, show=False)

    for i, phase in enumerate(GAITPHASES):
        title = list(phase.keys())[0] + " (phase " + str(i) + ")"
        log = ddp[i].getCallbacks()[0]
        crocoddyl.plotConvergence(log.costs,
                                  log.u_regs,
                                  log.x_regs,
                                  log.grads,
                                  log.stops,
                                  log.steps,
                                  figTitle=title,
                                  figIndex=i + 3,
                                  show=True if i == len(GAITPHASES) - 1 else False)
Exemplo n.º 3
0
cameraTF = [-0.03, 4.4, 2.3, -0.02, 0.56, 0.83, -0.03]
if WITHDISPLAY and WITHPLOT:
    display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
    solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
    solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
    solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
    solver.setCallbacks([crocoddyl.CallbackVerbose()])

# Solving the problem with the FDDP solver
solver.solve()

# Plotting the entire motion
if WITHPLOT:
    log = solver.getCallbacks()[0]
    crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
    crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.stops, log.grads, log.steps, figIndex=2)

# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(hector)
    hector.viewer.gui.addXYZaxis('world/wp', [1., 0., 0., 1.], .03, 0.5)
    hector.viewer.gui.applyConfiguration(
        'world/wp',
        target_pos.tolist() + [target_quat[0], target_quat[1], target_quat[2], target_quat[3]])

    display.displayFromSolver(solver)
Exemplo n.º 4
0
# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(
        anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
    display.displayFromSolver(boxddp)

# Plotting the entire motion
if WITHPLOT:
    plotSolution(boxfddp, figIndex=1, figTitle="Box-FDDP", show=False)
    plotSolution(boxddp, figIndex=3, figTitle="Box-DDP", show=False)

    log = boxfddp.getCallbacks()[0]
    crocoddyl.plotConvergence(log.costs,
                              log.u_regs,
                              log.x_regs,
                              log.grads,
                              log.stops,
                              log.steps,
                              show=False,
                              figTitle="Box-FDDP",
                              figIndex=5)
    log = boxddp.getCallbacks()[0]
    crocoddyl.plotConvergence(log.costs,
                              log.u_regs,
                              log.x_regs,
                              log.grads,
                              log.stops,
                              log.steps,
                              figTitle="Box-DDP",
                              figIndex=7)
Exemplo n.º 5
0
# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(
        anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
    for i, phase in enumerate(GAITPHASES):
        display.displayFromSolver(ddp[i])

# Plotting the entire motion
if WITHPLOT:
    xs = []
    us = []
    for i, phase in enumerate(GAITPHASES):
        xs.extend(ddp[i].xs[:-1])
        us.extend(ddp[i].us)
    log = ddp[0].getCallbacks()[0]
    plotSolution(anymal.model, xs, us, figIndex=1, show=False)

    for i, phase in enumerate(GAITPHASES):
        title = phase.keys()[0] + " (phase " + str(i) + ")"
        log = ddp[i].getCallbacks()[0]
        crocoddyl.plotConvergence(log.costs,
                                  log.control_regs,
                                  log.state_regs,
                                  log.gm_stops,
                                  log.th_stops,
                                  log.steps,
                                  figTitle=title,
                                  figIndex=i + 3,
                                  show=True if i == len(GAITPHASES) -
                                  1 else False)
Exemplo n.º 6
0
def run():
    # Loading the anymal model
    anymal = example_robot_data.load('anymal')

    # nq is the dimension fo the configuration vector representation
    # nv dimension of the velocity vector space

    # Defining the initial state of the robot
    q0 = anymal.model.referenceConfigurations['standing'].copy()
    v0 = pinocchio.utils.zero(anymal.model.nv)
    x0 = np.concatenate([q0, v0])

    # Setting up the 3d walking problem
    lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
    gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot, rhFoot)

    cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
    walking = {
        'stepLength': 0.25,
        'stepHeight': 0.15,
        'timeStep': 1e-2,
        'stepKnots': 100,
        'supportKnots': 2
    }
    # Creating a walking problem
    ddp = crocoddyl.SolverFDDP(
        gait.createWalkingProblem(x0, walking['stepLength'], walking['stepHeight'], walking['timeStep'],
                                  walking['stepKnots'], walking['supportKnots']))
    plot = False
    display = False
    if display:
        # Added the callback functions
        display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
        ddp.setCallbacks(
            [crocoddyl.CallbackLogger(),
             crocoddyl.CallbackVerbose(),
             crocoddyl.CallbackDisplay(display)])


    # Solving the problem with the DDP solver
    xs = [anymal.model.defaultState] * (ddp.problem.T + 1)
    us = ddp.problem.quasiStatic([anymal.model.defaultState] * ddp.problem.T)
    ddp.solve(xs, us, 100, False, 0.1)

    if display:
        # Defining the final state as initial one for the next phase
        # Display the entire motion
        display = crocoddyl.GepettoDisplay(anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
        display.displayFromSolver(ddp)
    # Plotting the entire motion
    if plot:
        plotSolution(ddp, figIndex=1, show=False)

        log = ddp.getCallbacks()[0]
        crocoddyl.plotConvergence(log.costs,
                                  log.u_regs,
                                  log.x_regs,
                                  log.grads,
                                  log.stops,
                                  log.steps,
                                  figTitle='walking',
                                  figIndex=3,
                                  show=True)