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)
for m, d in list(zip(boxddp.models(), boxddp.datas()))[:-1]
]
# Solve the DDP problem
boxddp_start = time.time()
boxddp.solve(xs, us, 1000, False, 0.1)
boxddp_end = time.time()
print("[Box-DDP] Solved in", boxddp_end - boxddp_start, "-", boxddp.iter,
"iterations")
# Plotting the entire motion
if WITHPLOT:
# Plot control vs limits
fig = plt.figure(1)
plt.title('Control ($u$)')
crocoddyl.plotOCSolution(us=boxddp.us, figIndex=1, show=False)
plt.xlim(0, len(boxddp.us) - 1)
plt.hlines(boxddp.models()[0].u_lb, 0, len(boxddp.us) - 1, 'r')
plt.hlines(boxddp.models()[0].u_ub, 0, len(boxddp.us) - 1, 'r')
plt.tight_layout()
# Plot convergence
log = boxddp.getCallbacks()[0]
crocoddyl.plotConvergence(log.costs,
log.control_regs,
log.state_regs,
log.gm_stops,
log.th_stops,
log.steps,
figIndex=2)
