Require reasonable unit vectors for UnitInertia functions (RobotLocom…

…otion#19926) Deprecates the previous liberal acceptance of any-length vectors. Co-authored-by: mitiguy <[email protected]>
alan0526 · Aug 15, 2023 · 75114dc · 75114dc
1 parent bbb65ef
commit 75114dc
Show file tree

Hide file tree

Showing 10 changed files with 464 additions and 265 deletions.
diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -733,26 +733,54 @@ def test_unit_inertia_api(self, T):
         self.assertIsInstance(UnitInertia.SolidBox(Lx=1, Ly=2, Lz=3),
                               UnitInertia)
         self.assertIsInstance(UnitInertia.SolidCube(L=2), UnitInertia)
-        self.assertIsInstance(UnitInertia.SolidCylinder(r=1.5, L=2),
+        self.assertIsInstance(UnitInertia.SolidCylinder(
+                                  radius=1.5, length=2, unit_vector=[0, 0, 1]),
                               UnitInertia)
-        self.assertIsInstance(
-            UnitInertia.SolidCylinder(r=1.5, L=2, b_E=[1, 2, 3]), UnitInertia)
-        self.assertIsInstance(UnitInertia.SolidCapsule(r=1, L=2), UnitInertia)
-        self.assertIsInstance(UnitInertia.SolidCapsule(r=1, L=2,
-                                                       unit_vector=[0, 0, 1]),
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.SolidCylinder(r=1.5, L=2),
+                                  UnitInertia)
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.SolidCylinder(
+                                      r=1.5, L=2, b_E=[1, 2, 3]),
+                                  UnitInertia)
+        self.assertIsInstance(UnitInertia.SolidCapsule(
+                                  radius=1, length=2, unit_vector=[0, 0, 1]),
                               UnitInertia)
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.SolidCapsule(r=1, L=2),
+                                  UnitInertia)
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.SolidCapsule(
+                                      r=1, L=2, unit_vector=[0, 0, 1]),
+                                  UnitInertia)
         self.assertIsInstance(UnitInertia.SolidCylinderAboutEnd(
             radius=0.1, length=0.4, unit_vector=[0, 0, 1]),
                               UnitInertia)
         with catch_drake_warnings(expected_count=1):
             self.assertIsInstance(UnitInertia.SolidCylinderAboutEnd(r=1, L=4),
                                   UnitInertia)
-        self.assertIsInstance(
-            UnitInertia.AxiallySymmetric(J=1, K=2, b_E=[1, 2, 3]), UnitInertia)
-        self.assertIsInstance(UnitInertia.StraightLine(K=1.5, b_E=[1, 2, 3]),
+        self.assertIsInstance(UnitInertia.AxiallySymmetric(
+                                  moment_parallel=1, moment_perpendicular=2,
+                                  unit_vector=[0, 0, 1]),
+                              UnitInertia)
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.AxiallySymmetric(
+                                      J=1, K=2, b_E=[1, 2, 3]),
+                                  UnitInertia)
+        self.assertIsInstance(UnitInertia.StraightLine(
+                                  moment_perpendicular=1.5,
+                                  unit_vector=[0, 0, 1]),
                               UnitInertia)
-        self.assertIsInstance(UnitInertia.ThinRod(L=1.5, b_E=[1, 2, 3]),
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.StraightLine(
+                                      K=1.5, b_E=[1, 2, 3]),
+                                  UnitInertia)
+        self.assertIsInstance(UnitInertia.ThinRod(
+                                  length=1.5, unit_vector=[0, 0, 1]),
                               UnitInertia)
+        with catch_drake_warnings(expected_count=1):
+            self.assertIsInstance(UnitInertia.ThinRod(L=1.5, b_E=[1, 2, 3]),
+                                  UnitInertia)
 
     @numpy_compare.check_all_types
     def test_spatial_inertia_api(self, T):

diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc
@@ -1152,34 +1152,32 @@ void DoScalarDependentDefinitions(py::module m, T) {
             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("r"),
-            py::arg("L"), py::arg("b_E") = Vector3<T>::UnitZ().eval(),
+        .def_static("SolidCylinder",
+            // TODO(jwnimmer-tri) Drop the overload_cast_explicit on 2023-12-01.
+            overload_cast_explicit<Class, const T&, const T&,
+                const Vector3<T>&>(&Class::SolidCylinder),
+            py::arg("radius"), py::arg("length"), py::arg("unit_vector"),
             cls_doc.SolidCylinder.doc)
-        .def_static("SolidCapsule", &Class::SolidCapsule, py::arg("r"),
-            py::arg("L"), py::arg("unit_vector") = Vector3<T>::UnitZ().eval(),
+        .def_static("SolidCapsule",
+            // TODO(jwnimmer-tri) Drop the overload_cast_explicit on 2023-12-01.
+            overload_cast_explicit<Class, const T&, const T&,
+                const Vector3<T>&>(&Class::SolidCapsule),
+            py::arg("radius"), py::arg("length"), py::arg("unit_vector"),
             cls_doc.SolidCapsule.doc)
-        // TODO(2023-11-01) Remove overload wrapping when deprecation complete.
         .def_static("SolidCylinderAboutEnd",
+            // TODO(jwnimmer-tri) Drop the overload_cast_explicit on 2023-12-01.
             overload_cast_explicit<Class, const T&, const T&,
                 const Vector3<T>&>(&Class::SolidCylinderAboutEnd),
             py::arg("radius"), py::arg("length"), py::arg("unit_vector"),
             cls_doc.SolidCylinderAboutEnd.doc)
-        .def_static("SolidCylinderAboutEnd",
-            WrapDeprecated(cls_doc.SolidCylinderAboutEnd.doc_deprecated,
-                [](const T& r, const T& L) {
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-                  return Class::SolidCylinderAboutEnd(r, L);
-#pragma GCC diagnostic pop
-                }),
-            py::arg("r"), py::arg("L"),
-            cls_doc.SolidCylinderAboutEnd.doc_deprecated)
-        .def_static("AxiallySymmetric", &Class::AxiallySymmetric, py::arg("J"),
-            py::arg("K"), py::arg("b_E"), cls_doc.AxiallySymmetric.doc)
-        .def_static("StraightLine", &Class::StraightLine, py::arg("K"),
-            py::arg("b_E"), cls_doc.StraightLine.doc)
-        .def_static("ThinRod", &Class::ThinRod, py::arg("L"), py::arg("b_E"),
-            cls_doc.ThinRod.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(
@@ -1191,6 +1189,57 @@ void DoScalarDependentDefinitions(py::module m, T) {
                   I(0, 0), I(1, 1), I(2, 2), I(0, 1), I(0, 2), I(1, 2));
             }));
     DefCopyAndDeepCopy(&cls);
+    // TODO(jwnimmer-tri) Remove as of 2023-12-01.
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+    constexpr char doc_factory_deprecated[] = R"""(
+This overload spelling of a UnitInertia factory method is deprecated.
+
+The named arguments for the UnitInertia factory methods have been renamed:
+ - ``r`` → ``radius``
+ - ``L`` → ``length``
+ - ``J`` → ``moment_parallel``
+ - ``K`` → ``moment_perpendicular``
+ - ``b_E`` → ``unit_vector``
+
+The old argument names will be removed from Drake on or after 2023-12-01.
+
+The ``unit_vector`` argument no longer has a default value, and must already
+have been normalized by the caller. Passing in a non-normalized unit vector
+will result in a warning for now, but an exception on or after 2023-12-01.
+)""";
+    cls  // BR
+        .def_static("SolidCylinder",
+            WrapDeprecated(doc_factory_deprecated,
+                [](const T& r, const T& L, const Vector3<T>& b_E) {
+                  return Class::SolidCylinder(r, L, b_E);
+                }),
+            py::arg("r"), py::arg("L"),
+            py::arg("b_E") = Vector3<T>::UnitZ().eval(), doc_factory_deprecated)
+        .def_static("SolidCylinderAboutEnd",
+            WrapDeprecated(doc_factory_deprecated,
+                [](const T& r, const T& L) {
+                  return Class::SolidCylinderAboutEnd(r, L);
+                }),
+            py::arg("r"), py::arg("L"), doc_factory_deprecated)
+        .def_static("SolidCapsule",
+            WrapDeprecated(doc_factory_deprecated,
+                [](const T& r, const T& L, const Vector3<T>& unit_vector) {
+                  return Class::SolidCapsule(r, L, unit_vector);
+                }),
+            py::arg("r"), py::arg("L"),
+            py::arg("unit_vector") = Vector3<T>::UnitZ().eval(),
+            doc_factory_deprecated)
+        .def_static("AxiallySymmetric",
+            WrapDeprecated(doc_factory_deprecated, &Class::AxiallySymmetric),
+            py::arg("J"), py::arg("K"), py::arg("b_E"), doc_factory_deprecated)
+        .def_static("StraightLine",
+            WrapDeprecated(doc_factory_deprecated, &Class::StraightLine),
+            py::arg("K"), py::arg("b_E"), doc_factory_deprecated)
+        .def_static("ThinRod",
+            WrapDeprecated(doc_factory_deprecated, &Class::ThinRod),
+            py::arg("L"), py::arg("b_E"), doc_factory_deprecated);
+#pragma GCC diagnostic pop
   }
 
   // SpatialInertia

