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)