[multibody] Change "InPlace" functions to return void #21438

jwnimmer-tri · 2024-05-15T23:41:45Z

Closes #21436, as a broader swipe at the same respelling.

Here's a tiny demo program that mimics the problem (see also godbolt):

#include <Eigen/Dense>

using Eigen::Vector3d;
using Vector6d = Eigen::Matrix<double, 6, 1>;

struct SpatialVelocity {
  Vector6d V_;

  const Vector3d& rotational() const {
    return *reinterpret_cast<const Vector3d*>(V_.data());
  }
  Vector3d& rotational() {
    return *reinterpret_cast<Vector3d*>(V_.data());
  }
  const Vector3d& translational() const {
    return *reinterpret_cast<const Vector3d*>(V_.data() + 3);
  }
  Vector3d& translational() {
    return *reinterpret_cast<Vector3d*>(V_.data() + 3);
  }

  SpatialVelocity& ShiftInPlace(const Vector3d& offset) {
    this->translational() += this->rotational().cross(offset);
    return *this;
  }

  SpatialVelocity Shift(const Vector3d& offset) const {
#ifndef FIXED
    return SpatialVelocity(*this).ShiftInPlace(offset);
#else
    SpatialVelocity result(*this);
    result.ShiftInPlace(offset);
    return result;
#endif
  }
};

Vector3d foo(const SpatialVelocity& x) {
  return x.Shift(Vector3d{1, 2, 3}).translational();
}

Here's the change in machine code for the demo:

--- master.asm	2024-05-15 16:41:13.018629991 -0700
+++ pr.asm	2024-05-15 16:41:19.778720632 -0700
@@ -1,27 +1,25 @@
 foo(SpatialVelocity const&):
         movsd   xmm1, QWORD PTR [rsi]
         movsd   xmm2, QWORD PTR [rsi+8]
         mov     rax, rdi
         movsd   xmm0, QWORD PTR .LC0[rip]
         addsd   xmm1, xmm1
         mulsd   xmm0, xmm2
         subsd   xmm1, xmm2
         movapd  xmm2, XMMWORD PTR [rsi+16]
         addsd   xmm1, QWORD PTR [rsi+40]
         movhpd  xmm2, QWORD PTR [rsi]
         mulpd   xmm2, XMMWORD PTR .LC1[rip]
         unpcklpd        xmm0, xmm0
-        movsd   QWORD PTR [rsp-16], xmm1
-        mov     rdx, QWORD PTR [rsp-16]
         movhpd  xmm0, QWORD PTR [rsi+16]
         subpd   xmm0, xmm2
         movupd  xmm2, XMMWORD PTR [rsi+24]
-        mov     QWORD PTR [rdi+16], rdx
+        movsd   QWORD PTR [rdi+16], xmm1
         addpd   xmm0, xmm2
         movups  XMMWORD PTR [rdi], xmm0
         ret

This change is

jwnimmer-tri

+@amcastro-tri for feature review, please.

\CC @mitiguy @sherm1 FYI

Reviewed 10 of 10 files at r1, all commit messages.
Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

sherm1

Great discovery and compelling exposition!
+@sherm1 for feature or platform
with one suggestion

Reviewed 10 of 10 files at r1, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri

multibody/tree/multibody_tree.cc line 2183 at r1 (raw file):

  // Re-express spatial inertia from frame W to frame F.
  const RotationMatrix<T> R_FW = (X_WF.rotation()).inverse();
  return M_SFo_W.ReExpress(R_FW);  // Returns M_SFo_F.

BTW this is a different case from the others. With this change a copy construction seems inevitable. Returning the local variable instead should force elision. How about this:

Suggestion:

M_SFo_W.ReExpressInPlace(R_FW);  // Now M_SFo_F.
return M_SFo_W;  // Returns M_SFo_F.

multibody/tree/multibody_tree.cc line 2324 at r1 (raw file):

  // Shift the spatial momentum from Wo to point P.
  return L_WS_W.Shift(p_WoP_W);

BTW same comment here. Shift in place and then return the local?

jwnimmer-tri · 2024-05-16T00:34:13Z

Great discovery ...

(For the record, cbarcelo first noted this in PR #21436 but it didn't quite register with me when I first read that PR.)

RussTedrake · 2024-05-16T01:08:47Z

multibody/math/spatial_acceleration.h

-        offset, angular_velocity_of_this_frame);
+    SpatialAcceleration<T> result(*this);
+    result.ShiftInPlace(offset, angular_velocity_of_this_frame);
+    return result;


In the other PR, you recommended that we need a clear comment so that nobody reverts the change. If there is no test coverage, presumably we need that here?

jwnimmer-tri

Reviewed 1 of 1 files at r2, all commit messages.
Dismissed @RussTedrake from a discussion.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

