From 00ad6ea3946071d4161971d1ce25fbcec5f25cd8 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 8 Aug 2024 15:52:28 +0800 Subject: [PATCH] [update] README [fix] teleop/ --- README.md | 3 +- teleop/Preprocessor.py | 72 ++++++++++++++++++++++++++++++- teleop/constants_vuer.py | 9 +++- teleop/robot_control/robot_arm.py | 6 +-- teleop/teleop_hand.py | 9 +--- teleop/teleop_hand_and_arm.py | 6 +++ 6 files changed, 91 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index bffb24d..434c4aa 100644 --- a/README.md +++ b/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 . ``` diff --git a/teleop/Preprocessor.py b/teleop/Preprocessor.py index 74b58c4..a38be3b 100644 --- a/teleop/Preprocessor.py +++ b/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 + diff --git a/teleop/constants_vuer.py b/teleop/constants_vuer.py index 712d22f..ab45e4a 100644 --- a/teleop/constants_vuer.py +++ b/teleop/constants_vuer.py @@ -25,4 +25,11 @@ hand2inspire_r_finger = np.array([[0, -1, 0, 0], grd_yup2grd_zup = np.array([[0, 0, -1, 0], [-1, 0, 0, 0], [0, 1, 0, 0], - [0, 0, 0, 1]]) \ No newline at end of file + [0, 0, 0, 1]]) + +# legacy +hand2inspire = np.array([[0, -1, 0, 0], + [0, 0, -1, 0], + [1, 0, 0, 0], + [0, 0, 0, 1]]) + diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index bc79ccc..88866ea 100644 --- a/teleop/robot_control/robot_arm.py +++ b/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 diff --git a/teleop/teleop_hand.py b/teleop/teleop_hand.py index fe3bec1..9dce152 100644 --- a/teleop/teleop_hand.py +++ b/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() diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index a347995..487db25 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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