# Setting up the 3d walking problem
lfFoot = 'lf_foot'
rfFoot = 'rf_foot'
lhFoot = 'lh_foot'
rhFoot = 'rh_foot'
walk = SimpleQuadrupedProblem(rmodel, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up the walking variables
comGoTo = 0.1 # meters
timeStep = 5e-2 # seconds
supportKnots = 2
# Creating the CoM problem and solving it
ddp = SolverDDP(walk.createProblem(x0, comGoTo, timeStep, supportKnots))
# ddp.callback = [ CallbackDDPVerbose() ]
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000,
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()))
# Checking the CoM displacement
x0 = ddp.xs[0]
xT = ddp.xs[-1]
q0 = a2m(x0[:rmodel.nq])
qT = a2m(xT[:rmodel.nq])
data0 = rmodel.createData()
dataT = rmodel.createData()
comT = pinocchio.centerOfMass(rmodel, dataT, qT)
com0 = pinocchio.centerOfMass(rmodel, data0, q0)
assert (norm(comT - com0 - np.matrix([[comGoTo], [0.], [0.]])) < 1e-3)
models[-1].differential.costs['xreg'].weight = 1000
models[-1].differential.costs['xreg'].cost.activation.weights[:] = 1
# ---------------------------------------------------------------------------------------------
# Solve both take-off and landing.
# Solve the initial phase (take-off).
PHASE_NAME = "initial"
problem = ShootingProblem(initialState=x0, runningModels=models[:imp], terminalModel=models[imp])
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()] # CallbackSolverDisplay(robot,rate=5) ]
ddp.th_stop = 1e-4
us0 = [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[:imp]
] + [np.zeros(0)] + [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
for m, d in list(zip(ddp.models(), ddp.datas()))[imp + 1:-1]
]
print("*** SOLVE %s ***" % PHASE_NAME)
ddp.solve(maxiter=PHASE_ITERATIONS[PHASE_NAME],
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()),
init_us=us0[:imp])
if PHASE_ITERATIONS[PHASE_NAME] == 0:
ddp.setCandidate(xs=[x for x in np.load(BACKUP_PATH + '%s.xs.npy' % PHASE_NAME)],
us=[u for u in np.load(BACKUP_PATH + '%s.us.npy' % PHASE_NAME)])
elif PHASE_BACKUP[PHASE_NAME]:
ddp.th_stop = 1e-6
ddp.setCandidate()
m = models[0]
d = m.createData()
m = m.differential
d = d.differential
# m=m.differential.contact['contact30']
# d=d.differential.contact['contact30']
m.calcDiff(d, ddp.xs[0], ddp.us[0])
# m.calc(d,ddp.xs[0])
ddp.solve(maxiter=1,
regInit=.1,
init_xs=[rmodel.defaultState] * len(ddp.models()),
init_us=[
_m.differential.quasiStatic(_d.differential, rmodel.defaultState)
for _m, _d in list(zip(ddp.models(), ddp.datas()))[:-1]
])
assert (ddp.cost < 1e5)
'''
# --- PLOT
np.set_printoptions(precision=4, linewidth=200, suppress=True)
import matplotlib.pylab as plt
plt.ion()
plt.plot([ d.differential.pinocchio.com[0][0,0] for d in ddp.datas() ])
'''
runningModels=models[:-1],
terminalModel=models[-1])
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if 'cb' in sys.argv and WITHDISPLAY:
ddp.callback.append(CallbackSolverDisplay(robot, rate=-1))
ddp.th_stop = 1e-6
fddp = SolverFDDP(problem)
fddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if 'cb' in sys.argv and WITHDISPLAY:
fddp.callback.append(CallbackSolverDisplay(robot, rate=-1))
fddp.th_stop = 1e-6
us0 = [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
if isinstance(m, IntegratedActionModelEuler) else np.zeros(0)
for m, d in zip(ddp.problem.runningModels, ddp.problem.runningDatas)
]
xs0 = [rmodel.defaultState] * len(ddp.models())
xs1 = [problem.initialState] * len(ddp.models())
dimp1 = ddp.datas()[imp1]
dimp2 = ddp.datas()[imp2]
print("*** SOLVE ***")
fddp.solve(maxiter=50,
# ,init_xs=xs0
# ,init_xs=xs1
# ,init_us=us0
)