diff --git a/common/text_logging.h b/common/text_logging.h
@@ -197,7 +197,7 @@ sink* get_dist_sink();
 ///   return data;
 /// }
 /// </pre>
-struct Warn {
+struct[[maybe_unused]] Warn {
   template <typename... Args>
   Warn(const char* a, const Args&... b) {
     // TODO(jwnimmer-tri) Ideally we would compile-time check our Warn format

diff --git a/examples/multibody/cylinder_with_multicontact/test/populate_cylinder_plant_test.cc b/examples/multibody/cylinder_with_multicontact/test/populate_cylinder_plant_test.cc
@@ -65,8 +65,11 @@ GTEST_TEST(PopulateCylinderPlant, VerifyPlant) {
 
   EXPECT_EQ(M_Bcm_B.get_mass(), mass);
   EXPECT_EQ(M_Bcm_B.get_com(), Vector3d::Zero());
-  EXPECT_EQ(M_Bcm_B.get_unit_inertia().get_moments(),
-            Vector3d(G_perp, G_perp, G_axis));
+
+  // An empirical tolerance: two bits = 2^2 times machine epsilon.
+  const double kTolerance = 4 * std::numeric_limits<double>::epsilon();
+  EXPECT_TRUE(CompareMatrices(M_Bcm_B.get_unit_inertia().get_moments(),
+                              Vector3d(G_perp, G_perp, G_axis), kTolerance));
   EXPECT_EQ(M_Bcm_B.get_unit_inertia().get_products(), Vector3d::Zero());
 
   // TODO(amcastro-tri): Verify geometry including:

diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc
@@ -736,9 +736,11 @@ TEST_F(MujocoParserTest, InertiaFromGeometry) {
 
   check_body("default", UnitInertia<double>::SolidSphere(0.1));
   check_body_spatial("sphere", inertia_from_inertial_tag);
-  check_body("capsule", UnitInertia<double>::SolidCapsule(0.1, 4.0));
+  check_body("capsule",
+      UnitInertia<double>::SolidCapsule(0.1, 4.0, Vector3d::UnitZ()));
   check_body("ellipsoid", UnitInertia<double>::SolidEllipsoid(0.1, 0.2, 0.3));
-  check_body("cylinder", UnitInertia<double>::SolidCylinder(0.1, 4.0));
+  check_body("cylinder",
+      UnitInertia<double>::SolidCylinder(0.1, 4.0, Vector3d::UnitZ()));
   check_body("box", UnitInertia<double>::SolidBox(0.2, 4.0, 6.0));
   check_body("box_from_default", UnitInertia<double>::SolidBox(0.2, 0.4, 0.6));
   check_body("ellipsoid_from_default",

diff --git a/multibody/tree/spatial_inertia.cc b/multibody/tree/spatial_inertia.cc
@@ -222,11 +222,6 @@ SpatialInertia<T> SpatialInertia<T>::ThinRodWithMass(
   ThrowUnlessValueIsPositiveFinite(mass, "mass", __func__);
   ThrowUnlessValueIsPositiveFinite(length, "length", __func__);
   ThrowUnlessVectorIsMagnitudeOne(unit_vector, __func__);
-
-  // Although a check is made that ‖unit_vector‖ ≈ 1, we normalize regardless.
-  // Note: UnitInertia::ThinRod() calls UnitInertia::AxiallySymmetric() which
-  // also normalizes unit_vector before use.
-  // TODO(Mitiguy) remove normalization in UnitInertia::AxiallySymmetric().
   const UnitInertia<T> G_BBcm_B =
       UnitInertia<T>::ThinRod(length, unit_vector);
   const Vector3<T> p_BoBcm_B = Vector3<T>::Zero();

diff --git a/multibody/tree/test/spatial_inertia_test.cc b/multibody/tree/test/spatial_inertia_test.cc
@@ -880,7 +880,7 @@ GTEST_TEST(SpatialInertia, Shift) {
   // First define it in frame B.
   SpatialInertia<double> M_BBcm_W(
       mass, Vector3d::Zero(),
-      UnitInertia<double>::SolidCylinder(radius, length));
+      UnitInertia<double>::SolidCylinder(radius, length, Vector3d::UnitZ()));
   // Then re-express in frame W.
   M_BBcm_W.ReExpressInPlace(R_WB);