Merge pull request #141 from brelkh/add-h1-2-motion-mode

Add motion mode support for H1_2

teleop/robot_control/robot_arm.py

@@ -625,8 +625,9 @@ class G1_23_JointIndex(IntEnum):
     kNotUsedJoint5 = 34
 
 class H1_2_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 H1_2_ArmController...")
         self.q_target = np.zeros(14)
@@ -652,6 +653,10 @@ class H1_2_ArmController:
             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)
@@ -726,6 +731,9 @@ class H1_2_ArmController:
         return cliped_arm_q_target
 
     def _ctrl_motor_state(self):
+        if self.motion_mode:
+            self.msg.motor_cmd[H1_2_JointIndex.kNotUsedJoint0].q = 1.0;
+
         while True:
             start_time = time.time()
 
@@ -791,6 +799,10 @@ class H1_2_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[H1_2_JointIndex.kNotUsedJoint0].q = weight;
+                        time.sleep(0.02)
                 logger_mp.info("[H1_2_ArmController] both arms have reached the home position.")
                 break
             current_attempts += 1

teleop/teleop_hand_and_arm.py

@@ -145,7 +145,7 @@ if __name__ == '__main__':
         arm_ctrl = G1_23_ArmController(motion_mode=args.motion, simulation_mode=args.sim)
     elif args.arm == "H1_2":
         arm_ik = H1_2_ArmIK()
-        arm_ctrl = H1_2_ArmController(simulation_mode=args.sim)
+        arm_ctrl = H1_2_ArmController(motion_mode=args.motion, simulation_mode=args.sim)
     elif args.arm == "H1":
         arm_ik = H1_ArmIK()
         arm_ctrl = H1_ArmController(simulation_mode=args.sim)