Merge pull request RobotLocomotion#9926 from RussTedrake/geometry_ins…

…pector

Add multibody_position_to_geometry_pose

bindings/pydrake/manipulation/BUILD.bazel

@@ -10,6 +10,7 @@ load(
 )
 load(
     "@drake//tools/skylark:drake_py.bzl",
+    "drake_py_binary",
     "drake_py_library",
     "drake_py_unittest",
 )

bindings/pydrake/manipulation/simple_ui.py

@@ -14,11 +14,13 @@
 class JointSliders(VectorSystem):
     """
     Provides a simple tcl/tk gui with one slider per joint of the
-    MultibodyPlant.
+    MultibodyPlant.  Any positions that are not associated with joints (e.g.
+    floating-base "mobilizers") are held constant at the default value
+    obtained from robot.CreateDefaultContext().
 
     @system{ JointSliders,
              , # no input ports
-             @output_port{joint positions} }
+             @output_port{positions} }
     """
 
     def __init__(self, robot, lower_limit=-10., upper_limit=10.,
@@ -74,12 +76,18 @@ def _reshape(x, num):
         self._DeclarePeriodicPublish(update_period_sec, 0.0)
 
         self._slider = []
+        self._slider_position_start = []
+        context = robot.CreateDefaultContext()
+        state = robot.tree().get_multibody_state_vector(context)
+        self._default_position = state[:robot.num_positions()]
+
         k = 0
         for i in range(0, robot.num_joints()):
             joint = robot.tree().get_joint(JointIndex(i))
             low = joint.lower_limits()
             upp = joint.upper_limits()
             for j in range(0, joint.num_positions()):
+                self._slider_position_start.append(joint.position_start() + j)
                 self._slider.append(tk.Scale(self.window,
                                              from_=max(low[j],
                                                        lower_limit[k]),
@@ -94,14 +102,28 @@ def _reshape(x, num):
         # TODO(russt): Consider resolving constraints in a slider event
         # callback.
 
-    def set(self, q):
+    def set_position(self, q):
+        """
+        Set all robot positions (corresponding to joint positions and
+        potentially positions not associated with any joint) to the
+        values in q.
+
+        Args:
+            q: a vector whose length is robot.num_positions().
+        """
+        self._default_position = q
+        for i in range(len(self._slider)):
+            self._slider[i].set(q[self._slider_position_start[i]])
+
+    def set_joint_position(self, q):
         """
         Set the slider positions to the values in q.
 
         Args:
-            q: a vector of length robot.num_positions()
+            q: a vector whose length is the same as the number of joint
+            positions (also the number of sliders) for the robot.
         """
-        assert(len(q) == len(self._slider))
+        assert(len(q) == len(self._default_position))
         for i in range(len(self._slider)):
             self._slider[i].set(q[i])
 
@@ -110,8 +132,9 @@ def _DoPublish(self, context, event):
         self.window.update()
 
     def _DoCalcVectorOutput(self, context, unused, unused2, output):
+        output[:] = self._default_position
         for i in range(0, len(self._slider)):
-            output[i] = self._slider[i].get()
+            output[self._slider_position_start[i]] = self._slider[i].get()
 
 
 class SchunkWsgButtons(LeafSystem):

bindings/pydrake/manipulation/test/simple_ui_test.py

@@ -30,7 +30,8 @@ def test_joint_slider(self):
         output = slider.AllocateOutput()
 
         q = [.1, .2]
-        slider.set(q)
+        slider.set_position(q)
+        slider.set_joint_position(q)
         slider.CalcOutput(context, output)
 
         np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q)

bindings/pydrake/systems/BUILD.bazel

@@ -361,6 +361,7 @@ drake_py_unittest(
 
 drake_py_unittest(
     name = "rendering_test",
+    data = ["//multibody/benchmarks/acrobot:models"],
     deps = [
         ":rendering_py",
         "//bindings/pydrake/multibody:multibody_tree_py",

bindings/pydrake/systems/rendering_py.cc

@@ -8,6 +8,7 @@
 #include "drake/bindings/pydrake/util/eigen_geometry_pybind.h"
 #include "drake/multibody/multibody_tree/math/spatial_velocity.h"
 #include "drake/systems/rendering/frame_velocity.h"
+#include "drake/systems/rendering/multibody_position_to_geometry_pose.h"
 #include "drake/systems/rendering/pose_aggregator.h"
 #include "drake/systems/rendering/pose_vector.h"
 
@@ -105,6 +106,20 @@ PYBIND11_MODULE(rendering, m) {
     .def("AddBundleInput", &PoseAggregator<T>::AddBundleInput,
          py_reference_internal, doc.PoseAggregator.AddBundleInput.doc);
 
+  py::class_<MultibodyPositionToGeometryPose<T>, LeafSystem<T>>(m,
+      "MultibodyPositionToGeometryPose", doc.MultibodyPositionToGeometryPose
+      .doc)
+      .def(py::init<const multibody::multibody_plant::MultibodyPlant<T>&>(),
+          doc.MultibodyPositionToGeometryPose.ctor.doc_3)
+      .def("get_input_port",
+          &MultibodyPositionToGeometryPose<T>::get_input_port,
+          py_reference_internal,
+          doc.MultibodyPositionToGeometryPose.get_input_port.doc)
+      .def("get_output_port",
+           &MultibodyPositionToGeometryPose<T>::get_output_port,
+           py_reference_internal,
+           doc.MultibodyPositionToGeometryPose.get_output_port.doc);
+
   // TODO(eric.cousineau): Add more systems as needed.
 }
 

bindings/pydrake/systems/test/rendering_test.py

@@ -2,6 +2,7 @@
 
 from pydrake.systems.rendering import (
     FrameVelocity,
+    MultibodyPositionToGeometryPose,
     PoseBundle,
     PoseAggregator,
     PoseVector,
@@ -11,6 +12,10 @@
 import unittest
 import numpy as np
 
+from pydrake.common import FindResourceOrThrow
+from pydrake.geometry import SceneGraph
+from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant
+from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile
 from pydrake.multibody.multibody_tree.math import (
     SpatialVelocity,
 )
@@ -187,3 +192,22 @@ def test_pose_aggregator(self):
         self.assertTrue(np.allclose(vel_actual.translational(), v))
         self.assertTrue(
             (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
+
+    def testMultibodyPositionToGeometryPose(self):
+        file_name = FindResourceOrThrow(
+            "drake/multibody/benchmarks/acrobot/acrobot.sdf")
+        plant = MultibodyPlant(time_step=0.01)
+        model_instance = AddModelFromSdfFile(
+            file_name=file_name, plant=plant)
+        scene_graph = SceneGraph()
+        plant.RegisterAsSourceForSceneGraph(scene_graph)
+        plant.Finalize()
+
+        to_pose = MultibodyPositionToGeometryPose(plant)
+
+        # Check the size of the input.
+        self.assertEqual(to_pose.get_input_port().size(), 2)
+
+        # Just check the spelling of the output port (size is not meaningful
+        # for Abstract-valued ports).
+        to_pose.get_output_port()

examples/manipulation_station/joint_teleop.py

@@ -101,7 +101,7 @@
 # Eval the output port once to read the initial positions of the IIWA.
 q0 = station.GetOutputPort("iiwa_position_measured").Eval(
     station_context).get_value()
-teleop.set(q0)
+teleop.set_position(q0)
 
 # This is important to avoid duplicate publishes to the hardware interface:
 simulator.set_publish_every_time_step(False)

manipulation/util/BUILD.bazel

@@ -54,6 +54,17 @@ drake_cc_library(
     ],
 )
 
+drake_py_binary(
+    name = "geometry_inspector",
+    srcs = ["geometry_inspector.py"],
+    add_test_rule = 1,
+    data = ["//multibody/benchmarks/acrobot:acrobot.sdf"],
+    test_rule_args = ["$(location //multibody/benchmarks/acrobot:acrobot.sdf) --test --position 0.1 0.2"],  # noqa
+    deps = [
+        "//bindings/pydrake",
+    ],
+)
+
 drake_cc_library(
     name = "robot_state_msg_translator",
     srcs = ["robot_state_msg_translator.cc"],

manipulation/util/geometry_inspector.py

@@ -0,0 +1,99 @@
+"""
+Simple tool that parses an SDF file from the command line and runs a simple
+system which takes joint positions from a JointSlider gui and publishes the
+resulting geometry poses to drake_visualizer.
+
+Make sure to `bazel run //tools:drake_visualizer` before executing this to see
+the visualization.
+
+Example usages (each on one line):
+bazel run //bindings/pydrake/multibody:geometry_inspector --
+  $HOME/drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf
+
+bazel run geometry_inspector --
+  $HOME/drake/multibody/benchmarks/acrobot/acrobot.sdf --position 0.1 0.2
+"""
+# TODO(russt): Add support for URDF, too.
+
+import argparse
+import numpy as np
+
+from pydrake.geometry import ConnectDrakeVisualizer, SceneGraph
+from pydrake.manipulation.simple_ui import JointSliders
+from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant
+from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile
+from pydrake.systems.analysis import Simulator
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.systems.rendering import MultibodyPositionToGeometryPose
+
+
+parser = argparse.ArgumentParser()
+parser.add_argument(
+    "filename",
+    type=str,
+    help="Full path to an SDF file.")
+position_group = parser.add_mutually_exclusive_group()
+position_group.add_argument(
+    "--position",
+    type=float,
+    nargs="+",
+    help="A list of positions which must be the same length as the number of "
+         "positions in the sdf model.  Note that most models have a "
+         "floating-base joint by default (unless the sdf explicitly welds the "
+         "base to the world, and so have 7 positions corresponding to the "
+         "quaternion representation of that floating-base position.",
+    default=[])
+position_group.add_argument(
+    "--joint_position",
+    type=float,
+    nargs="+",
+    help="A list of positions which must be the same length as the number of "
+         "positions ASSOCIATED WITH JOINTS in the sdf model.  This does not "
+         "include, e.g., floating-base coordinates, which will be assigned a "
+         "default value.",
+    default=[])
+parser.add_argument(
+    "--test",
+    action='store_true',
+    help="Disable opening the gui window for testing."
+)
+# TODO(russt): Add option to weld the base to the world pending the
+# availability of GetUniqueBaseBody requested in #9747.
+args = parser.parse_args()
+
+builder = DiagramBuilder()
+scene_graph = builder.AddSystem(SceneGraph())
+
+# Construct a MultibodyPlant and load the SDF into it.
+plant = MultibodyPlant()
+plant.RegisterAsSourceForSceneGraph(scene_graph)
+AddModelFromSdfFile(args.filename, plant)
+plant.Finalize(scene_graph)
+
+# Add sliders to set positions of the joints.
+sliders = builder.AddSystem(JointSliders(robot=plant))
+to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
+builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
+builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(
+    plant.get_source_id()))
+
+# Connect this to drake_visualizer.
+ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
+
+if len(args.position):
+    sliders.set_position(args.position)
+elif len(args.joint_position):
+    sliders.set_joint_position(args.joint_position)
+
+# Make the diagram and run it.
+diagram = builder.Build()
+simulator = Simulator(diagram)
+
+simulator.set_publish_every_time_step(False)
+
+if args.test:
+    sliders.window.withdraw()
+    simulator.StepTo(0.1)
+else:
+    simulator.set_target_realtime_rate(1.0)
+    simulator.StepTo(np.inf)

systems/rendering/BUILD.bazel

@@ -15,6 +15,7 @@ drake_cc_package_library(
     deps = [
         ":drake_visualizer_client",
         ":frame_velocity",
+        ":multibody_position_to_geometry_pose",
         ":pose_aggregator",
         ":pose_bundle",
         ":pose_bundle_to_draw_message",
@@ -106,6 +107,18 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "multibody_position_to_geometry_pose",
+    srcs = ["multibody_position_to_geometry_pose.cc"],
+    hdrs = ["multibody_position_to_geometry_pose.h"],
+    deps = [
+        "//common:pointer_cast",
+        "//geometry:frame_kinematics",
+        "//multibody/multibody_tree/multibody_plant",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_library(
     name = "render_pose_to_geometry_pose",
     srcs = ["render_pose_to_geometry_pose.cc"],
@@ -169,6 +182,18 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "multibody_position_to_geometry_pose_test",
+    data = ["//manipulation/models/iiwa_description:models"],
+    deps = [
+        ":multibody_position_to_geometry_pose",
+        "//common:find_resource",
+        "//common/test_utilities",
+        "//multibody/multibody_tree/parsing:multibody_plant_sdf_parser",
+        "//systems/framework/test_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "render_pose_to_geometry_pose_test",
     deps = [