def setUp(self):
# Creating the model
self.model = self.MODEL
self.data = self.model.createData()
# Defining the shooting problem
self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model)
# Creating the KKT solver
self.kkt = SolverKKT(self.problem)
# Setting up a warm-point
xs = [m.State.zero() for m in self.problem.runningModels + [self.problem.terminalModel]]
us = [np.zeros(m.nu) for m in self.problem.runningModels]
self.kkt.setCandidate(xs, us)
class SolverKKTTest(unittest.TestCase):
MODEL = ActionModelUnicycle()
def setUp(self):
# Creating the model
self.model = self.MODEL
self.data = self.model.createData()
# Defining the shooting problem
self.problem = ShootingProblem(self.model.State.zero(), [self.model, self.model], self.model)
# Creating the KKT solver
self.kkt = SolverKKT(self.problem)
# Setting up a warm-point
xs = [m.State.zero() for m in self.problem.runningModels + [self.problem.terminalModel]]
us = [np.zeros(m.nu) for m in self.problem.runningModels]
self.kkt.setCandidate(xs, us)
def test_dimension_of_kkt_problem(self):
# Compute the KKT matrix
self.kkt.calc()
# Getting the dimension of the KKT problem
nxu = len(list(filter(lambda x: x > 0, eig(self.kkt.kkt)[0])))
nx = len(list(filter(lambda x: x < 0, eig(self.kkt.kkt)[0])))
# Checking the dimension of the KKT problem
self.assertTrue(nxu == self.kkt.nx + self.kkt.nu, "Dimension of decision variables is wrong.")
self.assertTrue(nx == self.kkt.nx, "Dimension of state variables is wrong.")
def test_hessian_is_symmetric(self):
# Computing the KKT matrix
self.kkt.calc()
# Checking the symmetricity of the Hessian
self.assertTrue(np.linalg.norm(self.kkt.hess - self.kkt.hess.T) < 1e-9, "The Hessian isn't symmetric.")
def test_search_direction(self):
dxs, dus, ls = self.kkt.computeDirection()
# Checking that the first primal variable ensures initial constraint
self.assertTrue(
np.linalg.norm(dxs[0] - self.problem.initialState) < 1e-9, "Initial constraint isn't guaranteed.")
# Checking that primal variables ensures dynamic constraint (or its
# linear approximation)
LQR = isinstance(self.model, ActionModelLQR)
h = 1 if LQR else 1e-6
for i, _ in enumerate(dus):
# Computing the next state
xnext = self.model.calc(self.data, dxs[i] * h, dus[i] * h)[0] / h
# Checking that the next primal variable is consistant with the
# dynamics
self.assertTrue(np.allclose(xnext, dxs, atol=10 * h),
"Primal variables doesn't ensure dynamic constraints.")
assertNumDiff(data.Fu, dnum.Fu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lx, dnum.Lx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(data.Lu, dnum.Lu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxx, data.Lxx,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Lxu, data.Lxu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(dnum.Luu, data.Luu,
NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 3.16e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
# -------------------------------------------------------------------------------
# -------------------------------------------------------------------------------
# -------------------------------------------------------------------------------
# --- DDP FOR THE ARM ---
dmodel = DifferentialActionModelPositioning(rmodel)
model = IntegratedActionModelEuler(dmodel)
problem = ShootingProblem(model.State.zero() + 1, [model], model)
kkt = SolverKKT(problem)
kkt.th_stop = 1e-18
xkkt, ukkt, dkkt = kkt.solve()
ddp = SolverDDP(problem)
ddp.th_stop = 1e-18
xddp, uddp, dddp = ddp.solve()
assert (norm(uddp[0] - ukkt[0]) < 1e-6)
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,
isFeasible=True,
maxiter=20)
xddp, uddp, doneddp = ddp.solve(init_xs=x0s,
init_us=u0s,
isFeasible=True,
maxiter=20)
assert (donekkt)
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)
# Check that the new uddp_box is clamped at the new limit
# Check that all other values of uddp_box are the same as the previous uddp
if limit_lower:
assert (uddp_box[0][limit_id] == ddpbox.ul[limit_id])
ok_range = list(range(nu))
ok_range.pop(limit_id[0])
assert (np.allclose(uddp[0][ok_range], uddp_box[0][ok_range], atol=1e-9))
else:
assert (uddp_box[0][limit_id] == ddpbox.uu[limit_id])
ok_range = list(range(nu))
ok_range.pop(limit_id[0])
assert (np.allclose(uddp[0][ok_range], uddp_box[0][ok_range], atol=1e-9))
# KKT vs Box KKT
kkt = SolverKKT(problem)
xkkt, ukkt, donekkt = kkt.solve(maxiter=2, init_xs=xs, init_us=us)
boxkkt = SolverBoxKKT(problem)
xkkt_box, ukkt_box, donekkt_box = boxkkt.solve(maxiter=2, init_xs=xs, init_us=us)
assert (np.allclose(xkkt[0], xkkt_box[0], atol=1e-9))
assert (np.allclose(xkkt[1], xkkt_box[1], atol=1e-9))
assert (np.allclose(ukkt[0], ukkt_box[0], atol=1e-9))
# BOX DDP VS BOX KKT
xkkt_box, ukkt_box, donekkt_box = boxkkt.solve(maxiter=2,
init_xs=xs,
init_us=us,
qpsolver=quadprogWrapper,
ul=ddpbox.ul,
# --- TEST KKT --- # --------------------------------------------------- # --------------------------------------------------- # --------------------------------------------------- NX = 1 NU = 1 model = ActionModelUnicycle() data = model.createData() LQR = isinstance(model, ActionModelLQR) x = model.State.rand() u = np.zeros([model.nu]) problem = ShootingProblem(model.State.zero(), [model, model], model) kkt = SolverKKT(problem) xs = [m.State.zero() for m in problem.runningModels + [problem.terminalModel]] us = [np.zeros(m.nu) for m in problem.runningModels] # Test dimensions of KKT calc. kkt.setCandidate(xs, us) # Test that the solution respect the dynamics (or linear approx of it). dxs, dus, ls = kkt.computeDirection() x0, x1, x2 = dxs u0, u1 = dus l0, l1, l2 = ls # If LQR. test that a random solution respect the dynamics xs = [m.State.rand() for m in problem.runningModels + [problem.terminalModel]] us = [np.random.rand(m.nu) for m in problem.runningModels]