Segfaults from multibody elements that are not part of a tree
If a multibody element is created (e.g., a RigidBody) and it is not added to a plant (the element does not have a parent_multibody_tree), and a calculation is done that needs the multibody tree, a segfault ensues.
Note: The related issue #22804 that is specific to the Frame class was fixed by PR #22821. Classes that need to be investigated for the addition of guardrails against segfaults, include RigidBody, ForceElement, ModelInstance, Joint, JointActuator.
Below: Sample reproduction of a segfault for RigidBody (prototyped in rigid_body_test.cc).
// Create a RigidBody that is not part of a plant. Ensure an exception is thrown
// if there is a query for information about this RigidBody that needs the
// associated multibody tree.
TEST_F(RigidBodyTest, IsRigidBodyCalledWithNoMultibodyTree) {
RigidBody<double> body_not_in_multibody_tree("bad_body");
const systems::Context<double>& context = *context_;
// In debug builds, the next line throws the following exception:
// "This multibody element was not added to a MultibodyTree."
// In release builds, the next line seems to cause a segmentation fault.
const math::RigidTransform<double>& seg_fault_in_release_mode =
body_not_in_multibody_tree.EvalPoseInWorld(context);
unused(seg_fault_in_release_mode.IsExactlyIdentity());
}
Wondering if we thought of this earlier: Why do we allow public constructors for certain types of MultibodyElement.
If the only public construction of the MultibodyElement occurred via code such as:
plant.AddMultibodyElement<SpecificElement>(specificArguments);
there would be little reason to do inefficient runtime checks of whether a MultibodyElement has a parent tree. This guarantee also avoids the the development cost of adding unnecessary safeguards such as DrakeThrowUnless(has_parent_tree()).
Many examples in our code base use this already. For example, for a MultibodyPlant named four_bar:
four_bar.AddForceElement<LinearBushingRollPitchYaw>(
bc_bushing, cb_bushing, torque_stiffness_constants,
torque_damping_constants, force_stiffness_constants,
force_damping_constants);
If the only public construction of the MultibodyElement occurred via code such as: ...
This is not really viable in Python, or it least it would be very awkward.
... inefficient runtime checks ...
It's a well-predicted branch. It costs 1-2 cycles. This objection a giant nothingburger and distraction.