public void UpdateCurrentPose(Transform transform) { current_pose.position = transform.position; current_pose.orientation = transform.rotation; // Offset position to initial frame current_pose.position -= initial_pose.position; current_pose.position = Quaternion.Inverse(initial_pose.orientation) * current_pose.position; current_pose.orientation = QuaternionUtil.DispQ(initial_pose.orientation, current_pose.orientation); // Convert to robot frame current_pose.position = UnityToRobotFramePosition(current_pose.position); current_pose.orientation = UnityToRobotFrameOrientation(current_pose.orientation); }