Teach QuadrotorPlant about SceneGraph

Related to RobotLocomotion#10755.

bindings/pydrake/examples/quadrotor_py.cc

@@ -18,6 +18,7 @@ PYBIND11_MODULE(quadrotor, m) {
   m.doc() = "Bindings for the Quadrotor example.";
 
   py::module::import("pydrake.systems.framework");
+  py::module::import("pydrake.systems.primitives");
 
   // TODO(eric.cousineau): At present, we only bind doubles.
   // In the future, we will bind more scalar types, and enable scalar
@@ -31,7 +32,15 @@ PYBIND11_MODULE(quadrotor, m) {
           py::arg("m_arg"), py::arg("L_arg"), py::arg("I_arg"),
           py::arg("kF_arg"), py::arg("kM_arg"), doc.QuadrotorPlant.ctor.doc)
       .def("m", &QuadrotorPlant<T>::m, doc.QuadrotorPlant.m.doc)
-      .def("g", &QuadrotorPlant<T>::g, doc.QuadrotorPlant.g.doc);
+      .def("g", &QuadrotorPlant<T>::g, doc.QuadrotorPlant.g.doc)
+      .def("RegisterGeometry", &QuadrotorPlant<T>::RegisterGeometry,
+          py::arg("scene_graph"), doc.QuadrotorPlant.RegisterGeometry.doc)
+      .def("get_geometry_pose_output_port",
+          &QuadrotorPlant<T>::get_geometry_pose_output_port,
+          py_reference_internal,
+          doc.QuadrotorPlant.get_geometry_pose_output_port.doc)
+      .def("source_id", &QuadrotorPlant<T>::source_id,
+          doc.QuadrotorPlant.source_id.doc);
 
   m.def("StabilizingLQRController", &StabilizingLQRController,
       py::arg("quadrotor_plant"), py::arg("nominal_position"),

bindings/pydrake/examples/test/quadrotor_test.py

@@ -3,8 +3,7 @@
 import unittest
 import numpy as np
 
-import pydrake.systems.framework as framework
-from pydrake.systems.primitives import AffineSystem
+from pydrake.geometry import SceneGraph
 from pydrake.examples.quadrotor import QuadrotorPlant, StabilizingLQRController
 
 
@@ -19,4 +18,10 @@ def test_basics(self):
         self.assertEqual(quadrotor.m(), 1)
         self.assertEqual(quadrotor.g(), 9.81)
 
+        scene_graph = SceneGraph()
+        quadrotor.RegisterGeometry(scene_graph)
+
+        self.assertTrue(quadrotor.source_id().is_valid())
+        quadrotor.get_geometry_pose_output_port()
+
         StabilizingLQRController(quadrotor, np.zeros(3))

examples/pendulum/pendulum_plant.h

@@ -19,7 +19,7 @@ namespace pendulum {
 ///
 /// @system{PendulumPlant,
 ///    @input_port{tau},
-///    @output_port{state} @output_port{geometry pose}
+///    @output_port{state} @output_port{geometry_pose}
 /// }
 ///
 /// @tparam T The vector element type, which must be a valid Eigen scalar.

examples/quadrotor/BUILD.bazel

@@ -19,11 +19,14 @@ drake_cc_library(
     name = "quadrotor_plant",
     srcs = ["quadrotor_plant.cc"],
     hdrs = ["quadrotor_plant.h"],
+    data = [":models"],
     deps = [
         "//attic/util",
         "//common:default_scalars",
+        "//geometry:scene_graph",
         "//math:geometric_transform",
         "//math:gradient",
+        "//multibody/parsing",
         "//systems/controllers:linear_quadratic_regulator",
         "//systems/framework:leaf_system",
         "//systems/primitives:affine_system",
@@ -34,7 +37,6 @@ drake_cc_binary(
     name = "run_quadrotor_dynamics",
     srcs = ["run_quadrotor_dynamics.cc"],
     add_test_rule = 1,
-    data = [":models"],
     test_rule_args = [
         "--duration=0.1",
         "--initial_height=0.051",
@@ -59,20 +61,15 @@ drake_cc_binary(
     name = "run_quadrotor_lqr",
     srcs = ["run_quadrotor_lqr.cc"],
     add_test_rule = 1,
-    data = [":models"],
     test_rule_args = [
         "-simulation_trials=2",
         "-simulation_real_time_rate=0.0",
     ],
     deps = [
         ":quadrotor_plant",
-        "//attic/multibody:rigid_body_tree",
-        "//attic/multibody:rigid_body_tree_construction",
-        "//attic/multibody/parsers",
-        "//attic/multibody/rigid_body_plant",
-        "//attic/multibody/rigid_body_plant:drake_visualizer",
         "//common:find_resource",
         "//common:is_approx_equal_abstol",
+        "//geometry:geometry_visualization",
         "//lcm",
         "//systems/analysis:simulator",
         "//systems/framework:diagram",

examples/quadrotor/quadrotor_plant.cc

@@ -3,8 +3,12 @@
 #include <memory>
 
 #include "drake/common/default_scalars.h"
+#include "drake/common/find_resource.h"
 #include "drake/math/gradient.h"
+#include "drake/math/rigid_transform.h"
 #include "drake/math/rotation_matrix.h"
+#include "drake/multibody/parsing/parser.h"
+#include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/controllers/linear_quadratic_regulator.h"
 #include "drake/util/drakeGeometryUtil.h"
 
@@ -40,17 +44,26 @@ QuadrotorPlant<T>::QuadrotorPlant(double m_arg, double L_arg,
           systems::SystemTypeTag<quadrotor::QuadrotorPlant>{}),
       g_{9.81}, m_(m_arg), L_(L_arg), kF_(kF_arg), kM_(kM_arg), I_(I_arg) {
   // Four inputs -- one for each propellor.
-  this->DeclareInputPort(systems::kVectorValued, 4);
+  this->DeclareInputPort("propellor_force", systems::kVectorValued, 4);
   // State is x ,y , z, roll, pitch, yaw + velocities.
   this->DeclareContinuousState(12);
-  this->DeclareVectorOutputPort(systems::BasicVector<T>(12),
-                                &QuadrotorPlant::CopyStateOut);
+  state_port_ =
+      this->DeclareVectorOutputPort("state", systems::BasicVector<T>(12),
+                                    &QuadrotorPlant::CopyStateOut)
+          .get_index();
 }
 
 template <typename T>
 template <typename U>
 QuadrotorPlant<T>:: QuadrotorPlant(const QuadrotorPlant<U>& other)
-    : QuadrotorPlant<T>(other.m_, other.L_, other.I_, other.kF_, other.kM_) {}
+    : QuadrotorPlant<T>(other.m_, other.L_, other.I_, other.kF_, other.kM_) {
+  source_id_ = other.source_id();
+  frame_id_ = other.frame_id_;
+
+  if (source_id_.is_valid()) {
+    geometry_pose_port_ = AllocateGeometryPoseOutputPort();
+  }
+}
 
 template <typename T>
 QuadrotorPlant<T>::~QuadrotorPlant() {}
@@ -132,6 +145,56 @@ void QuadrotorPlant<T>::DoCalcTimeDerivatives(
   derivatives->SetFromVector(xDt);
 }
 
+template <typename T>
+systems::OutputPortIndex QuadrotorPlant<T>::AllocateGeometryPoseOutputPort() {
+  DRAKE_DEMAND(source_id_.is_valid() && frame_id_.is_valid());
+  return this
+      ->DeclareAbstractOutputPort(
+          "geometry_pose",
+          geometry::FramePoseVector<T>(source_id_, {frame_id_}),
+          &QuadrotorPlant<T>::CopyPoseOut)
+      .get_index();
+}
+
+template <typename T>
+void QuadrotorPlant<T>::RegisterGeometry(
+    geometry::SceneGraph<double>* scene_graph) {
+  DRAKE_DEMAND(!source_id_.is_valid());
+  DRAKE_DEMAND(scene_graph);
+
+  // Use (temporary) MultibodyPlant to parse the urdf and setup the
+  // scene_graph.
+  // TODO(SeanCurtis-TRI): Update this on resolution of #10775.
+  multibody::MultibodyPlant<double> mbp;
+  multibody::Parser parser(&mbp, scene_graph);
+
+  auto model_id = parser.AddModelFromFile(
+      FindResourceOrThrow("drake/examples/quadrotor/quadrotor.urdf"),
+      "quadrotor");
+  mbp.Finalize();
+
+  source_id_ = *mbp.get_source_id();
+  frame_id_ = mbp.GetBodyFrameIdOrThrow(mbp.GetBodyIndices(model_id)[0]);
+
+  // Now allocate the output port.
+  geometry_pose_port_ = AllocateGeometryPoseOutputPort();
+}
+
+template <typename T>
+void QuadrotorPlant<T>::CopyPoseOut(const systems::Context<T>& context,
+                                   geometry::FramePoseVector<T>* poses) const {
+  DRAKE_DEMAND(poses->size() == 1);
+  DRAKE_DEMAND(poses->source_id() == source_id_);
+
+  VectorX<T> state = context.get_continuous_state_vector().CopyToVector();
+
+  poses->clear();
+  math::RigidTransform<T> pose(
+      math::RollPitchYaw<T>(state.template segment<3>(3)),
+      state.template head<3>());
+  poses->set_value(frame_id_, pose.GetAsIsometry3());
+}
+
 std::unique_ptr<systems::AffineSystem<double>> StabilizingLQRController(
     const QuadrotorPlant<double>* quadrotor_plant,
     Eigen::Vector3d nominal_position) {

examples/quadrotor/quadrotor_plant.h

@@ -4,6 +4,7 @@
 
 #include <Eigen/Core>
 
+#include "drake/geometry/scene_graph.h"
 #include "drake/systems/framework/basic_vector.h"
 #include "drake/systems/framework/leaf_system.h"
 #include "drake/systems/primitives/affine_system.h"
@@ -15,6 +16,11 @@ namespace quadrotor {
 /// The Quadrotor - an underactuated aerial vehicle. This version of the
 /// Quadrotor is implemented to match the dynamics of the plant specified in
 /// the `quadrotor.urdf` model file.
+///
+/// @system{QuadrotorPlant,
+///    @input_port{propellor_force},
+///    @output_port{state} @output_port{geometry_pose}
+/// }
 template <typename T>
 class QuadrotorPlant final : public systems::LeafSystem<T> {
  public:
@@ -31,14 +37,36 @@ class QuadrotorPlant final : public systems::LeafSystem<T> {
   double m() const { return m_; }
   double g() const { return g_; }
 
- protected:
-  void CopyStateOut(const systems::Context<T>& context,
-                    systems::BasicVector<T>* output) const;
+  /// Registers this system as a source for the SceneGraph, adds the
+  /// quadrotor geometry, and creates the geometry_pose_output_port for this
+  /// system.  This must be called before the a SceneGraph's Context is
+  /// allocated.  It can only be called once.
+  // TODO(russt): this call only accepts doubles (not T) until SceneGraph
+  // supports symbolic.
+  void RegisterGeometry(geometry::SceneGraph<double>* scene_graph);
+
+  /// Returns the port to output the pose to SceneGraph.  Users must call
+  /// RegisterGeometry() first to enable this port.
+  const systems::OutputPort<T>& get_geometry_pose_output_port() const {
+    return systems::System<T>::get_output_port(geometry_pose_port_);
+  }
+
+  geometry::SourceId source_id() const { return source_id_; }
 
+ private:
   void DoCalcTimeDerivatives(
       const systems::Context<T>& context,
       systems::ContinuousState<T>* derivatives) const override;
 
+  void CopyStateOut(const systems::Context<T>& context,
+                    systems::BasicVector<T>* output) const;
+
+  systems::OutputPortIndex AllocateGeometryPoseOutputPort();
+
+  // Calculator method for the pose output port.
+  void CopyPoseOut(const systems::Context<T>& context,
+                   geometry::FramePoseVector<T>* poses) const;
+
   /// Declares that the system has no direct feedthrough from any input to any
   /// output.
   ///
@@ -50,7 +78,6 @@ class QuadrotorPlant final : public systems::LeafSystem<T> {
     return false;
   }
 
- private:
   // Allow different specializations to access each other's private data.
   template <typename> friend class QuadrotorPlant;
 
@@ -61,6 +88,15 @@ class QuadrotorPlant final : public systems::LeafSystem<T> {
   const double kF_;          // Force input constant.
   const double kM_;          // Moment input constant.
   const Eigen::Matrix3d I_;  // Moment of Inertia about the Center of Mass
+
+  // Port handles.
+  int state_port_{-1};
+  int geometry_pose_port_{-1};
+
+  // Geometry source identifier for this system to interact with SceneGraph.
+  geometry::SourceId source_id_{};
+  // The id for the quadrotor body.
+  geometry::FrameId frame_id_{};
 };
 
 /// Generates an LQR controller to move to @p nominal_position. Internally

examples/quadrotor/run_quadrotor_dynamics.cc

@@ -44,6 +44,8 @@ class Quadrotor : public systems::Diagram<T> {
   Quadrotor() {
     this->set_name("Quadrotor");
 
+    // TODO(SeanCurtis-TRI): Port this to SceneGraph pending resolution of
+    // #10775.
     auto tree = std::make_unique<RigidBodyTree<T>>();
     ModelInstanceIdTable model_id_table = AddModelInstanceFromUrdfFileToWorld(
         FindResourceOrThrow("drake/examples/quadrotor/quadrotor.urdf"),

examples/quadrotor/run_quadrotor_lqr.cc

@@ -10,12 +10,8 @@
 #include "drake/common/find_resource.h"
 #include "drake/common/is_approx_equal_abstol.h"
 #include "drake/examples/quadrotor/quadrotor_plant.h"
+#include "drake/geometry/geometry_visualization.h"
 #include "drake/lcm/drake_lcm.h"
-#include "drake/multibody/parsers/urdf_parser.h"
-#include "drake/multibody/rigid_body_plant/drake_visualizer.h"
-#include "drake/multibody/rigid_body_plant/rigid_body_plant.h"
-#include "drake/multibody/rigid_body_tree.h"
-#include "drake/multibody/rigid_body_tree_construction.h"
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/framework/diagram.h"
 #include "drake/systems/framework/diagram_builder.h"
@@ -40,11 +36,6 @@ int do_main() {
 
   DiagramBuilder<double> builder;
 
-  auto tree = std::make_unique<RigidBodyTree<double>>();
-  parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
-      FindResourceOrThrow("drake/examples/quadrotor/quadrotor.urdf"),
-      multibody::joints::kRollPitchYaw, tree.get());
-
   // The nominal hover position is at (0, 0, 1.0) in world coordinates.
   const Eigen::Vector3d kNominalPosition{((Eigen::Vector3d() << 0.0, 0.0, 1.0).
       finished())};
@@ -54,13 +45,15 @@ int do_main() {
   auto controller = builder.AddSystem(StabilizingLQRController(
       quadrotor, kNominalPosition));
   controller->set_name("controller");
-  auto visualizer =
-      builder.AddSystem<drake::systems::DrakeVisualizer>(*tree, &lcm);
-  visualizer->set_name("visualizer");
-
   builder.Connect(quadrotor->get_output_port(0), controller->get_input_port());
   builder.Connect(controller->get_output_port(), quadrotor->get_input_port(0));
-  builder.Connect(quadrotor->get_output_port(0), visualizer->get_input_port(0));
+
+  // Set up visualization
+  auto scene_graph = builder.AddSystem<geometry::SceneGraph>();
+  quadrotor->RegisterGeometry(scene_graph);
+  builder.Connect(quadrotor->get_geometry_pose_output_port(),
+      scene_graph->get_source_pose_port(quadrotor->source_id()));
+  geometry::ConnectDrakeVisualizer(&builder, *scene_graph);
 
   auto diagram = builder.Build();
   Simulator<double> simulator(*diagram);