def test_throw_handler(self):
ps = copra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0, self.nbStep)
controller = copra.LMPC(ps)
# Test trajectory constraint throws
with self.assertRaises(RuntimeError):
constr = copra.TrajectoryConstraint(np.identity(5), np.ones((2, )))
controller.add_constraint(constr)
# Test control constraint throws
with self.assertRaises(RuntimeError):
constr = copra.ControlConstraint(np.identity(5), np.ones((2, )))
controller.add_constraint(constr)
# Test mixed constraint throws
with self.assertRaises(RuntimeError):
constr = copra.MixedConstraint(np.identity(5), np.identity(5),
np.ones((2, )))
controller.add_constraint(constr)
# Test trajectory bound constraint throws
with self.assertRaises(RuntimeError):
constr = copra.TrajectoryBoundConstraint(np.ones((3, )),
np.ones((2, )))
controller.add_constraint(constr)
# Test control bound constraint throws
with self.assertRaises(RuntimeError):
constr = copra.ControlBoundConstraint(np.ones((3, )), np.ones(
(2, )))
controller.add_constraint(constr)
def test_lmpc_mixed(self):
print "test_lmpc_mixed"
ps = pyCopra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0, self.nbStep)
controller = pyCopra.LMPC(ps)
xCost = pyCopra.TargetCost(self.M, -self.xd)
uCost = pyCopra.ControlCost(self.N, -self.ud)
mixedConstr = pyCopra.MixedConstraint(self.Eineq, self.Gineq,
self.hineq)
xCost.weights(self.wx)
uCost.weights(self.wu)
controller.add_cost(xCost)
controller.add_cost(uCost)
controller.add_constraint(mixedConstr)
self.assertTrue(controller.solve())
control = controller.control()
fullTraj = controller.trajectory()
fTLen = len(fullTraj) / 2
posTraj = [0.] * fTLen
velTraj = [0.] * fTLen
for i in xrange(fTLen):
posTraj[i] = fullTraj[2 * i]
velTraj[i] = fullTraj[2 * i + 1]
self.assertAlmostEqual(self.xd[1], velTraj[-1], places=3)
self.assertLessEqual(max(posTraj), self.x0[0])
for i in xrange(self.Eineq.shape[0]):
res = 0
for j in xrange(self.Eineq.shape[1]):
res += self.Eineq[i, j] * fullTraj[i * self.Eineq.shape[1] + j]
for j in xrange(self.Gineq.shape[1]):
res += self.Gineq[i, j] * control[i * self.Gineq.shape[1] + j]
self.assertLessEqual(res, self.hineq[0])
print "Test lmpc with inequalities"
print controller.solve_time(), "s"
print controller.solve_and_build_time(), "s"
print