Beispiel #1
0
        crocoddyl.CallbackLogger(),
        crocoddyl.CallbackVerbose(),
    ])
else:
    boxddp.setCallbacks([crocoddyl.CallbackVerbose()])

xs = [robot_model.defaultState] * (boxddp.problem.T + 1)
us = boxddp.problem.quasiStatic([solo.model.defaultState] * boxddp.problem.T)

# Solve the DDP problem
boxddp.solve(xs, us, 100, False, 0.1)

# Plotting the entire motion
if WITHPLOT:
    # Plot control vs limits
    plotSolution(boxddp, bounds=True, figIndex=1, show=False)

    # Plot convergence
    log = boxddp.getCallbacks()[0]
    crocoddyl.plotConvergence(log.costs,
                              log.u_regs,
                              log.x_regs,
                              log.grads,
                              log.stops,
                              log.steps,
                              figIndex=3,
                              show=True)

# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(
        anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
    display.displayFromSolver(boxfddp)

print('*** SOLVE with Box-DDP ***')
boxddp.th_stop = 1e-7
boxddp.solve(xs, us, 30, False)

# 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,
Beispiel #3
0
    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(
        anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
    for i, phase in enumerate(GAITPHASES):
        display.displayFromSolver(ddp[i])

# Plotting the entire motion
if WITHPLOT:
    log = ddp[0].getCallbacks()[0]
    plotSolution(ddp, 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)
Beispiel #4
0
elif WITHPLOT:
    solver.setCallbacks(
        [crocoddyl.CallbackLogger(),
         crocoddyl.CallbackVerbose()])
else:
    solver.setCallbacks([crocoddyl.CallbackVerbose()])

# Solve the DDP problem
xs = [x0] * (solver.problem.T + 1)
us = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs, us, 100, False, 0.1)

# Plotting the entire motion
if WITHPLOT:
    # Plot control vs limits
    plotSolution(solver, bounds=True, figIndex=1, show=False)

    # Plot convergence
    log = solver.getCallbacks()[0]
    crocoddyl.plotConvergence(log.costs,
                              log.u_regs,
                              log.x_regs,
                              log.grads,
                              log.stops,
                              log.steps,
                              figIndex=3,
                              show=True)

# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(
Beispiel #5
0
    solver[i].solve(xs, us, 100, False)

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

# Display the entire motion
if WITHDISPLAY:
    display = crocoddyl.GepettoDisplay(
        anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
    for i, phase in enumerate(GAITPHASES):
        display.displayFromSolver(solver[i])

# Plotting the entire motion
if WITHPLOT:
    log = solver[0].getCallbacks()[0]
    plotSolution(solver, figIndex=1, show=False)

    for i, phase in enumerate(GAITPHASES):
        title = list(phase.keys())[0] + " (phase " + str(i) + ")"
        log = solver[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)
Beispiel #6
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)
            crocoddyl.CallbackSolverDisplay(hyq, 4, 4, cameraTF)
        ])
    else:
        ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])

    # Solving the problem with the DDP solver
    ddp[i].th_stop = 1e-9
    xs = [hyq.model.defaultState] * len(ddp[i].models())
    us = [
        m.quasicStatic(d, hyq.model.defaultState)
        for m, d in list(zip(ddp[i].models(), ddp[i].datas()))[:-1]
    ]
    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:
    for i, phase in enumerate(GAITPHASES):
        crocoddyl.displayTrajectory(hyq, ddp[i].xs, ddp[i].models()[0].dt)

# Plotting the entire motion
if WITHPLOT:
    xs = []
    us = []
    for i, phase in enumerate(GAITPHASES):
        xs.extend(ddp[i].xs)
        us.extend(ddp[i].us)
    plotSolution(hyq.model, xs, us)