Merge branch 'main' of https://github.com/unitreerobotics/xr_teleoperate into main

teleop/robot_control/robot_arm.py

@@ -345,8 +345,9 @@ class G1_29_JointIndex(IntEnum):
     kNotUsedJoint5 = 34
 
 class G1_23_ArmController:
-    def __init__(self, simulation_mode = False):
+    def __init__(self, motion_mode = False, simulation_mode = False):
         self.simulation_mode = simulation_mode
+        self.motion_mode = motion_mode
 
         logger_mp.info("Initialize G1_23_ArmController...")
         self.q_target = np.zeros(10)
@@ -372,7 +373,11 @@ class G1_23_ArmController:
             ChannelFactoryInitialize(1)
         else:
             ChannelFactoryInitialize(0)
-        self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd)
+        
+        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()
@@ -446,6 +451,9 @@ class G1_23_ArmController:
         return cliped_arm_q_target
 
     def _ctrl_motor_state(self):
+        if self.motion_mode:
+            self.msg.motor_cmd[G1_23_JointIndex.kNotUsedJoint0].q = 1.0;
+
         while True:
             start_time = time.time()
 
@@ -511,6 +519,10 @@ class G1_23_ArmController:
         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_23_JointIndex.kNotUsedJoint0].q = weight;
+                        time.sleep(0.02)
                 logger_mp.info("[G1_23_ArmController] both arms have reached the home position.")
                 break
             current_attempts += 1

teleop/teleop_hand_and_arm.py

@@ -139,7 +139,7 @@ if __name__ == '__main__':
         arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim)
         arm_ik = G1_29_ArmIK()
     elif args.arm == "G1_23":
-        arm_ctrl = G1_23_ArmController(simulation_mode=args.sim)
+        arm_ctrl = G1_23_ArmController(motion_mode=args.motion, simulation_mode=args.sim)
         arm_ik = G1_23_ArmIK()
     elif args.arm == "H1_2":
         arm_ctrl = H1_2_ArmController(simulation_mode=args.sim)