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() ])
'''
xref[:7] = x0[:7]
xref[3:7] = [0, 0, 0,
1] # TODO: remove this after adding assertion to include any case
pinocchio.forwardKinematics(rmodel, rdata, a2m(xref[:rmodel.nq]))
pinocchio.updateFramePlacements(rmodel, rdata)
c1.ref[:] = m2a(rdata.oMf[c1.frame].translation.copy())
problem = ShootingProblem(x0, [model], model)
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger()]
ddp.th_stop = 1e-18
xddp, uddp, doneddp = ddp.solve(maxiter=400)
assert (doneddp)
assert (norm(ddp.datas()[-1].differential.costs['pos'].residuals) < 1e-3)
assert (norm(
m2a(ddp.datas()[-1].differential.costs['pos'].pinocchio.oMf[
c1.frame].translation) - c1.ref) < 1e-3)
u0 = np.zeros(model.nu)
x1 = model.calc(data, problem.initialState, u0)[0]
x0s = [problem.initialState.copy(), x1]
u0s = [u0.copy()]
dmodel.costs['regu'].weight = 1e-3
kkt = SolverKKT(problem)
kkt.th_stop = 1e-18
xkkt, ukkt, donekkt = kkt.solve(init_xs=x0s,
init_us=u0s,
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
)
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]:
# ---------------------------------------------------------------------------------------------
# ---------------------------------------------------------------------------------------------
# ---------------------------------------------------------------------------------------------
# Solve only the take-off for an initial vanilla-flavor jump
# Solve the initial phase (take-off).
PHASE_NAME = "initial"
problem = ShootingProblem(initialState=x0,
runningModels=models[:imp],
terminalModel=models[imp])
ddp = SolverDDP(problem)
ddp.alphas = [4**(-n) for n in range(10)]
ddp.callback = [CallbackDDPVerbose()]
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]
]
if PHASE_ITERATIONS[PHASE_NAME] > 0:
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.xs = [x for x in np.load(BACKUP_PATH + '%s.xs.npy' % PHASE_NAME)]
ddp.us = [u for u in np.load(BACKUP_PATH + '%s.us.npy' % PHASE_NAME)]