def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def _DoPublish(self, context, event):
# TODO(russt): Change this to declare a periodic event with a
# callback instead of overriding _DoPublish, pending #9992.
LeafSystem._DoPublish(self, context, event)
if context.get_time() <= 1E-3:
return
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
for frame_i in range(pose_bundle.get_num_poses()):
link_name = pose_bundle.get_name(frame_i)
[source_name, frame_name] = self._parse_name(link_name)
model_id = pose_bundle.get_model_instance_id(frame_i)
# pose_matrix = pose_bundle.get_pose(frame_i)
pose_matrix = pose_bundle.get_transform(frame_i)
for j in range(self.num_link_geometries_by_link_name[link_name]):
offset = RigidTransform(
self.global_transform.GetAsMatrix4().dot(
# pose_matrix.matrix().dot(
pose_matrix.GetAsMatrix4().dot(
self.link_subgeometry_local_tfs_by_link_name[
link_name][j])))
location = offset.translation()
quaternion = offset.rotation().ToQuaternion()
geom_name = self._format_geom_name(source_name, frame_name,
model_id, j)
self.bsi.send_remote_call(
"update_object_parameters",
name="obj_" + geom_name,
location=location.tolist(),
rotation_mode='QUATERNION',
rotation_quaternion=quaternion.tolist())
n_cams = len(self.camera_tfs)
for i in range(len(self.camera_tfs)):
out_filepath = self.out_prefix + "%02d_%08d_label.bmp" % (
i, self.current_publish_num)
im = self.bsi.render_image("cam_%d" % i, filepath=out_filepath)
if self.show_figure:
plt.subplot(1, n_cams, i + 1)
plt.imshow(np.transpose(im, [1, 0, 2]))
if self.save_scene:
scene_filepath = self.out_prefix + "_scene.blend"
self.bsi.send_remote_call("save_current_scene",
path=scene_filepath)
self.current_publish_num += 1
if self.show_figure:
plt.pause(0.01)
print("Rendered a frame at %f seconds sim-time." %
(context.get_time()))
def test_deprecated_protected_aliases(self):
"""Tests a subset of protected aliases, pursuant to #9651."""
class OldSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
# Check a non-overridable method
with catch_drake_warnings(expected_count=1):
self._DeclareVectorInputPort("x", BasicVector(1))
def _DoPublish(self, context, events):
self.called_publish = True
# Ensure old overrides are still used
system = OldSystem()
context = system.CreateDefaultContext()
with catch_drake_warnings(expected_count=1):
system.Publish(context)
self.assertTrue(system.called_publish)
# Ensure documentation doesn't duplicate stuff.
with catch_drake_warnings(expected_count=1):
self.assertIn("deprecated", LeafSystem._DoPublish.__doc__)
# This will warn both on (a) calling the method and (b) on the
# invocation of the override.
with catch_drake_warnings(expected_count=2):
LeafSystem._DoPublish(system, context, [])
class AccidentallyBothSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_old_publish = False
self.called_new_publish = False
def DoPublish(self, context, events):
self.called_new_publish = True
def _DoPublish(self, context, events):
self.called_old_publish = True
system = AccidentallyBothSystem()
context = system.CreateDefaultContext()
# This will trigger no deprecations, as the newer publish is called.
system.Publish(context)
self.assertTrue(system.called_new_publish)
self.assertFalse(system.called_old_publish)
def test_deprecated_protected_aliases(self):
"""Tests a subset of protected aliases, pursuant to #9651."""
class OldSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
# Check a non-overridable method
with catch_drake_warnings(expected_count=1):
self._DeclareVectorInputPort("x", BasicVector(1))
def _DoPublish(self, context, events):
self.called_publish = True
# Ensure old overrides are still used
system = OldSystem()
context = system.CreateDefaultContext()
with catch_drake_warnings(expected_count=1):
system.Publish(context)
self.assertTrue(system.called_publish)
# Ensure documentation doesn't duplicate stuff.
with catch_drake_warnings(expected_count=1):
self.assertIn("deprecated", LeafSystem._DoPublish.__doc__)
# This will warn both on (a) calling the method and (b) on the
# invocation of the override.
with catch_drake_warnings(expected_count=2):
LeafSystem._DoPublish(system, context, [])
class AccidentallyBothSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_old_publish = False
self.called_new_publish = False
def DoPublish(self, context, events):
self.called_new_publish = True
def _DoPublish(self, context, events):
self.called_old_publish = True
system = AccidentallyBothSystem()
context = system.CreateDefaultContext()
# This will trigger no deprecations, as the newer publish is called.
system.Publish(context)
self.assertTrue(system.called_new_publish)
self.assertFalse(system.called_old_publish)
def _DoPublish(self, context, event):
# TODO(russt): Change this to declare a periodic event with a
# callback instead of overriding _DoPublish, pending #9992.
LeafSystem._DoPublish(self, context, event)
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
for frame_i in range(pose_bundle.get_num_poses()):
# SceneGraph currently sets the name in PoseBundle as
# "get_source_name::frame_name".
[source_name, frame_name] = pose_bundle.get_name(frame_i)\
.split("::")
model_id = pose_bundle.get_model_instance_id(frame_i)
# The MBP parsers only register the plant as a nameless source.
# TODO(russt): Use a more textual naming convention here?
self.vis[self.prefix][source_name][str(model_id)][frame_name]\
.set_transform(pose_bundle.get_pose(frame_i).matrix())
def _DoPublish(self, context, event):
# TODO(russt): Change this to declare a periodic event with a
# callback instead of overriding _DoPublish, pending #9992.
LeafSystem._DoPublish(self, context, event)
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
for frame_i in range(pose_bundle.get_num_poses()):
# SceneGraph currently sets the name in PoseBundle as
# "get_source_name::frame_name".
[source_name, frame_name] = pose_bundle.get_name(frame_i)\
.split("::")
model_id = pose_bundle.get_model_instance_id(frame_i)
# The MBP parsers only register the plant as a nameless source.
# TODO(russt): Use a more textual naming convention here?
self.vis[self.prefix][source_name][str(model_id)][frame_name]\
.set_transform(pose_bundle.get_pose(frame_i).matrix())
def _DoPublish(self, context, event):
LeafSystem._DoPublish(self, context, event)
point_cloud_P = self.EvalAbstractInput(context, 0).get_value()
# `Q` is a point in the point cloud.
p_PQs = point_cloud_P.xyzs()
# Use only valid points.
valid = np.logical_not(np.isnan(p_PQs))
valid = np.all(valid, axis=0) # Reduce along XYZ axis.
p_PQs = p_PQs[:, valid]
if point_cloud_P.has_rgbs():
rgbs = point_cloud_P.rgbs()[:, valid]
else:
# Need manual broadcasting.
count = p_PQs.shape[1]
rgbs = np.tile(np.array([self._default_rgb]).T, (1, count))
# pydrake `PointCloud.rgbs()` are on [0..255], while meshcat
# `PointCloud` colors are on [0..1].
rgbs = rgbs / 255. # Do not use in-place so we can promote types.
# Send to meshcat.
self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs))
self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())
def _DoPublish(self, context, event):
LeafSystem._DoPublish(self, context, event)
point_cloud_P = self.EvalAbstractInput(context, 0).get_value()
# `Q` is a point in the point cloud.
p_PQs = point_cloud_P.xyzs()
# Use only valid points.
valid = np.logical_not(np.isnan(p_PQs))
valid = np.all(valid, axis=0) # Reduce along XYZ axis.
p_PQs = p_PQs[:, valid]
if point_cloud_P.has_rgbs():
rgbs = point_cloud_P.rgbs()[:, valid]
else:
# Need manual broadcasting.
count = p_PQs.shape[1]
rgbs = np.tile(np.array([self._default_rgb]).T, (1, count))
# pydrake `PointCloud.rgbs()` are on [0..255], while meshcat
# `PointCloud` colors are on [0..1].
rgbs = rgbs / 255. # Do not use in-place so we can promote types.
# Send to meshcat.
self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs))
self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())
def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
self.called_publish = True
def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
self.called_publish = True
def _DoPublish(self, context, event):
LeafSystem._DoPublish(self, context, event)
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
X_WB_map = self._get_pose_map(pose_bundle)
self._draw_contact_forces(context, X_WB_map)
def _DoPublish(self, context, event):
LeafSystem._DoPublish(self, context, event)
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
X_WB_map = self._get_pose_map(pose_bundle)
self._draw_contact_forces(context, X_WB_map)