def __init__(self): LeafSystem_[T].__init__(self) self.input_port = self.DeclareAbstractInputPort( "in", AbstractValue.Make(default_value)) self.output_port = self.DeclareAbstractOutputPort( "out", lambda: AbstractValue.Make(default_value), self.DoCalcAbstractOutput)
def test_abstract_input_port_fix_string(self): model_value = AbstractValue.Make("") system = PassThrough(copy.copy(model_value)) context = system.CreateDefaultContext() input_port = system.get_input_port(0) # Fix to a literal. input_port.FixValue(context, "Alpha") value = input_port.Eval(context) self.assertEqual(type(value), type(model_value.get_value())) self.assertEqual(value, "Alpha") # Fix to a type-erased string. input_port.FixValue(context, AbstractValue.Make("Bravo")) value = input_port.Eval(context) self.assertEqual(type(value), type(model_value.get_value())) self.assertEqual(value, "Bravo") # Fix to a non-string. with self.assertRaises(RuntimeError): # A RuntimeError occurs when the Context detects that the # type-erased Value objects are incompatible. input_port.FixValue(context, AbstractValue.Make(1)) with self.assertRaises(TypeError): # A TypeError occurs when pybind Value.set_value cannot match any # overload for how to assign the argument into the erased storage. input_port.FixValue(context, 1) with self.assertRaises(TypeError): input_port.FixValue(context, np.array([2.]))
def test_no_rgb(self): self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(), AbstractValue.Make(self.pc_no_rgbs)) self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(), AbstractValue.Make(self.pc_no_rgbs)) fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval( self.context) self.assertEqual(fused_pc.size(), 2 * self.num_points) # The first point cloud should be from [-0.1 to 0.1]. # Tthe second point cloud should be from [0.9 to 1.1]. self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0) self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0) # Even if both input point clouds don't have rgbs, the fused point # cloud should contain rgbs of the default color. self.assertTrue(fused_pc.has_rgbs()) self.assertTrue( np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255]))) self.assertTrue( np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])))
def test_eval(self): """Tests evaluation (e.g. caching, API sugars, etc.).""" # `Eval` and `EvalAbstract`: Test with constant systems. model_values = [ AbstractValue.Make("Hello World"), AbstractValue.Make(BasicVector([1., 2., 3.])), ] for model_value in model_values: is_abstract = not isinstance(model_value.get_value(), BasicVector) if is_abstract: zoh = ConstantValueSource(copy.copy(model_value)) else: zoh = ConstantVectorSource(model_value.get_value().get_value()) context = zoh.CreateDefaultContext() output_port = zoh.get_output_port(0) value_abstract = output_port.EvalAbstract(context) value = output_port.Eval(context) self.assertEqual(type(value_abstract), type(model_value)) self.assertEqual(type(value), type(model_value.get_value())) if is_abstract: check = self.assertEqual else: def check(a, b): self.assertEqual(type(a.get_value()), type(b.get_value())) np.testing.assert_equal(a.get_value(), b.get_value()) check(value_abstract.get_value(), model_value.get_value()) check(value, model_value.get_value())
def test_mix_rgb(self): self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(), AbstractValue.Make(self.pc)) self.context.FixInputPort( self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(), AbstractValue.Make(self.pc_no_rgbs)) fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval( self.context) self.assertEqual(fused_pc.size(), 2 * self.num_points) # The first point cloud should be from [-0.1 to 0.1]. # The second point cloud should be from [0.9 to 1.1]. self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0) self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0) self.assertTrue(fused_pc.has_rgbs()) # We don't know in what order the two point clouds will be combined. rgb_first = np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255])) rgb_last = np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255])) no_rgb_first = np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255])) no_rgb_last = np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])) self.assertTrue((rgb_first and no_rgb_last) or (no_rgb_first and rgb_last))
def test_abstract_value_make(self): value = AbstractValue.Make("Hello world") self.assertTrue(isinstance(value, Value[str])) value = AbstractValue.Make(MoveOnlyType(10)) self.assertTrue(isinstance(value, Value[MoveOnlyType])) value = AbstractValue.Make({"x": 10}) self.assertTrue(isinstance(value, Value[object]))
def __init__(self): LeafSystem_[T].__init__(self) test_input_type = AbstractValue.Make(default_value) self.input_port = self._DeclareInputPort( PortDataType.kAbstractValued, 0) self.output_port = self._DeclareAbstractOutputPort( lambda: AbstractValue.Make(default_value), self._DoCalcAbstractOutput)
def test_ctor_api(self): """Tests construction of systems for systems whose executions semantics are not tested above. """ ConstantValueSource(AbstractValue.Make("Hello world")) ConstantVectorSource(source_value=[1., 2.]) ZeroOrderHold(period_sec=0.1, vector_size=2) ZeroOrderHold(period_sec=0.1, abstract_model_value=AbstractValue.Make("Hello world"))
def __init__(self): LeafSystem_[T].__init__(self) self.input_port = self.DeclareAbstractInputPort( "in", AbstractValue.Make(default_value)) self.output_port = self.DeclareAbstractOutputPort( "out", lambda: AbstractValue.Make(default_value), self.DoCalcAbstractOutput, prerequisites_of_calc=set([self.input_port.ticket()]))
def test_abstract_value_make(self): value = AbstractValue.Make("Hello world") self.assertIsInstance(value, Value[str]) value = AbstractValue.Make(MoveOnlyType(10)) self.assertIsInstance(value, Value[MoveOnlyType]) value = AbstractValue.Make({"x": 10}) self.assertIsInstance(value, Value[object]) for T in [float, AutoDiffXd, Expression]: value = AbstractValue.Make(BasicVector_[T](size=1)) self.assertIsInstance(value, Value[BasicVector_[T]])
def test_ctor_api(self): """Tests construction of systems for systems whose executions semantics are not tested above. """ ConstantValueSource(AbstractValue.Make("Hello world")) DiscreteTimeDelay(update_sec=0.1, delay_timesteps=5, vector_size=2) DiscreteTimeDelay( update_sec=0.1, delay_timesteps=5, abstract_model_value=AbstractValue.Make("Hello world")) ZeroOrderHold(period_sec=0.1, vector_size=2) ZeroOrderHold(period_sec=0.1, abstract_model_value=AbstractValue.Make("Hello world"))
def __init__(self, id_list, default_rgb=[255., 255., 255.]): """ A system that takes in N point clouds of points Si in frame Ci, and N RigidTransforms from frame Ci to F, to put each point cloud in a common frame F. The system returns one point cloud combining all of the transformed point clouds. Each point cloud must have XYZs. RGBs are optional. If absent, those points will be the provided default color. @param id_list A list containing the string IDs of all of the point clouds. This is often the serial number of the camera they came from, such as "1" for a simulated camera or "805212060373" for a real camera. @param default_rgb A list of length 3 containing the RGB values to use in the absence of PointCloud.rgbs. Values should be between 0 and 255. The default is white. @system{ @input_port{point_cloud_CiSi_id0} @input_port{X_FCi_id0} . . . @input_port{point_cloud_CiSi_idN} @input_port{X_FCi_idN} @output_port{point_cloud_FS} } """ LeafSystem.__init__(self) self._point_cloud_ports = {} self._transform_ports = {} self._id_list = id_list self._default_rgb = np.array(default_rgb) output_fields = Fields(BaseField.kXYZs | BaseField.kRGBs) for id in self._id_list: self._point_cloud_ports[id] = self.DeclareAbstractInputPort( "point_cloud_CiSi_{}".format(id), AbstractValue.Make(PointCloud(fields=output_fields))) self._transform_ports[id] = self.DeclareAbstractInputPort( "X_FCi_{}".format(id), AbstractValue.Make(RigidTransform.Identity())) self.DeclareAbstractOutputPort( "point_cloud_FS", lambda: AbstractValue.Make(PointCloud(fields=output_fields)), self.DoCalcOutput)
def test_utime_to_seconds(self): msg = header_t() msg.utime = int(1e6) with catch_drake_warnings(expected_count=1): dut = mut.PyUtimeMessageToSeconds(header_t) t_sec = dut.GetTimeInSeconds(AbstractValue.Make(msg)) self.assertEqual(t_sec, 1)
def show_cloud(pc, use_native=False, **kwargs): # kwargs go to ctor. builder = DiagramBuilder() # Add point cloud visualization. if use_native: viz = meshcat.Visualizer(zmq_url=ZMQ_URL) else: plant, scene_graph = AddMultibodyPlantSceneGraph(builder) plant.Finalize() viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) pc_viz = builder.AddSystem( MeshcatPointCloudVisualizer(viz, **kwargs)) # Make sure the system runs. diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() context = diagram.GetMutableSubsystemContext( pc_viz, diagram_context) context.FixInputPort( pc_viz.GetInputPort("point_cloud_P").get_index(), AbstractValue.Make(pc)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.StepTo(sim_time)
def test_deprecated_abstract_input_port(self): """This test case confirms that the deprecated API for abstract input ports continues to operate correctly, until such a time as we remove it. For an example of non-deprecated APIs to use abstract input ports, see the test_abstract_io_port case, above. """ test = self # A system that takes a Value[object] on its input, and parses the # input value's first element to a float on its output. class ParseFloatSystem(LeafSystem_[float]): def __init__(self): LeafSystem_[float].__init__(self) with catch_drake_warnings(expected_count=1): self.DeclareAbstractInputPort("in") self.DeclareVectorOutputPort("out", BasicVector(1), self._Out) def _Out(self, context, y_data): py_obj = self.EvalAbstractInput(context, 0).get_value()[0] y_data.SetAtIndex(0, float(py_obj)) system = ParseFloatSystem() context = system.CreateDefaultContext() output = system.AllocateOutput() context.FixInputPort(0, AbstractValue.Make(["22.2"])) system.CalcOutput(context, output) self.assertEqual(output.get_vector_data(0).GetAtIndex(0), 22.2)
def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=Isometry3.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = X_WP self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer_' + name) self.DeclarePeriodicPublish(draw_period, 0.0) self.DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud()))
def test_point_cloud_api(self): self.assertEqual(mut.PointCloud.T, np.float32) self.assertEqual(mut.PointCloud.D, np.float32) self.assertTrue( mut.PointCloud.IsDefaultValue(value=mut.PointCloud.kDefaultValue)) self.assertTrue( mut.PointCloud.IsInvalidValue(value=mut.PointCloud.kDefaultValue)) fields = mut.Fields(mut.BaseField.kXYZs) pc = mut.PointCloud(new_size=0, fields=fields) self.assertEqual(pc.fields(), fields) self.assertEqual(pc.size(), 0) pc.resize(new_size=2) self.assertEqual(pc.size(), 2) self.assertTrue(pc.has_xyzs()) self.assertEqual(pc.xyzs().shape, (3, 2)) # Test reference semantics with NumPy + Pybind11. test_xyzs = [[1., 2., 3.], [4., 5., 6.]] pc.mutable_xyzs().T[:] = test_xyzs np.testing.assert_equal(pc.xyzs().T, test_xyzs) test_xyz = [10., 20., 30.] pc.mutable_xyz(i=0)[:] = test_xyz np.testing.assert_equal(pc.xyz(i=0), test_xyz) np.testing.assert_equal(pc.xyzs().T[0], test_xyz) pc_new = mut.PointCloud() pc_new.SetFrom(other=pc) np.testing.assert_equal(pc.xyzs(), pc_new.xyzs()) # Test Systems' value registration. self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
def test_leaf_system_per_item_tickets(self): dut = LeafSystem() dut.DeclareAbstractParameter(AbstractValue.Make(1)) dut.DeclareAbstractState(AbstractValue.Make(1)) dut.DeclareDiscreteState(1) dut.DeclareVectorInputPort("u0", BasicVector(1)) dut.DeclareNumericParameter(BasicVector(1)) for func, arg in [ (dut.abstract_parameter_ticket, AbstractParameterIndex(0)), (dut.abstract_state_ticket, AbstractStateIndex(0)), (dut.cache_entry_ticket, CacheIndex(0)), (dut.discrete_state_ticket, DiscreteStateIndex(0)), (dut.input_port_ticket, InputPortIndex(0)), (dut.numeric_parameter_ticket, NumericParameterIndex(0)), ]: self.assertIsInstance(func(arg), DependencyTicket, func)
def test_context_api(self): # Capture miscellaneous functions not yet tested. model_value = AbstractValue.Make("Hello") class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self._DeclareContinuousState(1) self._DeclareDiscreteState(2) self._DeclareAbstractState(model_value.Clone()) system = TrivialSystem() context = system.CreateDefaultContext() self.assertTrue(context.get_state() is context.get_mutable_state()) self.assertTrue(context.get_continuous_state_vector() is context.get_mutable_continuous_state_vector()) self.assertEqual(context.get_num_discrete_state_groups(), 1) self.assertTrue(context.get_discrete_state_vector() is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state_vector()) self.assertTrue( context.get_discrete_state(0) is context.get_discrete_state().get_vector(0)) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state_vector()) self.assertTrue( context.get_mutable_discrete_state(0) is context.get_mutable_discrete_state().get_vector(0)) self.assertEqual(context.get_num_abstract_states(), 1) self.assertTrue(context.get_abstract_state() is context.get_mutable_abstract_state()) self.assertTrue( context.get_abstract_state(0) is context.get_mutable_abstract_state(0)) self.assertEqual( context.get_abstract_state(0).get_value(), model_value.get_value()) # Check AbstractValues API. values = context.get_abstract_state() self.assertEqual(values.size(), 1) self.assertEqual( values.get_value(0).get_value(), model_value.get_value()) self.assertEqual( values.get_mutable_value(0).get_value(), model_value.get_value()) values.CopyFrom(values.Clone()) # - Check diagram context accessors. builder = DiagramBuilder() builder.AddSystem(system) diagram = builder.Build() context = diagram.CreateDefaultContext() # Existence check. self.assertIsNot(diagram.GetMutableSubsystemState(system, context), None) subcontext = diagram.GetMutableSubsystemContext(system, context) self.assertIsNot(subcontext, None) self.assertIs(diagram.GetSubsystemContext(system, context), subcontext)
def test_image_to_lcm_image_array_t(self): # Test nominal constructor. dut = mut.ImageToLcmImageArrayT(color_frame_name="color", depth_frame_name="depth", label_frame_name="label", do_compress=False) for port in (dut.color_image_input_port(), dut.depth_image_input_port(), dut.label_image_input_port()): self._check_input(port) self._check_output(dut.image_array_t_msg_output_port()) # Test custom constructor, test functionality (up to getting abstract # value). dut = mut.ImageToLcmImageArrayT(do_compress=False) # Declare ports. for pixel_type in pixel_types: name = str(pixel_type) dut.DeclareImageInputPort[pixel_type](name=name) context = dut.CreateDefaultContext() for pixel_type in pixel_types: name = str(pixel_type) port = dut.GetInputPort(name) self._check_input(port) image = mut.Image[pixel_type](width=1, height=1) context.FixInputPort(port.get_index(), AbstractValue.Make(image)) output = dut.AllocateOutput() dut.CalcOutput(context, output) # N.B. This Value[] is a C++ LCM object. See # `lcm_py_bind_cpp_serializers.h` for more information. self.assertIsInstance(output.get_data(0), AbstractValue)
def test_vector_input_port_fix(self): np_zeros = np.array([0.]) model_value = AbstractValue.Make(BasicVector(np_zeros)) system = PassThrough(len(np_zeros)) context = system.CreateDefaultContext() input_port = system.get_input_port(0) # Fix to a scalar. input_port.FixValue(context, 1.) value = input_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np.array([1.])) # Fix to an ndarray. input_port.FixValue(context, np.array([2.])) value = input_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np.array([2.])) # Fix to a BasicVector. input_port.FixValue(context, BasicVector([3.])) value = input_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np.array([3.])) # Fix to a type-erased BasicVector. input_port.FixValue(context, AbstractValue.Make(BasicVector([4.]))) value = input_port.Eval(context) self.assertEqual(type(value), np.ndarray) np.testing.assert_equal(value, np.array([4.])) # Fix to wrong-sized vector. with self.assertRaises(RuntimeError): input_port.FixValue(context, np.array([0., 1.])) with self.assertRaises(RuntimeError): input_port.FixValue( context, AbstractValue.Make(BasicVector([0., 1.]))) # Fix to a non-vector. with self.assertRaises(TypeError): # A TypeError occurs when pybind Value.set_value cannot match any # overload for how to assign the argument into the erased storage. input_port.FixValue(context, "string") with self.assertRaises(RuntimeError): # A RuntimeError occurs when the Context detects that the # type-erased Value objects are incompatible. input_port.FixValue(context, AbstractValue.Make("string"))
def __init__(self): LeafSystem.__init__(self) self._DeclareAbstractOutputPort( "X_WE", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self._DeclarePeriodicPublish(0.1, 0.0) self.window = tk.Tk() self.window.title("End-Effector TeleOp") self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="roll", length=800, orient=tk.HORIZONTAL) self.roll.pack() self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="pitch", length=800, orient=tk.HORIZONTAL) self.pitch.pack() self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="yaw", length=800, orient=tk.HORIZONTAL) self.yaw.pack() self.x = tk.Scale(self.window, from_=0.1, to=0.8, resolution=-1, label="x", length=800, orient=tk.HORIZONTAL) self.x.pack() self.y = tk.Scale(self.window, from_=-0.3, to=0.3, resolution=-1, label="y", length=800, orient=tk.HORIZONTAL) self.y.pack() self.z = tk.Scale(self.window, from_=0, to=0.8, resolution=-1, label="z", length=800, orient=tk.HORIZONTAL) self.z.pack()
def __init__(self, config_file, viz=False, segment_scene_function=None, get_pose_function=None): """ A system that takes in 3 Drake PointClouds and ImageRgba8U from RGBDCameras and determines the pose of an object in them. The user must supply a segmentation function and pose alignment to determine the pose. If these functions aren't supplied, the returned pose will always be the 4x4 identity matrix. @param config_file str. A path to a .yml configuration file for the cameras. @param viz bool. If True, save the aligned and segmented point clouds as serialized numpy arrays. @param segment_scene_function A Python function that returns a subset of the scene point cloud. See self.SegmentScene for more details. @param get_pose_function A Python function that calculates a pose from a segmented point cloud. See self.GetPose for more details. @system{ @input_port{camera_left_rgb}, @input_port{camera_middle_rgb}, @input_port{camera_right_rgb}, @input_port{left_point_cloud}, @input_port{middle_point_cloud}, @input_port{right_point_cloud}, @output_port{X_WObject} } """ LeafSystem.__init__(self) # TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB # fields. self.left_rgb = self._DeclareAbstractInputPort( "camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.middle_rgb = self._DeclareAbstractInputPort( "camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.right_rgb = self._DeclareAbstractInputPort( "camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.left_depth = self._DeclareAbstractInputPort( "left_point_cloud", AbstractValue.Make(mut.PointCloud())) self.middle_depth = self._DeclareAbstractInputPort( "middle_point_cloud", AbstractValue.Make(mut.PointCloud())) self.right_depth = self._DeclareAbstractInputPort( "right_point_cloud", AbstractValue.Make(mut.PointCloud())) self._DeclareAbstractOutputPort( "X_WObject", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self.segment_scene_function = segment_scene_function self.get_pose_function = get_pose_function self._LoadConfigFile(config_file) self.viz = viz
def test_publisher(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm, publish_period=0.1) model_message = self._model_message() self._fix_and_publish(dut, AbstractValue.Make(model_message)) raw = lcm.get_last_published_message("TEST_CHANNEL") actual_message = quaternion_t.decode(raw) self.assert_lcm_equal(actual_message, model_message)
def test_publisher(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm, publish_period=0.1) subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t) model_message = self._model_message() self._fix_and_publish(dut, AbstractValue.Make(model_message)) lcm.HandleSubscriptions(0) self.assert_lcm_equal(subscriber.message, model_message)
def __init__(self, meshcat_viz, force_threshold=1e-2, contact_force_scale=10, plant=None): """ Args: meshcat_viz: A pydrake MeshcatVisualizer instance. force_threshold: contact forces whose norms are smaller than force_threshold are not displayed. contact_force_scale: a contact force with norm F (in Newtons) is displayed as a cylinder with length F/contact_force_scale (in meters). plant: the MultibodyPlant associated with meshcat_viz.scene_graph. """ LeafSystem.__init__(self) assert plant is not None self._meshcat_viz = meshcat_viz self._force_threshold = force_threshold self._contact_force_scale = contact_force_scale self._plant = plant self.set_name('meshcat_contact_visualizer') self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("pose_bundle", AbstractValue.Make(PoseBundle(0))) # Contact results input port from MultibodyPlant self.DeclareAbstractInputPort( "contact_results", AbstractValue.Make(ContactResults())) # Make force cylinders smaller at initialization. self._force_cylinder_radial_scale = 1. self._force_cylinder_longitudinal_scale = 100. # This system has undeclared states, see #4330. # - All contacts (previous and current), of type `_ContactState`. self._contacts = [] # - Unique key for contacts in meshcat. self._contact_key_counter = 0 # - Previous time at which contact was published. self._t_previous = 0.
def vis_callback(x): vis_diagram_context = diagram.CreateDefaultContext() mbp.SetPositions( diagram.GetMutableSubsystemContext(mbp, vis_diagram_context), x) pose_bundle = scene_graph.get_pose_bundle_output_port().Eval( diagram.GetMutableSubsystemContext(scene_graph, vis_diagram_context)) context = visualizer.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(pose_bundle)) visualizer.Publish(context)
def test_abstract_pass_through(self): model_value = AbstractValue.Make("Hello world") system = PassThrough(model_value) context = system.CreateDefaultContext() context.FixInputPort(0, model_value) output = system.AllocateOutput() input_eval = system.EvalAbstractInput(context, 0) compare_value(self, input_eval, model_value) system.CalcOutput(context, output) output_value = output.get_data(0) compare_value(self, output_value, model_value)
def show_cloud(pc, pc2=None, use_native=False, **kwargs): # kwargs go to ctor. builder = DiagramBuilder() # Add point cloud visualization. if use_native: viz = meshcat.Visualizer(zmq_url=ZMQ_URL) else: plant, scene_graph = AddMultibodyPlantSceneGraph(builder) plant.Finalize() viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) pc_viz = builder.AddSystem( MeshcatPointCloudVisualizer(viz, **kwargs)) if pc2: pc_viz2 = builder.AddSystem( MeshcatPointCloudVisualizer(viz, name='second_point_cloud', X_WP=se3_from_xyz([0, 0.3, 0]), default_rgb=[0., 255., 0.])) # Make sure the system runs. diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() context = diagram.GetMutableSubsystemContext( pc_viz, diagram_context) # TODO(eric.cousineau): Replace `AbstractValue.Make(pc)` with just # `pc` (#12086). pc_viz.GetInputPort("point_cloud_P").FixValue( context, AbstractValue.Make(pc)) if pc2: context = diagram.GetMutableSubsystemContext( pc_viz2, diagram_context) pc_viz2.GetInputPort("point_cloud_P").FixValue( context, AbstractValue.Make(pc2)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(sim_time)
def test_publisher(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm) model = self._model_message() dut.set_publish_period(period=0.1) context = dut.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(model)) dut.Publish(context) raw = lcm.get_last_published_message("TEST_CHANNEL") value = quaternion_t.decode(raw) self.assert_lcm_equal(value, model)