Port planar_scenegraph_visualizer from underactuated (RobotLocomotion…

…#12218)

Mesh support is removed for now
See RobotLocomotion#11104

Co-authored-by: Russ Tedrake <[email protected]>
Co-authored-by: Eric Cousineau <[email protected]>
Co-authored-by: Greg Izatt <[email protected]>

bindings/pydrake/examples/multibody/BUILD.bazel

@@ -67,9 +67,24 @@ drake_py_binary(
     ],
 )
 
+drake_py_binary(
+    name = "run_planar_scenegraph_visualizer",
+    srcs = ["run_planar_scenegraph_visualizer.py"],
+    add_test_rule = 1,
+    data = ["//examples/pendulum:models"],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":module_py",
+        "//bindings/pydrake/examples:manipulation_station_py",
+        "//bindings/pydrake/multibody",
+        "//bindings/pydrake/systems",
+    ],
+)
+
 PY_LIBRARIES = [
     ":cart_pole_passive_simulation",
     ":pendulum_lqr_monte_carlo_analysis",
+    ":run_planar_scenegraph_visualizer",
 ]
 
 # Package roll-up (for Bazel dependencies).

bindings/pydrake/examples/multibody/run_planar_scenegraph_visualizer.py

@@ -0,0 +1,121 @@
+"""
+Run examples of PlanarSceneGraphVisualizer, e.g. to visualize a pendulum.
+Usage demo: load a URDF, rig it up with a constant torque input, and draw it.
+"""
+
+import argparse
+
+import numpy as np
+
+from pydrake.common import FindResourceOrThrow
+from pydrake.examples.manipulation_station import ManipulationStation
+from pydrake.multibody.parsing import Parser
+from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
+from pydrake.systems.analysis import Simulator
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.systems.planar_scenegraph_visualizer import (
+    PlanarSceneGraphVisualizer)
+
+
+def run_pendulum_example(args):
+    builder = DiagramBuilder()
+    plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
+    parser = Parser(plant)
+    parser.AddModelFromFile(FindResourceOrThrow(
+        "drake/examples/pendulum/Pendulum.urdf"))
+    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"))
+    plant.Finalize()
+
+    pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
+    Tview = np.array([[1., 0., 0., 0.],
+                      [0., 0., 1., 0.],
+                      [0., 0., 0., 1.]],
+                     dtype=np.float64)
+    visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
+        scene_graph, T_VW=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]))
+    builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
+
+    diagram = builder.Build()
+    simulator = Simulator(diagram)
+    simulator.Initialize()
+    simulator.set_target_realtime_rate(1.0)
+
+    # Fix the input port to zero.
+    plant_context = diagram.GetMutableSubsystemContext(
+        plant, simulator.get_mutable_context())
+    plant_context.FixInputPort(
+        plant.get_actuation_input_port().get_index(),
+        np.zeros(plant.num_actuators()))
+    plant_context.SetContinuousState([0.5, 0.1])
+    simulator.AdvanceTo(args.duration)
+
+
+def run_manipulation_example(args):
+    builder = DiagramBuilder()
+    station = builder.AddSystem(ManipulationStation(time_step=0.005))
+    station.SetupManipulationClassStation()
+    station.Finalize()
+
+    plant = station.get_multibody_plant()
+    scene_graph = station.get_scene_graph()
+    pose_bundle_output_port = station.GetOutputPort("pose_bundle")
+
+    # Side-on view of the station.
+    Tview = np.array([[1., 0., 0., 0.],
+                      [0., 0., 1., 0.],
+                      [0., 0., 0., 1.]],
+                     dtype=np.float64)
+    visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
+        scene_graph, T_VW=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2],
+        draw_period=0.1))
+    builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))
+
+    diagram = builder.Build()
+    simulator = Simulator(diagram)
+    simulator.Initialize()
+    simulator.set_target_realtime_rate(1.0)
+
+    # Fix the control inputs to zero.
+    station_context = diagram.GetMutableSubsystemContext(
+        station, simulator.get_mutable_context())
+    station.GetInputPort("iiwa_position").FixValue(
+        station_context, station.GetIiwaPosition(station_context))
+    station.GetInputPort("iiwa_feedforward_torque").FixValue(
+        station_context, np.zeros(7))
+    station.GetInputPort("wsg_position").FixValue(
+        station_context, np.zeros(1))
+    station.GetInputPort("wsg_force_limit").FixValue(
+        station_context, [40.0])
+    simulator.AdvanceTo(args.duration)
+
+
+def main():
+    np.set_printoptions(precision=5, suppress=True)
+    parser = argparse.ArgumentParser(
+        description=__doc__,
+        formatter_class=argparse.RawDescriptionHelpFormatter
+    )
+    parser.add_argument("-T", "--duration",
+                        type=float,
+                        help="Duration to run sim in seconds.",
+                        default=1.0)
+    parser.add_argument("-m", "--models",
+                        type=str,
+                        nargs="*",
+                        help="Models to run, at least one of [pend, manip]",
+                        default=["pend"])
+    args = parser.parse_args()
+
+    for model in args.models:
+        if model == "pend":
+            run_pendulum_example(args)
+        elif model == "manip":
+            run_manipulation_example(args)
+        else:
+            print("Unrecognized model %s." % model)
+            parser.print_usage()
+            exit(1)
+
+
+if __name__ == "__main__":
+    main()

