Skip to content

[Admittance] applies control frame transform to mass matrix #1139

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

[Admittance] applies control frame transform to mass matrix #1139

Changes from 4 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
c544c64
fix: applies control frame transform to mass matrix in admittance con…
MarcoMagriDev May 22, 2024
6f16aa9
Merge branch 'master' into fix-admittance-controller-mass
MarcoMagriDev Jul 5, 2024
24b8ba4
Merge branch 'ros-controls:master' into fix-admittance-controller-mass
MarcoMagriDev Apr 23, 2025
86cabfa
fix: avoids compliance matrices change of coordinates
MarcoMagriDev Apr 24, 2025
87dc29c
Update comment
christophfroehlich Jul 25, 2025
5f9dbe9
Merge branch 'master' into fix-admittance-controller-mass
christophfroehlich Jul 25, 2025
6d280cc
Fix format
christophfroehlich Jul 25, 2025
bdb9ad6
Merge branch 'master' into fix-admittance-controller-mass
christophfroehlich Aug 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 16 additions & 27 deletions admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -235,30 +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<double, 6, 6> rot_base_control_6d = Eigen::Matrix<double, 6, 6>::Zero();
rot_base_control_6d.topLeftCorner<3, 3>() = rot_base_control;
rot_base_control_6d.bottomRightCorner<3, 3>() = rot_base_control;
Eigen::Matrix<double, 6, 6> K = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 3, 3> K_pos = Eigen::Matrix<double, 3, 3>::Zero();
Eigen::Matrix<double, 3, 3> K_rot = Eigen::Matrix<double, 3, 3>::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<double, 6, 6> D = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 3, 3> D_pos = Eigen::Matrix<double, 3, 3>::Zero();
Eigen::Matrix<double, 3, 3> D_rot = Eigen::Matrix<double, 3, 3>::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<double, 6, 6> M_inv = Eigen::Matrix<double, 6, 6>::Zero();
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;
Expand All @@ -280,16 +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<double, 6, 1> 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<double, 6, 1> 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<double, 6, 1> X_ddot =
admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X);
Eigen::Matrix<double, 6, 1> 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);
Copy link
Contributor

@pac48 pac48 Apr 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks good, but it might be a good idea to update the comment "F = Mx_ddot + Dx_dot + K*x"

Copy link
Contributor

@christophfroehlich christophfroehlich Jun 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@MarcoMagriDev could you fix the comment as requested please?

Copy link
Contributor

@christophfroehlich christophfroehlich Jul 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added my proposal to finish this.

Copy link
Contributor

@christophfroehlich christophfroehlich Aug 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pac48 could you give this a quick review please?

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);
Expand Down
Loading