[parsing] Improve URDF inertia invalidity handling

[rpoyner-tri]

This patch brings inertia invalidity reporting under diagnostic policy, and relaxes inertia problems from hard throws to warnings.

The move to diagnostic policy means that inertia warnings give file names and line numbers, which make it easier to fix problems.

The move from hard throws to warnings means that models with non-physical inertias can still be used with non-simulation workflows, such as visualization, kinematics, and analysis.

---

This change is 

[rpoyner-tri]

+@jwnimmer-tri for feature review.
+(release notes: fix)
FYI @mitiguy

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

a discussion (no related file):
Here is an piece of diagnostic output from a model found on Github:

There were a lot more warnings, but the above gives the general idea.

---

[jwnimmer-tri]

Reviewed 1 of 2 files at r1, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 203 at r1 (raw file):

    return SpatialInertia<double>::MakeFromCentralInertia(
        body_mass, p_BoBcm_B, I_BBcm_B);
  } catch (const std::exception& e) {

To me the biggest risk here is that we might demote programming errors on our part into warnings instead of throws. The scope of that risk is the amount of code within the try-block.

Some of the lines within here seem to me like they would be our fault if they throw, not the users's fault.

It is possible to narrow the try-block(s) to surround fewer operations?

We could create a const SpatialInertia<double> dummy_point_mass; up above and then reuse it in a few try-blocks, e.g.,

// Yes, catching exceptions violates the coding standard. It is done here to
// funnel the plethora of possible exceptions into parse-time warnings, since
// non-physical inertias are all too common, and the thrown messages are
// actually pretty useful.
RotationalInertia<double> I_BBcm_Bi;
try {
  I_BBcm_Bi = RotationalInertia<double>(ixx, iyy, izz, ixy, ixz, iyz);
} catch (const std::exception& e) {
  Warning(*node, e.what());
  return dummy_point_mass;
}

// <-- NOTE: No try-catch around this part. -->

// If this is a massless body, return a zero SpatialInertia.
if (body_mass == 0. && I_BBcm_Bi.get_moments().isZero() &&
    I_BBcm_Bi.get_products().isZero()) {
  return SpatialInertia<double>(body_mass, {0., 0., 0.}, {0., 0., 0});
}

...

---

multibody/parsing/detail_urdf_parser.cc line 207 at r1 (raw file):

    // Return an arbitrary but valid inertia, to allow parsing to
    // continue.
    return SpatialInertia<double>::PointMass(1., {0., 0., 0.});

On first glance, I would have expected to use body_mass here, so at least that part of the model is correct, even if the moments are misshapen. Is there any difficulty with using it? I suppose we would need to clamp it to be 0 < body_mass < inf but that isn't too onerous.

---

multibody/parsing/detail_urdf_parser.cc line 207 at r1 (raw file):

    // Return an arbitrary but valid inertia, to allow parsing to
    // continue.
    return SpatialInertia<double>::PointMass(1., {0., 0., 0.});

nit GSG wants 0.0 not 0. but in any case there's a more canonical spelling.

Suggestion:

Vector3d::Zero()

[jwnimmer-tri]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 203 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

To me the biggest risk here is that we might demote programming errors on our part into warnings instead of throws. The scope of that risk is the amount of code within the try-block.

Some of the lines within here seem to me like they would be our fault if they throw, not the users's fault.

It is possible to narrow the try-block(s) to surround fewer operations?

We could create a const SpatialInertia<double> dummy_point_mass; up above and then reuse it in a few try-blocks, e.g.,

// Yes, catching exceptions violates the coding standard. It is done here to
// funnel the plethora of possible exceptions into parse-time warnings, since
// non-physical inertias are all too common, and the thrown messages are
// actually pretty useful.
RotationalInertia<double> I_BBcm_Bi;
try {
  I_BBcm_Bi = RotationalInertia<double>(ixx, iyy, izz, ixy, ixz, iyz);
} catch (const std::exception& e) {
  Warning(*node, e.what());
  return dummy_point_mass;
}

// <-- NOTE: No try-catch around this part. -->

// If this is a massless body, return a zero SpatialInertia.
if (body_mass == 0. && I_BBcm_Bi.get_moments().isZero() &&
    I_BBcm_Bi.get_products().isZero()) {
  return SpatialInertia<double>(body_mass, {0., 0., 0.}, {0., 0., 0});
}

...

I see now that my particular example, the body_mass is still unverified, so might throw, so would need a try-catch. I think that's fixable too, but in any case the general point stands -- if there are any lines here that we do not expect to throw, we shouldn't wrap those lines in try.

[sherm1]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri and @rpoyner-tri)

