引入logging, 增强错误处理并添加类文档

src/Robot_arm_motion/robot_arm.py

@@ -4,6 +4,10 @@
 import time
 import sys
 import os
+import logging  # 【优化1】引入logging模块
+
+# 配置logging，使其输出到控制台并包含时间和日志级别
+logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
 
 """
 Franka Panda 机械臂自动抓取仿真 v1.1
@@ -16,14 +20,29 @@
                           "franka_emika_panda",
                           "grab_scene.xml")
 
-if not os.path.exists(SCENE_PATH):
-    print(f"❌ 场景文件不存在：{SCENE_PATH}")
-    sys.exit(1)
-
 
 # ========== 智能抓取控制器 ==========
 class PandaAutoGrab:
-    # 【优化1】定义状态机阶段的具名常量，提升代码可读性
+    """
+    【优化3】添加类级文档字符串
+    Franka Panda 机械臂智能抓取控制器 (基于MuJoCo)
+
+    该类实现了一个完整的、基于视觉的机械臂抓取和放置任务。
+    它通过一个状态机来编排一系列动作，并使用基于雅可比伪逆的操作空间控制
+    来精确地移动机械臂末端执行器。
+
+    核心算法:
+    - **状态机**: 采用阶段式状态机 (`_grab_phase_machine`) 来管理抓取流程。
+    - **运动学控制**: 使用雅可比矩阵的伪逆将末端执行器的笛卡尔空间速度
+      指令转换为关节空间的速度指令，实现高精度的位置跟踪。
+    - **PD控制器**: 在关节空间使用PD控制器将速度指令转换为力矩输出。
+
+    主要属性:
+    - model, data: MuJoCo的核心数据结构，分别表示模型和数据。
+    - current_phase: 当前状态机所处的阶段。
+    - 各类参数: 如PD增益、速度限制、位置容差等，均定义为类属性以便调整。
+    """
+    # 状态机阶段常量
     PHASE_MOVE_TO_INIT = 0
     PHASE_DETECT_CUBE = 1
     PHASE_MOVE_TO_CUBE_ABOVE = 2
@@ -40,14 +59,27 @@ class PandaAutoGrab:
 
     def __init__(self):
         """初始化Franka Panda机械臂抓取控制器，加载模型和初始化参数"""
-        self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
-        self.data = mujoco.MjData(self.model)
+        self.model = None
+        self.data = None
         self.viewer = None
         self.running = True
         self.step_counter = 0
         self.current_phase = 0
         self.grab_complete = False
 
+        # 尝试加载模型文件
+        try:
+            self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
+            self.data = mujoco.MjData(self.model)
+        except FileNotFoundError:
+            logging.error(f"场景文件不存在，请检查路径: {SCENE_PATH}")
+            sys.exit(1)
+        # 【优化2】增强错误处理，捕获MuJoCo特定的致命错误
+        except mujoco.FatalError as e:
+            logging.error(f"加载MJCF模型时发生致命错误: {e}")
+            logging.error("请检查XML文件的语法和引用的资源（如mesh, texture）是否正确。")
+            sys.exit(1)
+
         # 机械臂参数
         self.ee_body_id = self.model.body("hand").id
         self.joint_names = [f"joint{i}" for i in range(1, 8)]
@@ -97,10 +129,10 @@ def __init__(self):
         self.SIMULATION_SLEEP = 1 / 200  # 仿真循环的休眠时间
 
         # 打印模型信息
-        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)
+        logging.info("=" * 50)
+        logging.info("📌 模型Body列表: %s", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
+        logging.info("📌 模型Joint列表: %s", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
+        logging.info("=" * 50)
 
     def get_ee_pos(self) -> np.ndarray:
         """获取末端执行器位置
@@ -172,48 +204,43 @@ def _gripper_step(self, pos: float) -> None:
             self.data.ctrl[j_id] = pos
 
     def _grab_phase_machine(self) -> None:
-        """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作
-
-        状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
-        每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
-        """
-        # 【优化2】使用具名常量替代数字，使逻辑更清晰
+        """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作"""
         if self.current_phase == self.PHASE_MOVE_TO_INIT:
             if self._move_step(self.INIT_EE_POS):
