From 0ab38013ef031f9f48d30acf02e05bb136f20c5f Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Fri, 24 Apr 2020 21:01:08 -0400 Subject: [PATCH] multibody: adds a Propeller model for use with MultibodyPlant (#13135) * multibody: adds a Propeller model for use with MultibodyPlant Includes python bindings. Partially resolves #12401. --- bindings/pydrake/multibody/plant_py.cc | 41 +++++ bindings/pydrake/multibody/test/plant_test.py | 30 ++++ multibody/plant/BUILD.bazel | 20 +++ multibody/plant/propeller.cc | 64 +++++++ multibody/plant/propeller.h | 141 ++++++++++++++++ multibody/plant/test/propeller_test.cc | 156 ++++++++++++++++++ 6 files changed, 452 insertions(+) create mode 100644 multibody/plant/propeller.cc create mode 100644 multibody/plant/propeller.h create mode 100644 multibody/plant/test/propeller_test.cc diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 40e2328b66a6..696a0fef9f81 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -19,6 +19,7 @@ #include "drake/multibody/plant/externally_applied_spatial_force.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/point_pair_contact_info.h" +#include "drake/multibody/plant/propeller.h" #include "drake/multibody/tree/spatial_inertia.h" PYBIND11_MAKE_OPAQUE( @@ -913,6 +914,32 @@ void DoScalarDependentDefinitions(py::module m, T) { } AddValueInstantiation(m); } + + // Propeller + { + using Class = Propeller; + constexpr auto& cls_doc = doc.Propeller; + auto cls = DefineTemplateClassWithDefault>( + m, "Propeller", param, cls_doc.doc); + cls // BR + .def(py::init&, + double, double>(), + py::arg("body_index"), + py::arg("X_BP") = math::RigidTransform::Identity(), + py::arg("thrust_ratio") = 1.0, py::arg("moment_ratio") = 0.0, + doc.Propeller.ctor.doc_4args) + .def(py::init&>(), + py::arg("propeller_info"), doc.Propeller.ctor.doc_1args) + .def("num_propellers", &Class::num_propellers, + doc.Propeller.num_propellers.doc) + .def("get_command_input_port", &Class::get_command_input_port, + py_reference_internal, doc.Propeller.get_command_input_port.doc) + .def("get_body_poses_input_port", &Class::get_body_poses_input_port, + py_reference_internal, doc.Propeller.get_body_poses_input_port.doc) + .def("get_spatial_forces_output_port", + &Class::get_spatial_forces_output_port, py_reference_internal, + doc.Propeller.get_spatial_forces_output_port.doc); + } // NOLINTNEXTLINE(readability/fn_size) } } // namespace @@ -966,6 +993,20 @@ PYBIND11_MODULE(plant, m) { // Keep alive, transitive: `lcm` keeps `builder` alive. py::keep_alive<3, 1>(), doc.ConnectContactResultsToDrakeVisualizer.doc_3args); + + py::class_(m, "PropellerInfo", doc.PropellerInfo.doc) + .def(py::init&, + double, double>(), + py::arg("body_index"), + py::arg("X_BP") = math::RigidTransform::Identity(), + py::arg("thrust_ratio") = 1.0, py::arg("moment_ratio") = 0.0) + .def_readwrite("body_index", &PropellerInfo::body_index, + doc.PropellerInfo.body_index.doc) + .def_readwrite("X_BP", &PropellerInfo::X_BP, doc.PropellerInfo.X_BP.doc) + .def_readwrite("thrust_ratio", &PropellerInfo::thrust_ratio, + doc.PropellerInfo.thrust_ratio.doc) + .def_readwrite("moment_ratio", &PropellerInfo::moment_ratio, + doc.PropellerInfo.moment_ratio.doc); } // NOLINT(readability/fn_size) } // namespace pydrake diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index b153a7f36090..1a5db46b17ac 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -52,6 +52,8 @@ ExternallyAppliedSpatialForce_, MultibodyPlant_, PointPairContactInfo_, + PropellerInfo, + Propeller_, VectorExternallyAppliedSpatialForced_, ) from pydrake.multibody.parsing import Parser @@ -1350,3 +1352,31 @@ def get_body_from_frame_id(frame_id): id_, = plant.GetCollisionGeometriesForBody( body=plant.GetBodyByName("body1")) self.assertIsInstance(id_, GeometryId) + + def test_propeller(self): + plant = MultibodyPlant_[float](time_step=0.0) + file_name = FindResourceOrThrow( + "drake/multibody/benchmarks/acrobot/acrobot.sdf") + Parser(plant).AddModelFromFile(file_name) + plant.Finalize() + + info = PropellerInfo(body_index=BodyIndex(1), + X_BP=RigidTransform_[float](), + thrust_ratio=1.0, + moment_ratio=0.1) + self.assertEqual(info.thrust_ratio, 1.0) + self.assertEqual(info.moment_ratio, 0.1) + + prop = Propeller_[float](body_index=BodyIndex(1), + X_BP=RigidTransform_[float](), + thrust_ratio=1.0, + moment_ratio=0.1) + self.assertEqual(prop.num_propellers(), 1) + self.assertIsInstance(prop.get_command_input_port(), InputPort_[float]) + self.assertIsInstance(prop.get_body_poses_input_port(), + InputPort_[float]) + self.assertIsInstance(prop.get_spatial_forces_output_port(), + OutputPort_[float]) + + prop2 = Propeller_[float]([info, info]) + self.assertEqual(prop2.num_propellers(), 2) diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 3021847fef96..1f2db2b23cae 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -26,6 +26,7 @@ drake_cc_package_library( ":hydroelastic_traction", ":multibody_plant_core", ":point_pair_contact_info", + ":propeller", ":tamsi_solver", ":tamsi_solver_results", ], @@ -246,6 +247,15 @@ drake_cc_library( ], ) +drake_cc_library( + name = "propeller", + srcs = ["propeller.cc"], + hdrs = ["propeller.h"], + deps = [ + ":multibody_plant_core", + ], +) + drake_cc_googletest( name = "hydroelastic_traction_test", data = [ @@ -602,4 +612,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "propeller_test", + deps = [ + ":propeller", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + "//systems/framework", + ], +) + add_lint_tests() diff --git a/multibody/plant/propeller.cc b/multibody/plant/propeller.cc new file mode 100644 index 000000000000..d0a02d69b137 --- /dev/null +++ b/multibody/plant/propeller.cc @@ -0,0 +1,64 @@ +#include "drake/multibody/plant/propeller.h" + +namespace drake { +namespace multibody { + +template +Propeller::Propeller(const BodyIndex& body_index, + const math::RigidTransform& X_BP, + double thrust_ratio, double moment_ratio) + : Propeller({PropellerInfo(body_index, X_BP, thrust_ratio, + moment_ratio)}) {} + +template +Propeller::Propeller(const std::vector& propeller_info) + : systems::LeafSystem(systems::SystemTypeTag{}), + info_(propeller_info) { + this->DeclareInputPort("command", systems::kVectorValued, num_propellers()); + + this->DeclareAbstractInputPort("body_poses", + Value>>()); + + this->DeclareAbstractOutputPort( + "spatial_forces", + std::vector>(num_propellers()), + &Propeller::CalcSpatialForces); +} + +template +void Propeller::CalcSpatialForces( + const systems::Context& context, + std::vector>* spatial_forces) const { + spatial_forces->resize(num_propellers()); + + const auto& command = get_command_input_port().Eval(context); + const auto& poses = + get_body_poses_input_port() + .template Eval>>(context); + + for (int i = 0; i < num_propellers(); i++) { + const PropellerInfo& prop = info_[i]; + + // Map to the ExternalSpatialForce structure: + // - the origin of my frame P is Po == Bq, and + // - the origin of my frame B is Bo. + const math::RigidTransform& X_WB = poses[prop.body_index]; + const Eigen::Vector3d& p_BoPo_B = prop.X_BP.translation(); + const math::RigidTransform X_WP = X_WB * prop.X_BP.cast(); + const math::RotationMatrix& R_WP = X_WP.rotation(); + + const SpatialForce F_BPo_P( + Vector3(0, 0, command[i] * prop.moment_ratio), + Vector3(0, 0, command[i] * prop.thrust_ratio)); + const SpatialForce F_BPo_W = R_WP * F_BPo_P; + spatial_forces->at(i).body_index = prop.body_index; + spatial_forces->at(i).p_BoBq_B = p_BoPo_B; + spatial_forces->at(i).F_Bq_W = F_BPo_W; + } +} + +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::multibody::Propeller) diff --git a/multibody/plant/propeller.h b/multibody/plant/propeller.h new file mode 100644 index 000000000000..2a3561a2dbd6 --- /dev/null +++ b/multibody/plant/propeller.h @@ -0,0 +1,141 @@ +#pragma once + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/eigen_types.h" +#include "drake/multibody/plant/externally_applied_spatial_force.h" +#include "drake/multibody/tree/body.h" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace multibody { + +/** Parameters that describe the kinematic frame and force-production properties +of a single propeller. */ +struct PropellerInfo { + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PropellerInfo); + + explicit PropellerInfo(const BodyIndex& body_index_, + const math::RigidTransform& X_BP_ = + math::RigidTransform::Identity(), + double thrust_ratio_ = 1.0, double moment_ratio_ = 0.0) + : body_index(body_index_), + X_BP(X_BP_), + thrust_ratio(thrust_ratio_), + moment_ratio(moment_ratio_) {} + + /** The BodyIndex of a Body in the MultibodyPlant to which the propeller is + attached. The spatial forces will be applied to this body. */ + BodyIndex body_index; + + /** Pose of the propeller frame P measured in the body frame B. @default is + the identity matrix. */ + math::RigidTransform X_BP{}; + + /** The z component (in frame P) of the spatial force will have magnitude + `thrust_ratio*command` in Newtons. The default is 1 (command in Newtons), but + this can also be used to scale an actuator command to the resulting force. */ + double thrust_ratio{1.0}; + + /** The moment about the z axis (in frame P) of the spatial force will have + magnitude `moment_ratio*command` in Newton-meters. The default is 0, which makes the propeller a simple Cartesian force generator. */ + double moment_ratio{0.0}; +}; + +/** A System that connects to the MultibodyPlant in order to model the effects +of one or more controlled propellers acting on a Body. + +@system{Propeller, + @input_port{command} + @input_port{body_poses}, + @output_port{spatial_forces} +} + +- The command input is a BasicVector with one element per propeller. +- It is expected that the body_poses input should be connected to the + @ref MultibodyPlant::get_body_poses_output_port() "MultibodyPlant body_poses + output port". +- The output is of type std::vector>; it is + expected that this output will be connected to the @ref + MultibodyPlant::get_applied_spatial_force_input_port() + "externally_applied_spatial_force input port" of the MultibodyPlant. +- This system does not have any state. + +The resulting iᵗʰ spatial force will have a force component in the z-axis of +the iᵗʰ propeller frame with magnitude `thrust_ratio * command` Newtons, and a +moment around the z-axis with magnitude `moment_ratio * command` Newton-meters. +(Including these moments tends to be important -- a quadrotor does not have a +stabilizable linearization around a hovering fixed point in 3D without them). + +@note Set PropellerInfo::moment_ratio to zero if you want a simple thruster +which applies only a force (no moment) in the Propeller coordinates. + +@tparam_default_scalar +@ingroup multibody +*/ +template +class Propeller final : public systems::LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Propeller); + + /** Constructs a system describing a single propeller. + @see PropellerInfo for details on the arguments. */ + Propeller(const BodyIndex& body_index, + const math::RigidTransform& X_BP = + math::RigidTransform::Identity(), + double thrust_ratio = 1.0, double moment_ratio = 0.0); + + /** Constructs a system describing multiple propellers. + @see PropellerInfo. */ + explicit Propeller(const std::vector& propeller_info); + + /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */ + template + explicit Propeller(const Propeller& other) + : Propeller(std::vector(other.info_.begin(), + other.info_.end())) {} + + /** Returns the number of propellers modeled by this system. */ + int num_propellers() const { return info_.size(); } + + /** Returns a reference to the vector-valued input port for the propeller + commands. It has size `num_propellers()`. */ + const systems::InputPort& get_command_input_port() const { + return this->get_input_port(0); + } + + /** Returns a reference to the body_poses input port. It is anticipated + that this port will be connected the body_poses output port of a + MultibodyPlant. */ + const systems::InputPort& get_body_poses_input_port() const { + return this->get_input_port(1); + } + + /** Returns a reference to the spatial_forces output port. It is anticipated + that this port will be connected to the @ref + MultibodyPlant::get_applied_spatial_force_input_port() "applied_spatial_force" + input port of a MultibodyPlant. */ + const systems::OutputPort& get_spatial_forces_output_port() const { + return this->get_output_port(0); + } + + private: + // Calculates the spatial forces in the world frame as expected by the + // applied_spatial_force input port of MultibodyPlant. + void CalcSpatialForces( + const systems::Context& context, + std::vector>* spatial_forces) const; + + std::vector info_; + + // Declare friendship to enable scalar conversion. + template + friend class Propeller; +}; + +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::multibody::Propeller) diff --git a/multibody/plant/test/propeller_test.cc b/multibody/plant/test/propeller_test.cc new file mode 100644 index 000000000000..3a779b7835ec --- /dev/null +++ b/multibody/plant/test/propeller_test.cc @@ -0,0 +1,156 @@ +#include "drake/multibody/plant/propeller.h" + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/systems/framework/diagram_builder.h" + +namespace drake { +namespace multibody { +namespace { + +// Test that we can create a propeller and wire it up in a diagram to produce +// the forces/moments that we expect. Also confirms that the entire diagram can +// perform scalar conversion. +GTEST_TEST(PropellerTest, SinglePropTest) { + systems::DiagramBuilder builder; + + const double timestep = 0.0; + auto plant = builder.AddSystem>(timestep); + + const double mass = 1.0; + const auto M_B = + SpatialInertia(mass, Vector3::Zero(), + UnitInertia::TriaxiallySymmetric(1.0)); + const RigidBody& body = plant->AddRigidBody("mass", M_B); + plant->Finalize(); + + const double thrust_ratio = 3.2; + const double moment_ratio = 0.4; + // Add a propeller at (0,1,0) pointing along the negative y axis. + auto prop = builder.AddSystem>( + body.index(), + math::RigidTransform( + math::RotationMatrix::MakeXRotation(M_PI / 2.0), + Eigen::Vector3d(0, 1.0, 0.0)), + thrust_ratio, moment_ratio); + EXPECT_EQ(prop->num_propellers(), 1); + + builder.Connect(prop->get_spatial_forces_output_port(), + plant->get_applied_spatial_force_input_port()); + builder.Connect(plant->get_body_poses_output_port(), + prop->get_body_poses_input_port()); + + builder.ExportInput(prop->get_command_input_port(), "prop_command"); + + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + + for (const double command : {0.0, -1.4, 2.3}) { + context->FixInputPort(0, Vector1d(command)); + const Eigen::VectorXd acceleration = diagram->EvalTimeDerivatives(*context) + .get_generalized_velocity() + .CopyToVector(); + EXPECT_TRUE(CompareMatrices(acceleration.head<3>(), + Eigen::Vector3d(0, -command * moment_ratio, 0), + 1e-14)); + EXPECT_TRUE(CompareMatrices(acceleration.tail<3>(), + Eigen::Vector3d(0, -command * thrust_ratio, 0) + + plant->gravity_field().gravity_vector(), + 1e-14)); + } + + // Repeat the check, but with the body rotated and translated. + // Propeller is now at (1,2,1), pointing along the positive x-axis. + plant->SetFreeBodyPose( + &plant->GetMyMutableContextFromRoot(context.get()), body, + math::RigidTransform( + math::RotationMatrix::MakeZRotation(M_PI / 2.0), + Eigen::Vector3d::Ones())); + + for (const double command : {0.0, -1.4, 2.3}) { + context->FixInputPort(0, Vector1d(command)); + const Eigen::VectorXd acceleration = diagram->EvalTimeDerivatives(*context) + .get_generalized_velocity() + .CopyToVector(); + EXPECT_TRUE(CompareMatrices(acceleration.head<3>(), + Eigen::Vector3d(command * moment_ratio, 0, 0), + 1e-14)); + EXPECT_TRUE(CompareMatrices(acceleration.tail<3>(), + Eigen::Vector3d(command * thrust_ratio, 0, 0) + + plant->gravity_field().gravity_vector(), + 1e-14)); + } + + // Test that I can perform scalar conversion. + diagram->ToAutoDiffXd(); + diagram->ToSymbolic(); +} + +// Test that we can add multiple propellers to a MultibodyPlant (via a single +// Propeller system), and generate the forces/moments we expect. Also confirms +// that the entire diagram can perform scalar conversion. +GTEST_TEST(PropellerTest, BiRotorTest) { + systems::DiagramBuilder builder; + + const double timestep = 0.0; + auto plant = builder.AddSystem>(timestep); + + const double mass = 1.0; + const auto M_B = + SpatialInertia(mass, Vector3::Zero(), + UnitInertia::TriaxiallySymmetric(1.0)); + const RigidBody& body = plant->AddRigidBody("mass", M_B); + plant->Finalize(); + + const double thrust_ratio = 3.2; + const double moment_ratio = 0.4; + const double arm_length = 0.67; // distance from body origin to the prop. + + math::RigidTransform X_BP; + X_BP.set_translation(Eigen::Vector3d(arm_length, 0, 0)); + PropellerInfo prop1(body.index(), X_BP, thrust_ratio, moment_ratio); + X_BP.set_translation(Eigen::Vector3d(-arm_length, 0, 0)); + PropellerInfo prop2(body.index(), X_BP, thrust_ratio, moment_ratio); + auto props = builder.AddSystem>( + std::vector({prop1, prop2})); + EXPECT_EQ(props->num_propellers(), 2); + + builder.Connect(props->get_spatial_forces_output_port(), + plant->get_applied_spatial_force_input_port()); + builder.Connect(plant->get_body_poses_output_port(), + props->get_body_poses_input_port()); + + builder.ExportInput(props->get_command_input_port(), "prop_command"); + + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + + const Eigen::Vector2d command(.3, .4); + context->FixInputPort(0, command); + const Eigen::VectorXd acceleration = diagram->EvalTimeDerivatives(*context) + .get_generalized_velocity() + .CopyToVector(); + EXPECT_TRUE(CompareMatrices( + acceleration.head<3>(), + Eigen::Vector3d(0, (command[1] - command[0]) * arm_length * thrust_ratio, + (command[0] + command[1]) * moment_ratio), + 1e-14)); + EXPECT_TRUE(CompareMatrices( + acceleration.tail<3>(), + Eigen::Vector3d(0, 0, (command[0] + command[1]) * thrust_ratio) + + plant->gravity_field().gravity_vector(), + 1e-14)); + + // Test that I can perform scalar conversion. + diagram->ToAutoDiffXd(); + diagram->ToSymbolic(); +} + +} // namespace +} // namespace multibody +} // namespace drake