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)
