feat: decouple translation from rotation in admittance MSD integration

[Nabil-Miri]

Fix: SE(3) integration bug causing arc swing on pure rotation commands

Problem

Hello, commanding a pure end effector rotation using the CRISP Cartesian Admittance Controller caused the robot to swing sideways in an arc and return, rather than rotating in place. No translation was commanded, yet the EE position was being displaced.

Root cause

I am not very familiar with exp6 but the change fixed the problem. The admittance integration step updated the internal target pose with exp6 and a left multiplication:

pinocchio::SE3 delta = pinocchio::exp6(pinocchio::Motion(inner_motion_ * dt));
inner_SE3_ = delta * inner_SE3_;

Left multiplying applies delta in the world frame. For a pure angular velocity, exp6 produces a rotation R_delta with zero translation, so the existing target position is rotated about the base origin:

t_new = R_delta * t_old

Fix

Decouple the translation and rotation integration, matching the physically independent channels of the admittance MSD:

inner_SE3_.translation() += inner_motion_.head(3) * dt;
inner_SE3_.rotation() =  (pinocchio::exp3(Eigen::Vector3d(inner_motion_.tail(3) * dt))   * inner_SE3_.rotation()).eval();

• Translation uses a plain world frame Euler step, zero if no translation is commanded.
• Rotation uses exp3 (Rodrigues), which spins the orientation in place and never touches the position.
• The two channels stay independent, as the MSD model assumes.

Testing

Validated on a UR5e.

(Will add the videos soon)

BTW @domrachev03 Did you face such problem ?

[danielsanjosepro]

Hey! Thanks for the contribution. Yes the change makes sense, coupled translation and rotation can cause "unnatural" motions when only translating or rotating. Maybe to keep everything backwards compatible (because the previous code was not "wrong") we could add a parameter to choose the mode. What do you thunk about it?

[Nabil-Miri]

Hi Daniel,

I'm not deeply familiar with the exp6 internals, but as I understand it: when ω = 0 there is no difference between the two, and its only under rotation that exp6 introduces the extra translational component.

For backwards compatibility, adding a param is a nice idea. Regarding the default param, would it make sense to default to the new (decoupled) behavior and keep the old coupled exp6 as an opt-in flag? Otherwise new users would still face this issue unless they explicitly enable it. What do you think is more suitable?

[danielsanjosepro]

Yes lets make the default the decoupled version and keep it as a minor release:)

[Nabil-Miri]

I added the new param and changed the PR name from fix: to feat: for the minor release.
Is that correct?

[domrachev03]

Hi @Nabil-Miri! While I did not face this issue, I recall that this is a common problem; I should have noticed it myself. Thanks for pointing it out and fixing!