feat: decouple translation from rotation in admittance MSD integration

src/cartesian_admittance_controller.cpp

@@ -170,12 +170,19 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D
   Eigen::Vector<double, 6> adm_force = ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error;
   Eigen::Vector<double, 6> accel = adm_mass_inv_ * adm_force;
 
-  // 10. Semi-implicit Euler integration
+  // 10. Semi-implicit Euler integration of the admittance target
   double dt = period.seconds();
   inner_motion_ += accel * dt;
-  // Update inner_SE3_ using exponential map
-  pinocchio::SE3 delta = pinocchio::exp6(pinocchio::Motion(inner_motion_ * dt));
-  inner_SE3_ = delta * inner_SE3_;
+  if (params_.coupled_se3_integration) {
+    // Legacy: exp6 left-multiply couples translation and rotation.
+    pinocchio::SE3 delta = pinocchio::exp6(pinocchio::Motion(inner_motion_ * dt));
+    inner_SE3_ = delta * inner_SE3_;
+  } else {
+    // Default: integrate translation (Euler) and rotation (exp3) independently.
+    inner_SE3_.translation() += inner_motion_.head(3) * dt;
+    inner_SE3_.rotation() =
+      (pinocchio::exp3(Eigen::Vector3d(inner_motion_.tail(3) * dt)) * inner_SE3_.rotation()).eval();
+  }
 
   // 11. Use inner_SE3_ as the desired for impedance outer loop
   // Compute impedance error (same as CartesianController but using inner_SE3_ as target)

src/cartesian_admittance_controller.yaml

@@ -194,6 +194,11 @@ cartesian_admittance_controller:
     default_value: false
     description: "If true, we use the local jacobian for computations, otherwise we use the world jacobian."
 
+  coupled_se3_integration:
+    type: bool
+    default_value: false
+    description: "If true, use legacy exp6 integration that couples translation and rotation. If false (default), integrate translation (Euler) and rotation (exp3) independently so a pure rotation spins in place."
+
   topics:
     target_pose:
       type: string