diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py index 66ef23c..ab8579e 100644 --- a/teleop/open_television/_test_tv_wrapper.py +++ b/teleop/open_television/_test_tv_wrapper.py @@ -35,7 +35,7 @@ def run_test_tv_wrapper(): teleData = tv_wrapper.get_motion_state_data() logger_mp.info("=== TeleData Snapshot ===") - logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_rotation}") + logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_pose}") logger_mp.info(f"[Left EE Pose]:\n{teleData.left_arm_pose}") logger_mp.info(f"[Right EE Pose]:\n{teleData.right_arm_pose}") diff --git a/teleop/open_television/setup.py b/teleop/open_television/setup.py index 6799080..dc471af 100644 --- a/teleop/open_television/setup.py +++ b/teleop/open_television/setup.py @@ -14,7 +14,7 @@ setup( 'aiohttp_cors==0.7.0', 'aiortc==1.8.0', 'av==11.0.0', - # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. + # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. but it not support controller tracking. # 'vuer==0.0.35', # avp hand tracking works fine. pico is fine too, but the right eye occasionally goes black for a short time at start. @@ -32,11 +32,17 @@ setup( # from 'vuer==0.0.46' # avp hand tracking work fine. # to - 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button + # 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button # causes the screen to stay black and stuck loading. But if the button is clicked with the controller instead, everything works fine. # In pico controller tracking mode, using controller to click the "Virtual Reality" button # causes the screen to stay black and stuck loading. But if the button is clicked with a hand gesture instead, everything works fine. # avp \ pico all have hand marker visualization (RGB three-axis color coordinates). + + 'vuer==0.0.60', # a good version + # https://github.com/unitreerobotics/avp_teleoperate/issues/53 + # https://github.com/vuer-ai/vuer/issues/45 + # https://github.com/vuer-ai/vuer/issues/65 + ], python_requires='>=3.8', ) \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index d891fb9..6cba8a5 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -206,8 +206,8 @@ class TeleVision: MotionControllers( stream=True, key="motionControllers", - showLeft=False, - showRight=False, + left=True, + right=True, ), to="bgChildren", ) diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index cc2e1f4..92ca634 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -166,7 +166,7 @@ class TeleStateData: @dataclass class TeleData: - head_rotation: np.ndarray # (3,3) Head orientation matrix + head_pose: np.ndarray # (4,4) SE(3) pose of head matrix left_arm_pose: np.ndarray # (4,4) SE(3) pose of left arm right_arm_pose: np.ndarray # (4,4) SE(3) pose of right arm left_hand_pos: np.ndarray = None # (25,3) 3D positions of left hand joints @@ -259,7 +259,6 @@ class TeleVisionWrapper: right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_world_arm[0:3, 3] - Brobot_world_head[0:3, 3] # =====coordinate origin offset===== - Brobot_world_head_rot = Brobot_world_head[:3, :3] # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to visualize it. # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. @@ -340,7 +339,7 @@ class TeleVisionWrapper: hand_state = None return TeleData( - head_rotation=Brobot_world_head_rot, + head_pose=Brobot_world_head, left_arm_pose=left_IPunitree_Brobot_waist_arm, right_arm_pose=right_IPunitree_Brobot_waist_arm, left_hand_pos=left_IPunitree_Brobot_arm_hand_pos, @@ -368,7 +367,6 @@ class TeleVisionWrapper: right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] # =====coordinate origin offset===== - Brobot_world_head_rot = Brobot_world_head[:3, :3] # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to check it. # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. @@ -403,7 +401,7 @@ class TeleVisionWrapper: controller_state = None return TeleData( - head_rotation=Brobot_world_head_rot, + head_pose=Brobot_world_head, left_arm_pose=left_IPunitree_Brobot_waist_arm, right_arm_pose=right_IPunitree_Brobot_waist_arm, left_trigger_value=10.0 - self.tvuer.left_controller_trigger_value * 10,