重构相机初始化并完善_grab_phase_machine类型注解

src/Robot_arm_motion/robot_arm.py

@@ -78,7 +78,7 @@ def __init__(self):
         self.CAM_DISTANCE = 1.8  # 相机距离
         self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1])  # 相机注视点
 
-        # 【优化1】将仿真休眠时间提升为类内常量
+        # 仿真控制参数
         self.SIMULATION_SLEEP = 1 / 200  # 仿真循环的休眠时间
 
         # 打印模型信息
@@ -129,28 +129,18 @@ def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool:
         if error_norm < self.POS_TOLERANCE:
             return True  # 到达目标
 
-        # 计算雅克比矩阵
-        jacobian = self._compute_jacobian()  # 3×7
-
-        # ========== 修正：正确的阻尼伪逆计算 ==========
-        # 方法1：使用正则化参数的伪逆（推荐）
+        jacobian = self._compute_jacobian()
         jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))
 
-        # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度）
-        # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)
-
-        # 关节速度指令
         joint_vel_cmd = speed * jacobian_pinv @ error
         joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT)
 
-        # PD力矩计算
         torque = np.zeros(7)
         for i in range(7):
             angle_error = joint_vel_cmd[i] * 0.1
             torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]]
             torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT)
 
-        # 设置关节力矩
         for i in range(7):
             self.data.ctrl[self.joint_ids[i]] = torque[i]
 
@@ -166,34 +156,30 @@ def _gripper_step(self, pos: float) -> None:
             j_id = self.model.joint(j_name).id
             self.data.ctrl[j_id] = pos
 
-    def _grab_phase_machine(self):
+    def _grab_phase_machine(self) -> None:
         """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作
 
         状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
         每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
         """
         if self.current_phase == 0:
-            # 阶段0：移动到初始位置
             if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
                 self.current_phase = 1
                 self.step_counter = 0
 
         elif self.current_phase == 1:
-            # 阶段1：获取立方体位置
             self.cube_pos = self.get_cube_pos()
             print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
             self.current_phase = 2
 
         elif self.current_phase == 2:
-            # 阶段2：移动到立方体上方
             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.step_counter = 0
 
         elif self.current_phase == 3:
-            # 阶段3：打开夹爪
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
@@ -203,14 +189,12 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 4:
-            # 阶段4：下降抓取
             if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2):
                 print("\n✅ 下降到抓取高度")
                 self.current_phase = 5
                 self.step_counter = 0
 
         elif self.current_phase == 5:
-            # 阶段5：闭合夹爪
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
@@ -220,29 +204,25 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 6:
-            # 阶段6：抬升立方体
             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.step_counter = 0
 
         elif self.current_phase == 7:
-            # 阶段7：移动到放置点上方
             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.step_counter = 0
 
         elif self.current_phase == 8:
-            # 阶段8：下降放置
             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.step_counter = 0
 
         elif self.current_phase == 9:
-            # 阶段9：释放立方体
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
@@ -252,57 +232,56 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 10:
-            # 阶段10：撤离机械臂
             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.step_counter = 0
 
         elif self.current_phase == 11:
-            # 阶段11：返回初始位置
             if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
                 self.current_phase = 12
 
         elif self.current_phase == 12:
-            # 阶段12：抓取完成
             if not self.grab_complete:
                 print("\n" + "=" * 50)
                 print("✅ 智能抓取任务完成！")
                 print("=" * 50)
                 self.grab_complete = True
 
-    def run(self):
-        """单线程仿真主循环"""
-        # 初始化Viewer
-        self.viewer = viewer.launch_passive(self.model, self.data)
+    # 【优化1】新增方法：集中初始化相机参数
+    def _init_camera(self) -> None:
+        """初始化Viewer的相机视角"""
         self.viewer.cam.azimuth = self.CAM_AZIMUTH
         self.viewer.cam.elevation = self.CAM_ELEVATION
         self.viewer.cam.distance = self.CAM_DISTANCE
         self.viewer.cam.lookat = self.CAM_LOOKAT
 
+    def run(self):
+        """单线程仿真主循环"""
+        # 初始化Viewer
+        self.viewer = viewer.launch_passive(self.model, self.data)
+
+        # 【优化2】调用新方法初始化相机
+        self._init_camera()
+
         print("\n🚀 仿真已启动，开始自动抓取...")
         print("💡 关闭Viewer窗口可退出程序")
 
-        # 单线程主循环
-        # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
         try:
             while self.viewer.is_running():
                 if self.running and not self.grab_complete:
                     self._grab_phase_machine()
                 else:
-                    # 抓取完成后归零力矩
                     for i in range(7):
                         self.data.ctrl[self.joint_ids[i]] = 0
 
                 mujoco.mj_step(self.model, self.data)
                 self.viewer.sync()
-                # 【优化2】使用类内常量替代局部变量
                 time.sleep(self.SIMULATION_SLEEP)
         except KeyboardInterrupt:
             print("\n⚠️ 检测到Ctrl+C，正在退出仿真...")
 
-        # 清理
         self.running = False
         self.viewer.close()
         print("\n👋 仿真结束")