diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index a5177aa7aa..c66639816d 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -23,6 +23,21 @@ # ========== 智能抓取控制器 ========== class PandaAutoGrab: + # 【优化1】定义状态机阶段的具名常量，提升代码可读性 + PHASE_MOVE_TO_INIT = 0 + PHASE_DETECT_CUBE = 1 + PHASE_MOVE_TO_CUBE_ABOVE = 2 + PHASE_OPEN_GRIPPER = 3 + PHASE_MOVE_TO_GRAB_HEIGHT = 4 + PHASE_CLOSE_GRIPPER = 5 + PHASE_LIFT_CUBE = 6 + PHASE_MOVE_TO_PLACE_ABOVE = 7 + PHASE_MOVE_TO_PLACE_HEIGHT = 8 + PHASE_RELEASE_CUBE = 9 + PHASE_MOVE_BACK_FROM_PLACE = 10 + PHASE_MOVE_BACK_TO_INIT = 11 + PHASE_FINISHED = 12 + def __init__(self): """初始化Franka Panda机械臂抓取控制器，加载模型和初始化参数""" self.model = mujoco.MjModel.from_xml_path(SCENE_PATH) @@ -162,94 +177,94 @@ def _grab_phase_machine(self) -> None: 状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回， 每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。 """ - if self.current_phase == 0: + # 【优化2】使用具名常量替代数字，使逻辑更清晰 + if self.current_phase == self.PHASE_MOVE_TO_INIT: if self._move_step(self.INIT_EE_POS): print("\n✅ 到达初始位置") - self.current_phase = 1 + self.current_phase = self.PHASE_DETECT_CUBE self.step_counter = 0 - elif self.current_phase == 1: + elif self.current_phase == self.PHASE_DETECT_CUBE: self.cube_pos = self.get_cube_pos() print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}") - self.current_phase = 2 + self.current_phase = self.PHASE_MOVE_TO_CUBE_ABOVE - elif self.current_phase == 2: + elif self.current_phase == self.PHASE_MOVE_TO_CUBE_ABOVE: if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): print("\n✅ 到达立方体上方") - self.current_phase = 3 + self.current_phase = self.PHASE_OPEN_GRIPPER self.step_counter = 0 - elif self.current_phase == 3: + elif self.current_phase == self.PHASE_OPEN_GRIPPER: if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n✋ 打开夹爪") if self.step_counter > self.GRIPPER_WAIT_STEPS: - self.current_phase = 4 + self.current_phase = self.PHASE_MOVE_TO_GRAB_HEIGHT self.step_counter = 0 self.step_counter += 1 - elif self.current_phase == 4: + elif self.current_phase == self.PHASE_MOVE_TO_GRAB_HEIGHT: if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2): print("\n✅ 下降到抓取高度") - self.current_phase = 5 + self.current_phase = self.PHASE_CLOSE_GRIPPER self.step_counter = 0 - elif self.current_phase == 5: + elif self.current_phase == self.PHASE_CLOSE_GRIPPER: if self.step_counter == 0: self._gripper_step(self.gripper_close_pos) print("\n🤏 闭合夹爪抓取") if self.step_counter > self.GRIPPER_WAIT_STEPS: - self.current_phase = 6 + self.current_phase = self.PHASE_LIFT_CUBE self.step_counter = 0 self.step_counter += 1 - elif self.current_phase == 6: + elif self.current_phase == self.PHASE_LIFT_CUBE: lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT]) if self._move_step(lift_target, speed=0.3): print("\n✅ 抬升立方体") - self.current_phase = 7 + self.current_phase = self.PHASE_MOVE_TO_PLACE_ABOVE self.step_counter = 0 - elif self.current_phase == 7: + elif self.current_phase == self.PHASE_MOVE_TO_PLACE_ABOVE: if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): print("\n✅ 到达放置点上方") - self.current_phase = 8 + self.current_phase = self.PHASE_MOVE_TO_PLACE_HEIGHT self.step_counter = 0 - elif self.current_phase == 8: + elif self.current_phase == self.PHASE_MOVE_TO_PLACE_HEIGHT: if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2): print("\n✅ 下降到放置高度") - self.current_phase = 9 + self.current_phase = self.PHASE_RELEASE_CUBE self.step_counter = 0 - elif self.current_phase == 9: + elif self.current_phase == self.PHASE_RELEASE_CUBE: if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n🫳 释放立方体") if self.step_counter > self.GRIPPER_WAIT_STEPS: - self.current_phase = 10 + self.current_phase = self.PHASE_MOVE_BACK_FROM_PLACE self.step_counter = 0 self.step_counter += 1 - elif self.current_phase == 10: + elif self.current_phase == self.PHASE_MOVE_BACK_FROM_PLACE: if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3): print("\n✅ 撤离机械臂") - self.current_phase = 11 + self.current_phase = self.PHASE_MOVE_BACK_TO_INIT self.step_counter = 0 - elif self.current_phase == 11: + elif self.current_phase == self.PHASE_MOVE_BACK_TO_INIT: if self._move_step(self.INIT_EE_POS, speed=0.4): print("\n✅ 返回初始位置") - self.current_phase = 12 + self.current_phase = self.PHASE_FINISHED - elif self.current_phase == 12: + elif self.current_phase == self.PHASE_FINISHED: if not self.grab_complete: print("\n" + "=" * 50) print("✅ 智能抓取任务完成！") print("=" * 50) self.grab_complete = True - # 【优化1】新增方法：集中初始化相机参数 def _init_camera(self) -> None: """初始化Viewer的相机视角""" self.viewer.cam.azimuth = self.CAM_AZIMUTH @@ -259,10 +274,7 @@ def _init_camera(self) -> None: def run(self): """单线程仿真主循环""" - # 初始化Viewer self.viewer = viewer.launch_passive(self.model, self.data) - - # 【优化2】调用新方法初始化相机 self._init_camera() print("\n🚀 仿真已启动，开始自动抓取...")