def test_event_api(self): # TriggerType - existence check. TriggerType.kUnknown TriggerType.kInitialization TriggerType.kForced TriggerType.kTimed TriggerType.kPeriodic TriggerType.kPerStep TriggerType.kWitness # PublishEvent. # TODO(eric.cousineau): Test other event types when it is useful to # expose them. def callback(context, event): pass event = PublishEvent(trigger_type=TriggerType.kInitialization, callback=callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization) # Simple discrete-time system. system1 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.1) periodic_data = system1.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsInstance(periodic_data, PeriodicEventData) self.assertIsInstance(periodic_data.Clone(), PeriodicEventData) periodic_data.period_sec() periodic_data.offset_sec() # Simple continuous-time system. system2 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.0) periodic_data = system2.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsNone(periodic_data)
def test_event_api(self): # TriggerType - existence check. TriggerType.kUnknown TriggerType.kInitialization TriggerType.kForced TriggerType.kTimed TriggerType.kPeriodic TriggerType.kPerStep TriggerType.kWitness # PublishEvent. # TODO(eric.cousineau): Test other event types when it is useful to # expose them. def callback(context, event): pass event = PublishEvent( trigger_type=TriggerType.kInitialization, callback=callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization) # Simple discrete-time system. system1 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.1) periodic_data = system1.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsInstance(periodic_data, PeriodicEventData) self.assertIsInstance(periodic_data.Clone(), PeriodicEventData) periodic_data.period_sec() periodic_data.offset_sec() # Simple continuous-time system. system2 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.0) periodic_data = system2.GetUniquePeriodicDiscreteUpdateAttribute() self.assertIsNone(periodic_data)
def __init__(self, visualization_topic="/drake", period_sec=1. / 60, tf_topic="/tf", role=Role.kPerception): """ Arguments: ... role: The Role to visualize geometry for. """ LeafSystem.__init__(self) self._role = role self._geometry_query = self.DeclareAbstractInputPort( "geometry_query", AbstractValue.Make(QueryObject())) self.DeclareInitializationEvent( event=PublishEvent(trigger_type=TriggerType.kInitialization, callback=self._initialize)) self.DeclarePeriodicEvent(period_sec=period_sec, offset_sec=0., event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._publish_tf)) # TODO(eric.cousineau): Rather than explicitly allocate publishers, # this should probably instead output the messages. Either together as a # tuple, or separately. Should delegate that to `ConnectRvizVisualizer`. self._marker_pub = rospy.Publisher(visualization_topic, MarkerArray, queue_size=1) self._tf_pub = rospy.Publisher(tf_topic, TFMessage, queue_size=1) self._marker_array_old = None self._marker_array_old_stamp = None
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_continuous = False self.called_discrete = False self.called_initialize = False self.called_per_step = False self.called_periodic = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate(period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent(trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent(trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(PortDataType.kVectorValued, 1) self.DeclareVectorInputPort(name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_continuous = False self.called_discrete = False self.called_initialize = False self.called_per_step = False self.called_periodic = False self.called_getwitness = False self.called_witness = False self.called_guard = False self.called_reset = False self.called_system_reset = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate(period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent(trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent(trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(kUseDefaultName, PortDataType.kVectorValued, 1) self.DeclareVectorInputPort(name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort("noop", BasicVector(1), noop, prerequisites_of_calc=set( [self.nothing_ticket()])) self.witness = self.MakeWitnessFunction( "witness", WitnessFunctionDirection.kCrossesZero, self._witness) # Test bindings for both callback function signatures. self.reset_witness = self.MakeWitnessFunction( "reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent(self._reset)) self.system_reset_witness = self.MakeWitnessFunction( "system reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent( system_callback=self._system_reset))
def __init__(self, *args, update_period_sec=0.1): """ Constructs the system and displays the widgets. Args: update_period_sec: Specifies how often the kernel is queried for widget ui events. An output port is created for each element in *args. Each element of args must itself be an iterable collection (list, tuple, set, ...) of ipywidget elements, whose values will be concatenated into a single vector output. """ LeafSystem.__init__(self) self._widgets = args for i, widget_iterable in enumerate(self._widgets): for w in widget_iterable: assert isinstance(w, Widget), ( "args must be collections of widgets") display(w) port = self.DeclareVectorOutputPort( f"widget_group_{i}", len(widget_iterable), partial(self.DoCalcOutput, port_index=i)) port.disable_caching_by_default() self.DeclarePeriodicEvent(update_period_sec, 0.0, PublishEvent(self._process_event_queue))
def __init__(self, draw_period=1. / 30, facecolor=[1, 1, 1], figsize=None, ax=None): LeafSystem.__init__(self) self.set_name('pyplot_visualization') self.timestep = draw_period self.DeclarePeriodicPublish(draw_period, 0.0) if ax is None: self.fig = plt.figure(facecolor=facecolor, figsize=figsize) self.ax = plt.axes() self.fig.add_axes(self.ax) else: self.ax = ax self.fig = ax.get_figure() self.ax.axis('equal') self.ax.axis('off') self._show = (matplotlib.get_backend().lower() != 'template') def on_initialize(context, event): if self._show: self.fig.show() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def __init__(self, *, tf_broadcaster, joints, period_sec=1. / 60): super().__init__() if not isinstance(tf_broadcaster, TransformBroadcaster): raise TypeError('tf_broadcaster must be a TransformBroadcaster') self._tf_broadcaster = tf_broadcaster # System will publish transforms for these joints only self._joints = tuple(joints) if 0 == len(self._joints): raise ValueError('Need at least one joint to publish transforms') for joint in self._joints: if not Joint_.is_subclass_of_instantiation(type(joint)): raise TypeError('joints must be an iterable of Joint_[T]') self._time_input_port = self.DeclareAbstractInputPort( 'clock', AbstractValue.Make(float)) # This port receives body poses from MultibodyPlant self._poses_input_port = self.DeclareAbstractInputPort( 'body_poses', AbstractValue.Make([RigidTransform()])) # This event publishes TF transforms at a regular interval self.DeclarePeriodicEvent(period_sec=period_sec, offset_sec=0., event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._publish_tf))
def __init__(self): LeafSystem.__init__(self) self.DeclareVectorInputPort("touchdown_angle", BasicVector(1)) self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0) self.DeclareVectorOutputPort("state", BasicVector(8), self.CopyStateOut) # Parameters from Geyer05, p.23 self.mass = 80. # kg self.r0 = 1. # m self.gravity = 9.81 # m/s^2 # Define spring constant in terms of the dimensionless number. # Definition in section 2.4.3, values in figure 2.4. # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts). dimensionless_spring_constant = 10.7 self.stiffness = (dimensionless_spring_constant * self.mass * self.gravity / self.r0) self.last_apex = None # placeholder for writing return map result. self.touchdown_witness = self.MakeWitnessFunction( "touchdown", WitnessFunctionDirection.kPositiveThenNonPositive, self.foot_height, UnrestrictedUpdateEvent(self.touchdown)) self.takeoff_witness = self.MakeWitnessFunction( "takeoff", WitnessFunctionDirection.kPositiveThenNonPositive, self.leg_compression, UnrestrictedUpdateEvent(self.takeoff)) self.apex_witness = self.MakeWitnessFunction( "apex", WitnessFunctionDirection.kPositiveThenNonPositive, self.apex, PublishEvent(self.publish_apex))
def test_event_api(self): # TriggerType - existence check. TriggerType.kUnknown TriggerType.kInitialization TriggerType.kForced TriggerType.kTimed TriggerType.kPeriodic TriggerType.kPerStep TriggerType.kWitness # PublishEvent. # TODO(eric.cousineau): Test other event types when it is useful to # expose them. def callback(context, event): pass event = PublishEvent( trigger_type=TriggerType.kInitialization, callback=callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization)
def test_event_api(self): # TriggerType - existence check. TriggerType.kUnknown TriggerType.kInitialization TriggerType.kForced TriggerType.kTimed TriggerType.kPeriodic TriggerType.kPerStep TriggerType.kWitness # PublishEvent. # TODO(eric.cousineau): Test other event types when it is useful to # expose them. def callback(context, event): pass event = PublishEvent( trigger_type=TriggerType.kInitialization, callback=callback) self.assertIsInstance(event, Event) self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization)
def __init__(self, window=None, open_position=0.107, closed_position=0.002, force_limit=40, update_period_sec=0.05): """" Args: window: Optionally pass in a tkinter.Tk() object to add these widgets to. Default behavior is to create a new window. update_period_sec: Specifies how often the window update() method gets called. open_position: Target position for the finger when open. closed_position: Target position for the gripper when closed. force_limit: Force limit to send to Schunk WSG controller. """ LeafSystem.__init__(self) self.DeclareVectorOutputPort("position", BasicVector(1), self.CalcPositionOutput) self.DeclareVectorOutputPort("force_limit", BasicVector(1), self.CalcForceLimitOutput) if window is None: self.window = tk.Tk() self.window.title("Schunk WSG Buttons") else: self.window = window # Schedule window updates in either case (new or existing window): self.DeclarePeriodicEvent(update_period_sec, 0.0, PublishEvent(self._update)) self._open_button = tk.Button(self.window, text="Open Gripper (spacebar)", state=tk.DISABLED, command=self.open) self._open_button.pack() self._close_button = tk.Button(self.window, text="Close Gripper (spacebar)", command=self.close) self._close_button.pack() self._open_state = True self._open_position = open_position self._closed_position = closed_position self._force_limit = force_limit self.window.bind("<space>", self._space_callback)
def __init__(self, draw_period=None, facecolor=[1, 1, 1], figsize=None, ax=None, show=None): LeafSystem.__init__(self) # To help avoid small simulation timesteps, we use a default period # that has an exact representation in binary floating point; see # drake#15021 for details. default_draw_period = 1. / 32 self.set_name('pyplot_visualization') self.timestep = draw_period or default_draw_period self.DeclarePeriodicPublish(self.timestep, 0.0) if ax is None: self.fig = plt.figure(facecolor=facecolor, figsize=figsize) self.ax = plt.axes() self.fig.add_axes(self.ax) else: self.ax = ax self.fig = ax.get_figure() if show is None: show = (matplotlib.get_backend().lower() != 'template') self._show = show self.ax.axis('equal') self.ax.axis('off') if not show: # This is the preferred way to support the jupyter notebook # animation workflow and the `inline` backend grabbing an # extraneous render of the figure. plt.close(self.fig) self._is_recording = False self._recorded_contexts = [] def on_initialize(context, event): if self._show: self.fig.show() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def __init__(self, *, publisher, period_sec): super().__init__() self.DeclareAbstractOutputPort('clock', lambda: AbstractValue.Make(float), self._do_calculate_clock) self.DeclarePeriodicEvent(period_sec=period_sec, offset_sec=0., event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._do_accumulate_clock)) self._publisher = publisher self._period = period_sec self._time_state = self.DeclareAbstractState( AbstractValue.Make(float(0)))
def __init__(self, *, publisher, joints, period_sec=1. / 60): super().__init__() # System will publish transforms for these joints only self._joints = tuple(joints) if 0 == len(self._joints): raise ValueError('Need at least one joint to publish joint states') for joint in self._joints: if not Joint_.is_subclass_of_instantiation(type(joint)): raise TypeError('joints must be an iterable of Joint_[T]') # This event publishes TF transforms at a regular interval self.DeclarePeriodicEvent(period_sec=period_sec, offset_sec=0., event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._publish_joint_states))
def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_continuous = False self.called_discrete = False self.called_initialize = False # Ensure we have desired overloads. self._DeclarePeriodicPublish(0.1) self._DeclarePeriodicPublish(0.1, 0) self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.) self._DeclarePeriodicDiscreteUpdate( period_sec=0.1, offset_sec=0.) self._DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self._DeclareContinuousState(2) self._DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self, draw_period=None, facecolor=[1, 1, 1], figsize=None, ax=None, show=None): LeafSystem.__init__(self) # On Ubuntu the Debian package python3-tk is a recommended (but not # required) dependency of python3-matplotlib; help users understand # that by providing a nicer message upon a failure to import. try: import matplotlib.pyplot as plt self._plt = plt except ImportError as e: if e.name == 'tkinter': self._plt = None else: raise if self._plt is None: raise NotImplementedError( "On Ubuntu when using the default pyplot configuration (i.e.," " the TkAgg backend) you must 'sudo apt install python3-tk' to" " obtain Tk support. Alternatively, you may set MPLBACKEND to" " something else (e.g., Qt5Agg).") # To help avoid small simulation timesteps, we use a default period # that has an exact representation in binary floating point; see # drake#15021 for details. default_draw_period = 1. / 32 self.set_name('pyplot_visualization') self.timestep = draw_period or default_draw_period self.DeclarePeriodicPublish(self.timestep, 0.0) if ax is None: self.fig = self._plt.figure(facecolor=facecolor, figsize=figsize) self.ax = self._plt.axes() self.fig.add_axes(self.ax) else: self.ax = ax self.fig = ax.get_figure() if show is None: show = (matplotlib.get_backend().lower() != 'template') self._show = show self.ax.axis('equal') self.ax.axis('off') if not show: # This is the preferred way to support the jupyter notebook # animation workflow and the `inline` backend grabbing an # extraneous render of the figure. self._plt.close(self.fig) self._is_recording = False self._recorded_contexts = [] def on_initialize(context, event): if self._show: self.fig.show() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def __init__(self, robot, lower_limit=-10., upper_limit=10., resolution=0.01, length=200, update_period_sec=0.005): """" Args: robot: A MultibodyPlant. lower_limit: A scalar or vector of length robot.num_positions(). The lower limit of the slider will be the maximum value of this number and any limit specified in the Joint. upper_limit: A scalar or vector of length robot.num_positions(). The upper limit of the slider will be the minimum value of this number and any limit specified in the Joint. resolution: A scalar or vector of length robot.num_positions() that specifies the discretization of the slider. length: The length of the sliders. update_period_sec: Specifies how often the window update() method gets called. """ VectorSystem.__init__(self, 0, robot.num_positions()) # The widgets themselves have undeclared state. For now, we accept it, # and simply disable caching on the output port. self.get_output_port(0).disable_caching_by_default() def _reshape(x, num): x = np.array(x) assert len(x.shape) <= 1 return np.array(x) * np.ones(num) lower_limit = _reshape(lower_limit, robot.num_positions()) upper_limit = _reshape(upper_limit, robot.num_positions()) resolution = _reshape(resolution, robot.num_positions()) # Schedule window updates in either case (new or existing window): self.DeclarePeriodicEvent(update_period_sec, 0.0, PublishEvent(self._process_event_queue)) self._slider = [] self._slider_position_start = [] context = robot.CreateDefaultContext() self._default_position = robot.GetPositions(context) k = 0 for i in range(0, robot.num_joints()): joint = robot.get_joint(JointIndex(i)) low = joint.position_lower_limits() upp = joint.position_upper_limits() for j in range(0, joint.num_positions()): index = joint.position_start() + j description = joint.name() if joint.num_positions() > 1: description += f"[{j}]" self._slider_position_start.append(index) self._slider.append( FloatSlider(value=self._default_position[index], min=max(low[j], lower_limit[k]), max=min(upp[j], upper_limit[k]), step=resolution[k], continuous_update=True, description=description, style={'description_width': 'initial'}, layout=Layout(width=f"'{length}'"))) display(self._slider[k]) k += 1
def __init__(self, planar=False): """ @param planar if True, restricts the GUI and the output to have y=0, roll=0, yaw=0. """ LeafSystem.__init__(self) self.DeclareVectorOutputPort("rpy_xyz", 6, self.DoCalcOutput) # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self.DeclarePeriodicEvent(0.01, 0.0, PublishEvent(self._update_window)) self.planar = planar 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 (keys: ctrl-right, ctrl-left)", length=800, orient=tk.HORIZONTAL) self.roll.pack() self.roll.set(0) self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="pitch (keys: ctrl-d, ctrl-a)", length=800, orient=tk.HORIZONTAL) if not planar: self.pitch.pack() self.pitch.set(0) self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi, resolution=-1, label="yaw (keys: ctrl-up, ctrl-down)", length=800, orient=tk.HORIZONTAL) if not planar: self.yaw.pack() self.yaw.set(1.57) self.x = tk.Scale(self.window, from_=-0.6, to=0.8, resolution=-1, label="x (keys: right, left)", length=800, orient=tk.HORIZONTAL) self.x.pack() self.x.set(0) self.y = tk.Scale(self.window, from_=-0.8, to=0.3, resolution=-1, label="y (keys: d, a)", length=800, orient=tk.HORIZONTAL) if not planar: self.y.pack() self.y.set(0) self.z = tk.Scale(self.window, from_=0, to=1.1, resolution=-1, label="z (keys: up, down)", length=800, orient=tk.HORIZONTAL) self.z.pack() self.z.set(0) # The key bindings below provide teleop functionality via the # keyboard, and are somewhat arbitrary (inspired by gaming # conventions). Note that in order for the keyboard bindings to # be active, the teleop slider window must be the active window. def update(scale, value): return lambda event: scale.set(scale.get() + value) # Delta displacements for motion via keyboard teleop. rotation_delta = 0.05 # rad position_delta = 0.01 # m # Linear motion key bindings. self.window.bind("<Up>", update(self.z, +position_delta)) self.window.bind("<Down>", update(self.z, -position_delta)) if (not planar): self.window.bind("<d>", update(self.y, +position_delta)) self.window.bind("<a>", update(self.y, -position_delta)) self.window.bind("<Right>", update(self.x, +position_delta)) self.window.bind("<Left>", update(self.x, -position_delta)) # Rotational motion key bindings. self.window.bind("<Control-Right>", update(self.roll, +rotation_delta)) self.window.bind("<Control-Left>", update(self.roll, -rotation_delta)) if (not planar): self.window.bind("<Control-d>", update(self.pitch, +rotation_delta)) self.window.bind("<Control-a>", update(self.pitch, -rotation_delta)) self.window.bind("<Control-Up>", update(self.yaw, +rotation_delta)) self.window.bind("<Control-Down>", update(self.yaw, -rotation_delta))
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) 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, visible=Visible(), min_range=MinRange(), max_range=MaxRange(), value=Value()): """ Args: visible: An object with boolean elements for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for this to be the PoseSliders.Visible() namedtuple. Defaults to all true. min_range, max_range, value: Objects with float values for 'roll', 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the caller to use the PoseSliders.MinRange, MaxRange, and Value namedtuples. See those tuples for default values. """ LeafSystem.__init__(self) port = self.DeclareAbstractOutputPort( "pose", lambda: AbstractValue.Make(RigidTransform()), self.DoCalcOutput) # The widgets themselves have undeclared state. For now, we accept it, # and simply disable caching on the output port. # TODO(russt): consider implementing the more elaborate methods seen # in, e.g., LcmMessageSubscriber. port.disable_caching_by_default() # Note: This timing affects the keyboard teleop performance. A larger # time step causes more lag in the response. self.DeclarePeriodicEvent(0.01, 0.0, PublishEvent(self._process_event_queue)) self._roll = FloatSlider(min=min_range.roll, max=max_range.roll, value=value.roll, step=0.01, continuous_update=True, description="roll", layout=Layout(width='90%')) self._pitch = FloatSlider(min=min_range.pitch, max=max_range.pitch, value=value.pitch, step=0.01, continuous_update=True, description="pitch", layout=Layout(width='90%')) self._yaw = FloatSlider(min=min_range.yaw, max=max_range.yaw, value=value.yaw, step=0.01, continuous_update=True, description="yaw", layout=Layout(width='90%')) self._x = FloatSlider(min=min_range.x, max=max_range.x, value=value.x, step=0.01, continuous_update=True, description="x", orient='horizontal', layout=Layout(width='90%')) self._y = FloatSlider(min=min_range.y, max=max_range.y, value=value.y, step=0.01, continuous_update=True, description="y", layout=Layout(width='90%')) self._z = FloatSlider(min=min_range.z, max=max_range.z, value=value.z, step=0.01, continuous_update=True, description="z", layout=Layout(width='90%')) if visible.roll: display(self._roll) if visible.pitch: display(self._pitch) if visible.yaw: display(self._yaw) if visible.x: display(self._x) if visible.y: display(self._y) if visible.z: display(self._z)
def __init__(self, scene_graph, draw_period=0.033333, prefix="drake", zmq_url="default", open_browser=None): """ Args: scene_graph: A SceneGraph object. 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. Note: This call will not return until it connects to the meshcat-server. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self._DeclarePeriodicPublish(draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self._DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) 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) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self._DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def __init__(self, scene_graph, zmq_url="default", draw_period=0.033333, camera_tfs=[RigidTransform()], material_overrides=[], global_transform=RigidTransform(), env_map_path=None, out_prefix=None, show_figure=False, role=Role.kIllustration, save_scene=False): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes rendered images. zmq_url: Optionally set a url to connect to the blender server. Use zmp_url="default" to the value obtained by running a single blender server in another terminal. TODO(gizatt): 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:5556" to specify a specific address. camera tfs: List of RigidTransform camera tfs. material_overrides: A list of tuples of regex rules and material override arguments to be passed into a a register material call, not including names. (Those are assigned automatically by this class.) global transform: RigidTransform that gets premultiplied to every object. Note: This call will not return until it connects to the Blender server. """ LeafSystem.__init__(self) if out_prefix is not None: self.out_prefix = out_prefix else: self.out_prefix = "/tmp/drake_blender_vis_" self.current_publish_num = 0 self.set_name('blender_color_camera') self.DeclarePeriodicPublish(draw_period, 0.) self.draw_period = draw_period self.save_scene = save_scene # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) # Optional pose bundle of bounding boxes. self.DeclareAbstractInputPort("bounding_box_bundle", AbstractValue.Make(BoundingBoxBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:5556" elif zmq_url == "new": raise NotImplementedError("TODO") if zmq_url is not None: print("Connecting to blender server at zmq_url=" + zmq_url + "...") self.bsi = BlenderServerInterface(zmq_url=zmq_url) print("Connected to Blender server.") self._scene_graph = scene_graph self._role = role self.env_map_path = env_map_path # Compile regex for the material overrides self.material_overrides = [(re.compile(x[0]), x[1]) for x in material_overrides] self.global_transform = global_transform self.camera_tfs = camera_tfs def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) self.show_figure = show_figure if self.show_figure: plt.figure()
def __init__(self, port_size, slider_names=None, lower_limit=-10., upper_limit=10., resolution=-1, length=200, update_period_sec=0.0166, window=None, title="System inputs"): """ Args: port_size: Size of the input port that's being controlled. This is the number of sliders that will show up. slider_names: A list of strings describing the names of the sliders that should be displayed. lower_limit: The value(s) for the lower limits of each slider. See class documentation for more details. upper_limit: The value(s) for the upper limits of each slider. See class documentation for more details. resolution: A scalar or vector of length port_size that specifies the discretization that the slider will be rounded to. Use -1 (the default) to disable any rounding. For example, a resolution of 0.1 will round to the nearest 0.1. See class documentation for more details. length: The length of the sliders in pixels. update_period_sec: Specifies how often the window update() method gets called. Smaller values will theoretically make GUI values available to the simulation more quickly, but may require the simulation to take more steps than necessary. The default value is suitable for most applications. window: Optionally pass in a tkinter.Tk() object to add these widgets to. Default behavior is to create a new window. title: The string that appears as the title of the gui window. Default title is "System sliders" This parameter is only used if window is None. """ VectorSystem.__init__(self, 0, port_size) if window is None: self.window = tk.Tk() self.window.title(title) else: self.window = window self.port_size = port_size if slider_names is None: slider_names = ["Index " + str(i) for i in range(self.port_size)] if len(slider_names) != self.port_size: raise ValueError( f"Slider names size ({len(slider_names)}) doesn't " f"match port size ({self.port_size})") def input_to_vector(x, desc): """ Turn scalar inputs into vector of size self.port_size. Throws error if vector input is the wrong size, otherwise returning the vector. Args: x: scalar or vector input. desc: string describing the vector, used in error message. """ if np.isscalar(x): return np.repeat(x, self.port_size) if len(x) == self.port_size: return x raise ValueError( f"Size of {desc} ({len(x)}) doesn't " f"match port size ({self.port_size})" ) lower_limit = input_to_vector(lower_limit, "lower_limit") upper_limit = input_to_vector(upper_limit, "upper_limit") resolution = input_to_vector(resolution, "resolution") # Schedule window updates in either case (new or existing window): self.DeclarePeriodicEvent(update_period_sec, 0.0, PublishEvent(self._update_window)) self._sliders = [] # TODO: support a scroll bar for larger input sizes for i in range(self.port_size): slider = tk.Scale(self.window, from_=lower_limit[i], to=upper_limit[i], resolution=resolution[i], label=slider_names[i], length=length, orient=tk.HORIZONTAL) slider.pack() self._sliders.append(slider)
def __init__(self, scene_graph, 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, **kwargs): """ Args: scene_graph: A SceneGraph object. 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 dictionary describing which body frames to draw. It is keyed on the names of model instances, and the values are sets of the names of the bodies whose body frames are shown. For example, if we want to draw the frames of body "A" and "B" in model instance "1", then frames_to_draw is {"1": {"A", "B"}}. 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. Additional kwargs will be passed to the MeshcatVisualizer constructor. Note: This call will not return until it connects to the ``meshcat-server``. """ LeafSystem.__init__(self) 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 # Recording. self._is_recording = False self.reset_recording() # Pose bundle (from SceneGraph) input port. # TODO(tehbelinda): Rename the `lcm_visualization` port to match # SceneGraph once its output port has been updated. See #12214. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) 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.") self._scene_graph = scene_graph # Set background color (to match drake-visualizer). self.vis['/Background'].set_property("top_color", [242, 242, 255]) self.vis['/Background'].set_property("bottom_color", [77, 77, 89]) if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) # drawing coordinate frames self.frames_to_draw = frames_to_draw self.frames_opacity = frames_opacity self.axis_length = axis_length self.axis_radius = axis_radius
def __init__(self, scene_graph, zmq_url="default", draw_period=0.033333, camera_tfs=[RigidTransform()], global_transform=RigidTransform(), out_prefix=None, show_figure=False, save_scene=False): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes rendered images. zmq_url: Optionally set a url to connect to the blender server. Use zmp_url="default" to the value obtained by running a single blender server in another terminal. TODO(gizatt): 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:5556" to specify a specific address. camera tfs: List of RigidTransform camera tfs. global transform: RigidTransform that gets premultiplied to every object. Note: This call will not return until it connects to the Blender server. TODO(gizatt) Consolidate functionality with BlenderColorCamera into a single BlenderCamera. Needs careful architecting to still support rendering color + label images with different Blender servers in parallel. """ LeafSystem.__init__(self) if out_prefix is not None: self.out_prefix = out_prefix else: self.out_prefix = "/tmp/drake_blender_vis_labels_" self.current_publish_num = 0 self.set_name('blender_label_camera') self.DeclarePeriodicPublish(draw_period, 0.) self.draw_period = draw_period self.save_scene = save_scene # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:5556" elif zmq_url == "new": raise NotImplementedError("TODO") if zmq_url is not None: print("Connecting to blender server at zmq_url=" + zmq_url + "...") self.bsi = BlenderServerInterface(zmq_url=zmq_url) print("Connected to Blender server.") self._scene_graph = scene_graph self.global_transform = global_transform self.camera_tfs = camera_tfs def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) self.show_figure = show_figure if self.show_figure: plt.figure()
def __init__(self, scene_graph, 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): """ Args: scene_graph: A SceneGraph object. 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 dictionary describing which body frames to draw. It is keyed on the names of model instances, and the values are sets of the names of the bodies whose body frames are shown. For example, if we want to draw the frames of body "A" and "B" in model instance "1", then frames_to_draw is {"1": {"A", "B"}}. frames_opacity, axis_length and axis_radius are the opacity, length and radius of the coordinate axes to be drawn. Note: This call will not return until it connects to the ``meshcat-server``. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self.DeclarePeriodicPublish(draw_period, 0.0) self.draw_period = draw_period # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) 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) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) # drawing coordinate frames self.frames_to_draw = frames_to_draw self.frames_opacity = frames_opacity self.axis_length = axis_length self.axis_radius = axis_radius
def __init__(self, robot, lower_limit=-10., upper_limit=10., resolution=-1, length=200, update_period_sec=0.005, window=None, title=None): """" Args: robot: A MultibodyPlant. lower_limit: A scalar or vector of length robot.num_positions(). The lower limit of the slider will be the maximum value of this number and any limit specified in the Joint. upper_limit: A scalar or vector of length robot.num_positions(). The upper limit of the slider will be the minimum value of this number and any limit specified in the Joint. resolution: A scalar or vector of length robot.num_positions() that specifies the discretization of the slider. Use -1 (the default) to disable any rounding. length: The length of the sliders (passed as an argument to tk.Scale). update_period_sec: Specifies how often the window update() method gets called. window: Optionally pass in a tkinter.Tk() object to add these widgets to. Default behavior is to create a new window. title: The string that appears as the title of the gui window. Use None to generate a default title. This parameter is only used if a window==None. """ VectorSystem.__init__(self, 0, robot.num_positions()) def _reshape(x, num): x = np.array(x) assert len(x.shape) <= 1 return np.array(x) * np.ones(num) lower_limit = _reshape(lower_limit, robot.num_positions()) upper_limit = _reshape(upper_limit, robot.num_positions()) resolution = _reshape(resolution, robot.num_positions()) if title is None: if robot.num_model_instances() == 1: title = robot.GetModelInstanceName(0) + " Joints" else: title = "Multibody Joints" if window is None: self.window = tk.Tk() self.window.title(title) else: self.window = window # Schedule window updates in either case (new or existing window): self.DeclarePeriodicEvent(update_period_sec, 0.0, PublishEvent(self._update)) self._slider = [] self._slider_position_start = [] context = robot.CreateDefaultContext() state = robot.GetPositionsAndVelocities(context) self._default_position = state[:robot.num_positions()] k = 0 for i in range(0, robot.num_joints()): joint = robot.get_joint(JointIndex(i)) low = joint.position_lower_limits() upp = joint.position_upper_limits() for j in range(0, joint.num_positions()): self._slider_position_start.append(joint.position_start() + j) self._slider.append(tk.Scale(self.window, from_=max(low[j], lower_limit[k]), to=min(upp[j], upper_limit[k]), resolution=resolution[k], label=joint.name(), length=length, orient=tk.HORIZONTAL)) self._slider[k].pack() k += 1