As part of issue #18803 and PR #24843 which deal with automatic handling of kinematic-loops (closed-topology) systems, a Link (RigidBody) is split into a primary link and shadow link. The mass (and therefore the rotational inertia) of the original link is split into the primary and shadow link.
There are various issues that need to be addressed.
-
There should be no calls to the shadow link's default spatial inertia (or its properties), so any Set() or Get() methods that rely on shadow link default spatial inertia should throw exceptions. Note: Shadow link's inertia properties are calculated from properties sourced from the FrameBodyPoseCache (mirroring its primary's
share of the split). Reminder Drake mass properties are parameterized.
-
Any other "nonsense" setters/getters or queries to shadow links should throw exceptions. Also, what is "nonsense" may differ for internal and public functions.
-
All user-facing queries on a link should always return the user-defined properties associated with that link. For example, below is a snippet of an early iteration of a test in file auto_closed_topology_test.cc. It shows coupler.SetMass(context.get(), 3.0) followed coupler.get_mass(*context) returning 1.5. Although CalcTotalMass() totals the system mass correctly, the coupler's mass seems odd/wrong, particularly if an end-user (or even an Drake developer) does not know that the coupler link was split into two -- which seems like a multibody dynamics algorithmic artifact.
-
(from Sean) Would it be better if MbP::num_bodies() returned the user bodies? And a new method returned all bodies? I think there's a debate to be made about the documentation. But the advantage of doing so is that num_bodies() only included user bodies before shadows. And it would continue to do that after shadows. So, from that semantic perspective, it hasn't really changed. Just our description of the indices it returns. And, as you've carefully made sure that the user bodies are nicely coalesced before the shadow links, that's a very reasonable API.
GTEST_TEST(ClosedTopologyTest, RuntimeMassChangeReSplitsAndShadowIsReadOnly) {
constexpr double kTol = 1e-14;
std::unique_ptr<MultibodyPlant> plant = MakeFourBarPlant();
auto context = plant->CreateDefaultContext();
const Link& coupler = plant->GetBodyByName("coupler");
const Link& shadow = plant->GetBodyByName("coupler$1");
// Set the coupler's (physical) mass to 3 kg; the split follows to 1.5 kg on
// each of the coupler and its shadow, and the total tracks accordingly
// (driver 1 + rocker 2 + coupler 3 = 6 kg).
coupler.SetMass(context.get(), 3.0);
EXPECT_NEAR(coupler.get_mass(*context), 1.5, kTol);
EXPECT_NEAR(shadow.get_mass(*context), 1.5, kTol);
EXPECT_NEAR(plant->CalcTotalMass(*context), 6.0, kTol);
- We need to investigate user-facing Setters and Getters for things like link (RigidBody) spatial inertia, spatial momentum, kinetic energy, gravitational potential energy, ...
As part of issue #18803 and PR #24843 which deal with automatic handling of kinematic-loops (closed-topology) systems, a Link (RigidBody) is split into a primary link and shadow link. The mass (and therefore the rotational inertia) of the original link is split into the primary and shadow link.
There are various issues that need to be addressed.
There should be no calls to the shadow link's default spatial inertia (or its properties), so any Set() or Get() methods that rely on shadow link default spatial inertia should throw exceptions. Note: Shadow link's inertia properties are calculated from properties sourced from the FrameBodyPoseCache (mirroring its primary's
share of the split). Reminder Drake mass properties are parameterized.
Any other "nonsense" setters/getters or queries to shadow links should throw exceptions. Also, what is "nonsense" may differ for internal and public functions.
All user-facing queries on a link should always return the user-defined properties associated with that link. For example, below is a snippet of an early iteration of a test in file auto_closed_topology_test.cc. It shows coupler.SetMass(context.get(), 3.0) followed coupler.get_mass(*context) returning 1.5. Although CalcTotalMass() totals the system mass correctly, the coupler's mass seems odd/wrong, particularly if an end-user (or even an Drake developer) does not know that the coupler link was split into two -- which seems like a multibody dynamics algorithmic artifact.
(from Sean) Would it be better if MbP::num_bodies() returned the user bodies? And a new method returned all bodies? I think there's a debate to be made about the documentation. But the advantage of doing so is that num_bodies() only included user bodies before shadows. And it would continue to do that after shadows. So, from that semantic perspective, it hasn't really changed. Just our description of the indices it returns. And, as you've carefully made sure that the user bodies are nicely coalesced before the shadow links, that's a very reasonable API.
GTEST_TEST(ClosedTopologyTest, RuntimeMassChangeReSplitsAndShadowIsReadOnly) {
constexpr double kTol = 1e-14;
std::unique_ptr<MultibodyPlant> plant = MakeFourBarPlant();
auto context = plant->CreateDefaultContext();
const Link& coupler = plant->GetBodyByName("coupler");
const Link& shadow = plant->GetBodyByName("coupler$1");
// Set the coupler's (physical) mass to 3 kg; the split follows to 1.5 kg on
// each of the coupler and its shadow, and the total tracks accordingly
// (driver 1 + rocker 2 + coupler 3 = 6 kg).
coupler.SetMass(context.get(), 3.0);
EXPECT_NEAR(coupler.get_mass(*context), 1.5, kTol);
EXPECT_NEAR(shadow.get_mass(*context), 1.5, kTol);
EXPECT_NEAR(plant->CalcTotalMass(*context), 6.0, kTol);