def test_deprecated_math_prog_methods(self): plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.]) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=3, fixed_timestep=TimeStep(0.1)) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("once", DrakeDeprecationWarning) dirtran.num_vars() self.assertEqual(len(w), 1) expected_message = ("Use trajopt.prog().num_vars(...) instead " "of trajopt.num_vars(...).") self.assertIn(expected_message, str(w[0].message)) # Test a small subset of the exhaustive list. with warnings.catch_warnings(record=True) as w: warnings.simplefilter("once", DrakeDeprecationWarning) x = dirtran.NewContinuousVariables(3, 'x') dirtran.AddCost(x[0] + 2) dirtran.AddLinearCost(x[0]) dirtran.AddConstraint(x[1] == x[2]) self.assertEqual(len(w), 4) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("once", DrakeDeprecationWarning) result = mp.Solve(dirtran) self.assertEqual(len(w), 1) expected_message = ( "The trajectory optimization classes no longer derive from " "MathematicalProgram. Use Solve(trajopt.prog()).") self.assertIn(expected_message, str(w[0].message))
def test_finite_horizon_linear_quadratic_regulator(self): A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [1]]) C = np.identity(2) D = np.array([[0], [0]]) double_integrator = LinearSystem(A, B, C, D) Q = np.identity(2) R = np.identity(1) options = FiniteHorizonLinearQuadraticRegulatorOptions() options.Qf = Q self.assertIsNone(options.N) self.assertIsNone(options.x0) self.assertIsNone(options.u0) self.assertIsNone(options.xd) self.assertIsNone(options.ud) self.assertEqual(options.input_port_index, InputPortSelection.kUseFirstInputIfItExists) context = double_integrator.CreateDefaultContext() double_integrator.get_input_port(0).FixValue(context, 0.0) result = FiniteHorizonLinearQuadraticRegulator( system=double_integrator, context=context, t0=0, tf=0.1, Q=Q, R=R, options=options) self.assertIsInstance(result, FiniteHorizonLinearQuadraticRegulatorResult) self.assertIsInstance(result.x0, Trajectory) self.assertEqual(result.x0.value(0).shape, (2, 1)) self.assertIsInstance(result.u0, Trajectory) self.assertEqual(result.u0.value(0).shape, (1, 1)) self.assertIsInstance(result.K, Trajectory) self.assertEqual(result.K.value(0).shape, (1, 2)) self.assertIsInstance(result.S, Trajectory) self.assertEqual(result.S.value(0).shape, (2, 2)) self.assertIsInstance(result.k0, Trajectory) self.assertEqual(result.k0.value(0).shape, (1, 1)) self.assertIsInstance(result.sx, Trajectory) self.assertEqual(result.sx.value(0).shape, (2, 1)) self.assertIsInstance(result.s0, Trajectory) self.assertEqual(result.s0.value(0).shape, (1, 1)) regulator = MakeFiniteHorizonLinearQuadraticRegulator( system=double_integrator, context=context, t0=0, tf=0.1, Q=Q, R=R, options=options) self.assertEqual(regulator.get_input_port(0).size(), 2) self.assertEqual(regulator.get_output_port(0).size(), 1)
def test_direct_transcription(self): # Integrator. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=21) # Spell out most of the methods, regardless of whether they make sense # as a consistent optimization. The goal is to check the bindings, # not the implementation. t = dirtran.time() dt = dirtran.fixed_timestep() x = dirtran.state() x2 = dirtran.state(2) x0 = dirtran.initial_state() xf = dirtran.final_state() u = dirtran.input() u2 = dirtran.input(2) dirtran.AddRunningCost(x.dot(x)) dirtran.AddConstraintToAllKnotPoints(u[0] == 0) dirtran.AddFinalCost(2 * x.dot(x)) initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21], np.zeros((1, 2))) initial_x = PiecewisePolynomial() dirtran.SetInitialTrajectory(initial_u, initial_x) result = mp.Solve(dirtran.prog()) times = dirtran.GetSampleTimes(result) inputs = dirtran.GetInputSamples(result) states = dirtran.GetStateSamples(result) input_traj = dirtran.ReconstructInputTrajectory(result) state_traj = dirtran.ReconstructStateTrajectory(result)
def test_direct_transcription_continuous_time(self): # Test that the continuous-time constructor is also spelled correctly. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.]) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=3, fixed_timestep=TimeStep(0.1)) self.assertEqual(len(dirtran.linear_equality_constraints()), 3)
def test_direct_transcription_continuous_time(self): # Test that the continuous-time constructor is also spelled correctly. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.]) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=3, fixed_timestep=TimeStep(0.1)) with warnings.catch_warnings(record=True) as w: warnings.simplefilter("once", DrakeDeprecationWarning) self.assertEqual(len(dirtran.linear_equality_constraints()), 3) self.assertEqual(len(dirtran.prog().linear_equality_constraints()), 3)
def _do_test_direct_transcription(self, use_deprecated_solve): # Integrator. plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1) context = plant.CreateDefaultContext() dirtran = DirectTranscription(plant, context, num_time_samples=21) # Spell out most of the methods, regardless of whether they make sense # as a consistent optimization. The goal is to check the bindings, # not the implementation. t = dirtran.time() dt = dirtran.fixed_timestep() x = dirtran.state() x2 = dirtran.state(2) x0 = dirtran.initial_state() xf = dirtran.final_state() u = dirtran.input() u2 = dirtran.input(2) dirtran.AddRunningCost(x.dot(x)) dirtran.AddConstraintToAllKnotPoints(u[0] == 0) dirtran.AddFinalCost(2*x.dot(x)) initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21], np.zeros((1, 2))) initial_x = PiecewisePolynomial() dirtran.SetInitialTrajectory(initial_u, initial_x) if use_deprecated_solve: with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) dirtran.Solve() times = dirtran.GetSampleTimes() inputs = dirtran.GetInputSamples() states = dirtran.GetStateSamples() input_traj = dirtran.ReconstructInputTrajectory() state_traj = dirtran.ReconstructStateTrajectory() self.assertEqual(len(w), 6) else: result = mp.Solve(dirtran) times = dirtran.GetSampleTimes(result) inputs = dirtran.GetInputSamples(result) states = dirtran.GetStateSamples(result) input_traj = dirtran.ReconstructInputTrajectory(result) state_traj = dirtran.ReconstructStateTrajectory(result)
def test_linear_system_zero_size(self): # Explicitly test #12633. num_x = 0 num_y = 2 num_u = 2 A = np.zeros((num_x, num_x)) B = np.zeros((num_x, num_u)) C = np.zeros((num_y, num_x)) D = np.zeros((num_y, num_u)) self.assertIsNotNone(LinearSystem(A, B, C, D))
def test_event_api(self): # TriggerType - existence check. TriggerType.kUnknown TriggerType.kInitialization TriggerType.kForced TriggerType.kTimed TriggerType.kPeriodic TriggerType.kPerStep TriggerType.kWitness # PublishEvent. # TODO(eric.cousineau): Test other event types when it is useful to # expose them. def callback(context, event): pass event = PublishEvent(callback=callback) self.assertIsInstance(event, Event) event = PublishEvent(trigger_type=TriggerType.kInitialization, callback=callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization) def system_callback(system, context, event): pass event = PublishEvent(system_callback=system_callback) self.assertIsInstance(event, Event) event = PublishEvent(trigger_type=TriggerType.kInitialization, system_callback=system_callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization) # Simple discrete-time system. system1 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.1) periodic_data = system1.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsInstance(periodic_data, PeriodicEventData) self.assertIsInstance(periodic_data.Clone(), PeriodicEventData) periodic_data.period_sec() periodic_data.offset_sec() is_diff_eq, period = system1.IsDifferenceEquationSystem() self.assertTrue(is_diff_eq) self.assertEqual(period, periodic_data.period_sec()) # Simple continuous-time system. system2 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.0) periodic_data = system2.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsNone(periodic_data) is_diff_eq, period = system2.IsDifferenceEquationSystem() self.assertFalse(is_diff_eq)
def test_linear_quadratic_regulator(self): A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [1]]) C = np.identity(2) D = np.array([[0], [0]]) double_integrator = LinearSystem(A, B, C, D) Q = np.identity(2) R = np.identity(1) K_expected = np.array([[1, math.sqrt(3.)]]) S_expected = np.array([[math.sqrt(3), 1.], [1., math.sqrt(3)]]) (K, S) = LinearQuadraticRegulator(A, B, Q, R) np.testing.assert_almost_equal(K, K_expected) np.testing.assert_almost_equal(S, S_expected) controller = LinearQuadraticRegulator(double_integrator, Q, R) np.testing.assert_almost_equal(controller.D(), -K_expected) context = double_integrator.CreateDefaultContext() double_integrator.get_input_port(0).FixValue(context, [0]) controller = LinearQuadraticRegulator( double_integrator, context, Q, R, input_port_index=double_integrator.get_input_port().get_index()) np.testing.assert_almost_equal(controller.D(), -K_expected)
def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) context.FixInputPort(0, BasicVector([0])) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all())
def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) x0 = np.array([1, 2]) system.configure_default_state(x0=x0) system.SetDefaultContext(context) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) generator = RandomGenerator() system.SetRandomContext(context, generator) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) system.configure_random_state(covariance=np.eye(2)) system.SetRandomContext(context, generator) self.assertNotEqual( context.get_continuous_state_vector().CopyToVector()[1], x0[1]) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) self.assertEqual(system.get_input_port(0), system.get_input_port()) self.assertEqual(system.get_output_port(0), system.get_output_port()) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) system.get_input_port(0).FixValue(context, 0) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all())
def test_finite_horizon_linear_quadratic_regulator(self): A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [1]]) C = np.identity(2) D = np.array([[0], [0]]) double_integrator = LinearSystem(A, B, C, D) Q = np.identity(2) R = np.identity(1) options = FiniteHorizonLinearQuadraticRegulatorOptions() options.Qf = Q options.use_square_root_method = False self.assertIsNone(options.N) self.assertIsNone(options.x0) self.assertIsNone(options.u0) self.assertIsNone(options.xd) self.assertIsNone(options.ud) self.assertEqual(options.input_port_index, InputPortSelection.kUseFirstInputIfItExists) self.assertRegex( repr(options), "".join([ r"FiniteHorizonLinearQuadraticRegulatorOptions\(", # Don't be particular about numpy's whitespace in Qf. r"Qf=\[\[ *1\. *0\.\]\s*\[ *0\. *1\.\]\], " r"N=None, ", r"input_port_index=", r"InputPortSelection.kUseFirstInputIfItExists, ", r"use_square_root_method=False\)" ])) context = double_integrator.CreateDefaultContext() double_integrator.get_input_port(0).FixValue(context, 0.0) result = FiniteHorizonLinearQuadraticRegulator( system=double_integrator, context=context, t0=0, tf=0.1, Q=Q, R=R, options=options) self.assertIsInstance(result, FiniteHorizonLinearQuadraticRegulatorResult) self.assertIsInstance(result.x0, Trajectory) self.assertEqual(result.x0.value(0).shape, (2, 1)) self.assertIsInstance(result.u0, Trajectory) self.assertEqual(result.u0.value(0).shape, (1, 1)) self.assertIsInstance(result.K, Trajectory) self.assertEqual(result.K.value(0).shape, (1, 2)) self.assertIsInstance(result.S, Trajectory) self.assertEqual(result.S.value(0).shape, (2, 2)) self.assertIsInstance(result.k0, Trajectory) self.assertEqual(result.k0.value(0).shape, (1, 1)) self.assertIsInstance(result.sx, Trajectory) self.assertEqual(result.sx.value(0).shape, (2, 1)) self.assertIsInstance(result.s0, Trajectory) self.assertEqual(result.s0.value(0).shape, (1, 1)) regulator = MakeFiniteHorizonLinearQuadraticRegulator( system=double_integrator, context=context, t0=0, tf=0.1, Q=Q, R=R, options=options) self.assertEqual(regulator.get_input_port(0).size(), 2) self.assertEqual(regulator.get_output_port(0).size(), 1)
def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context .get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) context.FixInputPort(0, BasicVector([0])) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all())