sync recent changes

eval_robot/robot_control/robot_arm_test.py

@@ -0,0 +1,343 @@
+import numpy as np
+import threading
+import time
+from enum import IntEnum
+
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize  # dds
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState  # idl for g1, h1_2
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.utils.crc import CRC
+
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as go_LowCmd, LowState_ as go_LowState  # idl for h1
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+
+import logging_mp
+
+logger_mp = logging_mp.get_logger(__name__)
+
+kTopicLowCommand_Debug = "rt/lowcmd"
+kTopicLowCommand_Motion = "rt/arm_sdk"
+kTopicLowState = "rt/lowstate"
+
+G1_29_Num_Motors = 35
+G1_23_Num_Motors = 35
+H1_2_Num_Motors = 35
+H1_Num_Motors = 20
+
+
+class MotorState:
+    def __init__(self):
+        self.q = None
+        self.dq = None
+
+
+class G1_29_LowState:
+    def __init__(self):
+        self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)]
+
+class DataBuffer:
+    def __init__(self):
+        self.data = None
+        self.lock = threading.Lock()
+
+    def GetData(self):
+        with self.lock:
+            return self.data
+
+    def SetData(self, data):
+        with self.lock:
+            self.data = data
+
+
+class G1_29_ArmController:
+    def __init__(self, motion_mode=False, simulation_mode=False):
+        logger_mp.info("Initialize G1_29_ArmController...")
+        self.q_target = np.zeros(14)
+        self.tauff_target = np.zeros(14)
+        self.motion_mode = motion_mode
+        self.simulation_mode = simulation_mode
+        self.kp_high = 40.0
+        self.kd_high = 3.0
+        self.kp_low = 80.0
+        self.kd_low = 3.0
+        self.kp_wrist = 40.0
+        self.kd_wrist = 1.5
+
+        self.all_motor_q = None
+        self.arm_velocity_limit = 100.0
+        self.control_dt = 1.0 / 250.0
+
+        self._speed_gradual_max = False
+        self._gradual_start_time = None
+        self._gradual_time = None
+
+        # initialize lowcmd publisher and lowstate subscriber
+        if self.simulation_mode:
+            ChannelFactoryInitialize(1)
+        else:
+            ChannelFactoryInitialize(0)
+
+        if self.motion_mode:
+            self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd)
+        else:
+            self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd)
+        self.lowcmd_publisher.Init()
+        self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState)
+        self.lowstate_subscriber.Init()
+        self.lowstate_buffer = DataBuffer()
+
+        # initialize subscribe thread
+        self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state)
+        self.subscribe_thread.daemon = True
+        self.subscribe_thread.start()
+
+        while not self.lowstate_buffer.GetData():
+            time.sleep(0.1)
+            logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...")
+        logger_mp.info("[G1_29_ArmController] Subscribe dds ok.")
+
+        # initialize hg's lowcmd msg
+        self.crc = CRC()
+        self.msg = unitree_hg_msg_dds__LowCmd_()
+        self.msg.mode_pr = 0
+        self.msg.mode_machine = self.get_mode_machine()
+        print(self.msg)
+
+        self.all_motor_q = self.get_current_motor_q()
+        logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n")
+        logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n")
+        logger_mp.info("Lock all joints except two arms...\n")
+
+        arm_indices = set(member.value for member in G1_29_JointArmIndex)
+        for id in G1_29_JointIndex:
+            self.msg.motor_cmd[id].mode = 1
+            if id.value in arm_indices:
+                if self._Is_wrist_motor(id):
+                    self.msg.motor_cmd[id].kp = self.kp_wrist
+                    self.msg.motor_cmd[id].kd = self.kd_wrist
+                else:
+                    self.msg.motor_cmd[id].kp = self.kp_low
+                    self.msg.motor_cmd[id].kd = self.kd_low
+            else:
+                if self._Is_weak_motor(id):
+                    self.msg.motor_cmd[id].kp = self.kp_low
+                    self.msg.motor_cmd[id].kd = self.kd_low
+                else:
+                    self.msg.motor_cmd[id].kp = self.kp_high
+                    self.msg.motor_cmd[id].kd = self.kd_high
+            self.msg.motor_cmd[id].q = self.all_motor_q[id]
+        #print current motor q, kp, kd
+
+        logger_mp.info("Lock OK!\n") #motors are not locked x
+        # for i in range(10000):
+        #     print(self.get_current_motor_q())
+        #     time.sleep(0.05) 
+        # initialize publish thread
+        self.publish_thread = threading.Thread(target=self._ctrl_motor_state)
+        self.ctrl_lock = threading.Lock()
+        self.publish_thread.daemon = True
+        self.publish_thread.start()
+
+        logger_mp.info("Initialize G1_29_ArmController OK!\n")
+
+    def _subscribe_motor_state(self):
+        while True:
+            msg = self.lowstate_subscriber.Read()
+            if msg is not None:
+                lowstate = G1_29_LowState()
+                for id in range(G1_29_Num_Motors):
+                    lowstate.motor_state[id].q = msg.motor_state[id].q
+                    lowstate.motor_state[id].dq = msg.motor_state[id].dq
+                self.lowstate_buffer.SetData(lowstate)
+
+    def clip_arm_q_target(self, target_q, velocity_limit):
+        current_q = self.get_current_dual_arm_q()
+        delta = target_q - current_q
+        motion_scale = np.max(np.abs(delta)) / (velocity_limit * self.control_dt)
+        cliped_arm_q_target = current_q + delta / max(motion_scale, 1.0)
+        return cliped_arm_q_target
+
+    def _ctrl_motor_state(self):
+        if self.motion_mode:
+            self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0
+
+        while True:
+            start_time = time.time()
+            with self.ctrl_lock:
+                arm_q_target = self.q_target
+                arm_tauff_target = self.tauff_target
+
+            if self.simulation_mode:
+                cliped_arm_q_target = arm_q_target
+            else:
+                cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit=self.arm_velocity_limit)
+
+            for idx, id in enumerate(G1_29_JointArmIndex):
+                self.msg.motor_cmd[id].q = cliped_arm_q_target[idx]
+                self.msg.motor_cmd[id].dq = 0
+                self.msg.motor_cmd[id].tau = arm_tauff_target[idx]
+
+            self.msg.crc = self.crc.Crc(self.msg)
+            self.lowcmd_publisher.Write(self.msg)
+
+            if self._speed_gradual_max is True:
+                t_elapsed = start_time - self._gradual_start_time
+                self.arm_velocity_limit = 20.0 + (10.0 * min(1.0, t_elapsed / 5.0))
+
+            current_time = time.time()
+            all_t_elapsed = current_time - start_time
+            sleep_time = max(0, (self.control_dt - all_t_elapsed))
+            time.sleep(sleep_time)
+            # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}")
+            # logger_mp.debug(f"sleep_time:{sleep_time}")
+
+    def ctrl_dual_arm(self, q_target, tauff_target):
+        """Set control target values q & tau of the left and right arm motors."""
+        with self.ctrl_lock:
+            self.q_target = q_target
+            self.tauff_target = tauff_target
+
+    def get_mode_machine(self):
+        """Return current dds mode machine."""
+        return self.lowstate_subscriber.Read().mode_machine
+
+    def get_current_motor_q(self):
+        """Return current state q of all body motors."""
+        return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_29_JointIndex])
+
+    def get_current_dual_arm_q(self):
+        """Return current state q of the left and right arm motors."""
+        return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_29_JointArmIndex])
+
+    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."""
+        logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...")
+        max_attempts = 100
+        current_attempts = 0
+        with self.ctrl_lock:
+            self.q_target = np.zeros(14)
+            #self.q_target[G1_29_JointIndex.kLeftElbow] = 0.5
+            # 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 current_attempts < max_attempts:
+            current_q = self.get_current_dual_arm_q()
+            if np.all(np.abs(current_q) < tolerance):
+                if self.motion_mode:
+                    for weight in np.linspace(1, 0, num=101):
+                        self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight
+                        time.sleep(0.02)
+                logger_mp.info("[G1_29_ArmController] both arms have reached the home position.")
+                break
+            current_attempts += 1
+            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()
+        self._gradual_time = t
+        self._speed_gradual_max = True
+
+    def speed_instant_max(self):
+        """set arms velocity to the maximum value immediately, instead of gradually increasing."""
+        self.arm_velocity_limit = 30.0
+
+    def _Is_weak_motor(self, motor_index):
+        weak_motors = [
+            G1_29_JointIndex.kLeftAnklePitch.value,
+            G1_29_JointIndex.kRightAnklePitch.value,
+            # Left arm
+            G1_29_JointIndex.kLeftShoulderPitch.value,
+            G1_29_JointIndex.kLeftShoulderRoll.value,
+            G1_29_JointIndex.kLeftShoulderYaw.value,
+            G1_29_JointIndex.kLeftElbow.value,
+            # Right arm
+            G1_29_JointIndex.kRightShoulderPitch.value,
+            G1_29_JointIndex.kRightShoulderRoll.value,
+            G1_29_JointIndex.kRightShoulderYaw.value,
+            G1_29_JointIndex.kRightElbow.value,
+        ]
+        return motor_index.value in weak_motors
+
+    def _Is_wrist_motor(self, motor_index):
+        wrist_motors = [
+            G1_29_JointIndex.kLeftWristRoll.value,
+            G1_29_JointIndex.kLeftWristPitch.value,
+            G1_29_JointIndex.kLeftWristyaw.value,
+            G1_29_JointIndex.kRightWristRoll.value,
+            G1_29_JointIndex.kRightWristPitch.value,
+            G1_29_JointIndex.kRightWristYaw.value,
+        ]
+        return motor_index.value in wrist_motors
+
+
+class G1_29_JointArmIndex(IntEnum):
+    # Left arm
+    kLeftShoulderPitch = 15
+    kLeftShoulderRoll = 16
+    kLeftShoulderYaw = 17
+    kLeftElbow = 18
+    kLeftWristRoll = 19
+    kLeftWristPitch = 20
+    kLeftWristyaw = 21
+
+    # Right arm
+    kRightShoulderPitch = 22
+    kRightShoulderRoll = 23
+    kRightShoulderYaw = 24
+    kRightElbow = 25
+    kRightWristRoll = 26
+    kRightWristPitch = 27
+    kRightWristYaw = 28
+
+
+class G1_29_JointIndex(IntEnum):
+    # Left leg
+    kLeftHipPitch = 0
+    kLeftHipRoll = 1
+    kLeftHipYaw = 2
+    kLeftKnee = 3
+    kLeftAnklePitch = 4
+    kLeftAnkleRoll = 5
+
+    # Right leg
+    kRightHipPitch = 6
+    kRightHipRoll = 7
+    kRightHipYaw = 8
+    kRightKnee = 9
+    kRightAnklePitch = 10
+    kRightAnkleRoll = 11
+
+    kWaistYaw = 12 #we're c
+    kWaistRoll = 13
+    kWaistPitch = 14
+
+    # Left arm
+    kLeftShoulderPitch = 15
+    kLeftShoulderRoll = 16
+    kLeftShoulderYaw = 17
+    kLeftElbow = 18
+    kLeftWristRoll = 19
+    kLeftWristPitch = 20
+    kLeftWristyaw = 21
+
+    # Right arm
+    kRightShoulderPitch = 22
+    kRightShoulderRoll = 23
+    kRightShoulderYaw = 24
+    kRightElbow = 25
+    kRightWristRoll = 26
+    kRightWristPitch = 27
+    kRightWristYaw = 28
+
+    # not used
+    kNotUsedJoint0 = 29
+    kNotUsedJoint1 = 30
+    kNotUsedJoint2 = 31
+    kNotUsedJoint3 = 32
+    kNotUsedJoint4 = 33
+    kNotUsedJoint5 = 34
+

eval_robot/robot_control/robot_hand_brainco.py

@@ -0,0 +1,196 @@
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize  # dds
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_  # idl
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_
+
+import numpy as np
+from enum import IntEnum
+import threading
+import time
+from multiprocessing import Process, Array
+
+import logging_mp
+
+logger_mp = logging_mp.get_logger(__name__)
+
+brainco_Num_Motors = 6
+kTopicbraincoLeftCommand = "rt/brainco/left/cmd"
+kTopicbraincoLeftState = "rt/brainco/left/state"
+kTopicbraincoRightCommand = "rt/brainco/right/cmd"
+kTopicbraincoRightState = "rt/brainco/right/state"
+
+
+class Brainco_Controller:
+    def __init__(
+        self,
+        left_hand_array,
+        right_hand_array,
+        dual_hand_data_lock=None,
+        dual_hand_state_array=None,
+        dual_hand_action_array=None,
+        fps=100.0,
+        Unit_Test=False,
+        simulation_mode=False,
+    ):
+        logger_mp.info("Initialize Brainco_Controller...")
+        self.fps = fps
+        self.hand_sub_ready = False
+        self.Unit_Test = Unit_Test
+        self.simulation_mode = simulation_mode
+
+        if self.simulation_mode:
+            ChannelFactoryInitialize(1)
+        else:
+            ChannelFactoryInitialize(0)
+
+        # initialize handcmd publisher and handstate subscriber
+        self.LeftHandCmb_publisher = ChannelPublisher(kTopicbraincoLeftCommand, MotorCmds_)
+        self.LeftHandCmb_publisher.Init()
+        self.RightHandCmb_publisher = ChannelPublisher(kTopicbraincoRightCommand, MotorCmds_)
+        self.RightHandCmb_publisher.Init()
+
+        self.LeftHandState_subscriber = ChannelSubscriber(kTopicbraincoLeftState, MotorStates_)
+        self.LeftHandState_subscriber.Init()
+        self.RightHandState_subscriber = ChannelSubscriber(kTopicbraincoRightState, MotorStates_)
+        self.RightHandState_subscriber.Init()
+
+        # Shared Arrays for hand states
+        self.left_hand_state_array = Array("d", brainco_Num_Motors, lock=True)
+        self.right_hand_state_array = Array("d", brainco_Num_Motors, lock=True)
+
+        # initialize subscribe thread
+        self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state)
+        self.subscribe_state_thread.daemon = True
+        self.subscribe_state_thread.start()
+
+        while not self.hand_sub_ready:
+            time.sleep(0.1)
+            logger_mp.warning("[brainco_Controller] Waiting to subscribe dds...")
+        logger_mp.info("[brainco_Controller] Subscribe dds ok.")
+
+        hand_control_process = Process(
+            target=self.control_process,
+            args=(
+                left_hand_array,
+                right_hand_array,
+                self.left_hand_state_array,
+                self.right_hand_state_array,
+                dual_hand_data_lock,
+                dual_hand_state_array,
+                dual_hand_action_array,
+            ),
+        )
+        hand_control_process.daemon = True
+        hand_control_process.start()
+
+        logger_mp.info("Initialize brainco_Controller OK!\n")
+
+    def _subscribe_hand_state(self):
+        while True:
+            left_hand_msg = self.LeftHandState_subscriber.Read()
+            right_hand_msg = self.RightHandState_subscriber.Read()
+            self.hand_sub_ready = True
+            if left_hand_msg is not None and right_hand_msg is not None:
+                # Update left hand state
+                for idx, id in enumerate(Brainco_Left_Hand_JointIndex):
+                    self.left_hand_state_array[idx] = left_hand_msg.states[id].q
+                # Update right hand state
+                for idx, id in enumerate(Brainco_Right_Hand_JointIndex):
+                    self.right_hand_state_array[idx] = right_hand_msg.states[id].q
+            time.sleep(0.002)
+
+    def ctrl_dual_hand(self, left_q_target, right_q_target):
+        """
+        Set current left, right hand motor state target q
+        """
+        for idx, id in enumerate(Brainco_Left_Hand_JointIndex):
+            self.left_hand_msg.cmds[id].q = left_q_target[idx]
+        for idx, id in enumerate(Brainco_Right_Hand_JointIndex):
+            self.right_hand_msg.cmds[id].q = right_q_target[idx]
+
+        self.LeftHandCmb_publisher.Write(self.left_hand_msg)
+        self.RightHandCmb_publisher.Write(self.right_hand_msg)
+        # logger_mp.debug("hand ctrl publish ok.")
+
+    def control_process(
+        self,
+        left_hand_array,
+        right_hand_array,
+        left_hand_state_array,
+        right_hand_state_array,
+        dual_hand_data_lock=None,
+        dual_hand_state_array=None,
+        dual_hand_action_array=None,
+    ):
+        self.running = True
+
+        left_q_target = np.full(brainco_Num_Motors, 0)
+        right_q_target = np.full(brainco_Num_Motors, 0)
+
+        # initialize brainco hand's cmd msg
+        self.left_hand_msg = MotorCmds_()
+        self.left_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Left_Hand_JointIndex))]
+        self.right_hand_msg = MotorCmds_()
+        self.right_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Right_Hand_JointIndex))]
+
+        for idx, id in enumerate(Brainco_Left_Hand_JointIndex):
+            self.left_hand_msg.cmds[id].q = 0.0
+            self.left_hand_msg.cmds[id].dq = 1.0
+        for idx, id in enumerate(Brainco_Right_Hand_JointIndex):
+            self.right_hand_msg.cmds[id].q = 0.0
+            self.right_hand_msg.cmds[id].dq = 1.0
+
+        try:
+            while self.running:
+                start_time = time.time()
+                # get dual hand state
+                with left_hand_array.get_lock():
+                    left_hand_mat = np.array(left_hand_array[:]).copy()
+                with right_hand_array.get_lock():
+                    right_hand_mat = np.array(right_hand_array[:]).copy()
+
+                # Read left and right q_state from shared arrays
+                state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:])))
+
+                action_data = np.concatenate((left_hand_mat, right_hand_mat))
+                if dual_hand_data_lock is not None:
+                    with dual_hand_data_lock:
+                        dual_hand_state_array[:] = state_data
+                        dual_hand_action_array[:] = action_data
+
+                if dual_hand_state_array and dual_hand_action_array:
+                    with dual_hand_data_lock:
+                        left_q_target = left_hand_mat
+                        right_q_target = right_hand_mat
+
+                self.ctrl_dual_hand(left_q_target, right_q_target)
+                current_time = time.time()
+                time_elapsed = current_time - start_time
+                sleep_time = max(0, (1 / self.fps) - time_elapsed)
+                time.sleep(sleep_time)
+        finally:
+            logger_mp.info("brainco_Controller has been closed.")
+
+
+# according to the official documentation, https://www.brainco-hz.com/docs/revolimb-hand/product/parameters.html
+# the motor sequence is as shown in the table below
+# ┌──────┬───────┬────────────┬────────┬────────┬────────┬────────┐
+# │ Id   │   0   │     1      │   2    │   3    │   4    │   5    │
+# ├──────┼───────┼────────────┼────────┼────────┼────────┼────────┤
+# │Joint │ thumb │ thumb-aux  |  index │ middle │  ring  │  pinky │
+# └──────┴───────┴────────────┴────────┴────────┴────────┴────────┘
+class Brainco_Right_Hand_JointIndex(IntEnum):
+    kRightHandThumb = 0
+    kRightHandThumbAux = 1
+    kRightHandIndex = 2
+    kRightHandMiddle = 3
+    kRightHandRing = 4
+    kRightHandPinky = 5
+
+
+class Brainco_Left_Hand_JointIndex(IntEnum):
+    kLeftHandThumb = 0
+    kLeftHandThumbAux = 1
+    kLeftHandIndex = 2
+    kLeftHandMiddle = 3
+    kLeftHandRing = 4
+    kLeftHandPinky = 5

eval_robot/robot_control/robot_hand_inspire.py

@@ -0,0 +1,187 @@
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize  # dds
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_  # idl
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_
+
+import numpy as np
+from enum import IntEnum
+import threading
+import time
+from multiprocessing import Process, Array
+
+import logging_mp
+
+logger_mp = logging_mp.get_logger(__name__)
+
+Inspire_Num_Motors = 6
+kTopicInspireCommand = "rt/inspire/cmd"
+kTopicInspireState = "rt/inspire/state"
+
+
+class Inspire_Controller:
+    def __init__(
+        self,
+        left_hand_array,
+        right_hand_array,
+        dual_hand_data_lock=None,
+        dual_hand_state_array=None,
+        dual_hand_action_array=None,
+        fps=100.0,
+        Unit_Test=False,
+        simulation_mode=False,
+    ):
+        logger_mp.info("Initialize Inspire_Controller...")
+        self.fps = fps
+        self.Unit_Test = Unit_Test
+        self.simulation_mode = simulation_mode
+
+        if self.simulation_mode:
+            ChannelFactoryInitialize(1)
+        else:
+            ChannelFactoryInitialize(0)
+
+        # initialize handcmd publisher and handstate subscriber
+        self.HandCmb_publisher = ChannelPublisher(kTopicInspireCommand, MotorCmds_)
+        self.HandCmb_publisher.Init()
+
+        self.HandState_subscriber = ChannelSubscriber(kTopicInspireState, MotorStates_)
+        self.HandState_subscriber.Init()
+
+        # Shared Arrays for hand states
+        self.left_hand_state_array = Array("d", Inspire_Num_Motors, lock=True)
+        self.right_hand_state_array = Array("d", Inspire_Num_Motors, lock=True)
+
+        # initialize subscribe thread
+        self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state)
+        self.subscribe_state_thread.daemon = True
+        self.subscribe_state_thread.start()
+
+        while True:
+            if any(self.right_hand_state_array):  # any(self.left_hand_state_array) and
+                break
+            time.sleep(0.01)
+            logger_mp.warning("[Inspire_Controller] Waiting to subscribe dds...")
+        logger_mp.info("[Inspire_Controller] Subscribe dds ok.")
+
+        hand_control_process = Process(
+            target=self.control_process,
+            args=(
+                left_hand_array,
+                right_hand_array,
+                self.left_hand_state_array,
+                self.right_hand_state_array,
+                dual_hand_data_lock,
+                dual_hand_state_array,
+                dual_hand_action_array,
+            ),
+        )
+        hand_control_process.daemon = True
+        hand_control_process.start()
+
+        logger_mp.info("Initialize Inspire_Controller OK!\n")
+
+    def _subscribe_hand_state(self):
+        while True:
+            hand_msg = self.HandState_subscriber.Read()
+            if hand_msg is not None:
+                for idx, id in enumerate(Inspire_Left_Hand_JointIndex):
+                    self.left_hand_state_array[idx] = hand_msg.states[id].q
+                for idx, id in enumerate(Inspire_Right_Hand_JointIndex):
+                    self.right_hand_state_array[idx] = hand_msg.states[id].q
+            time.sleep(0.002)
+
+    def ctrl_dual_hand(self, left_q_target, right_q_target):
+        """
+        Set current left, right hand motor state target q
+        """
+        for idx, id in enumerate(Inspire_Left_Hand_JointIndex):
+            self.hand_msg.cmds[id].q = left_q_target[idx]
+        for idx, id in enumerate(Inspire_Right_Hand_JointIndex):
+            self.hand_msg.cmds[id].q = right_q_target[idx]
+
+        self.HandCmb_publisher.Write(self.hand_msg)
+        # logger_mp.debug("hand ctrl publish ok.")
+
+    def control_process(
+        self,
+        left_hand_array,
+        right_hand_array,
+        left_hand_state_array,
+        right_hand_state_array,
+        dual_hand_data_lock=None,
+        dual_hand_state_array=None,
+        dual_hand_action_array=None,
+    ):
+        self.running = True
+
+        left_q_target = np.full(Inspire_Num_Motors, 1.0)
+        right_q_target = np.full(Inspire_Num_Motors, 1.0)
+
+        # initialize inspire hand's cmd msg
+        self.hand_msg = MotorCmds_()
+        self.hand_msg.cmds = [
+            unitree_go_msg_dds__MotorCmd_()
+            for _ in range(len(Inspire_Right_Hand_JointIndex) + len(Inspire_Left_Hand_JointIndex))
+        ]
+
+        for idx, id in enumerate(Inspire_Left_Hand_JointIndex):
+            self.hand_msg.cmds[id].q = 1.0
+        for idx, id in enumerate(Inspire_Right_Hand_JointIndex):
+            self.hand_msg.cmds[id].q = 1.0
+
+        try:
+            while self.running:
+                start_time = time.time()
+
+                # get dual hand state
+                with left_hand_array.get_lock():
+                    left_hand_mat = np.array(left_hand_array[:]).copy()
+                with right_hand_array.get_lock():
+                    right_hand_mat = np.array(right_hand_array[:]).copy()
+
+                # Read left and right q_state from shared arrays
+                state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:])))
+
+                action_data = np.concatenate((left_hand_mat, right_hand_mat))
+                if dual_hand_data_lock is not None:
+                    with dual_hand_data_lock:
+                        dual_hand_state_array[:] = state_data
+                        dual_hand_action_array[:] = action_data
+
+                if dual_hand_state_array and dual_hand_action_array:
+                    with dual_hand_data_lock:
+                        left_q_target = left_hand_mat
+                        right_q_target = right_hand_mat
+
+                self.ctrl_dual_hand(left_q_target, right_q_target)
+                current_time = time.time()
+                time_elapsed = current_time - start_time
+                sleep_time = max(0, (1 / self.fps) - time_elapsed)
+                time.sleep(sleep_time)
+        finally:
+            logger_mp.info("Inspire_Controller has been closed.")
+
+
+# Update hand state, according to the official documentation, https://support.unitree.com/home/en/G1_developer/inspire_dfx_dexterous_hand
+# the state sequence is as shown in the table below
+# ┌──────┬───────┬──────┬────────┬────────┬────────────┬────────────────┬───────┬──────┬────────┬────────┬────────────┬────────────────┐
+# │ Id   │   0   │  1   │   2    │   3    │     4      │       5        │   6   │  7   │   8    │   9    │    10      │       11       │
+# ├──────┼───────┼──────┼────────┼────────┼────────────┼────────────────┼───────┼──────┼────────┼────────┼────────────┼────────────────┤
+# │      │                    Right Hand                                │                   Left Hand                                  │
+# │Joint │ pinky │ ring │ middle │ index  │ thumb-bend │ thumb-rotation │ pinky │ ring │ middle │ index  │ thumb-bend │ thumb-rotation │
+# └──────┴───────┴──────┴────────┴────────┴────────────┴────────────────┴───────┴──────┴────────┴────────┴────────────┴────────────────┘
+class Inspire_Right_Hand_JointIndex(IntEnum):
+    kRightHandPinky = 0
+    kRightHandRing = 1
+    kRightHandMiddle = 2
+    kRightHandIndex = 3
+    kRightHandThumbBend = 4
+    kRightHandThumbRotation = 5
+
+
+class Inspire_Left_Hand_JointIndex(IntEnum):
+    kLeftHandPinky = 6
+    kLeftHandRing = 7
+    kLeftHandMiddle = 8
+    kLeftHandIndex = 9
+    kLeftHandThumbBend = 10
+    kLeftHandThumbRotation = 11

eval_robot/robot_control/robot_hand_unitree.py

@@ -0,0 +1,403 @@
+# for dex3-1
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize  # dds
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_  # idl
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_
+
+# for gripper
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_  # idl
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_
+
+import numpy as np
+from enum import IntEnum
+import time
+import threading
+from multiprocessing import Process, Value, Array
+
+import logging_mp
+
+logging_mp.basic_config(level=logging_mp.INFO)
+logger_mp = logging_mp.get_logger(__name__)
+
+
+unitree_tip_indices = [4, 9, 14]  # [thumb, index, middle] in OpenXR
+Dex3_Num_Motors = 7
+kTopicDex3LeftCommand = "rt/dex3/left/cmd"
+kTopicDex3RightCommand = "rt/dex3/right/cmd"
+kTopicDex3LeftState = "rt/dex3/left/state"
+kTopicDex3RightState = "rt/dex3/right/state"
+
+
+class Dex3_1_Controller:
+    def __init__(
+        self,
+        left_hand_array_in,
+        right_hand_array_in,
+        dual_hand_data_lock=None,
+        dual_hand_state_array_out=None,
+        dual_hand_action_array_out=None,
+        fps=100.0,
+        Unit_Test=False,
+        simulation_mode=False,
+    ):
+        """
+        [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process
+        left_hand_array_in: [input] Left hand skeleton data (required from XR device) to hand_ctrl.control_process
+        right_hand_array_in: [input] Right hand skeleton data (required from XR device) to hand_ctrl.control_process
+        dual_hand_data_lock: Data synchronization lock for dual_hand_state_array and dual_hand_action_array
+        dual_hand_state_array_out: [output] Return left(7), right(7) hand motor state
+        dual_hand_action_array_out: [output] Return left(7), right(7) hand motor action
+        fps: Control frequency
+        Unit_Test: Whether to enable unit testing
+        simulation_mode: Whether to use simulation mode (default is False, which means using real robot)
+        """
+        logger_mp.info("Initialize Dex3_1_Controller...")
+
+        self.fps = fps
+        self.Unit_Test = Unit_Test
+        self.simulation_mode = simulation_mode
+
+        if self.simulation_mode:
+            ChannelFactoryInitialize(1)
+        else:
+            ChannelFactoryInitialize(0)
+
+        # initialize handcmd publisher and handstate subscriber
+        self.LeftHandCmb_publisher = ChannelPublisher(kTopicDex3LeftCommand, HandCmd_)
+        self.LeftHandCmb_publisher.Init()
+        self.RightHandCmb_publisher = ChannelPublisher(kTopicDex3RightCommand, HandCmd_)
+        self.RightHandCmb_publisher.Init()
+
+        self.LeftHandState_subscriber = ChannelSubscriber(kTopicDex3LeftState, HandState_)
+        self.LeftHandState_subscriber.Init()
+        self.RightHandState_subscriber = ChannelSubscriber(kTopicDex3RightState, HandState_)
+        self.RightHandState_subscriber.Init()
+
+        # Shared Arrays for hand states
+        self.left_hand_state_array = Array("d", Dex3_Num_Motors, lock=True)
+        self.right_hand_state_array = Array("d", Dex3_Num_Motors, lock=True)
+
+        # initialize subscribe thread
+        self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state)
+        self.subscribe_state_thread.daemon = True
+        self.subscribe_state_thread.start()
+
+        while True:
+            if any(self.left_hand_state_array) and any(self.right_hand_state_array):
+                break
+            time.sleep(0.01)
+            logger_mp.warning("[Dex3_1_Controller] Waiting to subscribe dds...")
+        logger_mp.info("[Dex3_1_Controller] Subscribe dds ok.")
+
+        hand_control_process = Process(
+            target=self.control_process,
+            args=(
+                left_hand_array_in,
+                right_hand_array_in,
+                self.left_hand_state_array,
+                self.right_hand_state_array,
+                dual_hand_data_lock,
+                dual_hand_state_array_out,
+                dual_hand_action_array_out,
+            ),
+        )
+        hand_control_process.daemon = True
+        hand_control_process.start()
+
+        logger_mp.info("Initialize Dex3_1_Controller OK!\n")
+
+    def _subscribe_hand_state(self):
+        while True:
+            left_hand_msg = self.LeftHandState_subscriber.Read()
+            right_hand_msg = self.RightHandState_subscriber.Read()
+            if left_hand_msg is not None and right_hand_msg is not None:
+                # Update left hand state
+                for idx, id in enumerate(Dex3_1_Left_JointIndex):
+                    self.left_hand_state_array[idx] = left_hand_msg.motor_state[id].q
+                # Update right hand state
+                for idx, id in enumerate(Dex3_1_Right_JointIndex):
+                    self.right_hand_state_array[idx] = right_hand_msg.motor_state[id].q
+            time.sleep(0.002)
+
+    class _RIS_Mode:
+        def __init__(self, id=0, status=0x01, timeout=0):
+            self.motor_mode = 0
+            self.id = id & 0x0F  # 4 bits for id
+            self.status = status & 0x07  # 3 bits for status
+            self.timeout = timeout & 0x01  # 1 bit for timeout
+
+        def _mode_to_uint8(self):
+            self.motor_mode |= self.id & 0x0F
+            self.motor_mode |= (self.status & 0x07) << 4
+            self.motor_mode |= (self.timeout & 0x01) << 7
+            return self.motor_mode
+
+    def ctrl_dual_hand(self, left_q_target, right_q_target):
+        """set current left, right hand motor state target q"""
+        for idx, id in enumerate(Dex3_1_Left_JointIndex):
+            self.left_msg.motor_cmd[id].q = left_q_target[idx]
+        for idx, id in enumerate(Dex3_1_Right_JointIndex):
+            self.right_msg.motor_cmd[id].q = right_q_target[idx]
+
+        self.LeftHandCmb_publisher.Write(self.left_msg)
+        self.RightHandCmb_publisher.Write(self.right_msg)
+        # logger_mp.debug("hand ctrl publish ok.")
+
+    def control_process(
+        self,
+        left_hand_array_in,
+        right_hand_array_in,
+        left_hand_state_array,
+        right_hand_state_array,
+        dual_hand_data_lock=None,
+        dual_hand_state_array_out=None,
+        dual_hand_action_array_out=None,
+    ):
+        self.running = True
+
+        # left_q_target = np.full(Dex3_Num_Motors, 0)
+        # right_q_target = np.full(Dex3_Num_Motors, 0)
+
+        q = 0.0
+        dq = 0.0
+        tau = 0.0
+        kp = 1.5
+        kd = 0.2
+
+        # initialize dex3-1's left hand cmd msg
+        self.left_msg = unitree_hg_msg_dds__HandCmd_()
+        for id in Dex3_1_Left_JointIndex:
+            ris_mode = self._RIS_Mode(id=id, status=0x01)
+            motor_mode = ris_mode._mode_to_uint8()
+            self.left_msg.motor_cmd[id].mode = motor_mode
+            self.left_msg.motor_cmd[id].q = q
+            self.left_msg.motor_cmd[id].dq = dq
+            self.left_msg.motor_cmd[id].tau = tau
+            self.left_msg.motor_cmd[id].kp = kp
+            self.left_msg.motor_cmd[id].kd = kd
+
+        # initialize dex3-1's right hand cmd msg
+        self.right_msg = unitree_hg_msg_dds__HandCmd_()
+        for id in Dex3_1_Right_JointIndex:
+            ris_mode = self._RIS_Mode(id=id, status=0x01)
+            motor_mode = ris_mode._mode_to_uint8()
+            self.right_msg.motor_cmd[id].mode = motor_mode
+            self.right_msg.motor_cmd[id].q = q
+            self.right_msg.motor_cmd[id].dq = dq
+            self.right_msg.motor_cmd[id].tau = tau
+            self.right_msg.motor_cmd[id].kp = kp
+            self.right_msg.motor_cmd[id].kd = kd
+
+        try:
+            while self.running:
+                start_time = time.time()
+
+                # get dual hand state
+                with left_hand_array_in.get_lock():
+                    left_hand_mat = np.array(left_hand_array_in[:]).copy()
+                with right_hand_array_in.get_lock():
+                    right_hand_mat = np.array(right_hand_array_in[:]).copy()
+
+                # Read left and right q_state from shared arrays
+                state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:])))
+
+                # get dual hand action
+                action_data = np.concatenate((left_hand_mat, right_hand_mat))
+                if dual_hand_state_array_out and dual_hand_action_array_out:
+                    with dual_hand_data_lock:
+                        dual_hand_state_array_out[:] = state_data
+                        dual_hand_action_array_out[:] = action_data
+
+                self.ctrl_dual_hand(left_hand_mat, right_hand_mat)
+                current_time = time.time()
+                time_elapsed = current_time - start_time
+                sleep_time = max(0, (1 / self.fps) - time_elapsed)
+                time.sleep(sleep_time)
+        finally:
+            print("Dex3_1_Controller has been closed.")
+
+
+class Dex3_1_Left_JointIndex(IntEnum):
+    kLeftHandThumb0 = 0
+    kLeftHandThumb1 = 1
+    kLeftHandThumb2 = 2
+    kLeftHandMiddle0 = 3
+    kLeftHandMiddle1 = 4
+    kLeftHandIndex0 = 5
+    kLeftHandIndex1 = 6
+
+
+class Dex3_1_Right_JointIndex(IntEnum):
+    kRightHandThumb0 = 0
+    kRightHandThumb1 = 1
+    kRightHandThumb2 = 2
+    kRightHandIndex0 = 3
+    kRightHandIndex1 = 4
+    kRightHandMiddle0 = 5
+    kRightHandMiddle1 = 6
+
+
+kTopicGripperLeftCommand = "rt/dex1/left/cmd"
+kTopicGripperLeftState = "rt/dex1/left/state"
+kTopicGripperRightCommand = "rt/dex1/right/cmd"
+kTopicGripperRightState = "rt/dex1/right/state"
+
+
+class Dex1_1_Gripper_Controller:
+    def __init__(
+        self,
+        left_gripper_value_in,
+        right_gripper_value_in,
+        dual_gripper_data_lock=None,
+        dual_gripper_state_out=None,
+        dual_gripper_action_out=None,
+        filter=True,
+        fps=200.0,
+        Unit_Test=False,
+        simulation_mode=False,
+    ):
+        """
+        [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process
+        left_gripper_value_in: [input] Left ctrl data (required from XR device) to control_thread
+        right_gripper_value_in: [input] Right ctrl data (required from XR device) to control_thread
+        dual_gripper_data_lock: Data synchronization lock for dual_gripper_state_array and dual_gripper_action_array
+        dual_gripper_state_out: [output] Return left(1), right(1) gripper motor state
+        dual_gripper_action_out: [output] Return left(1), right(1) gripper motor action
+        fps: Control frequency
+        Unit_Test: Whether to enable unit testing
+        simulation_mode: Whether to use simulation mode (default is False, which means using real robot)
+        """
+
+        logger_mp.info("Initialize Dex1_1_Gripper_Controller...")
+
+        self.fps = fps
+        self.Unit_Test = Unit_Test
+        self.gripper_sub_ready = False
+        self.simulation_mode = simulation_mode
+
+        if self.simulation_mode:
+            ChannelFactoryInitialize(1)
+        else:
+            ChannelFactoryInitialize(0)
+
+        # initialize handcmd publisher and handstate subscriber
+        self.LeftGripperCmb_publisher = ChannelPublisher(kTopicGripperLeftCommand, MotorCmds_)
+        self.LeftGripperCmb_publisher.Init()
+        self.RightGripperCmb_publisher = ChannelPublisher(kTopicGripperRightCommand, MotorCmds_)
+        self.RightGripperCmb_publisher.Init()
+
+        self.LeftGripperState_subscriber = ChannelSubscriber(kTopicGripperLeftState, MotorStates_)
+        self.LeftGripperState_subscriber.Init()
+        self.RightGripperState_subscriber = ChannelSubscriber(kTopicGripperRightState, MotorStates_)
+        self.RightGripperState_subscriber.Init()
+
+        # Shared Arrays for gripper states
+        self.left_gripper_state_value = Value("d", 0.0, lock=True)
+        self.right_gripper_state_value = Value("d", 0.0, lock=True)
+
+        # initialize subscribe thread
+        self.subscribe_state_thread = threading.Thread(target=self._subscribe_gripper_state)
+        self.subscribe_state_thread.daemon = True
+        self.subscribe_state_thread.start()
+
+        while not self.gripper_sub_ready:
+            time.sleep(0.01)
+            logger_mp.warning("[Dex1_1_Gripper_Controller] Waiting to subscribe dds...")
+        logger_mp.info("[Dex1_1_Gripper_Controller] Subscribe dds ok.")
+
+        self.gripper_control_thread = threading.Thread(
+            target=self.control_thread,
+            args=(
+                left_gripper_value_in,
+                right_gripper_value_in,
+                self.left_gripper_state_value,
+                self.right_gripper_state_value,
+                dual_gripper_data_lock,
+                dual_gripper_state_out,
+                dual_gripper_action_out,
+            ),
+        )
+        self.gripper_control_thread.daemon = True
+        self.gripper_control_thread.start()
+
+        logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!\n")
+
+    def _subscribe_gripper_state(self):
+        while True:
+            left_gripper_msg = self.LeftGripperState_subscriber.Read()
+            right_gripper_msg = self.RightGripperState_subscriber.Read()
+            self.gripper_sub_ready = True
+            if left_gripper_msg is not None and right_gripper_msg is not None:
+                self.left_gripper_state_value.value = left_gripper_msg.states[0].q
+                self.right_gripper_state_value.value = right_gripper_msg.states[0].q
+            time.sleep(0.002)
+
+    def ctrl_dual_gripper(self, dual_gripper_action):
+        """set current left, right gripper motor cmd target q"""
+        self.left_gripper_msg.cmds[0].q = dual_gripper_action[0]
+        self.right_gripper_msg.cmds[0].q = dual_gripper_action[1]
+
+        self.LeftGripperCmb_publisher.Write(self.left_gripper_msg)
+        self.RightGripperCmb_publisher.Write(self.right_gripper_msg)
+        # logger_mp.debug("gripper ctrl publish ok.")
+
+    def control_thread(
+        self,
+        left_gripper_value_in,
+        right_gripper_value_in,
+        left_gripper_state_value,
+        right_gripper_state_value,
+        dual_hand_data_lock=None,
+        dual_gripper_state_out=None,
+        dual_gripper_action_out=None,
+    ):
+        self.running = True
+        LEFT_MAPPED_MIN = 0.0  # The minimum initial motor position when the gripper closes at startup.
+        RIGHT_MAPPED_MIN = 0.0  # The minimum initial motor position when the gripper closes at startup.
+        # The maximum initial motor position when the gripper closes before calibration (with the rail stroke calculated as 0.6 cm/rad * 9 rad = 5.4 cm).
+
+        dq = 0.0
+        tau = 0.0
+        kp = 5.00
+        kd = 0.05
+        # initialize gripper cmd msg
+        self.left_gripper_msg = MotorCmds_()
+        self.left_gripper_msg.cmds = [unitree_go_msg_dds__MotorCmd_()]
+        self.right_gripper_msg = MotorCmds_()
+        self.right_gripper_msg.cmds = [unitree_go_msg_dds__MotorCmd_()]
+
+        self.left_gripper_msg.cmds[0].dq = dq
+        self.left_gripper_msg.cmds[0].tau = tau
+        self.left_gripper_msg.cmds[0].kp = kp
+        self.left_gripper_msg.cmds[0].kd = kd
+
+        self.right_gripper_msg.cmds[0].dq = dq
+        self.right_gripper_msg.cmds[0].tau = tau
+        self.right_gripper_msg.cmds[0].kp = kp
+        self.right_gripper_msg.cmds[0].kd = kd
+        try:
+            while self.running:
+                start_time = time.time()
+                # get dual hand skeletal point state from XR device
+                with left_gripper_value_in.get_lock():
+                    left_gripper_value = left_gripper_value_in.value
+                with right_gripper_value_in.get_lock():
+                    right_gripper_value = right_gripper_value_in.value
+                # get current dual gripper motor state
+                dual_gripper_state = np.array([left_gripper_state_value.value, right_gripper_state_value.value])
+                dual_gripper_action = np.array([left_gripper_value, right_gripper_value])
+
+                if dual_gripper_state_out and dual_gripper_action_out:
+                    with dual_hand_data_lock:
+                        dual_gripper_state_out[:] = dual_gripper_state - np.array([LEFT_MAPPED_MIN, RIGHT_MAPPED_MIN])
+                        dual_gripper_action_out[:] = dual_gripper_action - np.array([LEFT_MAPPED_MIN, RIGHT_MAPPED_MIN])
+                self.ctrl_dual_gripper(dual_gripper_action)
+                current_time = time.time()
+                time_elapsed = current_time - start_time
+                sleep_time = max(0, (1 / self.fps) - time_elapsed)
+                time.sleep(sleep_time)
+        finally:
+            logger_mp.info("Dex1_1_Gripper_Controller has been closed.")
+
+
+class Gripper_JointIndex(IntEnum):
+    kGripper = 0