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)
# 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)
cameraTF = [-0.03, 4.4, 2.3, -0.02, 0.56, 0.83, -0.03]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the FDDP solver
solver.solve()
# Plotting the entire motion
if WITHPLOT:
log = solver.getCallbacks()[0]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.stops, log.grads, log.steps, figIndex=2)
# Display the entire motion
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector)
hector.viewer.gui.addXYZaxis('world/wp', [1., 0., 0., 1.], .03, 0.5)
hector.viewer.gui.applyConfiguration(
'world/wp',
target_pos.tolist() + [target_quat[0], target_quat[1], target_quat[2], target_quat[3]])
display.displayFromSolver(solver)
# 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,
log.x_regs,
log.grads,
log.stops,
log.steps,
figTitle="Box-DDP",
figIndex=7)
# 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)
def run():
# Loading the anymal model
anymal = example_robot_data.load('anymal')
# nq is the dimension fo the configuration vector representation
# nv dimension of the velocity vector space
# Defining the initial state of the robot
q0 = anymal.model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(anymal.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot, rhFoot)
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
walking = {
'stepLength': 0.25,
'stepHeight': 0.15,
'timeStep': 1e-2,
'stepKnots': 100,
'supportKnots': 2
}
# Creating a walking problem
ddp = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, walking['stepLength'], walking['stepHeight'], walking['timeStep'],
walking['stepKnots'], walking['supportKnots']))
plot = False
display = False
if display:
# Added the callback functions
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp.setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
# Solving the problem with the DDP solver
xs = [anymal.model.defaultState] * (ddp.problem.T + 1)
us = ddp.problem.quasiStatic([anymal.model.defaultState] * ddp.problem.T)
ddp.solve(xs, us, 100, False, 0.1)
if display:
# Defining the final state as initial one for the next phase
# Display the entire motion
display = crocoddyl.GepettoDisplay(anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
display.displayFromSolver(ddp)
# Plotting the entire motion
if plot:
plotSolution(ddp, figIndex=1, show=False)
log = ddp.getCallbacks()[0]
crocoddyl.plotConvergence(log.costs,
log.u_regs,
log.x_regs,
log.grads,
log.stops,
log.steps,
figTitle='walking',
figIndex=3,
show=True)