# Solving the problem with the DDP solver
xs = [talos_legs.model.defaultState] * (ddp[i].problem.T + 1)
us = ddp[i].problem.quasiStatic([talos_legs.model.defaultState] * ddp[i].problem.T)
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(talos_legs, frameNames=[rightFoot, leftFoot])
for i, phase in enumerate(GAITPHASES):
display.displayFromSolver(ddp[i])
# Plotting the entire motion
if WITHPLOT:
plotSolution(ddp, bounds=False, 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)
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
if WITHDISPLAY:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_legs, 4, 4, cameraTF)])
else:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the DDP solver
ddp[i].th_stop = 1e-9
xs = [talos_legs.model.defaultState] * len(ddp[i].models())
us = [m.quasicStatic(d, talos_legs.model.defaultState) for m, d in list(zip(ddp[i].models(), ddp[i].datas()))[:-1]]
ddp[i].solve(xs, us, 1000, 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(talos_legs, 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(talos_legs.model, xs, us)
# Visualizing the solution in gepetto-viewer
if WITHDISPLAY:
display.displayFromSolver(solver)
# Get final state and end effector position
xT = solver.xs[-1]
pinocchio.forwardKinematics(rmodel, rdata, xT[:state.nq])
pinocchio.updateFramePlacements(rmodel, rdata)
com = pinocchio.centerOfMass(rmodel, rdata, xT[:state.nq])
finalPosEff = np.array(
rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation.T.flat)
print('Finally reached = ', finalPosEff)
print('Distance between hand and target = ',
np.linalg.norm(finalPosEff - target))
print('Distance to default state = ', np.linalg.norm(x0 - np.array(xT.flat)))
print('XY distance to CoM reference = ', np.linalg.norm(com[:2] - comRef[:2]))
# Plotting the entire motion
if WITHPLOT:
log = solver.getCallbacks()[0]
plotSolution(solver, bounds=False, figIndex=1, show=False)
crocoddyl.plotConvergence(log.costs,
log.u_regs,
log.x_regs,
log.grads,
log.stops,
log.steps,
figIndex=3)
# Display the entire motion
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_legs,
frameNames=[rightFoot, leftFoot])
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(talos_legs.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)
