diff --git a/bindings/pydrake/examples/quadrotor_py.cc b/bindings/pydrake/examples/quadrotor_py.cc index eea2241b7a7c..656aca984dd6 100644 --- a/bindings/pydrake/examples/quadrotor_py.cc +++ b/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::m, doc.QuadrotorPlant.m.doc) - .def("g", &QuadrotorPlant::g, doc.QuadrotorPlant.g.doc); + .def("g", &QuadrotorPlant::g, doc.QuadrotorPlant.g.doc) + .def("RegisterGeometry", &QuadrotorPlant::RegisterGeometry, + py::arg("scene_graph"), doc.QuadrotorPlant.RegisterGeometry.doc) + .def("get_geometry_pose_output_port", + &QuadrotorPlant::get_geometry_pose_output_port, + py_reference_internal, + doc.QuadrotorPlant.get_geometry_pose_output_port.doc) + .def("source_id", &QuadrotorPlant::source_id, + doc.QuadrotorPlant.source_id.doc); m.def("StabilizingLQRController", &StabilizingLQRController, py::arg("quadrotor_plant"), py::arg("nominal_position"), diff --git a/bindings/pydrake/examples/test/quadrotor_test.py b/bindings/pydrake/examples/test/quadrotor_test.py index 3315ccc853ef..cd79da9cbc35 100644 --- a/bindings/pydrake/examples/test/quadrotor_test.py +++ b/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)) diff --git a/examples/pendulum/pendulum_plant.h b/examples/pendulum/pendulum_plant.h index afd2ce680996..061347dace24 100644 --- a/examples/pendulum/pendulum_plant.h +++ b/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. diff --git a/examples/quadrotor/BUILD.bazel b/examples/quadrotor/BUILD.bazel index 752815648da7..e5dcc7e9b4d2 100644 --- a/examples/quadrotor/BUILD.bazel +++ b/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", diff --git a/examples/quadrotor/quadrotor_plant.cc b/examples/quadrotor/quadrotor_plant.cc index c4475c5de35c..742517b03fa8 100644 --- a/examples/quadrotor/quadrotor_plant.cc +++ b/examples/quadrotor/quadrotor_plant.cc @@ -3,8 +3,12 @@ #include #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::QuadrotorPlant(double m_arg, double L_arg, systems::SystemTypeTag{}), 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(12), - &QuadrotorPlant::CopyStateOut); + state_port_ = + this->DeclareVectorOutputPort("state", systems::BasicVector(12), + &QuadrotorPlant::CopyStateOut) + .get_index(); } template template QuadrotorPlant:: QuadrotorPlant(const QuadrotorPlant& other) - : QuadrotorPlant(other.m_, other.L_, other.I_, other.kF_, other.kM_) {} + : QuadrotorPlant(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 QuadrotorPlant::~QuadrotorPlant() {} @@ -132,6 +145,56 @@ void QuadrotorPlant::DoCalcTimeDerivatives( derivatives->SetFromVector(xDt); } +template +systems::OutputPortIndex QuadrotorPlant::AllocateGeometryPoseOutputPort() { + DRAKE_DEMAND(source_id_.is_valid() && frame_id_.is_valid()); + return this + ->DeclareAbstractOutputPort( + "geometry_pose", + geometry::FramePoseVector(source_id_, {frame_id_}), + &QuadrotorPlant::CopyPoseOut) + .get_index(); +} + +template +void QuadrotorPlant::RegisterGeometry( + geometry::SceneGraph* 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 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 +void QuadrotorPlant::CopyPoseOut(const systems::Context& context, + geometry::FramePoseVector* poses) const { + DRAKE_DEMAND(poses->size() == 1); + DRAKE_DEMAND(poses->source_id() == source_id_); + + VectorX state = context.get_continuous_state_vector().CopyToVector(); + + poses->clear(); + math::RigidTransform pose( + math::RollPitchYaw(state.template segment<3>(3)), + state.template head<3>()); + poses->set_value(frame_id_, pose.GetAsIsometry3()); +} + std::unique_ptr> StabilizingLQRController( const QuadrotorPlant* quadrotor_plant, Eigen::Vector3d nominal_position) { diff --git a/examples/quadrotor/quadrotor_plant.h b/examples/quadrotor/quadrotor_plant.h index c35d6b723fdb..a8c9882d88bc 100644 --- a/examples/quadrotor/quadrotor_plant.h +++ b/examples/quadrotor/quadrotor_plant.h @@ -4,6 +4,7 @@ #include +#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 class QuadrotorPlant final : public systems::LeafSystem { public: @@ -31,14 +37,36 @@ class QuadrotorPlant final : public systems::LeafSystem { double m() const { return m_; } double g() const { return g_; } - protected: - void CopyStateOut(const systems::Context& context, - systems::BasicVector* 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* scene_graph); + + /// Returns the port to output the pose to SceneGraph. Users must call + /// RegisterGeometry() first to enable this port. + const systems::OutputPort& get_geometry_pose_output_port() const { + return systems::System::get_output_port(geometry_pose_port_); + } + + geometry::SourceId source_id() const { return source_id_; } + private: void DoCalcTimeDerivatives( const systems::Context& context, systems::ContinuousState* derivatives) const override; + void CopyStateOut(const systems::Context& context, + systems::BasicVector* output) const; + + systems::OutputPortIndex AllocateGeometryPoseOutputPort(); + + // Calculator method for the pose output port. + void CopyPoseOut(const systems::Context& context, + geometry::FramePoseVector* 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 { return false; } - private: // Allow different specializations to access each other's private data. template friend class QuadrotorPlant; @@ -61,6 +88,15 @@ class QuadrotorPlant final : public systems::LeafSystem { 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 diff --git a/examples/quadrotor/run_quadrotor_dynamics.cc b/examples/quadrotor/run_quadrotor_dynamics.cc index 6a7e3d7aeffe..59ed86f09ebc 100644 --- a/examples/quadrotor/run_quadrotor_dynamics.cc +++ b/examples/quadrotor/run_quadrotor_dynamics.cc @@ -44,6 +44,8 @@ class Quadrotor : public systems::Diagram { Quadrotor() { this->set_name("Quadrotor"); + // TODO(SeanCurtis-TRI): Port this to SceneGraph pending resolution of + // #10775. auto tree = std::make_unique>(); ModelInstanceIdTable model_id_table = AddModelInstanceFromUrdfFileToWorld( FindResourceOrThrow("drake/examples/quadrotor/quadrotor.urdf"), diff --git a/examples/quadrotor/run_quadrotor_lqr.cc b/examples/quadrotor/run_quadrotor_lqr.cc index b6be265b196e..b1b09a194398 100644 --- a/examples/quadrotor/run_quadrotor_lqr.cc +++ b/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 builder; - auto tree = std::make_unique>(); - 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(*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(); + 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 simulator(*diagram);