def connect_visualizers(builder, plant, scene_graph):
# Connect this to drake_visualizer.
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
# Connect to Meshcat. If the consuming application needs to connect,
# e.g., JointSliders, the meshcat instance is required.
meshcat = None
if args.meshcat:
meshcat = Meshcat()
meshcat_vis_params = MeshcatVisualizerParams()
meshcat_vis_params.role = args.meshcat_role
MeshcatVisualizerCpp.AddToBuilder(builder=builder,
scene_graph=scene_graph,
meshcat=meshcat,
params=meshcat_vis_params)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
else:
if args.browser_new is not None:
args_parser.error("-w / --open-window require --meshcat")
# Connect to PyPlot.
if args.pyplot:
pyplot = ConnectPlanarSceneGraphVisualizer(builder, scene_graph)
return meshcat
def __init__(self, *, meshcat_port=None):
# Bookkeeping for update throtting.
self._last_update_time = time.time()
# Bookkeeping for subscriptions, keyed by LCM channel name.
self._message_types = {}
self._message_handlers = {}
self._message_pending_data = {}
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
_logger.info(f"Meldis is listening for LCM messages at {lcm_url}")
params = MeshcatParams(host="localhost", port=meshcat_port)
self.meshcat = Meshcat(params=params)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
# Bookkeeping for automatic shutdown.
self._last_poll = None
self._last_active = None
def connect_visualizers(builder, plant, scene_graph,
publish_contacts=True):
# Connect this to drake_visualizer or meldis. Meldis provides
# simultaneous visualization of illustration and proximity geometry.
ApplyVisualizationConfig(
config=VisualizationConfig(publish_contacts=publish_contacts),
plant=plant,
scene_graph=scene_graph,
builder=builder)
# Connect to Meshcat. If the consuming application needs to connect,
# e.g., JointSliders, the meshcat instance is required.
meshcat = None
if args.meshcat:
meshcat = Meshcat()
meshcat_vis_params = MeshcatVisualizerParams()
meshcat_vis_params.role = args.meshcat_role
MeshcatVisualizer.AddToBuilder(
builder=builder, scene_graph=scene_graph, meshcat=meshcat,
params=meshcat_vis_params)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
else:
if args.browser_new is not None:
args_parser.error("-w / --open-window require --meshcat")
# Connect to PyPlot.
if args.pyplot:
pyplot = ConnectPlanarSceneGraphVisualizer(builder, scene_graph)
return meshcat
def test_contact_visualizer(self, T):
meshcat = Meshcat()
params = ContactVisualizerParams()
params.publish_period = 0.123
params.default_color = Rgba(0.5, 0.5, 0.5)
params.prefix = "py_visualizer"
params.delete_on_initialization_event = False
params.force_threshold = 0.2
params.newtons_per_meter = 5
params.radius = 0.1
self.assertNotIn("object at 0x", repr(params))
params2 = ContactVisualizerParams(publish_period=0.4)
self.assertEqual(params2.publish_period, 0.4)
vis = ContactVisualizer_[T](meshcat=meshcat, params=params)
vis.Delete()
vis.contact_results_input_port()
builder = DiagramBuilder_[T]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01)
plant.Finalize()
ContactVisualizer_[T].AddToBuilder(builder=builder,
plant=plant,
meshcat=meshcat,
params=params)
ContactVisualizer_[T].AddToBuilder(
builder=builder,
contact_results_port=plant.get_contact_results_output_port(),
meshcat=meshcat,
params=params)
def test_internal_point_contact_visualizer(self):
"""A very basic existance test, since this class is internal use only.
The pydrake-internal user (meldis) has additional acceptance tests.
"""
meshcat = Meshcat()
params = ContactVisualizerParams()
dut = _PointContactVisualizer(meshcat=meshcat, params=params)
def test_joint_sliders(self):
# Load up an acrobot.
acrobot_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.urdf")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
parser.AddModelFromFile(acrobot_file)
plant.Finalize()
# Construct a sliders system, using every available option.
meshcat = Meshcat()
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=[-3.0, -6.0],
upper_limit=[3.0, 6.0],
step=[0.25, 0.50],
)
# Various methods should not crash.
dut.get_output_port()
dut.Delete()
# The constructor also accepts single values for broadcast (except for
# the initial value).
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=[0.1, 0.2],
lower_limit=-3.0,
upper_limit=3.0,
step=0.1,
)
dut.Delete()
# The constructor also accepts None directly, for optionals.
dut = JointSliders(
meshcat=meshcat,
plant=plant,
initial_value=None,
lower_limit=None,
upper_limit=None,
step=None,
)
dut.Delete()
# The constructor has default values, in any case.
dut = JointSliders(meshcat, plant)
# The Run function doesn't crash.
builder.AddSystem(dut)
diagram = builder.Build()
dut.Run(diagram=diagram, timeout=1.0)
# The SetPositions function doesn't crash (Acrobot has two positions).
dut.SetPositions(q=[1, 2])
def __init__(self, *, meshcat_port=None):
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
logging.info(f"Meldis is listening for LCM messages at {lcm_url}")
self.meshcat = Meshcat(port=meshcat_port)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
def StartMeshcat(open_window=False):
"""
A wrapper around the Meshcat constructor that supports Deepnote and Google
Colab via ngrok when necessary.
"""
prev_log_level = set_log_level("warn")
use_ngrok = False
if ("DEEPNOTE_PROJECT_ID" in os.environ):
# Deepnote exposes port 8080 (only). If we need multiple meshcats,
# then we fall back to ngrok.
try:
meshcat = Meshcat(8080)
except RuntimeError:
use_ngrok = True
else:
set_log_level(prev_log_level)
web_url = f"https://{os.environ['DEEPNOTE_PROJECT_ID']}.deepnoteproject.com" # noqa
print(f"Meshcat is now available at {web_url}")
if open_window:
display(Javascript(f'window.open("{web_url}");'))
return meshcat
if 'google.colab' in sys.modules:
use_ngrok = True
meshcat = Meshcat()
web_url = meshcat.web_url()
if use_ngrok:
from pyngrok import ngrok
http_tunnel = ngrok.connect(meshcat.port(), bind_tls=False)
web_url = http_tunnel.public_url
set_log_level(prev_log_level)
display(
HTML('Meshcat is now available at '
f'<a href="{web_url}">{web_url}</a>'))
if open_window:
if 'google.colab' in sys.modules:
from google.colab import output
output.eval_js(f'window.open("{web_url}");', ignore_result=True)
else:
display(Javascript(f'window.open("{web_url}");'))
return meshcat
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
'--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
"-w",
"--open-window",
dest="browser_new",
action="store_const",
const=1,
default=None,
help="Open the MeshCat display in a new browser window.")
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (joint sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a
# config file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/" +
"061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
geometry_query_port = station.GetOutputPort("geometry_query")
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
if args.setup == 'planar':
meshcat.Set2dRenderMode()
pyplot_visualizer = ConnectPlanarSceneGraphVisualizer(
builder, station.get_scene_graph(), geometry_query_port)
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
teleop = builder.AddSystem(
JointSliders(meshcat=meshcat, plant=station.get_controller_plant()))
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
if args.test:
iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
teleop.SetPositions(q0)
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const",
type=float,
default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
parser.add_argument(
'--schunk_collision_model',
type=str,
default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
parser.add_argument("--meshcat",
action="store_true",
default=False,
help="Enable visualization with meshcat.")
parser.add_argument(
"-w",
"--open-window",
dest="browser_new",
action="store_const",
const=1,
default=None,
help="Open the MeshCat display in a new browser window.")
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(schunk_model=schunk_model)
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
query_port = station.GetOutputPort("query_object")
DrakeVisualizer.AddToBuilder(builder, query_port)
if args.meshcat:
meshcat = Meshcat()
meshcat_visualizer = MeshcatVisualizer.AddToBuilder(
builder=builder, query_object_port=query_port, meshcat=meshcat)
if args.setup == 'planar':
meshcat.Set2dRenderMode()
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
class Meldis:
"""
MeshCat LCM Display Server (MeLDiS)
Offers a MeshCat vizualization server that listens for and draws Drake's
legacy LCM vizualization messages.
Refer to the pydrake.visualization.meldis module docs for details.
"""
def __init__(self, *, meshcat_port=None):
# Bookkeeping for update throtting.
self._last_update_time = time.time()
# Bookkeeping for subscriptions, keyed by LCM channel name.
self._message_types = {}
self._message_handlers = {}
self._message_pending_data = {}
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
_logger.info(f"Meldis is listening for LCM messages at {lcm_url}")
params = MeshcatParams(host="localhost", port=meshcat_port)
self.meshcat = Meshcat(params=params)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
# Bookkeeping for automatic shutdown.
self._last_poll = None
self._last_active = None
def _subscribe(self, channel, message_type, handler):
"""Subscribes the handler to the given channel, using message_type to
pass in a decoded message object (not the raw bytes). The handler will
only be called at some maximum frequency. Messages on the same channel
that arrive too quickly will be discarded.
"""
# Record this channel's type and handler.
assert self._message_types.get(channel, message_type) == message_type
self._message_types[channel] = message_type
self._message_handlers.setdefault(channel, []).append(handler)
# Subscribe using an internal function that implements "last one wins".
# It's important to service the LCM queue as frequently as possible:
# https://github.com/RobotLocomotion/drake/issues/15234
# https://github.com/lcm-proj/lcm/issues/345
# However, if the sender is transmitting visualization messages at
# a high rate (e.g., if a sim is running much faster than realtime),
# then we should only pass some of them along to MeshCat to avoid
# flooding it. The hander merely records the message data; we'll
# pass it along to MeshCat using our `self._should_update()` timer.
def _on_message(data):
self._message_pending_data[channel] = data
self._lcm.Subscribe(channel=channel, handler=_on_message)
def _invoke_subscriptions(self):
"""Posts any unhandled messages to their handlers and clears the
collection of unhandled messages.
"""
for channel, data in self._message_pending_data.items():
message = self._message_types[channel].decode(data)
for function in self._message_handlers[channel]:
function(message=message)
self._message_pending_data.clear()
def serve_forever(self, *, idle_timeout=None):
"""Run indefinitely, forwarding LCM => MeshCat messages.
If provided, the optional idle_timeout must be strictly positive and
this loop will sys.exit after that many seconds without any websocket
connections.
"""
while True:
self._lcm.HandleSubscriptions(timeout_millis=1000)
if not self._should_update():
continue
self._invoke_subscriptions()
self.meshcat.Flush()
self._check_for_shutdown(idle_timeout=idle_timeout)
def _should_update(self):
"""Post LCM-driven updates to MeshCat no faster than 40 Hz."""
now = time.time()
update_period = 0.025 # 40 Hz
remaining = update_period - (now - self._last_update_time)
if remaining > 0.0:
return False
else:
self._last_update_time = now
return True
def _check_for_shutdown(self, *, idle_timeout):
# Allow the user to opt-out of the timeout feature.
if idle_timeout is None:
return
assert idle_timeout > 0.0
# One-time initialization.
now = time.time()
if self._last_active is None:
self._last_active = now
return
# Only check once every 5 seconds.
if (self._last_poll is not None) and (now < self._last_poll + 5.0):
return
self._last_poll = now
# Check to see if any browser client(s) are connected.
if self.meshcat.GetNumActiveConnections() > 0:
self._last_active = now
return
# In case we are idle for too long, exit automatically.
if now > self._last_active + idle_timeout:
_logger.info("Meldis is exiting now; no browser was connected for"
f" >{idle_timeout} seconds")
sys.exit(1)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const", type=float, default=0.1,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
parser.add_argument(
"-w", "--open-window", dest="browser_new",
action="store_const", const=1, default=None,
help=(
"Open the MeshCat display in a new browser window. NOTE: the "
"slider controls are available in the meshcat viewer by clicking "
"'Open Controls' in the top-right corner."))
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (orientation sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
# If using meshcat, don't render the cameras, since RgbdCamera
# rendering only works with drake-visualizer. Without this check,
# running this code in a docker container produces libGL errors.
geometry_query_port = station.GetOutputPort("geometry_query")
# Connect the meshcat visualizer.
meshcat_visualizer = MeshcatVisualizer.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
# Configure the planar visualization.
if args.setup == 'planar':
meshcat.Set2dRenderMode()
# Connect and publish to drake visualizer.
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
image_to_lcm_image_array = builder.AddSystem(
ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
for name in station.get_camera_names():
cam_port = (
image_to_lcm_image_array
.DeclareImageInputPort[PixelType.kRgba8U](
"camera_" + name))
builder.Connect(
station.GetOutputPort("camera_" + name + "_rgb_image"),
cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=lcmt_image_array,
lcm=None,
publish_period=0.1,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(
image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.get_input_port(0))
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
if args.setup == 'planar':
# Extract the 3 joints that are not welded in the planar version.
iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2]
# The below constant is in body frame.
params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(EndEffectorTeleop(
meshcat, args.setup == 'planar'))
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
num_iiwa_joints = station.num_iiwa_joints()
if args.test:
iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
def __init__(self, *, meshcat_host=None, meshcat_port=None):
# Bookkeeping for update throtting.
self._last_update_time = time.time()
# Bookkeeping for subscriptions, keyed by LCM channel name.
self._message_types = {}
self._message_handlers = {}
self._message_pending_data = {}
self._poll_handlers = []
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
_logger.info(f"Meldis is listening for LCM messages at {lcm_url}")
params = MeshcatParams(host=meshcat_host or "localhost",
port=meshcat_port,
show_stats_plot=False)
self.meshcat = Meshcat(params=params)
default_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/DRAKE_VIEWER",
alpha_slider_name="Viewer α")
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=default_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=default_viewer.on_viewer_draw)
self._poll(handler=default_viewer.on_poll)
illustration_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/Visual Geometry",
alpha_slider_name="Visual α")
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT_ILLUSTRATION",
message_type=lcmt_viewer_load_robot,
handler=illustration_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW_ILLUSTRATION",
message_type=lcmt_viewer_draw,
handler=illustration_viewer.on_viewer_draw)
self._poll(handler=illustration_viewer.on_poll)
proximity_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/Collision Geometry",
alpha_slider_name="Collision α",
start_visible=False)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT_PROXIMITY",
message_type=lcmt_viewer_load_robot,
handler=proximity_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW_PROXIMITY",
message_type=lcmt_viewer_draw,
handler=proximity_viewer.on_viewer_draw)
self._poll(handler=proximity_viewer.on_poll)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
# Bookkeeping for automatic shutdown.
self._last_poll = None
self._last_active = None