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