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,
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)
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(
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)
# 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)