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); }