def _draw_deprecated(self, context):
"""Drawing via the deprecated PoseBundle mechanism"""
if not self._warned_pose_bundle_input_port_connected:
_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-11-01")
self._warned_pose_bundle_input_port_connected = True
pose_bundle = self._pose_bundle_port.Eval(context)
view_dir = np.cross(self._T_VW[0, :3], self._T_VW[1, :3])
for frame_i in range(pose_bundle.get_num_poses()):
# SceneGraph currently sets the name in PoseBundle as
# "get_source_name::frame_name".
full_name = pose_bundle.get_name(frame_i)
X_WB = pose_bundle.get_transform(frame_i)
patch_Wlist, _ = self._get_view_patches(full_name, X_WB)
for i, patch_W in enumerate(patch_Wlist):
# Project the object vertices from 3d in world frame W to 2d in
# view frame V (keeps homogeneous portion, removing it later).
patch_V = self._project_patch(patch_W)
body_fill = self._body_fill_dict[full_name][i]
# Use the latest vertices to update the body_fill.
self._update_body_fill_verts(body_fill, patch_V)
body_fill.zorder = X_WB.translation() @ view_dir
self.ax.set_title('t = {:.1f}'.format(context.get_time()))
def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD,
name="point_cloud", X_WP=RigidTransform.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)
_warn_deprecated(_DEPRECATION, date="2022-09-01")
self._meshcat_viz = _get_native_visualizer(meshcat_viz)
self._X_WP = RigidTransform(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 _getattr(name):
if name in ["AbstractValue", "Value"]:
old_symbol = f"{__name__}.{name}"
new_symbol = f"{_value.__name__}.{name}"
_deprecation._warn_deprecated(
f"{old_symbol} has moved to {new_symbol}. Please use it instead. "
f"This will be removed on or after 2020-08-01.")
return getattr(_value, name)
else:
raise AttributeError()
def __init__(self, vertices, normals):
super(HydroTriSurface, self).__init__()
_warn_deprecated(_DEPRECATION, date="2022-09-01")
vertices = np.asarray(vertices, dtype=np.float32)
normals = np.asarray(normals, dtype=np.float32)
assert (vertices.shape[0] % 3) == 0, "vertices must have 3N values"
assert vertices.shape[1] == 3, "`vertices` must be an 3Nx3 array"
assert normals.shape[1] == 3, "`normals` must be an 3Nx3 array"
assert vertices.shape[0] == normals.shape[0], \
"'vertices' and 'normals' must be the same size"
self.vertices = vertices
self.normals = normals
def _deprecation_handler(name):
binding_prefix = "Binding_"
if name.startswith(binding_prefix):
cls_name = name[len(binding_prefix):]
new_name = f"Binding[{cls_name}]"
_warn_deprecated(
f"{name} is deprecated; please use {new_name} instead.",
date="2021-11-01",
)
cls = globals()[cls_name]
return Binding[cls]
else:
raise AttributeError()
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 __init__(self,
meshcat_viz,
force_threshold=1e-2,
contact_force_scale=10,
plant=None,
contact_force_radius=0.01):
"""
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.
contact_force_radius: sets the constant radius of the cylinder used
to visualize the forces.
"""
LeafSystem.__init__(self)
_warn_deprecated(_DEPRECATION, date="2022-09-01")
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._radius = contact_force_radius
self.set_name('meshcat_contact_visualizer')
self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort(
"contact_results", AbstractValue.Make(ContactResults()))
# This system has undeclared states, see #4330.
self._published_contacts = []
# Zap any previous contact forces on this prefix
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
vis.delete()
def get_instantiation(self, param=None, throw_error=True):
"""Gets the instantiation for the given parameters.
Args:
param: Can be None, a single parameter, or a tuple/list of
parameters.
Returns:
(instantiation, param), where `param` is the resolved set of
parameters.
"""
param = self._param_resolve(param)
instantiation = self._instantiation_map.get(param)
if instantiation is TemplateBase._deferred:
assert self._instantiation_func is not None
instantiation = self._instantiation_func(param)
self._add_instantiation_internal(param, instantiation)
elif instantiation is None and throw_error:
raise RuntimeError("Invalid instantiation: {}".format(
self._instantiation_name(param)))
deprecation = self._deprecation_map.get(param)
if deprecation is not None:
_warn_deprecated(deprecation)
return (instantiation, param)
def __new__(cls, *args, **kwargs):
_warn_deprecated(_deprecation_msg)
return list(*args, **kwargs)
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 __init__(self,
scene_graph=None,
draw_period=_DEFAULT_PUBLISH_PERIOD,
prefix="drake",
zmq_url="default",
open_browser=None,
frames_to_draw=[],
frames_opacity=1.,
axis_length=0.15,
axis_radius=0.006,
delete_prefix_on_load=True,
role=Role.kIllustration,
prefer_hydro=False,
**kwargs):
"""
Args:
scene_graph: A SceneGraph object. This argument is optional, and
is only needed to enable calls to ``load()`` without providing
a Context.
draw_period: The rate at which this class publishes to the
visualizer.
prefix: Appears as the root of the tree structure in the meshcat
data structure
zmq_url: Optionally set a url to connect to the visualizer.
Use ``zmp_url="default"`` to the value obtained by running a
single ``meshcat-server`` in another terminal.
Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server
(as a child of this process); a new web browser will be opened
(the url will also be printed to the console).
Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a
specific address.
open_browser: Set to True to open the meshcat browser url in your
default web browser. The default value of None will open the
browser iff a new meshcat server is created as a subprocess.
Set to False to disable this.
frames_to_draw: a list or array containing pydrake.geometry.FrameId
for which body frames to draw. Note that these are frames
registered within the scene_graph. For multibody frames, users
may obtain the ids from plant using GetBodyFrameIdIfExists.
Currently, frames that are not body frames are not supported
as they do not exist in the scene_graph yet.
frames_opacity, axis_length and axis_radius are the opacity, length
and radius of the coordinate axes to be drawn.
delete_prefix_on_load: Specifies whether we should delete the
visualizer path associated with prefix on our load
initialization event. True ensures a clean slate for every
simulation. False allows for the possibility of caching object
meshes on the zmqserver/clients, to avoid repeatedly
downloading meshes over the websockets link, but will cause
geometry from previous simulations to remain in the scene. You
may call ``delete_prefix()`` manually to clear the scene.
role: Renders geometry of the specified pydrake.geometry.Role
type -- defaults to Role.kIllustration to draw visual geometry,
and also supports Role.kProximity to draw collision geometry.
prefer_hydro: If True (and role == Role.kProximity) any geometry
that has a mesh hydroelastic representation will be visualized
by that discrete mesh and not the declared primitive. In the
case of *compliant* hydroelastic geometries, only the surface
of the volume mesh will be drawn. This is *not* the *contact*
surface used to characterize contact between two geometries
with hydroelastic representations -- it is the mesh
representations of those geometries.
Additional kwargs will be passed to the meshcat.Visualizer constructor.
Note:
This call will not return until it connects to the
``meshcat-server``.
"""
LeafSystem.__init__(self)
_warn_deprecated(_DEPRECATION, date="2022-09-01", stacklevel=3)
self.set_name('meshcat_visualizer')
self.DeclarePeriodicPublish(draw_period, 0.0)
self.draw_period = draw_period
self._delete_prefix_on_load = delete_prefix_on_load
if role not in [Role.kIllustration, Role.kProximity]:
raise ValueError("Unsupported role type specified: ", role)
self._role = role
self._prefer_hydro = prefer_hydro
# Recording.
self._is_recording = False
self.reset_recording()
self._scene_graph = scene_graph
self._geometry_query_input_port = self.DeclareAbstractInputPort(
"geometry_query", AbstractValue.Make(QueryObject()))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
elif zmq_url == "new":
zmq_url = None
if zmq_url is None and open_browser is None:
open_browser = True
# Set up meshcat.
self.prefix = prefix
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url, **kwargs)
print("Connected to meshcat-server.")
# Set background color (to match drake-visualizer).
self.vis['/Background'].set_property("top_color", [0.95, 0.95, 1.0])
self.vis['/Background'].set_property("bottom_color", [.32, .32, .35])
if open_browser:
webbrowser.open(self.vis.url())
def on_initialize(context, event):
self.load(context)
# TODO(russt): #13776 recommends we stop using initialization events
# for this.
self.DeclareInitializationEvent(
event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=on_initialize))
# TODO(russt): Move this to a cache entry as in DrakeVisualizer.
# Requires #14287.
self._dynamic_frames = []
# drawing coordinate frames
self.frames_to_draw = set(frames_to_draw)
self.frames_opacity = frames_opacity
self.axis_length = axis_length
self.axis_radius = axis_radius
def __init__(self, option_strings, dest, nargs=None, **kwargs):
if nargs is not None:
raise ValueError("nargs not allowed")
super().__init__(option_strings, dest, **kwargs)
_warn_deprecated(_DEPRECATION, date="2022-09-01", stacklevel=5)