-                print("\n✅ 到达初始位置")
+                logging.info("✅ 到达初始位置")
                 self.current_phase = self.PHASE_DETECT_CUBE
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_DETECT_CUBE:
             self.cube_pos = self.get_cube_pos()
-            print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
+            logging.info("🎯 识别到立方体位置: %s", np.round(self.cube_pos, 3))
             self.current_phase = self.PHASE_MOVE_TO_CUBE_ABOVE
 
         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✅ 到达立方体上方")
+                logging.info("✅ 到达立方体上方")
                 self.current_phase = self.PHASE_OPEN_GRIPPER
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_OPEN_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
-                print("\n✋ 打开夹爪")
+                logging.info("✋ 打开夹爪")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_MOVE_TO_GRAB_HEIGHT
                 self.step_counter = 0
             self.step_counter += 1
 
         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✅ 下降到抓取高度")
+                logging.info("✅ 下降到抓取高度")
                 self.current_phase = self.PHASE_CLOSE_GRIPPER
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_CLOSE_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
-                print("\n🤏 闭合夹爪抓取")
+                logging.info("🤏 闭合夹爪抓取")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_LIFT_CUBE
                 self.step_counter = 0
@@ -222,47 +249,47 @@ def _grab_phase_machine(self) -> None:
         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✅ 抬升立方体")
+                logging.info("✅ 抬升立方体")
                 self.current_phase = self.PHASE_MOVE_TO_PLACE_ABOVE
                 self.step_counter = 0
 
         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✅ 到达放置点上方")
+                logging.info("✅ 到达放置点上方")
                 self.current_phase = self.PHASE_MOVE_TO_PLACE_HEIGHT
                 self.step_counter = 0
 
         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✅ 下降到放置高度")
+                logging.info("✅ 下降到放置高度")
                 self.current_phase = self.PHASE_RELEASE_CUBE
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_RELEASE_CUBE:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
-                print("\n🫳 释放立方体")
+                logging.info("🫳 释放立方体")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_MOVE_BACK_FROM_PLACE
                 self.step_counter = 0
             self.step_counter += 1
 
         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✅ 撤离机械臂")
+                logging.info("✅ 撤离机械臂")
                 self.current_phase = self.PHASE_MOVE_BACK_TO_INIT
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_MOVE_BACK_TO_INIT:
             if self._move_step(self.INIT_EE_POS, speed=0.4):
-                print("\n✅ 返回初始位置")
+                logging.info("✅ 返回初始位置")
                 self.current_phase = self.PHASE_FINISHED
 
         elif self.current_phase == self.PHASE_FINISHED:
             if not self.grab_complete:
-                print("\n" + "=" * 50)
-                print("✅ 智能抓取任务完成！")
-                print("=" * 50)
+                logging.info("=" * 50)
+                logging.info("✅ 智能抓取任务完成！")
+                logging.info("=" * 50)
                 self.grab_complete = True
 
     def _init_camera(self) -> None:
@@ -277,8 +304,8 @@ def run(self):
         self.viewer = viewer.launch_passive(self.model, self.data)
         self._init_camera()
 
-        print("\n🚀 仿真已启动，开始自动抓取...")
-        print("💡 关闭Viewer窗口可退出程序")
+        logging.info("🚀 仿真已启动，开始自动抓取...")
+        logging.info("💡 关闭Viewer窗口可退出程序")
 
         try:
             while self.viewer.is_running():
@@ -292,11 +319,11 @@ def run(self):
                 self.viewer.sync()
                 time.sleep(self.SIMULATION_SLEEP)
         except KeyboardInterrupt:
-            print("\n⚠️ 检测到Ctrl+C，正在退出仿真...")
+            logging.warning("⚠️ 检测到Ctrl+C，正在退出仿真...")
 
         self.running = False
         self.viewer.close()
-        print("\n👋 仿真结束")
+        logging.info("👋 仿真结束")
 
 
 # ========== 主函数 ==========
@@ -305,8 +332,5 @@ def run(self):
         panda = PandaAutoGrab()
         panda.run()
     except Exception as e:
-        print(f"\n❌ 程序错误：{e}")
-        import traceback
-
-        traceback.print_exc()
+        logging.error(f"❌ 程序发生未处理的错误: {e}", exc_info=True)
         sys.exit(1)