m.differential.costs['xreg'].weight = 0.0
m.differential.contact['contact16'].gains[1] = 30
m.differential.contact['contact30'].gains[1] = 30
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:
acc = dx[rmodel.nv:] / dt
u = pinocchio.rnea(rmodel, rdata, a2m(xp[:rmodel.nq]),
a2m(xp[rmodel.nq:]), a2m(acc))
m.differential.calc(d.differential, init.X[-1])
contactJ = d.differential.contact.J
f = np.dot(np.linalg.pinv(contactJ.T[:6, :]), u[:6])
u -= (np.dot(contactJ.transpose(), f))
init.U.append(np.array(u[6:]).squeeze().copy())
init.X.append(conf.X_init[-1])
# ---------------Display Initial Trajectory--------------
if conf.DISPLAY:
robot.initDisplay(loadModel=True)
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPVerbose()] # CallbackSolverTimer()]
if conf.RUNTIME_DISPLAY:
ddp.callback.append(CallbackSolverDisplay(robot, 4))
ddp.th_stop = 1e-9
ddp.solve(maxiter=1000, regInit=0.1, init_xs=init.X, init_us=init.U)
# ---------------Display Final Trajectory--------------
if conf.DISPLAY:
for x in init.X:
robot.display(a2m(x[:robot.nq]))
# sleep(0.005)
# ----------------------
# --- DDP VERSUS KKT : integrative test ---
np.random.seed(220)
model = ActionModelUnicycle()
nx = model.nx
nu = model.nu
T = 20
problem = ShootingProblem(model.State.zero() + 2, [model] * T, model)
xs = [m.State.rand() for m in problem.runningModels + [problem.terminalModel]]
us = [np.random.rand(nu) for m in problem.runningModels]
x0ref = problem.initialState
ddp = SolverDDP(problem)
kkt = SolverKKT(problem)
ddp.th_stop = 1e-18
kkt.th_stop = 1e-18
xkkt, ukkt, donekkt = kkt.solve(maxiter=200, init_xs=xs, init_us=us)
xddp, uddp, doneddp = ddp.solve(maxiter=200, init_xs=xs, init_us=us, regInit=0)
assert (donekkt)
assert (doneddp)
assert (norm(xkkt[0] - problem.initialState) < 1e-9)
assert (norm(xddp[0] - problem.initialState) < 1e-9)
for t in range(problem.T):
assert (norm(ukkt[t] - uddp[t]) < 1e-6)
assert (norm(xkkt[t + 1] - xddp[t + 1]) < 1e-6)
# --- Test with manifold dynamics
model = ActionModelUnicycleVar()