bindings/pydrake/systems/BUILD.bazel

@@ -269,6 +269,31 @@ drake_py_library(
     ],
 )
 
+drake_py_library(
+    name = "planar_scenegraph_visualizer_py",
+    srcs = ["planar_scenegraph_visualizer.py"],
+    imports = PACKAGE_INFO.py_imports,
+    deps = [
+        ":framework_py",
+        ":module_py",
+        ":pyplot_visualizer_py",
+        ":rendering_py",
+        "//bindings/pydrake:lcm_py",
+    ],
+)
+
+drake_py_library(
+    name = "pyplot_visualizer_py",
+    srcs = ["pyplot_visualizer.py"],
+    imports = PACKAGE_INFO.py_imports,
+    deps = [
+        ":framework_py",
+        ":module_py",
+        ":primitives_py",
+        "//bindings/pydrake:trajectories_py",
+    ],
+)
+
 PY_LIBRARIES_WITH_INSTALL = [
     ":analysis_py",
     ":controllers_py",
@@ -286,6 +311,8 @@ PY_LIBRARIES = [
     ":scalar_conversion_py",
     ":module_py",
     ":perception_py",
+    ":planar_scenegraph_visualizer_py",
+    ":pyplot_visualizer_py",
 ]
 
 # Symbol roll-up (for user ease).
@@ -450,6 +477,32 @@ drake_py_unittest(
     ],
 )
 
+drake_py_unittest(
+    name = "planar_scenegraph_visualizer_test",
+    data = [
+        "//examples/multibody/cart_pole:models",
+        "//manipulation/models/iiwa_description:models",
+    ],
+    deps = [
+        ":analysis_py",
+        ":framework_py",
+        ":planar_scenegraph_visualizer_py",
+        "//bindings/pydrake:geometry_py",
+        "//bindings/pydrake/multibody",
+    ],
+)
+
+drake_py_unittest(
+    name = "pyplot_visualizer_test",
+    deps = [
+        ":analysis_py",
+        ":framework_py",
+        ":primitives_py",
+        ":pyplot_visualizer_py",
+        "//bindings/pydrake:trajectories_py",
+    ],
+)
+
 drake_py_unittest(
     name = "rendering_test",
     data = ["//multibody/benchmarks/acrobot:models"],

bindings/pydrake/systems/all.py

@@ -4,7 +4,9 @@
 from .lcm import *
 from .meshcat_visualizer import *
 from .perception import *
+from .planar_scenegraph_visualizer import *
 from .primitives import *
+from .pyplot_visualizer import *
 from .rendering import *
 from .scalar_conversion import *
 from .sensors import *

bindings/pydrake/systems/meshcat_visualizer.py

@@ -243,6 +243,8 @@ def __init__(self,
         self.draw_period = draw_period
 
         # Pose bundle (from SceneGraph) input port.
+        # TODO(tehbelinda): Rename the `lcm_visualization` port to match
+        # SceneGraph once its output port has been updated. See #12214.
         self.DeclareAbstractInputPort("lcm_visualization",
                                       AbstractValue.Make(PoseBundle(0)))