a discussion (no related file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Here is an piece of diagnostic output from a model found on Github:

There were a lot more warnings, but the above gives the general idea.

Nice!

---

[jwnimmer-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 203 at r1 (raw file):

I see now that my particular example, the body_mass is still unverified, ...

Never mind, I was right the first time. The mass is already checked to be zero, so that line cannot throw.

[rpoyner-tri]

Excitingly, the patch as currently written emits different error messages in debug builds than in release builds. Thanks, DRAKE_ASSERT_VOID(). I'll at the very least need to adjust the test.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri and @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 203 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I see now that my particular example, the body_mass is still unverified, ...

Never mind, I was right the first time. The mass is already checked to be zero, so that line cannot throw.

I don't know that I have the full catalog, but certainly the RotIn ctor and the ReExpress operation and the SpatIn factory method can throw. See also test results above; we currently get different throws under debug and release mode. The useful content is essentially the same (it's almost always the triangle inequality).

---

multibody/parsing/detail_urdf_parser.cc line 207 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

On first glance, I would have expected to use body_mass here, so at least that part of the model is correct, even if the moments are misshapen. Is there any difficulty with using it? I suppose we would need to clamp it to be 0 < body_mass < inf but that isn't too onerous.

In this branch, body mass hasn't been checked. It won't be crazy (inf, nan), but could be negative, or zero. I can write an expression that guesses a more plausible mass.

[jwnimmer-tri]

My first reaction there is that we should avoid ever hitting Debug-only throwing, either by promoting the throws to be Release as well, or else by calling a different equivalent API that already throws, or by creating a variation of the function that does always throw, etc. The user should get the same Parser output no matter the build flavor.

[rpoyner-tri]

Agreed. No doubt the D_A_V()'s are there because the methods are used in Mbp, so I'll have to step carefully.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

[jwnimmer-tri]

Seems good. I will check back tomorrow with fresh eyes before LGTM.

You're welcome to assign platform in the meantime if you like.

Reviewed 1 of 2 files at r1, 1 of 1 files at r2, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 166 at r2 (raw file):

  // validity checking happen later.
  auto dummy_inertia = SpatialInertia<double>::PointMass(
      body_mass > 0.0 ? body_mass : 1.0,

nit IIRC the PointMass will throw on +inf.

In another thread you claimed it can't be inf, but my reading of ConvertToVector is that it allows infinities, because that's how istringstream rolls.

I suppose a unit test would be authoritative one way or another.

Suggestion:

(std::isfinite(body_mass) && body_mass > 0.0)

---

multibody/parsing/detail_urdf_parser.cc line 204 at r2 (raw file):

  if (body_mass == 0.0 && I_BBcm_Bi.get_moments().isZero() &&
      I_BBcm_Bi.get_products().isZero()) {
    return SpatialInertia<double>(body_mass, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0});

BTW If you prefer, see the suggestion below.

If nothing else, consider s/body_mass/0.0/ since every time I read this line I always forget that the argument is a known constant, not actually a variable.

Suggestion:

SpatialInertia<double>::PointMass(0.0, Vector3d::Zero())

---

multibody/parsing/test/detail_urdf_parser_test.cc line 1254 at r2 (raw file):

  // Test various invalid input values.
  constexpr const char* base = R"""(
    <robot name='point_mass'>

nit It's technically not a point mass unless the inertia is all zeros.

Suggestion:

foo

[rpoyner-tri]

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

---

multibody/parsing/detail_urdf_parser.cc line 166 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit IIRC the PointMass will throw on +inf.

In another thread you claimed it can't be inf, but my reading of ConvertToVector is that it allows infinities, because that's how istringstream rolls.

I suppose a unit test would be authoritative one way or another.

How would you propose to spell infinity in the input? Everything I've tried ends up erroring from ParseScalarAttribute(). The analogous unit test is here: https://github.com/RobotLocomotion/drake/blob/master/multibody/parsing/test/detail_tinyxml_test.cc#L104-L105

---

multibody/parsing/detail_urdf_parser.cc line 204 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW If you prefer, see the suggestion below.

If nothing else, consider s/body_mass/0.0/ since every time I read this line I always forget that the argument is a known constant, not actually a variable.

PointMass throws on 0 input mass: https://github.com/RobotLocomotion/drake/blob/master/multibody/tree/spatial_inertia.cc#L28-L32

I've improved the spelling as much as I know how.

[jwnimmer-tri]

Reviewed 1 of 2 files at r3, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 166 at r2 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

How would you propose to spell infinity in the input? Everything I've tried ends up erroring from ParseScalarAttribute(). The analogous unit test is here: https://github.com/RobotLocomotion/drake/blob/master/multibody/parsing/test/detail_tinyxml_test.cc#L104-L105

Aha. TIL it's platform-specific.

https://stackoverflow.com/questions/70971921/how-to-parse-floating-point-infinity-from-stdistream

With Apple Clang and libc++ (on macsim) the goldbolt code in that SO link shows that inf in the XML would be parsed as infinity. So using value='inf' and testing on macOS would show the problem.

In any case, I'm fine without a unit test now that I've tested manually. But I do think it's worth spending the code to guard against inf.

[jwnimmer-tri]

feature.

Reviewed 1 of 2 files at r3.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers (waiting on @rpoyner-tri)

---

multibody/parsing/test/detail_urdf_parser_test.cc line 1280 at r3 (raw file):

      fmt::format(base, "value='-1'",
                  "ixx='1' ixy='0' ixz='0' iyy='1' iyz='0' izz='1'"), "c");
  EXPECT_THAT(TakeWarning(), MatchesRegex(".*mass > 0.*fail.*"));

BTW Gut feeling -- matching vs "fail" here is not critical for testing and hazards churn here as the RotationalInertia messages evolve.

Ditto below.

Suggestion:

".*mass > 0.*"

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers (waiting on @jwnimmer-tri)

---

multibody/parsing/detail_urdf_parser.cc line 166 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Aha. TIL it's platform-specific.

https://stackoverflow.com/questions/70971921/how-to-parse-floating-point-infinity-from-stdistream

With Apple Clang and libc++ (on macsim) the goldbolt code in that SO link shows that inf in the XML would be parsed as infinity. So using value='inf' and testing on macOS would show the problem.

In any case, I'm fine without a unit test now that I've tested manually. But I do think it's worth spending the code to guard against inf.

Done.

[jwnimmer-tri]

Reviewed 2 of 2 files at r4, all commit messages.
Reviewable status: needs at least two assigned reviewers (waiting on @rpoyner-tri)

[rpoyner-tri]

+@ggould-tri for platform review please.

Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @ggould-tri)

[rpoyner-tri]

See #18773 for discussion and context.

Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @ggould-tri)

[ggould-tri]

Reviewed 2 of 2 files at r4, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform) (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

      (std::isfinite(body_mass) && body_mass > 0.0) ? body_mass : 1.0;
  auto dummy_inertia = SpatialInertia<double>::PointMass(plausible_dummy_mass,
                                                         Vector3d::Zero());

Is a point mass at zero offset a reasonable dummy? I can see it as reasonable for a link in a robot, but if this is a free body won't it have zero moments and cause the sim to diverge horribly as soon as it encounters nonzero torque?

Or do we believe that free bodies will never be described using urdf?

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform) (waiting on @ggould-tri)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, ggould-tri wrote…

Is a point mass at zero offset a reasonable dummy? I can see it as reasonable for a link in a robot, but if this is a free body won't it have zero moments and cause the sim to diverge horribly as soon as it encounters nonzero torque?

Or do we believe that free bodies will never be described using urdf?

In general my opinion is that once there are inertia parsing errors, the user should expect their dynamics to be wonky, so it's OK to ignore the offset. The big picture goal is to still allow kinematics in this case.

On the other hand, we do have a valid X_BBi already parsed, so it would be natural to use its .translation() here instead of Vector3d::Zero(). The user could plausibly claim that unless we warned about ignoring the translation, we should obey it when populating the plant.

[ggould-tri]

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In general my opinion is that once there are inertia parsing errors, the user should expect their dynamics to be wonky, so it's OK to ignore the offset. The big picture goal is to still allow kinematics in this case.

On the other hand, we do have a valid X_BBi already parsed, so it would be natural to use its .translation() here instead of Vector3d::Zero(). The user could plausibly claim that unless we warned about ignoring the translation, we should obey it when populating the plant.

I agree if the dynamics are merely "wonky", but not if they crash the sim. Possibly someone should actually test this with a free body with bad inertia. (I won't have a chance to do that test for a little while, though; @rpoyner-tri do you by any chance have an example to hand?).

A small nonzero moment would be more polite (I don't know offhand if just dropping in a translation of the point does that -- the COM will still be on the particle, so the translation relative to the COM will be zero, right?).

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform) (waiting on @rpoyner-tri and @sherm1)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, ggould-tri wrote…

I agree if the dynamics are merely "wonky", but not if they crash the sim. Possibly someone should actually test this with a free body with bad inertia. (I won't have a chance to do that test for a little while, though; @rpoyner-tri do you by any chance have an example to hand?).

A small nonzero moment would be more polite (I don't know offhand if just dropping in a translation of the point does that -- the COM will still be on the particle, so the translation relative to the COM will be zero, right?).

Tagging @sherm1 for advice.

The situation we have here is that the user has some typo in their URDF relating to the moments or products of inertia. We have a valid X_BBi transform and mass; it's only ixx,iyy,izz,ixy,ixz,iyz that we don't know.

Our goal here to warn the user that they have a typo and therefore that we will be ignoring their inertia values, but then still create the Body and give them our best guess at a MultibodyPlant, so they can still use the MbP for kinematics. If they try simulate the dynamics of course we'd expect a mismatch between what happens vs what they intended to happen.

What's the best "dummy inertia" to use in this case of a user's typo?

The current draft here uses a point mass. Grant is worried that a point mass on a free body would be trouble. Should we use a sphere inertia instead of a point, for example?

[sherm1]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform) (waiting on @mitiguy and @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Tagging @sherm1 for advice.

The situation we have here is that the user has some typo in their URDF relating to the moments or products of inertia. We have a valid X_BBi transform and mass; it's only ixx,iyy,izz,ixy,ixz,iyz that we don't know.

Our goal here to warn the user that they have a typo and therefore that we will be ignoring their inertia values, but then still create the Body and give them our best guess at a MultibodyPlant, so they can still use the MbP for kinematics. If they try simulate the dynamics of course we'd expect a mismatch between what happens vs what they intended to happen.

What's the best "dummy inertia" to use in this case of a user's typo?

The current draft here uses a point mass. Grant is worried that a point mass on a free body would be trouble. Should we use a sphere inertia instead of a point, for example?

Yes, a point mass is a poor choice here since it is likely to cause new problems. I would suggest something like SpatialInertia::SolidSphereWithDensity(1000,.1). That would be a mass of 1.3kg and inertia equivalent to a ball of ice 20cm in diameter (please check my math). An easier-to-picture version might be a smaller steel ball (density 8000 kg/m^3). A radius of 5cm gives 8000 4/3 π 0.05³ = 1.3 kg, less inertia than an ice ball but still enough to avoid trouble.

(SolidSphereWithMass would be easier to think about but I'm not sure we have that one yet -- @mitiguy ? )

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform) (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yes, a point mass is a poor choice here since it is likely to cause new problems. I would suggest something like SpatialInertia::SolidSphereWithDensity(1000,.1). That would be a mass of 1.3kg and inertia equivalent to a ball of ice 20cm in diameter (please check my math). An easier-to-picture version might be a smaller steel ball (density 8000 kg/m^3). A radius of 5cm gives 8000 4/3 π 0.05³ = 1.3 kg, less inertia than an ice ball but still enough to avoid trouble.

(SolidSphereWithMass would be easier to think about but I'm not sure we have that one yet -- @mitiguy ? )

In that case my vote is for SolidSphereWithDensity such that mass matches.

The question is whether we fix radius and solve for density (from mass), or fix density and solve for radius (from mass). The latter seems more natural to me, but I don't mind either way.

[sherm1]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee ggould-tri(platform) (waiting on @jwnimmer-tri and @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 175 at r6 (raw file):

      std::cbrt((3.0 / (4.0 * M_PI)) * plausible_volume);
  auto dummy_inertia = SpatialInertia<double>::SolidSphereWithDensity(
      kPlausibleDensity, plausible_radius).ShiftInPlace(-X_BBi.translation());

BTW This looks good to me. If there isn't one already, a unit test with a bad inertia and Bo != Bcm checking that we got the expected about-Bo inertia would be nice to have! Paul @mitiguy could likely generate a sphere inertia but about Bo calculated in some other way so the test is more than a tautology.

[rpoyner-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee ggould-tri(platform) (waiting on @jwnimmer-tri, @rpoyner-tri, and @sherm1)

---

multibody/parsing/detail_urdf_parser.cc line 175 at r6 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW This looks good to me. If there isn't one already, a unit test with a bad inertia and Bo != Bcm checking that we got the expected about-Bo inertia would be nice to have! Paul @mitiguy could likely generate a sphere inertia but about Bo calculated in some other way so the test is more than a tautology.

Yeah, I had thought about a unit test, but not (yet) about what it should look like. I will investigate.

[rpoyner-tri]

Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @ggould-tri and @jwnimmer-tri)

---

multibody/parsing/detail_urdf_parser.cc line 175 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Yeah, I had thought about a unit test, but not (yet) about what it should look like. I will investigate.

Added some minimal sanity checking of plausible inertias -- PTAL

[jwnimmer-tri]

Oops, I never gave my LGTM, did I? I thought I'd done it.

feature. Certainly the "parsing best practices" part of this code is in great shape now.

Possibly you should have an actual dynamicist review check math for the "dummy" inertia, though? I don't know if I fully understand it.

Reviewed 1 of 1 files at r7, all commit messages.
Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @rpoyner-tri)

[rpoyner-tri]

+@mitiguy for specialist review of the "plausible dummy inertia" code.

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @mitiguy)

[jwnimmer-tri]

In our f2f on Monday, we talked about a helper function that both SDFormat and URDF (and more?) would use. For clarity, here's the kind of signature I had in mind during that discussion.

/* Combines the given user inputs into a SpatialInertia. When any data is invalid,
warns into the policy and returns a best-effort approximation instead.
@param I_BBcm_Bi is ordered as ixx, iyy, izz, ixy, ixz, iyz */
SpatialInertia<double> ParseSpatialInertia(
    const DiagnosticPolicy& diagnostic,
    const RigidTransformd& X_BBi,
    double mass,
    const Vector6d& I_BBcm_Bi)

[rpoyner-tri]

I'm not against it. I figured I would roll the refactoring into the SDFormat patch, when we at least reach rule-of-2.

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @mitiguy)

[rpoyner-tri]

FYI @joemasterjohn here's the PR where the parser tries to substitute a "plausible inertia". PTAL if you have time.

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @mitiguy)

[mitiguy]

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 168 at r4 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Here's a version with a sold sphere at water density -- PTAL

Note: PR #19268 for SolidSphereWithMass() is in review.

---

multibody/parsing/detail_urdf_parser.cc line 175 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Added some minimal sanity checking of plausible inertias -- PTAL

Nit Consider semi-monogram notation and documentation to clarify the intended dummy_inertia. Also a note as to why the shift is needed is helpful. Consider code such as:

Code snippet:

  // Create Mdum_BBo_B, a plausible spatial inertia for B about Bo (B's origin).
  // To do this, create Mdum_BBcm (a plausible spatial inertia for B about
  // Bcm) as a solid sphere and then shift that spatial inertia from Bcm to Bo.
  // Note: It is unwise to directly create a solid sphere about Bo as this does
  // not guarantee that the spatial inertia about Bcm is valid. Bcm (B's center
  // of mass) is the ground-truth point for validty tests.
  const SpatialInertia<double> Mdum_BBcm = 
    SpatialInertia<double>::SolidSphereWithDensity(
       kPlausibleDensity, plausible_radius);
  const Vector3<double>& p_BoBcm_B = X_BBi.translation();  // Bi's origin is Bcm.
  const SpatialInertia<double> Mdum_BBo_B = Mdum_BBcm.Shift(-p_BoBcm_B);

[ggould-tri]

Reviewed 1 of 1 files at r7, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

[jwnimmer-tri]

Reviewed 1 of 1 files at r8, all commit messages.
Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

[rpoyner-tri]

Updates:

• integrated Paul's monogram-notation suggestions, with co-author credit
• based on f2f with Joe, added some text about the substitute inertia to warning text

Still waiting on @ggould-tri and @mitiguy approvals.

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @jwnimmer-tri)

[jwnimmer-tri]

Reviewed 1 of 1 files at r9, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @jwnimmer-tri and @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 190 at r9 (raw file):

  // Tell the user we are constructing a plausible but wrong model, when input
  // inertia is not usable.
  static constexpr char warn_format[] =

nit https://drake.mit.edu/styleguide/cppguide.html#Constant_Names

---

multibody/parsing/detail_urdf_parser.cc line 192 at r9 (raw file):

  static constexpr char warn_format[] =
      "{}\n"
      "Substituting a plausible-guess inertia '{}' instead.";

Working

I'm going to re-test locally to see if this is actually an improvement. On paper, it seems like not.

[jwnimmer-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

---

multibody/parsing/detail_urdf_parser.cc line 192 at r9 (raw file):

Added some text about the substitute inertia to warning text.

Could you explain the thought process?

I put a typo in the Pendulum and ran it. Here's what I see now:

jwnimmer@call-cps:~/jwnimmer-tri/drake$ bazel-bin/multibody/parsing/parser_manual_test examples/pendulum/Pendulum.urdf 
parsing examples/pendulum/Pendulum.urdf
/home/jwnimmer/jwnimmer-tri/drake/examples/pendulum/Pendulum.urdf:66: warning: MakeFromMomentsAndProductsOfInertia(): The rotational inertia
[1  0  0]
[0  0  0]
[0  0  0]
did not pass the test CouldBePhysicallyValid().
The associated principal moments of inertia:
0  0  1
do not satisfy the triangle inequality.
Substituting a plausible-guess inertia '
 mass = 0.5
 Center of mass = [0  0  0]
 Inertia about point P, I_BP =
[0.000484861            0            0]
[          0  0.000484861            0]
[          0            0  0.000484861]
' instead.
Parsed 1 models.

First of all, as a nit: the single quotes are useless. It should be something like "... plausible inertia instead:\n" and then the string output.

What's still not clear to me though is why adding even more voluminous text to the parser output is to the user's advantage.

If we printed out an URL XML stanza with a suggested fix, I could probably buy that. But as it stands, what do you think a user would do with this extra information? To me, it's just clutter that makes it more difficult to find the more important messages that will also be nearby.

[rpoyner-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @jwnimmer-tri)

---

multibody/parsing/detail_urdf_parser.cc line 192 at r9 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Added some text about the substitute inertia to warning text.

Could you explain the thought process?

I put a typo in the Pendulum and ran it. Here's what I see now:

jwnimmer@call-cps:~/jwnimmer-tri/drake$ bazel-bin/multibody/parsing/parser_manual_test examples/pendulum/Pendulum.urdf 
parsing examples/pendulum/Pendulum.urdf
/home/jwnimmer/jwnimmer-tri/drake/examples/pendulum/Pendulum.urdf:66: warning: MakeFromMomentsAndProductsOfInertia(): The rotational inertia
[1  0  0]
[0  0  0]
[0  0  0]
did not pass the test CouldBePhysicallyValid().
The associated principal moments of inertia:
0  0  1
do not satisfy the triangle inequality.
Substituting a plausible-guess inertia '
 mass = 0.5
 Center of mass = [0  0  0]
 Inertia about point P, I_BP =
[0.000484861            0            0]
[          0  0.000484861            0]
[          0            0  0.000484861]
' instead.
Parsed 1 models.

First of all, as a nit: the single quotes are useless. It should be something like "... plausible inertia instead:\n" and then the string output.

What's still not clear to me though is why adding even more voluminous text to the parser output is to the user's advantage.

If we printed out an URL XML stanza with a suggested fix, I could probably buy that. But as it stands, what do you think a user would do with this extra information? To me, it's just clutter that makes it more difficult to find the more important messages that will also be nearby.

Done.

[jwnimmer-tri]

Reviewed 1 of 1 files at r10, all commit messages.
Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

[mitiguy]

@rpoyner-tri Looks good. Overly generous with co-author credit -- but I like the idea that I am co-responsible for what follows :)

Reviewable status: LGTM missing from assignees ggould-tri(platform),mitiguy (waiting on @rpoyner-tri)

[mitiguy]

Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @rpoyner-tri)

[rpoyner-tri]

It's Monday, and @ggould-tri stamp is still blocking this patch.

Reviewable status: LGTM missing from assignee ggould-tri(platform) (waiting on @ggould-tri)

[ggould-tri]

Uh, didn't realize this was blocking. すみません

Reviewed 1 of 1 files at r10, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),jwnimmer-tri(platform),mitiguy