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