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)
ddpbox.computeDirection()
xddp_box, uddp_box, costddp_box = ddpbox.forwardPass(stepLength=1)
# 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 v1xx = problem.terminalData.Lxx