void HapticsPSM::compute_force_in_tip_frame(geometry_msgs::Wrench &wrench){
rot_mat6wrt0.setRPY(group->getCurrentRPY().at(0),
group->getCurrentRPY().at(1),
group->getCurrentRPY().at(2));
tf_vec3.setValue(wrench.force.x,wrench.force.y,wrench.force.z);
tf_vec3 = rot_mat6wrt0.transpose() * tf_vec3;
wrench.force.x = tf_vec3.getX();
wrench.force.y = tf_vec3.getY();
wrench.force.z = tf_vec3.getZ();
}
void MTMHaptics::convert_bodyForcetoSpatialForce(geometry_msgs::WrenchStamped &body_wrench){
visualize_haptic_force(body_force_pub);
rot_quat.setX(cur_mtm_pose.orientation.x);
rot_quat.setY(cur_mtm_pose.orientation.y);
rot_quat.setZ(cur_mtm_pose.orientation.z);
rot_quat.setW(cur_mtm_pose.orientation.w);
F7wrt0.setValue(body_wrench.wrench.force.x, body_wrench.wrench.force.y, body_wrench.wrench.force.z);
rot_matrix.setRotation(rot_quat);
F0wrt7 = rot_matrix.transpose() * F7wrt0;
body_wrench.wrench.force.x = F0wrt7.x();
body_wrench.wrench.force.y = F0wrt7.y();
body_wrench.wrench.force.z = F0wrt7.z();
visualize_haptic_force(spatial_force_pub);
}
