diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index af47e1d..46d89d4 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -345,9 +345,10 @@ 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) self.tauff_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 diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 2569cb7..e9a9b71 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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)