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
