提取初始位置常量并添加位置方法的类型注解

src/Robot_arm_motion/robot_arm.py

@@ -65,21 +65,32 @@ def __init__(self):
         # 位置控制参数
         self.POS_TOLERANCE = 0.003  # 末端执行器位置误差容忍阈值
 
-        # 【优化1】提取夹爪等待步数为类内常量
+        # 夹爪控制参数
         self.GRIPPER_WAIT_STEPS = 100  # 夹爪动作完成所需的等待步数
 
+        # 【优化1】提取机械臂初始位置为类内常量
+        self.INIT_EE_POS = np.array([0.4, 0.0, 0.2])  # 末端执行器初始目标位置
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
         print("📌 模型Joint列表：", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
         print("=" * 50)
 
-    def get_ee_pos(self):
-        """获取末端执行器位置"""
+    def get_ee_pos(self) -> np.ndarray:
+        """获取末端执行器位置
+
+        Returns:
+            np.ndarray: 末端执行器的三维位置坐标[x, y, z]
+        """
         return self.data.xpos[self.ee_body_id].copy()
 
-    def get_cube_pos(self):
-        """获取立方体位置"""
+    def get_cube_pos(self) -> np.ndarray:
+        """获取立方体位置
+
+        Returns:
+            np.ndarray: 立方体的三维位置坐标[x, y, z]
+        """
         return self.data.xpos[self.cube_body_id].copy()
 
     def _compute_jacobian(self):
@@ -153,7 +164,8 @@ def _grab_phase_machine(self):
         """
         if self.current_phase == 0:
             # 阶段0：移动到初始位置
-            if self._move_step(np.array([0.4, 0.0, 0.2])):
+            # 【优化2】使用类内常量替代硬编码的初始位置
+            if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
                 self.current_phase = 1
                 self.step_counter = 0
@@ -176,7 +188,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 4
                 self.step_counter = 0
@@ -194,7 +205,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 6
                 self.step_counter = 0
@@ -226,7 +236,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 10
                 self.step_counter = 0
@@ -241,7 +250,8 @@ def _grab_phase_machine(self):
 
         elif self.current_phase == 11:
             # 阶段11：返回初始位置
-            if self._move_step(np.array([0.4, 0.0, 0.2]), speed=0.4):
+            # 【优化2】使用类内常量替代硬编码的初始位置
+            if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
                 self.current_phase = 12