RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.19k stars 1.24k forks source link

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

Closed jwnimmer-tri closed 1 month ago

jwnimmer-tri commented 1 month ago

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 Reviewable

jwnimmer-tri commented 1 month ago

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.)

jwnimmer-tri commented 1 month ago

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

jwnimmer-tri commented 1 month ago

@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.

jwnimmer-tri commented 1 month ago

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.