In the other PR, you recommended that we need a clear comment so that nobody reverts the change. If there is no test coverage, presumably we need that here?

There will be CI test coverage (eventually, once we move some CI jobs into Production at the end of this PR grain).

multibody/tree/multibody_tree.cc line 2183 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this is a different case from the others. With this change a copy construction seems inevitable. Returning the local variable instead should force elision. How about this:

My intuition was that the machine code would be the same either way, so we should favor the more readable code on the page, which was this one. (Actually, my change failed to add back the clarifying const that was overtaken by the in-place stuf, which I've fixed now in the latest push.)

I can check the machine code dump here to see what exactly happens, though, if you're actually worried?

I don't like it when monogram local variables use lies for their names, with apologies in a nearby comment. I can tolerate it for inner-loop code where every instruction matters, but this function is not such a thing. (We're using a std::set<> up above for goodness sake.)

multibody/tree/multibody_tree.cc line 2324 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW same comment here. Shift in place and then return the local?

Ditto

sherm1

Reviewed 1 of 1 files at r2, all commit messages.
Reviewable status: LGTM missing from assignee amcastro-tri

multibody/tree/multibody_tree.cc line 2183 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

My intuition was that the machine code would be the same either way, so we should favor the more readable code on the page, which was this one. (Actually, my change failed to add back the clarifying const that was overtaken by the in-place stuf, which I've fixed now in the latest push.)

I can check the machine code dump here to see what exactly happens, though, if you're actually worried?

I don't like it when monogram local variables use lies for their names, with apologies in a nearby comment. I can tolerate it for inner-loop code where every instruction matters, but this function is not such a thing. (We're using a std::set<> up above for goodness sake.)

I didn't look at the whole function -- definitely doesn't matter here!

jwnimmer-tri · 2024-05-16T03:19:06Z

We can count Sherm as platform review (thanks!) and leave Alejandro for feature review.

jwnimmer-tri · 2024-05-16T15:52:51Z

@drake-jenkins-bot linux-arm-jammy-unprovisioned-gcc-bazel-experimental-release please

Note that this is expected to fail (for unrelated reasons). We just want some fresh data.

amcastro-tri

wow, amazing finding by @cbarcelo-bdai!

Thanks for finding all other usages of the pattern @jwnimmer-tri.

with a request for documentation.

Reviewed 9 of 10 files at r1, 1 of 1 files at r2, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees amcastro-tri,sherm1(platform)

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

There will be CI test coverage (eventually, once we move some CI jobs into Production at the end of this PR grain).

I still think this line is worth documenting, since the test coverage would only check that ARM doesn't fail.
Assuming this compiler bug is fixed in some future, reverting this to the original version would go unnoticed.
Ditto for all other occurrences.

jwnimmer-tri

Reviewable status: 1 unresolved discussion

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I still think this line is worth documenting, since the test coverage would only check that ARM doesn't fail.
Assuming this compiler bug is fixed in some future, reverting this to the original version would go unnoticed.
Ditto for all other occurrences.

@amcastro-tri Please give me the exact text you want. I can't figure out what the defect you have in mind here is. I don't see any realistic chance that someone would ever change the code in this function, so I'm not sure what we're trying to prevent / guard.

jwnimmer-tri

Reviewable status: 1 unresolved discussion

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

@amcastro-tri Please give me the exact text you want. I can't figure out what the defect you have in mind here is. I don't see any realistic chance that someone would ever change the code in this function, so I'm not sure what we're trying to prevent / guard.

Maybe a better answer is to edit the API docs for the "FooInPlace" functions to remind people never to call them as part of a return statement?

jwnimmer-tri

Reviewable status: 1 unresolved discussion

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Maybe a better answer is to edit the API docs for the "FooInPlace" functions to remind people never to call them as part of a return statement?

Working

Actually, I'll push a new update here soon.

The return value from FooInPlace is just a footgun. It's not used anywhere we care about, and leads to plenty of trouble like this. I'm just going to change the return value to void for all of the FooInPlace functions. They we don't need any comments warning people about the footgun because it's no longer there.

jwnimmer-tri

I pushed a bunch more changes. Please have a look.

-(release notes: none) +(release notes: breaking change)

Reviewed 13 of 13 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion

multibody/math/spatial_acceleration.h line 164 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Working

Actually, I'll push a new update here soon.

The return value from FooInPlace is just a footgun. It's not used anywhere we care about, and leads to plenty of trouble like this. I'm just going to change the return value to void for all of the FooInPlace functions. They we don't need any comments warning people about the footgun because it's no longer there.

Done. It caught a bunch more bugs.

multibody/tree/multibody_tree.cc line 2359 at r3 (raw file):

    const Vector3<T>& p_WoBo_W = X_WB.translation();
    L_WBo_W.ShiftInPlace(-p_WoBo_W);
    L_WS_W += L_WBo_W;  // Actually is `+= L_Wo_W`.

