def createProblem(gait_phase):
robot_model = example_robot_data.loadTalosLegs().model
rightFoot, leftFoot = 'right_sole_link', 'left_sole_link'
gait = SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot)
q0 = robot_model.referenceConfigurations['half_sitting'].copy()
v0 = pinocchio.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
type_of_gait = list(gait_phase.keys())[0]
value = gait_phase[type_of_gait]
if type_of_gait == 'walking':
# Creating a walking problem
problem = gait.createWalkingProblem(x0, value['stepLength'],
value['stepHeight'],
value['timeStep'],
value['stepKnots'],
value['supportKnots'])
xs = [robot_model.defaultState] * (len(problem.runningModels) + 1)
us = [
m.quasiStatic(d, robot_model.defaultState)
for m, d in list(zip(problem.runningModels, problem.runningDatas))
]
return xs, us, problem
'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}},
{'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]
ddp = [None] * len(GAITPHASES)
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
ddp[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])
ddp[i].setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot])
ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp[i].setCallbacks([