为状态机阶段定义具名常量，提升可读性

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🚀 仿真已启动，开始自动抓取...")