[update] add ctrl_dual_arm_go_home function

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]])

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]

teleop/robot_control/robot_arm.py

@@ -181,6 +181,20 @@ class G1_29_ArmController:
         '''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.'''
         self._gradual_start_time = time.time()

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()
+        if args.record:
             recorder.close()
         print("Finally, exiting program...")
         exit(0)

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):