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_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_bound(self): print "test_lmpc_bound" 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) trajConstr = pyCopra.TrajectoryBoundConstraint(self.xLower, self.xUpper) contConstr = pyCopra.ControlBoundConstraint(self.uLower, self.uUpper) 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 = 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]) self.assertLessEqual(max(velTraj), self.xUpper[1] + 1e-6) self.assertLessEqual(max(control), self.uUpper[0] + 1e-6) print "Test lmpc with bounds" print controller.solve_time(), "s" print controller.solve_and_build_time(), "s" print
def test_fail_construct_control(self): constr = copra.TrajectoryBoundConstraint()
def test_fail_construct_control(self): print "test_fail_construct_control" constr = pyCopra.TrajectoryBoundConstraint()