From c544c64831cd495371ffe397056dc472fda989ed Mon Sep 17 00:00:00 2001 From: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com> Date: Wed, 22 May 2024 14:24:22 +0200 Subject: [PATCH 1/4] fix: applies control frame transform to mass matrix in admittance controller --- .../admittance_controller/admittance_rule_impl.hpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9b03924882..e217a9f903 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -248,6 +248,17 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat D.block<3, 3>(0, 0) = D_pos; D.block<3, 3>(3, 3) = D_rot; + // The same for mass + Eigen::Matrix M_inv = Eigen::Matrix::Zero(); + Eigen::Matrix M_inv_pos = Eigen::Matrix::Zero(); + Eigen::Matrix M_inv_rot = Eigen::Matrix::Zero(); + M_inv_pos.diagonal() = admittance_state.mass_inv.block<3, 1>(0, 0); + M_inv_rot.diagonal() = admittance_state.mass_inv.block<3, 1>(3, 0); + M_inv_pos = rot_base_control * M_inv_pos * rot_base_control.transpose(); + M_inv_rot = rot_base_control * M_inv_rot * rot_base_control.transpose(); + M_inv.block<3, 3>(0, 0) = M_inv_pos; + M_inv.block<3, 3>(3, 3) = M_inv_rot; + // calculate admittance relative offset in base frame Eigen::Isometry3d desired_trans_base_ft; kinematics_->calculate_link_transform( @@ -276,8 +287,7 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x - Eigen::Matrix X_ddot = - admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); + Eigen::Matrix X_ddot = M_inv * (F_base - D * X_dot - K * X); bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, admittance_state.joint_acc); From 86cabfa039886085cf22dad4b8208092f020a9bd Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 24 Apr 2025 14:25:55 +0200 Subject: [PATCH 2/4] fix: avoids compliance matrices change of coordinates --- .../admittance_rule_impl.hpp | 47 +++++-------------- 1 file changed, 13 insertions(+), 34 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index b495214e10..629e484009 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -235,41 +235,22 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness // correspond to the six diagonal elements of the stiffness matrix expressed in the control frame auto rot_base_control = admittance_state.rot_base_control; + Eigen::Matrix rot_base_control_6d = Eigen::Matrix::Zero(); + rot_base_control_6d.topLeftCorner<3, 3>() = rot_base_control; + rot_base_control_6d.bottomRightCorner<3, 3>() = rot_base_control; Eigen::Matrix K = Eigen::Matrix::Zero(); - Eigen::Matrix K_pos = Eigen::Matrix::Zero(); - Eigen::Matrix K_rot = Eigen::Matrix::Zero(); - K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); - K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); - // Transform to the control frame - // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf - // Force Control by Luigi Villani and Joris De Schutter - // Page 200 - K_pos = rot_base_control * K_pos * rot_base_control.transpose(); - K_rot = rot_base_control * K_rot * rot_base_control.transpose(); - K.block<3, 3>(0, 0) = K_pos; - K.block<3, 3>(3, 3) = K_rot; + K.block<3, 3>(0, 0).diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K.block<3, 3>(3, 3).diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); // The same for damping Eigen::Matrix D = Eigen::Matrix::Zero(); - Eigen::Matrix D_pos = Eigen::Matrix::Zero(); - Eigen::Matrix D_rot = Eigen::Matrix::Zero(); - D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); - D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); - D_pos = rot_base_control * D_pos * rot_base_control.transpose(); - D_rot = rot_base_control * D_rot * rot_base_control.transpose(); - D.block<3, 3>(0, 0) = D_pos; - D.block<3, 3>(3, 3) = D_rot; + D.block<3, 3>(0, 0).diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D.block<3, 3>(3, 3).diagonal() = admittance_state.damping.block<3, 1>(3, 0); // The same for mass Eigen::Matrix M_inv = Eigen::Matrix::Zero(); - Eigen::Matrix M_inv_pos = Eigen::Matrix::Zero(); - Eigen::Matrix M_inv_rot = Eigen::Matrix::Zero(); - M_inv_pos.diagonal() = admittance_state.mass_inv.block<3, 1>(0, 0); - M_inv_rot.diagonal() = admittance_state.mass_inv.block<3, 1>(3, 0); - M_inv_pos = rot_base_control * M_inv_pos * rot_base_control.transpose(); - M_inv_rot = rot_base_control * M_inv_rot * rot_base_control.transpose(); - M_inv.block<3, 3>(0, 0) = M_inv_pos; - M_inv.block<3, 3>(3, 3) = M_inv_rot; + M_inv.block<3, 3>(0, 0).diagonal() = admittance_state.mass_inv.block<3, 1>(0, 0); + M_inv.block<3, 3>(3, 3).diagonal() = admittance_state.mass_inv.block<3, 1>(3, 0); // calculate admittance relative offset in base frame Eigen::Isometry3d desired_trans_base_ft; @@ -291,15 +272,13 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat auto F_base = admittance_state.wrench_base; // zero out any forces in the control frame - Eigen::Matrix F_control; - F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); - F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + Eigen::Matrix F_control = rot_base_control_6d.transpose() * F_base; F_control = F_control.cwiseProduct(admittance_state.selected_axes); - F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); - F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x - Eigen::Matrix X_ddot = M_inv * (F_base - D * X_dot - K * X); + Eigen::Matrix X_ddot = rot_base_control_6d * M_inv * + (F_control - D * rot_base_control_6d.transpose() * X_dot - + K * rot_base_control_6d.transpose() * X); bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, admittance_state.joint_acc); From 87dc29c555169556d813dd53cff396ca51d6aa76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 25 Jul 2025 09:24:13 +0200 Subject: [PATCH 3/4] Update comment --- .../include/admittance_controller/admittance_rule_impl.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 629e484009..b5ca8e61c1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -275,7 +275,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat Eigen::Matrix F_control = rot_base_control_6d.transpose() * F_base; F_control = F_control.cwiseProduct(admittance_state.selected_axes); - // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x + // Compute admittance control law in the base frame: F_control = M*R^T*x_ddot + D*R^T*x_dot + K*R^T*x + // with R being the rotation matrix from base to control frame Eigen::Matrix X_ddot = rot_base_control_6d * M_inv * (F_control - D * rot_base_control_6d.transpose() * X_dot - K * rot_base_control_6d.transpose() * X); From 6d280cc5e35bc7c69343f89703a8b6b335dd0e87 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 25 Jul 2025 07:44:23 +0000 Subject: [PATCH 4/4] Fix format --- .../include/admittance_controller/admittance_rule_impl.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 6c39105133..ae50592bb1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -267,7 +267,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat Eigen::Matrix F_control = rot_base_control_6d.transpose() * F_base; F_control = F_control.cwiseProduct(admittance_state.selected_axes); - // Compute admittance control law in the base frame: F_control = M*R^T*x_ddot + D*R^T*x_dot + K*R^T*x + // Compute admittance control law in the base frame: + // F_control = M*R^T*x_ddot + D*R^T*x_dot + K*R^T*x, // with R being the rotation matrix from base to control frame Eigen::Matrix X_ddot = rot_base_control_6d * M_inv * (F_control - D * rot_base_control_6d.transpose() * X_dot -