[update] README [fix] teleop/

README.md

@@ -23,6 +23,7 @@ conda activate tv
 # If you use `pip install`, Make sure pinocchio version is 3.1.0
 conda install pinocchio -c conda-forge
 pip install meshcat
+pip install casadi
 ```
 
 ## unitree_dds_wrapper
@@ -31,7 +32,7 @@ pip install meshcat
 # Install the Python version of the unitree_dds_wrapper.
 git clone https://github.com/unitreerobotics/unitree_dds_wrapper.git
 cd unitree_dds_wrapper/python
-pip3 install -e .
+pip install -e .
 ```
 
 

teleop/Preprocessor.py

@@ -1,7 +1,7 @@
 import math
 import numpy as np
 
-from constants_vuer import grd_yup2grd_zup, hand2inspire_l_arm, hand2inspire_r_arm,hand2inspire_l_finger,hand2inspire_r_finger
+from constants_vuer import grd_yup2grd_zup, hand2inspire_l_arm, hand2inspire_r_arm,hand2inspire_l_finger,hand2inspire_r_finger,hand2inspire
 from motion_utils import mat_update, fast_mat_inv
 
 
@@ -74,3 +74,73 @@ class VuerPreprocessor:
 
         return all_fingers
 
+
+class VuerPreprocessorLegacy:
+    def __init__(self):
+        self.vuer_head_mat = np.array([[1, 0, 0, 0],
+                                  [0, 1, 0, 1.5],
+                                  [0, 0, 1, -0.2],
+                                  [0, 0, 0, 1]])
+        self.vuer_right_wrist_mat = np.array([[1, 0, 0, 0.5],
+                                         [0, 1, 0, 1],
+                                         [0, 0, 1, -0.5],
+                                         [0, 0, 0, 1]])
+        self.vuer_left_wrist_mat = np.array([[1, 0, 0, -0.5],
+                                        [0, 1, 0, 1],
+                                        [0, 0, 1, -0.5],
+                                        [0, 0, 0, 1]])
+
+    def process(self, tv):
+        self.vuer_head_mat = mat_update(self.vuer_head_mat, tv.head_matrix.copy())
+        self.vuer_right_wrist_mat = mat_update(self.vuer_right_wrist_mat, tv.right_hand.copy())
+        self.vuer_left_wrist_mat = mat_update(self.vuer_left_wrist_mat, tv.left_hand.copy())
+
+        # change of basis
+        head_mat = grd_yup2grd_zup @ self.vuer_head_mat @ fast_mat_inv(grd_yup2grd_zup)
+        right_wrist_mat = grd_yup2grd_zup @ self.vuer_right_wrist_mat @ fast_mat_inv(grd_yup2grd_zup)
+        left_wrist_mat = grd_yup2grd_zup @ self.vuer_left_wrist_mat @ fast_mat_inv(grd_yup2grd_zup)
+
+        rel_left_wrist_mat = left_wrist_mat @ hand2inspire
+        rel_left_wrist_mat[0:3, 3] = rel_left_wrist_mat[0:3, 3] - head_mat[0:3, 3]
+
+        rel_right_wrist_mat = right_wrist_mat @ hand2inspire  # wTr = wTh @ hTr
+        rel_right_wrist_mat[0:3, 3] = rel_right_wrist_mat[0:3, 3] - head_mat[0:3, 3]
+
+        # homogeneous
+        left_fingers = np.concatenate([tv.left_landmarks.copy().T, np.ones((1, tv.left_landmarks.shape[0]))])
+        right_fingers = np.concatenate([tv.right_landmarks.copy().T, np.ones((1, tv.right_landmarks.shape[0]))])
+
+        # change of basis
+        left_fingers = grd_yup2grd_zup @ left_fingers
+        right_fingers = grd_yup2grd_zup @ right_fingers
+
+        rel_left_fingers = fast_mat_inv(left_wrist_mat) @ left_fingers
+        rel_right_fingers = fast_mat_inv(right_wrist_mat) @ right_fingers
+        rel_left_fingers = (hand2inspire.T @ rel_left_fingers)[0:3, :].T
+        rel_right_fingers = (hand2inspire.T @ rel_right_fingers)[0:3, :].T
+
+        return head_mat, rel_left_wrist_mat, rel_right_wrist_mat, rel_left_fingers, rel_right_fingers
+
+    def get_hand_gesture(self, tv):
+        self.vuer_right_wrist_mat = mat_update(self.vuer_right_wrist_mat, tv.right_hand.copy())
+        self.vuer_left_wrist_mat = mat_update(self.vuer_left_wrist_mat, tv.left_hand.copy())
+
+        # change of basis
+        right_wrist_mat = grd_yup2grd_zup @ self.vuer_right_wrist_mat @ fast_mat_inv(grd_yup2grd_zup)
+        left_wrist_mat = grd_yup2grd_zup @ self.vuer_left_wrist_mat @ fast_mat_inv(grd_yup2grd_zup)
+
+        left_fingers = np.concatenate([tv.left_landmarks.copy().T, np.ones((1, tv.left_landmarks.shape[0]))])
+        right_fingers = np.concatenate([tv.right_landmarks.copy().T, np.ones((1, tv.right_landmarks.shape[0]))])
+
+        # change of basis
+        left_fingers = grd_yup2grd_zup @ left_fingers
+        right_fingers = grd_yup2grd_zup @ right_fingers
+
+        rel_left_fingers = fast_mat_inv(left_wrist_mat) @ left_fingers
+        rel_right_fingers = fast_mat_inv(right_wrist_mat) @ right_fingers
+        rel_left_fingers = (hand2inspire.T @ rel_left_fingers)[0:3, :].T
+        rel_right_fingers = (hand2inspire.T @ rel_right_fingers)[0:3, :].T
+        all_fingers = np.concatenate([rel_left_fingers, rel_right_fingers], axis=0)
+
+        return all_fingers
+

