diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index c66639816d..809d02e913 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/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,33 +204,28 @@ 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 @@ -206,14 +233,14 @@ def _grab_phase_machine(self) -> None: 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,26 +249,26 @@ 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 @@ -249,20 +276,20 @@ def _grab_phase_machine(self) -> None: 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) \ No newline at end of file