def testConnectMeschatVisualizer(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
visualizer = ConnectMeshcatVisualizer(builder=builder,
scene_graph=scene_graph,
zmq_url=ZMQ_URL,
open_browser=False)
self.assertIsInstance(visualizer, MeshcatVisualizer)
vis2 = ConnectMeshcatVisualizer(
builder=builder,
scene_graph=scene_graph,
output_port=scene_graph.get_pose_bundle_output_port(),
zmq_url=ZMQ_URL,
open_browser=False)
vis2.set_name("vis2")
self.assertIsInstance(vis2, MeshcatVisualizer)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
def connect_to_meshcat(self):
self._meshcat = ConnectMeshcatVisualizer(
self._builder,
scene_graph=self._sg,
zmq_url="tcp://127.0.0.1:6000",
draw_period=1)
return self._meshcat
def connect_visualizers(builder, plant, scene_graph):
# Connect this to drake_visualizer.
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
# Connect to Meshcat.
if args.meshcat is not None:
meshcat_viz = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=args.meshcat)
# Connect to PyPlot.
if args.pyplot:
pyplot = ConnectPlanarSceneGraphVisualizer(builder, scene_graph)
def connect_visualizers(builder, plant, scene_graph):
# Connect this to drake_visualizer.
DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph)
# Connect to Meshcat.
if args.meshcat is not None:
meshcat_viz = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=args.meshcat)
# Connect to PyPlot.
if args.pyplot:
pyplot = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
pyplot.get_input_port(0))
def test_connect_meshcat_visualizer_deprecated_api(self):
"""Cart-Pole with simple geometry. This should produce two cart-poles,
with identical geometry, one under the prefix `vis` and another under
the prefix `vis2`. You should confirm in the visualizer that both
exist (by unchecking one at a time in the MeshCat controls menu)"""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.Finalize()
vis = ConnectMeshcatVisualizer(builder=builder,
prefix="vis",
scene_graph=scene_graph,
zmq_url=ZMQ_URL,
open_browser=False)
self.assertIsInstance(vis, MeshcatVisualizer)
vis2 = ConnectMeshcatVisualizer(
builder=builder,
prefix="vis2",
scene_graph=scene_graph,
output_port=scene_graph.get_pose_bundle_output_port(),
zmq_url=ZMQ_URL,
open_browser=False)
vis2.set_name("vis2")
self.assertIsInstance(vis2, MeshcatVisualizer)
vis.load()
vis2.load()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
with catch_drake_warnings(expected_count=1):
diagram.Publish(context)
return LinearQuadraticRegulator(
diagram,
context,
Q,
R,
input_port_index=diagram.get_input_port().get_index())
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
cartpole = Parser(plant).AddModelFromFile("cart_pole.sdf")
plant.Finalize()
# Setup visualization
visualizer = ConnectMeshcatVisualizer(builder,
scene_graph=scene_graph,
zmq_url="new")
visualizer.vis.delete()
visualizer.set_planar_viewpoint(xmin=-2.5, xmax=2.5, ymin=-1.0, ymax=2.5)
builder.ExportInput(plant.get_actuation_input_port(), "command")
diagram = builder.Build()
# problem block 1: trying to do LQR with the diagram
# returns an error about the diagram not supporting toAutoDiffXd
# same issue when trying to call diagram.toAutoDiffXd() directly
"""
controller = builder.AddSystem(BalancingLQR(diagram))
builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
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'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
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)
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.
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
elif args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
else:
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
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=image_array_t,
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))
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(args.setup == 'planar'))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
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(teleop.window))
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(SignalLogger(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:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.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(
"--time_step", type=float, default=0.005,
help="Time constant for the differential IK solver and 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'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
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()
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder, output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat, open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
params.set_timestep(args.time_step)
# True velocity limits for the IIWA14 (in rad/s, 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, args.time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(DualShock4Teleop(initialize_joystick()))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.time_step, 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)
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v1 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_pose_bundle_output_port(), v1.get_input_port(0))
v1.set_name("v1")
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v3 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_pose_bundle_output_port(), v3.get_input_port(0))
v3.set_name("v3")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v6 = ConnectMeshcatVisualizer(
builder,
scene_graph=sg,
output_port=sg.get_pose_bundle_output_port())
v6.set_name("v6")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v9 = ConnectMeshcatVisualizer(
builder,
scene_graph=None,
output_port=sg.get_pose_bundle_output_port())
v9.set_name("v9")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use pose_bundle API, give warning
v1.load(v1.GetMyContextFromRoot(context))
with catch_drake_warnings(expected_count=1):
v1.Publish(v1.GetMyContextFromRoot(context))
v1._warned_pose_bundle_input_port_connected = False
v1.load()
with catch_drake_warnings(expected_count=1):
v1.Publish(v1.GetMyContextFromRoot(context))
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Can't use pose_bundle API without passing a scene graph.
with self.assertRaises(RuntimeError):
v3.load(v3.GetMyContextFromRoot(context))
with self.assertRaises(RuntimeError):
v3.load()
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use pose_bundle API, give warning
v6.load(v6.GetMyContextFromRoot(context))
with catch_drake_warnings(expected_count=1):
v6.Publish(v6.GetMyContextFromRoot(context))
v6._warned_pose_bundle_input_port_connected = False
v6.load()
with catch_drake_warnings(expected_count=1):
v6.Publish(v6.GetMyContextFromRoot(context))
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Can't use pose_bundle API without passing a scene graph.
with self.assertRaises(RuntimeError):
v9.load(v9.GetMyContextFromRoot(context))
with self.assertRaises(RuntimeError):
v9.load()
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
def _connect_visualizer(self):
DrakeVisualizer.AddToBuilder(self.builder, self.scenegraph)
self.meshcat = ConnectMeshcatVisualizer(self.builder, self.scenegraph, zmq_url="new", open_browser=False)
self.meshcat.vis.jupyter_cell()
class Visualizer():
def __init__(self, urdf):
self._create_plant(urdf)
def _create_plant(self, urdf):
self.plant = MultibodyPlant(time_step=0.0)
self.scenegraph = SceneGraph()
self.plant.RegisterAsSourceForSceneGraph(self.scenegraph)
self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf))
self.builder = DiagramBuilder()
self.builder.AddSystem(self.scenegraph)
def _finalize_plant(self):
self.plant.Finalize()
def _add_trajectory_source(self, traj):
# Create the trajectory source
source = self.builder.AddSystem(TrajectorySource(traj))
pos2pose = self.builder.AddSystem(MultibodyPositionToGeometryPose(self.plant, input_multibody_state=True))
# Wire the source to the scene graph
self.builder.Connect(source.get_output_port(0), pos2pose.get_input_port())
self.builder.Connect(pos2pose.get_output_port(), self.scenegraph.get_source_pose_port(self.plant.get_source_id()))
def _add_renderer(self):
renderer_name = "renderer"
self.scenegraph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams()))
# Add a camera
depth_camera = DepthRenderCamera(
RenderCameraCore(
renderer_name,
CameraInfo(width=640, height=480, fov_y=np.pi/4),
ClippingRange(0.01, 10.0),
RigidTransform()),
DepthRange(0.01,10.0)
)
world_id = self.plant.GetBodyFrameIdOrThrow(self.plant.world_body().index())
X_WB = xyz_rpy_deg([4,0,0],[-90,0,90])
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
self.builder.AddSystem(sensor)
self.builder.Connect(self.scenegraph.get_query_output_port(), sensor.query_object_input_port())
def _connect_visualizer(self):
DrakeVisualizer.AddToBuilder(self.builder, self.scenegraph)
self.meshcat = ConnectMeshcatVisualizer(self.builder, self.scenegraph, zmq_url="new", open_browser=False)
self.meshcat.vis.jupyter_cell()
def _make_visualization(self, stop_time):
simulator = Simulator(self.builder.Build())
simulator.Initialize()
self.meshcat.vis.render_static()
# Set simulator context
simulator.get_mutable_context().SetTime(0.0)
# Start recording and simulate the diagram
self.meshcat.reset_recording()
self.meshcat.start_recording()
simulator.AdvanceTo(stop_time)
# Publish the recording
self.meshcat.publish_recording()
# Render
self.meshcat.vis.render_static()
input("View visualization. Press <ENTER> to end")
print("Finished")
def visualize_trajectory(self, xtraj=None):
self._finalize_plant()
print("Adding trajectory source")
xtraj = self._check_trajectory(xtraj)
self._add_trajectory_source(xtraj)
print("Adding renderer")
self._add_renderer()
print("Connecting to MeshCat")
self._connect_visualizer()
print("Making visualization")
self._make_visualization(xtraj.end_time())
def _check_trajectory(self, traj):
if traj is None:
plant_context = self.plant.CreateDefaultContext()
pose = self.plant.GetPositions(plant_context)
pose = np.column_stack((pose, pose))
pose = zero_pad_rows(pose, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold([0., 1.], pose)
elif type(traj) is np.ndarray:
if traj.ndim == 1:
traj = np.column_stack(traj, traj)
traj = zero_pad_rows(traj, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold([0.,1.], traj)
elif type(traj) is PiecewisePolynomial:
breaks = traj.get_segment_times()
values = traj.vector_values(breaks)
values = zero_pad_rows(values, self.plant.num_positions() + self.plant.num_velocities())
return PiecewisePolynomial.FirstOrderHold(breaks, values)
else:
raise ValueError("Trajectory must be a piecewise polynomial, an ndarray, or None")
RenderCameraCore(renderer_name,
CameraInfo(width=640, height=480, fov_y=np.pi / 4),
ClippingRange(0.01, 10.0), RigidTransform()),
DepthRange(0.01, 10.0))
world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index())
X_WB = xyz_rpy_deg([4, 0, 0], [-90, 0, 90])
sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera)
builder.AddSystem(sensor)
builder.Connect(scene_graph.get_query_output_port(),
sensor.query_object_input_port())
DrakeVisualizer.AddToBuilder(builder, scene_graph)
meshcat_vis = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url="new",
open_browser=True)
plant.Finalize()
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyMutableContextFromRoot(diagram_context)
sg_context = scene_graph.GetMyMutableContextFromRoot(diagram_context)
simulator = Simulator(diagram)
simulator.Initialize()
color = sensor.color_image_output_port().Eval(sensor_context).data
label = sensor.label_image_output_port().Eval(sensor_context).data
fig, ax = plt.subplots(1, 2, figsize=(10, 5))
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'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
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)
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(builder,
output_port=geometry_query_port,
zmq_url=args.meshcat,
open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
if args.setup == 'planar':
pyplot_visualizer = ConnectPlanarSceneGraphVisualizer(
builder, station.get_scene_graph(), geometry_query_port)
teleop = builder.AddSystem(
JointSliders(station.get_controller_plant(), length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
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(teleop.window))
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)
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
# 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.set_position(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:
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)
# simulator.AdvanceTo(5.0)
builder = DiagramBuilder()
# Adds both MultibodyPlant and SceneGraph and wires them together
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Adds the MeshcatVisualizer and wires it to the Scene Graph
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, open_browser=True)
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
meshcat.load()
diagram.Publish(context)
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(5)
