def test_region_of_attraction(self): x = Variable("x") sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3]) context = sys.CreateDefaultContext() options = RegionOfAttractionOptions() options.lyapunov_candidate = x*x options.state_variables = [x] V = RegionOfAttraction(system=sys, context=context, options=options)
def get_plant_and_pos(): """"Set up plant and position systems.""" # Inputs kappa_F = sym.Variable("kappa_F") kappa_R = sym.Variable("kappa_R") delta = sym.Variable("delta") # Outputs r = sym.Variable("r") r_dot = sym.Variable("r_dot") theta = sym.Variable("theta") theta_dot = sym.Variable("theta_dot") phi = sym.Variable("phi") phi_dot = sym.Variable("phi_dot") # v = r_dot r_hat + r theta_dot theta_hat v_r_hat = r_dot v_theta_hat = r * theta_dot beta = sym.atan2(v_r_hat, v_theta_hat) # Slip angles alpha_F = phi - delta - beta alpha_R = phi - beta F_xf = sym.cos(delta) * S_FL * kappa_F - sym.sin(delta) * S_FC * alpha_F F_xr = S_RL * kappa_R F_yf = sym.sin(delta) * S_FL * kappa_F + sym.cos(delta) * S_FC * alpha_F F_yr = S_RC * alpha_R F_x = F_xf + F_xr F_y = F_yr + F_yf plant_state = np.array([r, r_dot, theta_dot, phi, phi_dot]) theta_ddot = (F_x*sym.cos(phi) - F_y*sym.sin(phi)) / \ (m*r) - 2*r_dot * theta_dot/r plant_dynamics = np.array([ r_dot, (F_x * sym.sin(phi) + F_y * sym.cos(phi)) / m + r * theta_dot**2, theta_ddot, phi_dot, theta_ddot - (l_F * F_yf - l_R * F_yr) / Iz ]) plant_input = [kappa_F, kappa_R, delta] plant_vector_system = SymbolicVectorSystem(state=plant_state, input=plant_input, dynamics=plant_dynamics, output=plant_state) position_state = [theta] position_dynamics = [theta_dot] position_system = SymbolicVectorSystem(state=position_state, input=plant_state, dynamics=position_dynamics, output=position_state) return plant_vector_system, position_system
def test_region_of_attraction(self): x = Variable("x") sys = SymbolicVectorSystem(state=[x], dynamics=[-x + x**3]) context = sys.CreateDefaultContext() options = RegionOfAttractionOptions() options.lyapunov_candidate = x * x options.state_variables = [x] V = RegionOfAttraction(system=sys, context=context, options=options) self.assertEqual( repr(options), "".join([ "RegionOfAttractionOptions(", "lyapunov_candidate=pow(x, 2), ", "state_variables=[Variable('x', Continuous)])" ]))
def test_symbolic_vector_system(self): t = Variable("t") x = [Variable("x0"), Variable("x1")] u = [Variable("u0"), Variable("u1")] system = SymbolicVectorSystem(time=t, state=x, input=u, dynamics=[x[0] + x[1], t], output=[u[1]], time_period=0.0) context = system.CreateDefaultContext() self.assertEqual(context.num_continuous_states(), 2) self.assertEqual(context.num_discrete_state_groups(), 0) self.assertEqual(system.get_input_port(0).size(), 2) self.assertEqual(system.get_output_port(0).size(), 1)
def test_symbolic_vector_system(self): t = Variable("t") x = [Variable("x0"), Variable("x1")] u = [Variable("u0"), Variable("u1")] system = SymbolicVectorSystem(time=t, state=x, input=u, dynamics=[x[0] + x[1], t], output=[u[1]], time_period=0.0) context = system.CreateDefaultContext() self.assertEqual(context.num_continuous_states(), 2) self.assertEqual(context.num_discrete_state_groups(), 0) self.assertEqual(system.get_input_port(0).size(), 2) self.assertEqual(system.get_output_port(0).size(), 1)
def test_symbolic_vector_system_parameters(self): t = Variable("t") x = [Variable("x0"), Variable("x1")] u = [Variable("u0"), Variable("u1")] p = [Variable("p0"), Variable("p1")] system = SymbolicVectorSystem(time=t, state=x, input=u, parameter=p, dynamics=[p[0] * x[0] + x[1] + p[1], t], output=[u[1]], time_period=0.0) context = system.CreateDefaultContext() self.assertEqual(context.num_continuous_states(), 2) self.assertEqual(context.num_discrete_state_groups(), 0) self.assertEqual(system.get_input_port(0).size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertEqual(context.num_abstract_parameters(), 0) self.assertEqual(context.num_numeric_parameter_groups(), 1) self.assertEqual(context.get_numeric_parameter(0).size(), 2) self.assertTrue( system.dynamics_for_variable(x[0]).EqualTo(p[0] * x[0] + x[1] + p[1])) self.assertTrue(system.dynamics_for_variable(x[1]).EqualTo(t))
def test_dense_integration(self): x = Variable("x") sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3]) simulator = Simulator(sys) integrator = simulator.get_mutable_integrator() self.assertIsNone(integrator.get_dense_output()) integrator.StartDenseIntegration() pp = integrator.get_dense_output() self.assertIsInstance(pp, PiecewisePolynomial) simulator.AdvanceTo(1.0) self.assertIs(pp, integrator.StopDenseIntegration()) self.assertEqual(pp.start_time(), 0.0) self.assertEqual(pp.end_time(), 1.0) self.assertIsNone(integrator.get_dense_output())
def test_system_monitor(self): x = Variable("x") sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3]) simulator = Simulator(sys) def monitor(root_context): context = sys.GetMyContextFromRoot(root_context) if context.get_time() >= 1.: return EventStatus.ReachedTermination(sys, "Time reached") else: return EventStatus.DidNothing() self.assertIsNone(simulator.get_monitor()) simulator.set_monitor(monitor) self.assertIsNotNone(simulator.get_monitor()) status = simulator.AdvanceTo(2.) self.assertEqual( status.reason(), SimulatorStatus.ReturnReason.kReachedTerminationCondition) self.assertLess(status.return_time(), 1.1) simulator.clear_monitor() self.assertIsNone(simulator.get_monitor())
from pydrake.symbolic import Variable from pydrake.systems.primitives import SymbolicVectorSystem from pydrake.systems.framework import BasicVector, LeafSystem import matplotlib.pyplot as plt from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder from pydrake.systems.primitives import LogOutput # Define a new symbolic Variable x = Variable("x") # Define the system continuous_vector_system = SymbolicVectorSystem(state=[x], dynamics=[-x + x**3], output=[x]) # Discrete time system. Note the additional argument specifying the time period discrete_vector_system = SymbolicVectorSystem(state=[x], dynamics=[x**3], output=[x], time_period=1.0) # More complex system? Derive from LeafSystem # Not that in this simple form, this current system does not support autodiff class SimpleContinuousTimeSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.DeclareContinuousState(1) # One state variable