Someone please check me on the monogram here.

mitiguy

Reviewable status: 3 unresolved discussions

multibody/tree/multibody_tree.cc line 2358 at r3 (raw file):

    const RigidTransform<T>& X_WB = pc.get_X_WB(mobod_index);
    const Vector3<T>& p_WoBo_W = X_WB.translation();
    L_WBo_W.ShiftInPlace(-p_WoBo_W);

Spatial momentum has one of the most complex signatures. It involves a body or system, about a point, measured and expressed in a frame. There are shortcuts for general cases, but the one below should be explicit (fully documented). Below is my suggestion.

Suggestion:

  // After ShiftInPlace L_WBo_W is changed to L_WBWo_W, which is B's
  // spatial momentum about point Wo, measured and expressed in frame W.
  L_WBo_W.ShiftInPlace(-p_WoBo_W);  // After this line, L_WBo_W is now L_WBWo_W.

multibody/tree/multibody_tree.cc line 2359 at r3 (raw file):

    const Vector3<T>& p_WoBo_W = X_WB.translation();
    L_WBo_W.ShiftInPlace(-p_WoBo_W);
    L_WS_W += L_WBo_W;  // Actually is `+= L_Wo_W`.

Updated comment with suggested notation.

Suggestion:

   L_WS_W += L_WBo_W;  // Actually is `L_WS_W += L_WBWo_W`.

multibody/tree/multibody_tree.cc line 2359 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Someone please check me on the monogram here.

See above and below.

jwnimmer-tri

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: 2 unresolved discussions

multibody/tree/multibody_tree.cc line 2358 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Spatial momentum has one of the most complex signatures. It involves a body or system, about a point, measured and expressed in a frame. There are shortcuts for general cases, but the one below should be explicit (fully documented). Below is my suggestion.

Done. Thanks! (I nixed "line" so it fits in <80 cols.)

multibody/tree/multibody_tree.cc line 2359 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Updated comment with suggested notation.

Done. Thanks!

mitiguy

Reviewed 1 of 1 files at r4.
Reviewable status: complete! all discussions resolved, LGTM from assignees amcastro-tri,sherm1(platform)

jwnimmer-tri · 2024-05-17T15:24:26Z

Given the change to void I'm going to wait for @amcastro-tri to re-feature-review before I merge this.

I'll also ask please +@rpoyner-tri to do a backfill platform review. Sherm platform reviewed thru r2, but I don't think he's looked at r4. So you could look over the whole thing, or just r2..head.

rpoyner-tri

doc stuff

Reviewed 12 of 13 files at r3, 1 of 1 files at r4, all commit messages.
Reviewable status: 12 unresolved discussions, labeled "do not merge" (waiting on @jwnimmer-tri)

multibody/math/spatial_acceleration.h line 98 at r4 (raw file):

  /// expressed in frame E. The components of A_MC_E are calculated as: <pre>
  ///   α_MC_E = α_MB_E           (angular acceleration of `this` is unchanged).
  ///  a_MCo_E = a_MBo_E + α_MB_E x p_BoCo_E + ω_MB_E x (ω_MB_E x p_BoCo_E)

This return value description is stale.

Code quote:

  /// @retval A_MC_E reference to `this` spatial acceleration which has been
  /// modified to be frame C's spatial acceleration measured in frame M and
  /// expressed in frame E. The components of A_MC_E are calculated as: <pre>
  ///   α_MC_E = α_MB_E           (angular acceleration of `this` is unchanged).
  ///  a_MCo_E = a_MBo_E + α_MB_E x p_BoCo_E + ω_MB_E x (ω_MB_E x p_BoCo_E)

multibody/math/spatial_force.h line 82 at r4 (raw file):

  /// p_BpBq_E must have the same expressed-in frame E as `this` spatial force.
  /// @retval F_Bq_E reference to `this` spatial force which has been modified
  /// to be the spatial force on point Bq of B, expressed in frame E.

This return value description is stale.

Code quote:

  /// @retval F_Bq_E reference to `this` spatial force which has been modified
  /// to be the spatial force on point Bq of B, expressed in frame E.

multibody/math/spatial_momentum.h line 102 at r4 (raw file):

  ///   l_MS_E = l_MS_E         (translational momentum of `this` is unchanged).
  ///  h_MSQ_E = h_MSP_E + p_QP_E x l_MS_E
  ///          = h_MSP_E - p_PQ_E x l_MS_E

This return value description is stale.

Code quote:

  /// @retval L_MSQ_E reference to `this` spatial momentum which has been
  /// modified to be system S's spatial momentum about point Q, measured in
  /// frame M and expressed in frame E. The components of L_MSQ_E are: <pre>
  ///   l_MS_E = l_MS_E         (translational momentum of `this` is unchanged).
  ///  h_MSQ_E = h_MSP_E + p_QP_E x l_MS_E
  ///          = h_MSP_E - p_PQ_E x l_MS_E

