def make_diagram():
builder = DiagramBuilder()
adder1 = builder.AddNamedSystem("adder1", Adder(2, 2))
adder2 = builder.AddNamedSystem("adder2", Adder(1, 2))
builder.Connect(adder1.get_output_port(), adder2.get_input_port())
builder.ExportInput(adder1.get_input_port(0), "in0")
builder.ExportInput(adder1.get_input_port(1), "in1")
builder.ExportOutput(adder2.get_output_port(), "out")
diagram = builder.Build()
return adder1, adder2, diagram
def test_diagram_fan_out(self):
builder = DiagramBuilder()
adder = builder.AddSystem(Adder(6, 1))
adder.set_name("adder")
builder.ExportOutput(adder.get_output_port())
in0_index = builder.ExportInput(adder.get_input_port(0), "in0")
in1_index = builder.ExportInput(adder.get_input_port(1), "in1")
# Exercise ConnectInput overload bindings, with and without argument
# names.
builder.ConnectInput(in0_index, adder.get_input_port(2))
builder.ConnectInput("in1", adder.get_input_port(3))
builder.ConnectInput(diagram_port_name="in0",
input=adder.get_input_port(4))
builder.ConnectInput(diagram_port_index=in1_index,
input=adder.get_input_port(5))
diagram = builder.Build()
diagram.set_name("fan_out_diagram")
graph = diagram.GetGraphvizString()
# Check the desired input topology is in the graph.
self.assertRegex(graph, "_u0 -> .*:u0")
self.assertRegex(graph, "_u1 -> .*:u1")
self.assertRegex(graph, "_u0 -> .*:u2")
self.assertRegex(graph, "_u1 -> .*:u3")
self.assertRegex(graph, "_u0 -> .*:u4")
self.assertRegex(graph, "_u1 -> .*:u5")
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertIsInstance(system, SystemBase)
self.assertEqual(
system.GetSystemType(),
"drake::systems::Adder<double>")
system.set_name(name="adder")
self.assertEqual(system.get_name(), "adder")
self.assertEqual(system.GetSystemName(), "adder")
self.assertEqual(system.GetSystemPathname(), "::adder")
self.assertEqual(system.num_input_ports(), 3)
self.assertEqual(system.num_output_ports(), 1)
u1 = system.GetInputPort("u1")
self.assertEqual(u1.get_name(), "u1")
self.assertIn("u1", u1.GetFullDescription())
self.assertEqual(u1.get_index(), 1)
self.assertEqual(u1.size(), 10)
self.assertIsNotNone(u1.ticket())
self.assertIs(u1.get_system(), system)
y = system.GetOutputPort("sum")
self.assertEqual(y.get_name(), "sum")
self.assertEqual(y.get_index(), 0)
self.assertIsInstance(y.Allocate(), Value[BasicVector])
self.assertIs(y.get_system(), system)
y.disable_caching_by_default()
self.assertEqual(y, system.get_output_port())
def test_scalar_type_conversion(self):
float_system = Adder(1, 1)
float_context = float_system.CreateDefaultContext()
float_system.get_input_port(0).FixValue(float_context, 1.)
for T in [float, AutoDiffXd, Expression]:
system = Adder_[T](1, 1)
# N.B. Current scalar conversion does not permit conversion to and
# from the same type.
if T != AutoDiffXd:
methods = [Adder_[T].ToAutoDiffXd, Adder_[T].ToAutoDiffXdMaybe]
for method in methods:
system_ad = method(system)
self.assertIsInstance(system_ad, System_[AutoDiffXd])
self._compare_system_instances(system, system_ad)
if T != Expression:
methods = [Adder_[T].ToSymbolic, Adder_[T].ToSymbolicMaybe]
for method in methods:
system_sym = method(system)
self.assertIsInstance(system_sym, System_[Expression])
self._compare_system_instances(system, system_sym)
context = system.CreateDefaultContext()
system.FixInputPortsFrom(other_system=float_system,
other_context=float_context,
target_context=context)
u = system.get_input_port(0).Eval(context)
self.assertEqual(len(u), 1)
if T == float:
self.assertEqual(u[0], 1.)
elif T == AutoDiffXd:
self.assertEqual(u[0].value(), 1.)
else:
self.assertEqual(u[0].Evaluate(), 1.)
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertEqual(system.get_num_input_ports(), 3)
self.assertEqual(system.get_num_output_ports(), 1)
self.assertEqual(system.GetInputPort("u1").get_index(), 1)
self.assertEqual(system.GetOutputPort("sum").get_index(), 0)
def test_context_api(self):
system = Adder(3, 10)
context = system.CreateDefaultContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertTrue(context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
def __init__(self, num_inputs, size):
Diagram.__init__(self)
builder = DiagramBuilder()
adder = Adder(num_inputs, size)
builder.AddSystem(adder)
builder.ExportOutput(adder.get_output_port(0))
for i in range(num_inputs):
builder.ExportInput(adder.get_input_port(i))
builder.BuildInto(self)
def test_context_api(self):
system = Adder(3, 10)
context = system.CreateDefaultContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertEqual(system.get_num_input_ports(), 3)
self.assertEqual(system.get_num_output_ports(), 1)
# Test deprecated methods.
context = system.CreateDefaultContext()
with warnings.catch_warnings(record=True) as w:
c = system.AllocateOutput(context)
self.assertEqual(len(w), 1)
def test_ownership_vector(self):
system = Adder(1, 1)
context = system.CreateDefaultContext()
info = Info()
vector = DeleteListenerVector(info.record_deletion)
context.FixInputPort(0, vector)
del context
# WARNING
self.assertTrue(info.deleted)
self.assertTrue(vector is not None)
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertEqual(system.num_input_ports(), 3)
self.assertEqual(system.num_output_ports(), 1)
u1 = system.GetInputPort("u1")
self.assertEqual(u1.get_name(), "u1")
self.assertIn("u1", u1.GetFullDescription())
self.assertEqual(u1.get_index(), 1)
self.assertEqual(u1.size(), 10)
self.assertIsNotNone(u1.ticket())
self.assertEqual(system.GetOutputPort("sum").get_index(), 0)
def test_context_base_api(self):
system = Adder(3, 10)
context = system.AllocateContext()
self.assertIsInstance(context, ContextBase)
self.assertEqual(context.num_input_ports(), 3)
self.assertEqual(context.num_output_ports(), 1)
context.DisableCaching()
context.EnableCaching()
context.SetAllCacheEntriesOutOfDate()
context.FreezeCache()
self.assertTrue(context.is_cache_frozen())
context.UnfreezeCache()
self.assertFalse(context.is_cache_frozen())
def test_system_visitor(self):
builder = DiagramBuilder()
builder.AddNamedSystem("adder1", Adder(2, 2))
builder.AddNamedSystem("adder2", Adder(2, 2))
system = builder.Build()
system.set_name("diagram")
visited_systems = []
visited_diagrams = []
class MyVisitor(SystemVisitor):
def VisitSystem(self, system):
visited_systems.append(system.get_name())
def VisitDiagram(self, diagram):
visited_diagrams.append(diagram.get_name())
for sys in diagram.GetSystems():
sys.Accept(self)
visitor = MyVisitor()
system.Accept(v=visitor)
self.assertEqual(visited_systems, ["adder1", "adder2"])
self.assertEqual(visited_diagrams, ["diagram"])
def test_ownership_vector(self):
system = Adder(1, 1)
context = system.CreateDefaultContext()
info = Info()
vector = DeleteListenerVector(info.record_deletion)
context.FixInputPort(0, vector)
del context
# Same as above applications, using `py::keep_alive`.
self.assertFalse(info.deleted)
self.assertTrue(vector is not None)
# Ensure that we do not get segfault behavior when accessing / mutating
# the values.
self.assertTrue(np.allclose(vector.get_value(), [0.]))
vector.get_mutable_value()[:] = [10.]
self.assertTrue(np.allclose(vector.get_value(), [10.]))
def test_context_api(self):
system = Adder(3, 10)
context = system.CreateDefaultContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(
context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
context.SetAbstractState(index=0, value=True)
value = context.get_abstract_state(0)
self.assertTrue(value.get_value())
context.SetAbstractState(index=0, value=False)
value = context.get_abstract_state(0)
self.assertFalse(value.get_value())
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertEqual(
system.GetSystemType(),
"drake::systems::Adder<double>")
self.assertEqual(system.num_input_ports(), 3)
self.assertEqual(system.num_output_ports(), 1)
u1 = system.GetInputPort("u1")
self.assertEqual(u1.get_name(), "u1")
self.assertIn("u1", u1.GetFullDescription())
self.assertEqual(u1.get_index(), 1)
self.assertEqual(u1.size(), 10)
self.assertIsNotNone(u1.ticket())
y = system.GetOutputPort("sum")
self.assertEqual(y.get_index(), 0)
self.assertIsInstance(y.Allocate(), Value[BasicVector])
def add_arm_gripper(self, arm_name, arm_path, arm_base, X_arm,
gripper_path, arm_ee, gripper_base, X_gripper):
# Add arm
parser = Parser(self._mbp, self._sg)
arm_model_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = self._mbp.GetFrameByName(arm_base, arm_model_id)
self._mbp.WeldFrames(self._mbp.world_frame(), arm_base_frame, X_arm)
self._model_ids[arm_name] = arm_model_id
# Add gripper
gripper_name = arm_name+"_gripper"
arm_end_frame = self._mbp.GetFrameByName(arm_ee, arm_model_id)
self.add_floating_gripper(gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper)
# Add arm controller stack
ctrl_plant = MultibodyPlant(0)
parser = Parser(ctrl_plant)
ctrl_arm_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = ctrl_plant.GetFrameByName(arm_base, ctrl_arm_id)
ctrl_plant.WeldFrames(ctrl_plant.world_frame(), arm_base_frame, X_arm)
gripper_equivalent = ctrl_plant.AddRigidBody(
gripper_name+"_equivalent", ctrl_arm_id,
self.calculate_ee_composite_inertia(gripper_path))
arm_end_frame = ctrl_plant.GetFrameByName(arm_ee, ctrl_arm_id)
ctrl_plant.WeldFrames(arm_end_frame, gripper_equivalent.body_frame(),
X_gripper)
ctrl_plant.Finalize()
self._control_mbp[arm_name] = ctrl_plant
arm_num_positions = ctrl_plant.num_positions(ctrl_arm_id)
kp = 4000*np.ones(arm_num_positions)
ki = 0 * np.ones(arm_num_positions)
kd = 5*np.sqrt(kp)
arm_controller = self._builder.AddSystem(InverseDynamicsController(
ctrl_plant, kp, ki, kd, False))
adder = self._builder.AddSystem(Adder(2, arm_num_positions))
state_from_position = self._builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
arm_num_positions, self._mbp.time_step(), True))
# Add command pass through and state splitter
arm_command = self._builder.AddSystem(PassThrough(arm_num_positions))
state_split = self._builder.AddSystem(Demultiplexer(
2*arm_num_positions, arm_num_positions))
def finalize_func():
builder = self._builder
# Export positions commanded
command_input_name = arm_name + "_position"
command_output_name = arm_name + "_position_commanded"
self._port_names.extend([command_input_name, command_output_name])
builder.ExportInput(arm_command.get_input_port(0), command_input_name)
builder.ExportOutput(arm_command.get_output_port(0), command_output_name)
# Export arm state ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
state_split.get_input_port(0))
arm_q_name = arm_name + "_position_measured"
arm_v_name = arm_name + "_velocity_estimated"
arm_state_name = arm_name + "_state_measured"
self._port_names.extend([arm_q_name, arm_v_name, arm_state_name])
builder.ExportOutput(state_split.get_output_port(0), arm_q_name)
builder.ExportOutput(state_split.get_output_port(1), arm_v_name)
builder.ExportOutput(self._mbp.get_state_output_port(arm_model_id),
arm_state_name)
# Export controller stack ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
arm_controller.get_input_port_estimated_state())
builder.Connect(arm_controller.get_output_port_control(),
adder.get_input_port(0))
builder.Connect(adder.get_output_port(0),
self._mbp.get_actuation_input_port(arm_model_id))
builder.Connect(state_from_position.get_output_port(0),
arm_controller.get_input_port_desired_state())
builder.Connect(arm_command.get_output_port(0),
state_from_position.get_input_port(0))
torque_input_name = arm_name + "_feedforward_torque"
torque_output_cmd_name = arm_name + "_torque_commanded"
torque_output_est_name = arm_name + "_torque_measured"
self._port_names.extend([torque_input_name, torque_output_cmd_name,
torque_output_est_name])
builder.ExportInput(adder.get_input_port(1), torque_input_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_cmd_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_est_name)
external_torque_name = arm_name + "_torque_external"
self._port_names.append(external_torque_name)
builder.ExportOutput(
self._mbp.get_generalized_contact_forces_output_port(arm_model_id),
external_torque_name)
self._finalize_functions.append(finalize_func)
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = np.array([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2) # Test the BasicVector overload.
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
def test_system_base_api(self):
# Test a system with a different number of inputs from outputs.
system = Adder(3, 10)
self.assertEqual(system.get_num_input_ports(), 3)
self.assertEqual(system.get_num_output_ports(), 1)
def test_diagram_simulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
self.assertTrue(builder.empty())
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
self.assertFalse(builder.empty())
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
self.assertEqual(
builder.GetMutableSystems(),
[adder0, adder1, integrator])
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
self.assertEqual(
diagram.GetSystems(),
[adder0, adder1, integrator])
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
# Test binding for PrintSimulatorStatistics
PrintSimulatorStatistics(simulator)
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial)
+ xc_initial)
self.assertTrue(np.allclose(xc, xc_expected))
def test_context_api(self):
system = Adder(3, 10)
context = system.AllocateContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
context = system.CreateDefaultContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
# Test methods with two scalar types.
for T in [float, AutoDiffXd, Expression]:
systemT = Adder_[T](3, 10)
contextT = systemT.CreateDefaultContext()
for U in [float, AutoDiffXd, Expression]:
systemU = Adder_[U](3, 10)
contextU = systemU.CreateDefaultContext()
contextU.SetTime(0.5)
contextT.SetTimeStateAndParametersFrom(contextU)
if T == float:
self.assertEqual(contextT.get_time(), 0.5)
elif T == AutoDiffXd:
self.assertEqual(contextT.get_time().value(), 0.5)
else:
self.assertEqual(contextT.get_time().Evaluate(), 0.5)
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
context.SetAbstractState(index=0, value=True)
value = context.get_abstract_state(0)
self.assertTrue(value.get_value())
context.SetAbstractState(index=0, value=False)
value = context.get_abstract_state(0)
self.assertFalse(value.get_value())
def test_context_api(self):
system = Adder(3, 10)
context = system.AllocateContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
system.SetDefaultContext(context)
# Check random context method.
system.SetRandomContext(context=context, generator=RandomGenerator())
context = system.CreateDefaultContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
self.assertTrue(context.is_stateless())
self.assertFalse(context.has_only_continuous_state())
self.assertFalse(context.has_only_discrete_state())
self.assertEqual(context.num_total_states(), 0)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
# Test methods with two scalar types.
for T in [float, AutoDiffXd, Expression]:
systemT = Adder_[T](3, 10)
contextT = systemT.CreateDefaultContext()
for U in [float, AutoDiffXd, Expression]:
systemU = Adder_[U](3, 10)
contextU = systemU.CreateDefaultContext()
contextU.SetTime(0.5)
contextT.SetTimeStateAndParametersFrom(contextU)
if T == float:
self.assertEqual(contextT.get_time(), 0.5)
elif T == AutoDiffXd:
self.assertEqual(contextT.get_time().value(), 0.5)
else:
self.assertEqual(contextT.get_time().Evaluate(), 0.5)
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(
context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertTrue(
context.get_mutable_parameters().get_mutable_numeric_parameter(
0) is context.get_mutable_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
self.assertTrue(hasattr(context, "get_mutable_abstract_parameter"))
context.DisableCaching()
context.EnableCaching()
context.SetAllCacheEntriesOutOfDate()
context.FreezeCache()
self.assertTrue(context.is_cache_frozen())
context.UnfreezeCache()
self.assertFalse(context.is_cache_frozen())
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state().CopyToVector(), x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
context.SetTimeAndContinuousState(0.3, 2*x)
np.testing.assert_equal(context.get_time(), 0.3)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), 2*x)
self.assertNotEqual(pendulum.EvalPotentialEnergy(context=context), 0)
self.assertNotEqual(pendulum.EvalKineticEnergy(context=context), 0)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
def check_abstract_value_zero(context, expected_value):
# Check through Context, State, and AbstractValues APIs.
self.assertEqual(context.get_abstract_state(index=0).get_value(),
expected_value)
self.assertEqual(context.get_abstract_state().get_value(
index=0).get_value(), expected_value)
self.assertEqual(context.get_state().get_abstract_state()
.get_value(index=0).get_value(), expected_value)
context.SetAbstractState(index=0, value=True)
check_abstract_value_zero(context, True)
context.SetAbstractState(index=0, value=False)
check_abstract_value_zero(context, False)
value = context.get_mutable_state().get_mutable_abstract_state()\
.get_mutable_value(index=0)
value.set_value(True)
check_abstract_value_zero(context, True)
def __init__(self):
#import pdb; pdb.set_trace()
# Create a block diagram containing our system.
builder = DiagramBuilder()
# add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2)
plant_x = builder.AddSystem(DoubleIntegrator())
plant_y = builder.AddSystem(DoubleIntegrator())
plant_x.set_name("double_integrator_x")
plant_y.set_name("double_integrator_y")
# add the controller, lead compensator for now just to stablize it
controller_x = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_y = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_x.set_name("controller_x")
controller_y.set_name("controller_y")
# the perception's "Beam" model with the four sources of noise
x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
x_meas_fb.set_name("x_fb")
y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
y_meas_fb.set_name("y_fb")
y_perception = builder.AddSystem(Adder(num_inputs=2, size=1))
y_perception.set_name("range_measurment_y")
neg_y_meas = builder.AddSystem(Gain(k=-1., size=1))
neg_y_meas.set_name("neg_y_meas")
wall_position = builder.AddSystem(ConstantVectorSource([Y_wall]))
p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\
sampling_interval_sec=0.01))
p_hit.set_name("GaussionNoise(0,1)")
p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\
sampling_interval_sec=0.01))
p_short.set_name("ExponentialNoise(1)")
#p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\
# sampling_interval_sec=0.01))
p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\
sampling_interval_sec=0.01))
p_rand.set_name("UniformNoise(0,1)")
#import pdb; pdb.set_trace()
p_hit_Dx = builder.AddSystem(Demultiplexer(size=2))
p_hit_Dx.set_name('Dmux1')
p_short_Dx = builder.AddSystem(Demultiplexer(size=2))
p_short_Dx.set_name('Dmux2')
p_rand_Dx = builder.AddSystem(Demultiplexer(size=2))
p_rand_Dx.set_name('Dmux3')
normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_x.set_name("Sigma_x")
normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_y.set_name("Sigma_y")
expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_x.set_name("Exp_x")
expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_y.set_name("Exp_y")
randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_x.set_name("Rand_x")
randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_y.set_name("Rand_y")
#maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_x.set_name("Max_x")
#maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_y.set_name("Max_y")
# the summation to get the error (closing the loop)
summ_x = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_y = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_x.set_name("summation_x")
summ_y.set_name("summation_y")
neg_x = builder.AddSystem(Gain(k=-1., size=1))
neg_y = builder.AddSystem(Gain(k=-1., size=1))
neg_uy = builder.AddSystem(Gain(k=-1., size=1))
neg_x.set_name("neg_x")
neg_y.set_name("neg_y")
neg_uy.set_name("neg_uy")
# wire up all the blocks (summation to the controller to the plant ...)
builder.Connect(summ_x.get_output_port(0),
controller_x.get_input_port(0)) # e_x
builder.Connect(summ_y.get_output_port(0),
controller_y.get_input_port(0)) # e_y
builder.Connect(controller_x.get_output_port(0),
plant_x.get_input_port(0)) # u_x
builder.Connect(
controller_y.get_output_port(0),
neg_uy.get_input_port(0)) # u_y (to deal with directions)
builder.Connect(neg_uy.get_output_port(0),
plant_y.get_input_port(0)) # u_y
builder.Connect(
plant_x.get_output_port(0),
x_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(wall_position.get_output_port(0),
y_perception.get_input_port(0)) # perception
builder.Connect(plant_y.get_output_port(0),
neg_y_meas.get_input_port(0)) # perception
builder.Connect(neg_y_meas.get_output_port(0),
y_perception.get_input_port(1)) # perception, z meas
builder.Connect(
y_perception.get_output_port(0),
y_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(p_hit.get_output_port(0),
p_hit_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_hit_Dx.get_output_port(0),
normgain_x.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_x.get_output_port(0),
x_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_hit_Dx.get_output_port(1),
normgain_y.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_y.get_output_port(0),
y_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_short.get_output_port(0),
p_short_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_short_Dx.get_output_port(0),
expgain_x.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_x.get_output_port(0),
x_meas_fb.get_input_port(2)) # Exp dist
builder.Connect(p_short_Dx.get_output_port(1),
expgain_y.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_y.get_output_port(0),
y_meas_fb.get_input_port(2)) # Exp dist
#builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist
#builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand.get_output_port(0),
p_rand_Dx.get_input_port(0)) # normalize Uniform dist
builder.Connect(p_rand_Dx.get_output_port(0),
randgain_x.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_x.get_output_port(0),
x_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand_Dx.get_output_port(1),
randgain_y.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_y.get_output_port(0),
y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(x_meas_fb.get_output_port(0),
neg_x.get_input_port(0)) # -x
builder.Connect(y_meas_fb.get_output_port(0),
neg_y.get_input_port(0)) # -y
builder.Connect(neg_x.get_output_port(0),
summ_x.get_input_port(1)) # r-x
builder.Connect(neg_y.get_output_port(0),
summ_y.get_input_port(1)) # r-y
# Make the desired_state input of the controller, an input to the diagram.
builder.ExportInput(summ_x.get_input_port(0))
builder.ExportInput(summ_y.get_input_port(0))
# get telemetry
logger_x = LogOutput(plant_x.get_output_port(0), builder)
logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder)
logger_y = LogOutput(plant_y.get_output_port(0), builder)
logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder)
logger_x.set_name("logger_x_state")
logger_y.set_name("logger_y_state")
logger_noise_x.set_name("x_state_meas")
logger_noise_y.set_name("y_meas")
# compile the system
diagram = builder.Build()
diagram.set_name("diagram")
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# First we extract the subsystem context for the plant
plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context)
plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context)
# Then we can set the pendulum state, which is (x, y).
z0 = X_0
plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0)
z0 = Y_0
plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0)
# The diagram has a single input port (port index 0), which is the desired_state.
context.FixInputPort(
0,
[X_d]) # here we assume no perception, closing feedback on state X
context.FixInputPort(
1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0
# run the simulation
simulator.AdvanceTo(10)
t = logger_x.sample_times()
#import pdb; pdb.set_trace()
# Plot the results.
plt.figure()
plot_system_graphviz(diagram, max_depth=2)
plt.figure()
plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas')
plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))')
plt.plot(t, logger_x.data().transpose(), label='x_state')
plt.plot(t, logger_y.data().transpose(), label='y_state')
plt.xlabel('t')
plt.ylabel('x(t),y(t)')
plt.legend()
plt.show(block=True)
def test_add_named_system(self):
builder = DiagramBuilder()
adder1 = builder.AddNamedSystem("adder1", Adder(2, 3))
self.assertEqual(adder1.get_name(), "adder1")
adder2 = builder.AddNamedSystem(name="adder2", system=Adder(5, 8))
self.assertEqual(adder2.get_name(), "adder2")
import matplotlib.pyplot as plt
from pydrake.systems.drawing import plot_system_graphviz
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import Adder
builder = DiagramBuilder()
size = 1
adders = [
builder.AddSystem(Adder(1, size)),
builder.AddSystem(Adder(1, size)),
]
for i, adder in enumerate(adders):
adder.set_name("adders[{}]".format(i))
builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0))
builder.ExportInput(adders[0].get_input_port(0))
builder.ExportOutput(adders[1].get_output_port(0))
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram)
plt.show()