def test_lmpc_ineq(self):
ps = copra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0, self.nbStep)
controller = copra.LMPC(ps)
xCost = copra.TargetCost(self.M, -self.xd)
uCost = copra.ControlCost(self.N, -self.ud)
trajConstr = copra.TrajectoryConstraint(self.Eineq, self.fineq)
contConstr = copra.ControlConstraint(self.Gineq, self.hineq)
xCost.weights(self.wx)
uCost.weights(self.wu)
controller.add_cost(xCost)
controller.add_cost(uCost)
controller.add_constraint(trajConstr)
controller.add_constraint(contConstr)
self.assertTrue(controller.solve())
control = controller.control()
fullTraj = controller.trajectory()
fTLen = int(len(fullTraj) / 2)
posTraj = [0.] * fTLen
velTraj = [0.] * fTLen
for i in range(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])
self.assertLessEqual(np.amax(control), self.hineq[0])
print("Test lmpc with inequalities")
print(controller.solve_time(), "s")
print(controller.solve_and_build_time(), "s")
print()
def test_preview_systeme_still_exist(self):
ps = copra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0, self.nbStep)
controller = copra.LMPC(ps)
del ps
trajConstr = copra.TrajectoryConstraint(self.Eineq, self.fineq)
contConstr = copra.ControlConstraint(self.Gineq, self.hineq)
targetCost = copra.TargetCost(self.M, -self.xd)
controlCost = copra.ControlCost(self.N, -self.ud)
targetCost.weights(self.wx)
controlCost.weights(self.wu)
controller.add_constraint(trajConstr)
controller.add_constraint(contConstr)
controller.add_cost(targetCost)
controller.add_cost(controlCost)
self.assertTrue(controller.solve())
control = controller.control()
fullTraj = controller.trajectory()
fTLen = int(len(fullTraj) / 2)
posTraj = [0.] * fTLen
velTraj = [0.] * fTLen
for i in range(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])
self.assertLessEqual(np.amax(control), self.hineq[0])
def test_lmpc_eq(self):
print "test_lmpc_eq"
ps = pyCopra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0Eq, self.nbStep)
controller = pyCopra.LMPC(ps)
xCost = pyCopra.TargetCost(self.M, -self.xdEq)
uCost = pyCopra.ControlCost(self.N, -self.ud)
trajConstr = pyCopra.TrajectoryConstraint(self.Eeq, self.feq, False)
xCost.weights(self.wx)
uCost.weights(self.wu)
controller.add_cost(xCost)
controller.add_cost(uCost)
controller.add_constraint(trajConstr)
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] + 1e-6)
self.assertLessEqual(max(velTraj), self.feq[0] + 1e-6)
print "Test lmpc with equalities"
print controller.solve_time(), "s"
print controller.solve_and_build_time(), "s"
print
def test_constraint_and_cost_deletion(self):
print("Testing 'test_constraint_deletion'.")
print(
"In order to see the outputs, the copra must be installed under Debug mode."
)
ps = copra.PreviewSystem()
ps.system(self.A, self.B, self.c, self.x0, self.nbStep)
controller = copra.LMPC(ps)
trajConstr = copra.TrajectoryConstraint(self.Eineq, self.fineq)
contConstr = copra.ControlConstraint(self.Gineq, self.hineq)
trajEqConstr = copra.TrajectoryConstraint(self.Eeq, self.feq, False)
contEqConstr = copra.ControlConstraint(self.Geq, self.heq, False)
trajBdConstr = copra.TrajectoryBoundConstraint(self.xLower,
self.xUpper)
contBdConstr = copra.ControlBoundConstraint(self.uLower, self.uUpper)
targetCost = copra.TargetCost(self.M, -self.xd)
trajectoryCost = copra.TrajectoryCost(self.M, -self.xd)
controlCost = copra.ControlCost(self.N, -self.ud)
M_mixed = np.ones((1, 2))
mixedCost = copra.MixedCost(M_mixed, self.N, -self.ud)
controller.add_constraint(trajConstr)
controller.add_constraint(contConstr)
controller.add_constraint(trajEqConstr)
controller.add_constraint(contEqConstr)
controller.add_constraint(trajBdConstr)
controller.add_constraint(contBdConstr)
controller.add_cost(targetCost)
controller.add_cost(trajectoryCost)
controller.add_cost(controlCost)
controller.add_cost(mixedCost)
del trajConstr
targetCost.weights(self.wx)
controlCost.weights(self.wu)
del trajEqConstr
del contEqConstr
del trajBdConstr
del contBdConstr
del trajectoryCost
del mixedCost
self.assertFalse(controller.solve())
self.assertTrue(controller.solve()) # Has kept the contConstr only
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
