qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

stiffness frame bugfix

Browse Source
This commit is contained in:
QuentinLin
2024-07-22 08:54:55 +09:00
parent fce21f08b9
commit 25e88f56ed
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/joint/robot_interface_franka.cpp
View File

@@ -365,7 +365,7 @@ void RobotInterfaceFranka::_update_robot_state(const franka::RobotState &robot_s
current_joint_forces_array_ = robot_state.tau_J; current_joint_forces_array_ = robot_state.tau_J;
current_joint_forces_ = Eigen::Map<VectorXd>(current_joint_forces_array_.data(), 7); current_joint_forces_ = Eigen::Map<VectorXd>(current_joint_forces_array_.data(), 7);
current_stiffness_force_torque_array_ = robot_state.O_F_ext_hat_K; current_stiffness_force_torque_array_ = robot_state.K_F_ext_hat_K;
current_stiffness_force_[0] = current_stiffness_force_torque_array_[0]; current_stiffness_force_[0] = current_stiffness_force_torque_array_[0];
current_stiffness_force_[1] = current_stiffness_force_torque_array_[1]; current_stiffness_force_[1] = current_stiffness_force_torque_array_[1];
current_stiffness_force_[2] = current_stiffness_force_torque_array_[2]; current_stiffness_force_[2] = current_stiffness_force_torque_array_[2];
@@ -718,6 +718,7 @@ DQ RobotInterfaceFranka::get_stiffness_pose() {
const auto base_2_ee = homgenious_tf_to_DQ(current_effeector_pose_); const auto base_2_ee = homgenious_tf_to_DQ(current_effeector_pose_);
const auto ee_2_k = homgenious_tf_to_DQ(current_stiffness_pose_); const auto ee_2_k = homgenious_tf_to_DQ(current_stiffness_pose_);
return base_2_ee * ee_2_k; return base_2_ee * ee_2_k;
// return base_2_k;
} }
/** /**