# Path: robel/components/tracking/tracker.py
# class TrackerState:
#     """Data class that represents the state of the tracker."""
# 
#     def __init__(self,
#                  pos: Optional[np.ndarray] = None,
#                  rot: Optional[np.ndarray] = None,
#                  rot_euler: Optional[np.ndarray] = None,
#                  vel: Optional[np.ndarray] = None,
#                  angular_vel: Optional[np.ndarray] = None):
#         """Initializes a new state object.
# 
#         Args:
#             pos: The (x, y, z) position of the tracked position. The z-axis
#                 points upwards.
#             rot: The (3x3) rotation matrix of the tracked position.
#             rot_euler: The (rx, ry, rz) Euler rotation of the tracked position.
#             vel: The (x, y, z) velocity of the tracked position. This is only
#                 available for tracked joints.
#             angular_vel: The (rx, ry, rz) angular velocity of the tracked
#                 position. This is only available for tracked joints.
#         """
#         self.pos = pos
#         self.vel = vel
#         self.angular_vel = angular_vel
#         self._rot_mat = rot
#         self._rot_euler = rot_euler
# 
#     @property
#     def rot(self):
#         """Returns the 3x3 rotation matrix."""
#         if self._rot_mat is not None:
#             return self._rot_mat
#         if self._rot_euler is not None:
#             self._rot_mat = euler2mat(*self._rot_euler, axes='rxyz')
#         return self._rot_mat
# 
#     @rot.setter
#     def rot(self, value):
#         self._rot_mat = value
# 
#     @property
#     def rot_euler(self):
#         """Returns the (rx, ry, rz) Euler rotations."""
#         if self._rot_euler is not None:
#             return self._rot_euler
#         if self._rot_mat is not None:
#             self._rot_euler = mat2euler(self.rot, axes='rxyz')
#         return self._rot_euler
# 
#     @rot_euler.setter
#     def rot_euler(self, value):
#         self._rot_euler = value
# 
#     @property
#     def rot_quat(self):
#         """Returns the rotation as a quaternion."""
#         return mat2quat(self.rot)


import collections
import numpy as np
from typing import Optional, Union
from transforms3d.quaternions import quat2mat
from robel.components.tracking.tracker import TrackerState
            new_value = getattr(state, prop)
            if new_value is not None:
                setattr(cached_state, prop, new_value)
            else:
                # Get the cached value.
                setattr(state, prop, getattr(cached_state, prop))
        if not ignore_object_transform:
            state = self.transform_object_state(object_id, state)
        return state

    def get_cached_state(self, object_id: ObjectId) -> TrackerState:
        """Returns the cached state for the object."""
        return self._state_cache[object_id]

    def transform_raw_state(self, state: TrackerState) -> TrackerState:
        """Transforms the given state to the coordinate system."""
        pos = state.pos
        rot = state.rot
        vel = state.vel
        angular_vel = state.angular_vel
        if self._coordinate_transform:
            if pos is not None:
                pos = np.matmul(self._coordinate_transform, pos)
            if rot is not None:
                rot = np.matmul(self._coordinate_transform, rot)
                rot = np.matmul(rot, np.transpose(self._coordinate_transform))
            if vel is not None:
                vel = np.matmul(self._coordinate_transform, vel)
            if angular_vel is not None:
                angular_vel = np.matmul(self._coordinate_transform, angular_vel)

# Path: robel/components/tracking/tracker.py
# class TrackerState:
#     """Data class that represents the state of the tracker."""
# 
#     def __init__(self,
#                  pos: Optional[np.ndarray] = None,
#                  rot: Optional[np.ndarray] = None,
#                  rot_euler: Optional[np.ndarray] = None,
#                  vel: Optional[np.ndarray] = None,
#                  angular_vel: Optional[np.ndarray] = None):
#         """Initializes a new state object.
# 
#         Args:
#             pos: The (x, y, z) position of the tracked position. The z-axis
#                 points upwards.
#             rot: The (3x3) rotation matrix of the tracked position.
#             rot_euler: The (rx, ry, rz) Euler rotation of the tracked position.
#             vel: The (x, y, z) velocity of the tracked position. This is only
#                 available for tracked joints.
#             angular_vel: The (rx, ry, rz) angular velocity of the tracked
#                 position. This is only available for tracked joints.
#         """
#         self.pos = pos
#         self.vel = vel
#         self.angular_vel = angular_vel
#         self._rot_mat = rot
#         self._rot_euler = rot_euler
# 
#     @property
#     def rot(self):
#         """Returns the 3x3 rotation matrix."""
#         if self._rot_mat is not None:
#             return self._rot_mat
#         if self._rot_euler is not None:
#             self._rot_mat = euler2mat(*self._rot_euler, axes='rxyz')
#         return self._rot_mat
# 
#     @rot.setter
#     def rot(self, value):
#         self._rot_mat = value
# 
#     @property
#     def rot_euler(self):
#         """Returns the (rx, ry, rz) Euler rotations."""
#         if self._rot_euler is not None:
#             return self._rot_euler
#         if self._rot_mat is not None:
#             self._rot_euler = mat2euler(self.rot, axes='rxyz')
#         return self._rot_euler
# 
#     @rot_euler.setter
#     def rot_euler(self, value):
#         self._rot_euler = value
# 
#     @property
#     def rot_quat(self):
#         """Returns the rotation as a quaternion."""
#         return mat2quat(self.rot)


# Path: robel/components/tracking/utils/coordinate_system.py
import collections
import numpy as np
from typing import Optional, Union
from transforms3d.quaternions import quat2mat
from robel.components.tracking.tracker import TrackerState

            new_value = getattr(state, prop)
            if new_value is not None:
                setattr(cached_state, prop, new_value)
            else:
                # Get the cached value.
                setattr(state, prop, getattr(cached_state, prop))
        if not ignore_object_transform:
            state = self.transform_object_state(object_id, state)
        return state

    def get_cached_state(self, object_id: ObjectId) -> TrackerState:
        """Returns the cached state for the object."""
        return self._state_cache[object_id]

    def transform_raw_state(self, state: TrackerState) -> TrackerState:
        """Transforms the given state to the coordinate system."""
        pos = state.pos
        rot = state.rot
        vel = state.vel
        angular_vel = state.angular_vel
        if self._coordinate_transform:
            if pos is not None:
                pos = np.matmul(self._coordinate_transform, pos)
            if rot is not None:
                rot = np.matmul(self._coordinate_transform, rot)
                rot = np.matmul(rot, np.transpose(self._coordinate_transform))
            if vel is not None:
                vel = np.matmul(self._coordinate_transform, vel)
            if angular_vel is not None:
                angular_vel = np.matmul(self._coordinate_transform, angular_vel)

        return TrackerState(pos=pos, rot=rot, vel=vel, angular_vel=angular_vel)