def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
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.GetAsMatrix4())
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
vis = self.vis[self.prefix]
query_object = self.get_geometry_query_input_port().Eval(context)
for frame in self._dynamic_frames:
frame_vis = vis[frame['name']]
X_WF = query_object.GetPoseInWorld(frame['id'])
frame_vis.set_transform(X_WF.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(frame_vis,
self._recording_frame_num) as f:
f.set_transform(X_WF.GetAsMatrix4())
if self._is_recording:
self._recording_frame_num += 1
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] = self._parse_name(pose_bundle.get_name(frame_i))
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?
pose_matrix = pose_bundle.get_transform(frame_i)
cur_vis = (
self.vis[self.prefix][source_name][str(model_id)][frame_name])
cur_vis.set_transform(pose_matrix.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(
cur_vis, self._recording_frame_num) as cur_vis_frame:
cur_vis_frame.set_transform(pose_matrix.GetAsMatrix4())
if self._is_recording:
self._recording_frame_num += 1
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):
LeafSystem.DoPublish(self, context, event)
if self._show:
self.draw(context)
self.fig.canvas.draw()
plt.pause(1e-10)
if self._is_recording:
snapshot = self.AllocateContext()
snapshot.SetTimeStateAndParametersFrom(context)
self.FixInputPortsFrom(self, context, snapshot)
self._recorded_contexts.append(snapshot)
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
if (not self._warned_pose_bundle_input_port_connected
and self._lcm_visualization_input_port.HasValue(context)):
_warn_deprecated(
"The pose_bundle input port is deprecated. Use e.g.\n"
"builder.Connect("
"scene_graph.get_query_output_port(), "
"visualizer.get_geometry_query_input_port())\n"
"instead.",
date="2021-04-01")
self._warned_pose_bundle_input_port_connected = True
vis = self.vis[self.prefix]
if self.get_geometry_query_input_port().HasValue(context):
# New workflow, using SceneGraph's QueryObject.
query_object = self.get_geometry_query_input_port().Eval(context)
for frame in self._dynamic_frames:
frame_vis = vis[frame['name']]
X_WF = query_object.GetPoseInWorld(frame['id'])
frame_vis.set_transform(X_WF.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(
frame_vis, self._recording_frame_num) as f:
f.set_transform(X_WF.GetAsMatrix4())
else:
# Deprecated workflow.
pose_bundle = self._lcm_visualization_input_port.Eval(context)
for frame_i in range(pose_bundle.get_num_poses()):
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = pose_bundle.get_name(frame_i).replace("::", "/")
frame_vis = vis[name]
pose_matrix = pose_bundle.get_transform(frame_i)
frame_vis.set_transform(pose_matrix.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(
frame_vis, self._recording_frame_num) as f:
f.set_transform(pose_matrix.GetAsMatrix4())
if self._is_recording:
self._recording_frame_num += 1
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
if self._show:
self.draw(context)
self.fig.canvas.draw()
self._plt.pause(1e-10)
if self._is_recording:
snapshot = self.AllocateContext()
snapshot.SetTimeStateAndParametersFrom(context)
self.FixInputPortsFrom(self, context, snapshot)
self._recorded_contexts.append(snapshot)
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
vis = self.vis[self.prefix]
query_object = self.get_geometry_query_input_port().Eval(context)
for frame in self._dynamic_frames:
frame_vis = vis[frame['name']]
X_WF = query_object.GetPoseInWorld(frame['id'])
frame_vis.set_transform(X_WF.GetAsMatrix4())
if self._is_recording:
with self._animation.at_frame(
frame_vis, self._recording_frame_num) as f:
f.set_transform(X_WF.GetAsMatrix4())
if self._is_recording:
self._recording_frame_num += 1
def DoPublish(self, context, event):
# TODO(russt): Copied from meshcat_visualizer.py.
# Change this to declare a periodic event with a
# callback instead of overriding DoPublish, pending #9992.
LeafSystem.DoPublish(self, context, event)
self.callback_lock.acquire()
if self.stop:
self.stop = False
if context.get_time() > 0.5:
self.callback_lock.release()
raise StopIteration
#query_object = self.EvalAbstractInput(context, 0).get_value()
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
if self.grab_needs_update:
hydra_tf = self.grab_update_hydra_pose
self.grab_needs_update = False
# If grab point is colliding...
#print [x.distance for x in query_object.ComputeSignedDistanceToPoint(hydra_tf.matrix()[:3, 3])]
# Find closest body to current pose
grab_center = hydra_tf.matrix()[:3, 3]
closest_distance = np.inf
closest_body = self.mbp.get_body(BodyIndex(2))
for body_id in self.all_manipulable_body_ids:
body = self.mbp.get_body(body_id)
offset = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
dist = np.linalg.norm(grab_center - offset.translation())
if dist < closest_distance:
closest_distance = dist
closest_body = body
self.selected_body = closest_body
self.selected_body_init_offset = self.mbp.EvalBodyPoseInWorld(
self.mbp_context, self.selected_body)
self.callback_lock.release()
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)
mbp_state_vector = self.EvalVectorInput(context, 0).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, mbp_state_vector)
# Get pose of object
for body_name in self.body_names:
print(body_name, ": ")
print(
self.mbp.EvalBodyPoseInWorld(
self.mbp_context,
self.mbp.GetBodyByName(body_name)).matrix())
rigid_transform_dict = {}
for body_name in self.body_names:
rigid_transform_dict[body_name] = self.mbp.EvalBodyPoseInWorld(
self.mbp_context, self.mbp.GetBodyByName(body_name))
self._symbol_logger.log_symbols(rigid_transform_dict)
self._symbol_logger.print_curr_symbols()
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)
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):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# The contact frame's origin is located at contact_point and is
# oriented such that Cy is aligned with the contact force.
p_WC = contact_info.contact_point() # documented as in world.
if force_norm < 1e-6:
# We cannot rely on self._force_threshold to determine if the
# force can be normalized; that threshold can be zero.
R_WC = RotationMatrix()
else:
fhat_C_W = contact_info.contact_force() / force_norm
R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W,
axis_index=1)
X_WC = RigidTransform(R=R_WC, p=p_WC)
cvis.set_transform(X_WC.GetAsMatrix4())
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
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)
print("In publish at time ", context.get_time())
if context.get_time() <= 1E-3:
return
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
bbox_bundle = self.EvalAbstractInput(context, 1)
if bbox_bundle:
bbox_bundle = bbox_bundle.get_value()
for k in range(bbox_bundle.get_num_bboxes()):
pose = self.global_transform.multiply(
bbox_bundle.get_bbox_pose(k))
if (k + 1) > self.num_registered_bounding_boxes:
# Register a new one
color = bbox_bundle.get_bbox_color(k)
self.bsi.send_remote_call(
"register_material",
name="mat_bb_%d" % k,
material_type="color",
color=bbox_bundle.get_bbox_color(k))
self.bsi.send_remote_call("update_material_parameters",
type="Principled BSDF",
name="mat_bb_%d" % k,
**{"Specular": 0.0})
self.bsi.send_remote_call("update_material_parameters",
type=None,
name="mat_bb_%d" % k,
**{"blend_method": "ADD"})
self.bsi.send_remote_call(
"register_object",
name="obj_bb_%d" % k,
type="cube",
scale=[x / 2 for x in bbox_bundle.get_bbox_scale(k)],
location=pose.translation().tolist(),
quaternion=pose.quaternion().wxyz().tolist(),
material="mat_bb_%d" % k)
self.bsi.send_remote_call("apply_modifier_to_object",
name="obj_bb_%d" % k,
type="WIREFRAME",
thickness=0.1)
self.num_registered_bounding_boxes = k + 1
self.num_visible_bounding_boxes = k + 1
else:
# Update parameters of existing bbox + its material
self.bsi.send_remote_call(
"update_material_parameters",
type="Principled BSDF",
name="mat_bb_%d" % k,
**{"Base Color": bbox_bundle.get_bbox_color(k)})
self.bsi.send_remote_call(
"update_object_parameters",
name="obj_bb_%d" % k,
scale=[x / 2 for x in bbox_bundle.get_bbox_scale(k)],
location=pose.translation().tolist(),
rotation_mode='QUATERNION',
rotation_quaternion=pose.rotation().ToQuaternion(
).wxyz().tolist(),
hide_render=False)
self.num_visible_bounding_boxes = k + 1
for k in range(bbox_bundle.get_num_bboxes(),
self.num_visible_bounding_boxes):
self.bsi.send_remote_call("update_object_parameters",
name="obj_bb_%d" % k,
hide_render=True)
# print('pose_bundle type', type(pose_bundle))
for frame_i in range(pose_bundle.get_num_poses()):
link_name = pose_bundle.get_name(frame_i)
# print('link_name', link_name)
[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]):
# I am sure this can be written shorter than this, this bloat is from
# when this was isometry-based.
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().wxyz()
geom_name = self._format_geom_name(source_name, frame_name, 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.jpg" % (
i, self.current_publish_num)
self.last_out_filepath = out_filepath
self.bsi.send_remote_call("render",
camera_name="cam_%d" % i,
filepath=out_filepath)
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 DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
if (not self._warned_pose_bundle_input_port_connected
and self.get_input_port(0).HasValue(context)):
_warn_deprecated(
"The pose_bundle input port of MeshcatContactVisualizer is"
"deprecated; use the geometry_query inport port instead.",
date="2021-04-01")
self._warned_pose_bundle_input_port_connected = True
contact_results = self.EvalAbstractInput(context, 1).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# Frame C is located at the contact point, but with the world frame
# orientation.
if force_norm < 1e-6:
X_CGeom = tf.identity_matrix()
else:
# Rotates [0,1,0] to contact_force/force_norm.
angle_axis = np.cross(
np.array([0, 1, 0]),
contact_info.contact_force() / force_norm)
X_CGeom = tf.rotation_matrix(
np.arcsin(np.linalg.norm(angle_axis)), angle_axis)
X_WC = tf.translation_matrix(contact_info.contact_point())
cvis.set_transform(X_WC @ X_CGeom)
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
def DoPublish(self, context, events):
# N.B. Prevent recursion.
LeafSystem.DoPublish(self, context, events)
for func_declaration in self._func_declaration:
func_declaration.maybe_publish(context)
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
if self._show:
self.draw(context)
self.fig.canvas.draw()
plt.pause(1e-10)
