pass1 = models[10]
pass2 = models[21]
pass1.differential.costs['com'].weight = 100000
pass2.differential.costs['com'].weight = 100000
pass2.differential.costs['track16'].weight = 100000
# --- DDP
problem = ShootingProblem(initialState=x0,
runningModels=models[:-1],
terminalModel=models[-1])
ddp = SolverDDP(problem)
# ddp.callback = [ CallbackDDPLogger(), CallbackDDPVerbose() ]
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=[
problem = ShootingProblem(model.State.zero() + 2, [model] * T, model)
xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]]
us = [np.zeros(m.nu) for m in problem.runningModels]
x0ref = problem.initialState
xs[0] = np.random.rand(nx)
xs[1] = np.random.rand(nx)
us[0] = np.random.rand(nu)
# xs[1] = model.calc(data,xs[0],us[0])[0].copy()
ddpbox = SolverBoxDDP(problem)
ddpbox.qpsolver = quadprogWrapper
ddp = SolverDDP(problem)
ddp.setCandidate(xs, us)
ddp.computeDirection()
xddp, uddp, costddp = ddp.forwardPass(stepLength=1)
# The argmin value is barely within the limits of the control (one dimension is on limit)
ddpbox.ul = np.array([
min(np.minimum(uddp[0], min(us[0]))),
] * nu)
ddpbox.uu = np.array([
max(np.maximum(uddp[0], max(us[0]))),
] * nu)
# ddpbox.ul = np.array([-3.,]*nu)
# ddpbox.uu = np.array([3.,]*nu)
ddpbox.setCandidate(xs, us)
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]:
np.save(BACKUP_PATH + '%s.xs.npy' % PHASE_NAME, ddp.xs)
np.save(BACKUP_PATH + '%s.us.npy' % PHASE_NAME, ddp.us)
# ---------------------------------------------------------------------------------------------
# Solve both take-off and landing.
PHASE_NAME = "landing"
xsddp = ddp.xs
usddp = ddp.us
problem = ShootingProblem(initialState=x0, runningModels=models[:-1], terminalModel=models[-1])
ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()] # CallbackSolverDisplay(robot,rate=5,freq=10) ]
us = usddp + [
model = ActionModelLQR(1, 1, driftFree=False) data = model.createData() # model = ActionModelUnicycle() nx, nu = model.nx, model.nu problem = ShootingProblem(model.State.zero() + 1, [model], model) ddp = SolverDDP(problem) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] # xs[0][:] = problem.initialState xs[0] = np.random.rand(nx) us[0] = np.random.rand(nu) xs[1] = np.random.rand(nx) ddp.setCandidate(xs, us) ddp.computeDirection() xnew, unew, cnew = ddp.forwardPass(stepLength=1) # Check versus simple (1-step) DDP ddp.problem.calcDiff(xs, us) l0x = problem.runningDatas[0].Lx l0u = problem.runningDatas[0].Lu l0xx = problem.runningDatas[0].Lxx l0xu = problem.runningDatas[0].Lxu l0uu = problem.runningDatas[0].Luu f0x = problem.runningDatas[0].Fx f0u = problem.runningDatas[0].Fu x1pred = problem.runningDatas[0].xnext v1x = problem.terminalData.Lx