Exemple #1
0
 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)
Exemple #2
0
 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.)
Exemple #3
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)
Exemple #5
0
 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)
Exemple #6
0
 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
Exemple #7
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)
Exemple #8
0
 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)
Exemple #9
0
 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)
     # Test deprecated methods.
     context = system.CreateDefaultContext()
     with warnings.catch_warnings(record=True) as w:
         c = system.AllocateOutput(context)
         self.assertEqual(len(w), 1)
Exemple #10
0
 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)
Exemple #11
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())
Exemple #12
0
 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_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")
Exemple #15
0
 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)
     # Test deprecated methods.
     context = system.CreateDefaultContext()
     with warnings.catch_warnings(record=True) as w:
         c = system.AllocateOutput(context)
         self.assertEqual(len(w), 1)
Exemple #16
0
 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)
Exemple #17
0
    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"])
Exemple #18
0
 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)
Exemple #19
0
 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)
Exemple #21
0
 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)
Exemple #22
0
    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))
Exemple #23
0
    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 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_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")
Exemple #26
0
    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 __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)
Exemple #28
0
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()
Exemple #29
0
    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())