diff --git a/teleop/open_television/constants.py b/teleop/open_television/constants.py index 0d771ed..4b13257 100644 --- a/teleop/open_television/constants.py +++ b/teleop/open_television/constants.py @@ -28,13 +28,26 @@ const_head_vuer_mat = np.array([[1, 0, 0, 0], [0, 0, 1, -0.2], [0, 0, 0, 1]]) -const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.5], - [0, 1, 0, 1], - [0, 0, 1, -0.5], + +# For G1 initial position +const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.15], + [0, 1, 0, 1.13], + [0, 0, 1, -0.3], [0, 0, 0, 1]]) -const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.5], - [0, 1, 0, 1], - [0, 0, 1, -0.5], +# For G1 initial position +const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.15], + [0, 1, 0, 1.13], + [0, 0, 1, -0.3], [0, 0, 0, 1]]) +# legacy +# const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.5], +# [0, 1, 0, 1], +# [0, 0, 1, -0.5], +# [0, 0, 0, 1]]) + +# const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.5], +# [0, 1, 0, 1], +# [0, 0, 1, -0.5], +# [0, 0, 0, 1]]) diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index 9f0b733..1363ba5 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -72,9 +72,9 @@ class TeleVisionWrapper: # --------------------------------wrist------------------------------------- # TeleVision obtains a basis coordinate that is OpenXR Convention - head_vuer_mat = mat_update(const_head_vuer_mat, self.tv.head_matrix.copy()) - left_wrist_vuer_mat = mat_update(const_left_wrist_vuer_mat, self.tv.left_hand.copy()) - right_wrist_vuer_mat = mat_update(const_right_wrist_vuer_mat, self.tv.right_hand.copy()) + head_vuer_mat, head_flag = mat_update(const_head_vuer_mat, self.tv.head_matrix.copy()) + left_wrist_vuer_mat, left_wrist_flag = mat_update(const_left_wrist_vuer_mat, self.tv.left_hand.copy()) + right_wrist_vuer_mat, right_wrist_flag = mat_update(const_right_wrist_vuer_mat, self.tv.right_hand.copy()) # Change basis convention: VuerMat ((basis) OpenXR Convention) to WristMat ((basis) Robot Convention) # p.s. WristMat = T_{robot}_{openxr} * VuerMat * T_{robot}_{openxr}^T @@ -94,8 +94,8 @@ class TeleVisionWrapper: # Change wrist convention: WristMat ((Left Wrist) XR/AppleVisionPro Convention) to UnitreeWristMat((Left Wrist URDF) Unitree Convention) # Reason for right multiply (T_to_unitree_left_wrist) : Rotate 90 degrees counterclockwise about its own x-axis. # Reason for right multiply (T_to_unitree_right_wrist): Rotate 90 degrees clockwise about its own x-axis. - unitree_left_wrist = left_wrist_mat @ T_to_unitree_left_wrist - unitree_right_wrist = right_wrist_mat @ T_to_unitree_right_wrist + unitree_left_wrist = left_wrist_mat @ (T_to_unitree_left_wrist if left_wrist_flag else np.eye(4)) + unitree_right_wrist = right_wrist_mat @ (T_to_unitree_right_wrist if right_wrist_flag else np.eye(4)) # Transfer from WORLD to HEAD coordinate (translation only). unitree_left_wrist[0:3, 3] = unitree_left_wrist[0:3, 3] - head_mat[0:3, 3] diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index e72da77..bb4e2d7 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -180,6 +180,20 @@ class G1_29_ArmController: def get_current_dual_arm_dq(self): '''Return current state dq of the left and right arm motors.''' return np.array([self.lowstate_buffer.GetData().motor_state[id].dq for id in G1_29_JointArmIndex]) + + def ctrl_dual_arm_go_home(self): + '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' + print("[G1_29_ArmController] ctrl_dual_arm_go_home start...") + with self.ctrl_lock: + self.q_target = np.zeros(14) + self.tauff_target = np.zeros(14) + tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements + while True: + current_q = self.get_current_dual_arm_q() + if np.all(np.abs(current_q) < tolerance): + print("[G1_29_ArmController] both arms have reached the home position.") + break + time.sleep(0.05) def speed_gradual_max(self, t = 5.0): '''Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0.''' diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 7f89057..775e915 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -242,11 +242,13 @@ if __name__ == '__main__': except KeyboardInterrupt: print("KeyboardInterrupt, exiting program...") finally: + arm_ctrl.ctrl_dual_arm_go_home() tv_img_shm.unlink() tv_img_shm.close() if WRIST: wrist_img_shm.unlink() wrist_img_shm.close() - recorder.close() + if args.record: + recorder.close() print("Finally, exiting program...") exit(0) \ No newline at end of file diff --git a/teleop/utils/mat_tool.py b/teleop/utils/mat_tool.py index d3ad531..349489f 100644 --- a/teleop/utils/mat_tool.py +++ b/teleop/utils/mat_tool.py @@ -2,9 +2,9 @@ import numpy as np def mat_update(prev_mat, mat): if np.linalg.det(mat) == 0: - return prev_mat + return prev_mat, False # Return previous matrix and False flag if the new matrix is non-singular (determinant ≠ 0). else: - return mat + return mat, True def fast_mat_inv(mat):