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)