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