multibody/math/spatial_velocity.h line 81 at r4 (raw file):

  /// expressed in frame E. The components of V_MC_E are calculated as: <pre>
  ///  ω_MC_E = ω_MB_E                (angular velocity of `this` is unchanged).
  ///  v_MC_E = v_MB_E + ω_MB_E x p_BoCo_E     (translational velocity changes).

This return value description is stale.

Code quote:

  /// @retval V_MC_E reference to `this` spatial velocity which has been
  /// modified to be frame C's spatial velocity measured in frame M and
  /// expressed in frame E. The components of V_MC_E are calculated as: <pre>
  ///  ω_MC_E = ω_MB_E                (angular velocity of `this` is unchanged).
  ///  v_MC_E = v_MB_E + ω_MB_E x p_BoCo_E     (translational velocity changes).

multibody/tree/articulated_body_inertia.h line 232 at r4 (raw file):

  ///                   articulated body inertia is expressed in.
  /// @returns A reference to `this` articulated body inertia for articulated
  ///          body A but now computed about a new point R.

This return value description is stale.

Code quote:

  /// @returns A reference to `this` articulated body inertia for articulated
  ///          body A but now computed about a new point R.

multibody/tree/rotational_inertia.h line 622 at r4 (raw file):

  /// @param[in] R_AE RotationMatrix relating frames A and E.
  /// @return A reference to `this` rotational inertia about-point P, but
  ///         with `this` now expressed in frame A (instead of frame E).

This return value description is stale.

Code quote:

  /// @return A reference to `this` rotational inertia about-point P, but
  ///         with `this` now expressed in frame A (instead of frame E).

multibody/tree/rotational_inertia.h line 661 at r4 (raw file):

  ///         but with `this` shifted from about-point Bcm to about-point Q.
  ///         i.e., returns I_BQ_E,  B's rotational inertia about-point Bcm
  ///         expressed-in frame E.

This return value description is stale.

Code quote:

  /// @return A reference to `this` rotational inertia expressed-in frame E,
  ///         but with `this` shifted from about-point Bcm to about-point Q.
  ///         i.e., returns I_BQ_E,  B's rotational inertia about-point Bcm
  ///         expressed-in frame E.

multibody/tree/spatial_inertia.h line 756 at r4 (raw file):

  /// @param[in] R_AE Rotation matrix from frame E to frame A.
  /// @returns A reference to `this` rotational inertia about the same point P
  ///          but now re-expressed in frame A, that is, `M_SP_A`.

This return value description is stale.

Code quote:

  /// @returns A reference to `this` rotational inertia about the same point P
  ///          but now re-expressed in frame A, that is, `M_SP_A`.

multibody/tree/spatial_inertia.h line 790 at r4 (raw file):

  ///                   `this` spatial inertia is expressed in.
  /// @returns A reference to `this` spatial inertia for body or composite body
  ///          S but now computed about a new point Q.

This return value description is stale.

Code quote:

  /// @returns A reference to `this` spatial inertia for body or composite body
  ///          S but now computed about a new point Q.

multibody/tree/spatial_inertia.h line 943 at r4 (raw file):

  // @param[in] p_ScmP_E Position vector from Scm to P, expressed-in frame E.
  // @return A reference to M_SP_E, `this` spatial inertia that has been shifted
  // from about-point Scm to about-point P, expressed in frame E.

This return value description is stale.

Code quote:

  // @return A reference to M_SP_E, `this` spatial inertia that has been shifted
  // from about-point Scm to about-point P, expressed in frame E.

multibody/tree/unit_inertia.h line 130 at r4 (raw file):

  ///                     inertia is expressed.
  /// @returns A reference to `this` unit inertia, which has now been taken
  ///          about point Q so can be written as `G_BQ_E`.

This return value description is stale.

Code quote:

  /// @returns A reference to `this` unit inertia, which has now been taken
  ///          about point Q so can be written as `G_BQ_E`.

multibody/tree/unit_inertia.h line 159 at r4 (raw file):

  ///                     `this` inertia is expressed.
  /// @returns A reference to `this` unit inertia, which has now been taken
  ///          about point `Bcm` so can be written as `G_BBcm_E`, or `G_Bcm_E`.

This return value description is stale.

Code quote:

  /// @returns A reference to `this` unit inertia, which has now been taken
  ///          about point `Bcm` so can be written as `G_BBcm_E`, or `G_Bcm_E`.

jwnimmer-tri

Reviewed 8 of 8 files at r5.
Reviewable status: 12 unresolved discussions, labeled "do not merge"

multibody/math/spatial_acceleration.h line 98 at r4 (raw file):