From 189e7f0428850a32daffb645116e1c89c553cd35 Mon Sep 17 00:00:00 2001 From: Nabil Miri Date: Sun, 7 Jun 2026 22:59:17 +0200 Subject: [PATCH 1/2] fix: decouple translation from rotation in admittance MSD integration --- src/cartesian_admittance_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index a311e03..b705426 100644 --- a/src/cartesian_admittance_controller.cpp +++ b/src/cartesian_admittance_controller.cpp @@ -170,12 +170,12 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D Eigen::Vector adm_force = ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error; Eigen::Vector accel = adm_mass_inv_ * adm_force; - // 10. Semi-implicit Euler integration + // 10. Semi-implicit Euler integration (translation and rotation integrated separately) 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_; + 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) From 089d95ce8ef155a53196c3bca074f7aafe1087a1 Mon Sep 17 00:00:00 2001 From: Nabil Miri Date: Wed, 10 Jun 2026 11:11:23 +0200 Subject: [PATCH 2/2] feat: add coupled_se3_integration param, default to decoupled --- src/cartesian_admittance_controller.cpp | 15 +++++++++++---- src/cartesian_admittance_controller.yaml | 5 +++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index b705426..35e5edc 100644 --- a/src/cartesian_admittance_controller.cpp +++ b/src/cartesian_admittance_controller.cpp @@ -170,12 +170,19 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D Eigen::Vector adm_force = ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error; Eigen::Vector accel = adm_mass_inv_ * adm_force; - // 10. Semi-implicit Euler integration (translation and rotation integrated separately) + // 10. Semi-implicit Euler integration of the admittance target double dt = period.seconds(); inner_motion_ += accel * dt; - inner_SE3_.translation() += inner_motion_.head(3) * dt; - inner_SE3_.rotation() = - (pinocchio::exp3(Eigen::Vector3d(inner_motion_.tail(3) * dt)) * inner_SE3_.rotation()).eval(); + 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) diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml index 4636a55..8da4e7c 100644 --- a/src/cartesian_admittance_controller.yaml +++ b/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." + log: enabled: type: bool