diff --git a/bindings/pydrake/multibody/examples/door_hinge_inspector.ipynb b/bindings/pydrake/multibody/examples/door_hinge_inspector.ipynb index 8f3f5cf2db83..f27a06022e37 100644 --- a/bindings/pydrake/multibody/examples/door_hinge_inspector.ipynb +++ b/bindings/pydrake/multibody/examples/door_hinge_inspector.ipynb @@ -28,16 +28,15 @@ "import matplotlib.pyplot as plt\n", "from pydrake.multibody.plant import MultibodyPlant\n", "from pydrake.multibody.tree import (\n", - " DoorHingeConfig, DoorHinge, RevoluteJoint, SpatialInertia\n", + " DoorHingeConfig, DoorHinge, RevoluteJoint\n", ")\n", "\n", "# Build a simple MultibodyPlant that contains two bodies and a revolute joint.\n", "# Then add a DoorHinge ForceElement to the joint.\n", "plant = MultibodyPlant(0.0)\n", - "spatial_inertia = SpatialInertia()\n", "\n", - "body_a = plant.AddRigidBody(name=\"body_a\", M_BBo_B=spatial_inertia)\n", - "body_b = plant.AddRigidBody(name=\"body_b\", M_BBo_B=spatial_inertia)\n", + "body_a = plant.AddRigidBody(name=\"body_a\")\n", + "body_b = plant.AddRigidBody(name=\"body_b\")\n", "\n", "revolute_joint = plant.AddJoint(RevoluteJoint(\n", " name=\"revolve_joint\", frame_on_parent=body_a.body_frame(),\n", diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index e5f5887302a3..9c55df644e6d 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -241,12 +241,14 @@ void DoScalarDependentDefinitions(py::module m, T) { const SpatialInertia& s) -> auto& { return self->AddRigidBody(name, s); }, - py::arg("name"), py::arg("M_BBo_B"), py_rvp::reference_internal, - cls_doc.AddRigidBody.doc_2args) + py::arg("name"), + py::arg("M_BBo_B") = SpatialInertia::Zero(), + py_rvp::reference_internal, cls_doc.AddRigidBody.doc_2args) .def("AddRigidBody", py::overload_cast&>(&Class::AddRigidBody), - py::arg("name"), py::arg("model_instance"), py::arg("M_BBo_B"), + py::arg("name"), py::arg("model_instance"), + py::arg("M_BBo_B") = SpatialInertia::Zero(), py_rvp::reference_internal, cls_doc.AddRigidBody.doc_3args) .def("WeldFrames", &Class::WeldFrames, py::arg("frame_on_parent_F"), py::arg("frame_on_child_M"), diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index d7c3bb1f0cea..a5aa692ea366 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -201,7 +201,7 @@ def test_multibody_plant_construction_api(self, T): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) self.assertEqual(plant.time_step(), 0.0) - spatial_inertia = SpatialInertia() + spatial_inertia = SpatialInertia.NaN() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) body_mass = body.default_mass() @@ -786,7 +786,10 @@ def test_spatial_inertia_api(self, T): SpatialForce = SpatialForce_[T] SpatialVelocity = SpatialVelocity_[T] SpatialMomentum = SpatialMomentum_[T] - SpatialInertia() + SpatialInertia.Zero() + SpatialInertia.NaN() + with catch_drake_warnings(expected_count=1) as w: + SpatialInertia() SpatialInertia.MakeFromCentralInertia( mass=1.3, p_PScm_E=[0.1, -0.2, 0.3], I_SScm_E=RotationalInertia(Ixx=2.0, Iyy=2.3, Izz=2.4)) @@ -897,8 +900,10 @@ def test_legacy_unpickle_plant_module(self): @numpy_compare.check_all_types def test_rigid_body_api(self, T): TemplatedRigidBody = RigidBody_[T] - M = SpatialInertia_[float]() + M = SpatialInertia_[float].NaN() i = ModelInstanceIndex(0) + TemplatedRigidBody(body_name="body_name") + TemplatedRigidBody(body_name="body_name", model_instance=i) TemplatedRigidBody(body_name="body_name", M_BBo_B=M) TemplatedRigidBody(body_name="body_name", model_instance=i, M_BBo_B=M) # Make sure the default (float) version also works. @@ -1378,7 +1383,7 @@ def set_zero(): @numpy_compare.check_all_types def test_default_free_body_pose(self, T): plant = MultibodyPlant_[T](0.0) - body = plant.AddRigidBody("body", SpatialInertia_[float]()) + body = plant.AddRigidBody("body") plant.Finalize() # Test existence of default free body pose setting. X_WB_default = RigidTransform_[float]() @@ -1975,7 +1980,7 @@ def make_weld_joint(plant, P, C): def loop_body(make_joint, time_step): plant = MultibodyPlant_[T](time_step) - child = plant.AddRigidBody("Child", SpatialInertia_[float]()) + child = plant.AddRigidBody("Child") joint = make_joint( plant=plant, P=plant.world_frame(), C=child.body_frame()) joint_out = plant.AddJoint(joint) @@ -2344,12 +2349,8 @@ def test_actuation_matrix(self): def test_deprecated_weld_joint_api(self): plant = MultibodyPlant_[float](0.01) - body1 = plant.AddRigidBody( - name="body1", - M_BBo_B=SpatialInertia_[float]()) - body2 = plant.AddRigidBody( - name="body2", - M_BBo_B=SpatialInertia_[float]()) + body1 = plant.AddRigidBody(name="body1") + body2 = plant.AddRigidBody(name="body2") # No keywords defaults to the first constructor defined in the binding. # No warning. @@ -2361,12 +2362,8 @@ def test_deprecated_weld_joint_api(self): def test_deprecated_weld_frames_api(self): plant = MultibodyPlant_[float](0.01) - body1 = plant.AddRigidBody( - name="body1", - M_BBo_B=SpatialInertia_[float]()) - body2 = plant.AddRigidBody( - name="body2", - M_BBo_B=SpatialInertia_[float]()) + body1 = plant.AddRigidBody(name="body1") + body2 = plant.AddRigidBody(name="body2") # No keywords defaults to the first function named `WeldFrames` defined # in the binding. No warning. @@ -2478,7 +2475,7 @@ def test_frame_context_methods(self, T): def test_fixed_offset_frame_api(self, T): FixedOffsetFrame = FixedOffsetFrame_[T] P = MultibodyPlant_[T](0.0).world_frame() - B = RigidBody_[T]("body", SpatialInertia_[float]()) + B = RigidBody_[T]("body") X = RigidTransform_[float].Identity() FixedOffsetFrame(name="name", P=P, X_PF=X, model_instance=None) FixedOffsetFrame(name="name", bodyB=B, X_BF=X) @@ -2511,11 +2508,9 @@ def test_distance_constraint_api(self, T): plant.set_discrete_contact_approximation( DiscreteContactApproximation.kSap) - # Add a distance constraint. Since we won't be performing dynamics - # computations, using garbage inertia is ok for this test. - M_BBo_B = SpatialInertia_[float]() - body_A = plant.AddRigidBody(name="A", M_BBo_B=M_BBo_B) - body_B = plant.AddRigidBody(name="B", M_BBo_B=M_BBo_B) + # Add a distance constraint. + body_A = plant.AddRigidBody(name="A") + body_B = plant.AddRigidBody(name="B") p_AP = [0.0, 0.0, 0.0] p_BQ = [0.0, 0.0, 0.0] plant.AddDistanceConstraint( @@ -2533,11 +2528,9 @@ def test_ball_constraint_api(self, T): plant.set_discrete_contact_approximation( DiscreteContactApproximation.kSap) - # Add ball constraint. Since we won't be performing dynamics - # computations, using garbage inertia is ok for this test. - M_BBo_B = SpatialInertia_[float]() - body_A = plant.AddRigidBody(name="A", M_BBo_B=M_BBo_B) - body_B = plant.AddRigidBody(name="B", M_BBo_B=M_BBo_B) + # Add ball constraint. + body_A = plant.AddRigidBody(name="A") + body_B = plant.AddRigidBody(name="B") p_AP = [0.0, 0.0, 0.0] p_BQ = [0.0, 0.0, 0.0] plant.AddBallConstraint( @@ -2554,11 +2547,8 @@ def test_constraint_active_status_api(self): plant.set_discrete_contact_approximation( DiscreteContactApproximation.kSap) - # Since we won't be performing dynamics computations, - # using garbage inertia is ok for this test. - M_BBo_B = SpatialInertia_[float]() - body_A = plant.AddRigidBody(name="A", M_BBo_B=M_BBo_B) - body_B = plant.AddRigidBody(name="B", M_BBo_B=M_BBo_B) + body_A = plant.AddRigidBody(name="A") + body_B = plant.AddRigidBody(name="B") # Add ball and distance constraints. p_AP = [0.0, 0.0, 0.0] @@ -2645,11 +2635,9 @@ def test_weld_constraint_api(self, T): plant.set_discrete_contact_approximation( DiscreteContactApproximation.kSap) - # Add weld constraint. Since we won't be performing dynamics - # computations, using garbage inertia is ok for this test. - M = SpatialInertia_[float]() - body_A = plant.AddRigidBody(name="A", M_BBo_B=M) - body_B = plant.AddRigidBody(name="B", M_BBo_B=M) + # Add weld constraint. + body_A = plant.AddRigidBody(name="A") + body_B = plant.AddRigidBody(name="B") X_AP = RigidTransform_[float]() X_BQ = RigidTransform_[float]() plant.AddWeldConstraint( @@ -2667,11 +2655,9 @@ def test_remove_constraint(self, T): plant.set_discrete_contact_approximation( DiscreteContactApproximation.kSap) - # Add weld constraint. Since we won't be performing dynamics - # computations, using garbage inertia is ok for this test. - M = SpatialInertia_[float]() - body_A = plant.AddRigidBody(name="A", M_BBo_B=M) - body_B = plant.AddRigidBody(name="B", M_BBo_B=M) + # Add weld constraint. + body_A = plant.AddRigidBody(name="A") + body_B = plant.AddRigidBody(name="B") X_AP = RigidTransform_[float]() X_BQ = RigidTransform_[float]() id = plant.AddWeldConstraint( @@ -3203,8 +3189,7 @@ def test_free_base_bodies(self): plant = MultibodyPlant_[float](time_step=0.01) model_instance = plant.AddModelInstance("new instance") added_body = plant.AddRigidBody( - name="body", model_instance=model_instance, - M_BBo_B=SpatialInertia_[float]()) + name="body", model_instance=model_instance) plant.Finalize() self.assertTrue(plant.HasBodyNamed("body", model_instance)) self.assertTrue(plant.HasUniqueFreeBaseBody(model_instance)) diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc index 218d8ba978f4..8cf312feda89 100644 --- a/bindings/pydrake/multibody/tree_py.cc +++ b/bindings/pydrake/multibody/tree_py.cc @@ -319,10 +319,13 @@ void DoScalarDependentDefinitions(py::module m, T) { BindMultibodyElementMixin(&cls); cls // BR .def(py::init&>(), - py::arg("body_name"), py::arg("M_BBo_B"), cls_doc.ctor.doc_2args) + py::arg("body_name"), + py::arg("M_BBo_B") = SpatialInertia::Zero(), + cls_doc.ctor.doc_2args) .def(py::init&>(), - py::arg("body_name"), py::arg("model_instance"), py::arg("M_BBo_B"), + py::arg("body_name"), py::arg("model_instance"), + py::arg("M_BBo_B") = SpatialInertia::Zero(), cls_doc.ctor.doc_3args) .def("name", &Class::name, cls_doc.name.doc) .def("scoped_name", &Class::scoped_name, cls_doc.scoped_name.doc) diff --git a/bindings/pydrake/multibody/tree_py_inertia.cc b/bindings/pydrake/multibody/tree_py_inertia.cc index 864dc3f8fac3..c820b459bd59 100644 --- a/bindings/pydrake/multibody/tree_py_inertia.cc +++ b/bindings/pydrake/multibody/tree_py_inertia.cc @@ -1,5 +1,6 @@ #include "drake/bindings/pydrake/common/cpp_template_pybind.h" #include "drake/bindings/pydrake/common/default_scalars_pybind.h" +#include "drake/bindings/pydrake/common/deprecation_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/bindings/pydrake/pydrake_pybind.h" #include "drake/multibody/tree/geometry_spatial_inertia.h" @@ -254,11 +255,16 @@ void DoScalarDependentDefinitions(py::module m, T) { .def_static("HollowSphereWithMass", &Class::HollowSphereWithMass, py::arg("mass"), py::arg("radius"), cls_doc.HollowSphereWithMass.doc) - .def(py::init(), cls_doc.ctor.doc_0args) + .def_static("Zero", &Class::Zero, cls_doc.Zero.doc) + .def_static("NaN", &Class::NaN, cls_doc.NaN.doc) + // TODO(jwnimmer-tri) Remove this on 2024-10-01. + .def(py::init(WrapDeprecated(cls_doc.ctor.doc_deprecated, + []() { return std::make_unique(Class::NaN()); })), + cls_doc.ctor.doc_deprecated) .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) + py::arg("skip_validity_check") = false, cls_doc.ctor.doc) // 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) diff --git a/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py b/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py index bd037b3275ce..59f06fedf35f 100644 --- a/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py +++ b/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py @@ -10,7 +10,6 @@ from pydrake.multibody.parsing import Parser from pydrake.multibody.plant import ( AddMultibodyPlantSceneGraph, CoulombFriction) -from pydrake.multibody.tree import SpatialInertia, UnitInertia from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder from pydrake.systems.planar_scenegraph_visualizer import ( @@ -105,9 +104,7 @@ def test_procedural_geometry(self): box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. - box_body = mbp.AddRigidBody("box", SpatialInertia( - mass=1.0, p_PScm_E=np.array([0., 0., 0.]), - G_SP_E=UnitInertia(1.0, 1.0, 1.0))) + box_body = mbp.AddRigidBody("box") mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry( @@ -146,9 +143,7 @@ def scene_graph_with_mesh(filename, scale=1.0): world_body = mbp.world_body() mesh_shape = Mesh(filename, scale=scale) - mesh_body = mbp.AddRigidBody("mesh", SpatialInertia( - mass=1.0, p_PScm_E=np.array([0., 0., 0.]), - G_SP_E=UnitInertia(1.0, 1.0, 1.0))) + mesh_body = mbp.AddRigidBody("mesh") mbp.WeldFrames(world_body.body_frame(), mesh_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry( diff --git a/bindings/pydrake/visualization/_model_visualizer.py b/bindings/pydrake/visualization/_model_visualizer.py index fa54d3215113..8f39f3156ed8 100644 --- a/bindings/pydrake/visualization/_model_visualizer.py +++ b/bindings/pydrake/visualization/_model_visualizer.py @@ -22,7 +22,6 @@ from pydrake.multibody.meshcat import JointSliders from pydrake.multibody.tree import ( FixedOffsetFrame, - SpatialInertia, default_model_instance, ) from pydrake.planning import RobotDiagramBuilder @@ -293,8 +292,7 @@ def Finalize(self, position=None): model_instance=default_model_instance())) sensor_body = self._builder.plant().AddRigidBody( name="$rgbd_sensor_body", - model_instance=default_model_instance(), - M_BBo_B=SpatialInertia()) + model_instance=default_model_instance()) self._builder.plant().WeldFrames( frame_on_parent_F=sensor_offset_frame, frame_on_child_M=sensor_body.body_frame()) diff --git a/examples/hydroelastic/ball_plate/make_ball_plate_plant.cc b/examples/hydroelastic/ball_plate/make_ball_plate_plant.cc index 4b558a060714..abbd7fae2b2c 100644 --- a/examples/hydroelastic/ball_plate/make_ball_plate_plant.cc +++ b/examples/hydroelastic/ball_plate/make_ball_plate_plant.cc @@ -20,7 +20,6 @@ using multibody::CoulombFriction; using multibody::MultibodyPlant; using multibody::RigidBody; using multibody::SpatialInertia; -using multibody::UnitInertia; using math::RigidTransformd; using math::RotationMatrixd; using Eigen::Vector3d; diff --git a/geometry/optimization/test/c_iris_collision_geometry_test.cc b/geometry/optimization/test/c_iris_collision_geometry_test.cc index 7bb18c8a1f9c..494b7170157c 100644 --- a/geometry/optimization/test/c_iris_collision_geometry_test.cc +++ b/geometry/optimization/test/c_iris_collision_geometry_test.cc @@ -512,17 +512,11 @@ GTEST_TEST(DistanceToHalfspace, Test) { // properties. AddContactMaterial(0.1, 250.0, multibody::CoulombFriction{0.9, 0.5}, &proximity_properties); - // C-IRIS only considers robot kinematics, not dynamics. So we use an - // arbitrary inertia. - const multibody::SpatialInertia spatial_inertia( - 1, Eigen::Vector3d::Zero(), - multibody::UnitInertia(0.01, 0.01, 0.01, 0, 0, 0)); std::vector body_indices; for (int i = 0; i < 5; ++i) { body_indices.push_back( - plant->AddRigidBody("body" + std::to_string(i), spatial_inertia) - .index()); + plant->AddRigidBody("body" + std::to_string(i)).index()); } const Eigen::Vector3d box_size(0.05, 0.02, 0.03); const geometry::GeometryId body0_box = plant->RegisterCollisionGeometry( diff --git a/geometry/optimization/test/c_iris_test_utilities.cc b/geometry/optimization/test/c_iris_test_utilities.cc index e7589bdbc443..8771ca15bd74 100644 --- a/geometry/optimization/test/c_iris_test_utilities.cc +++ b/geometry/optimization/test/c_iris_test_utilities.cc @@ -39,15 +39,8 @@ CIrisToyRobotTest::CIrisToyRobotTest() { math::RigidTransform(Eigen::Vector3d(-0.1, -0.1, 0.2)), Cylinder(0.02, 0.1), "world_cylinder", proximity_properties); - // C-IRIS only considers robot kinematics, not dynamics. So we use an - // arbitrary inertia. - const multibody::SpatialInertia spatial_inertia( - 1, Eigen::Vector3d::Zero(), - multibody::UnitInertia(0.01, 0.01, 0.01)); - // body0 - body_indices_.push_back( - plant_->AddRigidBody("body0", spatial_inertia).index()); + body_indices_.push_back(plant_->AddRigidBody("body0").index()); const multibody::RigidBody& body0 = plant_->get_body(body_indices_[0]); plant_->AddJoint( @@ -64,8 +57,7 @@ CIrisToyRobotTest::CIrisToyRobotTest() { Sphere(0.08), "body0_sphere", proximity_properties); // body1 - body_indices_.push_back( - plant_->AddRigidBody("body1", spatial_inertia).index()); + body_indices_.push_back(plant_->AddRigidBody("body1").index()); const multibody::RigidBody& body1 = plant_->get_body(body_indices_[1]); const auto& joint1 = plant_->AddJoint( @@ -88,8 +80,7 @@ CIrisToyRobotTest::CIrisToyRobotTest() { Convex(convex_obj), "body1_convex", proximity_properties); // body2 - body_indices_.push_back( - plant_->AddRigidBody("body2", spatial_inertia).index()); + body_indices_.push_back(plant_->AddRigidBody("body2").index()); const auto& body2 = plant_->get_body(body_indices_[2]); const auto& joint2 = plant_->AddJoint( "joint2", body1, math::RigidTransformd(Eigen::Vector3d(0.2, 0, 0)), body2, @@ -106,8 +97,7 @@ CIrisToyRobotTest::CIrisToyRobotTest() { Sphere(0.07), "body2_sphere", proximity_properties); // body3 - body_indices_.push_back( - plant_->AddRigidBody("body3", spatial_inertia).index()); + body_indices_.push_back(plant_->AddRigidBody("body3").index()); const auto& body3 = plant_->get_body(body_indices_[3]); const auto& joint3 = plant_->AddJoint( "joint3", body0, math::RigidTransformd(Eigen::Vector3d(0, 0.05, 0.1)), @@ -168,13 +158,7 @@ CIrisRobotPolytopicGeometryTest::CIrisRobotPolytopicGeometryTest() { math::RigidTransform(Eigen::Vector3d(-0.1, -0.5, 0.2)), Convex(convex_obj), "world_convex", proximity_properties); - // C-IRIS only considers robot kinematics, not dynamics. So we use an - // arbitrary inertia. - const multibody::SpatialInertia spatial_inertia( - 1, Eigen::Vector3d::Zero(), - multibody::UnitInertia(0.01, 0.01, 0.01, 0, 0, 0)); - - auto add_body = [this, &spatial_inertia, &proximity_properties]( + auto add_body = [this, &proximity_properties]( const math::RigidTransformd& X_PF, const math::RigidTransformd& X_BM, const Eigen::Vector3d& axis, double theta_lower, @@ -186,8 +170,7 @@ CIrisRobotPolytopicGeometryTest::CIrisRobotPolytopicGeometryTest() { ? this->plant_->world_body() : this->plant_->get_body(this->body_indices_.back()); this->body_indices_.push_back( - this->plant_ - ->AddRigidBody("body" + std::to_string(body_index), spatial_inertia) + this->plant_->AddRigidBody("body" + std::to_string(body_index)) .index()); const auto& body = this->plant_->get_body(this->body_indices_.back()); diff --git a/multibody/optimization/test/optimization_with_contact_utilities.h b/multibody/optimization/test/optimization_with_contact_utilities.h index b05ef3527042..1ae91d3835a4 100644 --- a/multibody/optimization/test/optimization_with_contact_utilities.h +++ b/multibody/optimization/test/optimization_with_contact_utilities.h @@ -16,12 +16,10 @@ namespace test { struct SphereSpecification { SphereSpecification(double radius_in, double density, CoulombFriction friction_in) - : radius{radius_in}, friction{std::move(friction_in)} { - const double mass = 4.0 / 3 * M_PI * std::pow(radius, 3) * density; - const double I = 2.0 / 5.0 * mass * std::pow(radius, 2); // inertia - inertia = SpatialInertia(mass, Eigen::Vector3d::Zero(), - UnitInertia(I, I, I)); - } + : radius{radius_in}, + inertia{ + SpatialInertia::SolidSphereWithDensity(density, radius_in)}, + friction{std::move(friction_in)} {} double radius; SpatialInertia inertia; CoulombFriction friction; @@ -32,15 +30,10 @@ struct BoxSpecification { CoulombFriction friction_in, std::optional X_WB_in) : size(std::move(size_in)), + inertia{SpatialInertia::SolidBoxWithDensity(density, size[0], + size[1], size[2])}, friction{std::move(friction_in)}, - X_WB(std::move(X_WB_in)) { - const double mass = size(0) * size(1) * size(2) * density; - const UnitInertia I( - 1.0 / 12 * (size(1) * size(1) + size(2) * size(2)), - 1.0 / 12 * (size(0) * size(0) + size(2) * size(2)), - 1.0 / 12 * (size(0) * size(0) + size(1) * size(1))); - inertia = SpatialInertia(mass, Eigen::Vector3d::Zero(), I); - } + X_WB(std::move(X_WB_in)) {} // Full dimensions of a box (not the half dimensions). Eigen::Vector3d size; SpatialInertia inertia; diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc index 8480a9664618..76b8f18f50c6 100644 --- a/multibody/parsing/detail_mujoco_parser.cc +++ b/multibody/parsing/detail_mujoco_parser.cc @@ -409,7 +409,7 @@ class MujocoParser { private: const std::map>& mesh_inertia_; std::string name_; - SpatialInertia M_GG_G_; + SpatialInertia M_GG_G_{SpatialInertia::NaN()}; }; SpatialInertia ParseInertial(XMLElement* node) { @@ -419,7 +419,7 @@ class MujocoParser { double mass; if (!ParseScalarAttribute(node, "mass", &mass)) { Error(*node, "The inertial tag must include the mass attribute."); - return {}; + return SpatialInertia::NaN(); } // We interpret the MuJoCo XML documentation as saying that if a @@ -441,7 +441,7 @@ class MujocoParser { Error(*node, "The inertial tag must include either the diaginertia or " "fullinertia attribute."); - return {}; + return SpatialInertia::NaN(); } } @@ -457,7 +457,7 @@ class MujocoParser { std::unique_ptr shape{}; Vector4d rgba{.5, .5, .5, 1}; CoulombFriction friction{1.0, 1.0}; - SpatialInertia M_GBo_B{}; + SpatialInertia M_GBo_B{SpatialInertia::NaN()}; bool register_collision{true}; bool register_visual{true}; }; @@ -844,7 +844,7 @@ class MujocoParser { if (joint_node->NextSiblingElement("joint")) { const RigidBody& dummy_body = plant_->AddRigidBody( fmt::format("{}{}", body_name, dummy_bodies++), model_instance_, - SpatialInertia(0, {0, 0, 0}, {0, 0, 0})); + SpatialInertia::Zero()); ParseJoint(joint_node, *last_body, dummy_body, X_WP, RigidTransformd(), child_class); last_body = &dummy_body; @@ -1080,7 +1080,8 @@ class MujocoParser { if (std::filesystem::exists(filename)) { mesh_[name] = std::make_unique(filename, scale[0]); - mesh_inertia_[name] = CalcSpatialInertia(*mesh_[name], 1); + mesh_inertia_.insert_or_assign(name, + CalcSpatialInertia(*mesh_[name], 1)); } else if (std::filesystem::exists(original_filename)) { Warning( *node, diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 0af50f2b4817..c74d9f4417e8 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -214,12 +214,9 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { " the world link. The tag is being ignored."); } } else { - SpatialInertia M_BBo_B; + auto M_BBo_B = SpatialInertia::Zero(); XMLElement* inertial_node = node->FirstChildElement("inertial"); - if (!inertial_node) { - M_BBo_B = SpatialInertia(0, Vector3d::Zero(), - UnitInertia(0, 0, 0)); - } else { + if (inertial_node) { M_BBo_B = ExtractSpatialInertiaAboutBoExpressedInB(inertial_node); } diff --git a/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc b/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc index 0eb9d752918b..4726ee8c1444 100644 --- a/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc +++ b/multibody/parsing/test/detail_collision_filter_group_resolver_test.cc @@ -29,9 +29,8 @@ class CollisionFilterGroupResolverTest : public test::DiagnosticPolicyTestBase { std::optional model_instance) { auto model = model_instance.value_or(default_model_instance()); return plant_.RegisterCollisionGeometry( - plant_.AddRigidBody(name, model, SpatialInertia()), - RigidTransformd::Identity(), geometry::Sphere(1.0), name, - CoulombFriction()); + plant_.AddRigidBody(name, model), RigidTransformd::Identity(), + geometry::Sphere(1.0), name, CoulombFriction()); } protected: diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 3763ce0afc77..20d5340e21d8 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -1159,12 +1159,12 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// @param[in] M_BBo_B /// The SpatialInertia of the new rigid body to be added to `this` /// %MultibodyPlant, computed about the body frame origin `Bo` and expressed - /// in the body frame B. + /// in the body frame B. When not provided, defaults to zero. /// @returns A constant reference to the new RigidBody just added, which will /// remain valid for the lifetime of `this` %MultibodyPlant. - const RigidBody& AddRigidBody(const std::string& name, - ModelInstanceIndex model_instance, - const SpatialInertia& M_BBo_B) { + const RigidBody& AddRigidBody( + const std::string& name, ModelInstanceIndex model_instance, + const SpatialInertia& M_BBo_B = SpatialInertia::Zero()) { DRAKE_MBP_THROW_IF_FINALIZED(); // Add the actual rigid body to the model. const RigidBody& body = @@ -1204,13 +1204,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// @param[in] M_BBo_B /// The SpatialInertia of the new rigid body to be added to `this` /// %MultibodyPlant, computed about the body frame origin `Bo` and expressed - /// in the body frame B. + /// in the body frame B. When not provided, defaults to zero. /// @returns A constant reference to the new RigidBody just added, which will /// remain valid for the lifetime of `this` %MultibodyPlant. /// @throws std::exception if additional model instances have been created /// beyond the world and default instances. - const RigidBody& AddRigidBody(const std::string& name, - const SpatialInertia& M_BBo_B) { + const RigidBody& AddRigidBody( + const std::string& name, + const SpatialInertia& M_BBo_B = SpatialInertia::Zero()) { if (num_model_instances() != 2) { throw std::logic_error( "This model has more model instances than the default. Please " diff --git a/multibody/plant/test/contact_results_to_lcm_test.cc b/multibody/plant/test/contact_results_to_lcm_test.cc index 8ba3d2a18f1a..e1e3427cae74 100644 --- a/multibody/plant/test/contact_results_to_lcm_test.cc +++ b/multibody/plant/test/contact_results_to_lcm_test.cc @@ -277,9 +277,7 @@ class ContactResultsToLcmTest : public ::testing::Test { MultibodyPlant* plant, vector* body_names, unordered_map* id_to_body, FullBodyName ref_name) { - // To avoid unnecessary warnings/errors, use a non-zero spatial inertia. - const auto& body = plant->AddRigidBody( - body_name, model_index, SpatialInertia::MakeUnitary()); + const auto& body = plant->AddRigidBody(body_name, model_index); /* The expected format based on knowledge of the ContactResultToLcmSystem's implementation. */ body_names->push_back(fmt::format("{}({})", body_name, model_index)); @@ -844,9 +842,7 @@ class ConnectVisualizerTest : public ::testing::Test { plant_ = &system_pair.plant; scene_graph_ = &system_pair.scene_graph; - // To avoid unnecessary warnings/errors, use a non-zero spatial inertia. - const auto& body = - plant_->AddRigidBody("link", SpatialInertia::MakeUnitary()); + const auto& body = plant_->AddRigidBody("link"); plant_->RegisterCollisionGeometry(body, {}, Sphere(1.0), kGeoName, CoulombFriction{}); plant_->Finalize(); diff --git a/multibody/plant/test/deformable_driver_contact_kinematics_test.cc b/multibody/plant/test/deformable_driver_contact_kinematics_test.cc index c2c951b2cc7e..728842dd5647 100644 --- a/multibody/plant/test/deformable_driver_contact_kinematics_test.cc +++ b/multibody/plant/test/deformable_driver_contact_kinematics_test.cc @@ -118,7 +118,7 @@ class DeformableDriverContactKinematicsTest Fz and the contact normal is defined to point into the rigid body (in the -Fz direction), so Cz = -Fz. */ const RigidBody& rigid_body = - plant_->AddRigidBody("rigid_body", SpatialInertia()); + plant_->AddRigidBody("rigid_body", SpatialInertia::NaN()); rigid_geometry_id_ = plant_->RegisterCollisionGeometry( rigid_body, X_BR, geometry::Box(10, 10, 1), "dynamic_collision_geometry", rigid_proximity_props); diff --git a/multibody/plant/test/deformable_model_test.cc b/multibody/plant/test/deformable_model_test.cc index fa6d2acbcc8f..0ffe42c84b1f 100644 --- a/multibody/plant/test/deformable_model_test.cc +++ b/multibody/plant/test/deformable_model_test.cc @@ -271,7 +271,7 @@ TEST_F(DeformableModelTest, AddFixedConstraint) { DeformableBodyId deformable_id = RegisterSphere(100, X_WA); ASSERT_EQ(deformable_model_ptr_->GetFemModel(deformable_id).num_nodes(), 7); const RigidBody& rigid_body = - plant_->AddRigidBody("box", SpatialInertia()); + plant_->AddRigidBody("box", SpatialInertia::NaN()); geometry::Box box(1.0, 1.0, 1.0); const RigidTransformd X_BA(Vector3d(-2, 0, 0)); const RigidTransformd X_BG(Vector3d(-1, 0, 0)); @@ -309,7 +309,7 @@ TEST_F(DeformableModelTest, AddFixedConstraint) { /* Non-existant rigid body (registered with a different MbP). */ MultibodyPlant other_plant(0.0); const RigidBody& wrong_rigid_body = - other_plant.AddRigidBody("wrong body", SpatialInertia()); + other_plant.AddRigidBody("wrong body", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( deformable_model_ptr_->AddFixedConstraint(deformable_id, wrong_rigid_body, X_BA, box, X_BG), diff --git a/multibody/plant/test/multibody_plant_introspection_test.cc b/multibody/plant/test/multibody_plant_introspection_test.cc index 17658e814b5a..d2ebc926aee1 100644 --- a/multibody/plant/test/multibody_plant_introspection_test.cc +++ b/multibody/plant/test/multibody_plant_introspection_test.cc @@ -167,7 +167,7 @@ GTEST_TEST(MultibodyPlantIntrospection, NonUniqueBaseBody) { plant.AddRigidBody("free_body", default_model_instance(), SpatialInertia::MakeUnitary()); const RigidBody& fixed_body = plant.AddRigidBody( - "fixed_body", default_model_instance(), SpatialInertia()); + "fixed_body", default_model_instance(), SpatialInertia::NaN()); plant.WeldFrames(plant.world_frame(), fixed_body.body_frame()); plant.Finalize(); // Even though there is only one free body, the base body is not unique. diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index d96b0a22a721..429b1083e633 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -350,7 +350,7 @@ GTEST_TEST(MultibodyPlant, SimpleModelCreation) { // Therefore no more modeling elements can be added. Verify this. DRAKE_EXPECT_THROWS_MESSAGE( plant->AddRigidBody("AnotherBody", default_model_instance(), - SpatialInertia()), + SpatialInertia::NaN()), "Post-finalize calls to '.*' are not allowed; " "calls to this method must happen before Finalize\\(\\)."); DRAKE_EXPECT_THROWS_MESSAGE( @@ -392,7 +392,7 @@ GTEST_TEST(MultibodyPlant, SimpleModelCreation) { GTEST_TEST(MultibodyPlantTest, AddJointActuator) { MultibodyPlant plant(0.0); ModelInstanceIndex model_instance = plant.AddModelInstance("instance"); - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); auto body = &plant.AddRigidBody("body", model_instance, M_B); const Joint& planar_joint = plant.AddJoint(std::make_unique>( @@ -1412,7 +1412,7 @@ class SphereChainScenario { GTEST_TEST(MultibodyPlantTest, AutoBodySceneGraphRegistration) { MultibodyPlant plant(0.0); const RigidBody& body1 = - plant.AddRigidBody("body1", SpatialInertia()); + plant.AddRigidBody("body1", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.GetBodyFrameIdOrThrow(body1.index()), "Body 'body1' does not have geometry registered with it."); @@ -1427,7 +1427,7 @@ GTEST_TEST(MultibodyPlantTest, AutoBodySceneGraphRegistration) { // And new bodies have FrameIds immediately upon creation. const RigidBody& body2 = - plant.AddRigidBody("body2", SpatialInertia()); + plant.AddRigidBody("body2", SpatialInertia::NaN()); DRAKE_EXPECT_NO_THROW(plant.GetBodyFrameIdOrThrow(body2.index())); } @@ -1622,7 +1622,7 @@ GTEST_TEST(MultibodyPlantTest, GetBodiesWeldedTo) { // Add a new body, and weld it using `WeldFrames` (to ensure that topology is // updated via this API). const RigidBody& extra = plant.AddRigidBody( - "extra", default_model_instance(), SpatialInertia()); + "extra", default_model_instance(), SpatialInertia::NaN()); plant.WeldFrames(plant.world_frame(), extra.body_frame()); // Verify we can call GetBodiesWeldedTo() pre-finalize. @@ -1668,7 +1668,7 @@ GTEST_TEST(MultibodyPlantTest, GetBodiesKinematicallyAffectedBy) { const JointIndex elbow = plant.GetJointByName("weld").index(); // Add a new body, and weld it to the world body. const RigidBody& extra = plant.AddRigidBody( - "extra", default_model_instance(), SpatialInertia()); + "extra", default_model_instance(), SpatialInertia::NaN()); plant.WeldFrames(plant.world_frame(), extra.body_frame()); const std::vector joints1{shoulder}; @@ -1704,7 +1704,7 @@ GTEST_TEST(MultibodyPlantTest, ReversedWeldError) { // Add a new body, and weld it in the wrong direction using `WeldFrames`. const RigidBody& extra = plant.AddRigidBody( - "extra", default_model_instance(), SpatialInertia()); + "extra", default_model_instance(), SpatialInertia::NaN()); plant.WeldFrames(extra.body_frame(), plant.world_frame()); // The important property of this message is that it reports some identifier @@ -2320,7 +2320,7 @@ TEST_F(SplitPendulum, GetMultibodyPlantFromElement) { struct MyMBSystem : public internal::MultibodyTreeSystem { MyMBSystem() { rigid_body = - &mutable_tree().AddRigidBody("Body", SpatialInertia()); + &mutable_tree().AddRigidBody("Body", SpatialInertia::NaN()); Finalize(); } const RigidBody* rigid_body{}; @@ -3588,6 +3588,12 @@ GTEST_TEST(MultibodyPlantTest, RigidBodyParameters) { const RigidBody& cube = plant.AddRigidBody( "cube", SpatialInertia(cube_mass, cube_com, cube_unit_inertia)); + // These rigid bodies test the AddRigidBody overloads that do not specify any + // inertia. + const RigidBody& simple_1 = plant.AddRigidBody("simple_1"); + const RigidBody& simple_2 = + plant.AddRigidBody("simple_2", default_model_instance()); + plant.Finalize(); // Create a default context. @@ -3628,6 +3634,13 @@ GTEST_TEST(MultibodyPlantTest, RigidBodyParameters) { CompareMatrices(cube_inertia_in_context.get_unit_inertia().get_products(), cube_unit_inertia.get_products())); + // The default inertia should be zero. + for (const auto* simple : {&simple_1, &simple_2}) { + EXPECT_GE(simple->get_mass(*context), 0.0); + EXPECT_TRUE( + simple->CalcSpatialInertiaInBodyFrame(*context).IsPhysicallyValid()); + } + // Change parameters. const double new_sphere_radius = 1.5; const double new_sphere_mass = 3.9; @@ -3875,9 +3888,9 @@ GTEST_TEST(MultibodyPlantTests, ConstraintActiveStatus) { // arbitrarily choose one model approximation that uses the SAP solver. plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& body_A = - plant.AddRigidBody("body_A", SpatialInertia{}); + plant.AddRigidBody("body_A", SpatialInertia::NaN()); const RigidBody& body_B = - plant.AddRigidBody("body_B", SpatialInertia{}); + plant.AddRigidBody("body_B", SpatialInertia::NaN()); const RevoluteJoint& world_A = plant.AddJoint("world_A", plant.world_body(), std::nullopt, body_A, std::nullopt, Vector3d::UnitZ()); @@ -3944,9 +3957,9 @@ GTEST_TEST(MultibodyPlantTests, RemoveConstraint) { // arbitrarily choose one model approximation that uses the SAP solver. plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const Body& body_A = - plant.AddRigidBody("body_A", SpatialInertia{}); + plant.AddRigidBody("body_A", SpatialInertia::NaN()); const Body& body_B = - plant.AddRigidBody("body_B", SpatialInertia{}); + plant.AddRigidBody("body_B", SpatialInertia::NaN()); const RevoluteJoint& world_A = plant.AddJoint("world_A", plant.world_body(), std::nullopt, body_A, std::nullopt, Vector3d::UnitZ()); @@ -3992,7 +4005,7 @@ GTEST_TEST(MultibodyPlantTests, RemoveConstraint) { GTEST_TEST(MultibodyPlantTests, FixedOffsetFrameFunctions) { MultibodyPlant plant(0.0); const RigidBody& body_B = - plant.AddRigidBody("body_B", SpatialInertia{}); + plant.AddRigidBody("body_B", SpatialInertia::NaN()); // Weld body B to the world W which creates a fixed offset frame Wp fixed to // the world and a fixed offset frame P fixed to body B. diff --git a/multibody/plant/test/sap_driver_ball_constraints_test.cc b/multibody/plant/test/sap_driver_ball_constraints_test.cc index f393c525c257..188296ef5407 100644 --- a/multibody/plant/test/sap_driver_ball_constraints_test.cc +++ b/multibody/plant/test/sap_driver_ball_constraints_test.cc @@ -240,9 +240,9 @@ GTEST_TEST(BallConstraintsTests, VerifyIdMapping) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); const Vector3d p_AP(1, 2, 3); const Vector3d p_BQ(4, 5, 6); MultibodyConstraintId ball_id = @@ -283,9 +283,9 @@ GTEST_TEST(BallConstraintTests, FailOnTAMSI) { plant.set_discrete_contact_approximation( DiscreteContactApproximation::kTamsi); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE(plant.AddBallConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, Vector3d{0, 0, 0}), ".*TAMSI does not support ball constraints.*"); @@ -294,9 +294,9 @@ GTEST_TEST(BallConstraintTests, FailOnTAMSI) { GTEST_TEST(BallConstraintTests, FailOnContinuous) { MultibodyPlant plant{0.0}; const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddBallConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, Vector3d{0, 0, 0}), @@ -308,9 +308,9 @@ GTEST_TEST(BallConstraintTests, FailOnFinalized) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); plant.Finalize(); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddBallConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, @@ -322,7 +322,7 @@ GTEST_TEST(BallConstraintTests, FailOnSameBody) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddBallConstraint(bodyA, Vector3d{0, 0, 0}, bodyA, Vector3d{0, 0, 0}), diff --git a/multibody/plant/test/sap_driver_distance_constraints_test.cc b/multibody/plant/test/sap_driver_distance_constraints_test.cc index 91e5b81b67b7..07c010ee407d 100644 --- a/multibody/plant/test/sap_driver_distance_constraints_test.cc +++ b/multibody/plant/test/sap_driver_distance_constraints_test.cc @@ -242,9 +242,9 @@ GTEST_TEST(DistanceConstraintsTests, VerifyIdMapping) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); const Vector3d p_AP(1, 2, 3); const Vector3d p_BQ(4, 5, 6); const double distance = 1.2; @@ -289,9 +289,9 @@ GTEST_TEST(DistanceConstraintTests, FailOnTAMSI) { plant.set_discrete_contact_approximation( DiscreteContactApproximation::kTamsi); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddDistanceConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, Vector3d{0, 0, 0}, 1 /* distance */), @@ -301,9 +301,9 @@ GTEST_TEST(DistanceConstraintTests, FailOnTAMSI) { GTEST_TEST(DistanceConstraintTests, FailOnContinuous) { MultibodyPlant plant{0.0}; const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddDistanceConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, Vector3d{0, 0, 0}, 1 /* distance */), @@ -315,9 +315,9 @@ GTEST_TEST(DistanceConstraintTests, FailOnFinalized) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); plant.Finalize(); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddDistanceConstraint(bodyA, Vector3d{0, 0, 0}, bodyB, @@ -330,9 +330,9 @@ GTEST_TEST(DistanceConstraintTests, FailOnInvalidSpecs) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); // Fail on same body. DRAKE_EXPECT_THROWS_MESSAGE( plant.AddDistanceConstraint(bodyA, Vector3d{0, 0, 0}, bodyA, diff --git a/multibody/plant/test/sap_driver_weld_constraints_test.cc b/multibody/plant/test/sap_driver_weld_constraints_test.cc index c9c73b08a5a8..d759397e913d 100644 --- a/multibody/plant/test/sap_driver_weld_constraints_test.cc +++ b/multibody/plant/test/sap_driver_weld_constraints_test.cc @@ -279,9 +279,9 @@ GTEST_TEST(WeldConstraintsTests, VerifyIdMapping) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); const RigidTransformd X_AP(Vector3d(1, 2, 3)); const RigidTransformd X_BQ(Vector3d(4, 5, 6)); MultibodyConstraintId weld_id = @@ -322,9 +322,9 @@ GTEST_TEST(BallConstraintTests, FailOnTAMSI) { plant.set_discrete_contact_approximation( DiscreteContactApproximation::kTamsi); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE(plant.AddWeldConstraint(bodyA, RigidTransformd(), bodyB, RigidTransformd()), ".*TAMSI does not support weld constraints.*"); @@ -333,9 +333,9 @@ GTEST_TEST(BallConstraintTests, FailOnTAMSI) { GTEST_TEST(WeldConstraintTests, FailOnContinuous) { MultibodyPlant plant{0.0}; const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddWeldConstraint(bodyA, RigidTransformd{Vector3d{0, 0, 0}}, bodyB, RigidTransformd{Vector3d{0, 0, 0}}), @@ -347,9 +347,9 @@ GTEST_TEST(WeldConstraintTests, FailOnFinalized) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); const RigidBody& bodyB = - plant.AddRigidBody("B", SpatialInertia{}); + plant.AddRigidBody("B", SpatialInertia::NaN()); plant.Finalize(); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddWeldConstraint(bodyA, RigidTransformd{Vector3d{0, 0, 0}}, bodyB, @@ -361,7 +361,7 @@ GTEST_TEST(WeldConstraintTests, FailOnSameBody) { MultibodyPlant plant{0.1}; plant.set_discrete_contact_approximation(DiscreteContactApproximation::kSap); const RigidBody& bodyA = - plant.AddRigidBody("A", SpatialInertia{}); + plant.AddRigidBody("A", SpatialInertia::NaN()); DRAKE_EXPECT_THROWS_MESSAGE( plant.AddWeldConstraint(bodyA, RigidTransformd{Vector3d{0, 0, 0}}, bodyA, RigidTransformd{Vector3d{0, 0, 0}}), diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 432aca5f8b65..1f1569b1cdc8 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -74,7 +74,7 @@ MultibodyTree::MultibodyTree() { DRAKE_DEMAND(world_instance == world_model_instance()); world_rigid_body_ = &AddRigidBody("world", world_model_instance(), - SpatialInertia()); + SpatialInertia::NaN()); // `default_model_instance()` hardcodes the returned index. Make sure it's // correct. diff --git a/multibody/tree/multibody_tree_system.cc b/multibody/tree/multibody_tree_system.cc index 658316753f40..e337e6ce83a9 100644 --- a/multibody/tree/multibody_tree_system.cc +++ b/multibody/tree/multibody_tree_system.cc @@ -219,7 +219,8 @@ void MultibodyTreeSystem::Finalize() { // Allocate cache entry to store spatial inertia M_B_W(q) for each body. cache_indexes_.spatial_inertia_in_world = this->DeclareCacheEntry( std::string("spatial inertia in world (M_B_W)"), - std::vector>(internal_tree().num_bodies()), + std::vector>(internal_tree().num_bodies(), + SpatialInertia::NaN()), &MultibodyTreeSystem::CalcSpatialInertiasInWorld, {position_kinematics_cache_entry().ticket()}).cache_index(); @@ -239,7 +240,8 @@ void MultibodyTreeSystem::Finalize() { // Allocate cache entry for composite-body inertias Mc_B_W(q) for each body. cache_indexes_.composite_body_inertia_in_world = this->DeclareCacheEntry( std::string("composite body inertia in world (Mc_B_W)"), - std::vector>(internal_tree().num_bodies()), + std::vector>(internal_tree().num_bodies(), + SpatialInertia::NaN()), &MultibodyTreeSystem::CalcCompositeBodyInertiasInWorld, {position_kinematics_cache_entry().ticket()}).cache_index(); diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index 92cbbb9817aa..3724d09af052 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -197,11 +197,12 @@ class RigidBody : public MultibodyElement { /// A name associated with this body. /// @param[in] M_BBo_B /// Spatial inertia of this body B about the frame's origin `Bo` and - /// expressed in the body frame B. + /// expressed in the body frame B. When not provided, defaults to zero. /// @note See @ref multibody_spatial_inertia for details on the monogram /// notation used for spatial inertia quantities. - RigidBody(const std::string& body_name, - const SpatialInertia& M_BBo_B); + explicit RigidBody( + const std::string& body_name, + const SpatialInertia& M_BBo_B = SpatialInertia::Zero()); /// Constructs a %RigidBody named `body_name` with the given default /// SpatialInertia. @@ -212,12 +213,12 @@ class RigidBody : public MultibodyElement { /// The model instance associated with this body. /// @param[in] M_BBo_B /// Spatial inertia of this body B about the frame's origin `Bo` and - /// expressed in the body frame B. + /// expressed in the body frame B. When not provided, defaults to zero. /// @note See @ref multibody_spatial_inertia for details on the monogram /// notation used for spatial inertia quantities. - RigidBody(const std::string& body_name, - ModelInstanceIndex model_instance, - const SpatialInertia& M_BBo_B); + RigidBody( + const std::string& body_name, ModelInstanceIndex model_instance, + const SpatialInertia& M_BBo_B = SpatialInertia::Zero()); /// Returns this element's unique index. BodyIndex index() const { return this->template index_impl(); } diff --git a/multibody/tree/spatial_inertia.h b/multibody/tree/spatial_inertia.h index 234645e7f3a2..d3b4d4ac3c66 100644 --- a/multibody/tree/spatial_inertia.h +++ b/multibody/tree/spatial_inertia.h @@ -11,6 +11,7 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_bool.h" #include "drake/common/drake_copyable.h" +#include "drake/common/drake_deprecated.h" #include "drake/common/eigen_types.h" #include "drake/common/fmt_ostream.h" #include "drake/common/text_logging.h" @@ -463,10 +464,28 @@ class SpatialInertia { const T& density, const Vector3& p1, const Vector3& p2, const Vector3& p3); - /// Default SpatialInertia constructor initializes mass, center of mass and - /// rotational inertia to invalid NaN's for a quick detection of - /// uninitialized values. - SpatialInertia() {} + /// Initializes mass, center of mass and rotational inertia to zero. + static SpatialInertia Zero() { + return SpatialInertia(0, Vector3::Zero(), UnitInertia(0, 0, 0)); + } + + /// Initializes mass, center of mass and rotational inertia to invalid NaN's + /// for a quick detection of uninitialized values. + static SpatialInertia NaN() { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return SpatialInertia(); +#pragma GCC diagnostic pop + } + + DRAKE_DEPRECATED( + "2024-10-01", + "The default constructor is deprecated. If you really want NaNs, then " + "call the SpatialInertia::NaN() factory function. If you were were " + "only calling this constructor as part of adding a RigidBody to " + "MultibodyPlant, then instead you should omit the SpatialInertia " + "argument from MultibodyPlant::AddRigidBody(); it now defaults to zero.") + SpatialInertia() = default; /// Constructs a spatial inertia for a physical body or composite body S about /// a point P from a given mass, center of mass and rotational inertia. The diff --git a/multibody/tree/test/articulated_body_algorithm_test.cc b/multibody/tree/test/articulated_body_algorithm_test.cc index 997d40df0c8c..52267a0f39c8 100644 --- a/multibody/tree/test/articulated_body_algorithm_test.cc +++ b/multibody/tree/test/articulated_body_algorithm_test.cc @@ -66,9 +66,8 @@ GTEST_TEST(ArticulatedBodyInertiaAlgorithm, FeatherstoneExample) { box_link, {}); // Add a massless body that can rotate about x. - const RigidBody& massless_link = tree.AddRigidBody( - "massless", SpatialInertia(0, Vector3d::Zero(), - UnitInertia(0, 0, 0))); + const RigidBody& massless_link = + tree.AddRigidBody("massless", SpatialInertia::Zero()); tree.AddJoint( "revolute", box_link, {}, massless_link, {}, Vector3d(1, 0, 0)); @@ -176,9 +175,8 @@ GTEST_TEST(ArticulatedBodyInertiaAlgorithm, ModifiedFeatherstoneExample) { tree.AddJoint("ball", tree.world_body(), {}, box_link, {}); // Add a massless body that can rotate about x. - const RigidBody& massless_link = tree.AddRigidBody( - "massless", SpatialInertia(0, Vector3d::Zero(), - UnitInertia(0, 0, 0))); + const RigidBody& massless_link = + tree.AddRigidBody("massless", SpatialInertia::Zero()); const auto& BM_joint = tree.AddJoint( "revolute", box_link, {}, massless_link, {}, Vector3d(1, 0, 0)); diff --git a/multibody/tree/test/ball_rpy_joint_test.cc b/multibody/tree/test/ball_rpy_joint_test.cc index e9982f00d496..4aa741d06984 100644 --- a/multibody/tree/test/ball_rpy_joint_test.cc +++ b/multibody/tree/test/ball_rpy_joint_test.cc @@ -32,7 +32,7 @@ class BallRpyJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/body_node_test.cc b/multibody/tree/test/body_node_test.cc index 56c37037a84b..c4a1eac183ee 100644 --- a/multibody/tree/test/body_node_test.cc +++ b/multibody/tree/test/body_node_test.cc @@ -54,7 +54,7 @@ class DummyBody : public RigidBody { public: DummyBody(std::string name, BodyIndex index) : RigidBody(std::move(name), ModelInstanceIndex(0), - SpatialInertia()) { + SpatialInertia::NaN()) { // We need a body index for the body node test to be happy. MultibodyElementTester::set_index(this, index); } diff --git a/multibody/tree/test/frames_test.cc b/multibody/tree/test/frames_test.cc index 32a447ad34ab..fa70d2de1e41 100644 --- a/multibody/tree/test/frames_test.cc +++ b/multibody/tree/test/frames_test.cc @@ -43,7 +43,7 @@ class FrameTests : public ::testing::Test { // Using a NaN spatial inertia is ok in this unit test since all // computations only relate to frame kinematics (and therefore mass // properties do not play any role.) - SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/joint_actuator_test.cc b/multibody/tree/test/joint_actuator_test.cc index 45ec2ef22f0d..b3b08e86d91e 100644 --- a/multibody/tree/test/joint_actuator_test.cc +++ b/multibody/tree/test/joint_actuator_test.cc @@ -25,7 +25,7 @@ GTEST_TEST(JointActuatorTest, JointActuatorLimitTest) { // Spatial inertia for adding body. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Add bodies so we can add joints to them. const auto body1 = &tree.AddRigidBody("body1", M_B); diff --git a/multibody/tree/test/linear_spring_damper_test.cc b/multibody/tree/test/linear_spring_damper_test.cc index b79dea586102..342d17b0b1bd 100644 --- a/multibody/tree/test/linear_spring_damper_test.cc +++ b/multibody/tree/test/linear_spring_damper_test.cc @@ -93,8 +93,10 @@ class SpringDamperTester : public ::testing::Test { GTEST_TEST(LinearSpringDamper, BadParameters) { MultibodyPlant plant{0.}; - const auto& bodyA = plant.AddRigidBody("BodyA", SpatialInertia()); - const auto& bodyB = plant.AddRigidBody("BodyB", SpatialInertia()); + const auto& bodyA = + plant.AddRigidBody("BodyA", SpatialInertia::NaN()); + const auto& bodyB = + plant.AddRigidBody("BodyB", SpatialInertia::NaN()); // These are reasonable parameters. const double free_length{1.5}; // [m] diff --git a/multibody/tree/test/mobilizer_tester.h b/multibody/tree/test/mobilizer_tester.h index 030022f87e78..8c3300028d81 100644 --- a/multibody/tree/test/mobilizer_tester.h +++ b/multibody/tree/test/mobilizer_tester.h @@ -26,7 +26,7 @@ class MobilizerTester : public ::testing::Test { MobilizerTester() { // Spatial inertia for adding a body. The actual value is not important for // these tests since they are all kinematic. - const SpatialInertia M_B; + const auto M_B = SpatialInertia::NaN(); // Create an empty model. owned_tree_ = std::make_unique>(); diff --git a/multibody/tree/test/model_instance_test.cc b/multibody/tree/test/model_instance_test.cc index e5590573b98e..a9a6fc5fa37a 100644 --- a/multibody/tree/test/model_instance_test.cc +++ b/multibody/tree/test/model_instance_test.cc @@ -28,11 +28,11 @@ GTEST_TEST(ModelInstance, ModelInstanceTest) { const ModelInstanceIndex instance1 = tree.AddModelInstance("instance1"); const RigidBody& body1 = - tree.AddRigidBody("Body1", instance1, SpatialInertia()); + tree.AddRigidBody("Body1", instance1, SpatialInertia::NaN()); const RigidBody& body2 = - tree.AddRigidBody("Body2", instance1, SpatialInertia()); + tree.AddRigidBody("Body2", instance1, SpatialInertia::NaN()); const RigidBody& body3 = - tree.AddRigidBody("Body3", instance1, SpatialInertia()); + tree.AddRigidBody("Body3", instance1, SpatialInertia::NaN()); const auto& weld1 = tree.AddJoint( "weld1", tree.world_body(), math::RigidTransformd::Identity(), @@ -57,9 +57,9 @@ GTEST_TEST(ModelInstance, ModelInstanceTest) { const ModelInstanceIndex instance2 = tree.AddModelInstance("instance2"); const RigidBody& body4 = - tree.AddRigidBody("Body4", instance2, SpatialInertia()); + tree.AddRigidBody("Body4", instance2, SpatialInertia::NaN()); const RigidBody& body5 = - tree.AddRigidBody("Body5", instance2, SpatialInertia()); + tree.AddRigidBody("Body5", instance2, SpatialInertia::NaN()); const Joint& body4_body5 = tree.AddJoint( diff --git a/multibody/tree/test/multibody_forces_test.cc b/multibody/tree/test/multibody_forces_test.cc index 395c7bbb02d9..b1ba81f2920b 100644 --- a/multibody/tree/test/multibody_forces_test.cc +++ b/multibody/tree/test/multibody_forces_test.cc @@ -28,7 +28,7 @@ class MultibodyForcesTests : public ::testing::Test { // Creates a simple MultibodyTree model so that we can instantiate // MultibodyForces objects for this model. void SetUp() override { - SpatialInertia M; + const auto M = SpatialInertia::NaN(); const RigidBody& body1 = model_.AddRigidBody("Body1", M); const RigidBody& body2 = model_.AddRigidBody("Body2", M); model_.AddJoint("Joint1", model_.world_body(), std::nullopt, diff --git a/multibody/tree/test/multibody_tree_creation_test.cc b/multibody/tree/test/multibody_tree_creation_test.cc index df6ee5b15cf7..e85107027416 100644 --- a/multibody/tree/test/multibody_tree_creation_test.cc +++ b/multibody/tree/test/multibody_tree_creation_test.cc @@ -78,7 +78,7 @@ GTEST_TEST(MultibodyTree, BasicAPIToAddBodiesAndJoints) { // not performing any numerical computations. This is only to test API. // M_Bo_B is the spatial inertia about the body frame's origin Bo and // expressed in the body frame B. - SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); // Adds a new body to the world. const RigidBody& pendulum = @@ -146,7 +146,7 @@ GTEST_TEST(MultibodyTree, BasicAPIToAddBodiesAndJoints) { GTEST_TEST(MultibodyTree, TopologicalLoopDisallowed) { auto model = std::make_unique>(); const RigidBody& world_body = model->world_body(); - SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); const RigidBody& pendulum = model->AddRigidBody("pendulum", M_Bo_B); model->AddJoint( @@ -184,7 +184,7 @@ GTEST_TEST(MultibodyTree, MultibodyElementChecks) { // not performing any numerical computations. This is only to test API. // M_Bo_B is the spatial inertia about the body frame's origin Bo and // expressed in the body frame B. - SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); const RigidBody& body1 = model1->AddRigidBody("body1", M_Bo_B); const RigidBody& body2 = model2->AddRigidBody("body2", M_Bo_B); @@ -297,7 +297,7 @@ class TreeTopologyTests : public ::testing::Test { const RigidBody* AddTestBody(int i) { // NaN SpatialInertia to instantiate the RigidBody objects. // It is safe here since this tests only focus on topological information. - const SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); const RigidBody* body = &model_->AddRigidBody( fmt::format("TestBody_{}", i), M_Bo_B); bodies_.push_back(body); @@ -777,7 +777,7 @@ GTEST_TEST(WeldedBodies, CreateListOfWeldedBodies) { // inertia is not relevant and therefore we leave it un-initialized. auto AddRigidBody = [&model](const std::string& name) -> const RigidBody& { - return model.AddRigidBody(name, SpatialInertia()); + return model.AddRigidBody(name, SpatialInertia::NaN()); }; // Helper to add a joint between two bodies. For this test the actual type of diff --git a/multibody/tree/test/multibody_tree_test.cc b/multibody/tree/test/multibody_tree_test.cc index dc2a60396bdc..c31c150b5b19 100644 --- a/multibody/tree/test/multibody_tree_test.cc +++ b/multibody/tree/test/multibody_tree_test.cc @@ -231,7 +231,7 @@ GTEST_TEST(MultibodyTree, VerifyModelBasics) { // model. This is not allowed and an exception should be thrown. DRAKE_EXPECT_THROWS_MESSAGE( model->AddRigidBody("iiwa_link_5", default_model_instance(), - SpatialInertia()), + SpatialInertia::NaN()), ".* already contains a body named 'iiwa_link_5'. " "Body names must be unique within a given model."); @@ -314,7 +314,7 @@ GTEST_TEST(MultibodyTree, RetrievingAmbiguousNames) { const std::string link_name = "iiwa_link_5"; EXPECT_NO_THROW( model->AddRigidBody(link_name, other_model_instance, - SpatialInertia())); + SpatialInertia::NaN())); EXPECT_NO_THROW(model->Finalize()); // Link name is ambiguous, there are more than one bodies that use the name. @@ -341,7 +341,7 @@ class BadDerivedMBSystem : public MultibodyTreeSystem { public: explicit BadDerivedMBSystem(bool double_finalize) : MultibodyTreeSystem() { - mutable_tree().AddRigidBody("body", SpatialInertia()); + mutable_tree().AddRigidBody("body", SpatialInertia::NaN()); Finalize(); if (double_finalize) { Finalize(); @@ -1332,7 +1332,7 @@ class WeldMobilizerTest : public ::testing::Test { void SetUp() override { // Spatial inertia for each body. The actual value is not important for // these tests since they are all kinematic. - const SpatialInertia M_B; + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/planar_joint_test.cc b/multibody/tree/test/planar_joint_test.cc index 9f798a001887..c6caa400cf0b 100644 --- a/multibody/tree/test/planar_joint_test.cc +++ b/multibody/tree/test/planar_joint_test.cc @@ -34,7 +34,7 @@ class PlanarJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/prismatic_joint_test.cc b/multibody/tree/test/prismatic_joint_test.cc index b5239533064b..06c28386580c 100644 --- a/multibody/tree/test/prismatic_joint_test.cc +++ b/multibody/tree/test/prismatic_joint_test.cc @@ -41,7 +41,7 @@ class PrismaticJointTest : public ::testing::Test { std::unique_ptr> MakeModel() { // Spatial inertia for adding body. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/prismatic_spring_test.cc b/multibody/tree/test/prismatic_spring_test.cc index b7bf9f25435d..d5b442c23662 100644 --- a/multibody/tree/test/prismatic_spring_test.cc +++ b/multibody/tree/test/prismatic_spring_test.cc @@ -31,8 +31,8 @@ class SpringTester : public ::testing::Test { // Create an empty model. auto model = std::make_unique>(); - bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia()); - bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia()); + bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia::NaN()); + bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia::NaN()); model->AddJoint("WeldBodyAToWorld", model->world_body(), {}, *bodyA_, {}, diff --git a/multibody/tree/test/quaternion_floating_joint_test.cc b/multibody/tree/test/quaternion_floating_joint_test.cc index abf8b70e1a75..d00ffdad81a7 100644 --- a/multibody/tree/test/quaternion_floating_joint_test.cc +++ b/multibody/tree/test/quaternion_floating_joint_test.cc @@ -38,7 +38,7 @@ class QuaternionFloatingJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/revolute_joint_test.cc b/multibody/tree/test/revolute_joint_test.cc index 170f177a7d3f..c976a77bcaa2 100644 --- a/multibody/tree/test/revolute_joint_test.cc +++ b/multibody/tree/test/revolute_joint_test.cc @@ -44,7 +44,7 @@ class RevoluteJointTest : public ::testing::Test { std::unique_ptr> MakeModel() { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/revolute_spring_test.cc b/multibody/tree/test/revolute_spring_test.cc index ffe898fd04c1..5f10af375995 100644 --- a/multibody/tree/test/revolute_spring_test.cc +++ b/multibody/tree/test/revolute_spring_test.cc @@ -30,9 +30,9 @@ class SpringTester : public ::testing::Test { // Create an empty model. auto model = std::make_unique>(); - bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia()); - bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia()); - bodyC_ = &model->AddRigidBody("BodyC", SpatialInertia()); + bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia::NaN()); + bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia::NaN()); + bodyC_ = &model->AddRigidBody("BodyC", SpatialInertia::NaN()); model->AddJoint("WeldBodyAToWorld", model->world_body(), {}, *bodyA_, {}, diff --git a/multibody/tree/test/rigid_body_test.cc b/multibody/tree/test/rigid_body_test.cc index c82287ffa222..dc4745795032 100644 --- a/multibody/tree/test/rigid_body_test.cc +++ b/multibody/tree/test/rigid_body_test.cc @@ -16,14 +16,17 @@ using systems::Context; constexpr double kEpsilon = std::numeric_limits::epsilon(); -// Test rigid body constructor. -GTEST_TEST(RigidBody, RigidBodyConstructor) { +// Test rigid body 2-argument constructor. +GTEST_TEST(RigidBody, RigidBodyConstructor2) { // Construct a rigid body with a spatial inertia. const double mass = 2; const Vector3d p_BoBcm_B(0.4, 0.3, 0.2); const UnitInertia G_BBo_B(6, 7, 8); const SpatialInertia M_Bo_B(mass, p_BoBcm_B, G_BBo_B); - const RigidBody B("B", M_Bo_B); + const RigidBody B("foo", M_Bo_B); + + // Test that the name made it through. + EXPECT_EQ(B.name(), "foo"); // Test that after construction, RigidBody class properly returns its default // spatial inertia or parts of it. @@ -46,14 +49,17 @@ GTEST_TEST(RigidBody, RigidBodyConstructor) { EXPECT_TRUE(I_BBo_B.IsNearlyEqualTo(I_BBo_B_expected, 4.0*kEpsilon)); } -// Test rigid body constructor passing a string name. -GTEST_TEST(RigidBody, RigidBodyConstructorWithName) { - const std::string kLinkName = "LinkName"; - // For this test the numerical values of the spatial inertia are not - // important and therefore it is left uninitialized. - const SpatialInertia M_Bo_B; - const RigidBody B("LinkName", M_Bo_B); - EXPECT_EQ(B.name(), kLinkName); +// Test rigid body 1-argument constructor. +GTEST_TEST(RigidBody, RigidBodyConstructor1) { + // Construct a rigid body without specifying any inertia. + const RigidBody B("bar"); + EXPECT_EQ(B.name(), "bar"); + + // Sanity check that the inertia was zero. + EXPECT_GE(B.default_spatial_inertia().get_mass(), 0.0); + EXPECT_EQ(B.default_spatial_inertia().get_com(), Vector3d::Zero()); + EXPECT_GE(B.default_mass(), 0.0); + EXPECT_EQ(B.default_com(), Vector3d::Zero()); } // Fixture for a MultibodyTree model with a single rigid body and a Context. @@ -62,7 +68,7 @@ class RigidBodyTest : public ::testing::Test { void SetUp() override { // Create an empty model and then add a rigid body. auto model = std::make_unique>(); - const SpatialInertia M_BBo_B; // Default constructor is OK here + const auto M_BBo_B = SpatialInertia::NaN(); rigid_body_ = &model->AddRigidBody("rigidBody_B", M_BBo_B); // Finalize the model and create a default context for this system. diff --git a/multibody/tree/test/rpy_floating_joint_test.cc b/multibody/tree/test/rpy_floating_joint_test.cc index 6b9482ab1b86..cd5c33198180 100644 --- a/multibody/tree/test/rpy_floating_joint_test.cc +++ b/multibody/tree/test/rpy_floating_joint_test.cc @@ -39,7 +39,7 @@ class RpyFloatingJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/screw_joint_test.cc b/multibody/tree/test/screw_joint_test.cc index 38440be589bb..8e9fae969979 100644 --- a/multibody/tree/test/screw_joint_test.cc +++ b/multibody/tree/test/screw_joint_test.cc @@ -32,7 +32,7 @@ class ScrewJointTest : public ::testing::Test { // screw joint. void SetUp() override { auto model = std::make_unique>(); - body_ = &model->AddRigidBody("Body", SpatialInertia{}); + body_ = &model->AddRigidBody("Body", SpatialInertia::NaN()); // Add a screw joint between the world and body1: joint_ = &model->AddJoint("Joint", model->world_body(), diff --git a/multibody/tree/test/spatial_inertia_test.cc b/multibody/tree/test/spatial_inertia_test.cc index aa4066884546..435ec15c5347 100644 --- a/multibody/tree/test/spatial_inertia_test.cc +++ b/multibody/tree/test/spatial_inertia_test.cc @@ -46,10 +46,24 @@ SpatialInertia MakeArbitrarySpatialInertia() { I_Bcm_W); } -// Test default constructor which leaves entries initialized to NaN for a -// quick detection of uninitialized values. GTEST_TEST(SpatialInertia, DefaultConstructor) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" SpatialInertia I; +#pragma GCC diagnostic pop + ASSERT_TRUE(I.IsNaN()); +} + +GTEST_TEST(SpatialInertia, ZeroFactory) { + auto I = SpatialInertia::Zero(); + EXPECT_EQ(I.get_mass(), 0); + EXPECT_EQ(I.get_com(), Vector3d::Zero()); + EXPECT_EQ(I.get_unit_inertia().get_moments(), Vector3d::Zero()); + EXPECT_EQ(I.get_unit_inertia().get_products(), Vector3d::Zero()); +} + +GTEST_TEST(SpatialInertia, NaNFactory) { + auto I = SpatialInertia::NaN(); ASSERT_TRUE(I.IsNaN()); } @@ -1427,7 +1441,7 @@ GTEST_TEST(SpatialInertia, VerifyMinimumBoundingBoxLengths) { constexpr double kTolerance = 64 * std::numeric_limits::epsilon(); const double mass = 1.0; double a = 180, b = 0, c = 0; // rod's ½-length is 180 meters. - SpatialInertia M_BBcm_B; + auto M_BBcm_B = SpatialInertia::NaN(); M_BBcm_B = SpatialInertia::PointMass(mass, Vector3d(a, 0, 0)); M_BBcm_B += SpatialInertia::PointMass(mass, Vector3d(-a, 0, 0)); auto [abc, X_BA] = diff --git a/multibody/tree/test/tree_from_mobilizers_test.cc b/multibody/tree/test/tree_from_mobilizers_test.cc index 0f10b3b92998..b5e63d23e41e 100644 --- a/multibody/tree/test/tree_from_mobilizers_test.cc +++ b/multibody/tree/test/tree_from_mobilizers_test.cc @@ -422,7 +422,7 @@ TEST_F(PendulumTests, Finalize) { EXPECT_TRUE(model_->topology_is_valid()); // Valid after Finalize(). // Asserts that no more multibody elements can be added after finalize. - SpatialInertia M_Bo_B; + const auto M_Bo_B = SpatialInertia::NaN(); EXPECT_THROW(model_->AddRigidBody("B", M_Bo_B), std::logic_error); EXPECT_THROW( model_->AddFrame("F", *lower_link_, X_LEo_), diff --git a/multibody/tree/test/universal_joint_test.cc b/multibody/tree/test/universal_joint_test.cc index 6e95350cfccf..d99836fe5b00 100644 --- a/multibody/tree/test/universal_joint_test.cc +++ b/multibody/tree/test/universal_joint_test.cc @@ -32,7 +32,7 @@ class UniversalJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding bodies. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; // Default construction is ok for this. + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/multibody/tree/test/weld_joint_test.cc b/multibody/tree/test/weld_joint_test.cc index a47c321f1471..9c98973e53c2 100644 --- a/multibody/tree/test/weld_joint_test.cc +++ b/multibody/tree/test/weld_joint_test.cc @@ -24,7 +24,7 @@ class WeldJointTest : public ::testing::Test { void SetUp() override { // Spatial inertia for adding body. The actual value is not important for // these tests and therefore we do not initialize it. - const SpatialInertia M_B; + const auto M_B = SpatialInertia::NaN(); // Create an empty model. auto model = std::make_unique>(); diff --git a/planning/test/collision_avoidance_test.cc b/planning/test/collision_avoidance_test.cc index e4bcbbd53deb..6f9d975ac5b1 100644 --- a/planning/test/collision_avoidance_test.cc +++ b/planning/test/collision_avoidance_test.cc @@ -16,7 +16,6 @@ namespace { using drake::multibody::BodyIndex; using drake::multibody::default_model_instance; -using drake::multibody::SpatialInertia; using Eigen::VectorXd; using std::unique_ptr; @@ -29,8 +28,7 @@ double ZeroDistanceFunc(const VectorXd&, const VectorXd&) { unique_ptr> MakeRobot() { RobotDiagramBuilder builder; const auto model_instance = drake::multibody::default_model_instance(); - builder.plant().AddRigidBody("floater", model_instance, - SpatialInertia::MakeUnitary()); + builder.plant().AddRigidBody("floater", model_instance); return builder.Build(); } diff --git a/planning/test/collision_checker_test.cc b/planning/test/collision_checker_test.cc index 16f20787a931..48648a528a82 100644 --- a/planning/test/collision_checker_test.cc +++ b/planning/test/collision_checker_test.cc @@ -68,7 +68,6 @@ using multibody::ModelInstanceIndex; using multibody::MultibodyPlant; using multibody::RevoluteJoint; using multibody::RigidBody; -using multibody::SpatialInertia; using multibody::world_model_instance; using std::optional; using std::pair; @@ -88,8 +87,7 @@ ModelInstanceIndex AddEnvironmentModelInstance(MultibodyPlant* plant, DRAKE_DEMAND(plant->geometry_source_is_registered()); std::vector*> bodies; - const RigidBody& body = plant->AddRigidBody( - "env_body", instance, SpatialInertia::MakeUnitary()); + const RigidBody& body = plant->AddRigidBody("env_body", instance); for (int i = 0; i < num_geo; ++i) { plant->RegisterCollisionGeometry(body, RigidTransformd::Identity(), Sphere(0.01), fmt::format("g{}", i), @@ -292,8 +290,8 @@ pair>, ModelInstanceIndex> MakeModel( } else if (config.on_env_base) { const ModelInstanceIndex instance = builder_plant.AddModelInstance("floating"); - const RigidBody& floater = builder_plant.AddRigidBody( - "floater", instance, SpatialInertia::MakeUnitary()); + const RigidBody& floater = + builder_plant.AddRigidBody("floater", instance); // Connect the first chain body (1) to this floating base. builder_plant.AddJoint("floating", floater, {}, builder_plant.get_body(BodyIndex(1)), @@ -429,7 +427,7 @@ GTEST_TEST(CollisionCheckerTest, CollisionCheckerEmpty) { .self_collision_padding = 0}); EXPECT_THAT(dut.robot_model_instances(), testing::ElementsAre(robot)); - RigidBody free_body("free", world, {}); + RigidBody free_body("free", world); EXPECT_FALSE(dut.IsPartOfRobot(free_body)); EXPECT_FALSE(dut.IsPartOfRobot(BodyIndex(0))); diff --git a/planning/test/planning_test_helpers.cc b/planning/test/planning_test_helpers.cc index 3f431f7ebbf7..78073c3d44b6 100644 --- a/planning/test/planning_test_helpers.cc +++ b/planning/test/planning_test_helpers.cc @@ -18,7 +18,6 @@ using multibody::ModelInstanceIndex; using multibody::MultibodyPlant; using multibody::RevoluteJoint; using multibody::RigidBody; -using multibody::SpatialInertia; using multibody::parsing::ModelDirectives; using multibody::parsing::ProcessModelDirectives; @@ -60,9 +59,7 @@ ModelInstanceIndex AddChain(MultibodyPlant* plant, int n, int num_geo) { ModelInstanceIndex instance = plant->AddModelInstance(fmt::format("m{}", n)); std::vector*> bodies; for (int k = 0; k < n; ++k) { - bodies.push_back( - &plant->AddRigidBody(fmt::format("b{}", k), instance, - SpatialInertia::MakeUnitary())); + bodies.push_back(&plant->AddRigidBody(fmt::format("b{}", k), instance)); DRAKE_DEMAND(plant->geometry_source_is_registered()); for (int i = 0; i < num_geo; ++i) { plant->RegisterCollisionGeometry( diff --git a/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc b/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc index 19fdc0338d7d..55b51bcdee52 100644 --- a/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc +++ b/planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc @@ -46,7 +46,6 @@ using multibody::MultibodyPlant; using multibody::PlanarJoint; using multibody::RevoluteJoint; using multibody::RigidBody; -using multibody::SpatialInertia; using solvers::MathematicalProgram; bool GurobiOrMosekSolverUnavailableDuringMemoryCheck() { @@ -1843,14 +1842,10 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, ContinuousJointsApi) { GTEST_TEST(GcsTrajectoryOptimizationTest, GetContinuousJoints) { MultibodyPlant plant(0.0); - const RigidBody& first_body = - plant.AddRigidBody("first_body", SpatialInertia()); - const RigidBody& second_body = - plant.AddRigidBody("second_body", SpatialInertia()); - const RigidBody& third_body = - plant.AddRigidBody("third_body", SpatialInertia()); - const RigidBody& fourth_body = - plant.AddRigidBody("fourth_body", SpatialInertia()); + const RigidBody& first_body = plant.AddRigidBody("first_body"); + const RigidBody& second_body = plant.AddRigidBody("second_body"); + const RigidBody& third_body = plant.AddRigidBody("third_body"); + const RigidBody& fourth_body = plant.AddRigidBody("fourth_body"); // Add a planar joint without limits plant.AddJoint("first_joint", plant.world_body(), {}, first_body, diff --git a/systems/sensors/test/camera_config_functions_test.cc b/systems/sensors/test/camera_config_functions_test.cc index 9c9d77e5673c..448bd14b063c 100644 --- a/systems/sensors/test/camera_config_functions_test.cc +++ b/systems/sensors/test/camera_config_functions_test.cc @@ -36,7 +36,6 @@ using drake::math::RigidTransformd; using drake::multibody::AddMultibodyPlantSceneGraph; using drake::multibody::FixedOffsetFrame; using drake::multibody::MultibodyPlant; -using drake::multibody::SpatialInertia; using drake::schema::Transform; using drake::systems::DiagramBuilder; using drake::systems::lcm::LcmBuses; @@ -95,8 +94,7 @@ class CameraConfigFunctionsTest : public ::testing::Test { std::tie(plant_, scene_graph_) = AddMultibodyPlantSceneGraph(&builder_, 0); // Populate builder with sufficient stuff. - const auto& body = plant_->AddRigidBody( - "test_body", SpatialInertia::MakeUnitary()); + const auto& body = plant_->AddRigidBody("test_body"); body_frame_id_ = plant_->GetBodyFrameIdOrThrow(body.index()); plant_->AddFrame(std::make_unique>( "test_frame", body, RigidTransformd())); diff --git a/systems/sensors/test/sim_rgbd_sensor_test.cc b/systems/sensors/test/sim_rgbd_sensor_test.cc index 1c52606b8541..f6ffa75be925 100644 --- a/systems/sensors/test/sim_rgbd_sensor_test.cc +++ b/systems/sensors/test/sim_rgbd_sensor_test.cc @@ -38,7 +38,6 @@ using multibody::FixedOffsetFrame; using multibody::Frame; using multibody::MultibodyPlant; using multibody::RigidBody; -using multibody::SpatialInertia; using systems::lcm::LcmPublisherSystem; /* Returns a pointer to the named instance of TargetSystem (if it exists). */ @@ -67,10 +66,8 @@ class SimRgbdSensorTest : public ::testing::Test { void SetUp() override { std::tie(plant_, scene_graph_) = AddMultibodyPlantSceneGraph(&builder_, 1e-2); - bodyA_ = - &plant_->AddRigidBody("bodyA", SpatialInertia::MakeUnitary()); - bodyB_ = - &plant_->AddRigidBody("bodyB", SpatialInertia::MakeUnitary()); + bodyA_ = &plant_->AddRigidBody("bodyA"); + bodyB_ = &plant_->AddRigidBody("bodyB"); X_AF_ = RigidTransformd(Vector3d(-0.5, 0.5, 0.75)); frame_F_ = &plant_->AddFrame(std::make_unique>(