# 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/virtual_reality/device.py
# class VrDevice:
#     """Represents an OpenVR device."""
# 
#     def __init__(self, vr_system, index: int):
#         """Initializes a new VrDevice with the given device index."""
#         self._vr_system = vr_system
#         self._index = index
#         self._value_cache = {}
# 
#     @property
#     def index(self) -> int:
#         """Returns the device index."""
#         return self._index
# 
#     def is_connected(self) -> bool:
#         """Returns whether the device is connected."""
#         return self._vr_system.isTrackedDeviceConnected(self._index)
# 
#     def get_serial(self) -> str:
#         """Returns the serial number of the device."""
#         return self._get_string(openvr.Prop_SerialNumber_String)
# 
#     def get_model(self) -> str:
#         """Returns the model number of the device."""
#         return self._get_string(openvr.Prop_ModelNumber_String)
# 
#     def get_model_name(self) -> str:
#         """Returns the model name of the device."""
#         return self._get_string(openvr.Prop_RenderModelName_String)
# 
#     def get_summary(self) -> str:
#         """Returns a summary of information about the device."""
#         connected = self.is_connected()
#         info = '[{} - {}]'.format(self._index,
#                                   'Connected' if connected else 'Disconnected')
#         if connected:
#             info += ' {} ({})'.format(self.get_model_name(), self.get_serial())
#         return info
# 
#     def _get_string(self, prop_type) -> str:
#         """Returns a string property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getStringTrackedDeviceProperty(
#             self._index, prop_type).decode('utf-8')
#         self._value_cache[prop_type] = value
#         return value
# 
#     def _get_bool(self, prop_type) -> bool:
#         """Returns a boolean property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getBoolTrackedDeviceProperty(
#             self._index, prop_type)[0]
#         self._value_cache[prop_type] = value
#         return value
# 
#     def _get_float(self, prop_type) -> float:
#         """Returns a float property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getFloatTrackedDeviceProperty(
#             self._index, prop_type)[0]
#         self._value_cache[prop_type] = value
#         return value


import numpy as np
import openvr
from robel.components.tracking.tracker import TrackerState
from robel.components.tracking.virtual_reality.device import VrDevice
# See the License for the specific language governing permissions and
# limitations under the License.

"""Pose-related logic for OpenVR devices."""




class VrPoseBatch:
    """Represents a batch of poses calculated by the OpenVR system."""

    def __init__(self, vr_system, time_from_now: float = 0.0):
        """Initializes a new pose batch."""
        self._vr_system = vr_system
        # Query poses for all devices.
        self.poses = self._vr_system.getDeviceToAbsoluteTrackingPose(
            openvr.TrackingUniverseStanding, time_from_now,
            openvr.k_unMaxTrackedDeviceCount)

    def get_state(self, device: VrDevice) -> TrackerState:
        """Returns the tracking state for the given device."""
        if not self.poses[device.index].bPoseIsValid:
            state = TrackerState()
        else:
            vr_pose = np.ctypeslib.as_array(
                self.poses[device.index].mDeviceToAbsoluteTracking[:],
                shape=(3, 4))
            # Check that the pose is valid.
            # If all of the translations are 0, get from the cache.
            assert vr_pose.shape == (3, 4)

# 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/virtual_reality/device.py
# class VrDevice:
#     """Represents an OpenVR device."""
# 
#     def __init__(self, vr_system, index: int):
#         """Initializes a new VrDevice with the given device index."""
#         self._vr_system = vr_system
#         self._index = index
#         self._value_cache = {}
# 
#     @property
#     def index(self) -> int:
#         """Returns the device index."""
#         return self._index
# 
#     def is_connected(self) -> bool:
#         """Returns whether the device is connected."""
#         return self._vr_system.isTrackedDeviceConnected(self._index)
# 
#     def get_serial(self) -> str:
#         """Returns the serial number of the device."""
#         return self._get_string(openvr.Prop_SerialNumber_String)
# 
#     def get_model(self) -> str:
#         """Returns the model number of the device."""
#         return self._get_string(openvr.Prop_ModelNumber_String)
# 
#     def get_model_name(self) -> str:
#         """Returns the model name of the device."""
#         return self._get_string(openvr.Prop_RenderModelName_String)
# 
#     def get_summary(self) -> str:
#         """Returns a summary of information about the device."""
#         connected = self.is_connected()
#         info = '[{} - {}]'.format(self._index,
#                                   'Connected' if connected else 'Disconnected')
#         if connected:
#             info += ' {} ({})'.format(self.get_model_name(), self.get_serial())
#         return info
# 
#     def _get_string(self, prop_type) -> str:
#         """Returns a string property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getStringTrackedDeviceProperty(
#             self._index, prop_type).decode('utf-8')
#         self._value_cache[prop_type] = value
#         return value
# 
#     def _get_bool(self, prop_type) -> bool:
#         """Returns a boolean property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getBoolTrackedDeviceProperty(
#             self._index, prop_type)[0]
#         self._value_cache[prop_type] = value
#         return value
# 
#     def _get_float(self, prop_type) -> float:
#         """Returns a float property of the device."""
#         if prop_type in self._value_cache:
#             return self._value_cache[prop_type]
#         value = self._vr_system.getFloatTrackedDeviceProperty(
#             self._index, prop_type)[0]
#         self._value_cache[prop_type] = value
#         return value


# Path: robel/components/tracking/virtual_reality/poses.py
import numpy as np
import openvr
from robel.components.tracking.tracker import TrackerState
from robel.components.tracking.virtual_reality.device import VrDevice

# See the License for the specific language governing permissions and
# limitations under the License.

"""Pose-related logic for OpenVR devices."""




class VrPoseBatch:
    """Represents a batch of poses calculated by the OpenVR system."""

    def __init__(self, vr_system, time_from_now: float = 0.0):
        """Initializes a new pose batch."""
        self._vr_system = vr_system
        # Query poses for all devices.
        self.poses = self._vr_system.getDeviceToAbsoluteTrackingPose(
            openvr.TrackingUniverseStanding, time_from_now,
            openvr.k_unMaxTrackedDeviceCount)

    def get_state(self, device: VrDevice) -> TrackerState:
        """Returns the tracking state for the given device."""
        if not self.poses[device.index].bPoseIsValid:
            state = TrackerState()
        else:
            vr_pose = np.ctypeslib.as_array(
                self.poses[device.index].mDeviceToAbsoluteTracking[:],
                shape=(3, 4))
            # Check that the pose is valid.
            # If all of the translations are 0, get from the cache.
            assert vr_pose.shape == (3, 4)

            state = TrackerState(pos=vr_pose[:, 3], rot=vr_pose[:, :3])