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)