diff --git a/src/cartesian_admittance_controller.cpp b/src/cartesian_admittance_controller.cpp index 7d411ee..e58013d 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 + // 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) diff --git a/src/cartesian_admittance_controller.yaml b/src/cartesian_admittance_controller.yaml index 8bd6366..85a691b 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." + topics: target_pose: type: string