From b29351f70b7279ed4cb581de48409f59a726e962 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Wed, 27 Mar 2024 10:12:40 -0700 Subject: [PATCH] [pydrake] Split MbT inertia bindings into their own file (#21200) This will help build speed, and is necessary to solve ordering problems with which classes are loaded in what order. --- bindings/pydrake/multibody/BUILD.bazel | 6 +- bindings/pydrake/multibody/tree_py.cc | 270 +-------------- bindings/pydrake/multibody/tree_py.h | 23 ++ bindings/pydrake/multibody/tree_py_inertia.cc | 309 ++++++++++++++++++ 4 files changed, 340 insertions(+), 268 deletions(-) create mode 100644 bindings/pydrake/multibody/tree_py.h create mode 100644 bindings/pydrake/multibody/tree_py_inertia.cc diff --git a/bindings/pydrake/multibody/BUILD.bazel b/bindings/pydrake/multibody/BUILD.bazel index 7f7810b9ae3d..dffbbee6e168 100644 --- a/bindings/pydrake/multibody/BUILD.bazel +++ b/bindings/pydrake/multibody/BUILD.bazel @@ -106,7 +106,11 @@ drake_pybind_library( "//bindings/pydrake/common:type_pack", "//bindings/pydrake/common:type_safe_index_pybind", ], - cc_srcs = ["tree_py.cc"], + cc_srcs = [ + "tree_py.cc", + "tree_py.h", + "tree_py_inertia.cc", + ], package_info = PACKAGE_INFO, py_deps = [ ":math_py", diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc index 389deaf19adf..218d8ba978f4 100644 --- a/bindings/pydrake/multibody/tree_py.cc +++ b/bindings/pydrake/multibody/tree_py.cc @@ -1,3 +1,5 @@ +#include "drake/bindings/pydrake/multibody/tree_py.h" + #include "pybind11/eval.h" #include "drake/bindings/pydrake/common/cpp_template_pybind.h" @@ -15,7 +17,6 @@ #include "drake/multibody/tree/door_hinge.h" #include "drake/multibody/tree/force_element.h" #include "drake/multibody/tree/frame.h" -#include "drake/multibody/tree/geometry_spatial_inertia.h" #include "drake/multibody/tree/joint.h" #include "drake/multibody/tree/joint_actuator.h" #include "drake/multibody/tree/linear_bushing_roll_pitch_yaw.h" @@ -114,18 +115,6 @@ void DoScalarIndependentDefinitions(py::module m) { m.def("default_model_instance", &default_model_instance, doc.default_model_instance.doc); - // CalcSpatialInertia. - { - m.def("CalcSpatialInertia", - py::overload_cast(&CalcSpatialInertia), - py::arg("shape"), py::arg("density"), doc.CalcSpatialInertia.doc_shape); - - m.def("CalcSpatialInertia", - py::overload_cast&, double>( - &CalcSpatialInertia), - py::arg("mesh"), py::arg("density"), doc.CalcSpatialInertia.doc_mesh); - } - { using Class = DoorHingeConfig; constexpr auto& cls_doc = doc.DoorHingeConfig; @@ -1231,260 +1220,6 @@ void DoScalarDependentDefinitions(py::module m, T) { cls_doc.AddInForces.doc); DefCopyAndDeepCopy(&cls); } - - // Inertias - { - using Class = RotationalInertia; - constexpr auto& cls_doc = doc.RotationalInertia; - auto cls = DefineTemplateClassWithDefault( - m, "RotationalInertia", param, cls_doc.doc); - cls // BR - .def(py::init<>(), cls_doc.ctor.doc_0args) - .def(py::init(), py::arg("Ixx"), - py::arg("Iyy"), py::arg("Izz"), cls_doc.ctor.doc_3args) - .def(py::init(), - py::arg("Ixx"), py::arg("Iyy"), py::arg("Izz"), py::arg("Ixy"), - py::arg("Ixz"), py::arg("Iyz"), cls_doc.ctor.doc_6args) - .def(py::init&>(), py::arg("mass"), - py::arg("p_PQ_E"), cls_doc.ctor.doc_2args) - .def_static("TriaxiallySymmetric", &Class::TriaxiallySymmetric, - py::arg("I_triaxial"), cls_doc.TriaxiallySymmetric.doc) - .def("rows", &Class::rows, cls_doc.rows.doc) - .def("cols", &Class::cols, cls_doc.cols.doc) - .def("get_moments", &Class::get_moments, cls_doc.get_moments.doc) - .def("get_products", &Class::get_products, cls_doc.get_products.doc) - .def("Trace", &Class::Trace, cls_doc.Trace.doc) - .def("CalcMaximumPossibleMomentOfInertia", - &Class::CalcMaximumPossibleMomentOfInertia, - cls_doc.CalcMaximumPossibleMomentOfInertia.doc) - .def( - "__getitem__", - [](const Class& self, py::tuple key) -> T { - if (key.size() != 2) { - throw std::out_of_range("Expected [i,j] for __getitem__."); - } - const int i = py::cast(key[0]); - const int j = py::cast(key[1]); - return self(i, j); - }, - cls_doc.operator_call.doc) - .def("CopyToFullMatrix3", &Class::CopyToFullMatrix3, - cls_doc.CopyToFullMatrix3.doc) - .def("IsNearlyEqualTo", &Class::IsNearlyEqualTo, py::arg("other"), - py::arg("precision"), cls_doc.IsNearlyEqualTo.doc) - .def(py::self += py::self, cls_doc.operator_iadd.doc) - .def(py::self + py::self, cls_doc.operator_add.doc) - .def(py::self -= py::self, cls_doc.operator_isub.doc) - .def(py::self - py::self, cls_doc.operator_sub.doc) - .def(py::self *= T{}, cls_doc.operator_imul.doc) - .def(py::self * T{}, cls_doc.operator_mul.doc) - .def(T{} * py::self, cls_doc.operator_mul.doc) - .def(py::self * Vector3{}, cls_doc.operator_mul.doc) - .def(py::self /= T{}, cls_doc.operator_idiv.doc) - .def(py::self / T{}, cls_doc.operator_div.doc) - .def("SetToNaN", &Class::SetToNaN, cls_doc.SetToNaN.doc) - .def("SetZero", &Class::SetZero, cls_doc.SetZero.doc) - .def("IsNaN", &Class::IsNaN, cls_doc.IsNaN.doc) - // TODO(jwnimmer-tri) Need to bind cast<>. - .def("CalcPrincipalMomentsOfInertia", - &Class::CalcPrincipalMomentsOfInertia, - cls_doc.CalcPrincipalMomentsOfInertia.doc) - .def("CalcPrincipalMomentsAndAxesOfInertia", - &Class::CalcPrincipalMomentsAndAxesOfInertia, - cls_doc.CalcPrincipalMomentsAndAxesOfInertia.doc) - .def("CouldBePhysicallyValid", &Class::CouldBePhysicallyValid, - cls_doc.CouldBePhysicallyValid.doc) - .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), - cls_doc.ReExpress.doc) - .def("ShiftFromCenterOfMass", &Class::ShiftFromCenterOfMass, - py::arg("mass"), py::arg("p_BcmQ_E"), - cls_doc.ShiftFromCenterOfMass.doc) - .def("ShiftToCenterOfMass", &Class::ShiftToCenterOfMass, - py::arg("mass"), py::arg("p_QBcm_E"), - cls_doc.ShiftToCenterOfMass.doc) - .def("ShiftToThenAwayFromCenterOfMass", - &Class::ShiftToThenAwayFromCenterOfMass, py::arg("mass"), - py::arg("p_PBcm_E"), py::arg("p_QBcm_E"), - cls_doc.ShiftToThenAwayFromCenterOfMass.doc) - .def(py::pickle( - [](const Class& self) { return self.CopyToFullMatrix3(); }, - [](const Matrix3& I) { - // Invoke 6-argument constructor by specifying full (upper - // diagonal) inertia matrix. - return Class( - I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2)); - })); - DefCopyAndDeepCopy(&cls); - } - { - using Class = UnitInertia; - constexpr auto& cls_doc = doc.UnitInertia; - auto cls = DefineTemplateClassWithDefault>( - m, "UnitInertia", param, cls_doc.doc); - cls // BR - .def(py::init(), cls_doc.ctor.doc_0args) - .def(py::init(), py::arg("Ixx"), - py::arg("Iyy"), py::arg("Izz"), cls_doc.ctor.doc_3args) - .def(py::init(), - py::arg("Ixx"), py::arg("Iyy"), py::arg("Izz"), py::arg("Ixy"), - py::arg("Ixz"), py::arg("Iyz"), cls_doc.ctor.doc_6args) - .def(py::init([](const RotationalInertia& I) { return Class(I); }), - py::arg("I"), cls_doc.ctor.doc_1args) - .def("SetFromRotationalInertia", &Class::SetFromRotationalInertia, - py::arg("I"), py::arg("mass"), py_rvp::reference, - cls_doc.SetFromRotationalInertia.doc) - .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), - cls_doc.ReExpress.doc) - .def("ShiftFromCenterOfMass", &Class::ShiftFromCenterOfMass, - py::arg("p_BcmQ_E"), cls_doc.ShiftFromCenterOfMass.doc) - .def("ShiftToCenterOfMass", &Class::ShiftToCenterOfMass, - py::arg("p_QBcm_E"), cls_doc.ShiftToCenterOfMass.doc) - .def_static("PointMass", &Class::PointMass, py::arg("p_FQ"), - cls_doc.PointMass.doc) - .def_static("SolidEllipsoid", &Class::SolidEllipsoid, py::arg("a"), - py::arg("b"), py::arg("c"), cls_doc.SolidEllipsoid.doc) - .def_static("SolidSphere", &Class::SolidSphere, py::arg("r"), - cls_doc.SolidSphere.doc) - .def_static("HollowSphere", &Class::HollowSphere, py::arg("r"), - cls_doc.HollowSphere.doc) - .def_static("SolidBox", &Class::SolidBox, py::arg("Lx"), py::arg("Ly"), - py::arg("Lz"), cls_doc.SolidBox.doc) - .def_static( - "SolidCube", &Class::SolidCube, py::arg("L"), cls_doc.SolidCube.doc) - .def_static("SolidCylinder", &Class::SolidCylinder, py::arg("radius"), - py::arg("length"), py::arg("unit_vector"), - cls_doc.SolidCylinder.doc) - .def_static("SolidCapsule", &Class::SolidCapsule, py::arg("radius"), - py::arg("length"), py::arg("unit_vector"), cls_doc.SolidCapsule.doc) - .def_static("SolidCylinderAboutEnd", &Class::SolidCylinderAboutEnd, - py::arg("radius"), py::arg("length"), py::arg("unit_vector"), - cls_doc.SolidCylinderAboutEnd.doc) - .def_static("AxiallySymmetric", &Class::AxiallySymmetric, - py::arg("moment_parallel"), py::arg("moment_perpendicular"), - py::arg("unit_vector"), cls_doc.AxiallySymmetric.doc) - .def_static("StraightLine", &Class::StraightLine, - py::arg("moment_perpendicular"), py::arg("unit_vector"), - cls_doc.StraightLine.doc) - .def_static("ThinRod", &Class::ThinRod, py::arg("length"), - py::arg("unit_vector"), cls_doc.ThinRod.doc) - .def_static("TriaxiallySymmetric", &Class::TriaxiallySymmetric, - py::arg("I_triaxial"), cls_doc.TriaxiallySymmetric.doc) - .def(py::pickle( - [](const Class& self) { return self.CopyToFullMatrix3(); }, - [](const Matrix3& I) { - // Invoke 6-argument constructor by specifying full (upper - // diagonal) inertia matrix. - return Class( - I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2)); - })); - DefCopyAndDeepCopy(&cls); - } - - // SpatialInertia - { - using Class = SpatialInertia; - constexpr auto& cls_doc = doc.SpatialInertia; - auto cls = DefineTemplateClassWithDefault( - m, "SpatialInertia", param, cls_doc.doc); - cls // BR - .def_static("MakeFromCentralInertia", &Class::MakeFromCentralInertia, - py::arg("mass"), py::arg("p_PScm_E"), py::arg("I_SScm_E"), - cls_doc.MakeFromCentralInertia.doc) - .def_static("SolidBoxWithDensity", &Class::SolidBoxWithDensity, - py::arg("density"), py::arg("lx"), py::arg("ly"), py::arg("lz"), - cls_doc.SolidBoxWithDensity.doc) - .def_static("SolidBoxWithMass", &Class::SolidBoxWithMass, - py::arg("mass"), py::arg("lx"), py::arg("ly"), py::arg("lz"), - cls_doc.SolidBoxWithMass.doc) - .def_static("SolidCubeWithDensity", &Class::SolidCubeWithDensity, - py::arg("density"), py::arg("length"), - cls_doc.SolidCubeWithDensity.doc) - .def_static("SolidCapsuleWithDensity", &Class::SolidCapsuleWithDensity, - py::arg("density"), py::arg("radius"), py::arg("length"), - py::arg("unit_vector"), cls_doc.SolidCapsuleWithDensity.doc) - .def_static("SolidCapsuleWithMass", &Class::SolidCapsuleWithMass, - py::arg("mass"), py::arg("radius"), py::arg("length"), - py::arg("unit_vector"), cls_doc.SolidCapsuleWithMass.doc) - .def_static("SolidCylinderWithDensity", - &Class::SolidCylinderWithDensity, py::arg("density"), - py::arg("radius"), py::arg("length"), py::arg("unit_vector"), - cls_doc.SolidCylinderWithDensity.doc) - .def_static("SolidCylinderWithMass", &Class::SolidCylinderWithMass, - py::arg("mass"), py::arg("radius"), py::arg("length"), - py::arg("unit_vector"), cls_doc.SolidCylinderWithMass.doc) - .def_static("SolidCylinderWithDensityAboutEnd", - &Class::SolidCylinderWithDensityAboutEnd, py::arg("density"), - py::arg("radius"), py::arg("length"), py::arg("unit_vector"), - cls_doc.SolidCylinderWithDensityAboutEnd.doc) - .def_static("SolidCylinderWithMassAboutEnd", - &Class::SolidCylinderWithMassAboutEnd, py::arg("mass"), - py::arg("radius"), py::arg("length"), py::arg("unit_vector"), - cls_doc.SolidCylinderWithMassAboutEnd.doc) - .def_static("ThinRodWithMass", &Class::ThinRodWithMass, py::arg("mass"), - py::arg("length"), py::arg("unit_vector"), - cls_doc.ThinRodWithMass.doc) - .def_static("ThinRodWithMassAboutEnd", &Class::ThinRodWithMassAboutEnd, - py::arg("mass"), py::arg("length"), py::arg("unit_vector"), - cls_doc.ThinRodWithMassAboutEnd.doc) - .def_static("SolidEllipsoidWithDensity", - &Class::SolidEllipsoidWithDensity, py::arg("density"), py::arg("a"), - py::arg("b"), py::arg("c"), cls_doc.SolidEllipsoidWithDensity.doc) - .def_static("SolidEllipsoidWithMass", &Class::SolidEllipsoidWithMass, - py::arg("mass"), py::arg("a"), py::arg("b"), py::arg("c"), - cls_doc.SolidEllipsoidWithMass.doc) - .def_static("SolidSphereWithDensity", &Class::SolidSphereWithDensity, - py::arg("density"), py::arg("radius"), - cls_doc.SolidSphereWithDensity.doc) - .def_static("SolidSphereWithMass", &Class::SolidSphereWithMass, - py::arg("mass"), py::arg("radius"), cls_doc.SolidSphereWithMass.doc) - .def_static("HollowSphereWithDensity", &Class::HollowSphereWithDensity, - py::arg("area_density"), py::arg("radius"), - cls_doc.HollowSphereWithDensity.doc) - .def_static("HollowSphereWithMass", &Class::HollowSphereWithMass, - py::arg("mass"), py::arg("radius"), - cls_doc.HollowSphereWithMass.doc) - .def(py::init(), cls_doc.ctor.doc_0args) - .def(py::init>&, - const UnitInertia&, const bool>(), - py::arg("mass"), py::arg("p_PScm_E"), py::arg("G_SP_E"), - py::arg("skip_validity_check") = false, cls_doc.ctor.doc_4args) - // TODO(jwnimmer-tri) Need to bind cast<>. - .def("get_mass", &Class::get_mass, cls_doc.get_mass.doc) - .def("get_com", &Class::get_com, cls_doc.get_com.doc) - .def("CalcComMoment", &Class::CalcComMoment, cls_doc.CalcComMoment.doc) - .def("get_unit_inertia", &Class::get_unit_inertia, - cls_doc.get_unit_inertia.doc) - .def("CalcRotationalInertia", &Class::CalcRotationalInertia, - cls_doc.CalcRotationalInertia.doc) - .def("IsPhysicallyValid", &Class::IsPhysicallyValid, - cls_doc.IsPhysicallyValid.doc) - .def("CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid", - &Class::CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid, - cls_doc.CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid.doc) - .def("CopyToFullMatrix6", &Class::CopyToFullMatrix6, - cls_doc.CopyToFullMatrix6.doc) - .def("IsNaN", &Class::IsNaN, cls_doc.IsNaN.doc) - .def("SetNaN", &Class::SetNaN, cls_doc.SetNaN.doc) - .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), - cls_doc.ReExpress.doc) - .def("Shift", &Class::Shift, py::arg("p_PQ_E"), cls_doc.Shift.doc) - .def(py::self += py::self) - .def(py::self * SpatialAcceleration()) - .def(py::self * SpatialVelocity()) - .def(py::pickle( - [](const Class& self) { - return py::make_tuple( - self.get_mass(), self.get_com(), self.get_unit_inertia()); - }, - [](py::tuple t) { - DRAKE_THROW_UNLESS(t.size() == 3); - return Class(t[0].cast(), t[1].cast>(), - t[2].cast>()); - })); - DefCopyAndDeepCopy(&cls); - } // NOLINTNEXTLINE(readability/fn_size) } } // namespace @@ -1498,6 +1233,7 @@ PYBIND11_MODULE(tree, m) { py::module::import("pydrake.common.eigen_geometry"); py::module::import("pydrake.multibody.math"); + internal::DefineTreeInertia(m); DoScalarIndependentDefinitions(m); type_visit([m](auto dummy) { DoScalarDependentDefinitions(m, dummy); }, CommonScalarPack{}); diff --git a/bindings/pydrake/multibody/tree_py.h b/bindings/pydrake/multibody/tree_py.h new file mode 100644 index 000000000000..177cf92e6668 --- /dev/null +++ b/bindings/pydrake/multibody/tree_py.h @@ -0,0 +1,23 @@ +#pragma once + +/* This file declares some functions that help bind into the + drake.multibody.tree module. + + The implementations of these functions are parceled out into various *.cc + files as indicated in each function's documentation. */ + +#include "drake/bindings/pydrake/pydrake_pybind.h" + +namespace drake { +namespace pydrake { +namespace internal { + +/* Defines bindings per tree_py_inertia.cc. */ +void DefineTreeInertia(py::module m); + +// TODO(jwnimmer-tri) Add more functions here as we continue to split up the +// tree_py.cc file into difference pieces. + +} // namespace internal +} // namespace pydrake +} // namespace drake diff --git a/bindings/pydrake/multibody/tree_py_inertia.cc b/bindings/pydrake/multibody/tree_py_inertia.cc new file mode 100644 index 000000000000..864dc3f8fac3 --- /dev/null +++ b/bindings/pydrake/multibody/tree_py_inertia.cc @@ -0,0 +1,309 @@ +#include "drake/bindings/pydrake/common/cpp_template_pybind.h" +#include "drake/bindings/pydrake/common/default_scalars_pybind.h" +#include "drake/bindings/pydrake/documentation_pybind.h" +#include "drake/bindings/pydrake/pydrake_pybind.h" +#include "drake/multibody/tree/geometry_spatial_inertia.h" +#include "drake/multibody/tree/rotational_inertia.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/multibody/tree/unit_inertia.h" + +namespace drake { +namespace pydrake { +namespace internal { + +using multibody::SpatialAcceleration; +using multibody::SpatialVelocity; + +namespace { + +void DoScalarIndependentDefinitions(py::module m) { + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::multibody; + constexpr auto& doc = pydrake_doc.drake.multibody; + + { + m.def("CalcSpatialInertia", + py::overload_cast(&CalcSpatialInertia), + py::arg("shape"), py::arg("density"), doc.CalcSpatialInertia.doc_shape); + + m.def("CalcSpatialInertia", + py::overload_cast&, double>( + &CalcSpatialInertia), + py::arg("mesh"), py::arg("density"), doc.CalcSpatialInertia.doc_mesh); + } +} + +template +void DoScalarDependentDefinitions(py::module m, T) { + py::tuple param = GetPyParam(); + + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::multibody; + constexpr auto& doc = pydrake_doc.drake.multibody; + + { + using Class = RotationalInertia; + constexpr auto& cls_doc = doc.RotationalInertia; + auto cls = DefineTemplateClassWithDefault( + m, "RotationalInertia", param, cls_doc.doc); + cls // BR + .def(py::init<>(), cls_doc.ctor.doc_0args) + .def(py::init(), py::arg("Ixx"), + py::arg("Iyy"), py::arg("Izz"), cls_doc.ctor.doc_3args) + .def(py::init(), + py::arg("Ixx"), py::arg("Iyy"), py::arg("Izz"), py::arg("Ixy"), + py::arg("Ixz"), py::arg("Iyz"), cls_doc.ctor.doc_6args) + .def(py::init&>(), py::arg("mass"), + py::arg("p_PQ_E"), cls_doc.ctor.doc_2args) + .def_static("TriaxiallySymmetric", &Class::TriaxiallySymmetric, + py::arg("I_triaxial"), cls_doc.TriaxiallySymmetric.doc) + .def("rows", &Class::rows, cls_doc.rows.doc) + .def("cols", &Class::cols, cls_doc.cols.doc) + .def("get_moments", &Class::get_moments, cls_doc.get_moments.doc) + .def("get_products", &Class::get_products, cls_doc.get_products.doc) + .def("Trace", &Class::Trace, cls_doc.Trace.doc) + .def("CalcMaximumPossibleMomentOfInertia", + &Class::CalcMaximumPossibleMomentOfInertia, + cls_doc.CalcMaximumPossibleMomentOfInertia.doc) + .def( + "__getitem__", + [](const Class& self, py::tuple key) -> T { + if (key.size() != 2) { + throw std::out_of_range("Expected [i,j] for __getitem__."); + } + const int i = py::cast(key[0]); + const int j = py::cast(key[1]); + return self(i, j); + }, + cls_doc.operator_call.doc) + .def("CopyToFullMatrix3", &Class::CopyToFullMatrix3, + cls_doc.CopyToFullMatrix3.doc) + .def("IsNearlyEqualTo", &Class::IsNearlyEqualTo, py::arg("other"), + py::arg("precision"), cls_doc.IsNearlyEqualTo.doc) + .def(py::self += py::self, cls_doc.operator_iadd.doc) + .def(py::self + py::self, cls_doc.operator_add.doc) + .def(py::self -= py::self, cls_doc.operator_isub.doc) + .def(py::self - py::self, cls_doc.operator_sub.doc) + .def(py::self *= T{}, cls_doc.operator_imul.doc) + .def(py::self * T{}, cls_doc.operator_mul.doc) + .def(T{} * py::self, cls_doc.operator_mul.doc) + .def(py::self * Vector3{}, cls_doc.operator_mul.doc) + .def(py::self /= T{}, cls_doc.operator_idiv.doc) + .def(py::self / T{}, cls_doc.operator_div.doc) + .def("SetToNaN", &Class::SetToNaN, cls_doc.SetToNaN.doc) + .def("SetZero", &Class::SetZero, cls_doc.SetZero.doc) + .def("IsNaN", &Class::IsNaN, cls_doc.IsNaN.doc) + // TODO(jwnimmer-tri) Need to bind cast<>. + .def("CalcPrincipalMomentsOfInertia", + &Class::CalcPrincipalMomentsOfInertia, + cls_doc.CalcPrincipalMomentsOfInertia.doc) + .def("CalcPrincipalMomentsAndAxesOfInertia", + &Class::CalcPrincipalMomentsAndAxesOfInertia, + cls_doc.CalcPrincipalMomentsAndAxesOfInertia.doc) + .def("CouldBePhysicallyValid", &Class::CouldBePhysicallyValid, + cls_doc.CouldBePhysicallyValid.doc) + .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), + cls_doc.ReExpress.doc) + .def("ShiftFromCenterOfMass", &Class::ShiftFromCenterOfMass, + py::arg("mass"), py::arg("p_BcmQ_E"), + cls_doc.ShiftFromCenterOfMass.doc) + .def("ShiftToCenterOfMass", &Class::ShiftToCenterOfMass, + py::arg("mass"), py::arg("p_QBcm_E"), + cls_doc.ShiftToCenterOfMass.doc) + .def("ShiftToThenAwayFromCenterOfMass", + &Class::ShiftToThenAwayFromCenterOfMass, py::arg("mass"), + py::arg("p_PBcm_E"), py::arg("p_QBcm_E"), + cls_doc.ShiftToThenAwayFromCenterOfMass.doc) + .def(py::pickle( + [](const Class& self) { return self.CopyToFullMatrix3(); }, + [](const Matrix3& I) { + // Invoke 6-argument constructor by specifying full (upper + // diagonal) inertia matrix. + return Class( + I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2)); + })); + DefCopyAndDeepCopy(&cls); + } + + { + using Class = UnitInertia; + constexpr auto& cls_doc = doc.UnitInertia; + auto cls = DefineTemplateClassWithDefault>( + m, "UnitInertia", param, cls_doc.doc); + cls // BR + .def(py::init(), cls_doc.ctor.doc_0args) + .def(py::init(), py::arg("Ixx"), + py::arg("Iyy"), py::arg("Izz"), cls_doc.ctor.doc_3args) + .def(py::init(), + py::arg("Ixx"), py::arg("Iyy"), py::arg("Izz"), py::arg("Ixy"), + py::arg("Ixz"), py::arg("Iyz"), cls_doc.ctor.doc_6args) + .def(py::init([](const RotationalInertia& I) { return Class(I); }), + py::arg("I"), cls_doc.ctor.doc_1args) + .def("SetFromRotationalInertia", &Class::SetFromRotationalInertia, + py::arg("I"), py::arg("mass"), py_rvp::reference, + cls_doc.SetFromRotationalInertia.doc) + .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), + cls_doc.ReExpress.doc) + .def("ShiftFromCenterOfMass", &Class::ShiftFromCenterOfMass, + py::arg("p_BcmQ_E"), cls_doc.ShiftFromCenterOfMass.doc) + .def("ShiftToCenterOfMass", &Class::ShiftToCenterOfMass, + py::arg("p_QBcm_E"), cls_doc.ShiftToCenterOfMass.doc) + .def_static("PointMass", &Class::PointMass, py::arg("p_FQ"), + cls_doc.PointMass.doc) + .def_static("SolidEllipsoid", &Class::SolidEllipsoid, py::arg("a"), + py::arg("b"), py::arg("c"), cls_doc.SolidEllipsoid.doc) + .def_static("SolidSphere", &Class::SolidSphere, py::arg("r"), + cls_doc.SolidSphere.doc) + .def_static("HollowSphere", &Class::HollowSphere, py::arg("r"), + cls_doc.HollowSphere.doc) + .def_static("SolidBox", &Class::SolidBox, py::arg("Lx"), py::arg("Ly"), + py::arg("Lz"), cls_doc.SolidBox.doc) + .def_static( + "SolidCube", &Class::SolidCube, py::arg("L"), cls_doc.SolidCube.doc) + .def_static("SolidCylinder", &Class::SolidCylinder, py::arg("radius"), + py::arg("length"), py::arg("unit_vector"), + cls_doc.SolidCylinder.doc) + .def_static("SolidCapsule", &Class::SolidCapsule, py::arg("radius"), + py::arg("length"), py::arg("unit_vector"), cls_doc.SolidCapsule.doc) + .def_static("SolidCylinderAboutEnd", &Class::SolidCylinderAboutEnd, + py::arg("radius"), py::arg("length"), py::arg("unit_vector"), + cls_doc.SolidCylinderAboutEnd.doc) + .def_static("AxiallySymmetric", &Class::AxiallySymmetric, + py::arg("moment_parallel"), py::arg("moment_perpendicular"), + py::arg("unit_vector"), cls_doc.AxiallySymmetric.doc) + .def_static("StraightLine", &Class::StraightLine, + py::arg("moment_perpendicular"), py::arg("unit_vector"), + cls_doc.StraightLine.doc) + .def_static("ThinRod", &Class::ThinRod, py::arg("length"), + py::arg("unit_vector"), cls_doc.ThinRod.doc) + .def_static("TriaxiallySymmetric", &Class::TriaxiallySymmetric, + py::arg("I_triaxial"), cls_doc.TriaxiallySymmetric.doc) + .def(py::pickle( + [](const Class& self) { return self.CopyToFullMatrix3(); }, + [](const Matrix3& I) { + // Invoke 6-argument constructor by specifying full (upper + // diagonal) inertia matrix. + return Class( + I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2)); + })); + DefCopyAndDeepCopy(&cls); + } + + // SpatialInertia + { + using Class = SpatialInertia; + constexpr auto& cls_doc = doc.SpatialInertia; + auto cls = DefineTemplateClassWithDefault( + m, "SpatialInertia", param, cls_doc.doc); + cls // BR + .def_static("MakeFromCentralInertia", &Class::MakeFromCentralInertia, + py::arg("mass"), py::arg("p_PScm_E"), py::arg("I_SScm_E"), + cls_doc.MakeFromCentralInertia.doc) + .def_static("SolidBoxWithDensity", &Class::SolidBoxWithDensity, + py::arg("density"), py::arg("lx"), py::arg("ly"), py::arg("lz"), + cls_doc.SolidBoxWithDensity.doc) + .def_static("SolidBoxWithMass", &Class::SolidBoxWithMass, + py::arg("mass"), py::arg("lx"), py::arg("ly"), py::arg("lz"), + cls_doc.SolidBoxWithMass.doc) + .def_static("SolidCubeWithDensity", &Class::SolidCubeWithDensity, + py::arg("density"), py::arg("length"), + cls_doc.SolidCubeWithDensity.doc) + .def_static("SolidCapsuleWithDensity", &Class::SolidCapsuleWithDensity, + py::arg("density"), py::arg("radius"), py::arg("length"), + py::arg("unit_vector"), cls_doc.SolidCapsuleWithDensity.doc) + .def_static("SolidCapsuleWithMass", &Class::SolidCapsuleWithMass, + py::arg("mass"), py::arg("radius"), py::arg("length"), + py::arg("unit_vector"), cls_doc.SolidCapsuleWithMass.doc) + .def_static("SolidCylinderWithDensity", + &Class::SolidCylinderWithDensity, py::arg("density"), + py::arg("radius"), py::arg("length"), py::arg("unit_vector"), + cls_doc.SolidCylinderWithDensity.doc) + .def_static("SolidCylinderWithMass", &Class::SolidCylinderWithMass, + py::arg("mass"), py::arg("radius"), py::arg("length"), + py::arg("unit_vector"), cls_doc.SolidCylinderWithMass.doc) + .def_static("SolidCylinderWithDensityAboutEnd", + &Class::SolidCylinderWithDensityAboutEnd, py::arg("density"), + py::arg("radius"), py::arg("length"), py::arg("unit_vector"), + cls_doc.SolidCylinderWithDensityAboutEnd.doc) + .def_static("SolidCylinderWithMassAboutEnd", + &Class::SolidCylinderWithMassAboutEnd, py::arg("mass"), + py::arg("radius"), py::arg("length"), py::arg("unit_vector"), + cls_doc.SolidCylinderWithMassAboutEnd.doc) + .def_static("ThinRodWithMass", &Class::ThinRodWithMass, py::arg("mass"), + py::arg("length"), py::arg("unit_vector"), + cls_doc.ThinRodWithMass.doc) + .def_static("ThinRodWithMassAboutEnd", &Class::ThinRodWithMassAboutEnd, + py::arg("mass"), py::arg("length"), py::arg("unit_vector"), + cls_doc.ThinRodWithMassAboutEnd.doc) + .def_static("SolidEllipsoidWithDensity", + &Class::SolidEllipsoidWithDensity, py::arg("density"), py::arg("a"), + py::arg("b"), py::arg("c"), cls_doc.SolidEllipsoidWithDensity.doc) + .def_static("SolidEllipsoidWithMass", &Class::SolidEllipsoidWithMass, + py::arg("mass"), py::arg("a"), py::arg("b"), py::arg("c"), + cls_doc.SolidEllipsoidWithMass.doc) + .def_static("SolidSphereWithDensity", &Class::SolidSphereWithDensity, + py::arg("density"), py::arg("radius"), + cls_doc.SolidSphereWithDensity.doc) + .def_static("SolidSphereWithMass", &Class::SolidSphereWithMass, + py::arg("mass"), py::arg("radius"), cls_doc.SolidSphereWithMass.doc) + .def_static("HollowSphereWithDensity", &Class::HollowSphereWithDensity, + py::arg("area_density"), py::arg("radius"), + cls_doc.HollowSphereWithDensity.doc) + .def_static("HollowSphereWithMass", &Class::HollowSphereWithMass, + py::arg("mass"), py::arg("radius"), + cls_doc.HollowSphereWithMass.doc) + .def(py::init(), cls_doc.ctor.doc_0args) + .def(py::init>&, + const UnitInertia&, const bool>(), + py::arg("mass"), py::arg("p_PScm_E"), py::arg("G_SP_E"), + py::arg("skip_validity_check") = false, cls_doc.ctor.doc_4args) + // TODO(jwnimmer-tri) Need to bind cast<>. + .def("get_mass", &Class::get_mass, cls_doc.get_mass.doc) + .def("get_com", &Class::get_com, cls_doc.get_com.doc) + .def("CalcComMoment", &Class::CalcComMoment, cls_doc.CalcComMoment.doc) + .def("get_unit_inertia", &Class::get_unit_inertia, + cls_doc.get_unit_inertia.doc) + .def("CalcRotationalInertia", &Class::CalcRotationalInertia, + cls_doc.CalcRotationalInertia.doc) + .def("IsPhysicallyValid", &Class::IsPhysicallyValid, + cls_doc.IsPhysicallyValid.doc) + .def("CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid", + &Class::CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid, + cls_doc.CalcPrincipalSemiDiametersAndPoseForSolidEllipsoid.doc) + .def("CopyToFullMatrix6", &Class::CopyToFullMatrix6, + cls_doc.CopyToFullMatrix6.doc) + .def("IsNaN", &Class::IsNaN, cls_doc.IsNaN.doc) + .def("SetNaN", &Class::SetNaN, cls_doc.SetNaN.doc) + .def("ReExpress", &Class::ReExpress, py::arg("R_AE"), + cls_doc.ReExpress.doc) + .def("Shift", &Class::Shift, py::arg("p_PQ_E"), cls_doc.Shift.doc) + .def(py::self += py::self) + .def(py::self * SpatialAcceleration()) + .def(py::self * SpatialVelocity()) + .def(py::pickle( + [](const Class& self) { + return py::make_tuple( + self.get_mass(), self.get_com(), self.get_unit_inertia()); + }, + [](py::tuple t) { + DRAKE_THROW_UNLESS(t.size() == 3); + return Class(t[0].cast(), t[1].cast>(), + t[2].cast>()); + })); + DefCopyAndDeepCopy(&cls); + } +} + +} // namespace + +void DefineTreeInertia(py::module m) { + DoScalarIndependentDefinitions(m); + type_visit([m](auto dummy) { DoScalarDependentDefinitions(m, dummy); }, + CommonScalarPack{}); +} + +} // namespace internal +} // namespace pydrake +} // namespace drake