# Defining the walking gait parameters
jumping_gait = {
'jumpHeight': 0.15,
'jumpLength': [0.3, 0., 0.],
'timeStep': 1e-2,
'groundKnots': 20,
'flyingKnots': 20
}
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
# Creating a both solvers
boxfddp = crocoddyl.SolverBoxFDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
boxddp = crocoddyl.SolverBoxDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
# Added the callback functions
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(
anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxfddp.setCallbacks([
crocoddyl.CallbackLogger(),
terminalCostModel.addCost("limitCost", limitCost, 1e3)
# Create the action model
dmodelRunning = crocoddyl.DifferentialActionModelContactFwdDynamics(
state, actuation, contactModel, runningCostModel)
dmodelTerminal = crocoddyl.DifferentialActionModelContactFwdDynamics(
state, actuation, contactModel, terminalCostModel)
runningModel = crocoddyl.IntegratedActionModelEuler(dmodelRunning, DT)
terminalModel = crocoddyl.IntegratedActionModelEuler(dmodelTerminal, 0)
# Problem definition
x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
solver = crocoddyl.SolverBoxFDDP(problem)
if WITHDISPLAY and WITHPLOT:
solver.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(
crocoddyl.GepettoDisplay(robot,
4,
4,
frameNames=[rightFoot, leftFoot]))
])
elif WITHDISPLAY:
solver.setCallbacks([
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(
crocoddyl.GepettoDisplay(robot,
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}},
{'walking': {'stepLength': 0.6, 'stepHeight': 0.1,
'timeStep': 0.03, 'stepKnots': 35, 'supportKnots': 10}}]
cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
solver = [None] * len(GAITPHASES)
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
solver[i] = crocoddyl.SolverBoxFDDP(
gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots']))
solver[i].th_stop = 1e-7
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_legs,
4,
4,
cameraTF,
frameNames=[rightFoot, leftFoot])
solver[i].setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),