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)