Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Require unit vectors for UnitInertia functions. #19926

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
48 changes: 38 additions & 10 deletions bindings/pydrake/multibody/test/plant_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down
91 changes: 70 additions & 21 deletions bindings/pydrake/multibody/tree_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion common/text_logging.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
6 changes: 4 additions & 2 deletions multibody/parsing/test/detail_mujoco_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
5 changes: 0 additions & 5 deletions multibody/tree/spatial_inertia.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion multibody/tree/test/spatial_inertia_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down