teleop/constants_vuer.py

@@ -26,3 +26,10 @@ grd_yup2grd_zup = np.array([[0, 0, -1, 0],
                             [-1, 0, 0, 0],
                             [0, 1, 0, 0],
                             [0, 0, 0, 1]])
+
+# legacy
+hand2inspire = np.array([[0, -1, 0, 0],
+                         [0, 0, -1, 0],
+                         [1, 0, 0, 0],
+                         [0, 0, 0, 1]])
+

teleop/robot_control/robot_arm.py

@@ -89,6 +89,7 @@ class H1ArmController:
         for id in JointIndex:
             self.msg.motor_cmd[id].q = self.lowstate_subscriber.msg.motor_state[id].q
             self.q_target.append(self.msg.motor_cmd[id].q)
+        # print(f"Init q_pose is :{self.q_target}")
 
         duration = 1000
         init_q = np.array([self.lowstate_subscriber.msg.motor_state[id].q for id in JointIndex])
@@ -176,12 +177,11 @@ class H1ArmController:
                   
     def Control(self):
         while True:
-            ms_tmp_ptr_0 = self.motor_state_buffer.GetData()  
-            if ms_tmp_ptr_0: 
+            ms_tmp_ptr = self.motor_state_buffer.GetData()  
+            if ms_tmp_ptr: 
                 tem_q_desList = copy.deepcopy(self.q_desList)
                 tem_q_tau_ff = copy.deepcopy(self.q_tau_ff)
                 motor_command_tmp = MotorCommand()  
-                ms_tmp_ptr = self.motor_state_buffer.GetData()  
                 self.time += self.control_dt  
                 self.time = min(max(self.time, 0.0), self.init_duration)  
                 self.ratio = self.time / self.init_duration  

teleop/teleop_hand.py

@@ -7,7 +7,7 @@ import numpy as np
 import torch
 
 from TeleVision import OpenTeleVision
-from Preprocessor import VuerPreprocessor
+from Preprocessor import VuerPreprocessorLegacy as VuerPreprocessor
 from constants_vuer import tip_indices
 from dex_retargeting.retargeting_config import RetargetingConfig
 from pytransform3d import rotations
@@ -257,16 +257,9 @@ if __name__ == '__main__':
 
     try:
         while True:
-            time1 = time.time()
             head_rmat, left_pose, right_pose, left_qpos, right_qpos = teleoperator.step()
-            time2 = time.time()
-            print(f"TV time:{time2-time1}")
             left_img, right_img = simulator.step(head_rmat, left_pose, right_pose, left_qpos, right_qpos)
-            time3 = time.time()
-            print(f"sim time:{time3-time2}")
             np.copyto(teleoperator.img_array, np.hstack((left_img, right_img)))
-            time4 =time.time()
-            print(f"copy image:{time4-time3},total time:{time4-time1}")
 
     except KeyboardInterrupt:
         simulator.end()

teleop/teleop_hand_and_arm.py

@@ -15,6 +15,12 @@ import zmq
 import pickle
 import zlib
 
+import os 
+import sys
+current_dir = os.path.dirname(os.path.abspath(__file__))
+parent_dir = os.path.dirname(current_dir)
+sys.path.append(parent_dir)
+
 from robot_control.robot_hand import H1HandController
 from teleop.robot_control.robot_arm import H1ArmController
 from teleop.robot_control.robot_arm_ik import Arm_IK