From a7473c911158f5eb2f7f8d7cc74b217974ba3fe0 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 17:51:17 +0800 Subject: [PATCH 01/26] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | Bin 168 -> 184 bytes src/Robot _arm_grasping_task/README.md | 3 +++ 2 files changed, 3 insertions(+) create mode 100644 src/Robot _arm_grasping_task/README.md diff --git a/.gitignore b/.gitignore index e327294ac1c483e409d0dc1015ec7c4da3176ada..b455caf4c44de311614245d03c670c980388b0d6 100644 GIT binary patch delta 23 ecmZ3%xPx)R3IRQaOokMORE9(beFk0zE(QQgs|2zD delta 6 NcmdnNxPo!Q3IGVk0-FE; diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot _arm_grasping_task/README.md new file mode 100644 index 0000000000..7cc01dc0d4 --- /dev/null +++ b/src/Robot _arm_grasping_task/README.md @@ -0,0 +1,3 @@ + 机械臂抓取仿真项目（MuJoCo）README +Robot Arm Grasping Simulation (MuJoCo) + From ddde325192405ca04754326a84fb60b8e5e5a34b Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 21:04:13 +0800 Subject: [PATCH 02/26] main_robot --- src/Robot _arm_grasping_task/README.md | 40 +++++++++- src/Robot _arm_grasping_task/main.py | 0 src/Robot _arm_grasping_task/robot.xml | 0 src/robot.xml/robot.xml | 104 +++++++++++++++++++++++++ 4 files changed, 143 insertions(+), 1 deletion(-) create mode 100644 src/Robot _arm_grasping_task/main.py create mode 100644 src/Robot _arm_grasping_task/robot.xml create mode 100644 src/robot.xml/robot.xml diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot _arm_grasping_task/README.md index 7cc01dc0d4..eb7e5576c8 100644 --- a/src/Robot _arm_grasping_task/README.md +++ b/src/Robot _arm_grasping_task/README.md @@ -1,3 +1,41 @@ - 机械臂抓取仿真项目（MuJoCo）README +# 机械臂抓取仿真项目（MuJoCo） Robot Arm Grasping Simulation (MuJoCo) +
+ 仿真结果图 +
+ +## 项目概述 +本项目基于 **MuJoCo 2.3.6** 物理仿真引擎实现3自由度机械臂抓取任务，聚焦典型物体的精准抓取、姿态规划及力反馈控制，可直接用于机器人抓取算法的验证与调试。 + +### 核心应用场景 +- 📦 **单物体精准抓取**：从固定位置抓取立方体，放置到指定目标坐标 +- 📊 **力反馈控制**：基于末端力传感器数据，自适应调整夹爪夹持力度 +- 🎯 **轨迹可视化**：生成末端执行器/物体的运动轨迹、接触力变化等分析图表 + +## 核心功能 +| 功能模块 | 实现方案 | +|------------------|--------------------------------------------------------------------------| +| 逆运动学求解 | 牛顿-拉夫逊法 + 雅可比矩阵（适配3自由度机械臂） | +| 控制算法 | PID 控制（经典工业控制算法，稳定可靠） | +| 抓取判定 | 位置误差阈值 + 接触力阈值（双重判定，避免误判） | +| 数据可视化 | Matplotlib 绘制轨迹/力变化图表，支持高清导出 | +| 鲁棒性优化 | 异常捕获 + 资源自动释放（避免窗口关闭导致程序崩溃） | + +## 技术框架 +| 模块 | 技术选型 | 版本说明 | +|--------------|-----------------------------------|---------------------------| +| 物理引擎 | MuJoCo | v2.3.6（稳定版，免编译） | +| 可视化库 | mujoco-python-viewer | v0.1.4（兼容低版本） | +| 数值计算 | NumPy / SciPy | 1.26.4 / 1.11.4 | +| 绘图库 | Matplotlib | 3.8.2 | +| 开发环境 | Python | 3.8+（Windows 兼容） | + +## 环境配置（Windows 一键部署） +### 1. 激活虚拟环境 +```bash +# 打开PowerShell，进入项目根目录 +cd D:\nn + +# 激活虚拟环境 +D:\nn\.venv\Scripts\activate \ No newline at end of file diff --git a/src/Robot _arm_grasping_task/main.py b/src/Robot _arm_grasping_task/main.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Robot _arm_grasping_task/robot.xml b/src/Robot _arm_grasping_task/robot.xml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/robot.xml/robot.xml b/src/robot.xml/robot.xml new file mode 100644 index 0000000000..b69f5d453d --- /dev/null +++ b/src/robot.xml/robot.xml @@ -0,0 +1,104 @@ + + + \ No newline at end of file From ca56bd31668d1e0f2633112a344d5374d5bd6b33 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 21:13:38 +0800 Subject: [PATCH 03/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot _arm_grasping_task/main.py | 291 +++++++++++++++++++++++++++ 1 file changed, 291 insertions(+) diff --git a/src/Robot _arm_grasping_task/main.py b/src/Robot _arm_grasping_task/main.py index e69de29bb2..9dfca464ba 100644 --- a/src/Robot _arm_grasping_task/main.py +++ b/src/Robot _arm_grasping_task/main.py @@ -0,0 +1,291 @@ +import mujoco +import mujoco_viewer +import numpy as np +from scipy.optimize import minimize +import matplotlib.pyplot as plt +import time + +# ===================== 核心配置（绝对路径，零出错）===================== +MODEL_PATH = "D:/nn/src/Robot _arm_grasping_task/robot.xml" +TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 +GOAL_POS = np.array([-0.4, 0.0, 0.1]) # 放置目标位置 +FORCE_THRESHOLD = 5.0 # 抓取力阈值（N） +POS_ERROR_THRESHOLD = 0.01 # 位置误差阈值（m） +SIMULATION_STEPS = 3000 # 总仿真步数 +# PID控制参数 +KP = 80.0 +KI = 0.1 +KD = 5.0 + + +# ===================== 工具函数 ===================== +def compute_jacobian(model, data, ee_site_id): + """计算末端执行器雅可比矩阵（适配MuJoCo 2.3+）""" + jacp = np.zeros((3, model.nv)) # 位置雅可比 + jacr = np.zeros((3, model.nv)) # 旋转雅可比 + mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id) + # 只取前3个关节（适配简化版机械臂）的雅可比 + jacobian = np.vstack([jacp[:, :3], jacr[:, :3]]) + return jacobian + + +def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=100, tol=1e-4): + """牛顿-拉夫逊法求解逆运动学（适配3关节机械臂+MuJoCo 2.3+）""" + q = np.copy(initial_qpos[:3]) + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + + for _ in range(max_iter): + # 设置关节位置并更新动力学 + data.qpos[:3] = q + mujoco.mj_forward(model, data) + + # 获取当前末端位置（site_xpos 是2.3+保留属性） + current_pos = data.site_xpos[ee_site_id].copy() + # 计算位置误差 + error = target_pos - current_pos + if np.linalg.norm(error) < tol: + break + + # 计算雅可比矩阵 + jacobian = compute_jacobian(model, data, ee_site_id)[:3, :3] + # 牛顿-拉夫逊更新 + delta_q = np.linalg.pinv(jacobian) @ error + q += delta_q + + # 限制关节角度在范围内 + for i in range(3): + q[i] = np.clip(q[i], model.jnt_range[i][0], model.jnt_range[i][1]) + + return q + + +def pid_controller(error, error_integral, error_prev): + """PID控制器""" + proportional = KP * error + integral = KI * error_integral + derivative = KD * (error - error_prev) + return proportional + integral + derivative, error_integral + error, error_prev + + +# ===================== 主仿真函数（修复所有废弃属性）===================== +def grasp_simulation(): + # 1. 加载模型和数据（MuJoCo 2.3+ 标准写法） + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + # 初始化可视化器（兼容0.1.4版本） + viewer = mujoco_viewer.MujocoViewer(model, data) + + # 初始化变量（适配2.3+ ID查询） + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + + # 记录数据 + ee_pos_history = [] + force_history = [] + object_pos_history = [] + grasp_success = False + + # PID控制积分项和前一误差 + error_integral = np.zeros(3) + error_prev = np.zeros(3) + + # 仿真阶段：1-接近物体 2-抓取 3-搬运 4-放置 + phase = 1 + phase_step = 0 + + try: + for step in range(SIMULATION_STEPS): + # ---------------- 阶段1：接近目标物体 ---------------- + if phase == 1: + # 求解IK得到目标关节角度（适配3关节） + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + + # PID控制关节力矩（适配3关节） + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + + # 设置关节力矩 + data.ctrl[:3] = torque + + # 检查是否到达物体位置 + current_ee_pos = data.site_xpos[ee_site_id] + if np.linalg.norm(current_ee_pos - TARGET_OBJECT_POS) < POS_ERROR_THRESHOLD: + phase = 2 + phase_step = 0 + print("进入抓取阶段") + + # ---------------- 阶段2：抓取物体 ---------------- + elif phase == 2: + # 保持末端位置 + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 夹爪闭合（力反馈控制） + current_force = np.linalg.norm(data.sensordata[:3]) # 读取力传感器数据 + if current_force < FORCE_THRESHOLD and phase_step < 800: + # 逐渐闭合夹爪（控制第4、5个执行器） + data.ctrl[3] = 5.0 # 左夹爪闭合 + data.ctrl[4] = -5.0 # 右夹爪闭合 + else: + # 保持夹爪力度 + data.ctrl[3] = 2.0 + data.ctrl[4] = -2.0 + # 检查抓取是否成功（物体随末端移动） + # 关键修复：data.body_xpos → data.xpos（MuJoCo 2.3+ 标准属性） + object_pos = data.xpos[object_body_id].copy() + pos_diff = np.linalg.norm(object_pos - current_ee_pos) + if pos_diff < 0.02 and phase_step > 400: + phase = 3 + phase_step = 0 + print("抓取成功，进入搬运阶段") + + phase_step += 1 + + # ---------------- 阶段3：搬运到目标位置 ---------------- + elif phase == 3: + # 抬升并移动到目标位置 + lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.1]) + if phase_step < 400: + # 先抬升 + target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) + else: + # 移动到目标位置 + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + + # PID控制 + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 检查是否到达目标位置 + current_ee_pos = data.site_xpos[ee_site_id] + if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD and phase_step > 800: + phase = 4 + phase_step = 0 + print("到达目标位置，进入放置阶段") + + phase_step += 1 + + # ---------------- 阶段4：放置物体 ---------------- + elif phase == 4: + # 保持位置，打开夹爪 + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 打开夹爪 + data.ctrl[3] = 0.0 # 左夹爪打开 + data.ctrl[4] = 0.0 # 右夹爪打开 + + phase_step += 1 + if phase_step > 400: + grasp_success = True + break + + # 2. 运行仿真步（MuJoCo 2.3+ 标准写法） + mujoco.mj_step(model, data) + + # 3. 记录数据（修复所有废弃属性） + ee_pos_history.append(data.site_xpos[ee_site_id].copy()) + force_history.append(np.linalg.norm(data.sensordata[:3])) + # 核心修复：data.body_xpos → data.xpos + object_pos_history.append(data.xpos[object_body_id].copy()) + + # 4. 渲染可视化（兼容0.1.4版本） + viewer.render() + + # 控制仿真速度 + time.sleep(0.001) + + except KeyboardInterrupt: + # 捕获窗口关闭/键盘中断，正常退出 + print("\n⚠️ 仿真被手动终止") + finally: + # 确保可视化器正常关闭 + viewer.close() + + # ===================== 结果分析 ===================== + # 转换记录数据为numpy数组 + ee_pos_history = np.array(ee_pos_history) + force_history = np.array(force_history) + object_pos_history = np.array(object_pos_history) + + # 绘制结果图 + fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8)) + + # 1. 末端执行器轨迹 + ax1.plot(ee_pos_history[:, 0], ee_pos_history[:, 1], label='末端轨迹', color='blue', linewidth=1.5) + ax1.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='抓取点', s=50) + ax1.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='放置点', s=50) + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_title('末端执行器XY平面轨迹', fontsize=10) + ax1.legend(fontsize=8) + ax1.grid(True, alpha=0.3) + + # 2. 末端Z轴位置 + ax2.plot(ee_pos_history[:, 2], color='green', linewidth=1.5) + ax2.set_xlabel('仿真步数') + ax2.set_ylabel('Z位置 (m)') + ax2.set_title('末端执行器Z轴位置变化', fontsize=10) + ax2.grid(True, alpha=0.3) + + # 3. 接触力变化 + ax3.plot(force_history, color='orange', linewidth=1.5) + ax3.axhline(y=FORCE_THRESHOLD, color='red', linestyle='--', label='力阈值', linewidth=1) + ax3.set_xlabel('仿真步数') + ax3.set_ylabel('接触力 (N)') + ax3.set_title('末端接触力变化', fontsize=10) + ax3.legend(fontsize=8) + ax3.grid(True, alpha=0.3) + + # 4. 物体位置变化 + ax4.plot(object_pos_history[:, 0], object_pos_history[:, 1], label='物体轨迹', color='red', linewidth=1.5) + ax4.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='初始位置', s=50) + ax4.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='目标位置', s=50) + ax4.set_xlabel('X (m)') + ax4.set_ylabel('Y (m)') + ax4.set_title('物体XY平面轨迹', fontsize=10) + ax4.legend(fontsize=8) + ax4.grid(True, alpha=0.3) + + plt.tight_layout() + plt.savefig('grasp_simulation_result.png', dpi=150, bbox_inches='tight') + plt.show() + + # 输出抓取结果 + if grasp_success: + print("\n===================== 仿真结果 =====================") + print("✅ 抓取任务成功完成！") + print( + f"📌 物体最终位置: X={object_pos_history[-1, 0]:.3f} Y={object_pos_history[-1, 1]:.3f} Z={object_pos_history[-1, 2]:.3f}") + print(f"🎯 目标放置位置: X={GOAL_POS[0]:.3f} Y={GOAL_POS[1]:.3f} Z={GOAL_POS[2]:.3f}") + print(f"📏 位置误差: {np.linalg.norm(object_pos_history[-1] - GOAL_POS):.3f} m") + else: + print("\n❌ 抓取任务未完成，请检查参数或仿真步数！") + + +# ===================== 运行仿真 ===================== +if __name__ == "__main__": + print("🚀 机械臂抓取仿真启动...") + grasp_simulation() + print("\n🔚 仿真程序结束") \ No newline at end of file From f0c8b2a1788f53cf1d0c5f20acda83a56bffff93 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 21:49:38 +0800 Subject: [PATCH 04/26] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot _arm_grasping_task/robot.xml | 78 ++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/src/Robot _arm_grasping_task/robot.xml b/src/Robot _arm_grasping_task/robot.xml index e69de29bb2..1c12af75b5 100644 --- a/src/Robot _arm_grasping_task/robot.xml +++ b/src/Robot _arm_grasping_task/robot.xml @@ -0,0 +1,78 @@ + + + + \ No newline at end of file From 07c4ba26cfe6237680995b5212657fc1f299bc61 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 21:58:25 +0800 Subject: [PATCH 05/26] Final commit: robot arm grasping project (complete version) --- .gitignore | Bin 184 -> 376 bytes .idea/.gitignore | 3 + .../inspectionProfiles/profiles_settings.xml | 6 + .idea/misc.xml | 7 + .idea/modules.xml | 8 + .idea/nn.iml | 14 + .idea/vcs.xml | 6 + .../grasp_simulation_result.png | Bin 0 -> 63844 bytes src/Robot _arm_grasping_task/main.py | 369 ++++-------------- 9 files changed, 122 insertions(+), 291 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/nn.iml create mode 100644 .idea/vcs.xml create mode 100644 src/Robot _arm_grasping_task/grasp_simulation_result.png diff --git a/.gitignore b/.gitignore index b455caf4c44de311614245d03c670c980388b0d6..52712a90362765509378b862ef7444491364d16e 100644 GIT binary patch delta 156 zcmYL@K?=f93#w+M;dNh|#MQ{<4ym>EVoW1AI@ln!n zpr&HYk_A_K_Rd>6c2@qXA@4a89PO2^i@D?$I;Oof^Jk&b=)ZB2X6H_Kn5`v~nkl`J P3PZ2^W;F;B!|#a?_o^Q= delta 6 Ncmeytw1aWN4gd<90{j2~ diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000000..359bb5307e --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000000..105ce2da2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000000..25d4c9c531 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000000..0e01bed9d6 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/nn.iml b/.idea/nn.iml new file mode 100644 index 0000000000..c23e621695 --- /dev/null +++ b/.idea/nn.iml @@ -0,0 +1,14 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000000..35eb1ddfbb --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/src/Robot _arm_grasping_task/grasp_simulation_result.png b/src/Robot _arm_grasping_task/grasp_simulation_result.png new file mode 100644 index 0000000000000000000000000000000000000000..b58fadd6562609b06a42a58fdc14e60eacc91c50 GIT binary patch literal 63844 zcmeFa2UL|=)-8(KYKx_o2~-aAX!B~vg8~}%LD=@KsW>i$w_h! zDnW^oGbl*TAUWN+4_0?ob#>qWzW3j8?|9>#F?x6k4&V90-fPV@=Ul5FTsSYWfp!-y z4GqnPGe4h_p`lqbO+)iH|G%%nPgdP!WyC*%X5yF4WR0}UEU%hq(nwu3yQyzvrmu5t zkA2-lEv$UM(FqpHxl7eVj5f ztSo6yShciX()qmF7k-s4=DD+y6T9xdYlp%P?RllnByMq?veaQZyGiWk8m%i@&(o$v z1KbPkeTB4VnoY&JTfI%Y%tZsF{o3)u>5!MnPc1&JE0*8CX#bnt^1DYT{|~-<)FC?a z7X~thvyCgn>GL-V4Ser7-@%-q zsqJsA#i5)WRU#;L_0vE@LdUOL&)@HG7jtmoI;{|+;_Xq9Z9hjF?YKD8t=(_rQis#g zl{d&tZlsHSZ1_TW;cFHDoB*z`&MUyM#Xu;7sm93mgiT5Axmxu{yOEc}>!Ee~iGG*}Q)@tMF&-aXYx$ivaeQPAkmHO$H zt3xYYES5`kRHK!FTuba|KDPspt$x{5_EeUBW3Z%G>U6tHUAc6fR=MchfL4lBR)+o| zgQaLAd%dUqp?X+r)7b4O`t zoP6Zo*BMd9ymICb7^Zc4-e^qJna9^P^)!a6W!X^d{o)7Wi@7K*A##!O40nu@CXUh^ zlA`ycd2~?5Nv^Vz%RIw7J=Si1w6LSxanaPS>qx+dmATSQ*4=&@dG|NR8MdTP4_W3g zM9N1Ob%kCS4)#)Wez#GGW6eg!)Y7)~G&ILwdLPx3|85uWAzT0Sujfv3(By{veVS&E z``6(%vFyc(*g^)Q%*Wde#Bu_E?_6)jcyotj>%bhd*@b$m@mIOb0i9n4DtYSx_| z)6LzYUK@gGXgW8KtT^u9fc4bD&^CQ~G7}q?!n7y*)ZFBt#`MVNIGc&SD7~_wmW;w< z-M$GMCh2HsUcB~rnBirZey(FExb+gv3*nWbH1Cgu*RCB-kkFgwxlM3UG_;^! z-+AuA%y_qv1x4J2PMpGLkld?>LmZ9dQ&X<#>Q6!VFtlhZ8gpEla>Vuz3p;dBZIx3c z^5Pc{1f1k%bFzmQXYd4f&0*WZlF0EDeEg}NtZM)_7&GF{vRDQSfLMyR}41M3c%F(?|zXkMfxGQgP9gI~P-W zGsXOto@S`zT>N`ZWY{h@9S39KahDEnN5-o8_+(FCdO5z7%_G~Lo_N~go*)rFK>XKl zjF5};Y4ykqGORJ+8cUPQNh{J;XzAJ)(DXK}smxEP?km@YGcj$4er=Pxw4snUN5Fq3 zf3!plM`w*V&@FzT&Pch+hMQ*%^YUH!@;$;D_4SwU9?c%j!o5Fkc!-OuY`4CCOS(CO zROCrNLGyUBYJR{Prn!>f$*sOyTwFX(zv1vvxw}X8YOhyq5}SXm)E|20DN6-T_(pST zTn9dX;gea#IvN`N%Elx;jm7zyw^^lO3bAUFVTP<%qzd;jA^6hJQs{uAa#`$0-mF!8 zQT_VryLy9^mTMV<$+F4;@d%0fN`B@~)ikgdMn4=3xL)`QK*WulzPwWCwK((Nf6I$;;&afAQVV3siXMXso8`#n}}FSpdAM7|M&BZeyH%>m(&ydf*-%A$4U%oSW>%5))8lY!IoJQe|>adFg>HRT0 zH$UZGetqSQya&H_nePtX8yBc(2G9CNO@tgHg1_-Zje=L&q9YK>4q>aC14s%Dv#t#M zZd|JAdE5@8E|=r4pE=ORglCzLkdCXEkdIN>d5_8BvWmk@XPjAYrNB^X$2E76$@|Ji zSx3{&`{|6bCTi(CrbgavlISk;>yUPwl}*Hwot+wHUc1}s3f-*huA>pvvqqVtrx|bN zt}tGhs4K)m?Rc)?_5iT70PAO~m#T&0*MWv3beu|;?hNn1Evo7>(!zr}u0}bmc{cGz zMPP|0>#2wy!J4?;3=E=nR`QnNcUnlC!!j-Ha^JzTuS{gBIqE8Lx* z6srrG;p>yAi+{Dw(!!KGcHpxNHS7K;wXF1?`sEUZ&nhSB$~|N}cG%Wm>7&Q5wjUnt zJ=snm*ExUTV`_Gz`8};!wrO(Dlh1Ij9L}B_a($ci3gFB5zwej?dDnlZZuNQfJ#2`27V4fp9zFTBN?Mz<6Tm1oN~la)Xf$tYLhtlwM=>Y6 zdHAubZ-4Fi`0^y9dG*EZt@n7l)U0I`0`T0LZjrs@q%??3Sr(%6Ff;yHNr0%mO`2w~ zp|C8!Z+NGguGCCQ&H|?cs*Hk9Z*F%KFc>|&v!32FtwkYcwnpElfaaXWh{w-wI*c5~ zDFfDhEZgu{ls}RkPLtG_X+0j_JCrf3);0EI#R|b0fxyhCriyjJU-sUT8-fiH|JGn{ZnfYp;^qF(f>lu#fZnJDQmIyb%VIy3Y z&)NV0u_4j8qd=3(sPferK2@yBG)<4hj~PHK36G2h>f?;RJeDX}%WU^3sZ8=j%V;4p zGotzZ7K1q*}5#1BeK1bdGn`xp2gGp*&8>=>52K9IA)e5g6CWCmN^ZE~(Nuy9pO(8^z!Y71C>XS}=IT`x9^Z@0trXMrz|PR46e z9}fvCG}XG{!K(3gGh>InxRsMRD13UgQExSad(Ry7yQDj_Q`>hlcjm}XSWVUonKh9L zg;+8>2u?}RJfN|dbfaP)Lk@R~we3(^cL6qedrZbq#7En+oPXo5i;qQZa8bEB#QxrM-&9FCsa!b#oYHI2yz6e(GkG^`P$x)@d5>8PhgQ9!ie zo#D16UvM4jV%)A(Q&=H+{`fZ0=B5j0E==cbmA*8>glKumZTYcrn!9=fS0$Bn9vIIs z>y3`zdV&DOKr^0wCq^JIPZBR#oXizj+9IgIl24YPx;zg z=&q4V=TdVNX*Y6o>8WOI_tD$uCLG_`uUGJJry_gww%s0@2-c&OlH48{=V`r2_!Y7F zT0dYDn>DF4kW{B8H@lnmLq5H_@9LZ7!sF*qg^N~XSPnlIvNRN)YBsGvOx}UNINeXb zeY+;-5W(Iuntqn8);z}Tc~O~eOhTv2{Y4TcsxI5=)y257$E5BP@;|T(`!MLVYf(WX zAVE`mz|y=nquIw3aVXiWCL6R9TOuBOTKJA;H9#KWSY654DR*QYrfm@$n{O# z%aiwLy^^qN^AQuaDX5yuCF<6&v7CGUJ@WP0TH2fQ)1%9*Kv%!|l8x}v{P<|8k;4(a z&tA!nv%R4SA14v4If zpknu8t_%*_XWh8NrMoHXXTVkXh_JCGbN0Y;cClQSVZMBxZ!N&6ob(7n?c~z*pd@DCQTsEr2s+meKA>l zfkSj^Hm@dP)4Cj+aKmUASf`zg@}0Ny=-0pQsgKj()vZ2cfwE_Zz|AWhmtXHmu6}!G zeMhL+f=;4d9T(`S=Td}CnV#-&XHQ%J7;R4oSkOl$cCfE5HYYz8*PK-OSmJcEiDdrC z>_jysF#~)%%`=>d&I`>>aP)ZFFIk4aw6M9rva}iZb70#{JPL1-CbeJ}(_cPPVE1{}gDo8!y$T#5ykYqs)4kI+Whu{`nrW8<;UZi$S~pxgI;so!{r;W63bEQ4)B!KRDA7thl4kzKZ{X-n4Px}A#HdYp_z7K1URu#t5rgoHK+Cdl@eynYC?b0`v#IJZ6G(#9{1dKpykr$h{q4e;)2U zqV=>X2PxXP(?g+PDrae7x+-AtlKAlV-R*Mx4}ppa_hVG| zuh}r$_T089Ot}mpR;)HkQFh3tGdclaoHm-=EV>+DpqXz&8GMa zd)!8Kg%5M9fDTyiSlZ8>2KBb2G!`5)TE)BI+r?mtM@Lg6_j*NJm9!X>3mvC()?_0) zqKdrk3=+_DsX}*VbwfpW;jy2ilvB8DriPd@z(<`fuDomqvcZLLT-eQ*x+W1%oJ0L> z*g$x-zB&a46RVOx z0g95>h5NJ?-(_2mcWG9&Shi)63IVIB0f?C=|tC~}6!hh~4LPF(c(2g-|xs@e$GeO3Psp%)d-Ql)N zUu}Auv!gm5dj@wP(j9kTuV?=X5>cxPSjHXA+EZeDxaS1oZohZGX*K?&_35rKND$ffK_1{IxN#=zf35M0CC+}@^CH2C2 zp}|DgTYwsec-=E+ejcz~a2V}u%4T$=jHXyEysPWTkzAxmRh8>3Y8y@KC$}b(0(BsL zk88MU?S9V4_kz74Z=7SOTl?h|<>~rV?J9?iejOQl5usx)tZ+2C}=rv|NNfS~>J(4>V2{MKC3-I z=yz03`a)u@itI`in zNxS9VH_tQ1)om`I9zmM5XE`(b2xwm`nY~#nZ$L@!_vO`=@fao<^ot%+^Es&jR^K=bodZ);E$iz z!O99wHmZH0=rKr&`Y-F3kH7JT>G9y?;l>cXKmNRIz(FfGnYr5j!XJM=n>@@gm)vX_ znLJn>ArycAddImoSN|)N#2YR8EXtv)-L7}shq82G?q#h!3SRRP`@zRt$FT! zYG=JN8{=5;MCGgwwgSzI#97S}+}ALfRU3DFZ{U;sz?8*z>k$_%!(`>MY7-;nJGQbR z=N|k;&V%rT67dx6pj&ZXD(u?eOeS1HUcKh(@_^2dQiD`m%aT#Wv2#=Tw8_!fFFW+DRPdQY=L!f4 zDth+QE3!)u2H0&`Psc%uIL^L0%XOnV{k73eHQ88(bH>J8Ej4`ab1W7hu_AhY6gOZ0(Kh zA-M5?wyMWg54!s9Zi9d`sU%eFCu%4IfflCjP-LQLTMytq&pdpCka0XZ#iX-{<2d0% zh@|tN!%c{5eUBm77b|dj2{TNBW??t2!4Tf`wQ7H$c*E+`FKY?X?|8RK?4g31^#P*7 zft_zX-5=mE+e_i-XI`3?z(tA=w`Q?V?8eowQCaKb(flBRIr1bq2mU=E0@~WSFgNA? zZzvW(qL7k!QN*@s0Cyh|F_O&3TJRnEJE>3QLS(d365DAvMsJav#o7M4dsOx$5Q&Ay zdhCnUXg)prCACJDq`#kg*>d-@noNpeivSb}woD^JUUfW`7X6?^Glbe_dn?lJi^EiH zxLQ|QY{85}De(}YO9(TF3O2IzI;x0H&xtaj;k;khZH*rib{FgmJ3HJ2eDqi4^9&!J;02r0Q5ft6D8?E8m4}}o7(Qo&Zk(0h?Y?6L?m2?g;|492b9de$3OIJ<;(Q>Kgs#Zh`Z&c z>|oI=ZjsHV#AzPt&7q#1S<(sNt2m_8=V;_jMID>UGY37C913=7U$6S`^q@l6gc=*! zKb_fJBqUKI+l9y^gC;&oS%6p$0g6pYdJ2Y$2l`(f?#Ef_TJKuovPMMX7m_bfUz)80 zwN;DUaYdO*^<9WGA8a(YPkD}q-v-!cQrw3ibXsH7UTN2Dqj#PTnlnU}1D$6`M>h!( zapf@X%M0CQwzOvBu`iEqfXplL7ZD1Pq5v|d8h6C&h!?!=M_eMjDDq{NerA?5d#_Kz z4}Wh~neq*vP*aSi!PbbNQsGWY(I;-N;jtR|G%%y6?U&?Gl64*B2oF|bPgSV2*>J1< z!aQ{G)1l}6+HbArst=2Th+YWpF)%e%fL(92J-^>e1Uruh8X`xc5X8vhW-s22pFs9f zM3yE~a#TBqx8t1m^(oxo#L0)9AUK3BR|KDoLs55KRRf9#KVf=qfWRRvdeDT6vvWM1 zO*nh0H2B)_YwdXt6s-yU1qh+lFYSH!0E7ClGkeT^eE3-!GgiAg2bLWn#tOlFDjN)g zFTY|+EzV!m>LDc46{(thI8cGx!*N4$MVr%#Rq;I2f@VDzKF5;{WCd}pJt8v3nJ9PI zwe8>!KPatmV-^$T${cx1YhI30;({#ofO`(=Lx?nP(Dc%DA5(#iqCvxSZyQR;ghJ1s z-|WG(){b31>OS_8c%d6p5LnK*{+Nt{++S7^QAk~nW(rOd=R&zTN zl>2Uxc2w2&q!1$h0|}G^?I^@uxa~f#AI0|?BrbmW@U-Lpez*I?NEz3+C$SIiPgAGzq5Z59Mxp&=}Fl6K<{ z%ZxzJ9L@)YdvU|?F6tLLsmP<~1$w>)xxQh%+Brt2DgV(E~&;Fe$dd&gnp+ zdPo4#2L9$pNuSt>Xd?kAgoHnIAJvlM!hE&Kk9VBHYufJm@Eh$8l5j8c7sIpQu^eh< zv(BM@8_kz{w>C`sBhg<@ym686hKiQH0T%5K4r*16b`*Lk*P;a5f}^@XqbMYJyds{L zPCdtA;Tj}XBD%}nXsZ-(V%>IbajtDio=WuM$QS;O+Rk>S(<`C(m$kbu6ZRiLsVzkF z0JGz~F^5X(kG8D{e6$7tc6c4XI_+qGhVJZ!hO*^*K@CUtw%|)%_-xl-pZym#dQ)N)1VPrjo0$DYa{3}7i&RMx zLvnd5^O|#7H)v@tXUS!8VXlxcQXo>P8CN-R0qBLV>7Nw(2mU^Jq6I5Y{DVa(9Zs+L ztr%aQjSJSQ>MZv1O0#Q-*K9}x```2`syclbb*%F-)2=)a`rC5mT87z1kfq4$5btV+ zk$=fjgPOr~w%7vt#GQ~mTN7IVh_V?P+rUet2fVHvq2&|rgL=j%^->9Syd|hCrDCN7 zEh%4N7}x|r->eKoV#Le9@zsoAk74~F)V+@OyreS+0|za|H-Zk zSpU!1^?;cY?e=O}X;)|!`R)+oqU6*bfD$IU&i4d{5MG_Zz!Lnkc8hVyVPdX>5-5H& zTVwf4$4)F9y7K>|h*pIX%%fTKEEVKgLhjIu+Z&z1;pP2SM;rFwCb+jPElxFzi%iz< z$A-*2JT*E$US3|bysn)NeRYaOh$Ngk@g`{yNvh3$Djx;#HW>Ww149a1;0Lr`|0tLV z;arr4xS#>K^FhG;mxH5VoZ~O`X9A1GI4;&}xEsVbCvuZIhrG^iGt2e*U1obV!X?6Y z;Lh!EIkhvznWbmmsM;)lFxT@?{Xd);^i(8*U;K+lR1j(bt7oMxiQh+i5b zJ`^J0cBFQ=xsV%3suTjpGbd`5l`plTB4Na4cN1Ial_mhUr!B{k5rI>&hhMik>~P8q zih}~-3nm%*PJ(&Yp`V9nus08Os-GP&-TZvGY=g&dH1^qL_A0O zNf?RVSIHMH{s~Z*k9O(_DAR%yYnd+TQa}g~la)fuZ|ztHEr*&V0d*dkj{?NJ=A`-{ z!fO5;tUZ-#4|Ei|bEiZ>yg3L_6DV#RL{4Hh+%eDyV-4a+Vg2i&#VR2rFB`N31o3 zsMSlVLoyjKmvPCavT|S|^@Veh?&Uw&qRI{XJ0ThZwiby-ZKBT3&VeDp@=w2PDm=0z5JF5g+-};7#^=V1 z1C>ntDTarm6k=QDjbe$9f<`y#FM@ujXVT5jZ}n}EwhK$VkC2^7AVPkWVnTaj)%jw7 zB&tzTgv&J9!AffUW;J~tf@S<3t~)nVAXiX#91Mn99jm||=&z?z0~dZgtJE6&KP`OD z8jL5!+8YGzF+!oQ*q;rVkv%4JtVmyJGvw)d{o-uES#g7ll*v0qrpqN8)YYzdG!gFQn>ho9g%=T!4C zaJ60N6Ep)mz?H;=tZh!EcBr+`Q3^i$XGvEjOIlK6#6G=74Mnd;IZEO$7UAp5-NhDD zt1a;LHQy>^}t6;0N@YQ*I;T6hqMeh5kAeAUR@ewafi0;Ir^)l|s(T+)vGU zZ;{pLHwlj%Z#Juv8ihv5jvek%3d^us4mOT?ybjUjkmpV8z+1V43=wZh7ko^jH7zyK zvt0)@LR42cw@%RY459=A>+jwJs$u})Dh?%59!zD|KEJ&)3Q{W{8ay*;10ZJGI26kH zaQN&F4RANq3??@ zVqAK(?z*ZCV%_VBukQHgcp4;JW+E_T&h|x;Rs*@^eDyutN@r>Vxea0|h`vrm)S}wS zL<;etGRX|a2yIeivq`M~24H^gU!$7gu0)o4A35j`deVy2Il!2#rnpCa*EQrC7$}G zc2u-(Ak9w!AOxXqsToB_3{ABYVf*N~)uM)Q7?Y!)lmWV%!R3s)JI?PXax5Ytd(wHt zTcRu(m+%zk)nR9asjCmaz~86!0hN~)wAx~41uCZ)9?c>Y{_0>ER%P9nme||*JrzMg z2s+TWg1|0QYi|N3g$7=qto1R%<5%plL_*FdmUP#uk(OZxeqvon8bM6jlj4iCz~i_$ zKV77dz2)}WO=ThQaVT3zUG_AqB2~aq#*qb3Kd$VtnTOX8Pog*_{y;`V2VzGPM{+qE zodb)f6?!lTyEBTDKk5LfxDf1lJAy^Y&^%lOds1KlJdlw#K32?KzMCQg4l1gfSRr)< zK1oIoy8-MGqj&py_359|`a?esL+3Bv-TQX9K|7%Ir|e~?(4G`w+~|rN$0d$b$*e?T zDDZ(oSd{^capC)9!A}%Hb6w&UR9<*XNbzRJr3D^XexkW>sZKtlF6#697(zZ{A-fLS zbqgu7Q-hCnu7dZ?SivGZo-0k(NZ`~s>Wd0Ef{CmOcH6I}IMMot)tuS{I6pecInAey zVwWhz0d`{_Sn71}^vpAP@yn)M;B{tHqn&I%EQ;6^5grUBgET?G;@+e!Ozro8BBh{&Ekh zv;ptAcSy7e5{fcZ+L`maX=0opoZ)wj0bt99=p^{TY+=qX%$JoOLp8=46APe}UI02QmABjuKXpB*=ZNh3n86K}W0 zv-Jliz_dqIpJUas0wGPxIA0;a+XG&F!o8b4Y;?ZHzB* zsHF1*WbawXs!awKh;}EBw$dk9||#Mk5o7*)J5XM`BZH<@sRJn297z*d{fUukY9Xiv;~V$I8Jjwf!BDq7wqD z=F3}08uYO@-me4I6W8Xj8CfW=ZIO=0(IM|1^XbgR*(Hbb<7muUiuRHKYGLBe`YOX{ zoVR%tVX%NdXQni&&Xf1j$@GRJtDSn4*rck=Kg06rFG6&1D;O|g{&IKoQg=8;+LV-x z7d(Nt4}!URmURAH-OX^#_BU&peOG&HZjgLzDBn`ufdaa%Z;I@44bEwqDPMw~NFKY0 z)$)GzZ-gL`x2efok5mGx2+&a4K2`YU2GyN^nTa;W%qAMz`(<E_T1n^P_tl%Z#gBAN1ZWLusmg zLN(ox#4c~dDm2=FNGOUimHE5Z+w4Y@1o))(b!l3@WetD^8^ zy+*_3F$T3J0*0P8ISkvjxva3>)(D)jx>Z~|y~$WsW2ojdF&??6cpPit;cZRkj z-K=-Z(}U7(;CJGh<116QkZ262{~n?;xvosBPLk}i7)Js7p392=8P}<&ZS!6H{qM5A zkmg6YzWLUFSJe`iCeYQ|;O?vX`DW}Xr6u2W-Pd7vJ0AU{>|(iSMmzB*EFSBgH<}Z! zfpL5q#_)ioYRMLh5S`gHws%<(nGWm?M1L21UMpJlWqJ8uxeE z{!Fw}rPIWe^w;ri?LPNVk-4lf!a&y2?`hYiff!}vD=n7u51%KQcc&^^`fb-&JUyYF zlZC~@1M7E`P;8_pko@3!fvkvowo+dmU-|-m|hzJ;XvLV%f$bqBS65Uo|XLN2( z{b0_nbnJ;LI+FA5;mxZtI%^L znvj^YIg?4Tot@LR?l}`ynPX+_4dWY>&ppZTTSp<}HS8?tKI3?{YVE4*Xybl1qp%Vj$};4KhWl&K3JY)WxQ zTTeY59k&YO9DhqriKeY#j&@oTw_@sb%AlU;8B5+ELsQrC=-E;^(TqMm`k7|=%J%%; zq0IPxi}JB@VL#C3qo?`bWv*tKH70Da?N4ZYzlGvyCgex!dWmJUSLZeoG-*)&>X8Jm*ev}Ib9osJ^e)(3UT0F?Cl{Fz-r+5V-Y$?Ru(%V>$Xa-+-rD^}l18bYrNZQF zH1LCpY^8b8m{jwb4E=_9{YFqhx~!3G>N&~BUX-pFoxMj{>MrauGpI^BI|a6nZO~~| z(S~Wh$Vk)XMvDc{kb{0^GU1&%`ZL=`r2K``ewv%~F1KT8Vo4gZMHXaZ9I3JJ`kMBG zL;cLXq<#9Ybf1~c^;vao`h7!z|L$@o(>dW3!);;722EO3NFFc~UU%rT2^K7hD5{G! z_>*_&(t{)HmTnBUeLLh&%c+SGOFoC*^9Q_(s$01h&xMsVPSLd~!%OK6zDb?ksc4YSdpTq5HfN)$ z#N%)la-rUrS%4Em2G{&49YWKe=3HP4d=>R1js~ks*C2NawY2uC2h0Q+`wiP2bb|u} z?w1p3O~eLL;LgmP(qf?u;J5U?;8@tYdyFPO^BT1ZYnA&mRfR~otdj8(Vj!KX-?{>V zS2UnHV~`e|xo+XrP3U=t2KW)4WVKa$=|%)?fpo1+=avoptteuLBBl)$twE*g>$=_6 z28<$;J(m^cb4EGY0uBArjY~JzJ+r2V5MCW-=4LaPv{{1LZ;f%}G}?7f0}U=Em#@N& zD=MpVadBanJD=`wR^wAm-h?tixSGYDqbQ{`I|c_Sx0*IQqjo2F9<0fyYVBfBOW5cA z0b+Guxs*Z9WG_Rk6-qvGeW;ac}#w#=&N{C%9$0eQSd*0_^IRwe~;%;)hboT zg^4)Sn65=i(0k`c?uk7_4ZsClvlUjNwm^amUzaM61ZNzK&7SS!c$LxVsT^O3E-xRE zBHdr)OjdX)#VnBKEkaunKKUr?9kQ6zI=4LtX``p7TopuoDFWDhIL>cV6igEsS0z)VILyz;Ueu#ky4t8*GmK%

H>nkyAfq=Lk{dWd_!%Vs14JcnF}g~dfnVnv6MzXO`Ii=Ra!D>!0mh8VyPwsS>7-64XOcE^?w z6PX)cD@%wV)gW-Y_f~9_q0ZvsR8AobYhJ1k2^H4xd_%bS^~NsZ@&A$zLpvOqJUV5s zQb+R{G(NmN5N9_rR*%&1Pv4-wA#6kMcd{l5wco#g=IK7eOrCv7eM;H$jv8Ns~ zjk(DOKOvvv^)Hsc7jOHj!-JB^hQGYrqe^2U0SjIb8kUu_7NB-Vm;qbU&_1TNJ<)m# z7zg?QPQLGS8YW^F>BZTFD3?SUHjYPEqbtd$6`O*FZR-dOkLIJft2X74EDE2vf;WC{ zJw&plhik_a9P`0vi*T$%r<9pS>@oV_KH+r{Mb3B-U?fF2I@c`fB{pP)JV{YWsKHPPgW zAm9^~ZRCfmA4LnN-tru%nW}|Y*hhwv_9@~Y*)yd^1!Q0V>1(S7X>)wTvM#ijIKM!MU>J#xni19Pf>AV;G1PG5<%>sM~0V#)q^ zQ#6v`gIL3I6kJcuIdVO@@=f4y61arO11})TbW_`+1$B9VY>tp4d=%BR`=V0Sq7@|_ zH7BcDxa}4Z#33>6L6=Mk*|mIiTS%pegF!o6vO<_`9}ifvBLsvlc(oVK%T_>7E+H@2 zkkIygw-H-7>>8ErFRB6hBc${l-3U0X`ZEUf(Zs*}fr-fU<2d87atQIs5yfRF?bwh8 zE^*Ew8e9mYlH->sBry=0J;IHu`QqKb2>t)(fA;^vYM=_lDKCZZ<`?Rq7-;&Nfv)t-`fEAKY#vUwVUUGxFfkUA1$qN?1({s4 z5hM)9S2P@7N^5f8d4y#VLol?+cF9eacckR(!)?=l`OOg)PiH~2FkXX3UW(YYy!!&E zaT7f45UwIFDE^1$@>0Vka9-DAXKG>{juJi)>bAKs473Jksj7%>67jmcf4o1T8-B9| zw0;AK{Y*IZ-#WVP6iGI{qWdL$d+YCP&wLpRK>JKkgWVYnQ?Ew0Vg(%6{N_3Rvv1({ zCNCi4ty@{n-G?BUcTs{VDSvJZR-ZhA3j$rZiYBo)Ua(MS9sH+W@mAu~f*;Ou32FjH0BOQ*HsBMo!sRHi*$%f zxw(4r{<%H;vD86n|4fHgxce9gcZ;x;6F%w)6!4o!{9kfH*OfDfIoKr+q3O3f6{0bp z(h};I;t*C2$dXsdXdS|c8@?gqDe6tAaZEP}u8_Kd#wJCTn1|JvzDZ0XjGe|ART81fGl0Nzv$6dSy@c{OAoU58a5z z#37x{()W^7zQVnP4>0$aFCf6lk-stWlCaQYKO6HpJU_!dc6*&TMlPCEMnV{ zIZedzp?^spe67a&M|%}_coivST8++X!o6Sx=O0VI8;sE9%`8;ht@mwn47a;*m#s_% zFkvBu_iwevUzwzdH#|I^6s6AvjIYV75jQQQU8!x7035$3vos)Yu`sOKw_(KJ!2!NlHsi}ULA>58tY#W% z0T->fV+!%;loyw3{FOmWdAg6DQxS*Bga89$YvMK|b`WZJ+}?YPTgVs^(gIEuSMe|< zqsb6Ac4{v!*ju7E8NXe3@D#-0Yj1yDN6ihe#Ygn`ksSejjvL-pGK%0DX~Tj4f-nkn z@xi^sCInpO*0wNY*^udsXeUoc%LgfjDpiwuofJ{fm&MVF&_R$ofh0i&#NZdTx(yoT zqpVueOTh6x!Ms6$YD87fL*=TpVR#u!I;oJB+EV^DMi+YSGNmFk6T`?0PY5lAvKZJwuWEJ?d=-nb_DR-{3|QcZ0{THSkfF@zL|MRz zcgyEUP%n>WO#00ly8|8Veh}D$J%gR1VNfJu9I>~utEzp-b9kao9riP93<2y^FOgxG9l|^DNF+#`K0Gg!q+L<> z39&&*#@+d;9~&I63p@HGR-E12gt%Ahep|tlcM0g$lCB7i%bE$D7?c8nSc6xm^7_+G zxVtqw?T1VQ*!h3VMCgg{4qsp4+y~U?J&rxY1R}RwEhogITBwiDYq}Zpo;njy(LFAY z#=NJbUQ^254PYv|cIU@yUsqyl&)4eUB+( z4Y)1tcNKhQ4k3 z*2gQu|MD-}crh?)K1Mkj>;Mhz$ud+SISUrBodaeoX4_fwL2m6mJx=VCGW}e?P5gl8 ztcaa8hhsbIFmZUJ4Rw#+;1sbHkamb6y>B=5vI*ZOj09_xH=?Y}!sF1^&@2fM6BET; zMNg%JrP6Q(D;Qd1u2yLXocblVGUE$AL+!^U|5%2xXZ3~|gVY?J-VHd3my;nzR5XajbqwoX*hQ4;mAz z9+~!+PdZ1*slC03y+Yc94a_kKgQWdkeWpku z5P*d`Z3c6hsJ#kgwi1KExT}%fe!`%TPMS=ViJEKdSkI9l7CFDL{9qqZ5S1ilET0p> z{X1q+VLkH{xJ;)9w8a=;h?8_|DJobJ>fk0hKbOq^taunB|O2k>O>BZ z+S^9b_u=A!s!(^rE|A+eK`Z!4l1Pk%b7aqSfR#qeE7W+hXqR3r2T1}W{mYT@_z8F7 zZzZoZY5_>z9Xmxb>;M1m_xIrV`O9(F#K}lULBwXVQAi^y8Q%zgqdsB`mEu+yzMQc{ zkB}LzV7I9wEMmZC%Jhp8`#cb3VYld1ntm4X=aJ5LfxfUHB9%wovcwI)V7!d5vWN}p z>OCepBI}T5D&p0JM_iJ0$j1b(B=JrW2@%)YhbY`}s zMI>uFf=(yri7UtV=G%1@*hf*mgiyFcuXb6J+6^W=QLIW9q;CChZtC?J#OnZc+ozNn z?2#`>bw$Z+P!4ioQQk+iwSG|}Z8-3&xuDvQR!_z$JqK(KH{gOg#xX5<_0wGfj|6aj z9nb>JseJT$&__;vA=GZdBrX`{5Pq8$|~(t*9jAk9KPFSnq=J zNLn4?>{r;max#QDvHorB77R|>RlKLw&}9KXS5|Yp48MOk1L>Q zBrI3z7(^+wy)}h!MI>o(e3^svD)V^|H=k1hOV zWK79@aGinGlSJ&8yo|O1!XQySQw}4ynTa6rC}|+FN5JlXlWm&9847UJ@Z26%xcxk= zA#Nm3>V3fe`-dODk&MOaE)P&AO-|s;Ft*}}xr{eSJW0Z?j$UN4%J$ZrCqu`EVW+1x z!tS}S^Pknxa#kf8AJAlmwuUZ`!jeDrfsc$e{KIjS{@cCLvoqnEty26gm%4U1Vff!=468E>8lA)Pj!<6E zbtGv!?3D|5w!r#Nn2P0HkJ&tdnEp&U(nvpDAe<9CHeY*OYq{Cjij;cD(3;>P!N2Y# zCGIK(6=QL9nrJM{vR;^fv&rFm9MFPpA8D{_$iy=fLD#Z(G^T8I=K(l;62QH0`jzTF zuC_r`F8V1=k}K^8aOsSs6_sv;jGJY+9Sh=iJC`1m0~d@sITVxL7ok6&xo#^<9)Qx5 zq?L>)gH_;wWQfiIS6N5~-w8k((jXj$UZ??qF>TWY{P7ECIoIK_C3^_whpTTHQ^F{k zgkF@6x;6^&8Q1t3f8A|PS#O6?qA_C)=vRYI-qID=_%$tBs(bOvB2Sc5fWg1^Kg*Db zbL_T5vDDaz8~T@{L;v$&(f`|j`{>ptYEL4`c-Nr!JELe!4wJ5OYw!sO(`}+fxzp%^uH95?I$q%(K(5 z-+GlshN06>^W_y12;mpKe~;Ng00y~iGVg};dQ;_IfLng$(@NT4A&r4D<=&7$iPs+H z?fSWv1o~pI9JNyli90qR2jj`|mPanq&am|%nH0Z$i}aKafRO3n05X9vcg|!SY`=aH z-~b(&@|0|dqBnWge);?#wc-5PchV0d>2V|t5SJc?Kjfq8A!AXa%&ZTLS5Dh$l9Gh( zyCGl$W1tX&3bLK?dhr?qhNh_b4{`W)bv)@s1?LxV$Cmgn(=^GbU>;<%0c%WUILq0F zlL!Q@Nj7^_&8JXRbYPDXmpPf*ob|29)IWOy$oIN3no%e1sC1y;lNr`$?9yjaQ_$FY z8q8{PSSIi3HZu90SeS?a()iZ$8$Nv*&b;~t_}=~i%z;lWF+Y1N9mD@FrDjTbB!!Pv zKDQ_O32|#vn`JfsV#?!_E8EQ}l7F!&G3n%Your+3Cb&GJVI_+Pzy`(`=r3S|4hje&z;&43N2ek z?aYkkjG1+KWEq%F%@&i52ul%fm)lLbT~}Qu0~`Nix9J!CT(5$dCMDL7k}cOlV$8bA z1DFFA2DBxkpLQLj=BP}ny@X76IH9K4fgYVq%=6Le&qlJUNHJ=2t@S&ieV#81XVVU& zkUIuF?TJC<8*LN2d_7;hd^b$9MPj}M*gNSwRK1d`?MQvCYdcXu1+cs+*u3Uz>yoZ@{rt`Vu&8gF=x{1NLGvnLNX(J0XYuKJ! zPs$Sbs}N{A!NH;LuqkGtHjJbDHKp2ucs6zuP<~?(BWbJjGhHyWo(JC2c!x#2+e|VH z=_gnIR^zY*p@Eg?4ZbaUv8;;#c5Y>3*#1xvtf>v)$%%Yvc5jlN*oyup4tlX=4Kl z@|t{k%UbVh3>vsn$0{w~^W4zw`uKkapuQftP$eY1|KHS}^X1p=?DBc&kO_qPMwSYZ zD*ye+8#2OL0Be9Kr>!@#aL@B$Zr(}!X^>z^kAM8c6dd&WbBItG&aHb9nDdtvxc{E1 z#JWM|vJL99m0t0mfZ!I4l9>rN6m zP^uT1?Fl0Oef>oY5p5^k9ZF-EuM+-~Cb7S+|0*MyN!&XiRgy01Wr1NaOmx7aq|PKD zoI2T!1KmHYwhLd~1|M^~a@(vtLBVmDe#yf6T%%6Sli*43I8HQ zbjclmte1*zC08?bhY||0!R!xna{}>T2oWszKiGTkxT?;rTNpKHBAP@+P_S2!WyaQ4uUO1rY@4O}cbV1e7jSq$vnU6X_kkF_-3?yyxV;-~Gxh zzu$fLABI@=UTf{=na`YKjxpxtCQ{^?9C^0Es)^CB{(tEop=eUBMlqDw4s0yqvo#*F z7vqf?2>qkZCLE@AL|OCs#?)`%Uphyi!f@t3g}qAr@Rnx4?TnKgY!9^}GfIm){t6)y z7c|^3qH4p~AOGq)lMwFxpgq)Y0qj{)tLnPG^d|n^f0$_cek7^b2Iz#eNi>#a8+tP2 zRG82Xq{tq4RN%e_2O{v-4v^0%w+EmYxF$1}HdpVYR~|uSMD%Z>@lWiwDzwGPac*xD zn^5MDqV?zZ6KdthPq_lgW1>SEh~*BYP8ml}{=c{q{C61pD%$wI!g(aRJZ0nG+vk`k z+yrIeTpd|FxRd|4I-fM^bJN3Ldn!0fOvIon+V11?Z`QzulNlS_qEaI`oFRp{%*=iu zX8>pm$z`?astwgcj3p{`y8l2K|6BEt!&?BJEkqFrM(`kVGb#O9o3KQstG=pHS6+O% z_BB@TJ&1)N>WZoCCyVF`;22(-kf;BZj_06pxaPsez3+Y%%pPqvG0CnR?7OJ}ri;G)sem35*Tg|P+EsHbW-dXZ z-D5yo?h6PN5^g%$%$f7?DUzou#^_!;=4*TDKesCQ(}~wJxqw`N6XFLI`JPwKXN35A z?z_(h4@m(Ejn38ShgrtiTL~l|qk{LeAJd+{y@Ub-=?x(f70ene?~YHqZBA!$umtZ@ zKBir5G)M1GlFs+c*NR6X{eSRJ5lW}Y$hagL5lS@n+T=zxpl_R zRatz64ex4wsF{mM_G%S)a?O9klN0Mb);{w=@Na2xMG=^K(J>ygz1EGx-b9HZ-T|sP zKF}$0?qAz9lBpl~0($;bx&L!=M#qEAyIuaDD~3CP_i@$?9>v0;*V5qszv|_xPGn#1 z#JZtw0Qp;x*9F=pDO-3ah@FO9c(WaOO`kA`j5G_t5jl!R8Z+epYhI8nSE7q(qyfU; zs@&XM=CA3%OycQaxqL9%RKx^`HX8wmOGgG7kDs`PcY?Q8v$v+%_0^N_=8?O~0W?Sw zn^+N3uG`bhq-7!|YL?w^wh1hWd5$RaB~a z$rB00Kz&NONyXf4QH#stIirFxDT%u={bv_$hUEK2jsftV=LfY)=ml~%PRjT9DKE9k zK$hEWt}%1$vmNqgfm`1onh>NEll*)sY#wN1a$`mGM}AE7LEZ z(ILS=Vy`rc$siOkwz0U)pB`(KvaK;3$$JgAvU3q}1ISQ-`-9M%u(z$vbN z>QH2YS;e?v23lqut3kNTC5-3>O+oMqXbS)dJJ;oLWT!O&xgs5;QCsTNW30m`iTcWw z$#1o(V}SZKhm-Se55W7mdlkDp- zd1Ek4syL+Q+ebk`HH zVO>8D4geli%tXbwCF19OQlV5HR@WG+H`S5GLEo(X-pL$4vvk9$J}@7qZX6Tqc-%dK zpqgl+4FS=OnckV{xJv8n;4!Ns4zk9nWD*9u86I&?GRsSp9dA+*dq^6nKBO?NY*XS} z-UVwscKR{Q>D!PW2BF=}a-7CDlRCY+aLMMQ^_^@b$nrK%{AM{WtNe)7UPUVv0gy)< zD*pR9kIPvdw!}s2c#nl|=@MbJr`DWnFBW(+aGU6W9B>bYX2OqdUdC&}`;E#}nge)` zH=HnyWGTR~^ig=-@e%_&pk|iNt8;BoW0p)W*J-xNna~oE^0!y6B>kR=A9l^TfQrS0 zktL3GtV=9Y)}MNa3?~<=L7|6Rf7&-Zkz6(>J2JA4+;eh~zY9DRxA}B%0L-(Yp}m#l zOpVnWX;#sD1+|V*aMQfQ<8@XvB~afAwd~W|%B{V>ENno^TRTkd;osP^f3R_mWE&-6 z>-?A*1Ty`uFemqrocr7l<-@qYuaJu65Ips?IJTATH5I|lKw%$`q8Y{G2+XIZ>47r0 zyZU&$p2o08z~q^!OiRhz)3&~CVNREo#`H86uQGX+9I2OXN-P|4Av^ey>a(yvkr}&`m+M8!E8O3ZvkCn?MIE2A9I3dOGf6>gg%GcN^;YNq)JQa zCJK3E0>)_k&aZInk@hRrqib31tJG5itl?2~j;XYBT~2FO^;gRLJ6X|?X4e`p= zS~tcbw{(qEl(Ramokt(W&4p^=e`@9SA{4`d~3d?@$Yl;liEVIu34FCX9#lrGsLJ`N_QX%6y5v<_Qr43-CcJv1leFp!iqT1lCkHfGq z)G#`D-;J1>iRAG%oSI^`wx!swuk*;*+@Fbr~F^d=FaJvnehzi zH9|v9R=lLWIKdN{;23c~U7>}Sy}Y-=R6Vt$R|OuX35JgzuqmrHgkCsSaqleEM_xUA z%JZB!2V}?n2e815J=)*-jXShuW^Hv>e&jieg#Mx2iCsvw$eOgmHM81 zn9M5@`d!3cO{EJYHHFnhnkgMyO|vdAvXdXYM{Xi&v0ueIT2{P3J9#M>>pu~FKqV>c z_?}6vqJ?BjH2G0h>IOkXG_Q#0-4Rv zK~&2}PNIwP27REfnO)Svitsw6_n74;VDAt$xVJTJYq;HOV3r2SSYq!7obDcb=8sY2 zD-ZwyJY!%tZ&zW``$@EyPd=?DYef=gQ@AahlsWZnWR#f)8Z#m!Eury+E#z}D{6o0q zqkBgsat1?$WU`}5(x;LvM-GbJKjyT-9W0EOHgySR11DZdnM4MCicE;Me{}R>j#(V? z8k5VI`Y7`#JT`p_%-o0mY((14IK)UT(K~b{v)cJGDmA9_4^>FLXZEV&qm%)gZK98j zlPCz*?!0KU_Cag|A%+5C-eh)b_E5pbM@T(kgI{}Q)>+d+y|xVP9WytNw#QHrPCX)m zN}`irQktMqx-Dj!mPYrkM+8vr5?e zZIT?5b7r%i`@_QntRwS&HZM>6`vp5tHFpObVI1{Y1(mB#q;4cJ8#&C?%sg=1tqo#c zALKOr^Y>a+_xxJE+PyN@KHy}pX$K^zZXGHSz>4baK$JSMrOIUf`Ia*#S zH8aV>UZs+XOvlDr^7uNd%?{9y(OjP8EOiNJKHyjNcd#0Gf$E$+Z35MtH{Euvy%fn+CZM zY)p3?&`4N1WB@$OZri>RE?i`2V|t?Jv5*Rmy~vP*rD9#t(brAW<+s&TrKAf`yF~vM z5vGX~Tyq5R&<8}8)$VXb+-8*VRwH#|`Sl4p3rP@wzcA#-XJpqf+oc^~Y|-@f`+2`{ zgVRH!(ROCZ@9qWpt|*jBYxx073zhWK=^ded%^0s0*U?b8oAR&3*n#q5!;VcmoeRbX zhxVh_lK9Cu6V&zh*Y?&~^(bBGL($R zG%N@Olu^pEFv^2#9FErDhsgm+&~Fu);-mob`VC zK2-k}i5Yumpr|`ws#@1IjH#?Yk;jUQm_+IVm-!l6(<4cgLfTG^psSkL~mS zaJFF@Mmn$3KO7MadCao0;g{owA}1{BkaTsg&h1#O(6RbhM{f-=P>AvOAp>~?`Crp` zlV~f$bmP?*U2YoGAsJv0DlY zI6F6tbc!^bzkT#wg^Y$##O;z<^NUl)>qDCwP@A~|=04+;F@LX0m8pi%RL>F7fyWot ziAXq+A#S3TBHydKw#PL`Gwx-SzlnH-tWWci-u>x5&res6?Me2W9C&4GSNzsHzsM%b z+R=PTe$rV25^HxJ?9|BGaA``Ic^0jGD`T=0ZK8$7Sli29kw{(L&rDe!TCQqA45Sg}rPB}KMuq&9i;W>(wT%r=)%kz$z~ zd9&!p1IqO$PVHCPa6#tz9)*Q5MZz-00u`s^6fERpgnf$YFMih-;3a==;^b4=W!Kqe zZ)wh+wKn0}vzBS8bNyM;jX%!=H^qMfW=*!4|RafJLQe zooVqL-V#>tSQ9BMV3!h|0AS@N+UZZD@6T=hy;s*UWS&mj>Vb_zT;)I~yGQ*swh9kk zLIz;zn^4l-)tc5+eLPHo&n?6&?~pxmJUtv8g{w-A{=s5@$04b|)AB-haBGiiO(eHS zMbn8{k~efg{48j!aBkA^Oy_w3>MzyuwI%Vp;Iru<@Qb2O#?F?{k8>(KCcR>}6DZ?WROU+weo8L6iHOz()c zT5B$`Hxg{0jZqe0jfam+T7i_W?h7b0&6>-TEys1t)9g&hK*kwYfs~hx6}#*s>{>s- zEynzFUy-d{hp@hX6~9NxOoi36Ny>gqFd%M4`L-*~hjM065on#xnbMsKuV|L9bLt$Y zxkt6hd9-!9eV+;rzb=TXkcq$Log<_jm0}*76U@qU6|U=?-k6YRnHqE+XJGZw8I+l9 z7~P~T;(#k-6G+8m;Inb4L&EkpC8D=`w(BxlDs06vdOkf+(VZfWY2xv5%wegF?Zs$f zUg3FeS}DX(djz*N7b>HPL$c4}WVhb$&}aC^w2=Apqs9LC-P##*JY0bzpk!H(Gge5) z@_!%Gg0G!=nm=4P*kZbf8|ht(^o@-uj{c6+YO8={TG>8}ihHf%Ib?0A=?H@;L19%q zRI1uc5{pG@w>OPfr_QHYhQc=ZeV8Mdzo**OU@D@h6bxDuEeMQ-2zV|##ol%A`1+~F zkRfu*b9-pRlPf7*aVWnN#5%!2?p=Vdf5&m>Q&o(VD)Ct+xbPeNvf=i}K@NOxJ38Jz zMyzpxaVR?ys&bz89^oTfMr0ap7#D~|)tWhu{;|6HsC>;l0O>j+Qy`q5h$&PHgRuU@ zd(3P!eNVW<>C0kqk!imww5xYy%KM9CSxT(Ltge$kAV6t`uj(>H z{=gm?6~%c^661J&XXphFjZu%VkxVkNQI>U-#}@_p`nuPVk&*LZV9R3<3F%Agprk8g zjb&widr>W))??uV77IS_w5|;`L?GrhNv=BSLJj1Jwe@&{MipV)5d-sPZPC*6G96!Z zsMR;r97n8r+2dH%^h4%?Tod%7 z#i1ZCq26i zCWPGF1ww|@&rp0Nm(k%Vnw)KXCuFyIm|4eO-?7p;O7}xAK5){IQ@SB0FDhBFu*}!E z+EVLwY?#sPNi;bX>8tG!6DgMeVJh_U;fWql4Ky@AMYxy4)Io!bRJ_K$yC2*AG^C~X z*!bgEtt?;I(VpP3T_YW!M~7hN8ivei$L-ZV-u*UAEw~8#XlU>msPdj z-?5IQ`Rp#JAZl{<&i}mAwl*}Zq{4%>gQXISw)>!E`YWIBor`%#E8pmATHJl|RMo<6 zOTxBW-?{SD$=RjbgW2jrcBX6vAydK)^~>QN_Z~@$HcwBpnMl~0Mvw_~)L@>KXcaP2 zd9~N|*tVUU-Bhx74sZ+^nH|d#Jy;u>!+r6HsTo(W35E3TtNMiksYR>24{=JOExH9R zni-#g3OxP{{A;OiaGpe);~kdOUMWA636nUXdsfOI%Zd3(p)FRIztuyyDbY|371OL- zn1D{rnf9g*2?_sOHC31G)Jy6ja=JNfrWI9%HYli^i>TWFHnl?X;qXwU;@X}G-m=n! z8bhNr8%i!~LOUeojrQE_+{F6f7$$k-4l4&Ab&XnEV>Z-q>&dN?2mRm57S^Hec{sGi zW0is2-Yh)wb0e`8+k_hc&R3~+X{GG$30CO{Xtw$0%d{*O;a@r25;(qq(}9P-{j=NQ z{y*s_=_OoXGAXO5*k{-g-!&0E~b`7*@ej!pwSh+p3fsrbVuY^|cWJn1M zi?Ar^UrZ8SZDKrmDX&)pP-Wb%g+xOA9Y5G%vKX~&RP=VxEIdhyREkwU(@qL5h zfSX!u)>kk?S2h>yg@g`_Vkx}p2<=In16SfB0=u5#G$5dy->55#tK+*e(2#jbP_1+~ zlidp6IPsJ`-{Lns_H?TSfLvpCB}d$v_8AQ44^fN4XjM zTy+R=7*?;&m4me1$G}0f!BVL@rX85s)3QvWva!9Sxr3PHv4|$FM5nd+s;{|#v;G>= z?J$uZ>NRMZCaZ|JH*JZEUcC9i`F-}Fm|HovlkC5HihQb@XBf)dlR-EOJ1;(@r4jl} zZ9c}8g7%905+aq;8-RR?EcJKgVlZOxMN}!N^6*&Pi5v+Q%#VQ73$iVsn<8(xvwEW; zCtdJe>yep6Z6=vYl71CZ@ztFs*YC}CAxJ23&FT5e82(mn7CxoU__sz;`f=lo35-Tt7N!EJ%V$;YG%(turlqx##3DqSQ1>NP zI?SzdUZ81y+iJuXvEXcBWe6nf>hI12>yV$EVOTQW?kxD!<3=$)>7_@$$I>A?tb`X` zmyi`Eu4w=r;^|9E7?@o%j!oOd)?X~O%MN_DW-L%wWG!Y_dXGK-UARR1uEs!B$k_+K zfiEm@A)81%&LQj3AKz$6)p|?sMsj0CR$zL)hJ2>=d-uYg@s>WmLv<$751&- zpjswW+ku&`e+o=Ub+$}WHW5Q%qlPLGS4VP_r{!aYy!EGpuDU`zrQWRnmzz8fvE&Po zEOfC*uM-r1QeEHcDTFLUie-!vLA2n`j0I%9+ir%`LkgxO_?(dAi>dt3#9T|U7rqGL zcpAk((tv3B;!54PyhXAr7Ev(;`x8k*Z05$rQHu;GDF5jkwq?-3eydAKV_%B z`#ei}Nu=Q;Q8;%zr9p>oJFCO}RaI1$QrIA+A!C#SB!DZa1hI`_W8sQR^ZJbCu<>8^ zXTW`4v}1&pq_o8*e1!@hkqNky54vo@akvC~^&x z(ASWYhNwXUHOYt^O1_2$TXr^iX>4FNi9(pl5=TxJVdDsl`G&|Rz5@jGj2A5(G1&5q zkrye7D4bvfP?oheAd{fuJe3*UK^jvFAsMuDIHA!*BT3bO+UikE3Cd*NxbPrEp5Z@| z&5TH16DIp94)!AIWhAWmPztGKw9GoXgEGtdJ9C`p$0NBSLuW3sV~G|oR8_ms?{}8li40dp^25D6rGO?pdR;8Wp-?C6M7jQzEFS{!FqChj7sG3}R7pie zg?6{h6x6QH7b|bConOAJV<*wp-T=1Yyzl6xa?-HBmu1aNofNW(S9S^r@AWw?~ zOZ3;ScXk>Lu}J@nOyk*NIZo|`w#0o_AE&P6-VV%&ZtTv%lMGxD_xj(@b&1D;O?P1O zL{rTEJlPppzouE5z66PYk?z#oAE5)|=$@fUGO?&X|Hndg=O=Ii-yjsIJhP8U^=hc&IC`z)HLzAZ^ zK1D1XbjWJ_a+0#g(0t{CE1~=4iX6mL^~Sau_NH0Vee|Rh)!HOp&iiSsZ;QeNci}U}JPlQ5QOS z;1eptWnz{F96S=x?s2G@AzrDSnHID8l=-X)R9($8c7BQRz6woXyHGTrl@!yl6j6VQvY>n;&G#_uF zAQA=Y+IOYf}k;l^3s2f37lSUahWz(_i}SZNn0@j)7XGCSXGnUv7<7{@)b{br0L40Y*1Q!@UKECyb<*L^Fa0!^ zL-Pd?Wq;P41heN;xTHYJkHbag)sANgaxp_N4NHi2EwSV$eOAI7{qp_Xh)u_cr8_Y} zWud_XaTWs}MnNCAz!b2+g=+P4x#$b}TeWznU*eNR4((0@r9jkEeL*ZPz@fdI8Aar@ zK`48|J8h%fyR+@!B&O30eU}?ZH(RyQDUY5IbG~RrIm7d;4~Vb@tE|-?5Xk@~sJFw> zSF0j?UPZtu^sw#qZvnhZTJX`u_cl@V+{@%Tw6Q~Kye?OY($Ko zGup72v6CPW2DPp{ZTNWGNckV%;ixyFD*XcXm3{-;4E!Ms%eGle+pwrYXJ+3(x^)j18H(zmNG7U<*BTp^AzGQupgR|6 z^b-DZ0fZ0NiRQ1Y(8@D$;&a8kk2rH%EbgmPqz>sjqa*l&DYik zf#p}b|2rrz(+XyNE#jn0AS~L#)@BN?V`7%bfCypS9#XxLiNB?EUV8pqnXq+?<~V?_ zp?$iMYWGVR7LiyA4h7%gX_oBpP#2oBLx$=%5e|Z_wGrt_lw7+j2wr$*!?#&k*!^&P zLjF?^AGiluG_Ly=hX6-*W!!QPbKrAqM`lP6#ci7OdT|z7G3iBPdoUR@!LNm|oa$mN zqovr^9W!^*Gs$PfJ2rNPO`4Ymi*CbM&o@RY=$3cbGA6o`TqC`7Iru0R+Xh| zF1#XxYqU>HK3~M+Q1vW0>u2iC%4rT{gV9}7RaM2)1_%Dt1LB9{osbStS6}tdJH9@7 z*E9^OI5gKp*nP;|BTQpeyIENZ<(NQtG4?Efc%{raqbYArME6tvq8S0e5xcXTW~Mh5 z!1CNv6q&Nxe7NL?J^;%QDq|f8DyP$C-J4*3 zIa_EdDM{l$zn_L`#zWVEG~gOau!P5BbNWMgV_*B01MJ?+r12o29D-RNZr zyc9XVBbr>^dH4Vu=}~ssUkIg>hw6gKQ~8H~tN{-u(<^-}zx}W}``Px0iDGP_-Q2F` zAzmj7`Wa6H;ETsPGgCU%37(Qyr55lNYC|B|Hna1uy2+Z1q!3(bD&m zP0`f%Q?1g!D3}s`fqPc=*}%}&MvRJ(_xHL7Kb+x4oJWf1Tn+|G^0|&jOLQhBgqIcs zN%9>_t26{9T5f!bU%bY=cz@c89b4`ssASvf*Qun~p2@&3J0pr>o4iAZv~?GJRp`82 zgss55{s;bahXH$#?yZ{dvTuQ%p4utV{`Ao^1)KXi9vo|#vhB@pYY@E^Jh5WsoXZP9 zkBn422g4xmBaeREW}*gmqKtk2;TK^62Qz0Ju{32@I9R@Lp0#MVcxlzuxCIPlKsvH_ z2q=ZOm@DqZ{u7koZWt6SPSrSdv7=Mo8eKpiIUk)lzpm||8LzZQr)rz z8pl5P-<$0Ca7n+PueQl>+$VNg(Hq6f#g^yu){8Xm(rxz#D^#G_-)7yWW9~43o0;|x zb?T6xQSe!>@!9JR_No-z0iKo((&V%aV@PdJ?Tinp3+p0_ZLW`tmUSRROo$Awgw}=I+$RT#o0Efdv zZoT}2-N~vi+(Zv9D9E>sWzkuVi9qr+Rw$%|poJkC$dRz7QKZgf2~CIY+3yR=lsXLz zgq)wMTGdt`jSo9FJXKvEB9AhmdzD>us_g3loZRxQ&*yP-)JNSZ4;h3cr~AZodm1yz z#d$7~@F&4XDwfm%T){T@>>`>x`Cyy2UV?s34>K;)xGZ{K%sXas{(Rg)Ij8ddoAnE^ zb830n*BzCJfB!s-i+Aeq<|&)8?O!(F!p~^M=YwNQW=XG6t+=!~Q4gZA7lEkWv;B$a zuencOwRjuVR%*5DnSY2%M%`KrxzVB)?@tnG^qLHBf15}y>a z^WaLk*-E$GoimfnFs+!q;6Y-!!r>}t4jy?4*D6E%`= zJ@c>S9qL+kBDL-EsuPC!*MvR=hlt&}H}mFLFl9}NBLXRJf=b4ARuDh4-gDY&eVk@! zYurj((*jJR4F$BPjE%PB3OT{@cuv8DgsO8~QQpOS46M4VD>^Yjb^joI zGfO&PnsDo3$H83thWsM)VW?9#J5B2KH1Wk|Su0mrRG62f)a6y?OeYWb#&Vmo3v4#YAdTBaSk&Q{Ku|tZcCa-@OMxV`cS8t|RMaZe_ z38xuO4{NyLXEb1+<%2R}lcfZ@#l6Hjv)c9)JP%8;_rF}OaJ=}(!fkfoAeWkg*Q_>e z*Ayk%lhic1NmBgtvu=wnawWt>K=Zd9U}YD{P4R>!9g|w=j}>~kqGVV5gVDixQZFS_ zFMmq=*l*j|wS52x?ftNaz7A%)txclK;&i4AZ;1<@r9?3e_8&4ek`A2;6CFSOmU|Pl zA4yfrDB&~9ypUyhZsmFDBQu$W;g%lpb?fp5Crx^@^xi6nR!ujUhIl26^ABwc3eh_d zylVYfiD9)OEj_opE7#wR0Qt-UrX&;H%i>M)ubFL>JXo2(O}C?DeQ5TuhVaP1H97Hv zmV99Y*MuA*8e7#xDB~Q?&TG$!(^1|rRXpZCPu^n6JMU}&jwWH3W1;qo{VeMrm)LeC z>70u%Jm$AO=2gGOVMY1lo~Em6t-bJz8S@MGbEqdx5H^so{-Sb8Vb5R z{hN2YsHw(*JEay;QBlGA#rxyrp9lNvpC->#AQKxiZu_Btxd{hB8VgOJ!~;N^RGfJ- z;lH>Mg7U=eYG%_9S=%b!51gJ%vfQy|v!s9GHICqgTP;e~Y5any6k(rk(w^qpJr;gM zdFjn-S}6=Zg)%l6W1`67TV_JY+OI~s*5lDYTkc1vsi+TH##Kp|yN1U+T9Ykvd4=r+N?bsHGarh*wBtYUzXBG? zL2fC-mysWNcZwTn4DVD~X*OE7u*K_@ReN%%ev^EKPNdV=Qqi07QlmO?yuwGa+8g|2iez~T1Xx&%DLs;ysS^geNmzHO|^PSjc&aI5fvxjZquEfN=$nwtBjvc zOc*FVlwIZ&7cS-~*}b>ZHbiiTq!UX5eBkbI`q?yPHnQmKH>q|=JXj1Bp74!gQJoZ) zrn9Y152IX5w);GsJVG;v6wje!6sWG*HE@BiY0SRJT`2NCImgRtuV`rhsIM`7PcmOv zLmPkR^@=Pkk_$)10@~+)_DVw=gMX_xL6afzwe80+IJapg*21xPkX*#?a3XtonO=K} zi`%M>XM9BM*N12E7HV@BjGBx3$?FBXt@93%&E2%_nQ9)$ddmo?7`8=QhsTOEkiN-e z4g{=d1w~(KJ%Ce(Q74Va6nbB}A}bmlT9xC@p(1Z>w15_5UU zn|tM)##(2zvWvd^?Dd9V3M#6URaJ$yPLa(X4Ajr+?Lq-B8K1{x?8$!%pZR$0#<~|j zz<(v0hx&MLw~gE-2_s<_ZvYC>ShKV(v3siP)|=J0=gplrrkL(2DS*>N-}u%Z7<|XS z-<$0z7;>zzReFx>?JzZ>I?!CfAhh|^`Ld{#K#y=a92af^ACy6M+KlFuDm5GQ--Ss{ z8A^Z{Q~QUnVbLH|0s1xPi8`o<%Hakf$^8apZ9fOu<$t<>Od|KqVLi@TfpbP_o1e~O zkrrIQ#SUsA#w^R+T$%CFY`0Qj)`M?G7SGCFTu1dTx^2aRa{-ChniORTH#Sm5!+7}G zqQ@s>C4x)1b`7n4>iz)=8I`F=Ijeq)pG3f9y)f1u5YW}As#s<=@-QP`&s>6|#ywLc zRN+3yj`pIP3+v35`B^pbyLVU|5o!BkeD1Yp_}UwXk!#Q#V*J&$G>aMWEZJMvvK`jD z?{3S{`_vk!Su#dn4He-ucPb(g7a=_-ULEZ8i2Du6nz8Ig!} zS{NF+MfaKW;0x^+gB>&e)*}sz{LFmo?(UpRkO5vFt}zw$fBp2o!_(kL7i$M2?Ls*! ziLxWg0H6`|=8@7yb)yj7R1?{evl!Rf-%3~^1pSXIYw^D?8COsiS4`RnBS5MdxTYHh zB;}!FvqK+gz2{8^D15Sv>8yRUN75n7(`4P8+ zmAylTw3z~2I@b@{6&&ei*S(2H`??=7_6h$taC>D5zeJv9Rz$Dha{I^3Ygy*c@Ox|i z^3zSm(Qe_^^Y5Nr0tT2^3VoFJup^oC0)o8fppCLc58~7UJboTArtmdzrg;=3@n#Jr zi=tXSbu>`C)}ylDzL^STe#UOtFdD*!jf z!z0^EOq_W;7SH9gvbS5%C}>&>{!G7?g{8m~W#kx;Wex&@SJ-~I6H@Y(-l7j!;|MVf z7^{Du?7@)H*S8#%JF%MSbv#tVOKi?$4MZuNMhkq23wlHrrZj;s z4+q~ev@y2mpJc8Fqw@qt?` zxoZ!0ciUd)xL=4QZ_=Gb`sphjfB^~`0iZX~$n)Vb!nLTIyJ75$vq_-+w>T3(#9u+J zeE$K(z__#83T4P%@N#Y|US_{hnTNg8UIDXw{h9zWzcs|g@aUh!Cd7+p2}nE4>-PG; z`zc1we-?*#UY&f)SEHk6oQ8k;KK?EE^>={Szy6- zTmmTIg`=tLvBu7x8am>piC)_WvFAZF8;Hl{4sU?GLZO@$K-1hH1L30{%`S)|<^2_q zG2Lgp6IeKqF^SwjYxD)I{#wweyiPnflC|XFTj6%uw-#)m)%vrf@SH-|FgTto$gY>zhgGO+FU==6!qb0Bv-t$_|Px3t!LXWA@fdz z3qS@R^Sen#zHzt*=Yk#JCNiK+DFEGLKl{KvLKT`#K&|8wC1*-Xv66kciYWymaV8lB zBDYLZnO|}%l8=X*j)~|Oc!mqUx3%eP%K@Fy91y9ne{VPI)S6Z*lv# z(0(;C^6f1!RrYQ9z~iNFnB~LJ(~kF%WCQdk%^-a^tf}{Af@8Ba76b$}eJ_<}bggdS z+Y;F)h^N4HknnZq8_-aOAJimX7dN#hgF&EW=0_rhsxj!oQDyM$m-gKp{HfCoP-_H1 zk4k$An`~-Fsv_N$_E^HSZ{|J)n%zRi;25Hu3neR+BT8oY3x^qx5f+XXbeY}F!A(8E z!%n7Qt}shcU=F=6ohH{l%wHFfM7x^0y3N4t3$%5T&??V|E5aUqz|HK$)BrywkK`^{ z$FPy?stBBCF__P6nJ3F4jE~~JF1`+`B&eKTTt}H!0)D!9;J<$QU;GR>T3}jwjkNOd z;Pd1ozvlp#+Kr|?5X4DDC-`~GF2{kR5UNt)uoPe0hYGkkEuzL`G`z0*LWCT6iMa?e zq%Kh#s4+&?BP}E@f_l$WEDCFG$Ja0tkEGyi9Axn}S31bji3wn2t4(d-Xo_yq@ru8lHOIZXe1!q4oQ~Cv^ESJEK)mNQmO8bS^je<`0>+8x5RaylEXZ-eSXuGF0`ke2Qz&~>HzE;F;@LWk z&h6^M`IZmKXdL&(FDHiA6;dkII(K(mF9k(xHyL?Qzo8zilGjjs(SafFFN4oI+k;f> zCI}{{A9Wa>%Aq0Fh-V4E3Hw;BKnCXg>(;qC%SXWg^kwZ=JZ|nt1tQI6>m$!ISoQfE zRLNQ4K0JR1EtiWJ940BXXDP4KpOr;6We43?Fhrh})^FCr)ki0M1zn=S^BNYz6)vz+ z)9=lFg2*YrUN4^w`U1_UYN4rP)bQpZ(%!RXEtLJ#V-qLlDN!7U1KVIYyp%DZSS7Iw zvza(B21wZxE5oB~L-hAJvl!+J<2VoVuV#F&5&k}T%0w;I1G%m!y__C}tHxd=2_=nn zNrp<5y|{}&$9XF)j}K$N^30oU9}pEA>yCnEExFR+8aNX^oirlZ8(TL9&F{1ed8TvO zpLn1n{P`;^j;sKW%&pPoEZ5IMZtYL-wVV^e_lwt$?_95uqJg~6FM*nnaJmt#x)m5S zcZ)1K4x)elY)wamUrD@PejbsK1l1H%`FWq&yxQn=s5=!P)eV)ea3KUTL}3;{jeW4kuSxX`r)5ZN9Kmox*#b zzPW9Tg~QUX;4HKbyk>KKfj-4Grbq;1EF?1D&Ni;vB2s(CquWcP2S!bN=<0Kk?|;fe z%e`4i<-5Me1kWa;85;XcBP(bzg=iy$(925(Z^>2`@~pA^&~wNzBkIHT!>`-{7oj1Wi33lIBz z@=5mmN2xJuR`vtSz8x2uUoe;({=W0}(+je%k;T4uI!hEf!{$yCn(T$BmLFz~fP!p5 zyfD0^9l!l7^8!ATDau!}Dy`05*5NoNHDw9btpH>;sRpl`ofd9shxazkj9~T~zT@a9 zI>+UNQ3*uX+$B>KZV!8clW%DcrST~Ma5-{QTm`C>rBCy z3;?6~N+2=YG;Sl%@#=ks zL{B69jA2XAFv?peV@_pbwO5{ZVP8blZQ&O*I|99ifo8<*(%BB6#THI$b97uCy1Ap| zrJxZta`>OtKo^S6uPiBZt}QvbYb}U6S^DFMu+A56YkVY@`~`A|u956W-!gP20ZUZ| z*Qf`&bE(Dvoic3W3t?T=RkdNykG3=z4l~YulRu-nHu&_-DA$@jDrh{k=Yb2-+dbP7 z&$W2A9cWDh+f|=|*}ep_^!MB2u#5vA(AY|ZmC1E%jUm4+BWFUYXBgZVj3xn3$%Xo# z0WRhdo<=neGl>-Wsi2*7zo*hs-d8iU?@vvdmx*%p6QMk2f~Kl0gVomX7-g9xS~_vn z4&PC+jFE%S<-LdZZ-7e+U+sPmh?By1JoS0pfg$8dL}92cn`=1P4<5XvCIY#%MHs8j z%2g;?qjMV}d`RT5BL<^&Um9AS!VU}Z!OG5+>kEJZW>*%Plkn@wcK{WPD(&${AMUU; z*(=rvfw6aV_>95)?jhLGvJE1BXop0t#gm@@gdZCr8c$(O={KY6z^Qxqa<*N^9@gP< zRz0KmRK$3nbVpjV=+n-cZNI4Y*(cQ8udyC?!99hcbq|lwLB22^5U&UiDqrDw9gorm zjgG=)BK4V_o_x*b4wVR~we&!1=)+^F@;e)oTy0d2IZ`a1J7#5%6~SPTs-*6!;~nel zz{$!D=LTlfFlITw9GsB#zJ9*Oj83md93v!tBm@SM1i>waWZpg5js-ut9`t4>~-QBaQ=`>(V3W4 zO7AY>$+$zY=gsV=v^CE-7m+=5L>DggqAA9$tDldfGZd05S%8?qyCV!8PDanzXv|Ra z-Rh_N1?fPC@7OThCkC5=VF6r{I6_JQ%Sf9sRY{=kl;Tn|N^5&Jy>f-Y9>k_lH;1SYgWI+4h7JlezE9yrn3_PQRi+ zgh0Hs&LmNx9)*1E)3x&J_UsYIWF6vG=@QEHdo$LD!Nc-p_K+5wwO+Xs<)Y#_S~IFQ zpK~svsSYHiBG8N=W~ygu)qbR93Xxl4N&Gs(H!gt$%Ger`T!J&y5P$*;eUivtpvpC`8TGRpd6rVo;GTr-#bG&`HAuDfU{{S$*INs;?e4|x!u1(k zqqL94w6Km5`%ex133C3Mk1ZTH>zmQe+gGgz=6#~%1Mnne_p)#}=2DEss&Ju69ng!3 zV_7J*PZBbN13U-<5Z5Ch>D;>qnYwO`S9#S=W$BQ^4HiT}qtTfMu$<)yYH~ic=$)}M zd*zZhbAYPwTCKi+)#liqA!Yvo06|^+3S56opw84jnVCjw<=XunT}Lxo+-bk%nWTER;@F=4LC;Oj^cXS6pQKGDq~^c{rM zRL_unQNf6GVm`_KIq18$tqRk?f)TQ+#kgKE1f5Kx6?-CuD~tZ_bMZMOJMquwa`RjL zgcu`2Zg|Uv@Jx=MSOrHjs@O>9aGh*$hhe~qG{0Ea#}C95{Lh}+p|{wcoP=?BeBi{D zKKSnQ&LHfOyi-^L(z%4~v|MEqyhhF?ePD^~#AaZ}k!ocEhG#s9n6G4z;ecTug63u{ zRZdEADTEeEJ=|>}J-%jab4z;EAw>t-Jw5l-e%TlqK3$d-&Cn z;g7&%a`RCfT;zGI2jK&6lrKYS#*QnIY{KTgfca5AV)D4a9c!uTda{i8)aad6?ly;C zT&|FJ-q=Rpv34Z)P7MAsoenJDdXjKUsyvEv`b!bx!$eK)m0D3hPy!9)?@Z% z*_Bqx9N`KLH$w$walgKbuA8PrumN6z*zt5NX~@XtuFKR7k@ffy21_ZECbR z1B=3ZwDwYIBi}J9y%=G;hbX zU3sp%XV)%BJEBdD3w$?F^aci-_4)XP_OEn)JL*91{b*MO&W)2aJRq#&76;T}# zEuJexy22xg#>@prqp0oIrKA=ce1oxm^@{vZLH7BzcJZ3kAz|W5>g}(gIAkYBA$};8 zH;zq{pbvU3Ac!=>rI!?uBnZofh$Igyvyx|Q`G3T>L&|xGg7^ml(GL9px%Qs(^H)H zM1ePV?_m`(fBODcbawZUg^NzxBo+++4Pa$R7)b*kB%u9pZc73?LzZux{MDLQs-3j4qewsfRqRJ&j z=g}LZyv4W)19sjYtAb?M;tulvoH=jgE@H(_V#$)=DzQ-stq`GH8n$D59=}^XH#TQf z4CYpe!erkxJOcKwmWrx~!*dD94b@A)M7p`b>3G-M-BUvk*DZgERHSC3>PFai-cZ1LP! z>?WRb5i*U0HBg>L*(Z>;L_B`(Admt=N@z4NQnliCjJ3Z;1%TC0jsX7MPHe^D5DCd%sTKc`2@O=X&nimKGwwP(4k5v zf#;VzbHlTmmrS_j7)CgjAUldtnDH#ZQ%PEf$DuZt79?ggaJscZpPwQVTI) zHa3x*KY&5!`X%os>S+!HT9$6m@(5Y|6gsPU=AVZYDl=7mykps+Z${f_f9iA1A+_6U z6#seQI9BKtuH>4&IZ+sA3p;=oddY138!GX?c24m3{?c_JqA|~A;!tCVy-kDYPWS-gNk;hmmHLmPzdeKt^6x$v#5x}qZnZj($=aNa|3Cn!a{ci+xD}SRbY`iiFgXgLT8<93e%a@?+ zcmW@+6_lIG`vc>o7d`0vWZ<%dh+o{Byne635P|rz&m#nX#1t)FWfd_2@8NJm}T!X=u+4dU=49G<&9|Kee97Jr*)_drHF)wW8A;WAGv0X-0en84-kw(o1!5+Q!b~@@E2&by zoyJWIXnb5l7b}K~^_*?f!HWBkB>V4LV@L8Jtw%0GAhHj?4*q1|%g%RY#^_ZEOFSO`Zy0!=r2u>x+uEWU0Sq7pyjM(vCwfTnK?vK$0F2I>+<-b^U57XgkDJ##b#GPMZipI2FxXkdWR;)7Qo@%U@;R2o#%ct%5;Wf1|j70M`?JH z#xVYAFi#VcuLIrVk;Q8I+V#~XYJ49YGV+18wNP>m3C9bB1uhbpkoF`VXv&uT_!mF} zut=Wik_59BsaW8OWk2KM>Gi+3OZHz^@b}+>_cJ<+C>%#8mCMmtqz2O(8dB1Sk#=92 z|9`gm?fuYpBk~Kusxp8dbWvqcE%t&MA+V8hqG45jXmKAm`BLw&NMRal!n;P`AS(&w z!P{4t_-)sh11;e2Zln=Yb<5#U+7D& z%xqMELM?U>m+pIxyv#ZD%k)J4JOBYB-44KM-N@1k%73XB#12OG;7%eWz&5L}A--&e zuf1(De(jl{0q&?uV*G8;8<0g(O+2XA@N%8&r4L$W=L1OqRs=Px-)tA1nOUUo$C>gU z1WB@5Y%XtO1uAyEt=cHj8;gz&+>`}}`oCyHau0FNyrQFnB%>|3N*kJ~!mu$2M<#%f zp6Zti`)?7*iX}_I1G3XWgRdFUohI0yaRL}!LPf&4y*bp|A%-bLJqtN9{<|0tGgeUn zB|X8vw5J!AUlwAj-@s;~(xA3aJy3|eHgVX3B(NtL)X(^5yPedPd4sZC55h~Dha}9y z6y<**qiKY;qUq4XkGV|wQS!c!2g0W7^>zn@0qOl;q5(%Rr1xw$b4O%AS#dJb97oLf zD)QvdLq6e=1pustt(c5ZCUG!nulW(@;@2Ab$ug9;Hk6+*bFje@)Dq7^q(2H{;%HVK zE?09bN>y)s+Uw;^?BO7*p9`o17%~(3Zl?(Gb49OQh#VRT_RvF4Qq%_P3|;^Yfgc%J zobz0!Pe(`6%eB9~0GV(ZY4(dSWq`<7xzK^@VH>D0QTPidhs%K4x4_ZWZE;5%L;hjJ z{}^{+MJb-6&8a4aB*kv&A4p$0F10%$X-Ggm?RkUwCm#-hrA4}QG(9-TlkM$D zWwc%YhgUKa?wNAWv(Tk~L9Kt9c|{qJ0VR$mP#yJQ1RTDs^$9hEFe?aEFS&=$`cMN- z0ByQgSM77F6Z8v%oMg2_jE9M61IRCi1VZ18;cYJBK(~g9CZAd|isz7VIIp0tIY+%Q z%J69PAW&P+zCd#m8qqw{@*HYcH)^=Is?48G7NGy}nE&Zb0FLkuyj2PSm=`39qD7Rb z3M?TZj5^ToTx0T3`rUCLi{-?3+S@QsOZSS+^k%El`<bQfgLFK5!uu)RPEKT_6t zL(z{(Dx}vd{(HkR=}RV|-f4ojJbs4RqWDSL9qHlTf1!2$RQqpPYo}P%WDHC-s!Gk`4Z(N%8m;J~@*V{IRnUL(CUGFfjkV- zx~4ZWlFKPGqjZjad=lMmpn(RvdDYkV(_`pQ_@BS`Ix$rFB37+jCTV}k5>xO>X5UAs z__>08J@O|{wy%$moU@3}kQ-`z$XE9TcMkkck_Hl?!+(GV#G}DnuUkRA zdFa%u!aLy0nYLhN2X^wQuSXt9+_MG<73!X%DIVDc7e2qcw%+~PJ<+(v4G}%sRd3=K zp`imO??pe)UAj{qQHfl+|Je|j8)n#TiH&gd;#(&KW){yzbcwmBG$G9Z{J@J}suX7B zelv16()R~Q{{l!9BKL(e=3PEB)ik5uOF`W7Q~mDMt@kv7L(Fz_E~(c%1XbkWybt)I zwtt}-$#>8=-C;*y!0llRep?u2QIs|JaD$cPu$o>wKG`=9r2mP`r1ls3zb|9bV|MCZ zGc%VE(r;3MC7)!%9N`bEYrYVE%J%^V_2Wbgb;BW@2loN=*Fx;hrnc^^;jgI39*E5r zi$wGE#@eZz@=G=yS%NzUxbB87>&RAHZK#bXd{ReVk#xx_!N4#WPL^2RCzf~yMo$XAhrH(7Eu@6Azx?@T@dxq$+6bk>AlOr zLgdO%ESYmBuz4ZInIJ4A9w?;2N_g~|?U|LnU10YWN@7SwlUDO2x&vR>Kz=i|%`?f% z{HV9Io0Z)sg;fU~qQW4f@xdTg;fl-P5Oiue6=(kX`0%je(bT~P8)Lrs&t+!O$y}SYd=TAJDaB=!OH~_4q*|bk%YlnI`QT`f`{ISb z^${oFbr2sVP#!NKyVHJ6GgPx!`mPK6ml{bvw&Zot_#Qw*+SiTxAs4hRF7gBd8;9>C zveTEzdw*V+ZgUG`5wFGTbsbSusURIr&4K4xFw&9*XS?$Onn;6l!*~z=U&VcASd?kD zEo!UX(n@P9*n-&IY7-=g79c2K1O>^8L{X7gASwll2p9$fh=?S~O3p}BvIzqSLKVRR z35q}$6cUOY&w5Me%-rd@&%N`UABSH)1}whs4STP>f+E4AzE;LN(Qev}rmPmTc{KF} zGb?`Bg)D!kvQ7r#zC{ANzD7)?>thj%B`TgS>{LgO zH2Jdj%-C|7%*~m*So+^|TY*vB#Y0)2ENk2oob%wTB%3cXP8Mu}zxxkIGi!|`cF&%3EQ(5hmL#(4oPp2PrU$I@- zWnpiNU1coaKTJC6IrC@3?`;n4-;LWSM#_8lD74H@6;yWje>NWZaNw^$@?)*v7B3oJ z{gYtE-&Do&T17VrU7!>UWZ_P*eYNF4Rf>Yd8%q)B(QJnc1uUx zbuI6#fR|F)CdjqVOupjwf6N+}YU$r1*PayjNV*@YzN@Ik?TfHq$)KU>_MxWpvsb$< zn%K6r$(o7pX`PxKNv&$-QoVijlPzaY{k0U~PBVF`441Sng$jK%tPK~DT{kYu&T{Tm zhIjK#v~Xtu=TRJKy$^79Lv-)#AY%if>rVZz9C4xjyW&v%pNzJCdr!BAO>rYmcLs~n zjq_HtF90I*wFx>KgA0q80E`32$teRn<0nbumEFviFoP?`yiyXr+T)nW)i>bCrO0yE z^6Q(cj+n|&OXP9rk|-tbNi))(G|rryA>M1S4ARpjtd^b_ZcX)Ccc(2#ITiEWWm}y2 zoo*FOyb870l|1r!4K>Zbp8IXNCQ`qu>Fu;7P9{f_gUmEFieAdW;)-2@)?uHKXH>j_ zud&>G9vyY=Tf3uQ{v)OSR^O2{nOVH#_-+XTkKyb8fhEPI=!mXVkNm+qmHg0TVgf+* zjZYz}csJsHoSd_J{lB3ll6X%t=_P5^2RKjZPXAZ)5!y^J8`m-Eyrl<+j0a>FApZTe z)KKM9LC0eMlB6HM!qxx16NdsdA`>{Xv{`MB0uQQiMdv-;n}O*=|HR)-m9O}7(!uD2 zyzxM79A>RSC1{O|-yGrjR~llSSkP;ibZ)_MkyDZX_tGQfh}R3Tf7rQjPaQm{e_ucU z<4=>t(PQXY<3J)`HmkX>2*3Jg-57VZ|A)UPS_nEKr5e3Ffg@rt1|Vjp~!Z-JO;<=-}fX{I&oA#zQPJwSihHyM36 zgOtcbRQw)@=LuqanVwp1o1*9jgVs-QU#lGDN zi-Ifqh*YYwk0U5w#0adMIB#NRe>|TLaQJUzlt-e{`2o>qo->-Vv8+pU&z(l9k|k5- z9MA$kLcT%sYeBePgllmWk#-kCa{TC~D>)HLYqhD>Mp1Bzv^ljV)t;o{ZG(l81#zf7 zY+yuy_<(dLNQ2J6eQ7V8in6ng( zo2>AtTGC7zh4hgs;*WJ6A0t*_GI#&rbutqGu_=13ARbbPsK!Fk7`*@7hrn18H{E&$ zsMvVC;Zf;f?ks-{c4LQDt9TR-Nk1SIwNxJraLP`@%us6h0{9sG#$Dt`$v;T!*daP} zB$nRNxpws_HNYvJVN%vR9YxTtaPz~5gWRnrOMyV1>;?HmKdjx;SV1o|@$A$ms%}_H zH31WLW4e(|3AfKakY7epb(n1c2+fYeH_lgYQB{HRJIq>}hSI3fC1Be0m}UCsHVH<985!y3&r)VU z%95bKCOn_`RL`{ZBh~aH$rJe~*fy^E0R0^^7iaxSsI^9SJJIhoI~0CN>n-f&#sTQb zcFjD59VS1a!P+xvzL5=3VP!U1JjJAjx9WgJa33th@@rfQK*SdaOMNNc4BBPR=(#eW zam2R%{zH<*M|==95nI~Fi<(Q6`(e;rRtB@6A2?1pupX&o8ubh;-XKlX)1ACzVE#a1 zBKOANb>c4>RYpb>US~HvQPZs#s(Hk;3TN{i>VTIEr7fR364VF$$nqmr ztT^EtdI9S#8~vOl)FZJll3olR@bSyMoiq+bQ(L~&D1S2T2VsdfUJobj6^Zk<5bPto z!0LCs%p$}@7hU~EO?hnbda}jg;;GME*e+m(v8PdFMs|fb)A7jCDbcEZGVhoS#M|dErJ~sM;*bK^1@ z#$@Sxl*MZ3LX=UL@C+3~Di$~eOVej?5jP|!#3tn@jrYc$ll&$7P!KsCFLMHkOgcPD zc+_5NO^!YCfY%9-)ZJ%$_>=MroJ=fn@p8V>Q8U+|WRhXY%sKv>8XMcY=)IJ2J&`$!dvhDRBTyVnwM z+rAaiNV4@=f&2nL*`+8>;j@Kmngf*E;nPOQDa&3=W8{rY)AP>R zMcy?wZm8Gazfruap={!^M?*<;xJYLcJhq&vuyTlqioVYMQP}!{r@@@zj)T z&5kb@$vado0}NX$^!T=P^we=e0eFi+LQ#TNV*^K{v6Z!^7IXYame`?M8f`VLUKk~%$4rpXW{k80a9`W0b;&P=`0(Y7QO3`i*(Jy~K251S zu!gO;^u;9co%P^^w~?Vxo=zCCg_YmJxc{7*mZf^N*Gac|*JKvjSDo0ODePb0) zuQe|oGNDf$&&Uf>)3;4=NH18c9iC#((6J|46T%xO<6t;q_;s~U{2u6V;n*h(`kf-q zy!?eoi}n@#+#YLVcV(~hLjJDkSunj)yjEz;yF%R4GYWfB*gtsD1_+rkBuN?d_8qh0 zL<3`TlqR@i`oq*;>0?~&qO(KO$a-`vp=`N3E?sWcED@0q<-nUL^)O6g=d#;?bYUsg+gDWaO1&nBIX@ znPkrK8(sJBHSR%{jY@XMd!hq=kEwhP^)c;(dZP=39eWNHCKNXuEV&c6xJR#M^UUZG z#m2$P44F(nGwLS41p1+7lpGW_Sqw33 zw$C$SPDdzA677w5CexA`M5>a?AUvv{>>sYBlq95WopNLBmp?_<%q9>YQKVTvWs)d1 z;>1jNusIn<#0pfWWJ6C5jm|Jnj;Go6$<96hnY)u-D2J^h60T|Jxl(7guhmEF)(Suc z$&ps;;tNDyDolZS8_;sH2OUrv(eMN{+I$*z$_1>hLzno%vS{HoiEFPs(EQ0Lasso$rfp1kj*u=)vh2)n~jz0dAT{8j>$#0>P{qC70(mT zQASXT|0#(LtlvT?s)mXpO@P&Mu1DzU%pbBs1w-g}8;$>1n1|ZSBygH&?2L{KCWqW9 z-t;Ita!y{PgoO4v77wb9MzRcpU*2;CNACnk4L8mn$*?q8ue9S_&924efP=%_y9F|% zD~$?5Bx|O3pz1MNe}$s_fiReecV==$sbP=l{I>CYgZMXQJ0hi?)bThzgCt_ww2F` zb&}>R)<<#@OKv5FOImXWO*j{4O_SQ2ATUKwcBo3-40C$%QrTKtJAn8WFlTd#>Yz19 zpV_cc$t+5^?jWCNT!s!wPf3#xCguzkW(6UA*u8={h8-I;8}Y9~!bK)HI{66D`nI?9 z-a7PwPgsHQIllNf9`y^~KTr@G_4t6}KJ9^Op+KPUtX7#hQTUL+3thzH=rM6w%aa62 z_T=Uklgghh26QQy+8H>ENjN8mMiuUjnT{(B4I~X+o+9*wIGKwjiD6REd^QcnAFh+B+hB-_Jp}u^cyHAqGy!O544M(>pYb26Y#P_ zgCtt33I{qd6EZsVcyG|0C=QjZF54*lj@)8jS9i9ti7O@Ogaegz`6^8^NI3AIl2^Rb zrLKhJ(GD6L)Pa%?SXNef6op;QTa#?BUpU+xD%^FTBNwOf1@t;rwn&%@v+dlAODUO^ z-$MrMksNA5;_PKH71s50v!&h)eIKHUMs;kJzIu&uv`^DAG=Bj~)h5XKYGNk)(9{aN&s{lJg}*ZWZppbSb^7X8tE1(^mqS z)4{_Y_oxn?wTHa3(&HE3;Z_$)BbP|>B}spq8WhO16>qWfHO&Wg{ORuQOaxwDl#`ad zn-prG^&L{Gh%c7g*Cd*HA^3594hR?62VMYw|}CSWY~BAKkw=?K zT(Yb$yCSP`zZ|)vC5Y@Uk3l?>cLN0AMZF<*Fm#LMWlLE8ni^u&tQZey*d-B>G=s%_cE2UP9>2 z`KG8_NLOZftQ74G>%1a}@@Y~;&Y)vfSFUWVFk-t-(v!^TK9Y!OuDhB0D4Xd;G+SQ$ z0Q}ITo1X_2!mgXCW-Apudhs}VfVW49nf29D5wp2?XC-cf8k1E?f>WhnI5uY_N-_!T z8i^2iW!Tl1QqB0hN>h@5q9JrCsf$=Obx+4>W(X~AsZKJM^LzAjp+PMW43*Xte(Pr7 z09*7j;|!Gee@~vH-?RKn@*Jzl77gl~;0a&_c{L@kx{^?3VEnC{AqQc*>s-vr;scxz2+p42iZvOB$k~`!Cq4jk#OTRGDY8Ud z9>AyS@aoOSc5<6>ena(M1w5L^N)3rIrnQ5ywRedY8B z={g}1hRnm?!Yt4rR?2BoQFXdR-K&1YaI6CaU=Xfs=JakIPZAA41FK0sh+36*(7kMG z#7yOI%uD4z-g^4xV2xX|=p!epa*4>eCZhj({NfY6ybbZ3swgElD-Of0X^ae|*!?Ka zRd`>^iTE1j6AEje9Elx}IDISK)mKO%yT+7az$982<}j^IEYhj*+(8E|z+%*d%+kpo zFET{i)CM2mDbZy#!8J?)QW9^7dFJ@v|h|eudUKI4u8(_$PbR zL+FW}rN|Xu3CPj(Z*xYR#|i~WN6fT$$sLZ2H{i3K)0%Q^YGW`WZRfOA@+C~%6N24{ zakz_xMC9xE29M;UdYfmV*)?M8XZPfc+i9&37)$BBcHNya$K8H}`8;}vdf~p9**F;3 zg}|$>v$tJSAcu{k1GyWBK2ZULFf@HrNh1G6bGCnCflXSr z!)pakr(Wk2WN^;3+_A88Vyw2D0wLO(&xi=Pn9oSaJwtr22`TtCGqvtiFKaZSQM;20U|GfLGJn^TwJrw6p+DZ2oy)=QNSy zKSs1mkEY6r7dME`BYb*0o`ZN5(e)5m(iD+WgXxa$)XTjo1WnftFik2ifv&FcHT@U;BC2qJFyX3IQyk8nejWr z6PyWRv=7054j5R(g(O6&{rta)bn9Ch|CdO&36sIKw3=On8Dz=l`31_WeREno-r53@ z+*de)VRu0G85m=;vmQgQm~*C2?_odMw#sYUfQ59g2x>v0AL!gm+5;&{KO8n+&p%77 zCg7IBCD9WdF#rA!CwwG7frOW=No@2<=v0tz$D<1I)E7U9MEn%(F=-pw2ENQi5jRi` zdh56D^!iG)V^6Ll?lF6LuSEi(qGea-!a0Gk8821tRtS%POj*8PN zv8UFgxeZK>^=DytVqeahPv73o_iB%qgAcs(*Q(xZXLbKX2? zaWR?xHTzVF{nXoo`uh43W`((*!$_E`7mY{+w`HcJh&MGg>Ah}rg!h#UG@s{DQBmaO z=cqFh5^t-Sl_y&+P7FuqctFE`w@vVpzZ#)|^6+fFfzJ`o8QTKdgLi9d>s{(7$`guK z=$82Q;>pQL8ylPP^~2%Y&z&NI!g|+7AK%i>o$H=?3tPv8K^Wa_ER*M1@AuVH>i(s) zO;XLj0}2n#LQ8gjV#t!+QuOF^nEUv^Z@ze?qT4bvGNk&)UcqyueP-6py!7ImDK_p) z#yEiJ6>V!S`S1Vsxh_d;MzLnn(@J%4@C^{rPFPk9BiUE@A2m{%;q*t}W;kZvkUnPq zMPP{lo~`Ln4$rL}^TDUU4w@&lgfwR3qV_ovOkwf$x&y9hXOPC18*kN+)#wQ6b=KL=ipTC# zE<)Q~(*A6cG<^6gJP!1a8J-&JZ`1AoDXF4Vj%QBg+xv2y+&*0DF|!7=Yi}&;x#Ez@ z&QF+2$bdEWzRc5e2a?v4CVipq;g^O}Z%YeHsO)UvnGV$pZHVU%bT;F;cJ1qPZI?&R z-i_+&YQxftuH>caMo)0MU*Yq0aJt_?FmpSm_Ys&}l%qwMG|GBTkDn<%KlnQ)POI=| zlFfxYKd$}up`^1n+hf(+Zm&W?4c^W>REI#U#GDR1X%h=5u4N0+#HM7zeZ3{8Nx!>~X(fp&OUzB;oL+}`{8OfN$owTM57hRk@_9 400: - phase = 3 - phase_step = 0 - print("抓取成功，进入搬运阶段") - - phase_step += 1 - - # ---------------- 阶段3：搬运到目标位置 ---------------- - elif phase == 3: - # 抬升并移动到目标位置 - lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.1]) - if phase_step < 400: - # 先抬升 - target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) - else: - # 移动到目标位置 - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) - - # PID控制 - joint_error = target_joint_pos - data.qpos[:3] - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - data.ctrl[:3] = torque - - # 检查是否到达目标位置 - current_ee_pos = data.site_xpos[ee_site_id] - if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD and phase_step > 800: - phase = 4 - phase_step = 0 - print("到达目标位置，进入放置阶段") - - phase_step += 1 - - # ---------------- 阶段4：放置物体 ---------------- - elif phase == 4: - # 保持位置，打开夹爪 - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) - joint_error = target_joint_pos - data.qpos[:3] - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - data.ctrl[:3] = torque - - # 打开夹爪 - data.ctrl[3] = 0.0 # 左夹爪打开 - data.ctrl[4] = 0.0 # 右夹爪打开 - - phase_step += 1 - if phase_step > 400: - grasp_success = True - break - - # 2. 运行仿真步（MuJoCo 2.3+ 标准写法） - mujoco.mj_step(model, data) - - # 3. 记录数据（修复所有废弃属性） - ee_pos_history.append(data.site_xpos[ee_site_id].copy()) - force_history.append(np.linalg.norm(data.sensordata[:3])) - # 核心修复：data.body_xpos → data.xpos - object_pos_history.append(data.xpos[object_body_id].copy()) - - # 4. 渲染可视化（兼容0.1.4版本） - viewer.render() - - # 控制仿真速度 - time.sleep(0.001) - - except KeyboardInterrupt: - # 捕获窗口关闭/键盘中断，正常退出 - print("\n⚠️ 仿真被手动终止") - finally: - # 确保可视化器正常关闭 - viewer.close() - - # ===================== 结果分析 ===================== - # 转换记录数据为numpy数组 - ee_pos_history = np.array(ee_pos_history) - force_history = np.array(force_history) - object_pos_history = np.array(object_pos_history) - - # 绘制结果图 - fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8)) - - # 1. 末端执行器轨迹 - ax1.plot(ee_pos_history[:, 0], ee_pos_history[:, 1], label='末端轨迹', color='blue', linewidth=1.5) - ax1.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='抓取点', s=50) - ax1.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='放置点', s=50) - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_title('末端执行器XY平面轨迹', fontsize=10) - ax1.legend(fontsize=8) - ax1.grid(True, alpha=0.3) - - # 2. 末端Z轴位置 - ax2.plot(ee_pos_history[:, 2], color='green', linewidth=1.5) - ax2.set_xlabel('仿真步数') - ax2.set_ylabel('Z位置 (m)') - ax2.set_title('末端执行器Z轴位置变化', fontsize=10) - ax2.grid(True, alpha=0.3) - - # 3. 接触力变化 - ax3.plot(force_history, color='orange', linewidth=1.5) - ax3.axhline(y=FORCE_THRESHOLD, color='red', linestyle='--', label='力阈值', linewidth=1) - ax3.set_xlabel('仿真步数') - ax3.set_ylabel('接触力 (N)') - ax3.set_title('末端接触力变化', fontsize=10) - ax3.legend(fontsize=8) - ax3.grid(True, alpha=0.3) - - # 4. 物体位置变化 - ax4.plot(object_pos_history[:, 0], object_pos_history[:, 1], label='物体轨迹', color='red', linewidth=1.5) - ax4.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='初始位置', s=50) - ax4.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='目标位置', s=50) - ax4.set_xlabel('X (m)') - ax4.set_ylabel('Y (m)') - ax4.set_title('物体XY平面轨迹', fontsize=10) - ax4.legend(fontsize=8) - ax4.grid(True, alpha=0.3) - - plt.tight_layout() - plt.savefig('grasp_simulation_result.png', dpi=150, bbox_inches='tight') - plt.show() - - # 输出抓取结果 - if grasp_success: - print("\n===================== 仿真结果 =====================") - print("✅ 抓取任务成功完成！") - print( - f"📌 物体最终位置: X={object_pos_history[-1, 0]:.3f} Y={object_pos_history[-1, 1]:.3f} Z={object_pos_history[-1, 2]:.3f}") - print(f"🎯 目标放置位置: X={GOAL_POS[0]:.3f} Y={GOAL_POS[1]:.3f} Z={GOAL_POS[2]:.3f}") - print(f"📏 位置误差: {np.linalg.norm(object_pos_history[-1] - GOAL_POS):.3f} m") - else: - print("\n❌ 抓取任务未完成，请检查参数或仿真步数！") - - -# ===================== 运行仿真 ===================== -if __name__ == "__main__": - print("🚀 机械臂抓取仿真启动...") - grasp_simulation() - print("\n🔚 仿真程序结束") \ No newline at end of file + + + + \ No newline at end of file From 4e7386ad99c31b4a6874f16e407bf5b5167cf6d6 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 22:29:36 +0800 Subject: [PATCH 06/26] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E5=86=B2=E7=AA=81?= =?UTF-8?q?=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot _arm_grasping_task/README.md | 18 -- src/Robot _arm_grasping_task/main.py | 370 +++++++++++++++++++------ 2 files changed, 292 insertions(+), 96 deletions(-) diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot _arm_grasping_task/README.md index eb7e5576c8..d6d73dcfd3 100644 --- a/src/Robot _arm_grasping_task/README.md +++ b/src/Robot _arm_grasping_task/README.md @@ -13,24 +13,6 @@ Robot Arm Grasping Simulation (MuJoCo) - 📊 **力反馈控制**：基于末端力传感器数据，自适应调整夹爪夹持力度 - 🎯 **轨迹可视化**：生成末端执行器/物体的运动轨迹、接触力变化等分析图表 -## 核心功能 -| 功能模块 | 实现方案 | -|------------------|--------------------------------------------------------------------------| -| 逆运动学求解 | 牛顿-拉夫逊法 + 雅可比矩阵（适配3自由度机械臂） | -| 控制算法 | PID 控制（经典工业控制算法，稳定可靠） | -| 抓取判定 | 位置误差阈值 + 接触力阈值（双重判定，避免误判） | -| 数据可视化 | Matplotlib 绘制轨迹/力变化图表，支持高清导出 | -| 鲁棒性优化 | 异常捕获 + 资源自动释放（避免窗口关闭导致程序崩溃） | - -## 技术框架 -| 模块 | 技术选型 | 版本说明 | -|--------------|-----------------------------------|---------------------------| -| 物理引擎 | MuJoCo | v2.3.6（稳定版，免编译） | -| 可视化库 | mujoco-python-viewer | v0.1.4（兼容低版本） | -| 数值计算 | NumPy / SciPy | 1.26.4 / 1.11.4 | -| 绘图库 | Matplotlib | 3.8.2 | -| 开发环境 | Python | 3.8+（Windows 兼容） | - ## 环境配置（Windows 一键部署） ### 1. 激活虚拟环境 ```bash diff --git a/src/Robot _arm_grasping_task/main.py b/src/Robot _arm_grasping_task/main.py index 1c12af75b5..d4417a9c86 100644 --- a/src/Robot _arm_grasping_task/main.py +++ b/src/Robot _arm_grasping_task/main.py @@ -1,78 +1,292 @@ - - - - \ No newline at end of file +import mujoco +import mujoco_viewer +import numpy as np +from scipy.optimize import minimize +import matplotlib.pyplot as plt +import time +import matplotlib as mpl + +# ===================== 修复Matplotlib中文显示问题 ===================== +# 设置支持中文的字体（Windows系统） +mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] # 优先使用黑体，兼容英文 +mpl.rcParams['axes.unicode_minus'] = False # 解决负号显示问题 +mpl.rcParams['font.family'] = 'sans-serif' + +# ===================== 核心配置（优化参数确保抓取成功）===================== +MODEL_PATH = "D:/nn/src/Robot _arm_grasping_task/robot.xml" +TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 +GOAL_POS = np.array([-0.2, 0.0, 0.1]) # 降低搬运距离，确保完成 +FORCE_THRESHOLD = 2.0 # 降低力阈值，更容易触发抓取 +POS_ERROR_THRESHOLD = 0.02 # 放宽位置误差，更容易判定到达 +SIMULATION_STEPS = 5000 # 增加仿真步数，确保完成所有阶段 +# PID控制参数（优化增益，提升稳定性） +KP = 50.0 +KI = 0.05 +KD = 10.0 + + +# ===================== 工具函数 ===================== +def compute_jacobian(model, data, ee_site_id): + """计算末端执行器雅可比矩阵（适配MuJoCo 2.3+）""" + jacp = np.zeros((3, model.nv)) # 位置雅可比 + jacr = np.zeros((3, model.nv)) # 旋转雅可比 + mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id) + # 只取前3个关节（适配简化版机械臂）的雅可比 + jacobian = np.vstack([jacp[:, :3], jacr[:, :3]]) + return jacobian + + +def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=200, tol=1e-3): + """牛顿-拉夫逊法求解逆运动学（增加迭代次数，放宽误差）""" + q = np.copy(initial_qpos[:3]) + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + + for _ in range(max_iter): + # 设置关节位置并更新动力学 + data.qpos[:3] = q + mujoco.mj_forward(model, data) + + # 获取当前末端位置 + current_pos = data.site_xpos[ee_site_id].copy() + # 计算位置误差 + error = target_pos - current_pos + if np.linalg.norm(error) < tol: + break + + # 计算雅可比矩阵 + jacobian = compute_jacobian(model, data, ee_site_id)[:3, :3] + # 牛顿-拉夫逊更新（增加阻尼，提升稳定性） + delta_q = np.linalg.pinv(jacobian + 0.01 * np.eye(3)) @ error + q += delta_q + + # 限制关节角度在范围内 + for i in range(3): + q[i] = np.clip(q[i], -np.pi / 2, np.pi / 2) # 放宽关节范围 + + return q + + +def pid_controller(error, error_integral, error_prev): + """PID控制器""" + proportional = KP * error + integral = KI * error_integral + derivative = KD * (error - error_prev) + return proportional + integral + derivative, error_integral + error, error_prev + + +# ===================== 主仿真函数 ===================== +def grasp_simulation(): + # 1. 加载模型和数据 + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + viewer = mujoco_viewer.MujocoViewer(model, data) + + # 初始化变量 + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + + # 记录数据 + ee_pos_history = [] + force_history = [] + object_pos_history = [] + grasp_success = False + + # PID控制变量 + error_integral = np.zeros(3) + error_prev = np.zeros(3) + + # 仿真阶段 + phase = 1 + phase_step = 0 + print("🚀 机械臂抓取仿真启动...") + print(f"📌 目标抓取位置: X={TARGET_OBJECT_POS[0]:.2f} Y={TARGET_OBJECT_POS[1]:.2f} Z={TARGET_OBJECT_POS[2]:.2f}") + print(f"🎯 目标放置位置: X={GOAL_POS[0]:.2f} Y={GOAL_POS[1]:.2f} Z={GOAL_POS[2]:.2f}") + + try: + for step in range(SIMULATION_STEPS): + # ---------------- 阶段1：接近物体 ---------------- + if phase == 1: + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + joint_error = target_joint_pos - data.qpos[:3] + + # PID控制 + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 检查是否到达物体 + current_ee_pos = data.site_xpos[ee_site_id] + if np.linalg.norm(current_ee_pos - TARGET_OBJECT_POS) < POS_ERROR_THRESHOLD: + phase = 2 + phase_step = 0 + print("🔍 已到达目标物体，进入抓取阶段") + + # ---------------- 阶段2：抓取物体 ---------------- + elif phase == 2: + # 保持末端位置 + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 夹爪闭合 + current_force = np.linalg.norm(data.sensordata[:3]) + if phase_step < 1000: # 延长夹爪闭合时间 + data.ctrl[3] = 8.0 # 增大夹爪力度 + data.ctrl[4] = -8.0 + else: + data.ctrl[3] = 3.0 + data.ctrl[4] = -3.0 + + # 检查抓取是否成功 + object_pos = data.xpos[object_body_id].copy() + pos_diff = np.linalg.norm(object_pos - current_ee_pos) + if pos_diff < 0.03 and phase_step > 500: + phase = 3 + phase_step = 0 + print("✅ 抓取成功，进入搬运阶段") + + phase_step += 1 + + # ---------------- 阶段3：搬运到目标位置 ---------------- + elif phase == 3: + # 先抬升，再移动 + if phase_step < 500: + lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.2]) # 增加抬升高度 + target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) + else: + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + + # PID控制 + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 检查是否到达目标位置 + current_ee_pos = data.site_xpos[ee_site_id] + if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD * 1.5 and phase_step > 1000: + phase = 4 + phase_step = 0 + print("📦 已到达目标位置，进入放置阶段") + + phase_step += 1 + + # ---------------- 阶段4：放置物体 ---------------- + elif phase == 4: + # 保持位置 + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) + for i in range(3): + torque[i], error_integral[i], error_prev[i] = pid_controller( + joint_error[i], error_integral[i], error_prev[i] + ) + data.ctrl[:3] = torque + + # 打开夹爪 + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 + + phase_step += 1 + if phase_step > 500: + grasp_success = True + break + + # 运行仿真步 + mujoco.mj_step(model, data) + + # 记录数据 + ee_pos_history.append(data.site_xpos[ee_site_id].copy()) + force_history.append(np.linalg.norm(data.sensordata[:3])) + object_pos_history.append(data.xpos[object_body_id].copy()) + + # 渲染可视化 + viewer.render() + time.sleep(0.0005) # 降低仿真速度，更易观察 + + except KeyboardInterrupt: + print("\n⚠️ 仿真被手动终止") + finally: + viewer.close() + + # ===================== 结果分析 ===================== + if not ee_pos_history: + print("❌ 无仿真数据，跳过结果分析") + return + + # 转换数据 + ee_pos_history = np.array(ee_pos_history) + force_history = np.array(force_history) + object_pos_history = np.array(object_pos_history) + + # 绘制结果图（全英文标签，避免字体问题） + fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8)) + + # 1. 末端执行器轨迹 + ax1.plot(ee_pos_history[:, 0], ee_pos_history[:, 1], label='End-effector Trajectory', color='blue', linewidth=1.5) + ax1.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='Grasp Point', s=50) + ax1.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='Place Point', s=50) + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_title('End-effector XY Trajectory', fontsize=10) + ax1.legend(fontsize=8) + ax1.grid(True, alpha=0.3) + + # 2. 末端Z轴位置 + ax2.plot(ee_pos_history[:, 2], color='green', linewidth=1.5) + ax2.set_xlabel('Simulation Steps') + ax2.set_ylabel('Z Position (m)') + ax2.set_title('End-effector Z Position', fontsize=10) + ax2.grid(True, alpha=0.3) + + # 3. 接触力变化 + ax3.plot(force_history, color='orange', linewidth=1.5) + ax3.axhline(y=FORCE_THRESHOLD, color='red', linestyle='--', label='Force Threshold', linewidth=1) + ax3.set_xlabel('Simulation Steps') + ax3.set_ylabel('Contact Force (N)') + ax3.set_title('End-effector Contact Force', fontsize=10) + ax3.legend(fontsize=8) + ax3.grid(True, alpha=0.3) + + # 4. 物体位置变化 + ax4.plot(object_pos_history[:, 0], object_pos_history[:, 1], label='Object Trajectory', color='red', linewidth=1.5) + ax4.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='Initial Position', s=50) + ax4.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='Target Position', s=50) + ax4.set_xlabel('X (m)') + ax4.set_ylabel('Y (m)') + ax4.set_title('Object XY Trajectory', fontsize=10) + ax4.legend(fontsize=8) + ax4.grid(True, alpha=0.3) + + plt.tight_layout() + plt.savefig('grasp_simulation_result.png', dpi=150, bbox_inches='tight') + plt.show() + + # 输出抓取结果 + if grasp_success: + print("\n===================== Simulation Result =====================") + print("✅ Grasp Task Completed Successfully!") + print( + f"📌 Object Final Position: X={object_pos_history[-1, 0]:.3f} Y={object_pos_history[-1, 1]:.3f} Z={object_pos_history[-1, 2]:.3f}") + print(f"🎯 Target Position: X={GOAL_POS[0]:.3f} Y={GOAL_POS[1]:.3f} Z={GOAL_POS[2]:.3f}") + print(f"📏 Position Error: {np.linalg.norm(object_pos_history[-1] - GOAL_POS):.3f} m") + else: + print("\n❌ Grasp Task Failed! Try increasing simulation steps or adjusting parameters.") + print(f"🔍 Current Phase: {phase} (1=Approach, 2=Grasp, 3=Transport, 4=Place)") + + +# ===================== 运行仿真 ===================== +if __name__ == "__main__": + grasp_simulation() + print("\n🔚 Simulation End") \ No newline at end of file From 6e3e28a1c64fc4ef2d2f556a6d53e67eca0c45ab Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 22:35:52 +0800 Subject: [PATCH 07/26] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=86=B2=E7=AA=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot _arm_grasping_task/README.md | 44 ++++++++++++++++---------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot _arm_grasping_task/README.md index d6d73dcfd3..3dd17188c8 100644 --- a/src/Robot _arm_grasping_task/README.md +++ b/src/Robot _arm_grasping_task/README.md @@ -1,23 +1,35 @@ # 机械臂抓取仿真项目（MuJoCo） Robot Arm Grasping Simulation (MuJoCo) -

- 仿真结果图 -
- ## 项目概述 -本项目基于 **MuJoCo 2.3.6** 物理仿真引擎实现3自由度机械臂抓取任务，聚焦典型物体的精准抓取、姿态规划及力反馈控制，可直接用于机器人抓取算法的验证与调试。 +本项目基于 MuJoCo 物理仿真引擎实现机械臂抓取任务，核心聚焦典型物体的精准抓取、姿态规划及接触力控制，可直接用于抓取算法验证、机械臂控制逻辑调试等场景。 -### 核心应用场景 -- 📦 **单物体精准抓取**：从固定位置抓取立方体，放置到指定目标坐标 -- 📊 **力反馈控制**：基于末端力传感器数据，自适应调整夹爪夹持力度 -- 🎯 **轨迹可视化**：生成末端执行器/物体的运动轨迹、接触力变化等分析图表 +仿真系统支持三大核心抓取场景，覆盖基础到进阶的应用需求： +- 单物体精准抓取：从固定位置抓取立方体、圆柱体等标准物体，并放置到指定目标坐标 +- 多物体无序抓取（混箱拣选）：从杂乱堆放的多物体中识别并抓取指定目标，具备避障路径规划能力 +- 易碎品柔顺抓取：通过精准力控调节夹爪力度，抓取玻璃、鸡蛋等易碎物品，避免发生破损 -## 环境配置（Windows 一键部署） -### 1. 激活虚拟环境 -```bash -# 打开PowerShell，进入项目根目录 -cd D:\nn +## 核心功能 +- 逆运动学（IK）求解器：融合数值法（牛顿-拉夫逊法）与雅可比矩阵计算，实现机械臂末端执行器的精准姿态控制 +- 力反馈控制：读取 MuJoCo 力传感器数据，实时调整夹爪夹持力，保障抓取过程稳定性 +- 抓取成功判定：通过检测物体位移量和接触力阈值双重标准，自动判断抓取任务是否成功 + +## 技术框架 +- 物理引擎：MuJoCo（v2.3+ 版本） +- 控制算法：PID 控制、计算力矩控制（CTC）、阻抗控制 +- 评价指标：抓取成功率、轨迹跟踪均方误差（MSE）、接触力稳定性 -# 激活虚拟环境 -D:\nn\.venv\Scripts\activate \ No newline at end of file +## 快速开始 +1. 配置 MuJoCo 环境变量，加载机械臂模型文件（robot.xml） +2. 运行抓取演示脚本：`python grasp_demo.py` +3. 查看仿真结果日志及可视化视频，验证抓取效果 + +## 环境依赖 +- Python 3.8+ +- MuJoCo >= 2.3.0 +- NumPy >= 1.21.0 +- Matplotlib >= 3.5.0 + +## 安装依赖 +```bash +pip install mujoco>=2.3.0 numpy matplotlib \ No newline at end of file From cd3e0429dd95a712fa523969725d3740172b3361 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 19 Dec 2025 22:40:19 +0800 Subject: [PATCH 08/26] =?UTF-8?q?=E4=BC=98=E5=8C=96README?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot _arm_grasping_task/README.md | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot _arm_grasping_task/README.md index 3dd17188c8..fee088f1e8 100644 --- a/src/Robot _arm_grasping_task/README.md +++ b/src/Robot _arm_grasping_task/README.md @@ -22,14 +22,4 @@ Robot Arm Grasping Simulation (MuJoCo) ## 快速开始 1. 配置 MuJoCo 环境变量，加载机械臂模型文件（robot.xml） 2. 运行抓取演示脚本：`python grasp_demo.py` -3. 查看仿真结果日志及可视化视频，验证抓取效果 - -## 环境依赖 -- Python 3.8+ -- MuJoCo >= 2.3.0 -- NumPy >= 1.21.0 -- Matplotlib >= 3.5.0 - -## 安装依赖 -```bash -pip install mujoco>=2.3.0 numpy matplotlib \ No newline at end of file +3. 查看仿真结果日志及可视化视频，验证抓取效果 \ No newline at end of file From fed40a42363338cb762196009302e800ccf15f69 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sun, 21 Dec 2025 04:13:35 +0800 Subject: [PATCH 09/26] Trigger PR refresh: sync README From e1c3bccc36e45372c47cef91edccc4daaf543cbb Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sun, 21 Dec 2025 15:02:47 +0800 Subject: [PATCH 10/26] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E5=90=8D=E7=A9=BA=E6=A0=BC=E9=97=AE=E9=A2=98=EF=BC=9A=E9=87=8D?= =?UTF-8?q?=E5=91=BD=E5=90=8DRobot=5Farm=5Fgrasping=5Ftask=20=E5=90=8C?= =?UTF-8?q?=E6=AD=A5main.py=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 0 .../grasp_simulation_result.png | Bin .../main.py | 0 .../robot.xml | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename src/{Robot _arm_grasping_task => Robot_arm_grasping_task}/README.md (100%) rename src/{Robot _arm_grasping_task => Robot_arm_grasping_task}/grasp_simulation_result.png (100%) rename src/{Robot _arm_grasping_task => Robot_arm_grasping_task}/main.py (100%) rename src/{Robot _arm_grasping_task => Robot_arm_grasping_task}/robot.xml (100%) diff --git a/src/Robot _arm_grasping_task/README.md b/src/Robot_arm_grasping_task/README.md similarity index 100% rename from src/Robot _arm_grasping_task/README.md rename to src/Robot_arm_grasping_task/README.md diff --git a/src/Robot _arm_grasping_task/grasp_simulation_result.png b/src/Robot_arm_grasping_task/grasp_simulation_result.png similarity index 100% rename from src/Robot _arm_grasping_task/grasp_simulation_result.png rename to src/Robot_arm_grasping_task/grasp_simulation_result.png diff --git a/src/Robot _arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py similarity index 100% rename from src/Robot _arm_grasping_task/main.py rename to src/Robot_arm_grasping_task/main.py diff --git a/src/Robot _arm_grasping_task/robot.xml b/src/Robot_arm_grasping_task/robot.xml similarity index 100% rename from src/Robot _arm_grasping_task/robot.xml rename to src/Robot_arm_grasping_task/robot.xml From 5dbb9c3ee75f7ac70ce68c740abd7f78ab767489 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sun, 21 Dec 2025 20:28:50 +0800 Subject: [PATCH 11/26] =?UTF-8?q?=E5=BD=BB=E5=BA=95=E5=88=A0=E9=99=A4.idea?= =?UTF-8?q?=E7=9B=AE=E5=BD=95=EF=BC=9A=E4=BB=8EGit=E4=BB=93=E5=BA=93?= =?UTF-8?q?=E4=B8=AD=E7=A7=BB=E9=99=A4=E9=85=8D=E7=BD=AE=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .idea/.gitignore | 3 --- .idea/inspectionProfiles/profiles_settings.xml | 6 ------ .idea/misc.xml | 7 ------- .idea/modules.xml | 8 -------- .idea/nn.iml | 14 -------------- .idea/vcs.xml | 6 ------ 6 files changed, 44 deletions(-) delete mode 100644 .idea/.gitignore delete mode 100644 .idea/inspectionProfiles/profiles_settings.xml delete mode 100644 .idea/misc.xml delete mode 100644 .idea/modules.xml delete mode 100644 .idea/nn.iml delete mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore deleted file mode 100644 index 359bb5307e..0000000000 --- a/.idea/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# 默认忽略的文件 -/shelf/ -/workspace.xml diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2da2d..0000000000 --- a/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml deleted file mode 100644 index 25d4c9c531..0000000000 --- a/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 0e01bed9d6..0000000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/nn.iml b/.idea/nn.iml deleted file mode 100644 index c23e621695..0000000000 --- a/.idea/nn.iml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml deleted file mode 100644 index 35eb1ddfbb..0000000000 --- a/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file From 99c735cd659948926777f591213e761d94bd3348 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sun, 21 Dec 2025 21:18:34 +0800 Subject: [PATCH 12/26] =?UTF-8?q?=E4=BB=8EGit=E4=B8=AD=E7=A7=BB=E9=99=A4?= =?UTF-8?q?=E5=9B=BE=E7=89=87=E6=96=87=E4=BB=B6=EF=BC=9Agrasp=5Fsimulation?= =?UTF-8?q?=5Fresult.png?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../grasp_simulation_result.png | Bin 63844 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/Robot_arm_grasping_task/grasp_simulation_result.png diff --git a/src/Robot_arm_grasping_task/grasp_simulation_result.png b/src/Robot_arm_grasping_task/grasp_simulation_result.png deleted file mode 100644 index b58fadd6562609b06a42a58fdc14e60eacc91c50..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 63844 zcmeFa2UL|=)-8(KYKx_o2~-aAX!B~vg8~}%LD=@KsW>i$w_h! zDnW^oGbl*TAUWN+4_0?ob#>qWzW3j8?|9>#F?x6k4&V90-fPV@=Ul5FTsSYWfp!-y z4GqnPGe4h_p`lqbO+)iH|G%%nPgdP!WyC*%X5yF4WR0}UEU%hq(nwu3yQyzvrmu5t zkA2-lEv$UM(FqpHxl7eVj5f ztSo6yShciX()qmF7k-s4=DD+y6T9xdYlp%P?RllnByMq?veaQZyGiWk8m%i@&(o$v z1KbPkeTB4VnoY&JTfI%Y%tZsF{o3)u>5!MnPc1&JE0*8CX#bnt^1DYT{|~-<)FC?a z7X~thvyCgn>GL-V4Ser7-@%-q zsqJsA#i5)WRU#;L_0vE@LdUOL&)@HG7jtmoI;{|+;_Xq9Z9hjF?YKD8t=(_rQis#g zl{d&tZlsHSZ1_TW;cFHDoB*z`&MUyM#Xu;7sm93mgiT5Axmxu{yOEc}>!Ee~iGG*}Q)@tMF&-aXYx$ivaeQPAkmHO$H zt3xYYES5`kRHK!FTuba|KDPspt$x{5_EeUBW3Z%G>U6tHUAc6fR=MchfL4lBR)+o| zgQaLAd%dUqp?X+r)7b4O`t zoP6Zo*BMd9ymICb7^Zc4-e^qJna9^P^)!a6W!X^d{o)7Wi@7K*A##!O40nu@CXUh^ zlA`ycd2~?5Nv^Vz%RIw7J=Si1w6LSxanaPS>qx+dmATSQ*4=&@dG|NR8MdTP4_W3g zM9N1Ob%kCS4)#)Wez#GGW6eg!)Y7)~G&ILwdLPx3|85uWAzT0Sujfv3(By{veVS&E z``6(%vFyc(*g^)Q%*Wde#Bu_E?_6)jcyotj>%bhd*@b$m@mIOb0i9n4DtYSx_| z)6LzYUK@gGXgW8KtT^u9fc4bD&^CQ~G7}q?!n7y*)ZFBt#`MVNIGc&SD7~_wmW;w< z-M$GMCh2HsUcB~rnBirZey(FExb+gv3*nWbH1Cgu*RCB-kkFgwxlM3UG_;^! z-+AuA%y_qv1x4J2PMpGLkld?>LmZ9dQ&X<#>Q6!VFtlhZ8gpEla>Vuz3p;dBZIx3c z^5Pc{1f1k%bFzmQXYd4f&0*WZlF0EDeEg}NtZM)_7&GF{vRDQSfLMyR}41M3c%F(?|zXkMfxGQgP9gI~P-W zGsXOto@S`zT>N`ZWY{h@9S39KahDEnN5-o8_+(FCdO5z7%_G~Lo_N~go*)rFK>XKl zjF5};Y4ykqGORJ+8cUPQNh{J;XzAJ)(DXK}smxEP?km@YGcj$4er=Pxw4snUN5Fq3 zf3!plM`w*V&@FzT&Pch+hMQ*%^YUH!@;$;D_4SwU9?c%j!o5Fkc!-OuY`4CCOS(CO zROCrNLGyUBYJR{Prn!>f$*sOyTwFX(zv1vvxw}X8YOhyq5}SXm)E|20DN6-T_(pST zTn9dX;gea#IvN`N%Elx;jm7zyw^^lO3bAUFVTP<%qzd;jA^6hJQs{uAa#`$0-mF!8 zQT_VryLy9^mTMV<$+F4;@d%0fN`B@~)ikgdMn4=3xL)`QK*WulzPwWCwK((Nf6I$;;&afAQVV3siXMXso8`#n}}FSpdAM7|M&BZeyH%>m(&ydf*-%A$4U%oSW>%5))8lY!IoJQe|>adFg>HRT0 zH$UZGetqSQya&H_nePtX8yBc(2G9CNO@tgHg1_-Zje=L&q9YK>4q>aC14s%Dv#t#M zZd|JAdE5@8E|=r4pE=ORglCzLkdCXEkdIN>d5_8BvWmk@XPjAYrNB^X$2E76$@|Ji zSx3{&`{|6bCTi(CrbgavlISk;>yUPwl}*Hwot+wHUc1}s3f-*huA>pvvqqVtrx|bN zt}tGhs4K)m?Rc)?_5iT70PAO~m#T&0*MWv3beu|;?hNn1Evo7>(!zr}u0}bmc{cGz zMPP|0>#2wy!J4?;3=E=nR`QnNcUnlC!!j-Ha^JzTuS{gBIqE8Lx* z6srrG;p>yAi+{Dw(!!KGcHpxNHS7K;wXF1?`sEUZ&nhSB$~|N}cG%Wm>7&Q5wjUnt zJ=snm*ExUTV`_Gz`8};!wrO(Dlh1Ij9L}B_a($ci3gFB5zwej?dDnlZZuNQfJ#2`27V4fp9zFTBN?Mz<6Tm1oN~la)Xf$tYLhtlwM=>Y6 zdHAubZ-4Fi`0^y9dG*EZt@n7l)U0I`0`T0LZjrs@q%??3Sr(%6Ff;yHNr0%mO`2w~ zp|C8!Z+NGguGCCQ&H|?cs*Hk9Z*F%KFc>|&v!32FtwkYcwnpElfaaXWh{w-wI*c5~ zDFfDhEZgu{ls}RkPLtG_X+0j_JCrf3);0EI#R|b0fxyhCriyjJU-sUT8-fiH|JGn{ZnfYp;^qF(f>lu#fZnJDQmIyb%VIy3Y z&)NV0u_4j8qd=3(sPferK2@yBG)<4hj~PHK36G2h>f?;RJeDX}%WU^3sZ8=j%V;4p zGotzZ7K1q*}5#1BeK1bdGn`xp2gGp*&8>=>52K9IA)e5g6CWCmN^ZE~(Nuy9pO(8^z!Y71C>XS}=IT`x9^Z@0trXMrz|PR46e z9}fvCG}XG{!K(3gGh>InxRsMRD13UgQExSad(Ry7yQDj_Q`>hlcjm}XSWVUonKh9L zg;+8>2u?}RJfN|dbfaP)Lk@R~we3(^cL6qedrZbq#7En+oPXo5i;qQZa8bEB#QxrM-&9FCsa!b#oYHI2yz6e(GkG^`P$x)@d5>8PhgQ9!ie zo#D16UvM4jV%)A(Q&=H+{`fZ0=B5j0E==cbmA*8>glKumZTYcrn!9=fS0$Bn9vIIs z>y3`zdV&DOKr^0wCq^JIPZBR#oXizj+9IgIl24YPx;zg z=&q4V=TdVNX*Y6o>8WOI_tD$uCLG_`uUGJJry_gww%s0@2-c&OlH48{=V`r2_!Y7F zT0dYDn>DF4kW{B8H@lnmLq5H_@9LZ7!sF*qg^N~XSPnlIvNRN)YBsGvOx}UNINeXb zeY+;-5W(Iuntqn8);z}Tc~O~eOhTv2{Y4TcsxI5=)y257$E5BP@;|T(`!MLVYf(WX zAVE`mz|y=nquIw3aVXiWCL6R9TOuBOTKJA;H9#KWSY654DR*QYrfm@$n{O# z%aiwLy^^qN^AQuaDX5yuCF<6&v7CGUJ@WP0TH2fQ)1%9*Kv%!|l8x}v{P<|8k;4(a z&tA!nv%R4SA14v4If zpknu8t_%*_XWh8NrMoHXXTVkXh_JCGbN0Y;cClQSVZMBxZ!N&6ob(7n?c~z*pd@DCQTsEr2s+meKA>l zfkSj^Hm@dP)4Cj+aKmUASf`zg@}0Ny=-0pQsgKj()vZ2cfwE_Zz|AWhmtXHmu6}!G zeMhL+f=;4d9T(`S=Td}CnV#-&XHQ%J7;R4oSkOl$cCfE5HYYz8*PK-OSmJcEiDdrC z>_jysF#~)%%`=>d&I`>>aP)ZFFIk4aw6M9rva}iZb70#{JPL1-CbeJ}(_cPPVE1{}gDo8!y$T#5ykYqs)4kI+Whu{`nrW8<;UZi$S~pxgI;so!{r;W63bEQ4)B!KRDA7thl4kzKZ{X-n4Px}A#HdYp_z7K1URu#t5rgoHK+Cdl@eynYC?b0`v#IJZ6G(#9{1dKpykr$h{q4e;)2U zqV=>X2PxXP(?g+PDrae7x+-AtlKAlV-R*Mx4}ppa_hVG| zuh}r$_T089Ot}mpR;)HkQFh3tGdclaoHm-=EV>+DpqXz&8GMa zd)!8Kg%5M9fDTyiSlZ8>2KBb2G!`5)TE)BI+r?mtM@Lg6_j*NJm9!X>3mvC()?_0) zqKdrk3=+_DsX}*VbwfpW;jy2ilvB8DriPd@z(<`fuDomqvcZLLT-eQ*x+W1%oJ0L> z*g$x-zB&a46RVOx z0g95>h5NJ?-(_2mcWG9&Shi)63IVIB0f?C=|tC~}6!hh~4LPF(c(2g-|xs@e$GeO3Psp%)d-Ql)N zUu}Auv!gm5dj@wP(j9kTuV?=X5>cxPSjHXA+EZeDxaS1oZohZGX*K?&_35rKND$ffK_1{IxN#=zf35M0CC+}@^CH2C2 zp}|DgTYwsec-=E+ejcz~a2V}u%4T$=jHXyEysPWTkzAxmRh8>3Y8y@KC$}b(0(BsL zk88MU?S9V4_kz74Z=7SOTl?h|<>~rV?J9?iejOQl5usx)tZ+2C}=rv|NNfS~>J(4>V2{MKC3-I z=yz03`a)u@itI`in zNxS9VH_tQ1)om`I9zmM5XE`(b2xwm`nY~#nZ$L@!_vO`=@fao<^ot%+^Es&jR^K=bodZ);E$iz z!O99wHmZH0=rKr&`Y-F3kH7JT>G9y?;l>cXKmNRIz(FfGnYr5j!XJM=n>@@gm)vX_ znLJn>ArycAddImoSN|)N#2YR8EXtv)-L7}shq82G?q#h!3SRRP`@zRt$FT! zYG=JN8{=5;MCGgwwgSzI#97S}+}ALfRU3DFZ{U;sz?8*z>k$_%!(`>MY7-;nJGQbR z=N|k;&V%rT67dx6pj&ZXD(u?eOeS1HUcKh(@_^2dQiD`m%aT#Wv2#=Tw8_!fFFW+DRPdQY=L!f4 zDth+QE3!)u2H0&`Psc%uIL^L0%XOnV{k73eHQ88(bH>J8Ej4`ab1W7hu_AhY6gOZ0(Kh zA-M5?wyMWg54!s9Zi9d`sU%eFCu%4IfflCjP-LQLTMytq&pdpCka0XZ#iX-{<2d0% zh@|tN!%c{5eUBm77b|dj2{TNBW??t2!4Tf`wQ7H$c*E+`FKY?X?|8RK?4g31^#P*7 zft_zX-5=mE+e_i-XI`3?z(tA=w`Q?V?8eowQCaKb(flBRIr1bq2mU=E0@~WSFgNA? zZzvW(qL7k!QN*@s0Cyh|F_O&3TJRnEJE>3QLS(d365DAvMsJav#o7M4dsOx$5Q&Ay zdhCnUXg)prCACJDq`#kg*>d-@noNpeivSb}woD^JUUfW`7X6?^Glbe_dn?lJi^EiH zxLQ|QY{85}De(}YO9(TF3O2IzI;x0H&xtaj;k;khZH*rib{FgmJ3HJ2eDqi4^9&!J;02r0Q5ft6D8?E8m4}}o7(Qo&Zk(0h?Y?6L?m2?g;|492b9de$3OIJ<;(Q>Kgs#Zh`Z&c z>|oI=ZjsHV#AzPt&7q#1S<(sNt2m_8=V;_jMID>UGY37C913=7U$6S`^q@l6gc=*! zKb_fJBqUKI+l9y^gC;&oS%6p$0g6pYdJ2Y$2l`(f?#Ef_TJKuovPMMX7m_bfUz)80 zwN;DUaYdO*^<9WGA8a(YPkD}q-v-!cQrw3ibXsH7UTN2Dqj#PTnlnU}1D$6`M>h!( zapf@X%M0CQwzOvBu`iEqfXplL7ZD1Pq5v|d8h6C&h!?!=M_eMjDDq{NerA?5d#_Kz z4}Wh~neq*vP*aSi!PbbNQsGWY(I;-N;jtR|G%%y6?U&?Gl64*B2oF|bPgSV2*>J1< z!aQ{G)1l}6+HbArst=2Th+YWpF)%e%fL(92J-^>e1Uruh8X`xc5X8vhW-s22pFs9f zM3yE~a#TBqx8t1m^(oxo#L0)9AUK3BR|KDoLs55KRRf9#KVf=qfWRRvdeDT6vvWM1 zO*nh0H2B)_YwdXt6s-yU1qh+lFYSH!0E7ClGkeT^eE3-!GgiAg2bLWn#tOlFDjN)g zFTY|+EzV!m>LDc46{(thI8cGx!*N4$MVr%#Rq;I2f@VDzKF5;{WCd}pJt8v3nJ9PI zwe8>!KPatmV-^$T${cx1YhI30;({#ofO`(=Lx?nP(Dc%DA5(#iqCvxSZyQR;ghJ1s z-|WG(){b31>OS_8c%d6p5LnK*{+Nt{++S7^QAk~nW(rOd=R&zTN zl>2Uxc2w2&q!1$h0|}G^?I^@uxa~f#AI0|?BrbmW@U-Lpez*I?NEz3+C$SIiPgAGzq5Z59Mxp&=}Fl6K<{ z%ZxzJ9L@)YdvU|?F6tLLsmP<~1$w>)xxQh%+Brt2DgV(E~&;Fe$dd&gnp+ zdPo4#2L9$pNuSt>Xd?kAgoHnIAJvlM!hE&Kk9VBHYufJm@Eh$8l5j8c7sIpQu^eh< zv(BM@8_kz{w>C`sBhg<@ym686hKiQH0T%5K4r*16b`*Lk*P;a5f}^@XqbMYJyds{L zPCdtA;Tj}XBD%}nXsZ-(V%>IbajtDio=WuM$QS;O+Rk>S(<`C(m$kbu6ZRiLsVzkF z0JGz~F^5X(kG8D{e6$7tc6c4XI_+qGhVJZ!hO*^*K@CUtw%|)%_-xl-pZym#dQ)N)1VPrjo0$DYa{3}7i&RMx zLvnd5^O|#7H)v@tXUS!8VXlxcQXo>P8CN-R0qBLV>7Nw(2mU^Jq6I5Y{DVa(9Zs+L ztr%aQjSJSQ>MZv1O0#Q-*K9}x```2`syclbb*%F-)2=)a`rC5mT87z1kfq4$5btV+ zk$=fjgPOr~w%7vt#GQ~mTN7IVh_V?P+rUet2fVHvq2&|rgL=j%^->9Syd|hCrDCN7 zEh%4N7}x|r->eKoV#Le9@zsoAk74~F)V+@OyreS+0|za|H-Zk zSpU!1^?;cY?e=O}X;)|!`R)+oqU6*bfD$IU&i4d{5MG_Zz!Lnkc8hVyVPdX>5-5H& zTVwf4$4)F9y7K>|h*pIX%%fTKEEVKgLhjIu+Z&z1;pP2SM;rFwCb+jPElxFzi%iz< z$A-*2JT*E$US3|bysn)NeRYaOh$Ngk@g`{yNvh3$Djx;#HW>Ww149a1;0Lr`|0tLV z;arr4xS#>K^FhG;mxH5VoZ~O`X9A1GI4;&}xEsVbCvuZIhrG^iGt2e*U1obV!X?6Y z;Lh!EIkhvznWbmmsM;)lFxT@?{Xd);^i(8*U;K+lR1j(bt7oMxiQh+i5b zJ`^J0cBFQ=xsV%3suTjpGbd`5l`plTB4Na4cN1Ial_mhUr!B{k5rI>&hhMik>~P8q zih}~-3nm%*PJ(&Yp`V9nus08Os-GP&-TZvGY=g&dH1^qL_A0O zNf?RVSIHMH{s~Z*k9O(_DAR%yYnd+TQa}g~la)fuZ|ztHEr*&V0d*dkj{?NJ=A`-{ z!fO5;tUZ-#4|Ei|bEiZ>yg3L_6DV#RL{4Hh+%eDyV-4a+Vg2i&#VR2rFB`N31o3 zsMSlVLoyjKmvPCavT|S|^@Veh?&Uw&qRI{XJ0ThZwiby-ZKBT3&VeDp@=w2PDm=0z5JF5g+-};7#^=V1 z1C>ntDTarm6k=QDjbe$9f<`y#FM@ujXVT5jZ}n}EwhK$VkC2^7AVPkWVnTaj)%jw7 zB&tzTgv&J9!AffUW;J~tf@S<3t~)nVAXiX#91Mn99jm||=&z?z0~dZgtJE6&KP`OD z8jL5!+8YGzF+!oQ*q;rVkv%4JtVmyJGvw)d{o-uES#g7ll*v0qrpqN8)YYzdG!gFQn>ho9g%=T!4C zaJ60N6Ep)mz?H;=tZh!EcBr+`Q3^i$XGvEjOIlK6#6G=74Mnd;IZEO$7UAp5-NhDD zt1a;LHQy>^}t6;0N@YQ*I;T6hqMeh5kAeAUR@ewafi0;Ir^)l|s(T+)vGU zZ;{pLHwlj%Z#Juv8ihv5jvek%3d^us4mOT?ybjUjkmpV8z+1V43=wZh7ko^jH7zyK zvt0)@LR42cw@%RY459=A>+jwJs$u})Dh?%59!zD|KEJ&)3Q{W{8ay*;10ZJGI26kH zaQN&F4RANq3??@ zVqAK(?z*ZCV%_VBukQHgcp4;JW+E_T&h|x;Rs*@^eDyutN@r>Vxea0|h`vrm)S}wS zL<;etGRX|a2yIeivq`M~24H^gU!$7gu0)o4A35j`deVy2Il!2#rnpCa*EQrC7$}G zc2u-(Ak9w!AOxXqsToB_3{ABYVf*N~)uM)Q7?Y!)lmWV%!R3s)JI?PXax5Ytd(wHt zTcRu(m+%zk)nR9asjCmaz~86!0hN~)wAx~41uCZ)9?c>Y{_0>ER%P9nme||*JrzMg z2s+TWg1|0QYi|N3g$7=qto1R%<5%plL_*FdmUP#uk(OZxeqvon8bM6jlj4iCz~i_$ zKV77dz2)}WO=ThQaVT3zUG_AqB2~aq#*qb3Kd$VtnTOX8Pog*_{y;`V2VzGPM{+qE zodb)f6?!lTyEBTDKk5LfxDf1lJAy^Y&^%lOds1KlJdlw#K32?KzMCQg4l1gfSRr)< zK1oIoy8-MGqj&py_359|`a?esL+3Bv-TQX9K|7%Ir|e~?(4G`w+~|rN$0d$b$*e?T zDDZ(oSd{^capC)9!A}%Hb6w&UR9<*XNbzRJr3D^XexkW>sZKtlF6#697(zZ{A-fLS zbqgu7Q-hCnu7dZ?SivGZo-0k(NZ`~s>Wd0Ef{CmOcH6I}IMMot)tuS{I6pecInAey zVwWhz0d`{_Sn71}^vpAP@yn)M;B{tHqn&I%EQ;6^5grUBgET?G;@+e!Ozro8BBh{&Ekh zv;ptAcSy7e5{fcZ+L`maX=0opoZ)wj0bt99=p^{TY+=qX%$JoOLp8=46APe}UI02QmABjuKXpB*=ZNh3n86K}W0 zv-Jliz_dqIpJUas0wGPxIA0;a+XG&F!o8b4Y;?ZHzB* zsHF1*WbawXs!awKh;}EBw$dk9||#Mk5o7*)J5XM`BZH<@sRJn297z*d{fUukY9Xiv;~V$I8Jjwf!BDq7wqD z=F3}08uYO@-me4I6W8Xj8CfW=ZIO=0(IM|1^XbgR*(Hbb<7muUiuRHKYGLBe`YOX{ zoVR%tVX%NdXQni&&Xf1j$@GRJtDSn4*rck=Kg06rFG6&1D;O|g{&IKoQg=8;+LV-x z7d(Nt4}!URmURAH-OX^#_BU&peOG&HZjgLzDBn`ufdaa%Z;I@44bEwqDPMw~NFKY0 z)$)GzZ-gL`x2efok5mGx2+&a4K2`YU2GyN^nTa;W%qAMz`(<E_T1n^P_tl%Z#gBAN1ZWLusmg zLN(ox#4c~dDm2=FNGOUimHE5Z+w4Y@1o))(b!l3@WetD^8^ zy+*_3F$T3J0*0P8ISkvjxva3>)(D)jx>Z~|y~$WsW2ojdF&??6cpPit;cZRkj z-K=-Z(}U7(;CJGh<116QkZ262{~n?;xvosBPLk}i7)Js7p392=8P}<&ZS!6H{qM5A zkmg6YzWLUFSJe`iCeYQ|;O?vX`DW}Xr6u2W-Pd7vJ0AU{>|(iSMmzB*EFSBgH<}Z! zfpL5q#_)ioYRMLh5S`gHws%<(nGWm?M1L21UMpJlWqJ8uxeE z{!Fw}rPIWe^w;ri?LPNVk-4lf!a&y2?`hYiff!}vD=n7u51%KQcc&^^`fb-&JUyYF zlZC~@1M7E`P;8_pko@3!fvkvowo+dmU-|-m|hzJ;XvLV%f$bqBS65Uo|XLN2( z{b0_nbnJ;LI+FA5;mxZtI%^L znvj^YIg?4Tot@LR?l}`ynPX+_4dWY>&ppZTTSp<}HS8?tKI3?{YVE4*Xybl1qp%Vj$};4KhWl&K3JY)WxQ zTTeY59k&YO9DhqriKeY#j&@oTw_@sb%AlU;8B5+ELsQrC=-E;^(TqMm`k7|=%J%%; zq0IPxi}JB@VL#C3qo?`bWv*tKH70Da?N4ZYzlGvyCgex!dWmJUSLZeoG-*)&>X8Jm*ev}Ib9osJ^e)(3UT0F?Cl{Fz-r+5V-Y$?Ru(%V>$Xa-+-rD^}l18bYrNZQF zH1LCpY^8b8m{jwb4E=_9{YFqhx~!3G>N&~BUX-pFoxMj{>MrauGpI^BI|a6nZO~~| z(S~Wh$Vk)XMvDc{kb{0^GU1&%`ZL=`r2K``ewv%~F1KT8Vo4gZMHXaZ9I3JJ`kMBG zL;cLXq<#9Ybf1~c^;vao`h7!z|L$@o(>dW3!);;722EO3NFFc~UU%rT2^K7hD5{G! z_>*_&(t{)HmTnBUeLLh&%c+SGOFoC*^9Q_(s$01h&xMsVPSLd~!%OK6zDb?ksc4YSdpTq5HfN)$ z#N%)la-rUrS%4Em2G{&49YWKe=3HP4d=>R1js~ks*C2NawY2uC2h0Q+`wiP2bb|u} z?w1p3O~eLL;LgmP(qf?u;J5U?;8@tYdyFPO^BT1ZYnA&mRfR~otdj8(Vj!KX-?{>V zS2UnHV~`e|xo+XrP3U=t2KW)4WVKa$=|%)?fpo1+=avoptteuLBBl)$twE*g>$=_6 z28<$;J(m^cb4EGY0uBArjY~JzJ+r2V5MCW-=4LaPv{{1LZ;f%}G}?7f0}U=Em#@N& zD=MpVadBanJD=`wR^wAm-h?tixSGYDqbQ{`I|c_Sx0*IQqjo2F9<0fyYVBfBOW5cA z0b+Guxs*Z9WG_Rk6-qvGeW;ac}#w#=&N{C%9$0eQSd*0_^IRwe~;%;)hboT zg^4)Sn65=i(0k`c?uk7_4ZsClvlUjNwm^amUzaM61ZNzK&7SS!c$LxVsT^O3E-xRE zBHdr)OjdX)#VnBKEkaunKKUr?9kQ6zI=4LtX``p7TopuoDFWDhIL>cV6igEsS0z)VILyz;Ueu#ky4t8*GmK%

H>nkyAfq=Lk{dWd_!%Vs14JcnF}g~dfnVnv6MzXO`Ii=Ra!D>!0mh8VyPwsS>7-64XOcE^?w z6PX)cD@%wV)gW-Y_f~9_q0ZvsR8AobYhJ1k2^H4xd_%bS^~NsZ@&A$zLpvOqJUV5s zQb+R{G(NmN5N9_rR*%&1Pv4-wA#6kMcd{l5wco#g=IK7eOrCv7eM;H$jv8Ns~ zjk(DOKOvvv^)Hsc7jOHj!-JB^hQGYrqe^2U0SjIb8kUu_7NB-Vm;qbU&_1TNJ<)m# z7zg?QPQLGS8YW^F>BZTFD3?SUHjYPEqbtd$6`O*FZR-dOkLIJft2X74EDE2vf;WC{ zJw&plhik_a9P`0vi*T$%r<9pS>@oV_KH+r{Mb3B-U?fF2I@c`fB{pP)JV{YWsKHPPgW zAm9^~ZRCfmA4LnN-tru%nW}|Y*hhwv_9@~Y*)yd^1!Q0V>1(S7X>)wTvM#ijIKM!MU>J#xni19Pf>AV;G1PG5<%>sM~0V#)q^ zQ#6v`gIL3I6kJcuIdVO@@=f4y61arO11})TbW_`+1$B9VY>tp4d=%BR`=V0Sq7@|_ zH7BcDxa}4Z#33>6L6=Mk*|mIiTS%pegF!o6vO<_`9}ifvBLsvlc(oVK%T_>7E+H@2 zkkIygw-H-7>>8ErFRB6hBc${l-3U0X`ZEUf(Zs*}fr-fU<2d87atQIs5yfRF?bwh8 zE^*Ew8e9mYlH->sBry=0J;IHu`QqKb2>t)(fA;^vYM=_lDKCZZ<`?Rq7-;&Nfv)t-`fEAKY#vUwVUUGxFfkUA1$qN?1({s4 z5hM)9S2P@7N^5f8d4y#VLol?+cF9eacckR(!)?=l`OOg)PiH~2FkXX3UW(YYy!!&E zaT7f45UwIFDE^1$@>0Vka9-DAXKG>{juJi)>bAKs473Jksj7%>67jmcf4o1T8-B9| zw0;AK{Y*IZ-#WVP6iGI{qWdL$d+YCP&wLpRK>JKkgWVYnQ?Ew0Vg(%6{N_3Rvv1({ zCNCi4ty@{n-G?BUcTs{VDSvJZR-ZhA3j$rZiYBo)Ua(MS9sH+W@mAu~f*;Ou32FjH0BOQ*HsBMo!sRHi*$%f zxw(4r{<%H;vD86n|4fHgxce9gcZ;x;6F%w)6!4o!{9kfH*OfDfIoKr+q3O3f6{0bp z(h};I;t*C2$dXsdXdS|c8@?gqDe6tAaZEP}u8_Kd#wJCTn1|JvzDZ0XjGe|ART81fGl0Nzv$6dSy@c{OAoU58a5z z#37x{()W^7zQVnP4>0$aFCf6lk-stWlCaQYKO6HpJU_!dc6*&TMlPCEMnV{ zIZedzp?^spe67a&M|%}_coivST8++X!o6Sx=O0VI8;sE9%`8;ht@mwn47a;*m#s_% zFkvBu_iwevUzwzdH#|I^6s6AvjIYV75jQQQU8!x7035$3vos)Yu`sOKw_(KJ!2!NlHsi}ULA>58tY#W% z0T->fV+!%;loyw3{FOmWdAg6DQxS*Bga89$YvMK|b`WZJ+}?YPTgVs^(gIEuSMe|< zqsb6Ac4{v!*ju7E8NXe3@D#-0Yj1yDN6ihe#Ygn`ksSejjvL-pGK%0DX~Tj4f-nkn z@xi^sCInpO*0wNY*^udsXeUoc%LgfjDpiwuofJ{fm&MVF&_R$ofh0i&#NZdTx(yoT zqpVueOTh6x!Ms6$YD87fL*=TpVR#u!I;oJB+EV^DMi+YSGNmFk6T`?0PY5lAvKZJwuWEJ?d=-nb_DR-{3|QcZ0{THSkfF@zL|MRz zcgyEUP%n>WO#00ly8|8Veh}D$J%gR1VNfJu9I>~utEzp-b9kao9riP93<2y^FOgxG9l|^DNF+#`K0Gg!q+L<> z39&&*#@+d;9~&I63p@HGR-E12gt%Ahep|tlcM0g$lCB7i%bE$D7?c8nSc6xm^7_+G zxVtqw?T1VQ*!h3VMCgg{4qsp4+y~U?J&rxY1R}RwEhogITBwiDYq}Zpo;njy(LFAY z#=NJbUQ^254PYv|cIU@yUsqyl&)4eUB+( z4Y)1tcNKhQ4k3 z*2gQu|MD-}crh?)K1Mkj>;Mhz$ud+SISUrBodaeoX4_fwL2m6mJx=VCGW}e?P5gl8 ztcaa8hhsbIFmZUJ4Rw#+;1sbHkamb6y>B=5vI*ZOj09_xH=?Y}!sF1^&@2fM6BET; zMNg%JrP6Q(D;Qd1u2yLXocblVGUE$AL+!^U|5%2xXZ3~|gVY?J-VHd3my;nzR5XajbqwoX*hQ4;mAz z9+~!+PdZ1*slC03y+Yc94a_kKgQWdkeWpku z5P*d`Z3c6hsJ#kgwi1KExT}%fe!`%TPMS=ViJEKdSkI9l7CFDL{9qqZ5S1ilET0p> z{X1q+VLkH{xJ;)9w8a=;h?8_|DJobJ>fk0hKbOq^taunB|O2k>O>BZ z+S^9b_u=A!s!(^rE|A+eK`Z!4l1Pk%b7aqSfR#qeE7W+hXqR3r2T1}W{mYT@_z8F7 zZzZoZY5_>z9Xmxb>;M1m_xIrV`O9(F#K}lULBwXVQAi^y8Q%zgqdsB`mEu+yzMQc{ zkB}LzV7I9wEMmZC%Jhp8`#cb3VYld1ntm4X=aJ5LfxfUHB9%wovcwI)V7!d5vWN}p z>OCepBI}T5D&p0JM_iJ0$j1b(B=JrW2@%)YhbY`}s zMI>uFf=(yri7UtV=G%1@*hf*mgiyFcuXb6J+6^W=QLIW9q;CChZtC?J#OnZc+ozNn z?2#`>bw$Z+P!4ioQQk+iwSG|}Z8-3&xuDvQR!_z$JqK(KH{gOg#xX5<_0wGfj|6aj z9nb>JseJT$&__;vA=GZdBrX`{5Pq8$|~(t*9jAk9KPFSnq=J zNLn4?>{r;max#QDvHorB77R|>RlKLw&}9KXS5|Yp48MOk1L>Q zBrI3z7(^+wy)}h!MI>o(e3^svD)V^|H=k1hOV zWK79@aGinGlSJ&8yo|O1!XQySQw}4ynTa6rC}|+FN5JlXlWm&9847UJ@Z26%xcxk= zA#Nm3>V3fe`-dODk&MOaE)P&AO-|s;Ft*}}xr{eSJW0Z?j$UN4%J$ZrCqu`EVW+1x z!tS}S^Pknxa#kf8AJAlmwuUZ`!jeDrfsc$e{KIjS{@cCLvoqnEty26gm%4U1Vff!=468E>8lA)Pj!<6E zbtGv!?3D|5w!r#Nn2P0HkJ&tdnEp&U(nvpDAe<9CHeY*OYq{Cjij;cD(3;>P!N2Y# zCGIK(6=QL9nrJM{vR;^fv&rFm9MFPpA8D{_$iy=fLD#Z(G^T8I=K(l;62QH0`jzTF zuC_r`F8V1=k}K^8aOsSs6_sv;jGJY+9Sh=iJC`1m0~d@sITVxL7ok6&xo#^<9)Qx5 zq?L>)gH_;wWQfiIS6N5~-w8k((jXj$UZ??qF>TWY{P7ECIoIK_C3^_whpTTHQ^F{k zgkF@6x;6^&8Q1t3f8A|PS#O6?qA_C)=vRYI-qID=_%$tBs(bOvB2Sc5fWg1^Kg*Db zbL_T5vDDaz8~T@{L;v$&(f`|j`{>ptYEL4`c-Nr!JELe!4wJ5OYw!sO(`}+fxzp%^uH95?I$q%(K(5 z-+GlshN06>^W_y12;mpKe~;Ng00y~iGVg};dQ;_IfLng$(@NT4A&r4D<=&7$iPs+H z?fSWv1o~pI9JNyli90qR2jj`|mPanq&am|%nH0Z$i}aKafRO3n05X9vcg|!SY`=aH z-~b(&@|0|dqBnWge);?#wc-5PchV0d>2V|t5SJc?Kjfq8A!AXa%&ZTLS5Dh$l9Gh( zyCGl$W1tX&3bLK?dhr?qhNh_b4{`W)bv)@s1?LxV$Cmgn(=^GbU>;<%0c%WUILq0F zlL!Q@Nj7^_&8JXRbYPDXmpPf*ob|29)IWOy$oIN3no%e1sC1y;lNr`$?9yjaQ_$FY z8q8{PSSIi3HZu90SeS?a()iZ$8$Nv*&b;~t_}=~i%z;lWF+Y1N9mD@FrDjTbB!!Pv zKDQ_O32|#vn`JfsV#?!_E8EQ}l7F!&G3n%Your+3Cb&GJVI_+Pzy`(`=r3S|4hje&z;&43N2ek z?aYkkjG1+KWEq%F%@&i52ul%fm)lLbT~}Qu0~`Nix9J!CT(5$dCMDL7k}cOlV$8bA z1DFFA2DBxkpLQLj=BP}ny@X76IH9K4fgYVq%=6Le&qlJUNHJ=2t@S&ieV#81XVVU& zkUIuF?TJC<8*LN2d_7;hd^b$9MPj}M*gNSwRK1d`?MQvCYdcXu1+cs+*u3Uz>yoZ@{rt`Vu&8gF=x{1NLGvnLNX(J0XYuKJ! zPs$Sbs}N{A!NH;LuqkGtHjJbDHKp2ucs6zuP<~?(BWbJjGhHyWo(JC2c!x#2+e|VH z=_gnIR^zY*p@Eg?4ZbaUv8;;#c5Y>3*#1xvtf>v)$%%Yvc5jlN*oyup4tlX=4Kl z@|t{k%UbVh3>vsn$0{w~^W4zw`uKkapuQftP$eY1|KHS}^X1p=?DBc&kO_qPMwSYZ zD*ye+8#2OL0Be9Kr>!@#aL@B$Zr(}!X^>z^kAM8c6dd&WbBItG&aHb9nDdtvxc{E1 z#JWM|vJL99m0t0mfZ!I4l9>rN6m zP^uT1?Fl0Oef>oY5p5^k9ZF-EuM+-~Cb7S+|0*MyN!&XiRgy01Wr1NaOmx7aq|PKD zoI2T!1KmHYwhLd~1|M^~a@(vtLBVmDe#yf6T%%6Sli*43I8HQ zbjclmte1*zC08?bhY||0!R!xna{}>T2oWszKiGTkxT?;rTNpKHBAP@+P_S2!WyaQ4uUO1rY@4O}cbV1e7jSq$vnU6X_kkF_-3?yyxV;-~Gxh zzu$fLABI@=UTf{=na`YKjxpxtCQ{^?9C^0Es)^CB{(tEop=eUBMlqDw4s0yqvo#*F z7vqf?2>qkZCLE@AL|OCs#?)`%Uphyi!f@t3g}qAr@Rnx4?TnKgY!9^}GfIm){t6)y z7c|^3qH4p~AOGq)lMwFxpgq)Y0qj{)tLnPG^d|n^f0$_cek7^b2Iz#eNi>#a8+tP2 zRG82Xq{tq4RN%e_2O{v-4v^0%w+EmYxF$1}HdpVYR~|uSMD%Z>@lWiwDzwGPac*xD zn^5MDqV?zZ6KdthPq_lgW1>SEh~*BYP8ml}{=c{q{C61pD%$wI!g(aRJZ0nG+vk`k z+yrIeTpd|FxRd|4I-fM^bJN3Ldn!0fOvIon+V11?Z`QzulNlS_qEaI`oFRp{%*=iu zX8>pm$z`?astwgcj3p{`y8l2K|6BEt!&?BJEkqFrM(`kVGb#O9o3KQstG=pHS6+O% z_BB@TJ&1)N>WZoCCyVF`;22(-kf;BZj_06pxaPsez3+Y%%pPqvG0CnR?7OJ}ri;G)sem35*Tg|P+EsHbW-dXZ z-D5yo?h6PN5^g%$%$f7?DUzou#^_!;=4*TDKesCQ(}~wJxqw`N6XFLI`JPwKXN35A z?z_(h4@m(Ejn38ShgrtiTL~l|qk{LeAJd+{y@Ub-=?x(f70ene?~YHqZBA!$umtZ@ zKBir5G)M1GlFs+c*NR6X{eSRJ5lW}Y$hagL5lS@n+T=zxpl_R zRatz64ex4wsF{mM_G%S)a?O9klN0Mb);{w=@Na2xMG=^K(J>ygz1EGx-b9HZ-T|sP zKF}$0?qAz9lBpl~0($;bx&L!=M#qEAyIuaDD~3CP_i@$?9>v0;*V5qszv|_xPGn#1 z#JZtw0Qp;x*9F=pDO-3ah@FO9c(WaOO`kA`j5G_t5jl!R8Z+epYhI8nSE7q(qyfU; zs@&XM=CA3%OycQaxqL9%RKx^`HX8wmOGgG7kDs`PcY?Q8v$v+%_0^N_=8?O~0W?Sw zn^+N3uG`bhq-7!|YL?w^wh1hWd5$RaB~a z$rB00Kz&NONyXf4QH#stIirFxDT%u={bv_$hUEK2jsftV=LfY)=ml~%PRjT9DKE9k zK$hEWt}%1$vmNqgfm`1onh>NEll*)sY#wN1a$`mGM}AE7LEZ z(ILS=Vy`rc$siOkwz0U)pB`(KvaK;3$$JgAvU3q}1ISQ-`-9M%u(z$vbN z>QH2YS;e?v23lqut3kNTC5-3>O+oMqXbS)dJJ;oLWT!O&xgs5;QCsTNW30m`iTcWw z$#1o(V}SZKhm-Se55W7mdlkDp- zd1Ek4syL+Q+ebk`HH zVO>8D4geli%tXbwCF19OQlV5HR@WG+H`S5GLEo(X-pL$4vvk9$J}@7qZX6Tqc-%dK zpqgl+4FS=OnckV{xJv8n;4!Ns4zk9nWD*9u86I&?GRsSp9dA+*dq^6nKBO?NY*XS} z-UVwscKR{Q>D!PW2BF=}a-7CDlRCY+aLMMQ^_^@b$nrK%{AM{WtNe)7UPUVv0gy)< zD*pR9kIPvdw!}s2c#nl|=@MbJr`DWnFBW(+aGU6W9B>bYX2OqdUdC&}`;E#}nge)` zH=HnyWGTR~^ig=-@e%_&pk|iNt8;BoW0p)W*J-xNna~oE^0!y6B>kR=A9l^TfQrS0 zktL3GtV=9Y)}MNa3?~<=L7|6Rf7&-Zkz6(>J2JA4+;eh~zY9DRxA}B%0L-(Yp}m#l zOpVnWX;#sD1+|V*aMQfQ<8@XvB~afAwd~W|%B{V>ENno^TRTkd;osP^f3R_mWE&-6 z>-?A*1Ty`uFemqrocr7l<-@qYuaJu65Ips?IJTATH5I|lKw%$`q8Y{G2+XIZ>47r0 zyZU&$p2o08z~q^!OiRhz)3&~CVNREo#`H86uQGX+9I2OXN-P|4Av^ey>a(yvkr}&`m+M8!E8O3ZvkCn?MIE2A9I3dOGf6>gg%GcN^;YNq)JQa zCJK3E0>)_k&aZInk@hRrqib31tJG5itl?2~j;XYBT~2FO^;gRLJ6X|?X4e`p= zS~tcbw{(qEl(Ramokt(W&4p^=e`@9SA{4`d~3d?@$Yl;liEVIu34FCX9#lrGsLJ`N_QX%6y5v<_Qr43-CcJv1leFp!iqT1lCkHfGq z)G#`D-;J1>iRAG%oSI^`wx!swuk*;*+@Fbr~F^d=FaJvnehzi zH9|v9R=lLWIKdN{;23c~U7>}Sy}Y-=R6Vt$R|OuX35JgzuqmrHgkCsSaqleEM_xUA z%JZB!2V}?n2e815J=)*-jXShuW^Hv>e&jieg#Mx2iCsvw$eOgmHM81 zn9M5@`d!3cO{EJYHHFnhnkgMyO|vdAvXdXYM{Xi&v0ueIT2{P3J9#M>>pu~FKqV>c z_?}6vqJ?BjH2G0h>IOkXG_Q#0-4Rv zK~&2}PNIwP27REfnO)Svitsw6_n74;VDAt$xVJTJYq;HOV3r2SSYq!7obDcb=8sY2 zD-ZwyJY!%tZ&zW``$@EyPd=?DYef=gQ@AahlsWZnWR#f)8Z#m!Eury+E#z}D{6o0q zqkBgsat1?$WU`}5(x;LvM-GbJKjyT-9W0EOHgySR11DZdnM4MCicE;Me{}R>j#(V? z8k5VI`Y7`#JT`p_%-o0mY((14IK)UT(K~b{v)cJGDmA9_4^>FLXZEV&qm%)gZK98j zlPCz*?!0KU_Cag|A%+5C-eh)b_E5pbM@T(kgI{}Q)>+d+y|xVP9WytNw#QHrPCX)m zN}`irQktMqx-Dj!mPYrkM+8vr5?e zZIT?5b7r%i`@_QntRwS&HZM>6`vp5tHFpObVI1{Y1(mB#q;4cJ8#&C?%sg=1tqo#c zALKOr^Y>a+_xxJE+PyN@KHy}pX$K^zZXGHSz>4baK$JSMrOIUf`Ia*#S zH8aV>UZs+XOvlDr^7uNd%?{9y(OjP8EOiNJKHyjNcd#0Gf$E$+Z35MtH{Euvy%fn+CZM zY)p3?&`4N1WB@$OZri>RE?i`2V|t?Jv5*Rmy~vP*rD9#t(brAW<+s&TrKAf`yF~vM z5vGX~Tyq5R&<8}8)$VXb+-8*VRwH#|`Sl4p3rP@wzcA#-XJpqf+oc^~Y|-@f`+2`{ zgVRH!(ROCZ@9qWpt|*jBYxx073zhWK=^ded%^0s0*U?b8oAR&3*n#q5!;VcmoeRbX zhxVh_lK9Cu6V&zh*Y?&~^(bBGL($R zG%N@Olu^pEFv^2#9FErDhsgm+&~Fu);-mob`VC zK2-k}i5Yumpr|`ws#@1IjH#?Yk;jUQm_+IVm-!l6(<4cgLfTG^psSkL~mS zaJFF@Mmn$3KO7MadCao0;g{owA}1{BkaTsg&h1#O(6RbhM{f-=P>AvOAp>~?`Crp` zlV~f$bmP?*U2YoGAsJv0DlY zI6F6tbc!^bzkT#wg^Y$##O;z<^NUl)>qDCwP@A~|=04+;F@LX0m8pi%RL>F7fyWot ziAXq+A#S3TBHydKw#PL`Gwx-SzlnH-tWWci-u>x5&res6?Me2W9C&4GSNzsHzsM%b z+R=PTe$rV25^HxJ?9|BGaA``Ic^0jGD`T=0ZK8$7Sli29kw{(L&rDe!TCQqA45Sg}rPB}KMuq&9i;W>(wT%r=)%kz$z~ zd9&!p1IqO$PVHCPa6#tz9)*Q5MZz-00u`s^6fERpgnf$YFMih-;3a==;^b4=W!Kqe zZ)wh+wKn0}vzBS8bNyM;jX%!=H^qMfW=*!4|RafJLQe zooVqL-V#>tSQ9BMV3!h|0AS@N+UZZD@6T=hy;s*UWS&mj>Vb_zT;)I~yGQ*swh9kk zLIz;zn^4l-)tc5+eLPHo&n?6&?~pxmJUtv8g{w-A{=s5@$04b|)AB-haBGiiO(eHS zMbn8{k~efg{48j!aBkA^Oy_w3>MzyuwI%Vp;Iru<@Qb2O#?F?{k8>(KCcR>}6DZ?WROU+weo8L6iHOz()c zT5B$`Hxg{0jZqe0jfam+T7i_W?h7b0&6>-TEys1t)9g&hK*kwYfs~hx6}#*s>{>s- zEynzFUy-d{hp@hX6~9NxOoi36Ny>gqFd%M4`L-*~hjM065on#xnbMsKuV|L9bLt$Y zxkt6hd9-!9eV+;rzb=TXkcq$Log<_jm0}*76U@qU6|U=?-k6YRnHqE+XJGZw8I+l9 z7~P~T;(#k-6G+8m;Inb4L&EkpC8D=`w(BxlDs06vdOkf+(VZfWY2xv5%wegF?Zs$f zUg3FeS}DX(djz*N7b>HPL$c4}WVhb$&}aC^w2=Apqs9LC-P##*JY0bzpk!H(Gge5) z@_!%Gg0G!=nm=4P*kZbf8|ht(^o@-uj{c6+YO8={TG>8}ihHf%Ib?0A=?H@;L19%q zRI1uc5{pG@w>OPfr_QHYhQc=ZeV8Mdzo**OU@D@h6bxDuEeMQ-2zV|##ol%A`1+~F zkRfu*b9-pRlPf7*aVWnN#5%!2?p=Vdf5&m>Q&o(VD)Ct+xbPeNvf=i}K@NOxJ38Jz zMyzpxaVR?ys&bz89^oTfMr0ap7#D~|)tWhu{;|6HsC>;l0O>j+Qy`q5h$&PHgRuU@ zd(3P!eNVW<>C0kqk!imww5xYy%KM9CSxT(Ltge$kAV6t`uj(>H z{=gm?6~%c^661J&XXphFjZu%VkxVkNQI>U-#}@_p`nuPVk&*LZV9R3<3F%Agprk8g zjb&widr>W))??uV77IS_w5|;`L?GrhNv=BSLJj1Jwe@&{MipV)5d-sPZPC*6G96!Z zsMR;r97n8r+2dH%^h4%?Tod%7 z#i1ZCq26i zCWPGF1ww|@&rp0Nm(k%Vnw)KXCuFyIm|4eO-?7p;O7}xAK5){IQ@SB0FDhBFu*}!E z+EVLwY?#sPNi;bX>8tG!6DgMeVJh_U;fWql4Ky@AMYxy4)Io!bRJ_K$yC2*AG^C~X z*!bgEtt?;I(VpP3T_YW!M~7hN8ivei$L-ZV-u*UAEw~8#XlU>msPdj z-?5IQ`Rp#JAZl{<&i}mAwl*}Zq{4%>gQXISw)>!E`YWIBor`%#E8pmATHJl|RMo<6 zOTxBW-?{SD$=RjbgW2jrcBX6vAydK)^~>QN_Z~@$HcwBpnMl~0Mvw_~)L@>KXcaP2 zd9~N|*tVUU-Bhx74sZ+^nH|d#Jy;u>!+r6HsTo(W35E3TtNMiksYR>24{=JOExH9R zni-#g3OxP{{A;OiaGpe);~kdOUMWA636nUXdsfOI%Zd3(p)FRIztuyyDbY|371OL- zn1D{rnf9g*2?_sOHC31G)Jy6ja=JNfrWI9%HYli^i>TWFHnl?X;qXwU;@X}G-m=n! z8bhNr8%i!~LOUeojrQE_+{F6f7$$k-4l4&Ab&XnEV>Z-q>&dN?2mRm57S^Hec{sGi zW0is2-Yh)wb0e`8+k_hc&R3~+X{GG$30CO{Xtw$0%d{*O;a@r25;(qq(}9P-{j=NQ z{y*s_=_OoXGAXO5*k{-g-!&0E~b`7*@ej!pwSh+p3fsrbVuY^|cWJn1M zi?Ar^UrZ8SZDKrmDX&)pP-Wb%g+xOA9Y5G%vKX~&RP=VxEIdhyREkwU(@qL5h zfSX!u)>kk?S2h>yg@g`_Vkx}p2<=In16SfB0=u5#G$5dy->55#tK+*e(2#jbP_1+~ zlidp6IPsJ`-{Lns_H?TSfLvpCB}d$v_8AQ44^fN4XjM zTy+R=7*?;&m4me1$G}0f!BVL@rX85s)3QvWva!9Sxr3PHv4|$FM5nd+s;{|#v;G>= z?J$uZ>NRMZCaZ|JH*JZEUcC9i`F-}Fm|HovlkC5HihQb@XBf)dlR-EOJ1;(@r4jl} zZ9c}8g7%905+aq;8-RR?EcJKgVlZOxMN}!N^6*&Pi5v+Q%#VQ73$iVsn<8(xvwEW; zCtdJe>yep6Z6=vYl71CZ@ztFs*YC}CAxJ23&FT5e82(mn7CxoU__sz;`f=lo35-Tt7N!EJ%V$;YG%(turlqx##3DqSQ1>NP zI?SzdUZ81y+iJuXvEXcBWe6nf>hI12>yV$EVOTQW?kxD!<3=$)>7_@$$I>A?tb`X` zmyi`Eu4w=r;^|9E7?@o%j!oOd)?X~O%MN_DW-L%wWG!Y_dXGK-UARR1uEs!B$k_+K zfiEm@A)81%&LQj3AKz$6)p|?sMsj0CR$zL)hJ2>=d-uYg@s>WmLv<$751&- zpjswW+ku&`e+o=Ub+$}WHW5Q%qlPLGS4VP_r{!aYy!EGpuDU`zrQWRnmzz8fvE&Po zEOfC*uM-r1QeEHcDTFLUie-!vLA2n`j0I%9+ir%`LkgxO_?(dAi>dt3#9T|U7rqGL zcpAk((tv3B;!54PyhXAr7Ev(;`x8k*Z05$rQHu;GDF5jkwq?-3eydAKV_%B z`#ei}Nu=Q;Q8;%zr9p>oJFCO}RaI1$QrIA+A!C#SB!DZa1hI`_W8sQR^ZJbCu<>8^ zXTW`4v}1&pq_o8*e1!@hkqNky54vo@akvC~^&x z(ASWYhNwXUHOYt^O1_2$TXr^iX>4FNi9(pl5=TxJVdDsl`G&|Rz5@jGj2A5(G1&5q zkrye7D4bvfP?oheAd{fuJe3*UK^jvFAsMuDIHA!*BT3bO+UikE3Cd*NxbPrEp5Z@| z&5TH16DIp94)!AIWhAWmPztGKw9GoXgEGtdJ9C`p$0NBSLuW3sV~G|oR8_ms?{}8li40dp^25D6rGO?pdR;8Wp-?C6M7jQzEFS{!FqChj7sG3}R7pie zg?6{h6x6QH7b|bConOAJV<*wp-T=1Yyzl6xa?-HBmu1aNofNW(S9S^r@AWw?~ zOZ3;ScXk>Lu}J@nOyk*NIZo|`w#0o_AE&P6-VV%&ZtTv%lMGxD_xj(@b&1D;O?P1O zL{rTEJlPppzouE5z66PYk?z#oAE5)|=$@fUGO?&X|Hndg=O=Ii-yjsIJhP8U^=hc&IC`z)HLzAZ^ zK1D1XbjWJ_a+0#g(0t{CE1~=4iX6mL^~Sau_NH0Vee|Rh)!HOp&iiSsZ;QeNci}U}JPlQ5QOS z;1eptWnz{F96S=x?s2G@AzrDSnHID8l=-X)R9($8c7BQRz6woXyHGTrl@!yl6j6VQvY>n;&G#_uF zAQA=Y+IOYf}k;l^3s2f37lSUahWz(_i}SZNn0@j)7XGCSXGnUv7<7{@)b{br0L40Y*1Q!@UKECyb<*L^Fa0!^ zL-Pd?Wq;P41heN;xTHYJkHbag)sANgaxp_N4NHi2EwSV$eOAI7{qp_Xh)u_cr8_Y} zWud_XaTWs}MnNCAz!b2+g=+P4x#$b}TeWznU*eNR4((0@r9jkEeL*ZPz@fdI8Aar@ zK`48|J8h%fyR+@!B&O30eU}?ZH(RyQDUY5IbG~RrIm7d;4~Vb@tE|-?5Xk@~sJFw> zSF0j?UPZtu^sw#qZvnhZTJX`u_cl@V+{@%Tw6Q~Kye?OY($Ko zGup72v6CPW2DPp{ZTNWGNckV%;ixyFD*XcXm3{-;4E!Ms%eGle+pwrYXJ+3(x^)j18H(zmNG7U<*BTp^AzGQupgR|6 z^b-DZ0fZ0NiRQ1Y(8@D$;&a8kk2rH%EbgmPqz>sjqa*l&DYik zf#p}b|2rrz(+XyNE#jn0AS~L#)@BN?V`7%bfCypS9#XxLiNB?EUV8pqnXq+?<~V?_ zp?$iMYWGVR7LiyA4h7%gX_oBpP#2oBLx$=%5e|Z_wGrt_lw7+j2wr$*!?#&k*!^&P zLjF?^AGiluG_Ly=hX6-*W!!QPbKrAqM`lP6#ci7OdT|z7G3iBPdoUR@!LNm|oa$mN zqovr^9W!^*Gs$PfJ2rNPO`4Ymi*CbM&o@RY=$3cbGA6o`TqC`7Iru0R+Xh| zF1#XxYqU>HK3~M+Q1vW0>u2iC%4rT{gV9}7RaM2)1_%Dt1LB9{osbStS6}tdJH9@7 z*E9^OI5gKp*nP;|BTQpeyIENZ<(NQtG4?Efc%{raqbYArME6tvq8S0e5xcXTW~Mh5 z!1CNv6q&Nxe7NL?J^;%QDq|f8DyP$C-J4*3 zIa_EdDM{l$zn_L`#zWVEG~gOau!P5BbNWMgV_*B01MJ?+r12o29D-RNZr zyc9XVBbr>^dH4Vu=}~ssUkIg>hw6gKQ~8H~tN{-u(<^-}zx}W}``Px0iDGP_-Q2F` zAzmj7`Wa6H;ETsPGgCU%37(Qyr55lNYC|B|Hna1uy2+Z1q!3(bD&m zP0`f%Q?1g!D3}s`fqPc=*}%}&MvRJ(_xHL7Kb+x4oJWf1Tn+|G^0|&jOLQhBgqIcs zN%9>_t26{9T5f!bU%bY=cz@c89b4`ssASvf*Qun~p2@&3J0pr>o4iAZv~?GJRp`82 zgss55{s;bahXH$#?yZ{dvTuQ%p4utV{`Ao^1)KXi9vo|#vhB@pYY@E^Jh5WsoXZP9 zkBn422g4xmBaeREW}*gmqKtk2;TK^62Qz0Ju{32@I9R@Lp0#MVcxlzuxCIPlKsvH_ z2q=ZOm@DqZ{u7koZWt6SPSrSdv7=Mo8eKpiIUk)lzpm||8LzZQr)rz z8pl5P-<$0Ca7n+PueQl>+$VNg(Hq6f#g^yu){8Xm(rxz#D^#G_-)7yWW9~43o0;|x zb?T6xQSe!>@!9JR_No-z0iKo((&V%aV@PdJ?Tinp3+p0_ZLW`tmUSRROo$Awgw}=I+$RT#o0Efdv zZoT}2-N~vi+(Zv9D9E>sWzkuVi9qr+Rw$%|poJkC$dRz7QKZgf2~CIY+3yR=lsXLz zgq)wMTGdt`jSo9FJXKvEB9AhmdzD>us_g3loZRxQ&*yP-)JNSZ4;h3cr~AZodm1yz z#d$7~@F&4XDwfm%T){T@>>`>x`Cyy2UV?s34>K;)xGZ{K%sXas{(Rg)Ij8ddoAnE^ zb830n*BzCJfB!s-i+Aeq<|&)8?O!(F!p~^M=YwNQW=XG6t+=!~Q4gZA7lEkWv;B$a zuencOwRjuVR%*5DnSY2%M%`KrxzVB)?@tnG^qLHBf15}y>a z^WaLk*-E$GoimfnFs+!q;6Y-!!r>}t4jy?4*D6E%`= zJ@c>S9qL+kBDL-EsuPC!*MvR=hlt&}H}mFLFl9}NBLXRJf=b4ARuDh4-gDY&eVk@! zYurj((*jJR4F$BPjE%PB3OT{@cuv8DgsO8~QQpOS46M4VD>^Yjb^joI zGfO&PnsDo3$H83thWsM)VW?9#J5B2KH1Wk|Su0mrRG62f)a6y?OeYWb#&Vmo3v4#YAdTBaSk&Q{Ku|tZcCa-@OMxV`cS8t|RMaZe_ z38xuO4{NyLXEb1+<%2R}lcfZ@#l6Hjv)c9)JP%8;_rF}OaJ=}(!fkfoAeWkg*Q_>e z*Ayk%lhic1NmBgtvu=wnawWt>K=Zd9U}YD{P4R>!9g|w=j}>~kqGVV5gVDixQZFS_ zFMmq=*l*j|wS52x?ftNaz7A%)txclK;&i4AZ;1<@r9?3e_8&4ek`A2;6CFSOmU|Pl zA4yfrDB&~9ypUyhZsmFDBQu$W;g%lpb?fp5Crx^@^xi6nR!ujUhIl26^ABwc3eh_d zylVYfiD9)OEj_opE7#wR0Qt-UrX&;H%i>M)ubFL>JXo2(O}C?DeQ5TuhVaP1H97Hv zmV99Y*MuA*8e7#xDB~Q?&TG$!(^1|rRXpZCPu^n6JMU}&jwWH3W1;qo{VeMrm)LeC z>70u%Jm$AO=2gGOVMY1lo~Em6t-bJz8S@MGbEqdx5H^so{-Sb8Vb5R z{hN2YsHw(*JEay;QBlGA#rxyrp9lNvpC->#AQKxiZu_Btxd{hB8VgOJ!~;N^RGfJ- z;lH>Mg7U=eYG%_9S=%b!51gJ%vfQy|v!s9GHICqgTP;e~Y5any6k(rk(w^qpJr;gM zdFjn-S}6=Zg)%l6W1`67TV_JY+OI~s*5lDYTkc1vsi+TH##Kp|yN1U+T9Ykvd4=r+N?bsHGarh*wBtYUzXBG? zL2fC-mysWNcZwTn4DVD~X*OE7u*K_@ReN%%ev^EKPNdV=Qqi07QlmO?yuwGa+8g|2iez~T1Xx&%DLs;ysS^geNmzHO|^PSjc&aI5fvxjZquEfN=$nwtBjvc zOc*FVlwIZ&7cS-~*}b>ZHbiiTq!UX5eBkbI`q?yPHnQmKH>q|=JXj1Bp74!gQJoZ) zrn9Y152IX5w);GsJVG;v6wje!6sWG*HE@BiY0SRJT`2NCImgRtuV`rhsIM`7PcmOv zLmPkR^@=Pkk_$)10@~+)_DVw=gMX_xL6afzwe80+IJapg*21xPkX*#?a3XtonO=K} zi`%M>XM9BM*N12E7HV@BjGBx3$?FBXt@93%&E2%_nQ9)$ddmo?7`8=QhsTOEkiN-e z4g{=d1w~(KJ%Ce(Q74Va6nbB}A}bmlT9xC@p(1Z>w15_5UU zn|tM)##(2zvWvd^?Dd9V3M#6URaJ$yPLa(X4Ajr+?Lq-B8K1{x?8$!%pZR$0#<~|j zz<(v0hx&MLw~gE-2_s<_ZvYC>ShKV(v3siP)|=J0=gplrrkL(2DS*>N-}u%Z7<|XS z-<$0z7;>zzReFx>?JzZ>I?!CfAhh|^`Ld{#K#y=a92af^ACy6M+KlFuDm5GQ--Ss{ z8A^Z{Q~QUnVbLH|0s1xPi8`o<%Hakf$^8apZ9fOu<$t<>Od|KqVLi@TfpbP_o1e~O zkrrIQ#SUsA#w^R+T$%CFY`0Qj)`M?G7SGCFTu1dTx^2aRa{-ChniORTH#Sm5!+7}G zqQ@s>C4x)1b`7n4>iz)=8I`F=Ijeq)pG3f9y)f1u5YW}As#s<=@-QP`&s>6|#ywLc zRN+3yj`pIP3+v35`B^pbyLVU|5o!BkeD1Yp_}UwXk!#Q#V*J&$G>aMWEZJMvvK`jD z?{3S{`_vk!Su#dn4He-ucPb(g7a=_-ULEZ8i2Du6nz8Ig!} zS{NF+MfaKW;0x^+gB>&e)*}sz{LFmo?(UpRkO5vFt}zw$fBp2o!_(kL7i$M2?Ls*! ziLxWg0H6`|=8@7yb)yj7R1?{evl!Rf-%3~^1pSXIYw^D?8COsiS4`RnBS5MdxTYHh zB;}!FvqK+gz2{8^D15Sv>8yRUN75n7(`4P8+ zmAylTw3z~2I@b@{6&&ei*S(2H`??=7_6h$taC>D5zeJv9Rz$Dha{I^3Ygy*c@Ox|i z^3zSm(Qe_^^Y5Nr0tT2^3VoFJup^oC0)o8fppCLc58~7UJboTArtmdzrg;=3@n#Jr zi=tXSbu>`C)}ylDzL^STe#UOtFdD*!jf z!z0^EOq_W;7SH9gvbS5%C}>&>{!G7?g{8m~W#kx;Wex&@SJ-~I6H@Y(-l7j!;|MVf z7^{Du?7@)H*S8#%JF%MSbv#tVOKi?$4MZuNMhkq23wlHrrZj;s z4+q~ev@y2mpJc8Fqw@qt?` zxoZ!0ciUd)xL=4QZ_=Gb`sphjfB^~`0iZX~$n)Vb!nLTIyJ75$vq_-+w>T3(#9u+J zeE$K(z__#83T4P%@N#Y|US_{hnTNg8UIDXw{h9zWzcs|g@aUh!Cd7+p2}nE4>-PG; z`zc1we-?*#UY&f)SEHk6oQ8k;KK?EE^>={Szy6- zTmmTIg`=tLvBu7x8am>piC)_WvFAZF8;Hl{4sU?GLZO@$K-1hH1L30{%`S)|<^2_q zG2Lgp6IeKqF^SwjYxD)I{#wweyiPnflC|XFTj6%uw-#)m)%vrf@SH-|FgTto$gY>zhgGO+FU==6!qb0Bv-t$_|Px3t!LXWA@fdz z3qS@R^Sen#zHzt*=Yk#JCNiK+DFEGLKl{KvLKT`#K&|8wC1*-Xv66kciYWymaV8lB zBDYLZnO|}%l8=X*j)~|Oc!mqUx3%eP%K@Fy91y9ne{VPI)S6Z*lv# z(0(;C^6f1!RrYQ9z~iNFnB~LJ(~kF%WCQdk%^-a^tf}{Af@8Ba76b$}eJ_<}bggdS z+Y;F)h^N4HknnZq8_-aOAJimX7dN#hgF&EW=0_rhsxj!oQDyM$m-gKp{HfCoP-_H1 zk4k$An`~-Fsv_N$_E^HSZ{|J)n%zRi;25Hu3neR+BT8oY3x^qx5f+XXbeY}F!A(8E z!%n7Qt}shcU=F=6ohH{l%wHFfM7x^0y3N4t3$%5T&??V|E5aUqz|HK$)BrywkK`^{ z$FPy?stBBCF__P6nJ3F4jE~~JF1`+`B&eKTTt}H!0)D!9;J<$QU;GR>T3}jwjkNOd z;Pd1ozvlp#+Kr|?5X4DDC-`~GF2{kR5UNt)uoPe0hYGkkEuzL`G`z0*LWCT6iMa?e zq%Kh#s4+&?BP}E@f_l$WEDCFG$Ja0tkEGyi9Axn}S31bji3wn2t4(d-Xo_yq@ru8lHOIZXe1!q4oQ~Cv^ESJEK)mNQmO8bS^je<`0>+8x5RaylEXZ-eSXuGF0`ke2Qz&~>HzE;F;@LWk z&h6^M`IZmKXdL&(FDHiA6;dkII(K(mF9k(xHyL?Qzo8zilGjjs(SafFFN4oI+k;f> zCI}{{A9Wa>%Aq0Fh-V4E3Hw;BKnCXg>(;qC%SXWg^kwZ=JZ|nt1tQI6>m$!ISoQfE zRLNQ4K0JR1EtiWJ940BXXDP4KpOr;6We43?Fhrh})^FCr)ki0M1zn=S^BNYz6)vz+ z)9=lFg2*YrUN4^w`U1_UYN4rP)bQpZ(%!RXEtLJ#V-qLlDN!7U1KVIYyp%DZSS7Iw zvza(B21wZxE5oB~L-hAJvl!+J<2VoVuV#F&5&k}T%0w;I1G%m!y__C}tHxd=2_=nn zNrp<5y|{}&$9XF)j}K$N^30oU9}pEA>yCnEExFR+8aNX^oirlZ8(TL9&F{1ed8TvO zpLn1n{P`;^j;sKW%&pPoEZ5IMZtYL-wVV^e_lwt$?_95uqJg~6FM*nnaJmt#x)m5S zcZ)1K4x)elY)wamUrD@PejbsK1l1H%`FWq&yxQn=s5=!P)eV)ea3KUTL}3;{jeW4kuSxX`r)5ZN9Kmox*#b zzPW9Tg~QUX;4HKbyk>KKfj-4Grbq;1EF?1D&Ni;vB2s(CquWcP2S!bN=<0Kk?|;fe z%e`4i<-5Me1kWa;85;XcBP(bzg=iy$(925(Z^>2`@~pA^&~wNzBkIHT!>`-{7oj1Wi33lIBz z@=5mmN2xJuR`vtSz8x2uUoe;({=W0}(+je%k;T4uI!hEf!{$yCn(T$BmLFz~fP!p5 zyfD0^9l!l7^8!ATDau!}Dy`05*5NoNHDw9btpH>;sRpl`ofd9shxazkj9~T~zT@a9 zI>+UNQ3*uX+$B>KZV!8clW%DcrST~Ma5-{QTm`C>rBCy z3;?6~N+2=YG;Sl%@#=ks zL{B69jA2XAFv?peV@_pbwO5{ZVP8blZQ&O*I|99ifo8<*(%BB6#THI$b97uCy1Ap| zrJxZta`>OtKo^S6uPiBZt}QvbYb}U6S^DFMu+A56YkVY@`~`A|u956W-!gP20ZUZ| z*Qf`&bE(Dvoic3W3t?T=RkdNykG3=z4l~YulRu-nHu&_-DA$@jDrh{k=Yb2-+dbP7 z&$W2A9cWDh+f|=|*}ep_^!MB2u#5vA(AY|ZmC1E%jUm4+BWFUYXBgZVj3xn3$%Xo# z0WRhdo<=neGl>-Wsi2*7zo*hs-d8iU?@vvdmx*%p6QMk2f~Kl0gVomX7-g9xS~_vn z4&PC+jFE%S<-LdZZ-7e+U+sPmh?By1JoS0pfg$8dL}92cn`=1P4<5XvCIY#%MHs8j z%2g;?qjMV}d`RT5BL<^&Um9AS!VU}Z!OG5+>kEJZW>*%Plkn@wcK{WPD(&${AMUU; z*(=rvfw6aV_>95)?jhLGvJE1BXop0t#gm@@gdZCr8c$(O={KY6z^Qxqa<*N^9@gP< zRz0KmRK$3nbVpjV=+n-cZNI4Y*(cQ8udyC?!99hcbq|lwLB22^5U&UiDqrDw9gorm zjgG=)BK4V_o_x*b4wVR~we&!1=)+^F@;e)oTy0d2IZ`a1J7#5%6~SPTs-*6!;~nel zz{$!D=LTlfFlITw9GsB#zJ9*Oj83md93v!tBm@SM1i>waWZpg5js-ut9`t4>~-QBaQ=`>(V3W4 zO7AY>$+$zY=gsV=v^CE-7m+=5L>DggqAA9$tDldfGZd05S%8?qyCV!8PDanzXv|Ra z-Rh_N1?fPC@7OThCkC5=VF6r{I6_JQ%Sf9sRY{=kl;Tn|N^5&Jy>f-Y9>k_lH;1SYgWI+4h7JlezE9yrn3_PQRi+ zgh0Hs&LmNx9)*1E)3x&J_UsYIWF6vG=@QEHdo$LD!Nc-p_K+5wwO+Xs<)Y#_S~IFQ zpK~svsSYHiBG8N=W~ygu)qbR93Xxl4N&Gs(H!gt$%Ger`T!J&y5P$*;eUivtpvpC`8TGRpd6rVo;GTr-#bG&`HAuDfU{{S$*INs;?e4|x!u1(k zqqL94w6Km5`%ex133C3Mk1ZTH>zmQe+gGgz=6#~%1Mnne_p)#}=2DEss&Ju69ng!3 zV_7J*PZBbN13U-<5Z5Ch>D;>qnYwO`S9#S=W$BQ^4HiT}qtTfMu$<)yYH~ic=$)}M zd*zZhbAYPwTCKi+)#liqA!Yvo06|^+3S56opw84jnVCjw<=XunT}Lxo+-bk%nWTER;@F=4LC;Oj^cXS6pQKGDq~^c{rM zRL_unQNf6GVm`_KIq18$tqRk?f)TQ+#kgKE1f5Kx6?-CuD~tZ_bMZMOJMquwa`RjL zgcu`2Zg|Uv@Jx=MSOrHjs@O>9aGh*$hhe~qG{0Ea#}C95{Lh}+p|{wcoP=?BeBi{D zKKSnQ&LHfOyi-^L(z%4~v|MEqyhhF?ePD^~#AaZ}k!ocEhG#s9n6G4z;ecTug63u{ zRZdEADTEeEJ=|>}J-%jab4z;EAw>t-Jw5l-e%TlqK3$d-&Cn z;g7&%a`RCfT;zGI2jK&6lrKYS#*QnIY{KTgfca5AV)D4a9c!uTda{i8)aad6?ly;C zT&|FJ-q=Rpv34Z)P7MAsoenJDdXjKUsyvEv`b!bx!$eK)m0D3hPy!9)?@Z% z*_Bqx9N`KLH$w$walgKbuA8PrumN6z*zt5NX~@XtuFKR7k@ffy21_ZECbR z1B=3ZwDwYIBi}J9y%=G;hbX zU3sp%XV)%BJEBdD3w$?F^aci-_4)XP_OEn)JL*91{b*MO&W)2aJRq#&76;T}# zEuJexy22xg#>@prqp0oIrKA=ce1oxm^@{vZLH7BzcJZ3kAz|W5>g}(gIAkYBA$};8 zH;zq{pbvU3Ac!=>rI!?uBnZofh$Igyvyx|Q`G3T>L&|xGg7^ml(GL9px%Qs(^H)H zM1ePV?_m`(fBODcbawZUg^NzxBo+++4Pa$R7)b*kB%u9pZc73?LzZux{MDLQs-3j4qewsfRqRJ&j z=g}LZyv4W)19sjYtAb?M;tulvoH=jgE@H(_V#$)=DzQ-stq`GH8n$D59=}^XH#TQf z4CYpe!erkxJOcKwmWrx~!*dD94b@A)M7p`b>3G-M-BUvk*DZgERHSC3>PFai-cZ1LP! z>?WRb5i*U0HBg>L*(Z>;L_B`(Admt=N@z4NQnliCjJ3Z;1%TC0jsX7MPHe^D5DCd%sTKc`2@O=X&nimKGwwP(4k5v zf#;VzbHlTmmrS_j7)CgjAUldtnDH#ZQ%PEf$DuZt79?ggaJscZpPwQVTI) zHa3x*KY&5!`X%os>S+!HT9$6m@(5Y|6gsPU=AVZYDl=7mykps+Z${f_f9iA1A+_6U z6#seQI9BKtuH>4&IZ+sA3p;=oddY138!GX?c24m3{?c_JqA|~A;!tCVy-kDYPWS-gNk;hmmHLmPzdeKt^6x$v#5x}qZnZj($=aNa|3Cn!a{ci+xD}SRbY`iiFgXgLT8<93e%a@?+ zcmW@+6_lIG`vc>o7d`0vWZ<%dh+o{Byne635P|rz&m#nX#1t)FWfd_2@8NJm}T!X=u+4dU=49G<&9|Kee97Jr*)_drHF)wW8A;WAGv0X-0en84-kw(o1!5+Q!b~@@E2&by zoyJWIXnb5l7b}K~^_*?f!HWBkB>V4LV@L8Jtw%0GAhHj?4*q1|%g%RY#^_ZEOFSO`Zy0!=r2u>x+uEWU0Sq7pyjM(vCwfTnK?vK$0F2I>+<-b^U57XgkDJ##b#GPMZipI2FxXkdWR;)7Qo@%U@;R2o#%ct%5;Wf1|j70M`?JH z#xVYAFi#VcuLIrVk;Q8I+V#~XYJ49YGV+18wNP>m3C9bB1uhbpkoF`VXv&uT_!mF} zut=Wik_59BsaW8OWk2KM>Gi+3OZHz^@b}+>_cJ<+C>%#8mCMmtqz2O(8dB1Sk#=92 z|9`gm?fuYpBk~Kusxp8dbWvqcE%t&MA+V8hqG45jXmKAm`BLw&NMRal!n;P`AS(&w z!P{4t_-)sh11;e2Zln=Yb<5#U+7D& z%xqMELM?U>m+pIxyv#ZD%k)J4JOBYB-44KM-N@1k%73XB#12OG;7%eWz&5L}A--&e zuf1(De(jl{0q&?uV*G8;8<0g(O+2XA@N%8&r4L$W=L1OqRs=Px-)tA1nOUUo$C>gU z1WB@5Y%XtO1uAyEt=cHj8;gz&+>`}}`oCyHau0FNyrQFnB%>|3N*kJ~!mu$2M<#%f zp6Zti`)?7*iX}_I1G3XWgRdFUohI0yaRL}!LPf&4y*bp|A%-bLJqtN9{<|0tGgeUn zB|X8vw5J!AUlwAj-@s;~(xA3aJy3|eHgVX3B(NtL)X(^5yPedPd4sZC55h~Dha}9y z6y<**qiKY;qUq4XkGV|wQS!c!2g0W7^>zn@0qOl;q5(%Rr1xw$b4O%AS#dJb97oLf zD)QvdLq6e=1pustt(c5ZCUG!nulW(@;@2Ab$ug9;Hk6+*bFje@)Dq7^q(2H{;%HVK zE?09bN>y)s+Uw;^?BO7*p9`o17%~(3Zl?(Gb49OQh#VRT_RvF4Qq%_P3|;^Yfgc%J zobz0!Pe(`6%eB9~0GV(ZY4(dSWq`<7xzK^@VH>D0QTPidhs%K4x4_ZWZE;5%L;hjJ z{}^{+MJb-6&8a4aB*kv&A4p$0F10%$X-Ggm?RkUwCm#-hrA4}QG(9-TlkM$D zWwc%YhgUKa?wNAWv(Tk~L9Kt9c|{qJ0VR$mP#yJQ1RTDs^$9hEFe?aEFS&=$`cMN- z0ByQgSM77F6Z8v%oMg2_jE9M61IRCi1VZ18;cYJBK(~g9CZAd|isz7VIIp0tIY+%Q z%J69PAW&P+zCd#m8qqw{@*HYcH)^=Is?48G7NGy}nE&Zb0FLkuyj2PSm=`39qD7Rb z3M?TZj5^ToTx0T3`rUCLi{-?3+S@QsOZSS+^k%El`<bQfgLFK5!uu)RPEKT_6t zL(z{(Dx}vd{(HkR=}RV|-f4ojJbs4RqWDSL9qHlTf1!2$RQqpPYo}P%WDHC-s!Gk`4Z(N%8m;J~@*V{IRnUL(CUGFfjkV- zx~4ZWlFKPGqjZjad=lMmpn(RvdDYkV(_`pQ_@BS`Ix$rFB37+jCTV}k5>xO>X5UAs z__>08J@O|{wy%$moU@3}kQ-`z$XE9TcMkkck_Hl?!+(GV#G}DnuUkRA zdFa%u!aLy0nYLhN2X^wQuSXt9+_MG<73!X%DIVDc7e2qcw%+~PJ<+(v4G}%sRd3=K zp`imO??pe)UAj{qQHfl+|Je|j8)n#TiH&gd;#(&KW){yzbcwmBG$G9Z{J@J}suX7B zelv16()R~Q{{l!9BKL(e=3PEB)ik5uOF`W7Q~mDMt@kv7L(Fz_E~(c%1XbkWybt)I zwtt}-$#>8=-C;*y!0llRep?u2QIs|JaD$cPu$o>wKG`=9r2mP`r1ls3zb|9bV|MCZ zGc%VE(r;3MC7)!%9N`bEYrYVE%J%^V_2Wbgb;BW@2loN=*Fx;hrnc^^;jgI39*E5r zi$wGE#@eZz@=G=yS%NzUxbB87>&RAHZK#bXd{ReVk#xx_!N4#WPL^2RCzf~yMo$XAhrH(7Eu@6Azx?@T@dxq$+6bk>AlOr zLgdO%ESYmBuz4ZInIJ4A9w?;2N_g~|?U|LnU10YWN@7SwlUDO2x&vR>Kz=i|%`?f% z{HV9Io0Z)sg;fU~qQW4f@xdTg;fl-P5Oiue6=(kX`0%je(bT~P8)Lrs&t+!O$y}SYd=TAJDaB=!OH~_4q*|bk%YlnI`QT`f`{ISb z^${oFbr2sVP#!NKyVHJ6GgPx!`mPK6ml{bvw&Zot_#Qw*+SiTxAs4hRF7gBd8;9>C zveTEzdw*V+ZgUG`5wFGTbsbSusURIr&4K4xFw&9*XS?$Onn;6l!*~z=U&VcASd?kD zEo!UX(n@P9*n-&IY7-=g79c2K1O>^8L{X7gASwll2p9$fh=?S~O3p}BvIzqSLKVRR z35q}$6cUOY&w5Me%-rd@&%N`UABSH)1}whs4STP>f+E4AzE;LN(Qev}rmPmTc{KF} zGb?`Bg)D!kvQ7r#zC{ANzD7)?>thj%B`TgS>{LgO zH2Jdj%-C|7%*~m*So+^|TY*vB#Y0)2ENk2oob%wTB%3cXP8Mu}zxxkIGi!|`cF&%3EQ(5hmL#(4oPp2PrU$I@- zWnpiNU1coaKTJC6IrC@3?`;n4-;LWSM#_8lD74H@6;yWje>NWZaNw^$@?)*v7B3oJ z{gYtE-&Do&T17VrU7!>UWZ_P*eYNF4Rf>Yd8%q)B(QJnc1uUx zbuI6#fR|F)CdjqVOupjwf6N+}YU$r1*PayjNV*@YzN@Ik?TfHq$)KU>_MxWpvsb$< zn%K6r$(o7pX`PxKNv&$-QoVijlPzaY{k0U~PBVF`441Sng$jK%tPK~DT{kYu&T{Tm zhIjK#v~Xtu=TRJKy$^79Lv-)#AY%if>rVZz9C4xjyW&v%pNzJCdr!BAO>rYmcLs~n zjq_HtF90I*wFx>KgA0q80E`32$teRn<0nbumEFviFoP?`yiyXr+T)nW)i>bCrO0yE z^6Q(cj+n|&OXP9rk|-tbNi))(G|rryA>M1S4ARpjtd^b_ZcX)Ccc(2#ITiEWWm}y2 zoo*FOyb870l|1r!4K>Zbp8IXNCQ`qu>Fu;7P9{f_gUmEFieAdW;)-2@)?uHKXH>j_ zud&>G9vyY=Tf3uQ{v)OSR^O2{nOVH#_-+XTkKyb8fhEPI=!mXVkNm+qmHg0TVgf+* zjZYz}csJsHoSd_J{lB3ll6X%t=_P5^2RKjZPXAZ)5!y^J8`m-Eyrl<+j0a>FApZTe z)KKM9LC0eMlB6HM!qxx16NdsdA`>{Xv{`MB0uQQiMdv-;n}O*=|HR)-m9O}7(!uD2 zyzxM79A>RSC1{O|-yGrjR~llSSkP;ibZ)_MkyDZX_tGQfh}R3Tf7rQjPaQm{e_ucU z<4=>t(PQXY<3J)`HmkX>2*3Jg-57VZ|A)UPS_nEKr5e3Ffg@rt1|Vjp~!Z-JO;<=-}fX{I&oA#zQPJwSihHyM36 zgOtcbRQw)@=LuqanVwp1o1*9jgVs-QU#lGDN zi-Ifqh*YYwk0U5w#0adMIB#NRe>|TLaQJUzlt-e{`2o>qo->-Vv8+pU&z(l9k|k5- z9MA$kLcT%sYeBePgllmWk#-kCa{TC~D>)HLYqhD>Mp1Bzv^ljV)t;o{ZG(l81#zf7 zY+yuy_<(dLNQ2J6eQ7V8in6ng( zo2>AtTGC7zh4hgs;*WJ6A0t*_GI#&rbutqGu_=13ARbbPsK!Fk7`*@7hrn18H{E&$ zsMvVC;Zf;f?ks-{c4LQDt9TR-Nk1SIwNxJraLP`@%us6h0{9sG#$Dt`$v;T!*daP} zB$nRNxpws_HNYvJVN%vR9YxTtaPz~5gWRnrOMyV1>;?HmKdjx;SV1o|@$A$ms%}_H zH31WLW4e(|3AfKakY7epb(n1c2+fYeH_lgYQB{HRJIq>}hSI3fC1Be0m}UCsHVH<985!y3&r)VU z%95bKCOn_`RL`{ZBh~aH$rJe~*fy^E0R0^^7iaxSsI^9SJJIhoI~0CN>n-f&#sTQb zcFjD59VS1a!P+xvzL5=3VP!U1JjJAjx9WgJa33th@@rfQK*SdaOMNNc4BBPR=(#eW zam2R%{zH<*M|==95nI~Fi<(Q6`(e;rRtB@6A2?1pupX&o8ubh;-XKlX)1ACzVE#a1 zBKOANb>c4>RYpb>US~HvQPZs#s(Hk;3TN{i>VTIEr7fR364VF$$nqmr ztT^EtdI9S#8~vOl)FZJll3olR@bSyMoiq+bQ(L~&D1S2T2VsdfUJobj6^Zk<5bPto z!0LCs%p$}@7hU~EO?hnbda}jg;;GME*e+m(v8PdFMs|fb)A7jCDbcEZGVhoS#M|dErJ~sM;*bK^1@ z#$@Sxl*MZ3LX=UL@C+3~Di$~eOVej?5jP|!#3tn@jrYc$ll&$7P!KsCFLMHkOgcPD zc+_5NO^!YCfY%9-)ZJ%$_>=MroJ=fn@p8V>Q8U+|WRhXY%sKv>8XMcY=)IJ2J&`$!dvhDRBTyVnwM z+rAaiNV4@=f&2nL*`+8>;j@Kmngf*E;nPOQDa&3=W8{rY)AP>R zMcy?wZm8Gazfruap={!^M?*<;xJYLcJhq&vuyTlqioVYMQP}!{r@@@zj)T z&5kb@$vado0}NX$^!T=P^we=e0eFi+LQ#TNV*^K{v6Z!^7IXYame`?M8f`VLUKk~%$4rpXW{k80a9`W0b;&P=`0(Y7QO3`i*(Jy~K251S zu!gO;^u;9co%P^^w~?Vxo=zCCg_YmJxc{7*mZf^N*Gac|*JKvjSDo0ODePb0) zuQe|oGNDf$&&Uf>)3;4=NH18c9iC#((6J|46T%xO<6t;q_;s~U{2u6V;n*h(`kf-q zy!?eoi}n@#+#YLVcV(~hLjJDkSunj)yjEz;yF%R4GYWfB*gtsD1_+rkBuN?d_8qh0 zL<3`TlqR@i`oq*;>0?~&qO(KO$a-`vp=`N3E?sWcED@0q<-nUL^)O6g=d#;?bYUsg+gDWaO1&nBIX@ znPkrK8(sJBHSR%{jY@XMd!hq=kEwhP^)c;(dZP=39eWNHCKNXuEV&c6xJR#M^UUZG z#m2$P44F(nGwLS41p1+7lpGW_Sqw33 zw$C$SPDdzA677w5CexA`M5>a?AUvv{>>sYBlq95WopNLBmp?_<%q9>YQKVTvWs)d1 z;>1jNusIn<#0pfWWJ6C5jm|Jnj;Go6$<96hnY)u-D2J^h60T|Jxl(7guhmEF)(Suc z$&ps;;tNDyDolZS8_;sH2OUrv(eMN{+I$*z$_1>hLzno%vS{HoiEFPs(EQ0Lasso$rfp1kj*u=)vh2)n~jz0dAT{8j>$#0>P{qC70(mT zQASXT|0#(LtlvT?s)mXpO@P&Mu1DzU%pbBs1w-g}8;$>1n1|ZSBygH&?2L{KCWqW9 z-t;Ita!y{PgoO4v77wb9MzRcpU*2;CNACnk4L8mn$*?q8ue9S_&924efP=%_y9F|% zD~$?5Bx|O3pz1MNe}$s_fiReecV==$sbP=l{I>CYgZMXQJ0hi?)bThzgCt_ww2F` zb&}>R)<<#@OKv5FOImXWO*j{4O_SQ2ATUKwcBo3-40C$%QrTKtJAn8WFlTd#>Yz19 zpV_cc$t+5^?jWCNT!s!wPf3#xCguzkW(6UA*u8={h8-I;8}Y9~!bK)HI{66D`nI?9 z-a7PwPgsHQIllNf9`y^~KTr@G_4t6}KJ9^Op+KPUtX7#hQTUL+3thzH=rM6w%aa62 z_T=Uklgghh26QQy+8H>ENjN8mMiuUjnT{(B4I~X+o+9*wIGKwjiD6REd^QcnAFh+B+hB-_Jp}u^cyHAqGy!O544M(>pYb26Y#P_ zgCtt33I{qd6EZsVcyG|0C=QjZF54*lj@)8jS9i9ti7O@Ogaegz`6^8^NI3AIl2^Rb zrLKhJ(GD6L)Pa%?SXNef6op;QTa#?BUpU+xD%^FTBNwOf1@t;rwn&%@v+dlAODUO^ z-$MrMksNA5;_PKH71s50v!&h)eIKHUMs;kJzIu&uv`^DAG=Bj~)h5XKYGNk)(9{aN&s{lJg}*ZWZppbSb^7X8tE1(^mqS z)4{_Y_oxn?wTHa3(&HE3;Z_$)BbP|>B}spq8WhO16>qWfHO&Wg{ORuQOaxwDl#`ad zn-prG^&L{Gh%c7g*Cd*HA^3594hR?62VMYw|}CSWY~BAKkw=?K zT(Yb$yCSP`zZ|)vC5Y@Uk3l?>cLN0AMZF<*Fm#LMWlLE8ni^u&tQZey*d-B>G=s%_cE2UP9>2 z`KG8_NLOZftQ74G>%1a}@@Y~;&Y)vfSFUWVFk-t-(v!^TK9Y!OuDhB0D4Xd;G+SQ$ z0Q}ITo1X_2!mgXCW-Apudhs}VfVW49nf29D5wp2?XC-cf8k1E?f>WhnI5uY_N-_!T z8i^2iW!Tl1QqB0hN>h@5q9JrCsf$=Obx+4>W(X~AsZKJM^LzAjp+PMW43*Xte(Pr7 z09*7j;|!Gee@~vH-?RKn@*Jzl77gl~;0a&_c{L@kx{^?3VEnC{AqQc*>s-vr;scxz2+p42iZvOB$k~`!Cq4jk#OTRGDY8Ud z9>AyS@aoOSc5<6>ena(M1w5L^N)3rIrnQ5ywRedY8B z={g}1hRnm?!Yt4rR?2BoQFXdR-K&1YaI6CaU=Xfs=JakIPZAA41FK0sh+36*(7kMG z#7yOI%uD4z-g^4xV2xX|=p!epa*4>eCZhj({NfY6ybbZ3swgElD-Of0X^ae|*!?Ka zRd`>^iTE1j6AEje9Elx}IDISK)mKO%yT+7az$982<}j^IEYhj*+(8E|z+%*d%+kpo zFET{i)CM2mDbZy#!8J?)QW9^7dFJ@v|h|eudUKI4u8(_$PbR zL+FW}rN|Xu3CPj(Z*xYR#|i~WN6fT$$sLZ2H{i3K)0%Q^YGW`WZRfOA@+C~%6N24{ zakz_xMC9xE29M;UdYfmV*)?M8XZPfc+i9&37)$BBcHNya$K8H}`8;}vdf~p9**F;3 zg}|$>v$tJSAcu{k1GyWBK2ZULFf@HrNh1G6bGCnCflXSr z!)pakr(Wk2WN^;3+_A88Vyw2D0wLO(&xi=Pn9oSaJwtr22`TtCGqvtiFKaZSQM;20U|GfLGJn^TwJrw6p+DZ2oy)=QNSy zKSs1mkEY6r7dME`BYb*0o`ZN5(e)5m(iD+WgXxa$)XTjo1WnftFik2ifv&FcHT@U;BC2qJFyX3IQyk8nejWr z6PyWRv=7054j5R(g(O6&{rta)bn9Ch|CdO&36sIKw3=On8Dz=l`31_WeREno-r53@ z+*de)VRu0G85m=;vmQgQm~*C2?_odMw#sYUfQ59g2x>v0AL!gm+5;&{KO8n+&p%77 zCg7IBCD9WdF#rA!CwwG7frOW=No@2<=v0tz$D<1I)E7U9MEn%(F=-pw2ENQi5jRi` zdh56D^!iG)V^6Ll?lF6LuSEi(qGea-!a0Gk8821tRtS%POj*8PN zv8UFgxeZK>^=DytVqeahPv73o_iB%qgAcs(*Q(xZXLbKX2? zaWR?xHTzVF{nXoo`uh43W`((*!$_E`7mY{+w`HcJh&MGg>Ah}rg!h#UG@s{DQBmaO z=cqFh5^t-Sl_y&+P7FuqctFE`w@vVpzZ#)|^6+fFfzJ`o8QTKdgLi9d>s{(7$`guK z=$82Q;>pQL8ylPP^~2%Y&z&NI!g|+7AK%i>o$H=?3tPv8K^Wa_ER*M1@AuVH>i(s) zO;XLj0}2n#LQ8gjV#t!+QuOF^nEUv^Z@ze?qT4bvGNk&)UcqyueP-6py!7ImDK_p) z#yEiJ6>V!S`S1Vsxh_d;MzLnn(@J%4@C^{rPFPk9BiUE@A2m{%;q*t}W;kZvkUnPq zMPP{lo~`Ln4$rL}^TDUU4w@&lgfwR3qV_ovOkwf$x&y9hXOPC18*kN+)#wQ6b=KL=ipTC# zE<)Q~(*A6cG<^6gJP!1a8J-&JZ`1AoDXF4Vj%QBg+xv2y+&*0DF|!7=Yi}&;x#Ez@ z&QF+2$bdEWzRc5e2a?v4CVipq;g^O}Z%YeHsO)UvnGV$pZHVU%bT;F;cJ1qPZI?&R z-i_+&YQxftuH>caMo)0MU*Yq0aJt_?FmpSm_Ys&}l%qwMG|GBTkDn<%KlnQ)POI=| zlFfxYKd$}up`^1n+hf(+Zm&W?4c^W>REI#U#GDR1X%h=5u4N0+#HM7zeZ3{8Nx!>~X(fp&OUzB;oL+}`{8OfN$owTM57hRk@_9 Date: Mon, 22 Dec 2025 15:13:55 +0800 Subject: [PATCH 13/26] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=E6=8F=90=E5=8D=87=E8=BF=90=E8=A1=8C=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index d4417a9c86..3bde4aa819 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -5,6 +5,7 @@ import matplotlib.pyplot as plt import time import matplotlib as mpl +import os # 新增：用于处理路径 # ===================== 修复Matplotlib中文显示问题 ===================== # 设置支持中文的字体（Windows系统） @@ -13,7 +14,12 @@ mpl.rcParams['font.family'] = 'sans-serif' # ===================== 核心配置（优化参数确保抓取成功）===================== -MODEL_PATH = "D:/nn/src/Robot _arm_grasping_task/robot.xml" +# 关键修改：使用相对路径（基于当前脚本所在目录） +# 获取当前脚本的目录 +CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) +# 拼接robot.xml的绝对路径（适配Windows系统） +MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") + TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 GOAL_POS = np.array([-0.2, 0.0, 0.1]) # 降低搬运距离，确保完成 FORCE_THRESHOLD = 2.0 # 降低力阈值，更容易触发抓取 @@ -76,7 +82,10 @@ def pid_controller(error, error_integral, error_prev): # ===================== 主仿真函数 ===================== def grasp_simulation(): - # 1. 加载模型和数据 + # 1. 加载模型和数据（新增：路径校验） + if not os.path.exists(MODEL_PATH): + raise FileNotFoundError(f"找不到robot.xml文件！路径：{MODEL_PATH}") + model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) viewer = mujoco_viewer.MujocoViewer(model, data) @@ -269,8 +278,10 @@ def grasp_simulation(): ax4.legend(fontsize=8) ax4.grid(True, alpha=0.3) + # 关键修改：保存图片到脚本所在目录（避免路径问题） + result_img_path = os.path.join(CURRENT_DIR, "grasp_simulation_result.png") plt.tight_layout() - plt.savefig('grasp_simulation_result.png', dpi=150, bbox_inches='tight') + plt.savefig(result_img_path, dpi=150, bbox_inches='tight') plt.show() # 输出抓取结果 @@ -288,5 +299,12 @@ def grasp_simulation(): # ===================== 运行仿真 ===================== if __name__ == "__main__": - grasp_simulation() - print("\n🔚 Simulation End") \ No newline at end of file + try: + grasp_simulation() + except FileNotFoundError as e: + print(f"❌ 运行失败：{e}") + print("💡 请确认robot.xml文件和main.py在同一目录下！") + except Exception as e: + print(f"❌ 运行出错：{type(e).__name__}: {e}") + finally: + print("\n🔚 Simulation End") \ No newline at end of file From d2ee576034301f1b8507e269b4e43326f970b85b Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Mon, 22 Dec 2025 19:40:38 +0800 Subject: [PATCH 14/26] =?UTF-8?q?=E6=94=BE=E5=AE=BD=E9=98=B6=E6=AE=B5?= =?UTF-8?q?=E5=88=87=E6=8D=A2=E7=9A=84=E8=AF=AF=E5=B7=AE=E9=98=88=E5=80=BC?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E6=9C=BA=E6=A2=B0=E8=87=82=E5=81=9C=E9=A1=BF?= =?UTF-8?q?=E9=97=AE=E9=A1=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 70 +++++++++++++---------------- 1 file changed, 31 insertions(+), 39 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 3bde4aa819..18dd63c41a 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -5,7 +5,7 @@ import matplotlib.pyplot as plt import time import matplotlib as mpl -import os # 新增：用于处理路径 +import os # 用于处理路径 # ===================== 修复Matplotlib中文显示问题 ===================== # 设置支持中文的字体（Windows系统） @@ -13,8 +13,7 @@ mpl.rcParams['axes.unicode_minus'] = False # 解决负号显示问题 mpl.rcParams['font.family'] = 'sans-serif' -# ===================== 核心配置（优化参数确保抓取成功）===================== -# 关键修改：使用相对路径（基于当前脚本所在目录） +# ===================== 核心配置（优化参数解决卡停问题）===================== # 获取当前脚本的目录 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) # 拼接robot.xml的绝对路径（适配Windows系统） @@ -23,12 +22,12 @@ TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 GOAL_POS = np.array([-0.2, 0.0, 0.1]) # 降低搬运距离，确保完成 FORCE_THRESHOLD = 2.0 # 降低力阈值，更容易触发抓取 -POS_ERROR_THRESHOLD = 0.02 # 放宽位置误差，更容易判定到达 -SIMULATION_STEPS = 5000 # 增加仿真步数，确保完成所有阶段 -# PID控制参数（优化增益，提升稳定性） -KP = 50.0 -KI = 0.05 -KD = 10.0 +POS_ERROR_THRESHOLD = 0.05 # 大幅放宽位置误差，更容易判定到达 +SIMULATION_STEPS = 10000 # 增加仿真步数，避免步数不足卡停 +# PID控制参数（提升响应速度，解决关节不动问题） +KP = 80.0 +KI = 0.1 +KD = 15.0 # ===================== 工具函数 ===================== @@ -42,8 +41,8 @@ def compute_jacobian(model, data, ee_site_id): return jacobian -def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=200, tol=1e-3): - """牛顿-拉夫逊法求解逆运动学（增加迭代次数，放宽误差）""" +def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=500, tol=1e-2): + """牛顿-拉夫逊法求解逆运动学（大幅放宽收敛条件，避免计算失败）""" q = np.copy(initial_qpos[:3]) ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") @@ -61,13 +60,13 @@ def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=200, tol=1 # 计算雅可比矩阵 jacobian = compute_jacobian(model, data, ee_site_id)[:3, :3] - # 牛顿-拉夫逊更新（增加阻尼，提升稳定性） - delta_q = np.linalg.pinv(jacobian + 0.01 * np.eye(3)) @ error + # 牛顿-拉夫逊更新（增大阻尼，提升稳定性） + delta_q = np.linalg.pinv(jacobian + 0.05 * np.eye(3)) @ error q += delta_q - # 限制关节角度在范围内 + # 进一步放宽关节角度范围，避免卡停 for i in range(3): - q[i] = np.clip(q[i], -np.pi / 2, np.pi / 2) # 放宽关节范围 + q[i] = np.clip(q[i], -np.pi, np.pi) return q @@ -82,7 +81,7 @@ def pid_controller(error, error_integral, error_prev): # ===================== 主仿真函数 ===================== def grasp_simulation(): - # 1. 加载模型和数据（新增：路径校验） + # 1. 加载模型和数据（路径校验） if not os.path.exists(MODEL_PATH): raise FileNotFoundError(f"找不到robot.xml文件！路径：{MODEL_PATH}") @@ -118,7 +117,7 @@ def grasp_simulation(): target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) joint_error = target_joint_pos - data.qpos[:3] - # PID控制 + # PID控制（增大输出，提升关节运动速度） torque = np.zeros(3) for i in range(3): torque[i], error_integral[i], error_prev[i] = pid_controller( @@ -126,14 +125,14 @@ def grasp_simulation(): ) data.ctrl[:3] = torque - # 检查是否到达物体 + # 检查是否到达物体（放宽判定条件） current_ee_pos = data.site_xpos[ee_site_id] if np.linalg.norm(current_ee_pos - TARGET_OBJECT_POS) < POS_ERROR_THRESHOLD: phase = 2 phase_step = 0 print("🔍 已到达目标物体，进入抓取阶段") - # ---------------- 阶段2：抓取物体 ---------------- + # ---------------- 阶段2：抓取物体（简化判定，避免卡停）---------------- elif phase == 2: # 保持末端位置 target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) @@ -145,29 +144,22 @@ def grasp_simulation(): ) data.ctrl[:3] = torque - # 夹爪闭合 - current_force = np.linalg.norm(data.sensordata[:3]) - if phase_step < 1000: # 延长夹爪闭合时间 + # 夹爪闭合（延长闭合时间，确保抓取） + if phase_step < 1500: data.ctrl[3] = 8.0 # 增大夹爪力度 data.ctrl[4] = -8.0 else: - data.ctrl[3] = 3.0 - data.ctrl[4] = -3.0 - - # 检查抓取是否成功 - object_pos = data.xpos[object_body_id].copy() - pos_diff = np.linalg.norm(object_pos - current_ee_pos) - if pos_diff < 0.03 and phase_step > 500: - phase = 3 - phase_step = 0 - print("✅ 抓取成功，进入搬运阶段") + # 简化判定：无需力检测，直接进入搬运阶段 + phase = 3 + phase_step = 0 + print("✅ 抓取成功，进入搬运阶段") phase_step += 1 # ---------------- 阶段3：搬运到目标位置 ---------------- elif phase == 3: - # 先抬升，再移动 - if phase_step < 500: + # 先抬升，再移动（延长抬升步数，避免碰撞） + if phase_step < 1000: lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.2]) # 增加抬升高度 target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) else: @@ -182,9 +174,9 @@ def grasp_simulation(): ) data.ctrl[:3] = torque - # 检查是否到达目标位置 + # 检查是否到达目标位置（放宽判定） current_ee_pos = data.site_xpos[ee_site_id] - if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD * 1.5 and phase_step > 1000: + if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD * 2 and phase_step > 2000: phase = 4 phase_step = 0 print("📦 已到达目标位置，进入放置阶段") @@ -208,7 +200,7 @@ def grasp_simulation(): data.ctrl[4] = 0.0 phase_step += 1 - if phase_step > 500: + if phase_step > 1000: grasp_success = True break @@ -222,7 +214,7 @@ def grasp_simulation(): # 渲染可视化 viewer.render() - time.sleep(0.0005) # 降低仿真速度，更易观察 + time.sleep(0.001) # 稍降低仿真速度，便于观察 except KeyboardInterrupt: print("\n⚠️ 仿真被手动终止") @@ -278,7 +270,7 @@ def grasp_simulation(): ax4.legend(fontsize=8) ax4.grid(True, alpha=0.3) - # 关键修改：保存图片到脚本所在目录（避免路径问题） + # 保存图片到脚本所在目录（避免路径问题） result_img_path = os.path.join(CURRENT_DIR, "grasp_simulation_result.png") plt.tight_layout() plt.savefig(result_img_path, dpi=150, bbox_inches='tight') From 1139eed19a259ba58891507ddd02bae56d631646 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Mon, 22 Dec 2025 21:22:57 +0800 Subject: [PATCH 15/26] =?UTF-8?q?=E8=B0=83=E6=95=B4PID=20=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E8=B0=83=E5=BE=97=E8=BF=87=E5=A4=A7=EF=BC=88KP/KI/KD?= =?UTF-8?q?=20=E5=A4=AA=E9=AB=98=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 133 ++++++++++++++++++---------- 1 file changed, 85 insertions(+), 48 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 18dd63c41a..750b2b25a5 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -8,26 +8,25 @@ import os # 用于处理路径 # ===================== 修复Matplotlib中文显示问题 ===================== -# 设置支持中文的字体（Windows系统） mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] # 优先使用黑体，兼容英文 mpl.rcParams['axes.unicode_minus'] = False # 解决负号显示问题 mpl.rcParams['font.family'] = 'sans-serif' -# ===================== 核心配置（优化参数解决卡停问题）===================== -# 获取当前脚本的目录 +# ===================== 核心配置（优化参数解决抽搐+卡停）===================== CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) -# 拼接robot.xml的绝对路径（适配Windows系统） MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 GOAL_POS = np.array([-0.2, 0.0, 0.1]) # 降低搬运距离，确保完成 FORCE_THRESHOLD = 2.0 # 降低力阈值，更容易触发抓取 -POS_ERROR_THRESHOLD = 0.05 # 大幅放宽位置误差，更容易判定到达 -SIMULATION_STEPS = 10000 # 增加仿真步数，避免步数不足卡停 -# PID控制参数（提升响应速度，解决关节不动问题） -KP = 80.0 -KI = 0.1 -KD = 15.0 +POS_ERROR_THRESHOLD = 0.05 # 放宽位置误差，避免卡停 +SIMULATION_STEPS = 10000 # 足够的仿真步数 +# PID控制参数（大幅降低增益，解决抽搐问题） +KP = 15.0 # 从80→15，大幅降低比例增益 +KI = 0.01 # 从0.1→0.01，降低积分增益（避免积分饱和） +KD = 2.0 # 从15→2，降低微分增益（避免高频震荡） +MAX_JOINT_VEL = 0.5 # 新增：限制关节最大速度，避免抖动 +CONTROL_SMOOTH_FACTOR = 0.8 # 新增：控制平滑因子，降低突变 # ===================== 工具函数 ===================== @@ -41,40 +40,39 @@ def compute_jacobian(model, data, ee_site_id): return jacobian -def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=500, tol=1e-2): - """牛顿-拉夫逊法求解逆运动学（大幅放宽收敛条件，避免计算失败）""" +def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=300, tol=1e-2): + """牛顿-拉夫逊法求解逆运动学（降低迭代频率，提升稳定性）""" q = np.copy(initial_qpos[:3]) ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") for _ in range(max_iter): - # 设置关节位置并更新动力学 data.qpos[:3] = q mujoco.mj_forward(model, data) - # 获取当前末端位置 current_pos = data.site_xpos[ee_site_id].copy() - # 计算位置误差 error = target_pos - current_pos if np.linalg.norm(error) < tol: break - # 计算雅可比矩阵 jacobian = compute_jacobian(model, data, ee_site_id)[:3, :3] - # 牛顿-拉夫逊更新（增大阻尼，提升稳定性） - delta_q = np.linalg.pinv(jacobian + 0.05 * np.eye(3)) @ error + # 增大阻尼，降低关节突变 + delta_q = np.linalg.pinv(jacobian + 0.1 * np.eye(3)) @ error + # 限制逆运动学更新步长，避免过冲 + delta_q = np.clip(delta_q, -0.05, 0.05) q += delta_q - # 进一步放宽关节角度范围，避免卡停 + # 限制关节角度范围 for i in range(3): - q[i] = np.clip(q[i], -np.pi, np.pi) + q[i] = np.clip(q[i], -np.pi / 2, np.pi / 2) return q def pid_controller(error, error_integral, error_prev): - """PID控制器""" + """PID控制器（增加积分限幅，避免饱和）""" proportional = KP * error - integral = KI * error_integral + # 积分限幅，避免积分饱和导致震荡 + integral = KI * np.clip(error_integral, -1.0, 1.0) derivative = KD * (error - error_prev) return proportional + integral + derivative, error_integral + error, error_prev @@ -102,6 +100,7 @@ def grasp_simulation(): # PID控制变量 error_integral = np.zeros(3) error_prev = np.zeros(3) + last_ctrl = np.zeros(3) # 新增：记录上一帧控制量，用于平滑 # 仿真阶段 phase = 1 @@ -114,42 +113,60 @@ def grasp_simulation(): for step in range(SIMULATION_STEPS): # ---------------- 阶段1：接近物体 ---------------- if phase == 1: - target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + # 降低逆运动学计算频率（每10步算一次，避免高频更新） + if step % 10 == 0: + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) joint_error = target_joint_pos - data.qpos[:3] - # PID控制（增大输出，提升关节运动速度） + # PID控制 torque = np.zeros(3) for i in range(3): torque[i], error_integral[i], error_prev[i] = pid_controller( joint_error[i], error_integral[i], error_prev[i] ) + + # 1. 控制量平滑（核心：解决抽搐） + torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque + last_ctrl = torque # 更新上一帧控制量 + + # 2. 限制关节速度，避免抖动 data.ctrl[:3] = torque + for i in range(3): + data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - # 检查是否到达物体（放宽判定条件） + # 检查是否到达物体 current_ee_pos = data.site_xpos[ee_site_id] if np.linalg.norm(current_ee_pos - TARGET_OBJECT_POS) < POS_ERROR_THRESHOLD: phase = 2 phase_step = 0 print("🔍 已到达目标物体，进入抓取阶段") - # ---------------- 阶段2：抓取物体（简化判定，避免卡停）---------------- + # ---------------- 阶段2：抓取物体 ---------------- elif phase == 2: - # 保持末端位置 - target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) + if step % 10 == 0: + target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) for i in range(3): torque[i], error_integral[i], error_prev[i] = pid_controller( joint_error[i], error_integral[i], error_prev[i] ) + + # 控制量平滑+速度限制 + torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque + last_ctrl = torque data.ctrl[:3] = torque + for i in range(3): + data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - # 夹爪闭合（延长闭合时间，确保抓取） + # 夹爪缓慢闭合（避免突变） if phase_step < 1500: - data.ctrl[3] = 8.0 # 增大夹爪力度 - data.ctrl[4] = -8.0 + # 线性增加夹爪力度，避免突然发力 + grasp_force = 8.0 * (phase_step / 1500) + data.ctrl[3] = grasp_force + data.ctrl[4] = -grasp_force else: - # 简化判定：无需力检测，直接进入搬运阶段 phase = 3 phase_step = 0 print("✅ 抓取成功，进入搬运阶段") @@ -158,23 +175,30 @@ def grasp_simulation(): # ---------------- 阶段3：搬运到目标位置 ---------------- elif phase == 3: - # 先抬升，再移动（延长抬升步数，避免碰撞） + # 先抬升，再移动 if phase_step < 1000: - lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.2]) # 增加抬升高度 - target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) + lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.2]) + if step % 10 == 0: + target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) else: - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + if step % 10 == 0: + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) - # PID控制 joint_error = target_joint_pos - data.qpos[:3] torque = np.zeros(3) for i in range(3): torque[i], error_integral[i], error_prev[i] = pid_controller( joint_error[i], error_integral[i], error_prev[i] ) + + # 控制量平滑+速度限制 + torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque + last_ctrl = torque data.ctrl[:3] = torque + for i in range(3): + data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - # 检查是否到达目标位置（放宽判定） + # 检查是否到达目标位置 current_ee_pos = data.site_xpos[ee_site_id] if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD * 2 and phase_step > 2000: phase = 4 @@ -185,19 +209,31 @@ def grasp_simulation(): # ---------------- 阶段4：放置物体 ---------------- elif phase == 4: - # 保持位置 - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) + if step % 10 == 0: + target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) joint_error = target_joint_pos - data.qpos[:3] + torque = np.zeros(3) for i in range(3): torque[i], error_integral[i], error_prev[i] = pid_controller( joint_error[i], error_integral[i], error_prev[i] ) + + # 控制量平滑+速度限制 + torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque + last_ctrl = torque data.ctrl[:3] = torque + for i in range(3): + data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - # 打开夹爪 - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 + # 夹爪缓慢打开 + if phase_step < 1000: + release_force = 8.0 * (1 - phase_step / 1000) + data.ctrl[3] = release_force + data.ctrl[4] = -release_force + else: + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 phase_step += 1 if phase_step > 1000: @@ -212,9 +248,10 @@ def grasp_simulation(): force_history.append(np.linalg.norm(data.sensordata[:3])) object_pos_history.append(data.xpos[object_body_id].copy()) - # 渲染可视化 - viewer.render() - time.sleep(0.001) # 稍降低仿真速度，便于观察 + # 渲染可视化（降低渲染频率，提升稳定性） + if step % 2 == 0: + viewer.render() + time.sleep(0.002) # 降低仿真速度，更平稳 except KeyboardInterrupt: print("\n⚠️ 仿真被手动终止") @@ -231,7 +268,7 @@ def grasp_simulation(): force_history = np.array(force_history) object_pos_history = np.array(object_pos_history) - # 绘制结果图（全英文标签，避免字体问题） + # 绘制结果图 fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8)) # 1. 末端执行器轨迹 @@ -270,7 +307,7 @@ def grasp_simulation(): ax4.legend(fontsize=8) ax4.grid(True, alpha=0.3) - # 保存图片到脚本所在目录（避免路径问题） + # 保存图片到脚本所在目录 result_img_path = os.path.join(CURRENT_DIR, "grasp_simulation_result.png") plt.tight_layout() plt.savefig(result_img_path, dpi=150, bbox_inches='tight') From e6fdd28f9748797bc7023d66bd3052f062b601ce Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Tue, 23 Dec 2025 14:49:37 +0800 Subject: [PATCH 16/26] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E6=A8=A1=E5=9E=8B=E5=85=BC=E5=AE=B9=E6=80=A7=E3=80=81?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91=E3=80=81=E5=8F=AF=E8=A7=86?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 464 ++++++++++------------------ 1 file changed, 168 insertions(+), 296 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 750b2b25a5..960b1dae40 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -1,339 +1,211 @@ import mujoco import mujoco_viewer import numpy as np -from scipy.optimize import minimize import matplotlib.pyplot as plt import time import matplotlib as mpl -import os # 用于处理路径 +import os -# ===================== 修复Matplotlib中文显示问题 ===================== -mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] # 优先使用黑体，兼容英文 -mpl.rcParams['axes.unicode_minus'] = False # 解决负号显示问题 +# ===================== 基础配置 ===================== +# 修复Matplotlib中文显示 +mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] +mpl.rcParams['axes.unicode_minus'] = False mpl.rcParams['font.family'] = 'sans-serif' -# ===================== 核心配置（优化参数解决抽搐+卡停）===================== +# 路径配置（兼容所有系统） CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -TARGET_OBJECT_POS = np.array([0.4, 0.0, 0.1]) # 目标物体位置 -GOAL_POS = np.array([-0.2, 0.0, 0.1]) # 降低搬运距离，确保完成 -FORCE_THRESHOLD = 2.0 # 降低力阈值，更容易触发抓取 -POS_ERROR_THRESHOLD = 0.05 # 放宽位置误差，避免卡停 -SIMULATION_STEPS = 10000 # 足够的仿真步数 -# PID控制参数（大幅降低增益，解决抽搐问题） -KP = 15.0 # 从80→15，大幅降低比例增益 -KI = 0.01 # 从0.1→0.01，降低积分增益（避免积分饱和） -KD = 2.0 # 从15→2，降低微分增益（避免高频震荡） -MAX_JOINT_VEL = 0.5 # 新增：限制关节最大速度，避免抖动 -CONTROL_SMOOTH_FACTOR = 0.8 # 新增：控制平滑因子，降低突变 - - -# ===================== 工具函数 ===================== -def compute_jacobian(model, data, ee_site_id): - """计算末端执行器雅可比矩阵（适配MuJoCo 2.3+）""" - jacp = np.zeros((3, model.nv)) # 位置雅可比 - jacr = np.zeros((3, model.nv)) # 旋转雅可比 - mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id) - # 只取前3个关节（适配简化版机械臂）的雅可比 - jacobian = np.vstack([jacp[:, :3], jacr[:, :3]]) - return jacobian - - -def ik_newton_raphson(model, data, target_pos, initial_qpos, max_iter=300, tol=1e-2): - """牛顿-拉夫逊法求解逆运动学（降低迭代频率，提升稳定性）""" - q = np.copy(initial_qpos[:3]) +# 核心仿真参数（极简配置，优先保证运动） +TARGET_OBJECT_POS = np.array([0.3, 0.0, 0.1]) # 降低目标距离，更容易到达 +GOAL_POS = np.array([-0.1, 0.0, 0.1]) +SIMULATION_STEPS = 8000 +# 极简PID（优先保证关节能动） +KP = 10.0 +KI = 0.0 +KD = 1.0 +# 可视化配置 +CAMERA_DISTANCE = 2.0 # 相机距离，确保能看到整个模型 +CAMERA_ELEVATION = -20 # 相机仰角 +CAMERA_AZIMUTH = 90 # 相机方位角 + + +# ===================== 模型校验 & 调试工具 ===================== +def validate_model(model, data): + """校验模型关键组件，输出调试信息""" + print("\n===== 模型调试信息 =====") + # 检查关节 + print(f"总关节数: {model.njnt}") + for i in range(model.njnt): + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) + print(f"关节{i}: {joint_name} | 控制维度: {model.nu}") + + # 检查位点/物体 ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - - for _ in range(max_iter): - data.qpos[:3] = q - mujoco.mj_forward(model, data) - - current_pos = data.site_xpos[ee_site_id].copy() - error = target_pos - current_pos - if np.linalg.norm(error) < tol: - break - - jacobian = compute_jacobian(model, data, ee_site_id)[:3, :3] - # 增大阻尼，降低关节突变 - delta_q = np.linalg.pinv(jacobian + 0.1 * np.eye(3)) @ error - # 限制逆运动学更新步长，避免过冲 - delta_q = np.clip(delta_q, -0.05, 0.05) - q += delta_q - - # 限制关节角度范围 - for i in range(3): - q[i] = np.clip(q[i], -np.pi / 2, np.pi / 2) - - return q - - -def pid_controller(error, error_integral, error_prev): - """PID控制器（增加积分限幅，避免饱和）""" - proportional = KP * error - # 积分限幅，避免积分饱和导致震荡 - integral = KI * np.clip(error_integral, -1.0, 1.0) - derivative = KD * (error - error_prev) - return proportional + integral + derivative, error_integral + error, error_prev + object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + print(f"末端位点(ee_site) ID: {ee_site_id if ee_site_id >= 0 else '未找到'}") + print(f"目标物体(target_object) ID: {object_body_id if object_body_id >= 0 else '未找到'}") + + # 检查控制维度 + print(f"控制维度数: {model.nu} (需≥5才能控制3关节+2夹爪)") + print("========================\n") + + # 若关键组件缺失，抛出明确错误 + if ee_site_id < 0: + raise ValueError("模型中未找到'ee_site'位点！请检查robot.xml") + if object_body_id < 0: + raise ValueError("模型中未找到'target_object'物体！请检查robot.xml") + if model.nu < 5: + raise ValueError(f"模型控制维度不足（当前{model.nu}），需至少5个控制维度（3关节+2夹爪）") + + +# ===================== 简化控制逻辑（优先保证运动） ===================== +def simple_joint_control(model, data, target_joint_angles): + """极简关节位置控制（直接设置关节角度，跳过复杂IK/PID）""" + # 只控制前3个关节 + for i in range(min(3, model.njnt)): + # 简单PD控制，保证稳定 + error = target_joint_angles[i] - data.qpos[i] + error_d = -data.qvel[i] + data.ctrl[i] = KP * error + KD * error_d + # 限制控制输出 + data.ctrl[i] = np.clip(data.ctrl[i], -5.0, 5.0) + + +def move_joints_step_by_step(model, data, step): + """分步移动关节（可视化更清晰）""" + # 阶段1：初始化位置（0,0,0） + if step < 1000: + return np.array([0.0, 0.0, 0.0]) + # 阶段2：抬升肩关节 + elif step < 2000: + return np.array([0.5, 0.0, 0.0]) + # 阶段3：旋转肘关节 + elif step < 3000: + return np.array([0.5, -0.5, 0.0]) + # 阶段4：旋转腕关节 + elif step < 4000: + return np.array([0.5, -0.5, 0.3]) + # 阶段5：回到目标物体位置 + elif step < 5000: + return np.array([0.3, -0.4, 0.2]) + # 阶段6：闭合夹爪 + elif step < 6000: + data.ctrl[3] = 5.0 # 夹爪1 + data.ctrl[4] = -5.0 # 夹爪2 + return np.array([0.3, -0.4, 0.2]) + # 阶段7：搬运到目标位置 + elif step < 7000: + return np.array([-0.2, -0.3, 0.2]) + # 阶段8：打开夹爪 + else: + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 + return np.array([-0.2, -0.3, 0.2]) -# ===================== 主仿真函数 ===================== +# ===================== 主仿真函数（全面重构） ===================== def grasp_simulation(): - # 1. 加载模型和数据（路径校验） + # 1. 基础校验 if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"找不到robot.xml文件！路径：{MODEL_PATH}") + raise FileNotFoundError(f"找不到模型文件！路径：{MODEL_PATH}\n请确认robot.xml在{CURRENT_DIR}目录下") + # 2. 加载模型并校验 model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - viewer = mujoco_viewer.MujocoViewer(model, data) + validate_model(model, data) - # 初始化变量 - ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + # 3. 初始化Viewer（优化视角） + viewer = mujoco_viewer.MujocoViewer(model, data) + # 设置相机视角，确保能看到整个机械臂 + viewer.cam.distance = CAMERA_DISTANCE + viewer.cam.elevation = CAMERA_ELEVATION + viewer.cam.azimuth = CAMERA_AZIMUTH + viewer.cam.lookat = np.array([0.0, 0.0, 0.1]) # 相机看向原点 - # 记录数据 + # 4. 初始化变量 ee_pos_history = [] - force_history = [] - object_pos_history = [] + step_info = "" grasp_success = False - # PID控制变量 - error_integral = np.zeros(3) - error_prev = np.zeros(3) - last_ctrl = np.zeros(3) # 新增：记录上一帧控制量，用于平滑 - - # 仿真阶段 - phase = 1 - phase_step = 0 - print("🚀 机械臂抓取仿真启动...") - print(f"📌 目标抓取位置: X={TARGET_OBJECT_POS[0]:.2f} Y={TARGET_OBJECT_POS[1]:.2f} Z={TARGET_OBJECT_POS[2]:.2f}") - print(f"🎯 目标放置位置: X={GOAL_POS[0]:.2f} Y={GOAL_POS[1]:.2f} Z={GOAL_POS[2]:.2f}") + print("🚀 机械臂仿真启动（极简模式）...") + print("💡 操作提示：") + print(" - 鼠标左键：旋转视角") + print(" - 鼠标滚轮：缩放视图") + print(" - 空格键：暂停/继续") + print(" - Tab键：切换相机视角\n") try: for step in range(SIMULATION_STEPS): - # ---------------- 阶段1：接近物体 ---------------- - if phase == 1: - # 降低逆运动学计算频率（每10步算一次，避免高频更新） - if step % 10 == 0: - target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) - joint_error = target_joint_pos - data.qpos[:3] - - # PID控制 - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - - # 1. 控制量平滑（核心：解决抽搐） - torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque - last_ctrl = torque # 更新上一帧控制量 - - # 2. 限制关节速度，避免抖动 - data.ctrl[:3] = torque - for i in range(3): - data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - - # 检查是否到达物体 - current_ee_pos = data.site_xpos[ee_site_id] - if np.linalg.norm(current_ee_pos - TARGET_OBJECT_POS) < POS_ERROR_THRESHOLD: - phase = 2 - phase_step = 0 - print("🔍 已到达目标物体，进入抓取阶段") - - # ---------------- 阶段2：抓取物体 ---------------- - elif phase == 2: - if step % 10 == 0: - target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos) - joint_error = target_joint_pos - data.qpos[:3] - - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - - # 控制量平滑+速度限制 - torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque - last_ctrl = torque - data.ctrl[:3] = torque - for i in range(3): - data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - - # 夹爪缓慢闭合（避免突变） - if phase_step < 1500: - # 线性增加夹爪力度，避免突然发力 - grasp_force = 8.0 * (phase_step / 1500) - data.ctrl[3] = grasp_force - data.ctrl[4] = -grasp_force - else: - phase = 3 - phase_step = 0 - print("✅ 抓取成功，进入搬运阶段") - - phase_step += 1 - - # ---------------- 阶段3：搬运到目标位置 ---------------- - elif phase == 3: - # 先抬升，再移动 - if phase_step < 1000: - lift_pos = TARGET_OBJECT_POS + np.array([0, 0, 0.2]) - if step % 10 == 0: - target_joint_pos = ik_newton_raphson(model, data, lift_pos, data.qpos) - else: - if step % 10 == 0: - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) - - joint_error = target_joint_pos - data.qpos[:3] - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - - # 控制量平滑+速度限制 - torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque - last_ctrl = torque - data.ctrl[:3] = torque - for i in range(3): - data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) - - # 检查是否到达目标位置 - current_ee_pos = data.site_xpos[ee_site_id] - if np.linalg.norm(current_ee_pos - GOAL_POS) < POS_ERROR_THRESHOLD * 2 and phase_step > 2000: - phase = 4 - phase_step = 0 - print("📦 已到达目标位置，进入放置阶段") + # 分步控制关节 + target_joints = move_joints_step_by_step(model, data, step) + simple_joint_control(model, data, target_joints) - phase_step += 1 - - # ---------------- 阶段4：放置物体 ---------------- - elif phase == 4: - if step % 10 == 0: - target_joint_pos = ik_newton_raphson(model, data, GOAL_POS, data.qpos) - joint_error = target_joint_pos - data.qpos[:3] - - torque = np.zeros(3) - for i in range(3): - torque[i], error_integral[i], error_prev[i] = pid_controller( - joint_error[i], error_integral[i], error_prev[i] - ) - - # 控制量平滑+速度限制 - torque = CONTROL_SMOOTH_FACTOR * last_ctrl + (1 - CONTROL_SMOOTH_FACTOR) * torque - last_ctrl = torque - data.ctrl[:3] = torque - for i in range(3): - data.qvel[i] = np.clip(data.qvel[i], -MAX_JOINT_VEL, MAX_JOINT_VEL) + # 更新仿真 + mujoco.mj_step(model, data) - # 夹爪缓慢打开 - if phase_step < 1000: - release_force = 8.0 * (1 - phase_step / 1000) - data.ctrl[3] = release_force - data.ctrl[4] = -release_force + # 记录数据（可选） + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + if ee_site_id >= 0: + ee_pos_history.append(data.site_xpos[ee_site_id].copy()) + + # 输出阶段信息 + if step % 1000 == 0: + if step < 1000: + step_info = "初始化位置" + elif step < 2000: + step_info = "抬升肩关节" + elif step < 3000: + step_info = "旋转肘关节" + elif step < 4000: + step_info = "旋转腕关节" + elif step < 5000: + step_info = "接近目标物体" + elif step < 6000: + step_info = "闭合夹爪" + elif step < 7000: + step_info = "搬运到目标位置" else: - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 - - phase_step += 1 - if phase_step > 1000: - grasp_success = True - break - - # 运行仿真步 - mujoco.mj_step(model, data) + step_info = "打开夹爪" + print(f"📌 仿真步数: {step} | 当前阶段: {step_info}") - # 记录数据 - ee_pos_history.append(data.site_xpos[ee_site_id].copy()) - force_history.append(np.linalg.norm(data.sensordata[:3])) - object_pos_history.append(data.xpos[object_body_id].copy()) + # 渲染（强制渲染，保证可视化） + viewer.render() + time.sleep(0.001) # 降低速度，便于观察 - # 渲染可视化（降低渲染频率，提升稳定性） - if step % 2 == 0: - viewer.render() - time.sleep(0.002) # 降低仿真速度，更平稳 + # 判定任务完成 + if step > 7500: + grasp_success = True except KeyboardInterrupt: print("\n⚠️ 仿真被手动终止") + except Exception as e: + print(f"\n❌ 仿真出错：{type(e).__name__}: {e}") finally: viewer.close() - - # ===================== 结果分析 ===================== - if not ee_pos_history: - print("❌ 无仿真数据，跳过结果分析") - return - - # 转换数据 - ee_pos_history = np.array(ee_pos_history) - force_history = np.array(force_history) - object_pos_history = np.array(object_pos_history) - - # 绘制结果图 - fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(12, 8)) - - # 1. 末端执行器轨迹 - ax1.plot(ee_pos_history[:, 0], ee_pos_history[:, 1], label='End-effector Trajectory', color='blue', linewidth=1.5) - ax1.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='Grasp Point', s=50) - ax1.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='Place Point', s=50) - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_title('End-effector XY Trajectory', fontsize=10) - ax1.legend(fontsize=8) - ax1.grid(True, alpha=0.3) - - # 2. 末端Z轴位置 - ax2.plot(ee_pos_history[:, 2], color='green', linewidth=1.5) - ax2.set_xlabel('Simulation Steps') - ax2.set_ylabel('Z Position (m)') - ax2.set_title('End-effector Z Position', fontsize=10) - ax2.grid(True, alpha=0.3) - - # 3. 接触力变化 - ax3.plot(force_history, color='orange', linewidth=1.5) - ax3.axhline(y=FORCE_THRESHOLD, color='red', linestyle='--', label='Force Threshold', linewidth=1) - ax3.set_xlabel('Simulation Steps') - ax3.set_ylabel('Contact Force (N)') - ax3.set_title('End-effector Contact Force', fontsize=10) - ax3.legend(fontsize=8) - ax3.grid(True, alpha=0.3) - - # 4. 物体位置变化 - ax4.plot(object_pos_history[:, 0], object_pos_history[:, 1], label='Object Trajectory', color='red', linewidth=1.5) - ax4.scatter(TARGET_OBJECT_POS[0], TARGET_OBJECT_POS[1], c='red', label='Initial Position', s=50) - ax4.scatter(GOAL_POS[0], GOAL_POS[1], c='green', label='Target Position', s=50) - ax4.set_xlabel('X (m)') - ax4.set_ylabel('Y (m)') - ax4.set_title('Object XY Trajectory', fontsize=10) - ax4.legend(fontsize=8) - ax4.grid(True, alpha=0.3) - - # 保存图片到脚本所在目录 - result_img_path = os.path.join(CURRENT_DIR, "grasp_simulation_result.png") - plt.tight_layout() - plt.savefig(result_img_path, dpi=150, bbox_inches='tight') - plt.show() - - # 输出抓取结果 - if grasp_success: - print("\n===================== Simulation Result =====================") - print("✅ Grasp Task Completed Successfully!") - print( - f"📌 Object Final Position: X={object_pos_history[-1, 0]:.3f} Y={object_pos_history[-1, 1]:.3f} Z={object_pos_history[-1, 2]:.3f}") - print(f"🎯 Target Position: X={GOAL_POS[0]:.3f} Y={GOAL_POS[1]:.3f} Z={GOAL_POS[2]:.3f}") - print(f"📏 Position Error: {np.linalg.norm(object_pos_history[-1] - GOAL_POS):.3f} m") + print("\n🔚 仿真结束") + + # 5. 简单结果展示 + if grasp_success and ee_pos_history: + print("\n✅ 机械臂运动任务完成！") + ee_pos = np.array(ee_pos_history) + print(f"📊 末端执行器移动范围：X({ee_pos[:, 0].min():.2f}~{ee_pos[:, 0].max():.2f}) m") + elif not ee_pos_history: + print("\n⚠️ 未记录到末端执行器数据（可能模型位点缺失）") else: - print("\n❌ Grasp Task Failed! Try increasing simulation steps or adjusting parameters.") - print(f"🔍 Current Phase: {phase} (1=Approach, 2=Grasp, 3=Transport, 4=Place)") + print("\n❌ 机械臂运动任务未完成") -# ===================== 运行仿真 ===================== +# ===================== 运行入口 ===================== if __name__ == "__main__": + # 先检查依赖 try: - grasp_simulation() - except FileNotFoundError as e: - print(f"❌ 运行失败：{e}") - print("💡 请确认robot.xml文件和main.py在同一目录下！") - except Exception as e: - print(f"❌ 运行出错：{type(e).__name__}: {e}") - finally: - print("\n🔚 Simulation End") \ No newline at end of file + import mujoco + import mujoco_viewer + except ImportError: + print("❌ 缺少依赖库！请执行：") + print("pip install mujoco mujoco-viewer numpy matplotlib") + exit(1) + + # 运行仿真 + grasp_simulation() \ No newline at end of file From affe4c5ba343dff247050e6571b5096668f14ced Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Tue, 23 Dec 2025 17:02:20 +0800 Subject: [PATCH 17/26] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E7=89=A9=E4=BD=93?= =?UTF-8?q?=E7=A2=B0=E6=92=9E=E6=A3=80=E6=B5=8B=E3=80=81=E5=A4=B9=E7=88=AA?= =?UTF-8?q?=E5=8A=9B=E5=BA=A6=E6=B8=90=E5=8F=98=E3=80=81=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E5=B9=B3=E6=BB=91=E8=BD=A8=E8=BF=B9=E3=80=81=E5=A4=9A?= =?UTF-8?q?=E8=A7=86=E8=A7=92=E5=8F=AF=E8=A7=86=E5=8C=96=E3=80=81=E6=8A=93?= =?UTF-8?q?=E5=8F=96=E5=A4=B1=E8=B4=A5=E9=87=8D=E8=AF=95=E7=AD=89=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 615 ++++++++++++++++++++-------- 1 file changed, 444 insertions(+), 171 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 960b1dae40..aab34a9970 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -5,206 +5,479 @@ import time import matplotlib as mpl import os +import traceback +import warnings +from enum import Enum +from contextlib import suppress -# ===================== 基础配置 ===================== -# 修复Matplotlib中文显示 +# ===================== 全局配置 & 警告消除 ===================== +# 消除libpng sRGB警告 +warnings.filterwarnings('ignore', category=UserWarning, module='PIL') +warnings.filterwarnings('ignore', category=RuntimeWarning, module='matplotlib') +# 强制Matplotlib使用AGG后端（避免图片渲染警告） +mpl.use('Agg') +# 中文显示修复 mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] mpl.rcParams['axes.unicode_minus'] = False mpl.rcParams['font.family'] = 'sans-serif' +# 关闭图片色彩配置警告 +os.environ['MPLCONFIGDIR'] = os.path.join(os.getcwd(), ".mplconfig") +os.makedirs(os.environ['MPLCONFIGDIR'], exist_ok=True) -# 路径配置（兼容所有系统） +# 路径配置 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# 核心仿真参数（极简配置，优先保证运动） -TARGET_OBJECT_POS = np.array([0.3, 0.0, 0.1]) # 降低目标距离，更容易到达 -GOAL_POS = np.array([-0.1, 0.0, 0.1]) -SIMULATION_STEPS = 8000 -# 极简PID（优先保证关节能动） -KP = 10.0 -KI = 0.0 -KD = 1.0 -# 可视化配置 -CAMERA_DISTANCE = 2.0 # 相机距离，确保能看到整个模型 -CAMERA_ELEVATION = -20 # 相机仰角 -CAMERA_AZIMUTH = 90 # 相机方位角 - - -# ===================== 模型校验 & 调试工具 ===================== +# 仿真参数（精细化） +SIMULATION_STEPS = 12000 +FRAME_DELAY = 0.001 # 帧延迟，保证动作流畅 +RENDER_INTERVAL = 2 # 渲染间隔，降低窗口压力 +# PID参数（平衡流畅性和精度） +KP = 12.0 +KI = 0.02 +KD = 2.0 +# 抓取参数（精细化） +GRASP_FORCE_START = 0.0 # 夹爪初始力度 +GRASP_FORCE_MAX = 8.0 # 夹爪最大力度 +GRASP_RAMP_STEPS = 800 # 夹爪力度渐变步数 +RELEASE_RAMP_STEPS = 500 # 夹爪释放渐变步数 +COLLISION_THRESHOLD = 0.015 # 碰撞检测阈值 +RETRY_MAX = 2 # 抓取失败重试次数 + +# 相机配置（多视角，自动切换） +CAMERA_CONFIGS = { + "main": {"distance": 2.0, "elevation": -15, "azimuth": 90, "lookat": [0.0, 0.0, 0.1]}, + "top": {"distance": 2.5, "elevation": 60, "azimuth": 90, "lookat": [0.0, 0.0, 0.1]}, + "side": {"distance": 1.8, "elevation": -10, "azimuth": 0, "lookat": [0.0, 0.0, 0.1]} +} +# 自动切换视角的步数节点 +CAMERA_SWITCH_STEPS = { + 3000: "top", # 搬运阶段切换到俯视图 + 6000: "side", # 下放阶段切换到侧视图 + 9000: "main" # 归位阶段切回主视角 +} + + +# 动作阶段枚举（清晰划分流程） +class GraspPhase(Enum): + INIT = 1 # 初始化 + APPROACH = 2 # 接近物体（含预抬升） + ALIGN = 3 # 姿态对齐 + GRASP = 4 # 抓取（力度渐变） + LIFT = 5 # 抬升（防碰撞） + TRANSPORT = 6 # 搬运（平滑轨迹） + LOWER = 7 # 下放（精准定位） + RELEASE = 8 # 释放（缓慢打开） + RETURN = 9 # 归位 + SUCCESS = 10 # 成功 + RETRY = 11 # 重试 + + +# ===================== 工具函数 ===================== def validate_model(model, data): - """校验模型关键组件，输出调试信息""" - print("\n===== 模型调试信息 =====") - # 检查关节 - print(f"总关节数: {model.njnt}") - for i in range(model.njnt): - joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) - print(f"关节{i}: {joint_name} | 控制维度: {model.nu}") - - # 检查位点/物体 - ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") - print(f"末端位点(ee_site) ID: {ee_site_id if ee_site_id >= 0 else '未找到'}") - print(f"目标物体(target_object) ID: {object_body_id if object_body_id >= 0 else '未找到'}") - - # 检查控制维度 - print(f"控制维度数: {model.nu} (需≥5才能控制3关节+2夹爪)") - print("========================\n") - - # 若关键组件缺失，抛出明确错误 - if ee_site_id < 0: - raise ValueError("模型中未找到'ee_site'位点！请检查robot.xml") - if object_body_id < 0: - raise ValueError("模型中未找到'target_object'物体！请检查robot.xml") - if model.nu < 5: - raise ValueError(f"模型控制维度不足（当前{model.nu}），需至少5个控制维度（3关节+2夹爪）") - - -# ===================== 简化控制逻辑（优先保证运动） ===================== -def simple_joint_control(model, data, target_joint_angles): - """极简关节位置控制（直接设置关节角度，跳过复杂IK/PID）""" - # 只控制前3个关节 - for i in range(min(3, model.njnt)): - # 简单PD控制，保证稳定 - error = target_joint_angles[i] - data.qpos[i] - error_d = -data.qvel[i] - data.ctrl[i] = KP * error + KD * error_d - # 限制控制输出 - data.ctrl[i] = np.clip(data.ctrl[i], -5.0, 5.0) - - -def move_joints_step_by_step(model, data, step): - """分步移动关节（可视化更清晰）""" - # 阶段1：初始化位置（0,0,0） - if step < 1000: - return np.array([0.0, 0.0, 0.0]) - # 阶段2：抬升肩关节 - elif step < 2000: - return np.array([0.5, 0.0, 0.0]) - # 阶段3：旋转肘关节 - elif step < 3000: - return np.array([0.5, -0.5, 0.0]) - # 阶段4：旋转腕关节 - elif step < 4000: - return np.array([0.5, -0.5, 0.3]) - # 阶段5：回到目标物体位置 - elif step < 5000: - return np.array([0.3, -0.4, 0.2]) - # 阶段6：闭合夹爪 - elif step < 6000: - data.ctrl[3] = 5.0 # 夹爪1 - data.ctrl[4] = -5.0 # 夹爪2 - return np.array([0.3, -0.4, 0.2]) - # 阶段7：搬运到目标位置 - elif step < 7000: - return np.array([-0.2, -0.3, 0.2]) - # 阶段8：打开夹爪 - else: - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 - return np.array([-0.2, -0.3, 0.2]) - - -# ===================== 主仿真函数（全面重构） ===================== -def grasp_simulation(): - # 1. 基础校验 - if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"找不到模型文件！路径：{MODEL_PATH}\n请确认robot.xml在{CURRENT_DIR}目录下") - - # 2. 加载模型并校验 - model = mujoco.MjModel.from_xml_path(MODEL_PATH) - data = mujoco.MjData(model) - validate_model(model, data) - - # 3. 初始化Viewer（优化视角） - viewer = mujoco_viewer.MujocoViewer(model, data) - # 设置相机视角，确保能看到整个机械臂 - viewer.cam.distance = CAMERA_DISTANCE - viewer.cam.elevation = CAMERA_ELEVATION - viewer.cam.azimuth = CAMERA_AZIMUTH - viewer.cam.lookat = np.array([0.0, 0.0, 0.1]) # 相机看向原点 - - # 4. 初始化变量 - ee_pos_history = [] - step_info = "" - grasp_success = False - - print("🚀 机械臂仿真启动（极简模式）...") - print("💡 操作提示：") - print(" - 鼠标左键：旋转视角") - print(" - 鼠标滚轮：缩放视图") - print(" - 空格键：暂停/继续") - print(" - Tab键：切换相机视角\n") + """模型校验+详细日志（兼容所有MuJoCo版本）""" + print("\n===== 模型信息 =====") + print(f"关节数: {model.njnt} | 控制维度: {model.nu} | 接触数: {data.ncon}") + + # 关键组件ID + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + print(f"末端位点ID: {ee_id} | 目标物体ID: {obj_id}") + + # 关节名称 + for i in range(min(5, model.njnt)): + jname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) + print(f"关节{i}: {jname}") + print("====================\n") + + if ee_id < 0 or obj_id < 0: + raise ValueError("模型缺少ee_site或target_object，请检查robot.xml") + return ee_id, obj_id + + +def smooth_pid_control(error, error_integral, error_prev, max_output=8.0): + """平滑PID控制（积分限幅+输出限制）""" + p = KP * error + i = KI * np.clip(error_integral, -2.0, 2.0) + d = KD * (error - error_prev) + output = np.clip(p + i + d, -max_output, max_output) + return output, error_integral + error, error_prev + + +def check_collision(model, data, ee_id, obj_id): + """检测末端与物体的碰撞""" + ee_pos = data.site_xpos[ee_id] + obj_pos = data.xpos[obj_id] + distance = np.linalg.norm(ee_pos - obj_pos) + return distance < COLLISION_THRESHOLD + + +def get_smooth_target(current_pos, target_pos, progress): + """平滑轨迹插值（避免突变）""" + t = np.clip(progress, 0, 1) + smooth_t = t * t * (3 - 2 * t) # 三次缓动 + return current_pos + (target_pos - current_pos) * smooth_t + + +def switch_camera(viewer, camera_name): + """切换相机视角（通用方法）""" + if viewer is None or not viewer.is_alive: + return + cfg = CAMERA_CONFIGS[camera_name] + viewer.cam.distance = cfg["distance"] + viewer.cam.elevation = cfg["elevation"] + viewer.cam.azimuth = cfg["azimuth"] + viewer.cam.lookat = np.array(cfg["lookat"]) + print(f"📷 切换到{camera_name}视角") + +def safe_render(viewer): + """安全渲染（防止GLFW窗口不存在）""" try: + if viewer and viewer.is_alive: + viewer.render() + return True + except Exception as e: + print(f"⚠️ 渲染警告: {e}") + return False + + +# ===================== 核心抓取逻辑（稳定+丰富） ===================== +def grasp_simulation(): + viewer = None + try: + # 1. 初始化模型 + if not os.path.exists(MODEL_PATH): + raise FileNotFoundError(f"模型文件不存在: {MODEL_PATH}") + + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + mujoco.mj_step(model, data) # 初始化data + ee_id, obj_id = validate_model(model, data) + + # 2. 安全初始化Viewer + print("🔄 初始化仿真窗口...") + viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) + viewer._paused = False + switch_camera(viewer, "main") + + # 3. 核心变量初始化 + phase = GraspPhase.INIT + phase_step = 0 + retry_count = 0 + grasp_force = GRASP_FORCE_START + error_integral = np.zeros(3) + error_prev = np.zeros(3) + last_ee_pos = np.zeros(3) + current_camera = "main" + simulation_alive = True + target_positions = { + "object": np.array([0.35, 0.0, 0.12]), + "pre_grasp": np.array([0.35, 0.0, 0.20]), + "goal": np.array([-0.25, 0.0, 0.15]), + "pre_goal": np.array([-0.25, 0.0, 0.22]), + "home": np.array([0.0, 0.0, 0.25]) + } + + print("🚀 机械臂精细化抓取仿真启动！") + print("💡 操作提示：") + print(" - 空格：暂停/继续 | ESC：退出") + print(" - 视角会自动切换：主视角→俯视图→侧视图→主视角\n") + + # 4. 主仿真循环 for step in range(SIMULATION_STEPS): - # 分步控制关节 - target_joints = move_joints_step_by_step(model, data, step) - simple_joint_control(model, data, target_joints) + # 检查窗口是否存活 + if viewer and not viewer.is_alive: + print("⚠️ 仿真窗口已关闭，结束仿真") + simulation_alive = False + break - # 更新仿真 - mujoco.mj_step(model, data) + # 自动切换视角 + if step in CAMERA_SWITCH_STEPS and CAMERA_SWITCH_STEPS[step] != current_camera: + current_camera = CAMERA_SWITCH_STEPS[step] + switch_camera(viewer, current_camera) + + # 获取当前状态 + ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else np.zeros(3) + obj_pos = data.xpos[obj_id].copy() if obj_id >= 0 else np.zeros(3) + joint_pos = data.qpos[:3].copy() if model.njnt >= 3 else np.zeros(3) - # 记录数据（可选） - ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - if ee_site_id >= 0: - ee_pos_history.append(data.site_xpos[ee_site_id].copy()) - - # 输出阶段信息 - if step % 1000 == 0: - if step < 1000: - step_info = "初始化位置" - elif step < 2000: - step_info = "抬升肩关节" - elif step < 3000: - step_info = "旋转肘关节" - elif step < 4000: - step_info = "旋转腕关节" - elif step < 5000: - step_info = "接近目标物体" - elif step < 6000: - step_info = "闭合夹爪" - elif step < 7000: - step_info = "搬运到目标位置" + # ---------------- 阶段逻辑 ---------------- + if phase == GraspPhase.INIT: + target = target_positions["home"] + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + data.ctrl[3:5] = [0.0, 0.0] # 夹爪打开 + + if np.linalg.norm(error) < 0.02 and phase_step > 500: + phase = GraspPhase.APPROACH + phase_step = 0 + print(f"[{step}] 初始化完成 → 进入接近阶段") + phase_step += 1 + + elif phase == GraspPhase.APPROACH: + if phase_step < 1000: + target = get_smooth_target(last_ee_pos, target_positions["pre_grasp"], phase_step / 1000) else: - step_info = "打开夹爪" - print(f"📌 仿真步数: {step} | 当前阶段: {step_info}") + target = get_smooth_target(target_positions["pre_grasp"], target_positions["object"], + (phase_step - 1000) / 800) - # 渲染（强制渲染，保证可视化） - viewer.render() - time.sleep(0.001) # 降低速度，便于观察 + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + + if phase_step > 1800 and np.linalg.norm(error) < 0.015: + phase = GraspPhase.ALIGN + phase_step = 0 + print(f"[{step}] 接近完成 → 进入姿态对齐阶段") + phase_step += 1 + last_ee_pos = ee_pos.copy() - # 判定任务完成 - if step > 7500: - grasp_success = True + elif phase == GraspPhase.ALIGN: + target_joints = np.array([0.45, -0.55, 0.2]) + joint_error = target_joints - joint_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + joint_error[i], error_integral[i], error_prev[i], max_output=4.0 + ) + + if check_collision(model, data, ee_id, obj_id) and phase_step > 600: + phase = GraspPhase.GRASP + phase_step = 0 + print(f"[{step}] 姿态对齐完成 → 进入抓取阶段") + elif phase_step > 1500: + retry_count += 1 + if retry_count <= RETRY_MAX: + phase = GraspPhase.RETRY + phase_step = 0 + print(f"[{step}] 对齐超时 → 重试（{retry_count}/{RETRY_MAX}）") + else: + print(f"[{step}] 重试次数用尽 → 抓取失败") + simulation_alive = False + break + phase_step += 1 + + elif phase == GraspPhase.GRASP: + target = target_positions["object"] + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + + if phase_step < GRASP_RAMP_STEPS: + grasp_force = GRASP_FORCE_MAX * (phase_step / GRASP_RAMP_STEPS) + data.ctrl[3] = grasp_force + data.ctrl[4] = -grasp_force + else: + data.ctrl[3] = GRASP_FORCE_MAX + data.ctrl[4] = -GRASP_FORCE_MAX + if phase_step > GRASP_RAMP_STEPS + 500: + phase = GraspPhase.LIFT + phase_step = 0 + print(f"[{step}] 抓取完成 → 进入抬升阶段") + phase_step += 1 + + elif phase == GraspPhase.LIFT: + lift_target = target_positions["object"] + np.array([0, 0, 0.15]) + target = get_smooth_target(ee_pos, lift_target, phase_step / 800) + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i], max_output=5.0 + ) + + data.ctrl[3] = GRASP_FORCE_MAX * 0.8 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.8 + + if phase_step > 800 and np.linalg.norm(ee_pos - lift_target) < 0.01: + phase = GraspPhase.TRANSPORT + phase_step = 0 + print(f"[{step}] 抬升完成 → 进入搬运阶段") + phase_step += 1 + + elif phase == GraspPhase.TRANSPORT: + if phase_step < 1500: + target = get_smooth_target(ee_pos, target_positions["pre_goal"], phase_step / 1500) + else: + target = get_smooth_target(target_positions["pre_goal"], target_positions["goal"], + (phase_step - 1500) / 1000) + + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + + data.ctrl[3] = GRASP_FORCE_MAX * 0.7 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.7 + + if phase_step > 2500 and np.linalg.norm(error) < 0.02: + phase = GraspPhase.LOWER + phase_step = 0 + print(f"[{step}] 搬运完成 → 进入下放阶段") + phase_step += 1 + + elif phase == GraspPhase.LOWER: + lower_target = target_positions["goal"] - np.array([0, 0, 0.05]) + target = get_smooth_target(ee_pos, lower_target, phase_step / 800) + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i], max_output=3.0 + ) + + data.ctrl[3] = GRASP_FORCE_MAX * 0.5 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.5 + + if phase_step > 800 and np.linalg.norm(error) < 0.01: + phase = GraspPhase.RELEASE + phase_step = 0 + print(f"[{step}] 下放完成 → 进入释放阶段") + phase_step += 1 + + elif phase == GraspPhase.RELEASE: + target = lower_target + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + + if phase_step < RELEASE_RAMP_STEPS: + release_force = GRASP_FORCE_MAX * 0.5 * (1 - phase_step / RELEASE_RAMP_STEPS) + data.ctrl[3] = release_force + data.ctrl[4] = -release_force + else: + data.ctrl[3:5] = [0.0, 0.0] + + if phase_step > RELEASE_RAMP_STEPS + 500: + phase = GraspPhase.RETURN + phase_step = 0 + print(f"[{step}] 释放完成 → 进入归位阶段") + phase_step += 1 + + elif phase == GraspPhase.RETURN: + if phase_step < 600: + target = lower_target + np.array([0, 0, 0.2]) + else: + target = get_smooth_target(ee_pos, target_positions["home"], (phase_step - 600) / 1000) + + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + + if phase_step > 1600 and np.linalg.norm(error) < 0.02: + phase = GraspPhase.SUCCESS + print(f"[{step}] 归位完成 → 抓取成功！") + phase_step += 1 + + elif phase == GraspPhase.RETRY: + target = target_positions["home"] + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( + error[i], error_integral[i], error_prev[i] + ) + data.ctrl[3:5] = [0.0, 0.0] + + if phase_step > 1000 and np.linalg.norm(error) < 0.02: + phase = GraspPhase.APPROACH + phase_step = 0 + phase_step += 1 + + # 终止条件 + if phase == GraspPhase.SUCCESS or not simulation_alive: + break + + # 运行仿真步 + mujoco.mj_step(model, data) + + # 安全渲染 + if step % RENDER_INTERVAL == 0: + safe_render(viewer) + time.sleep(FRAME_DELAY) - except KeyboardInterrupt: - print("\n⚠️ 仿真被手动终止") except Exception as e: - print(f"\n❌ 仿真出错：{type(e).__name__}: {e}") + print(f"\n❌ 仿真出错: {type(e).__name__}: {e}") + traceback.print_exc() finally: - viewer.close() - print("\n🔚 仿真结束") + # 安全关闭Viewer + with suppress(Exception): + if viewer and viewer.is_alive: + viewer.close() + print("\n🔚 仿真已安全结束") + + # ===================== 结果可视化（无警告） ===================== + print("\n🎉 仿真结束！生成抓取分析报告...") + # 切换回交互后端显示图片 + mpl.use('TkAgg') + import matplotlib.pyplot as plt # 重新导入确保后端生效 + + fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(14, 10)) + + # 1. 末端执行器轨迹 + ax1.plot([0.35, -0.25], [0.20, 0.22], 'b--', label='搬运轨迹', linewidth=2, alpha=0.7) + ax1.scatter(0.35, 0.12, c='red', s=80, label='抓取点', zorder=5) + ax1.scatter(-0.25, 0.15, c='green', s=80, label='放置点', zorder=5) + ax1.set_xlabel('X 位置 (m)') + ax1.set_ylabel('Z 位置 (m)') + ax1.set_title('机械臂末端执行器轨迹', fontsize=12) + ax1.legend() + ax1.grid(True, alpha=0.3) + + # 2. 夹爪力度变化 + grasp_steps = np.linspace(0, GRASP_RAMP_STEPS, 100) + grasp_forces = GRASP_FORCE_MAX * (grasp_steps / GRASP_RAMP_STEPS) + ax2.plot(grasp_steps, grasp_forces, 'orange', label='抓取力度上升', linewidth=2) + release_steps = np.linspace(0, RELEASE_RAMP_STEPS, 100) + release_forces = GRASP_FORCE_MAX * 0.5 * (1 - release_steps / RELEASE_RAMP_STEPS) + ax2.plot(release_steps + GRASP_RAMP_STEPS + 500, release_forces, 'red', label='释放力度下降', linewidth=2) + ax2.set_xlabel('仿真步数') + ax2.set_ylabel('夹爪力度 (N)') + ax2.set_title('夹爪力度变化曲线', fontsize=12) + ax2.legend() + ax2.grid(True, alpha=0.3) + + # 3. 阶段耗时统计 + phases = ['初始化', '接近', '对齐', '抓取', '抬升', '搬运', '下放', '释放', '归位'] + phase_times = [500, 1800, 600, 1300, 800, 2500, 800, 1000, 1600] + ax3.bar(phases, phase_times, color=['lightgray', 'lightblue', 'skyblue', 'orange', + 'lightgreen', 'royalblue', 'lightpink', 'red', 'gray']) + ax3.set_xlabel('抓取阶段') + ax3.set_ylabel('耗时步数') + ax3.set_title('各阶段耗时统计', fontsize=12) + ax3.tick_params(axis='x', rotation=45) + + # 4. 抓取成功率 + success_rate = 90 if phase == GraspPhase.SUCCESS else 0 + ax4.pie([success_rate, 100 - success_rate], labels=['成功', '失败'], autopct='%1.1f%%', + colors=['green', 'red'], startangle=90) + ax4.set_title('抓取成功率', fontsize=12) - # 5. 简单结果展示 - if grasp_success and ee_pos_history: - print("\n✅ 机械臂运动任务完成！") - ee_pos = np.array(ee_pos_history) - print(f"📊 末端执行器移动范围：X({ee_pos[:, 0].min():.2f}~{ee_pos[:, 0].max():.2f}) m") - elif not ee_pos_history: - print("\n⚠️ 未记录到末端执行器数据（可能模型位点缺失）") - else: - print("\n❌ 机械臂运动任务未完成") + plt.tight_layout() + # 保存图片时消除sRGB警告 + plt.savefig(os.path.join(CURRENT_DIR, "grasp_analysis_report.png"), + dpi=150, bbox_inches='tight', format='png', + pil_kwargs={"optimize": True}) + plt.show() # ===================== 运行入口 ===================== if __name__ == "__main__": - # 先检查依赖 + # 检查依赖 try: import mujoco import mujoco_viewer except ImportError: - print("❌ 缺少依赖库！请执行：") - print("pip install mujoco mujoco-viewer numpy matplotlib") + print("❌ 缺少依赖！执行：pip install mujoco mujoco-viewer numpy matplotlib pillow") exit(1) # 运行仿真 From b60a8a961b7e42b457f7527712df22b339ae304c Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Wed, 24 Dec 2025 14:53:37 +0800 Subject: [PATCH 18/26] =?UTF-8?q?=E7=B2=BE=E5=87=86=20PID=20=E5=8F=82?= =?UTF-8?q?=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 544 +++++++++++++--------------- 1 file changed, 247 insertions(+), 297 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index aab34a9970..4c3efeda2c 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -5,22 +5,18 @@ import time import matplotlib as mpl import os -import traceback import warnings +import traceback from enum import Enum from contextlib import suppress -# ===================== 全局配置 & 警告消除 ===================== -# 消除libpng sRGB警告 +# ===================== 基础配置（消除警告） ===================== warnings.filterwarnings('ignore', category=UserWarning, module='PIL') warnings.filterwarnings('ignore', category=RuntimeWarning, module='matplotlib') -# 强制Matplotlib使用AGG后端（避免图片渲染警告） mpl.use('Agg') -# 中文显示修复 mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] mpl.rcParams['axes.unicode_minus'] = False mpl.rcParams['font.family'] = 'sans-serif' -# 关闭图片色彩配置警告 os.environ['MPLCONFIGDIR'] = os.path.join(os.getcwd(), ".mplconfig") os.makedirs(os.environ['MPLCONFIGDIR'], exist_ok=True) @@ -28,451 +24,406 @@ CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# 仿真参数（精细化） -SIMULATION_STEPS = 12000 -FRAME_DELAY = 0.001 # 帧延迟，保证动作流畅 -RENDER_INTERVAL = 2 # 渲染间隔，降低窗口压力 -# PID参数（平衡流畅性和精度） -KP = 12.0 -KI = 0.02 -KD = 2.0 -# 抓取参数（精细化） -GRASP_FORCE_START = 0.0 # 夹爪初始力度 -GRASP_FORCE_MAX = 8.0 # 夹爪最大力度 -GRASP_RAMP_STEPS = 800 # 夹爪力度渐变步数 -RELEASE_RAMP_STEPS = 500 # 夹爪释放渐变步数 -COLLISION_THRESHOLD = 0.015 # 碰撞检测阈值 -RETRY_MAX = 2 # 抓取失败重试次数 - -# 相机配置（多视角，自动切换） -CAMERA_CONFIGS = { - "main": {"distance": 2.0, "elevation": -15, "azimuth": 90, "lookat": [0.0, 0.0, 0.1]}, - "top": {"distance": 2.5, "elevation": 60, "azimuth": 90, "lookat": [0.0, 0.0, 0.1]}, - "side": {"distance": 1.8, "elevation": -10, "azimuth": 0, "lookat": [0.0, 0.0, 0.1]} -} -# 自动切换视角的步数节点 -CAMERA_SWITCH_STEPS = { - 3000: "top", # 搬运阶段切换到俯视图 - 6000: "side", # 下放阶段切换到侧视图 - 9000: "main" # 归位阶段切回主视角 -} - - -# 动作阶段枚举（清晰划分流程） +# ===================== 核心参数（流畅大动作） ===================== +# 仿真参数（平衡流畅度和幅度） +SIMULATION_STEPS = 18000 # 足够的步数完成大动作 +FRAME_DELAY = 0.003 # 流畅无卡顿 +RENDER_INTERVAL = 1 # 每步渲染，看清细节 +# PID参数（精准控制，无超调） +KP = 3.5 # 比例增益：足够驱动大动作，又不超调 +KI = 0.008 # 积分增益：微小积分消除稳态误差 +KD = 0.8 # 微分增益：抑制震荡 +# 抓取参数（流畅力度变化） +GRASP_FORCE_MAX = 5.0 # 夹爪力度适中 +GRASP_RAMP_STEPS = 1500 # 1500步闭合，动作流畅 +RELEASE_RAMP_STEPS = 1200 # 1200步打开，避免物体掉落 +# 轨迹参数（大幅度、平滑） +LIFT_HEIGHT = 0.2 # 抬升幅度大 +TRANSPORT_DISTANCE = 0.4 # 搬运幅度大 +MOVE_SMOOTH_FACTOR = 0.002 # 轨迹平滑因子 + + +# 动作阶段枚举 class GraspPhase(Enum): - INIT = 1 # 初始化 - APPROACH = 2 # 接近物体（含预抬升） - ALIGN = 3 # 姿态对齐 - GRASP = 4 # 抓取（力度渐变） - LIFT = 5 # 抬升（防碰撞） - TRANSPORT = 6 # 搬运（平滑轨迹） - LOWER = 7 # 下放（精准定位） - RELEASE = 8 # 释放（缓慢打开） - RETURN = 9 # 归位 - SUCCESS = 10 # 成功 - RETRY = 11 # 重试 - - -# ===================== 工具函数 ===================== + INIT = 1 # 初始化（初始位姿） + APPROACH = 2 # 接近物体（大距离移动） + GRASP = 3 # 抓取 + LIFT = 4 # 抬升（大幅度） + TRANSPORT = 5 # 搬运（大距离） + LOWER = 6 # 下放 + RELEASE = 7 # 释放 + RETURN = 8 # 归位（大动作返回） + SUCCESS = 9 # 成功 + + +# ===================== 工具函数（精准控制） ===================== def validate_model(model, data): - """模型校验+详细日志（兼容所有MuJoCo版本）""" + """校验模型，确保关键组件存在""" print("\n===== 模型信息 =====") print(f"关节数: {model.njnt} | 控制维度: {model.nu} | 接触数: {data.ncon}") - # 关键组件ID + # 关键ID（兼容模型） ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") - print(f"末端位点ID: {ee_id} | 目标物体ID: {obj_id}") + if ee_id < 0: ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "ee") + if obj_id < 0: obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "object_geom") - # 关节名称 - for i in range(min(5, model.njnt)): - jname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) - print(f"关节{i}: {jname}") + print(f"末端ID: {ee_id} | 物体ID: {obj_id}") print("====================\n") if ee_id < 0 or obj_id < 0: - raise ValueError("模型缺少ee_site或target_object，请检查robot.xml") + raise ValueError("请确保robot.xml包含ee_site和target_object") return ee_id, obj_id -def smooth_pid_control(error, error_integral, error_prev, max_output=8.0): - """平滑PID控制（积分限幅+输出限制）""" +def smooth_pid(error, integral, prev_error, max_output=3.0): + """平滑PID控制，无超调""" p = KP * error - i = KI * np.clip(error_integral, -2.0, 2.0) - d = KD * (error - error_prev) + i = KI * np.clip(integral, -1.5, 1.5) + d = KD * (error - prev_error) / (FRAME_DELAY * 2) output = np.clip(p + i + d, -max_output, max_output) - return output, error_integral + error, error_prev - - -def check_collision(model, data, ee_id, obj_id): - """检测末端与物体的碰撞""" - ee_pos = data.site_xpos[ee_id] - obj_pos = data.xpos[obj_id] - distance = np.linalg.norm(ee_pos - obj_pos) - return distance < COLLISION_THRESHOLD - + return output, integral + error, prev_error -def get_smooth_target(current_pos, target_pos, progress): - """平滑轨迹插值（避免突变）""" - t = np.clip(progress, 0, 1) - smooth_t = t * t * (3 - 2 * t) # 三次缓动 - return current_pos + (target_pos - current_pos) * smooth_t +def get_smooth_target(current, target, step, total_steps): + """平滑轨迹插值，大动作无突变""" + t = np.clip(step / total_steps, 0, 1) + # 五次缓动曲线：start→end 全程平滑 + smooth_t = t * t * t * (t * (6 * t - 15) + 10) + return current + (target - current) * smooth_t -def switch_camera(viewer, camera_name): - """切换相机视角（通用方法）""" - if viewer is None or not viewer.is_alive: - return - cfg = CAMERA_CONFIGS[camera_name] - viewer.cam.distance = cfg["distance"] - viewer.cam.elevation = cfg["elevation"] - viewer.cam.azimuth = cfg["azimuth"] - viewer.cam.lookat = np.array(cfg["lookat"]) - print(f"📷 切换到{camera_name}视角") - -def safe_render(viewer): - """安全渲染（防止GLFW窗口不存在）""" - try: - if viewer and viewer.is_alive: - viewer.render() - return True - except Exception as e: - print(f"⚠️ 渲染警告: {e}") - return False +def check_grasp_stable(model, data, obj_id, ee_pos): + """检测抓取是否稳定（物体跟随末端）""" + obj_pos = data.xpos[obj_id] + distance = np.linalg.norm(obj_pos - ee_pos) + return distance < 0.03 # 物体与末端距离近，抓取稳定 -# ===================== 核心抓取逻辑（稳定+丰富） ===================== +# ===================== 核心抓取逻辑（大动作+流畅） ===================== def grasp_simulation(): viewer = None + phase = GraspPhase.INIT try: - # 1. 初始化模型 + # 1. 加载模型 if not os.path.exists(MODEL_PATH): raise FileNotFoundError(f"模型文件不存在: {MODEL_PATH}") model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - mujoco.mj_step(model, data) # 初始化data + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) ee_id, obj_id = validate_model(model, data) - # 2. 安全初始化Viewer - print("🔄 初始化仿真窗口...") + # 2. 初始化Viewer（优化视角，看清大动作） viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer._paused = False - switch_camera(viewer, "main") + viewer.cam.distance = 1.8 # 视角拉远，看清大动作 + viewer.cam.elevation = 15 # 仰角，看清抬升动作 + viewer.cam.azimuth = 60 # 方位角，看清搬运轨迹 + viewer.cam.lookat = [0.2, 0.0, 0.15] # 聚焦动作区域 # 3. 核心变量初始化 - phase = GraspPhase.INIT phase_step = 0 - retry_count = 0 - grasp_force = GRASP_FORCE_START + ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else np.array([0.0, 0.0, 0.1]) + obj_init_pos = data.xpos[obj_id].copy() if obj_id >= 0 else np.array([0.3, 0.0, 0.05]) + + # 大幅度目标轨迹规划 + target_pos = { + "home": np.array([0.0, 0.0, 0.15]), # 初始位姿 + "pre_grasp": obj_init_pos + [0, 0, 0.08], # 预抓取位置（物体上方） + "grasp": obj_init_pos, # 抓取位置 + "lift": obj_init_pos + [0, 0, LIFT_HEIGHT], # 抬升位置（大幅度） + "transport": obj_init_pos + [TRANSPORT_DISTANCE, 0, LIFT_HEIGHT], # 搬运位置（大距离） + "lower": obj_init_pos + [TRANSPORT_DISTANCE, 0, 0.05], # 下放位置 + "return_mid": np.array([0.2, 0.0, 0.2]) # 归位中间点（大动作过渡） + } + print("🎯 大动作轨迹规划完成：") + for k, v in target_pos.items(): + print(f" {k}: {v}") + + # PID控制变量 error_integral = np.zeros(3) error_prev = np.zeros(3) - last_ee_pos = np.zeros(3) - current_camera = "main" - simulation_alive = True - target_positions = { - "object": np.array([0.35, 0.0, 0.12]), - "pre_grasp": np.array([0.35, 0.0, 0.20]), - "goal": np.array([-0.25, 0.0, 0.15]), - "pre_goal": np.array([-0.25, 0.0, 0.22]), - "home": np.array([0.0, 0.0, 0.25]) - } + grasp_force = 0.0 - print("🚀 机械臂精细化抓取仿真启动！") - print("💡 操作提示：") - print(" - 空格：暂停/继续 | ESC：退出") - print(" - 视角会自动切换：主视角→俯视图→侧视图→主视角\n") + print("\n🚀 机械臂大幅度流畅抓取仿真启动！") + print("💡 动作流程：初始位姿→大距离接近→抓取→大幅度抬升→远距离搬运→下放→释放→大动作归位\n") - # 4. 主仿真循环 + # 4. 主仿真循环（大动作+流畅） for step in range(SIMULATION_STEPS): - # 检查窗口是否存活 if viewer and not viewer.is_alive: - print("⚠️ 仿真窗口已关闭，结束仿真") - simulation_alive = False + print("⚠️ 窗口关闭，结束仿真") break - # 自动切换视角 - if step in CAMERA_SWITCH_STEPS and CAMERA_SWITCH_STEPS[step] != current_camera: - current_camera = CAMERA_SWITCH_STEPS[step] - switch_camera(viewer, current_camera) - # 获取当前状态 - ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else np.zeros(3) - obj_pos = data.xpos[obj_id].copy() if obj_id >= 0 else np.zeros(3) - joint_pos = data.qpos[:3].copy() if model.njnt >= 3 else np.zeros(3) + ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else ee_pos + obj_pos = data.xpos[obj_id].copy() if obj_id >= 0 else obj_init_pos - # ---------------- 阶段逻辑 ---------------- + # ---------------- 阶段1：初始化（初始位姿，大动作归位） ---------------- if phase == GraspPhase.INIT: - target = target_positions["home"] + target = target_pos["home"] error = target - ee_pos + # 大动作归位，控制输出适中 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=2.5 ) - data.ctrl[3:5] = [0.0, 0.0] # 夹爪打开 + # 夹爪打开 + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 - if np.linalg.norm(error) < 0.02 and phase_step > 500: + # 初始化完成：位置误差小，且等待足够步数 + if np.linalg.norm(error) < 0.008 and phase_step > 2000: phase = GraspPhase.APPROACH phase_step = 0 - print(f"[{step}] 初始化完成 → 进入接近阶段") + print(f"[{step}] 初始化完成 → 开始大距离接近物体") phase_step += 1 + # ---------------- 阶段2：接近物体（大距离移动，流畅） ---------------- elif phase == GraspPhase.APPROACH: - if phase_step < 1000: - target = get_smooth_target(last_ee_pos, target_positions["pre_grasp"], phase_step / 1000) + # 分两步：先到预抓取位置，再下降到抓取点（大动作） + if phase_step < 3000: + target = get_smooth_target(ee_pos, target_pos["pre_grasp"], phase_step, 3000) else: - target = get_smooth_target(target_positions["pre_grasp"], target_positions["object"], - (phase_step - 1000) / 800) + target = get_smooth_target(target_pos["pre_grasp"], target_pos["grasp"], phase_step - 3000, 2000) error = target - ee_pos + # 大动作控制，输出稍大但无超调 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] - ) - - if phase_step > 1800 and np.linalg.norm(error) < 0.015: - phase = GraspPhase.ALIGN - phase_step = 0 - print(f"[{step}] 接近完成 → 进入姿态对齐阶段") - phase_step += 1 - last_ee_pos = ee_pos.copy() - - elif phase == GraspPhase.ALIGN: - target_joints = np.array([0.45, -0.55, 0.2]) - joint_error = target_joints - joint_pos - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - joint_error[i], error_integral[i], error_prev[i], max_output=4.0 + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=3.0 ) + # 夹爪保持打开 + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 - if check_collision(model, data, ee_id, obj_id) and phase_step > 600: + # 接近完成：到达抓取点，动作稳定 + if phase_step > 5000 and np.linalg.norm(error) < 0.01: phase = GraspPhase.GRASP phase_step = 0 - print(f"[{step}] 姿态对齐完成 → 进入抓取阶段") - elif phase_step > 1500: - retry_count += 1 - if retry_count <= RETRY_MAX: - phase = GraspPhase.RETRY - phase_step = 0 - print(f"[{step}] 对齐超时 → 重试（{retry_count}/{RETRY_MAX}）") - else: - print(f"[{step}] 重试次数用尽 → 抓取失败") - simulation_alive = False - break + print(f"[{step}] 大距离接近完成 → 开始抓取") phase_step += 1 + # ---------------- 阶段3：抓取（流畅闭合） ---------------- elif phase == GraspPhase.GRASP: - target = target_positions["object"] + # 保持末端在抓取点 + target = target_pos["grasp"] error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=1.0 ) + # 夹爪流畅闭合 if phase_step < GRASP_RAMP_STEPS: grasp_force = GRASP_FORCE_MAX * (phase_step / GRASP_RAMP_STEPS) - data.ctrl[3] = grasp_force - data.ctrl[4] = -grasp_force + data.ctrl[3] = grasp_force # 夹爪1闭合 + data.ctrl[4] = -grasp_force # 夹爪2闭合 else: + # 保持闭合，确认抓取稳定 data.ctrl[3] = GRASP_FORCE_MAX data.ctrl[4] = -GRASP_FORCE_MAX - if phase_step > GRASP_RAMP_STEPS + 500: + # 检测抓取稳定：物体跟随末端 + if check_grasp_stable(model, data, obj_id, ee_pos) and phase_step > GRASP_RAMP_STEPS + 800: phase = GraspPhase.LIFT phase_step = 0 - print(f"[{step}] 抓取完成 → 进入抬升阶段") + print(f"[{step}] 抓取稳定 → 开始大幅度抬升") + phase_step += 1 + # ---------------- 阶段4：抬升（大幅度、流畅） ---------------- elif phase == GraspPhase.LIFT: - lift_target = target_positions["object"] + np.array([0, 0, 0.15]) - target = get_smooth_target(ee_pos, lift_target, phase_step / 800) + target = target_pos["lift"] error = target - ee_pos + # 抬升控制：输出适中，动作流畅 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i], max_output=5.0 + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=2.0 ) + # 保持夹爪力度 + data.ctrl[3] = GRASP_FORCE_MAX * 0.9 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.9 - data.ctrl[3] = GRASP_FORCE_MAX * 0.8 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.8 - - if phase_step > 800 and np.linalg.norm(ee_pos - lift_target) < 0.01: + # 抬升完成：到达目标高度，幅度明显 + if phase_step > 2000 and np.linalg.norm(error) < 0.01: phase = GraspPhase.TRANSPORT phase_step = 0 - print(f"[{step}] 抬升完成 → 进入搬运阶段") + print(f"[{step}] 大幅度抬升完成 → 开始远距离搬运") phase_step += 1 + # ---------------- 阶段5：搬运（大距离、平滑） ---------------- elif phase == GraspPhase.TRANSPORT: - if phase_step < 1500: - target = get_smooth_target(ee_pos, target_positions["pre_goal"], phase_step / 1500) - else: - target = get_smooth_target(target_positions["pre_goal"], target_positions["goal"], - (phase_step - 1500) / 1000) - + target = get_smooth_target(ee_pos, target_pos["transport"], phase_step, 3000) error = target - ee_pos + # 搬运控制：平滑大动作 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=2.5 ) + # 保持夹爪力度，防止物体掉落 + data.ctrl[3] = GRASP_FORCE_MAX * 0.8 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.8 - data.ctrl[3] = GRASP_FORCE_MAX * 0.7 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.7 - - if phase_step > 2500 and np.linalg.norm(error) < 0.02: + # 搬运完成：到达目标位置，大距离移动完成 + if phase_step > 3000 and np.linalg.norm(error) < 0.015: phase = GraspPhase.LOWER phase_step = 0 - print(f"[{step}] 搬运完成 → 进入下放阶段") + print(f"[{step}] 远距离搬运完成 → 开始下放") phase_step += 1 + # ---------------- 阶段6：下放（流畅） ---------------- elif phase == GraspPhase.LOWER: - lower_target = target_positions["goal"] - np.array([0, 0, 0.05]) - target = get_smooth_target(ee_pos, lower_target, phase_step / 800) + target = target_pos["lower"] error = target - ee_pos + # 下放控制：缓慢流畅 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i], max_output=3.0 + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=1.5 ) + # 降低夹爪力度，准备释放 + data.ctrl[3] = GRASP_FORCE_MAX * 0.6 + data.ctrl[4] = -GRASP_FORCE_MAX * 0.6 - data.ctrl[3] = GRASP_FORCE_MAX * 0.5 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.5 - - if phase_step > 800 and np.linalg.norm(error) < 0.01: + if phase_step > 1500 and np.linalg.norm(error) < 0.01: phase = GraspPhase.RELEASE phase_step = 0 - print(f"[{step}] 下放完成 → 进入释放阶段") + print(f"[{step}] 下放完成 → 开始释放物体") phase_step += 1 + # ---------------- 阶段7：释放（流畅打开） ---------------- elif phase == GraspPhase.RELEASE: - target = lower_target + # 保持末端位置，避免物体掉落 + target = target_pos["lower"] error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=1.0 ) + # 夹爪流畅打开 if phase_step < RELEASE_RAMP_STEPS: - release_force = GRASP_FORCE_MAX * 0.5 * (1 - phase_step / RELEASE_RAMP_STEPS) + release_force = GRASP_FORCE_MAX * 0.6 * (1 - phase_step / RELEASE_RAMP_STEPS) data.ctrl[3] = release_force data.ctrl[4] = -release_force else: - data.ctrl[3:5] = [0.0, 0.0] + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 # 完全打开 - if phase_step > RELEASE_RAMP_STEPS + 500: + if phase_step > RELEASE_RAMP_STEPS + 800: phase = GraspPhase.RETURN phase_step = 0 - print(f"[{step}] 释放完成 → 进入归位阶段") + print(f"[{step}] 释放完成 → 开始大动作归位") phase_step += 1 + # ---------------- 阶段8：归位（大动作返回） ---------------- elif phase == GraspPhase.RETURN: - if phase_step < 600: - target = lower_target + np.array([0, 0, 0.2]) + # 分两步归位：先到中间点，再回初始位姿（大动作） + if phase_step < 2000: + target = get_smooth_target(ee_pos, target_pos["return_mid"], phase_step, 2000) else: - target = get_smooth_target(ee_pos, target_positions["home"], (phase_step - 600) / 1000) + target = get_smooth_target(target_pos["return_mid"], target_pos["home"], phase_step - 2000, 2000) error = target - ee_pos + # 归位控制：大动作流畅返回 for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] + data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( + error[i], error_integral[i], error_prev[i], max_output=2.5 ) + # 夹爪保持打开 + data.ctrl[3] = 0.0 + data.ctrl[4] = 0.0 - if phase_step > 1600 and np.linalg.norm(error) < 0.02: + # 归位完成：回到初始位姿，整个流程结束 + if phase_step > 4000 and np.linalg.norm(error) < 0.01: phase = GraspPhase.SUCCESS - print(f"[{step}] 归位完成 → 抓取成功！") + print(f"[{step}] 大动作归位完成 → 整个抓取流程成功！") + break phase_step += 1 - elif phase == GraspPhase.RETRY: - target = target_positions["home"] - error = target - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid_control( - error[i], error_integral[i], error_prev[i] - ) - data.ctrl[3:5] = [0.0, 0.0] - - if phase_step > 1000 and np.linalg.norm(error) < 0.02: - phase = GraspPhase.APPROACH - phase_step = 0 - phase_step += 1 - - # 终止条件 - if phase == GraspPhase.SUCCESS or not simulation_alive: - break - - # 运行仿真步 + # ---------------- 仿真步进 & 渲染 ---------------- mujoco.mj_step(model, data) - - # 安全渲染 - if step % RENDER_INTERVAL == 0: - safe_render(viewer) + if viewer: + try: + viewer.render() + except: + pass time.sleep(FRAME_DELAY) except Exception as e: print(f"\n❌ 仿真出错: {type(e).__name__}: {e}") traceback.print_exc() finally: - # 安全关闭Viewer with suppress(Exception): if viewer and viewer.is_alive: viewer.close() - print("\n🔚 仿真已安全结束") + print("\n🔚 仿真结束") - # ===================== 结果可视化（无警告） ===================== - print("\n🎉 仿真结束！生成抓取分析报告...") - # 切换回交互后端显示图片 + # ===================== 结果可视化（大动作轨迹） ===================== + print("\n🎉 生成大动作抓取轨迹报告...") mpl.use('TkAgg') - import matplotlib.pyplot as plt # 重新导入确保后端生效 - - fig, ((ax1, ax2), (ax3, ax4)) = plt.subplots(2, 2, figsize=(14, 10)) - - # 1. 末端执行器轨迹 - ax1.plot([0.35, -0.25], [0.20, 0.22], 'b--', label='搬运轨迹', linewidth=2, alpha=0.7) - ax1.scatter(0.35, 0.12, c='red', s=80, label='抓取点', zorder=5) - ax1.scatter(-0.25, 0.15, c='green', s=80, label='放置点', zorder=5) - ax1.set_xlabel('X 位置 (m)') - ax1.set_ylabel('Z 位置 (m)') - ax1.set_title('机械臂末端执行器轨迹', fontsize=12) - ax1.legend() + import matplotlib.pyplot as plt + + # 绘制大动作轨迹图 + fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6)) + + # 1. 三维轨迹投影（X-Z平面，展示大动作） + trajectory_x = [ + target_pos["home"][0], + target_pos["grasp"][0], + target_pos["lift"][0], + target_pos["transport"][0], + target_pos["lower"][0], + target_pos["home"][0] + ] + trajectory_z = [ + target_pos["home"][2], + target_pos["grasp"][2], + target_pos["lift"][2], + target_pos["transport"][2], + target_pos["lower"][2], + target_pos["home"][2] + ] + # 绘制轨迹（大动作明显） + ax1.plot(trajectory_x, trajectory_z, 'b-o', linewidth=3, markersize=8, label='机械臂末端轨迹') + ax1.scatter(target_pos["grasp"][0], target_pos["grasp"][2], c='red', s=150, label='抓取点', zorder=5) + ax1.scatter(target_pos["lower"][0], target_pos["lower"][2], c='green', s=150, label='放置点', zorder=5) + ax1.set_xlabel('X 位置 (m)', fontsize=12) + ax1.set_ylabel('Z 位置 (m)', fontsize=12) + ax1.set_title('机械臂大幅度抓取轨迹（X-Z平面）', fontsize=14) + ax1.legend(fontsize=10) ax1.grid(True, alpha=0.3) - - # 2. 夹爪力度变化 + # 标注动作阶段 + ax1.annotate('初始位姿', (target_pos["home"][0], target_pos["home"][2]), + xytext=(5, 5), textcoords='offset points', fontsize=9) + ax1.annotate('大幅度抬升', (target_pos["lift"][0], target_pos["lift"][2]), + xytext=(5, 5), textcoords='offset points', fontsize=9) + ax1.annotate('远距离搬运', (target_pos["transport"][0], target_pos["transport"][2]), + xytext=(5, 5), textcoords='offset points', fontsize=9) + + # 2. 夹爪力度变化（流畅曲线） grasp_steps = np.linspace(0, GRASP_RAMP_STEPS, 100) grasp_forces = GRASP_FORCE_MAX * (grasp_steps / GRASP_RAMP_STEPS) - ax2.plot(grasp_steps, grasp_forces, 'orange', label='抓取力度上升', linewidth=2) release_steps = np.linspace(0, RELEASE_RAMP_STEPS, 100) - release_forces = GRASP_FORCE_MAX * 0.5 * (1 - release_steps / RELEASE_RAMP_STEPS) - ax2.plot(release_steps + GRASP_RAMP_STEPS + 500, release_forces, 'red', label='释放力度下降', linewidth=2) - ax2.set_xlabel('仿真步数') - ax2.set_ylabel('夹爪力度 (N)') - ax2.set_title('夹爪力度变化曲线', fontsize=12) - ax2.legend() + release_forces = GRASP_FORCE_MAX * 0.6 * (1 - release_steps / RELEASE_RAMP_STEPS) + + ax2.plot(grasp_steps, grasp_forces, 'orange', linewidth=3, label='夹爪闭合（力度上升）') + ax2.plot(release_steps + GRASP_RAMP_STEPS + 3000, release_forces, + 'red', linewidth=3, label='夹爪打开（力度下降）') + ax2.axhline(y=GRASP_FORCE_MAX, color='gray', linestyle='--', alpha=0.7, label='最大力度') + ax2.set_xlabel('仿真步数', fontsize=12) + ax2.set_ylabel('夹爪力度 (N)', fontsize=12) + ax2.set_title('夹爪力度流畅变化曲线', fontsize=14) + ax2.legend(fontsize=10) ax2.grid(True, alpha=0.3) - # 3. 阶段耗时统计 - phases = ['初始化', '接近', '对齐', '抓取', '抬升', '搬运', '下放', '释放', '归位'] - phase_times = [500, 1800, 600, 1300, 800, 2500, 800, 1000, 1600] - ax3.bar(phases, phase_times, color=['lightgray', 'lightblue', 'skyblue', 'orange', - 'lightgreen', 'royalblue', 'lightpink', 'red', 'gray']) - ax3.set_xlabel('抓取阶段') - ax3.set_ylabel('耗时步数') - ax3.set_title('各阶段耗时统计', fontsize=12) - ax3.tick_params(axis='x', rotation=45) - - # 4. 抓取成功率 - success_rate = 90 if phase == GraspPhase.SUCCESS else 0 - ax4.pie([success_rate, 100 - success_rate], labels=['成功', '失败'], autopct='%1.1f%%', - colors=['green', 'red'], startangle=90) - ax4.set_title('抓取成功率', fontsize=12) - plt.tight_layout() - # 保存图片时消除sRGB警告 - plt.savefig(os.path.join(CURRENT_DIR, "grasp_analysis_report.png"), - dpi=150, bbox_inches='tight', format='png', - pil_kwargs={"optimize": True}) + plt.savefig(os.path.join(CURRENT_DIR, "big_move_grasp_report.png"), + dpi=150, bbox_inches='tight', pil_kwargs={"optimize": True}) plt.show() # ===================== 运行入口 ===================== if __name__ == "__main__": - # 检查依赖 try: import mujoco import mujoco_viewer @@ -480,5 +431,4 @@ def grasp_simulation(): print("❌ 缺少依赖！执行：pip install mujoco mujoco-viewer numpy matplotlib pillow") exit(1) - # 运行仿真 grasp_simulation() \ No newline at end of file From 4ba3e4a11eb5cae493150e8caa5736b45bd08de1 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Wed, 24 Dec 2025 19:28:06 +0800 Subject: [PATCH 19/26] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E4=B8=8A=E6=89=8B=E7=A8=8B=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 734 +++++++++++++--------------- 1 file changed, 335 insertions(+), 399 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 4c3efeda2c..926491c149 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -2,433 +2,369 @@ import mujoco_viewer import numpy as np import matplotlib.pyplot as plt -import time import matplotlib as mpl import os import warnings -import traceback -from enum import Enum +import time +import glfw # 直接用glfw检测按键，兼容所有版本 from contextlib import suppress # ===================== 基础配置（消除警告） ===================== -warnings.filterwarnings('ignore', category=UserWarning, module='PIL') -warnings.filterwarnings('ignore', category=RuntimeWarning, module='matplotlib') -mpl.use('Agg') +warnings.filterwarnings('ignore') +mpl.use('TkAgg') mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] mpl.rcParams['axes.unicode_minus'] = False -mpl.rcParams['font.family'] = 'sans-serif' -os.environ['MPLCONFIGDIR'] = os.path.join(os.getcwd(), ".mplconfig") -os.makedirs(os.environ['MPLCONFIGDIR'], exist_ok=True) -# 路径配置 +# 路径配置（适配你的原有robot.xml路径） CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 核心参数（流畅大动作） ===================== -# 仿真参数（平衡流畅度和幅度） -SIMULATION_STEPS = 18000 # 足够的步数完成大动作 -FRAME_DELAY = 0.003 # 流畅无卡顿 -RENDER_INTERVAL = 1 # 每步渲染，看清细节 -# PID参数（精准控制，无超调） -KP = 3.5 # 比例增益：足够驱动大动作，又不超调 -KI = 0.008 # 积分增益：微小积分消除稳态误差 -KD = 0.8 # 微分增益：抑制震荡 -# 抓取参数（流畅力度变化） -GRASP_FORCE_MAX = 5.0 # 夹爪力度适中 -GRASP_RAMP_STEPS = 1500 # 1500步闭合，动作流畅 -RELEASE_RAMP_STEPS = 1200 # 1200步打开，避免物体掉落 -# 轨迹参数（大幅度、平滑） -LIFT_HEIGHT = 0.2 # 抬升幅度大 -TRANSPORT_DISTANCE = 0.4 # 搬运幅度大 -MOVE_SMOOTH_FACTOR = 0.002 # 轨迹平滑因子 - - -# 动作阶段枚举 -class GraspPhase(Enum): - INIT = 1 # 初始化（初始位姿） - APPROACH = 2 # 接近物体（大距离移动） - GRASP = 3 # 抓取 - LIFT = 4 # 抬升（大幅度） - TRANSPORT = 5 # 搬运（大距离） - LOWER = 6 # 下放 - RELEASE = 7 # 释放 - RETURN = 8 # 归位（大动作返回） - SUCCESS = 9 # 成功 - - -# ===================== 工具函数（精准控制） ===================== -def validate_model(model, data): - """校验模型，确保关键组件存在""" - print("\n===== 模型信息 =====") - print(f"关节数: {model.njnt} | 控制维度: {model.nu} | 接触数: {data.ncon}") - - # 关键ID（兼容模型） - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") - if ee_id < 0: ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "ee") - if obj_id < 0: obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "object_geom") - - print(f"末端ID: {ee_id} | 物体ID: {obj_id}") - print("====================\n") - - if ee_id < 0 or obj_id < 0: - raise ValueError("请确保robot.xml包含ee_site和target_object") - return ee_id, obj_id - - -def smooth_pid(error, integral, prev_error, max_output=3.0): - """平滑PID控制，无超调""" - p = KP * error - i = KI * np.clip(integral, -1.5, 1.5) - d = KD * (error - prev_error) / (FRAME_DELAY * 2) - output = np.clip(p + i + d, -max_output, max_output) - return output, integral + error, prev_error - - -def get_smooth_target(current, target, step, total_steps): - """平滑轨迹插值，大动作无突变""" - t = np.clip(step / total_steps, 0, 1) - # 五次缓动曲线：start→end 全程平滑 - smooth_t = t * t * t * (t * (6 * t - 15) + 10) - return current + (target - current) * smooth_t - - -def check_grasp_stable(model, data, obj_id, ee_pos): - """检测抓取是否稳定（物体跟随末端）""" - obj_pos = data.xpos[obj_id] - distance = np.linalg.norm(obj_pos - ee_pos) - return distance < 0.03 # 物体与末端距离近，抓取稳定 - - -# ===================== 核心抓取逻辑（大动作+流畅） ===================== -def grasp_simulation(): - viewer = None - phase = GraspPhase.INIT - try: - # 1. 加载模型 - if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"模型文件不存在: {MODEL_PATH}") - - model = mujoco.MjModel.from_xml_path(MODEL_PATH) - data = mujoco.MjData(model) - mujoco.mj_resetData(model, data) - mujoco.mj_forward(model, data) - ee_id, obj_id = validate_model(model, data) - - # 2. 初始化Viewer（优化视角，看清大动作） - viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer.cam.distance = 1.8 # 视角拉远，看清大动作 - viewer.cam.elevation = 15 # 仰角，看清抬升动作 - viewer.cam.azimuth = 60 # 方位角，看清搬运轨迹 - viewer.cam.lookat = [0.2, 0.0, 0.15] # 聚焦动作区域 - - # 3. 核心变量初始化 - phase_step = 0 - ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else np.array([0.0, 0.0, 0.1]) - obj_init_pos = data.xpos[obj_id].copy() if obj_id >= 0 else np.array([0.3, 0.0, 0.05]) - - # 大幅度目标轨迹规划 - target_pos = { - "home": np.array([0.0, 0.0, 0.15]), # 初始位姿 - "pre_grasp": obj_init_pos + [0, 0, 0.08], # 预抓取位置（物体上方） - "grasp": obj_init_pos, # 抓取位置 - "lift": obj_init_pos + [0, 0, LIFT_HEIGHT], # 抬升位置（大幅度） - "transport": obj_init_pos + [TRANSPORT_DISTANCE, 0, LIFT_HEIGHT], # 搬运位置（大距离） - "lower": obj_init_pos + [TRANSPORT_DISTANCE, 0, 0.05], # 下放位置 - "return_mid": np.array([0.2, 0.0, 0.2]) # 归位中间点（大动作过渡） - } - print("🎯 大动作轨迹规划完成：") - for k, v in target_pos.items(): - print(f" {k}: {v}") - - # PID控制变量 - error_integral = np.zeros(3) - error_prev = np.zeros(3) - grasp_force = 0.0 - - print("\n🚀 机械臂大幅度流畅抓取仿真启动！") - print("💡 动作流程：初始位姿→大距离接近→抓取→大幅度抬升→远距离搬运→下放→释放→大动作归位\n") - - # 4. 主仿真循环（大动作+流畅） - for step in range(SIMULATION_STEPS): - if viewer and not viewer.is_alive: - print("⚠️ 窗口关闭，结束仿真") +# ===================== 核心控制参数（微调适配原有模型） ===================== +# 手动控制参数（适配原有模型的关节范围，低速易控） +MANUAL_SPEED = 0.03 # 比之前略小，适配原有模型的关节灵敏度 +GRASP_FORCE = 3.5 # 微调力度，适配原有夹爪尺寸 +# 自动控制参数（适配原有模型的物体位置） +AUTO_LIFT_HEIGHT = 0.12 # 适配原有模型的抬升范围 +AUTO_TRANSPORT_X = -0.15 # 适配原有模型的搬运范围 + +# ===================== 全局控制变量 ===================== +control_cmd = { + 'forward': 0, # 前（W） + 'backward': 0, # 后（S） + 'left': 0, # 左（A） + 'right': 0, # 右（D） + 'up': 0, # 上（Q） + 'down': 0, # 下（E） + 'grasp': 0, # 抓取（空格） + 'release': 0, # 释放（R） + 'auto': False, # 一键自动抓取（Z） + 'reset': False # 重置（C） +} + + +# ===================== 兼容版按键检测函数（核心修复） ===================== +def check_keyboard_input(viewer): + """ + 兼容所有版本mujoco-viewer的按键检测 + 替代原有get_key()方法，解决属性不存在问题 + """ + # 重置所有指令（避免按键粘连） + for key in control_cmd.keys(): + if key != 'auto' and key != 'reset': + control_cmd[key] = 0 + + # 方式1：适配新版mujoco-viewer（有window属性） + if hasattr(viewer, 'window') and viewer.window is not None: + window = viewer.window + # W键 - 前 + if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS: + control_cmd['forward'] = 1 + # S键 - 后 + if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS: + control_cmd['backward'] = 1 + # A键 - 左 + if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS: + control_cmd['left'] = 1 + # D键 - 右 + if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS: + control_cmd['right'] = 1 + # Q键 - 上 + if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS: + control_cmd['up'] = 1 + # E键 - 下 + if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS: + control_cmd['down'] = 1 + # 空格键 - 抓取 + if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS: + control_cmd['grasp'] = 1 + # R键 - 释放 + if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS: + control_cmd['release'] = 1 + # Z键 - 一键自动抓取 + if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS: + control_cmd['auto'] = True + # C键 - 重置 + if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS: + control_cmd['reset'] = True + # ESC键 - 关闭窗口 + if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: + glfw.set_window_should_close(window, True) + + # 方式2：适配旧版mujoco-viewer（无window属性，备用方案） + else: + # 旧版无法实时检测按键，提供替代操作方式 + print("\n⚠️ 检测到旧版mujoco-viewer，按键控制受限！") + print(" 替代操作：按Z键（一键自动抓取）或C键（重置）继续") + # 仅保留核心功能（自动抓取/重置） + # 按任意键触发自动抓取（简化适配） + control_cmd['auto'] = True + + +# ===================== 核心控制函数（仅微调适配原有模型） ===================== +def init_model_and_viewer(): + """初始化模型（完全适配原有robot.xml，不修改模型）""" + if not os.path.exists(MODEL_PATH): + raise FileNotFoundError(f"未找到原有robot.xml文件: {MODEL_PATH}") + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) + + # 初始化Viewer（微调视角，适配原有模型的显示） + viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) + viewer.cam.distance = 1.8 # 微调视角距离，看清原有模型 + viewer.cam.elevation = 12 # 微调仰角，适配原有模型的高度 + viewer.cam.azimuth = 50 # 微调方位角，看清物体位置 + viewer.cam.lookat = [0.15, 0.0, 0.12] # 适配原有模型的物体位置 + + # 兼容原有模型的ID命名（不修改模型，仅适配识别） + ee_id = -1 + obj_id = -1 + # 尝试所有可能的末端命名（适配原有模型） + for name in ["ee_site", "ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) + if ee_id >= 0: + break + if ee_id < 0: + for name in ["ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if ee_id >= 0: + break + # 尝试所有可能的物体命名（适配原有模型） + for name in ["target_object", "object", "ball"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if obj_id >= 0: + break + if obj_id < 0: + for name in ["object_geom", "ball_geom"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + if obj_id >= 0: break - # 获取当前状态 - ee_pos = data.site_xpos[ee_id].copy() if ee_id >= 0 else ee_pos - obj_pos = data.xpos[obj_id].copy() if obj_id >= 0 else obj_init_pos - - # ---------------- 阶段1：初始化（初始位姿，大动作归位） ---------------- - if phase == GraspPhase.INIT: - target = target_pos["home"] - error = target - ee_pos - # 大动作归位，控制输出适中 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=2.5 - ) - # 夹爪打开 - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 - - # 初始化完成：位置误差小，且等待足够步数 - if np.linalg.norm(error) < 0.008 and phase_step > 2000: - phase = GraspPhase.APPROACH - phase_step = 0 - print(f"[{step}] 初始化完成 → 开始大距离接近物体") - phase_step += 1 - - # ---------------- 阶段2：接近物体（大距离移动，流畅） ---------------- - elif phase == GraspPhase.APPROACH: - # 分两步：先到预抓取位置，再下降到抓取点（大动作） - if phase_step < 3000: - target = get_smooth_target(ee_pos, target_pos["pre_grasp"], phase_step, 3000) - else: - target = get_smooth_target(target_pos["pre_grasp"], target_pos["grasp"], phase_step - 3000, 2000) - - error = target - ee_pos - # 大动作控制，输出稍大但无超调 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=3.0 - ) - # 夹爪保持打开 - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 - - # 接近完成：到达抓取点，动作稳定 - if phase_step > 5000 and np.linalg.norm(error) < 0.01: - phase = GraspPhase.GRASP - phase_step = 0 - print(f"[{step}] 大距离接近完成 → 开始抓取") - phase_step += 1 - - # ---------------- 阶段3：抓取（流畅闭合） ---------------- - elif phase == GraspPhase.GRASP: - # 保持末端在抓取点 - target = target_pos["grasp"] - error = target - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=1.0 - ) - - # 夹爪流畅闭合 - if phase_step < GRASP_RAMP_STEPS: - grasp_force = GRASP_FORCE_MAX * (phase_step / GRASP_RAMP_STEPS) - data.ctrl[3] = grasp_force # 夹爪1闭合 - data.ctrl[4] = -grasp_force # 夹爪2闭合 - else: - # 保持闭合，确认抓取稳定 - data.ctrl[3] = GRASP_FORCE_MAX - data.ctrl[4] = -GRASP_FORCE_MAX - # 检测抓取稳定：物体跟随末端 - if check_grasp_stable(model, data, obj_id, ee_pos) and phase_step > GRASP_RAMP_STEPS + 800: - phase = GraspPhase.LIFT - phase_step = 0 - print(f"[{step}] 抓取稳定 → 开始大幅度抬升") - - phase_step += 1 - - # ---------------- 阶段4：抬升（大幅度、流畅） ---------------- - elif phase == GraspPhase.LIFT: - target = target_pos["lift"] - error = target - ee_pos - # 抬升控制：输出适中，动作流畅 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=2.0 - ) - # 保持夹爪力度 - data.ctrl[3] = GRASP_FORCE_MAX * 0.9 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.9 - - # 抬升完成：到达目标高度，幅度明显 - if phase_step > 2000 and np.linalg.norm(error) < 0.01: - phase = GraspPhase.TRANSPORT - phase_step = 0 - print(f"[{step}] 大幅度抬升完成 → 开始远距离搬运") - phase_step += 1 - - # ---------------- 阶段5：搬运（大距离、平滑） ---------------- - elif phase == GraspPhase.TRANSPORT: - target = get_smooth_target(ee_pos, target_pos["transport"], phase_step, 3000) - error = target - ee_pos - # 搬运控制：平滑大动作 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=2.5 - ) - # 保持夹爪力度，防止物体掉落 - data.ctrl[3] = GRASP_FORCE_MAX * 0.8 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.8 - - # 搬运完成：到达目标位置，大距离移动完成 - if phase_step > 3000 and np.linalg.norm(error) < 0.015: - phase = GraspPhase.LOWER - phase_step = 0 - print(f"[{step}] 远距离搬运完成 → 开始下放") - phase_step += 1 - - # ---------------- 阶段6：下放（流畅） ---------------- - elif phase == GraspPhase.LOWER: - target = target_pos["lower"] - error = target - ee_pos - # 下放控制：缓慢流畅 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=1.5 - ) - # 降低夹爪力度，准备释放 - data.ctrl[3] = GRASP_FORCE_MAX * 0.6 - data.ctrl[4] = -GRASP_FORCE_MAX * 0.6 - - if phase_step > 1500 and np.linalg.norm(error) < 0.01: - phase = GraspPhase.RELEASE - phase_step = 0 - print(f"[{step}] 下放完成 → 开始释放物体") - phase_step += 1 - - # ---------------- 阶段7：释放（流畅打开） ---------------- - elif phase == GraspPhase.RELEASE: - # 保持末端位置，避免物体掉落 - target = target_pos["lower"] - error = target - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=1.0 - ) - - # 夹爪流畅打开 - if phase_step < RELEASE_RAMP_STEPS: - release_force = GRASP_FORCE_MAX * 0.6 * (1 - phase_step / RELEASE_RAMP_STEPS) - data.ctrl[3] = release_force - data.ctrl[4] = -release_force - else: - data.ctrl[3] = 0.0 - data.ctrl[4] = 0.0 # 完全打开 - - if phase_step > RELEASE_RAMP_STEPS + 800: - phase = GraspPhase.RETURN - phase_step = 0 - print(f"[{step}] 释放完成 → 开始大动作归位") - phase_step += 1 - - # ---------------- 阶段8：归位（大动作返回） ---------------- - elif phase == GraspPhase.RETURN: - # 分两步归位：先到中间点，再回初始位姿（大动作） - if phase_step < 2000: - target = get_smooth_target(ee_pos, target_pos["return_mid"], phase_step, 2000) - else: - target = get_smooth_target(target_pos["return_mid"], target_pos["home"], phase_step - 2000, 2000) - - error = target - ee_pos - # 归位控制：大动作流畅返回 - for i in range(min(3, model.njnt)): - data.ctrl[i], error_integral[i], error_prev[i] = smooth_pid( - error[i], error_integral[i], error_prev[i], max_output=2.5 - ) - # 夹爪保持打开 + print("✅ 适配原有robot.xml完成！") + print("🎮 操作指南（适配原有模型）：") + print(" W/S：前后移动 A/D：左右移动 Q/E：上下移动（低速易控）") + print(" 空格：抓取 R：释放 Z：一键自动抓取（适配原有模型）") + print(" C：重置 ESC：退出") + + return model, data, viewer, ee_id, obj_id + + +def manual_control(model, data, ee_id): + """手动控制（仅微调参数，适配原有模型的关节响应）""" + # 安全获取末端位置（适配原有模型） + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + + # 计算目标位置（微调速度，适配原有模型） + target_pos = ee_pos.copy() + target_pos[0] += control_cmd['forward'] * MANUAL_SPEED + target_pos[0] -= control_cmd['backward'] * MANUAL_SPEED + target_pos[1] += control_cmd['left'] * MANUAL_SPEED + target_pos[1] -= control_cmd['right'] * MANUAL_SPEED + target_pos[2] += control_cmd['up'] * MANUAL_SPEED + target_pos[2] -= control_cmd['down'] * MANUAL_SPEED + + # 微调控制增益（适配原有模型的关节传动比，避免转圈） + error = target_pos - ee_pos + gain = 4.0 # 微调增益，适配原有模型的关节灵敏度 + for i in range(min(3, model.njnt)): + # 更严格的输出限制，彻底避免转圈 + data.ctrl[i] = np.clip(error[i] * gain, -1.8, 1.8) + + # 抓取控制（微调力度，适配原有夹爪） + if control_cmd['grasp']: + # 适配原有模型的夹爪控制维度 + if model.nu >= 4: + data.ctrl[3] = GRASP_FORCE + if model.nu >= 5: + data.ctrl[4] = -GRASP_FORCE + elif control_cmd['release']: + if model.nu >= 4: + data.ctrl[3] = 0.0 + if model.nu >= 5: + data.ctrl[4] = 0.0 + + +def auto_grasp(model, data, ee_id, obj_id): + """一键自动抓取（仅微调轨迹，适配原有模型的物体位置）""" + print("🔄 开始适配原有模型的一键自动抓取...") + # 安全获取物体位置（适配原有模型） + obj_pos = np.array([0.2, 0.0, 0.05]) # 适配原有模型的默认物体位置 + if obj_id >= 0: + try: + obj_pos = data.xpos[obj_id].copy() + except: + pass + + # 阶段1：移动到物体上方（微调距离，适配原有模型） + step = 0 + while step < 600 and viewer.is_alive: # 增加窗口存活检测 + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [0, 0, 0.07] # 微调高度，适配原有模型 + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + mujoco.mj_step(model, data) + viewer.render() # 自动抓取时也渲染，避免窗口卡死 + step += 1 + + # 阶段2：下降抓取（微调力度，适配原有夹爪） + step = 0 + while step < 400 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = obj_pos + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) + # 适配原有模型的夹爪控制 + if model.nu >= 4: + data.ctrl[3] = GRASP_FORCE + if model.nu >= 5: + data.ctrl[4] = -GRASP_FORCE + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段3：抬升（微调高度，适配原有模型） + step = 0 + while step < 450 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 3.2, -1.1, 1.1) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段4：搬运（微调距离，适配原有模型） + step = 0 + while step < 700 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段5：下放释放（适配原有模型） + step = 0 + while step < 450 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.04] # 微调下放高度 + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) + # 延迟释放，适配原有模型 + if step > 250: + if model.nu >= 4: data.ctrl[3] = 0.0 + if model.nu >= 5: data.ctrl[4] = 0.0 + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段6：归位（适配原有模型的初始位置） + step = 0 + while step < 600 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + ee_pos = data.site_xpos[ee_id].copy() + except: + ee_pos = data.xpos[ee_id].copy() + target = np.array([0.0, 0.0, 0.12]) # 微调归位位置 + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + print("🎉 适配原有模型的自动抓取完成！") + + +# ===================== 主程序（修复后版本） ===================== +def main(): + global viewer # 声明全局变量，让auto_grasp能访问 + model, data, viewer, ee_id, obj_id = init_model_and_viewer() - # 归位完成：回到初始位姿，整个流程结束 - if phase_step > 4000 and np.linalg.norm(error) < 0.01: - phase = GraspPhase.SUCCESS - print(f"[{step}] 大动作归位完成 → 整个抓取流程成功！") - break - phase_step += 1 - - # ---------------- 仿真步进 & 渲染 ---------------- + try: + while viewer.is_alive: + # 核心修复：用兼容版按键检测替代get_key() + check_keyboard_input(viewer) + + # 执行控制（适配原有模型） + if control_cmd['reset']: + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) + print("🔄 原有模型已重置到初始状态！") + control_cmd['reset'] = False + elif control_cmd['auto']: + auto_grasp(model, data, ee_id, obj_id) + control_cmd['auto'] = False + else: + manual_control(model, data, ee_id) + + # 仿真步进（微调延迟，适配原有模型的帧率） mujoco.mj_step(model, data) - if viewer: - try: - viewer.render() - except: - pass - time.sleep(FRAME_DELAY) + viewer.render() + time.sleep(0.004) # 微调延迟，适配原有模型的流畅度 except Exception as e: - print(f"\n❌ 仿真出错: {type(e).__name__}: {e}") - traceback.print_exc() + print(f"\n❌ 运行出错（适配原有模型时）: {e}") + import traceback + traceback.print_exc() # 打印详细错误栈，方便排查 finally: with suppress(Exception): - if viewer and viewer.is_alive: - viewer.close() - print("\n🔚 仿真结束") - - # ===================== 结果可视化（大动作轨迹） ===================== - print("\n🎉 生成大动作抓取轨迹报告...") - mpl.use('TkAgg') - import matplotlib.pyplot as plt - - # 绘制大动作轨迹图 - fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6)) - - # 1. 三维轨迹投影（X-Z平面，展示大动作） - trajectory_x = [ - target_pos["home"][0], - target_pos["grasp"][0], - target_pos["lift"][0], - target_pos["transport"][0], - target_pos["lower"][0], - target_pos["home"][0] - ] - trajectory_z = [ - target_pos["home"][2], - target_pos["grasp"][2], - target_pos["lift"][2], - target_pos["transport"][2], - target_pos["lower"][2], - target_pos["home"][2] - ] - # 绘制轨迹（大动作明显） - ax1.plot(trajectory_x, trajectory_z, 'b-o', linewidth=3, markersize=8, label='机械臂末端轨迹') - ax1.scatter(target_pos["grasp"][0], target_pos["grasp"][2], c='red', s=150, label='抓取点', zorder=5) - ax1.scatter(target_pos["lower"][0], target_pos["lower"][2], c='green', s=150, label='放置点', zorder=5) - ax1.set_xlabel('X 位置 (m)', fontsize=12) - ax1.set_ylabel('Z 位置 (m)', fontsize=12) - ax1.set_title('机械臂大幅度抓取轨迹（X-Z平面）', fontsize=14) - ax1.legend(fontsize=10) - ax1.grid(True, alpha=0.3) - # 标注动作阶段 - ax1.annotate('初始位姿', (target_pos["home"][0], target_pos["home"][2]), - xytext=(5, 5), textcoords='offset points', fontsize=9) - ax1.annotate('大幅度抬升', (target_pos["lift"][0], target_pos["lift"][2]), - xytext=(5, 5), textcoords='offset points', fontsize=9) - ax1.annotate('远距离搬运', (target_pos["transport"][0], target_pos["transport"][2]), - xytext=(5, 5), textcoords='offset points', fontsize=9) - - # 2. 夹爪力度变化（流畅曲线） - grasp_steps = np.linspace(0, GRASP_RAMP_STEPS, 100) - grasp_forces = GRASP_FORCE_MAX * (grasp_steps / GRASP_RAMP_STEPS) - release_steps = np.linspace(0, RELEASE_RAMP_STEPS, 100) - release_forces = GRASP_FORCE_MAX * 0.6 * (1 - release_steps / RELEASE_RAMP_STEPS) - - ax2.plot(grasp_steps, grasp_forces, 'orange', linewidth=3, label='夹爪闭合（力度上升）') - ax2.plot(release_steps + GRASP_RAMP_STEPS + 3000, release_forces, - 'red', linewidth=3, label='夹爪打开（力度下降）') - ax2.axhline(y=GRASP_FORCE_MAX, color='gray', linestyle='--', alpha=0.7, label='最大力度') - ax2.set_xlabel('仿真步数', fontsize=12) - ax2.set_ylabel('夹爪力度 (N)', fontsize=12) - ax2.set_title('夹爪力度流畅变化曲线', fontsize=14) - ax2.legend(fontsize=10) - ax2.grid(True, alpha=0.3) - - plt.tight_layout() - plt.savefig(os.path.join(CURRENT_DIR, "big_move_grasp_report.png"), - dpi=150, bbox_inches='tight', pil_kwargs={"optimize": True}) - plt.show() + viewer.close() + print("\n🔚 程序退出（未修改任何robot.xml内容）") # ===================== 运行入口 ===================== if __name__ == "__main__": + # 检查依赖（新增glfw检查） try: import mujoco import mujoco_viewer - except ImportError: - print("❌ 缺少依赖！执行：pip install mujoco mujoco-viewer numpy matplotlib pillow") + import glfw + except ImportError as e: + missing_lib = str(e).split()[-1] + print(f"❌ 缺少依赖 {missing_lib}！执行以下命令安装：") + print(f" pip install mujoco mujoco-viewer glfw numpy matplotlib") exit(1) - grasp_simulation() \ No newline at end of file + main() \ No newline at end of file From d12591adc99bce20406f9a8e45d8b4ac3c637dcd Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Wed, 24 Dec 2025 22:23:10 +0800 Subject: [PATCH 20/26] =?UTF-8?q?=E6=8A=93=E5=8F=96=E6=B5=81=E7=A8=8B?= =?UTF-8?q?=E5=81=9A=E8=BD=BB=E9=87=8F=E5=8C=96=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 384 ++++++++++++++-------------- 1 file changed, 194 insertions(+), 190 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 926491c149..b7a39efd37 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -6,153 +6,91 @@ import os import warnings import time -import glfw # 直接用glfw检测按键，兼容所有版本 +import glfw from contextlib import suppress -# ===================== 基础配置（消除警告） ===================== +# ===================== 基础配置 ===================== warnings.filterwarnings('ignore') mpl.use('TkAgg') mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] mpl.rcParams['axes.unicode_minus'] = False -# 路径配置（适配你的原有robot.xml路径） +# 路径配置 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 核心控制参数（微调适配原有模型） ===================== -# 手动控制参数（适配原有模型的关节范围，低速易控） -MANUAL_SPEED = 0.03 # 比之前略小，适配原有模型的关节灵敏度 -GRASP_FORCE = 3.5 # 微调力度，适配原有夹爪尺寸 -# 自动控制参数（适配原有模型的物体位置） -AUTO_LIFT_HEIGHT = 0.12 # 适配原有模型的抬升范围 -AUTO_TRANSPORT_X = -0.15 # 适配原有模型的搬运范围 - -# ===================== 全局控制变量 ===================== +# ===================== 核心优化参数（流畅性重点） ===================== +# 手动控制：更低速、更平滑 +MANUAL_SPEED = 0.025 # 进一步降低速度，减少抖动 +GRASP_FORCE = 3.8 # 微调抓取力度，保证抓稳且不卡顿 +# 自动抓取：平滑轨迹参数 +AUTO_LIFT_HEIGHT = 0.12 +AUTO_TRANSPORT_X = -0.15 +SMOOTH_GAIN = 3.0 # 降低控制增益，减少超调 +SMOOTH_CLIP = 1.0 # 更严格的输出限制，避免猛冲 +ACCEL_FACTOR = 0.05 # 加速度因子，让动作渐进加速/减速 + +# ===================== 全局变量 ===================== control_cmd = { - 'forward': 0, # 前（W） - 'backward': 0, # 后（S） - 'left': 0, # 左（A） - 'right': 0, # 右（D） - 'up': 0, # 上（Q） - 'down': 0, # 下（E） - 'grasp': 0, # 抓取（空格） - 'release': 0, # 释放（R） - 'auto': False, # 一键自动抓取（Z） - 'reset': False # 重置（C） + 'forward': 0, 'backward': 0, 'left': 0, 'right': 0, + 'up': 0, 'down': 0, 'grasp': 0, 'release': 0, + 'auto': False, 'reset': False } +# 平滑控制缓存（记录上一步的控制输出，避免突变） +last_ctrl = np.zeros(10) # 适配最多10个关节 -# ===================== 兼容版按键检测函数（核心修复） ===================== +# ===================== 兼容版按键检测 ===================== def check_keyboard_input(viewer): - """ - 兼容所有版本mujoco-viewer的按键检测 - 替代原有get_key()方法，解决属性不存在问题 - """ - # 重置所有指令（避免按键粘连） for key in control_cmd.keys(): if key != 'auto' and key != 'reset': control_cmd[key] = 0 - # 方式1：适配新版mujoco-viewer（有window属性） if hasattr(viewer, 'window') and viewer.window is not None: window = viewer.window - # W键 - 前 - if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS: - control_cmd['forward'] = 1 - # S键 - 后 - if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS: - control_cmd['backward'] = 1 - # A键 - 左 - if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS: - control_cmd['left'] = 1 - # D键 - 右 - if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS: - control_cmd['right'] = 1 - # Q键 - 上 - if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS: - control_cmd['up'] = 1 - # E键 - 下 - if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS: - control_cmd['down'] = 1 - # 空格键 - 抓取 - if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS: - control_cmd['grasp'] = 1 - # R键 - 释放 - if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS: - control_cmd['release'] = 1 - # Z键 - 一键自动抓取 - if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS: - control_cmd['auto'] = True - # C键 - 重置 - if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS: - control_cmd['reset'] = True - # ESC键 - 关闭窗口 + # 基础移动按键 + control_cmd['forward'] = 1 if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS else 0 + control_cmd['backward'] = 1 if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS else 0 + control_cmd['left'] = 1 if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS else 0 + control_cmd['right'] = 1 if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS else 0 + control_cmd['up'] = 1 if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS else 0 + control_cmd['down'] = 1 if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS else 0 + # 抓取/释放/自动/重置 + control_cmd['grasp'] = 1 if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS else 0 + control_cmd['release'] = 1 if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS else 0 + control_cmd['auto'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False + control_cmd['reset'] = True if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS else False + # ESC退出 if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: glfw.set_window_should_close(window, True) - - # 方式2：适配旧版mujoco-viewer（无window属性，备用方案） else: - # 旧版无法实时检测按键，提供替代操作方式 - print("\n⚠️ 检测到旧版mujoco-viewer，按键控制受限！") - print(" 替代操作：按Z键（一键自动抓取）或C键（重置）继续") - # 仅保留核心功能（自动抓取/重置） - # 按任意键触发自动抓取（简化适配） + print("\n⚠️ 旧版mujoco-viewer，按Z触发自动抓取，C重置") control_cmd['auto'] = True -# ===================== 核心控制函数（仅微调适配原有模型） ===================== -def init_model_and_viewer(): - """初始化模型（完全适配原有robot.xml，不修改模型）""" - if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"未找到原有robot.xml文件: {MODEL_PATH}") - model = mujoco.MjModel.from_xml_path(MODEL_PATH) - data = mujoco.MjData(model) - mujoco.mj_resetData(model, data) - mujoco.mj_forward(model, data) - - # 初始化Viewer（微调视角，适配原有模型的显示） - viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer.cam.distance = 1.8 # 微调视角距离，看清原有模型 - viewer.cam.elevation = 12 # 微调仰角，适配原有模型的高度 - viewer.cam.azimuth = 50 # 微调方位角，看清物体位置 - viewer.cam.lookat = [0.15, 0.0, 0.12] # 适配原有模型的物体位置 - - # 兼容原有模型的ID命名（不修改模型，仅适配识别） - ee_id = -1 - obj_id = -1 - # 尝试所有可能的末端命名（适配原有模型） - for name in ["ee_site", "ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) - if ee_id >= 0: - break - if ee_id < 0: - for name in ["ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if ee_id >= 0: - break - # 尝试所有可能的物体命名（适配原有模型） - for name in ["target_object", "object", "ball"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if obj_id >= 0: - break - if obj_id < 0: - for name in ["object_geom", "ball_geom"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) - if obj_id >= 0: - break - - print("✅ 适配原有robot.xml完成！") - print("🎮 操作指南（适配原有模型）：") - print(" W/S：前后移动 A/D：左右移动 Q/E：上下移动（低速易控）") - print(" 空格：抓取 R：释放 Z：一键自动抓取（适配原有模型）") - print(" C：重置 ESC：退出") - - return model, data, viewer, ee_id, obj_id +# ===================== 平滑控制函数（核心优化） ===================== +def smooth_control(target_ctrl, last_ctrl, joint_idx): + """ + 平滑控制输出，避免关节突变（解决抖动/卡顿） + :param target_ctrl: 目标控制值 + :param last_ctrl: 上一步控制值 + :param joint_idx: 关节索引 + :return: 平滑后的控制值 + """ + # 渐进逼近目标值，避免猛冲 + delta = target_ctrl - last_ctrl[joint_idx] + smoothed = last_ctrl[joint_idx] + delta * ACCEL_FACTOR + # 限制最大变化量，彻底避免抖动 + smoothed = np.clip(smoothed, -SMOOTH_CLIP, SMOOTH_CLIP) + # 更新缓存 + last_ctrl[joint_idx] = smoothed + return smoothed def manual_control(model, data, ee_id): - """手动控制（仅微调参数，适配原有模型的关节响应）""" - # 安全获取末端位置（适配原有模型） + """手动控制（增加平滑逻辑，动作更丝滑）""" + global last_ctrl + # 安全获取末端位置 ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: @@ -160,178 +98,251 @@ def manual_control(model, data, ee_id): except: ee_pos = data.xpos[ee_id].copy() - # 计算目标位置（微调速度，适配原有模型） + # 计算目标位置（低速，易控） target_pos = ee_pos.copy() - target_pos[0] += control_cmd['forward'] * MANUAL_SPEED - target_pos[0] -= control_cmd['backward'] * MANUAL_SPEED - target_pos[1] += control_cmd['left'] * MANUAL_SPEED - target_pos[1] -= control_cmd['right'] * MANUAL_SPEED - target_pos[2] += control_cmd['up'] * MANUAL_SPEED - target_pos[2] -= control_cmd['down'] * MANUAL_SPEED - - # 微调控制增益（适配原有模型的关节传动比，避免转圈） + target_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * MANUAL_SPEED + target_pos[1] += (control_cmd['left'] - control_cmd['right']) * MANUAL_SPEED + target_pos[2] += (control_cmd['up'] - control_cmd['down']) * MANUAL_SPEED + + # 计算误差并平滑控制（核心优化） error = target_pos - ee_pos - gain = 4.0 # 微调增益，适配原有模型的关节灵敏度 for i in range(min(3, model.njnt)): - # 更严格的输出限制，彻底避免转圈 - data.ctrl[i] = np.clip(error[i] * gain, -1.8, 1.8) + target_ctrl = error[i] * SMOOTH_GAIN + # 平滑输出，避免关节突变 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - # 抓取控制（微调力度，适配原有夹爪） + # 抓取控制（渐进加力，避免夹爪猛夹） if control_cmd['grasp']: - # 适配原有模型的夹爪控制维度 + # 渐进增加抓取力，避免物体被弹飞 if model.nu >= 4: - data.ctrl[3] = GRASP_FORCE + data.ctrl[3] = min(data.ctrl[3] + 0.1, GRASP_FORCE) if model.nu >= 5: - data.ctrl[4] = -GRASP_FORCE + data.ctrl[4] = max(data.ctrl[4] - 0.1, -GRASP_FORCE) elif control_cmd['release']: + # 渐进释放，避免物体掉落 if model.nu >= 4: - data.ctrl[3] = 0.0 + data.ctrl[3] = max(data.ctrl[3] - 0.1, 0.0) if model.nu >= 5: - data.ctrl[4] = 0.0 + data.ctrl[4] = min(data.ctrl[4] + 0.1, 0.0) def auto_grasp(model, data, ee_id, obj_id): - """一键自动抓取（仅微调轨迹，适配原有模型的物体位置）""" - print("🔄 开始适配原有模型的一键自动抓取...") - # 安全获取物体位置（适配原有模型） - obj_pos = np.array([0.2, 0.0, 0.05]) # 适配原有模型的默认物体位置 + """一键自动抓取（全流程平滑优化，无卡顿）""" + global last_ctrl + print("🔄 开始平滑自动抓取...") + # 重置平滑缓存 + last_ctrl = np.zeros(10) + + # 安全获取物体位置 + obj_pos = np.array([0.2, 0.0, 0.05]) if obj_id >= 0: try: obj_pos = data.xpos[obj_id].copy() except: pass - # 阶段1：移动到物体上方（微调距离，适配原有模型） + # 阶段1：缓慢移动到物体上方（平滑逼近，无猛冲） step = 0 - while step < 600 and viewer.is_alive: # 增加窗口存活检测 + while step < 800 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [0, 0, 0.07] # 微调高度，适配原有模型 + + # 目标位置：物体上方（高度微调，避免碰撞） + target = obj_pos + [0, 0, 0.08] error = target - ee_pos + + # 平滑控制关节，无抖动 for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.8 # 更慢速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) - viewer.render() # 自动抓取时也渲染，避免窗口卡死 + viewer.render() step += 1 - # 阶段2：下降抓取（微调力度，适配原有夹爪） + # 阶段2：缓慢下降（渐进接近，避免压碎物体） step = 0 - while step < 400 and viewer.is_alive: + while step < 600 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + + # 动态调整目标：根据物体位置实时微调，避免偏差 + target = obj_pos + [0, 0, 0.01] # 仅贴近物体，不压下去 error = target - ee_pos + for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) - # 适配原有模型的夹爪控制 + target_ctrl = error[i] * SMOOTH_GAIN * 0.5 # 极慢速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + + # 渐进闭合夹爪（核心优化：避免猛夹导致物体掉落） if model.nu >= 4: - data.ctrl[3] = GRASP_FORCE + data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) if model.nu >= 5: - data.ctrl[4] = -GRASP_FORCE + data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段3：抬升（微调高度，适配原有模型） + # 阶段3：缓慢抬升（确认抓稳后再抬升） step = 0 - while step < 450 and viewer.is_alive: + grasp_confirmed = False + while step < 500 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + + # 确认抓稳后再抬升（避免刚夹就抬导致掉落） + if step > 100: + grasp_confirmed = True + + if grasp_confirmed: + target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + else: + target = obj_pos + [0, 0, 0.01] # 先保持位置 + error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.2, -1.1, 1.1) + target_ctrl = error[i] * SMOOTH_GAIN * 0.7 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段4：搬运（微调距离，适配原有模型） + # 阶段4：平稳搬运（匀速移动，无晃动） step = 0 - while step < 700 and viewer.is_alive: + while step < 800 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.6 # 更平稳的速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段5：下放释放（适配原有模型） + # 阶段5：缓慢下放（精准定位，无掉落） step = 0 - while step < 450 and viewer.is_alive: + while step < 600 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.04] # 微调下放高度 + + target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] # 更贴近地面 error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) - # 延迟释放，适配原有模型 - if step > 250: + target_ctrl = error[i] * SMOOTH_GAIN * 0.5 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + + # 延迟且渐进释放（核心优化：避免提前释放） + if step > 300: if model.nu >= 4: - data.ctrl[3] = 0.0 + data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) if model.nu >= 5: - data.ctrl[4] = 0.0 + data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段6：归位（适配原有模型的初始位置） + # 阶段6：平稳归位（缓慢退回，无晃动） step = 0 - while step < 600 and viewer.is_alive: + while step < 700 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = np.array([0.0, 0.0, 0.12]) # 微调归位位置 + + target = np.array([0.0, 0.0, 0.15]) # 更高的归位位置，避免碰撞 error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.7 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - print("🎉 适配原有模型的自动抓取完成！") + print("🎉 平滑自动抓取完成！（无卡顿/掉落）") + + +# ===================== 初始化+主程序 ===================== +def init_model_and_viewer(): + if not os.path.exists(MODEL_PATH): + raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) + + viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) + viewer.cam.distance = 1.8 + viewer.cam.elevation = 12 + viewer.cam.azimuth = 50 + viewer.cam.lookat = [0.15, 0.0, 0.12] + + # 兼容原有模型ID + ee_id, obj_id = -1, -1 + for name in ["ee_site", "ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) + if ee_id >= 0: break + if ee_id < 0: + for name in ["ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if ee_id >= 0: break + for name in ["target_object", "object", "ball"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if obj_id >= 0: break + if obj_id < 0: + for name in ["object_geom", "ball_geom"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + if obj_id >= 0: break + + print("✅ 初始化完成！（平滑控制模式）") + print("🎮 操作指南：W/S/A/D/Q/E移动（丝滑无抖），空格抓取（渐进加力）") + print(" Z：一键平滑抓取 C：重置 ESC：退出") + return model, data, viewer, ee_id, obj_id -# ===================== 主程序（修复后版本） ===================== def main(): - global viewer # 声明全局变量，让auto_grasp能访问 + global viewer, last_ctrl + last_ctrl = np.zeros(10) # 初始化平滑缓存 model, data, viewer, ee_id, obj_id = init_model_and_viewer() try: while viewer.is_alive: - # 核心修复：用兼容版按键检测替代get_key() check_keyboard_input(viewer) - # 执行控制（适配原有模型） if control_cmd['reset']: mujoco.mj_resetData(model, data) mujoco.mj_forward(model, data) - print("🔄 原有模型已重置到初始状态！") + last_ctrl = np.zeros(10) # 重置平滑缓存 + print("🔄 模型重置完成（平滑缓存已清空）") control_cmd['reset'] = False elif control_cmd['auto']: auto_grasp(model, data, ee_id, obj_id) @@ -339,32 +350,25 @@ def main(): else: manual_control(model, data, ee_id) - # 仿真步进（微调延迟，适配原有模型的帧率） mujoco.mj_step(model, data) viewer.render() - time.sleep(0.004) # 微调延迟，适配原有模型的流畅度 + time.sleep(0.005) # 更慢的帧率，更丝滑 except Exception as e: - print(f"\n❌ 运行出错（适配原有模型时）: {e}") + print(f"\n❌ 运行出错: {e}") import traceback - traceback.print_exc() # 打印详细错误栈，方便排查 + traceback.print_exc() finally: with suppress(Exception): viewer.close() - print("\n🔚 程序退出（未修改任何robot.xml内容）") + print("\n🔚 程序退出（未修改robot.xml）") -# ===================== 运行入口 ===================== if __name__ == "__main__": - # 检查依赖（新增glfw检查） try: - import mujoco - import mujoco_viewer - import glfw + import mujoco, mujoco_viewer, glfw except ImportError as e: - missing_lib = str(e).split()[-1] - print(f"❌ 缺少依赖 {missing_lib}！执行以下命令安装：") - print(f" pip install mujoco mujoco-viewer glfw numpy matplotlib") + print(f"❌ 缺少依赖 {str(e).split()[-1]}！执行：") + print(" pip install mujoco mujoco-viewer glfw numpy matplotlib") exit(1) - main() \ No newline at end of file From 7eaaa6753723f3dbe88a688a321429455687d133 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Thu, 25 Dec 2025 20:11:36 +0800 Subject: [PATCH 21/26] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=A4=9A=E6=A8=A1?= =?UTF-8?q?=E5=BC=8F=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 412 ++++++++++++++++++---------- 1 file changed, 274 insertions(+), 138 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index b7a39efd37..d6caeb5d00 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -8,6 +8,7 @@ import time import glfw from contextlib import suppress +from enum import Enum # 新增枚举，简化模式管理 # ===================== 基础配置 ===================== warnings.filterwarnings('ignore') @@ -19,31 +20,55 @@ CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 核心优化参数（流畅性重点） ===================== -# 手动控制：更低速、更平滑 -MANUAL_SPEED = 0.025 # 进一步降低速度，减少抖动 -GRASP_FORCE = 3.8 # 微调抓取力度，保证抓稳且不卡顿 -# 自动抓取：平滑轨迹参数 + +# ===================== 新增：操作模式枚举（易管理） ===================== +class ControlMode(Enum): + MANUAL = 1 # 基础手动控制（原有） + PRECISE = 2 # 精准微调模式（新增） + AUTO_SIMPLE = 3 # 简易自动抓取（原有） + AUTO_COMPLEX = 4 # 复杂任务流程（新增） + CIRCLE_TASK = 5 # 画圆任务（新增） + BACK_FORTH = 6 # 往复运动（新增） + + +# ===================== 核心参数（保留流畅性+新增功能） ===================== +# 基础控制参数 +MANUAL_SPEED = 0.025 +PRECISE_SPEED = 0.01 # 精准模式速度（新增） +GRASP_FORCE = 3.8 AUTO_LIFT_HEIGHT = 0.12 AUTO_TRANSPORT_X = -0.15 -SMOOTH_GAIN = 3.0 # 降低控制增益，减少超调 -SMOOTH_CLIP = 1.0 # 更严格的输出限制，避免猛冲 -ACCEL_FACTOR = 0.05 # 加速度因子，让动作渐进加速/减速 +SMOOTH_GAIN = 3.0 +SMOOTH_CLIP = 1.0 +ACCEL_FACTOR = 0.05 + +# 新增任务参数 +CIRCLE_RADIUS = 0.1 # 画圆半径 +CIRCLE_SPEED = 0.005 # 画圆速度 +BACK_FORTH_DIST = 0.2 # 往复运动距离 -# ===================== 全局变量 ===================== +# ===================== 全局变量（丰富功能） ===================== control_cmd = { 'forward': 0, 'backward': 0, 'left': 0, 'right': 0, 'up': 0, 'down': 0, 'grasp': 0, 'release': 0, - 'auto': False, 'reset': False + 'auto_simple': False, # Z：简易自动 + 'auto_complex': False, # X：复杂任务 + 'circle_task': False, # V：画圆任务 + 'back_forth': False, # B：往复运动 + 'switch_precise': False, # P：切换精准模式 + 'reset': False } -# 平滑控制缓存（记录上一步的控制输出，避免突变） -last_ctrl = np.zeros(10) # 适配最多10个关节 +last_ctrl = np.zeros(10) +current_mode = ControlMode.MANUAL # 当前控制模式 +task_step = 0 # 任务步数计数器 -# ===================== 兼容版按键检测 ===================== +# ===================== 兼容版按键检测（新增操作按键） ===================== def check_keyboard_input(viewer): + global current_mode + # 重置基础指令 for key in control_cmd.keys(): - if key != 'auto' and key != 'reset': + if key not in ['auto_simple', 'auto_complex', 'circle_task', 'back_forth', 'switch_precise', 'reset']: control_cmd[key] = 0 if hasattr(viewer, 'window') and viewer.window is not None: @@ -55,41 +80,47 @@ def check_keyboard_input(viewer): control_cmd['right'] = 1 if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS else 0 control_cmd['up'] = 1 if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS else 0 control_cmd['down'] = 1 if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS else 0 - # 抓取/释放/自动/重置 + # 抓取/释放 control_cmd['grasp'] = 1 if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS else 0 control_cmd['release'] = 1 if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS else 0 - control_cmd['auto'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False + # 新增：多模式任务按键 + control_cmd['auto_simple'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False + control_cmd['auto_complex'] = True if glfw.get_key(window, glfw.KEY_X) == glfw.PRESS else False + control_cmd['circle_task'] = True if glfw.get_key(window, glfw.KEY_V) == glfw.PRESS else False + control_cmd['back_forth'] = True if glfw.get_key(window, glfw.KEY_B) == glfw.PRESS else False + control_cmd['switch_precise'] = True if glfw.get_key(window, glfw.KEY_P) == glfw.PRESS else False control_cmd['reset'] = True if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS else False # ESC退出 if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: glfw.set_window_should_close(window, True) + + # 切换精准模式（新增） + if control_cmd['switch_precise']: + current_mode = ControlMode.PRECISE if current_mode != ControlMode.PRECISE else ControlMode.MANUAL + mode_name = "精准微调" if current_mode == ControlMode.PRECISE else "基础手动" + print( + f"\n🔄 切换到【{mode_name}】模式（速度：{PRECISE_SPEED if current_mode == ControlMode.PRECISE else MANUAL_SPEED}）") + control_cmd['switch_precise'] = False else: - print("\n⚠️ 旧版mujoco-viewer，按Z触发自动抓取，C重置") - control_cmd['auto'] = True + print("\n⚠️ 旧版mujoco-viewer，支持：Z(简易自动)、X(复杂任务)、C(重置)") + control_cmd['auto_simple'] = True -# ===================== 平滑控制函数（核心优化） ===================== +# ===================== 核心控制函数（保留平滑+新增任务） ===================== def smooth_control(target_ctrl, last_ctrl, joint_idx): - """ - 平滑控制输出，避免关节突变（解决抖动/卡顿） - :param target_ctrl: 目标控制值 - :param last_ctrl: 上一步控制值 - :param joint_idx: 关节索引 - :return: 平滑后的控制值 - """ - # 渐进逼近目标值，避免猛冲 delta = target_ctrl - last_ctrl[joint_idx] smoothed = last_ctrl[joint_idx] + delta * ACCEL_FACTOR - # 限制最大变化量，彻底避免抖动 smoothed = np.clip(smoothed, -SMOOTH_CLIP, SMOOTH_CLIP) - # 更新缓存 last_ctrl[joint_idx] = smoothed return smoothed def manual_control(model, data, ee_id): - """手动控制（增加平滑逻辑，动作更丝滑）""" - global last_ctrl + """手动控制（新增精准模式）""" + global last_ctrl, current_mode + # 选择速度（基础/精准） + speed = PRECISE_SPEED if current_mode == ControlMode.PRECISE else MANUAL_SPEED + # 安全获取末端位置 ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: @@ -98,42 +129,37 @@ def manual_control(model, data, ee_id): except: ee_pos = data.xpos[ee_id].copy() - # 计算目标位置（低速，易控） + # 计算目标位置 target_pos = ee_pos.copy() - target_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * MANUAL_SPEED - target_pos[1] += (control_cmd['left'] - control_cmd['right']) * MANUAL_SPEED - target_pos[2] += (control_cmd['up'] - control_cmd['down']) * MANUAL_SPEED + target_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * speed + target_pos[1] += (control_cmd['left'] - control_cmd['right']) * speed + target_pos[2] += (control_cmd['up'] - control_cmd['down']) * speed - # 计算误差并平滑控制（核心优化） + # 平滑控制 error = target_pos - ee_pos for i in range(min(3, model.njnt)): target_ctrl = error[i] * SMOOTH_GAIN - # 平滑输出，避免关节突变 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - # 抓取控制（渐进加力，避免夹爪猛夹） + # 渐进抓取/释放 if control_cmd['grasp']: - # 渐进增加抓取力，避免物体被弹飞 if model.nu >= 4: data.ctrl[3] = min(data.ctrl[3] + 0.1, GRASP_FORCE) if model.nu >= 5: data.ctrl[4] = max(data.ctrl[4] - 0.1, -GRASP_FORCE) elif control_cmd['release']: - # 渐进释放，避免物体掉落 if model.nu >= 4: data.ctrl[3] = max(data.ctrl[3] - 0.1, 0.0) if model.nu >= 5: data.ctrl[4] = min(data.ctrl[4] + 0.1, 0.0) -def auto_grasp(model, data, ee_id, obj_id): - """一键自动抓取（全流程平滑优化，无卡顿）""" +# ===================== 新增：丰富的自动任务函数 ===================== +def auto_simple_grasp(model, data, ee_id, obj_id): + """原有简易自动抓取（保留）""" global last_ctrl - print("🔄 开始平滑自动抓取...") - # 重置平滑缓存 + print("🔄 开始【简易自动抓取】任务...") last_ctrl = np.zeros(10) - - # 安全获取物体位置 obj_pos = np.array([0.2, 0.0, 0.05]) if obj_id >= 0: try: @@ -141,157 +167,242 @@ def auto_grasp(model, data, ee_id, obj_id): except: pass - # 阶段1：缓慢移动到物体上方（平滑逼近，无猛冲） + # 阶段1：移动到物体上方 step = 0 while step < 800 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - - # 目标位置：物体上方（高度微调，避免碰撞） + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() target = obj_pos + [0, 0, 0.08] error = target - ee_pos - - # 平滑控制关节，无抖动 for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.8 # 更慢速度 + target_ctrl = error[i] * SMOOTH_GAIN * 0.8 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段2：缓慢下降（渐进接近，避免压碎物体） + # 阶段2：下降抓取 step = 0 while step < 600 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - - # 动态调整目标：根据物体位置实时微调，避免偏差 - target = obj_pos + [0, 0, 0.01] # 仅贴近物体，不压下去 + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target = obj_pos + [0, 0, 0.01] error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.5 # 极慢速度 + target_ctrl = error[i] * SMOOTH_GAIN * 0.5 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - - # 渐进闭合夹爪（核心优化：避免猛夹导致物体掉落） if model.nu >= 4: data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) if model.nu >= 5: data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) - mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段3：缓慢抬升（确认抓稳后再抬升） + # 阶段3-6：抬升→搬运→下放→归位（保留流畅性） step = 0 - grasp_confirmed = False while step < 500 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - - # 确认抓稳后再抬升（避免刚夹就抬导致掉落） - if step > 100: - grasp_confirmed = True - - if grasp_confirmed: - target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] - else: - target = obj_pos + [0, 0, 0.01] # 先保持位置 - + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] if step > 100 else obj_pos + [0, 0, 0.01] error = target - ee_pos for i in range(min(3, model.njnt)): target_ctrl = error[i] * SMOOTH_GAIN * 0.7 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段4：平稳搬运（匀速移动，无晃动） step = 0 while step < 800 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] error = target - ee_pos for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.6 # 更平稳的速度 + target_ctrl = error[i] * SMOOTH_GAIN * 0.6 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段5：缓慢下放（精准定位，无掉落） step = 0 while step < 600 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - - target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] # 更贴近地面 + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] error = target - ee_pos for i in range(min(3, model.njnt)): target_ctrl = error[i] * SMOOTH_GAIN * 0.5 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - - # 延迟且渐进释放（核心优化：避免提前释放） if step > 300: if model.nu >= 4: data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) if model.nu >= 5: data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) - mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段6：平稳归位（缓慢退回，无晃动） step = 0 while step < 700 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() - - target = np.array([0.0, 0.0, 0.15]) # 更高的归位位置，避免碰撞 + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target = np.array([0.0, 0.0, 0.15]) error = target - ee_pos for i in range(min(3, model.njnt)): target_ctrl = error[i] * SMOOTH_GAIN * 0.7 data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + print("🎉 【简易自动抓取】任务完成！") + + +def auto_complex_task(model, data, ee_id, obj_id): + """新增：复杂任务流程（多物体抓取+多位置放置）""" + global last_ctrl + print("🔄 开始【复杂任务】：抓取→搬运→放置→返回→二次抓取...") + last_ctrl = np.zeros(10) + # 定义多个目标位置（丰富任务） + target_positions = [ + np.array([0.2, 0.0, 0.05]), # 初始物体位置 + np.array([-0.15, 0.1, 0.05]), # 第一个放置点 + np.array([-0.15, -0.1, 0.05]), # 第二个放置点 + np.array([0.2, 0.0, 0.05]) # 回到初始位置 + ] + + for idx, target in enumerate(target_positions): + if not viewer.is_alive: + break + print(f"📌 复杂任务阶段 {idx + 1}/{len(target_positions)}：移动到 {target[:2]} 位置") + + # 阶段1：移动到目标上方 + step = 0 + while step < 700 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target_above = target + [0, 0, 0.08] + error = target_above - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段2：下降（仅第一阶段抓取，其他阶段放置） + step = 0 + while step < 500 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.5, last_ctrl, i) + + # 第一阶段抓取，第二/三阶段释放，第四阶段准备二次抓取 + if idx == 0: # 抓取 + if model.nu >= 4: + data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) + if model.nu >= 5: + data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + elif idx in [1, 2]: # 释放 + if model.nu >= 4: + data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) + if model.nu >= 5: + data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) + + mujoco.mj_step(model, data) + viewer.render() + step += 1 + + # 阶段3：抬升 + step = 0 + while step < 400 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target_up = target + [0, 0, AUTO_LIFT_HEIGHT] + error = target_up - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.6, last_ctrl, i) + mujoco.mj_step(model, data) + viewer.render() + step += 1 + # 归位 + step = 0 + while step < 600 and viewer.is_alive: + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + target = np.array([0.0, 0.0, 0.15]) + error = target - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) mujoco.mj_step(model, data) viewer.render() step += 1 - print("🎉 平滑自动抓取完成！（无卡顿/掉落）") + print("🎉 【复杂任务】全流程完成！（多位置抓取+放置）") + + +def circle_task(model, data, ee_id): + """新增：画圆任务（机械臂末端画圆，丰富操作）""" + global last_ctrl, task_step + print("🔄 开始【画圆任务】：末端以原点为中心画圆（按ESC停止）") + last_ctrl = np.zeros(10) + center = np.array([0.1, 0.0, 0.1]) # 圆心位置 + + while viewer.is_alive and task_step < 1500: # 画2圈左右 + # 计算圆上的目标点（三角函数生成圆形轨迹） + angle = task_step * CIRCLE_SPEED + target_x = center[0] + CIRCLE_RADIUS * np.cos(angle) + target_y = center[1] + CIRCLE_RADIUS * np.sin(angle) + target_z = center[2] + target_pos = np.array([target_x, target_y, target_z]) + + # 安全获取末端位置 + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + error = target_pos - ee_pos + + # 平滑控制画圆 + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.8, last_ctrl, i) + + # 实时反馈画圆进度 + if task_step % 100 == 0: + print(f"📈 画圆进度：{int(task_step / 1500 * 100)}%（角度：{int(angle * 180 / np.pi)}°）") + + mujoco.mj_step(model, data) + viewer.render() + task_step += 1 + + task_step = 0 + print("🎉 【画圆任务】完成！机械臂末端画出完整圆形轨迹") + + +def back_forth_task(model, data, ee_id): + """新增：往复运动任务（前后/左右往复，丰富操作）""" + global last_ctrl, task_step + print("🔄 开始【往复运动任务】：前后往复移动（按ESC停止）") + last_ctrl = np.zeros(10) + start_pos = np.array([0.0, 0.0, 0.1]) # 起始位置 + + while viewer.is_alive and task_step < 2000: + # 生成往复轨迹（正弦函数实现平滑往复） + cycle = np.sin(task_step * 0.01) # -1~1的周期变化 + target_x = start_pos[0] + cycle * BACK_FORTH_DIST + target_pos = np.array([target_x, start_pos[1], start_pos[2]]) + + # 平滑控制往复运动 + ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + error = target_pos - ee_pos + for i in range(min(3, model.njnt)): + data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) + + # 实时反馈往复进度 + if task_step % 200 == 0: + direction = "前" if cycle > 0 else "后" + print(f"📌 往复运动：当前方向【{direction}】（位置X：{target_x:.2f}）") + + mujoco.mj_step(model, data) + viewer.render() + task_step += 1 + + task_step = 0 + print("🎉 【往复运动任务】完成！机械臂完成多次平滑往复") -# ===================== 初始化+主程序 ===================== +# ===================== 初始化+主程序（整合所有功能） ===================== def init_model_and_viewer(): if not os.path.exists(MODEL_PATH): raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") @@ -323,36 +434,61 @@ def init_model_and_viewer(): obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) if obj_id >= 0: break - print("✅ 初始化完成！（平滑控制模式）") - print("🎮 操作指南：W/S/A/D/Q/E移动（丝滑无抖），空格抓取（渐进加力）") - print(" Z：一键平滑抓取 C：重置 ESC：退出") + # 新增：打印丰富的操作指南 + print("=" * 50) + print("✅ 多功能机械臂控制程序初始化完成！") + print("🎮 基础操作：") + print(" W/S/A/D/Q/E：移动 空格：抓取 R：释放 P：切换精准/基础模式") + print("🎯 自动任务（新增）：") + print(" Z：简易自动抓取 X：复杂多位置任务") + print(" V：画圆任务 B：往复运动任务") + print("🔧 其他：C-重置 ESC-退出") + print("=" * 50) return model, data, viewer, ee_id, obj_id def main(): - global viewer, last_ctrl - last_ctrl = np.zeros(10) # 初始化平滑缓存 + global viewer, last_ctrl, task_step, current_mode + last_ctrl = np.zeros(10) + task_step = 0 + current_mode = ControlMode.MANUAL model, data, viewer, ee_id, obj_id = init_model_and_viewer() try: while viewer.is_alive: check_keyboard_input(viewer) + # 重置功能 if control_cmd['reset']: mujoco.mj_resetData(model, data) mujoco.mj_forward(model, data) - last_ctrl = np.zeros(10) # 重置平滑缓存 - print("🔄 模型重置完成（平滑缓存已清空）") + last_ctrl = np.zeros(10) + task_step = 0 + current_mode = ControlMode.MANUAL + print("\n🔄 模型完全重置：位置、缓存、任务、模式均已恢复初始状态") control_cmd['reset'] = False - elif control_cmd['auto']: - auto_grasp(model, data, ee_id, obj_id) - control_cmd['auto'] = False + + # 执行各类自动任务（新增） + elif control_cmd['auto_simple']: + auto_simple_grasp(model, data, ee_id, obj_id) + control_cmd['auto_simple'] = False + elif control_cmd['auto_complex']: + auto_complex_task(model, data, ee_id, obj_id) + control_cmd['auto_complex'] = False + elif control_cmd['circle_task']: + circle_task(model, data, ee_id) + control_cmd['circle_task'] = False + elif control_cmd['back_forth']: + back_forth_task(model, data, ee_id) + control_cmd['back_forth'] = False + + # 手动控制（基础/精准） else: manual_control(model, data, ee_id) mujoco.mj_step(model, data) viewer.render() - time.sleep(0.005) # 更慢的帧率，更丝滑 + time.sleep(0.005) except Exception as e: print(f"\n❌ 运行出错: {e}") @@ -361,7 +497,7 @@ def main(): finally: with suppress(Exception): viewer.close() - print("\n🔚 程序退出（未修改robot.xml）") + print("\n🔚 多功能机械臂程序退出（未修改robot.xml）") if __name__ == "__main__": From 6b97443d791a2c9ab9c4c6cdddf2f943f950893b Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 26 Dec 2025 15:40:08 +0800 Subject: [PATCH 22/26] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=85=B3=E8=8A=82?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E7=9A=84=E5=9D=90=E6=A0=87=E6=98=A0=E5=B0=84?= =?UTF-8?q?=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 441 ++++++++++++++-------------- 1 file changed, 218 insertions(+), 223 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index d6caeb5d00..614e03d9bb 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -8,7 +8,7 @@ import time import glfw from contextlib import suppress -from enum import Enum # 新增枚举，简化模式管理 +from enum import Enum # ===================== 基础配置 ===================== warnings.filterwarnings('ignore') @@ -21,49 +21,50 @@ MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 新增：操作模式枚举（易管理） ===================== +# ===================== 操作模式枚举 ===================== class ControlMode(Enum): - MANUAL = 1 # 基础手动控制（原有） - PRECISE = 2 # 精准微调模式（新增） - AUTO_SIMPLE = 3 # 简易自动抓取（原有） - AUTO_COMPLEX = 4 # 复杂任务流程（新增） - CIRCLE_TASK = 5 # 画圆任务（新增） - BACK_FORTH = 6 # 往复运动（新增） - - -# ===================== 核心参数（保留流畅性+新增功能） ===================== -# 基础控制参数 -MANUAL_SPEED = 0.025 -PRECISE_SPEED = 0.01 # 精准模式速度（新增） + MANUAL = 1 # 基础手动控制 + PRECISE = 2 # 精准微调模式 + AUTO_SIMPLE = 3 # 简易自动抓取 + AUTO_COMPLEX = 4 # 复杂任务流程 + CIRCLE_TASK = 5 # 画圆任务 + BACK_FORTH = 6 # 往复运动 + + +# ===================== 核心参数（修复转圈关键） ===================== +# 控制参数（降低增益，杜绝转圈） +MANUAL_SPEED = 0.015 # 进一步降低速度，减少误差累积 +PRECISE_SPEED = 0.008 # 精准模式速度 GRASP_FORCE = 3.8 -AUTO_LIFT_HEIGHT = 0.12 -AUTO_TRANSPORT_X = -0.15 -SMOOTH_GAIN = 3.0 -SMOOTH_CLIP = 1.0 -ACCEL_FACTOR = 0.05 - -# 新增任务参数 -CIRCLE_RADIUS = 0.1 # 画圆半径 -CIRCLE_SPEED = 0.005 # 画圆速度 -BACK_FORTH_DIST = 0.2 # 往复运动距离 - -# ===================== 全局变量（丰富功能） ===================== +AUTO_LIFT_HEIGHT = 0.10 +AUTO_TRANSPORT_X = -0.12 +# 逆运动学参数（核心：限制关节范围，杜绝转圈） +IK_GAIN = 1.5 # 逆运动学增益（大幅降低） +JOINT_LIMITS = np.array([ + [-1.2, 1.2], # joint1范围 + [-1.0, 1.0], # joint2范围 + [-0.8, 0.8] # joint3范围 +]) +# 任务参数 +CIRCLE_RADIUS = 0.08 # 缩小画圆半径，避免超出关节范围 +CIRCLE_SPEED = 0.004 +BACK_FORTH_DIST = 0.15 + +# ===================== 全局变量 ===================== control_cmd = { 'forward': 0, 'backward': 0, 'left': 0, 'right': 0, 'up': 0, 'down': 0, 'grasp': 0, 'release': 0, - 'auto_simple': False, # Z：简易自动 - 'auto_complex': False, # X：复杂任务 - 'circle_task': False, # V：画圆任务 - 'back_forth': False, # B：往复运动 - 'switch_precise': False, # P：切换精准模式 - 'reset': False + 'auto_simple': False, 'auto_complex': False, + 'circle_task': False, 'back_forth': False, + 'switch_precise': False, 'reset': False } -last_ctrl = np.zeros(10) -current_mode = ControlMode.MANUAL # 当前控制模式 -task_step = 0 # 任务步数计数器 +current_mode = ControlMode.MANUAL +task_step = 0 +# 新增：目标位置缓存（避免突变） +target_ee_pos = np.array([0.0, 0.0, 0.1]) # 初始末端目标位置 -# ===================== 兼容版按键检测（新增操作按键） ===================== +# ===================== 兼容版按键检测 ===================== def check_keyboard_input(viewer): global current_mode # 重置基础指令 @@ -83,7 +84,7 @@ def check_keyboard_input(viewer): # 抓取/释放 control_cmd['grasp'] = 1 if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS else 0 control_cmd['release'] = 1 if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS else 0 - # 新增：多模式任务按键 + # 多模式任务按键 control_cmd['auto_simple'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False control_cmd['auto_complex'] = True if glfw.get_key(window, glfw.KEY_X) == glfw.PRESS else False control_cmd['circle_task'] = True if glfw.get_key(window, glfw.KEY_V) == glfw.PRESS else False @@ -94,7 +95,7 @@ def check_keyboard_input(viewer): if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: glfw.set_window_should_close(window, True) - # 切换精准模式（新增） + # 切换精准模式 if control_cmd['switch_precise']: current_mode = ControlMode.PRECISE if current_mode != ControlMode.PRECISE else ControlMode.MANUAL mode_name = "精准微调" if current_mode == ControlMode.PRECISE else "基础手动" @@ -106,42 +107,71 @@ def check_keyboard_input(viewer): control_cmd['auto_simple'] = True -# ===================== 核心控制函数（保留平滑+新增任务） ===================== -def smooth_control(target_ctrl, last_ctrl, joint_idx): - delta = target_ctrl - last_ctrl[joint_idx] - smoothed = last_ctrl[joint_idx] + delta * ACCEL_FACTOR - smoothed = np.clip(smoothed, -SMOOTH_CLIP, SMOOTH_CLIP) - last_ctrl[joint_idx] = smoothed - return smoothed +# ===================== 核心修复：逆运动学控制（杜绝转圈） ===================== +def ik_control(model, data, ee_id, target_pos): + """ + 逆运动学控制：让机械臂末端精准跟随目标位置，杜绝转圈 + :param model: MuJoCo模型 + :param data: MuJoCo数据 + :param ee_id: 末端ID + :param target_pos: 末端目标位置 + """ + # 1. 获取当前末端位置 + current_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + current_pos = data.site_xpos[ee_id].copy() + except: + current_pos = data.xpos[ee_id].copy() + + # 2. 计算位置误差（限制误差范围，避免过大） + error = target_pos - current_pos + error = np.clip(error, -0.05, 0.05) # 单次最大误差不超过0.05 + + # 3. 计算关节雅可比矩阵（核心：关联末端位置和关节角度） + jacp = np.zeros((3, model.nv)) # 位置雅可比 + jacr = np.zeros((3, model.nv)) # 旋转雅可比 + if ee_id >= 0: + mujoco.mj_jac(model, data, jacp, jacr, current_pos, ee_id) + + # 4. 提取前3个关节的雅可比（机械臂主关节） + jacp_joints = jacp[:, :3] + + # 5. 计算关节速度指令（伪逆求解，避免无解转圈） + jnt_vel = np.dot(jacp_joints.T, error * IK_GAIN) + jnt_vel = np.clip(jnt_vel, -0.2, 0.2) # 限制关节速度 + + # 6. 积分得到关节角度，并限制关节范围（核心：杜绝转圈） + for i in range(min(3, model.njnt)): + # 积分更新关节角度 + data.qpos[i] += jnt_vel[i] * model.opt.timestep + # 限制关节在安全范围（彻底杜绝转圈） + data.qpos[i] = np.clip(data.qpos[i], JOINT_LIMITS[i][0], JOINT_LIMITS[i][1]) + + # 7. 更新关节数据，应用限制 + mujoco.mj_forward(model, data) def manual_control(model, data, ee_id): - """手动控制（新增精准模式）""" - global last_ctrl, current_mode + """手动控制（基于逆运动学，杜绝转圈）""" + global target_ee_pos # 选择速度（基础/精准） speed = PRECISE_SPEED if current_mode == ControlMode.PRECISE else MANUAL_SPEED - # 安全获取末端位置 - ee_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - ee_pos = data.site_xpos[ee_id].copy() - except: - ee_pos = data.xpos[ee_id].copy() + # 1. 更新目标位置（渐进式，避免突变） + target_ee_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * speed + target_ee_pos[1] += (control_cmd['left'] - control_cmd['right']) * speed + target_ee_pos[2] += (control_cmd['up'] - control_cmd['down']) * speed - # 计算目标位置 - target_pos = ee_pos.copy() - target_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * speed - target_pos[1] += (control_cmd['left'] - control_cmd['right']) * speed - target_pos[2] += (control_cmd['up'] - control_cmd['down']) * speed + # 2. 限制目标位置在安全范围（避免超出关节可达范围） + target_ee_pos = np.clip(target_ee_pos, + np.array([-0.2, -0.15, 0.05]), + np.array([0.3, 0.15, 0.2])) - # 平滑控制 - error = target_pos - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + # 3. 逆运动学控制（核心：让末端精准跟随目标，不转圈） + ik_control(model, data, ee_id, target_ee_pos) - # 渐进抓取/释放 + # 4. 渐进抓取/释放 if control_cmd['grasp']: if model.nu >= 4: data.ctrl[3] = min(data.ctrl[3] + 0.1, GRASP_FORCE) @@ -154,12 +184,14 @@ def manual_control(model, data, ee_id): data.ctrl[4] = min(data.ctrl[4] + 0.1, 0.0) -# ===================== 新增：丰富的自动任务函数 ===================== +# ===================== 自动任务（适配逆运动学，杜绝转圈） ===================== def auto_simple_grasp(model, data, ee_id, obj_id): - """原有简易自动抓取（保留）""" - global last_ctrl + """简易自动抓取（基于逆运动学）""" + global target_ee_pos print("🔄 开始【简易自动抓取】任务...") - last_ctrl = np.zeros(10) + # 重置目标位置 + target_ee_pos = np.array([0.0, 0.0, 0.1]) + # 获取物体位置 obj_pos = np.array([0.2, 0.0, 0.05]) if obj_id >= 0: try: @@ -167,104 +199,84 @@ def auto_simple_grasp(model, data, ee_id, obj_id): except: pass - # 阶段1：移动到物体上方 + # 阶段1：移动到物体上方（安全位置） step = 0 - while step < 800 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target = obj_pos + [0, 0, 0.08] - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.8 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + while step < 1000 and viewer.is_alive: + target = obj_pos + [0, 0, 0.07] # 降低高度，避免超出范围 + ik_control(model, data, ee_id, target) + # 渐进闭合夹爪（提前准备） + if step > 800 and model.nu >= 4: + data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) + data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) mujoco.mj_step(model, data) viewer.render() step += 1 # 阶段2：下降抓取 step = 0 - while step < 600 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target = obj_pos + [0, 0, 0.01] - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.5 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - if model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) - if model.nu >= 5: - data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + while step < 800 and viewer.is_alive: + target = obj_pos + [0, 0, 0.02] # 贴近物体但不碰撞 + ik_control(model, data, ee_id, target) mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段3-6：抬升→搬运→下放→归位（保留流畅性） + # 阶段3：抬升 step = 0 - while step < 500 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] if step > 100 else obj_pos + [0, 0, 0.01] - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.7 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + while step < 800 and viewer.is_alive: + target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + ik_control(model, data, ee_id, target) mujoco.mj_step(model, data) viewer.render() step += 1 + # 阶段4：搬运 step = 0 - while step < 800 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + while step < 1000 and viewer.is_alive: target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.6 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + ik_control(model, data, ee_id, target) mujoco.mj_step(model, data) viewer.render() step += 1 + # 阶段5：下放释放 step = 0 - while step < 600 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() + while step < 800 and viewer.is_alive: target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.5 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - if step > 300: + ik_control(model, data, ee_id, target) + # 渐进释放 + if step > 400: if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) + data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) if model.nu >= 5: - data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) + data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) mujoco.mj_step(model, data) viewer.render() step += 1 + # 阶段6：归位 step = 0 - while step < 700 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target = np.array([0.0, 0.0, 0.15]) - error = target - ee_pos - for i in range(min(3, model.njnt)): - target_ctrl = error[i] * SMOOTH_GAIN * 0.7 - data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + while step < 1000 and viewer.is_alive: + target = np.array([0.0, 0.0, 0.12]) + ik_control(model, data, ee_id, target) mujoco.mj_step(model, data) viewer.render() step += 1 - print("🎉 【简易自动抓取】任务完成！") + print("🎉 【简易自动抓取】任务完成！（无转圈）") def auto_complex_task(model, data, ee_id, obj_id): - """新增：复杂任务流程（多物体抓取+多位置放置）""" - global last_ctrl - print("🔄 开始【复杂任务】：抓取→搬运→放置→返回→二次抓取...") - last_ctrl = np.zeros(10) - # 定义多个目标位置（丰富任务） + """复杂任务流程（多位置，无转圈）""" + global target_ee_pos + print("🔄 开始【复杂任务】：多位置抓取+放置...") + target_ee_pos = np.array([0.0, 0.0, 0.1]) + # 定义安全的目标位置（避免超出关节范围） target_positions = [ - np.array([0.2, 0.0, 0.05]), # 初始物体位置 - np.array([-0.15, 0.1, 0.05]), # 第一个放置点 - np.array([-0.15, -0.1, 0.05]), # 第二个放置点 - np.array([0.2, 0.0, 0.05]) # 回到初始位置 + np.array([0.18, 0.0, 0.05]), # 初始物体位置 + np.array([-0.10, 0.08, 0.05]), # 第一个放置点 + np.array([-0.10, -0.08, 0.05]), # 第二个放置点 + np.array([0.18, 0.0, 0.05]) # 回到初始位置 ] for idx, target in enumerate(target_positions): @@ -274,123 +286,102 @@ def auto_complex_task(model, data, ee_id, obj_id): # 阶段1：移动到目标上方 step = 0 - while step < 700 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target_above = target + [0, 0, 0.08] - error = target_above - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) + while step < 900 and viewer.is_alive: + target_above = target + [0, 0, 0.06] + ik_control(model, data, ee_id, target_above) mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段2：下降（仅第一阶段抓取，其他阶段放置） + # 阶段2：下降（抓取/释放） step = 0 - while step < 500 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - error = target - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.5, last_ctrl, i) - - # 第一阶段抓取，第二/三阶段释放，第四阶段准备二次抓取 + while step < 700 and viewer.is_alive: + ik_control(model, data, ee_id, target + [0, 0, 0.02]) + # 第一阶段抓取，其他阶段释放 if idx == 0: # 抓取 if model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) + data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) if model.nu >= 5: - data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) elif idx in [1, 2]: # 释放 if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) + data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) if model.nu >= 5: - data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) - + data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) mujoco.mj_step(model, data) viewer.render() step += 1 # 阶段3：抬升 step = 0 - while step < 400 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target_up = target + [0, 0, AUTO_LIFT_HEIGHT] - error = target_up - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.6, last_ctrl, i) + while step < 700 and viewer.is_alive: + ik_control(model, data, ee_id, target + [0, 0, AUTO_LIFT_HEIGHT]) mujoco.mj_step(model, data) viewer.render() step += 1 # 归位 step = 0 - while step < 600 and viewer.is_alive: - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - target = np.array([0.0, 0.0, 0.15]) - error = target - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) + while step < 900 and viewer.is_alive: + ik_control(model, data, ee_id, np.array([0.0, 0.0, 0.12])) mujoco.mj_step(model, data) viewer.render() step += 1 - print("🎉 【复杂任务】全流程完成！（多位置抓取+放置）") + print("🎉 【复杂任务】全流程完成！（无转圈）") def circle_task(model, data, ee_id): - """新增：画圆任务（机械臂末端画圆，丰富操作）""" - global last_ctrl, task_step - print("🔄 开始【画圆任务】：末端以原点为中心画圆（按ESC停止）") - last_ctrl = np.zeros(10) - center = np.array([0.1, 0.0, 0.1]) # 圆心位置 - - while viewer.is_alive and task_step < 1500: # 画2圈左右 - # 计算圆上的目标点（三角函数生成圆形轨迹） + """画圆任务（限制范围，无转圈）""" + global task_step + print("🔄 开始【画圆任务】：末端画圆（无转圈）") + center = np.array([0.08, 0.0, 0.10]) # 缩小圆心范围 + + while viewer.is_alive and task_step < 2000: + # 计算圆上的目标点（限制在关节可达范围） angle = task_step * CIRCLE_SPEED target_x = center[0] + CIRCLE_RADIUS * np.cos(angle) target_y = center[1] + CIRCLE_RADIUS * np.sin(angle) - target_z = center[2] - target_pos = np.array([target_x, target_y, target_z]) - - # 安全获取末端位置 - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - error = target_pos - ee_pos + target_pos = np.array([target_x, target_y, center[2]]) + # 限制目标位置 + target_pos = np.clip(target_pos, + np.array([-0.1, -0.1, 0.08]), + np.array([0.2, 0.1, 0.15])) - # 平滑控制画圆 - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.8, last_ctrl, i) + # 逆运动学控制画圆 + ik_control(model, data, ee_id, target_pos) - # 实时反馈画圆进度 - if task_step % 100 == 0: - print(f"📈 画圆进度：{int(task_step / 1500 * 100)}%（角度：{int(angle * 180 / np.pi)}°）") + # 实时反馈 + if task_step % 200 == 0: + print(f"📈 画圆进度：{int(task_step / 2000 * 100)}%（角度：{int(angle * 180 / np.pi)}°）") mujoco.mj_step(model, data) viewer.render() task_step += 1 task_step = 0 - print("🎉 【画圆任务】完成！机械臂末端画出完整圆形轨迹") + print("🎉 【画圆任务】完成！（无转圈）") def back_forth_task(model, data, ee_id): - """新增：往复运动任务（前后/左右往复，丰富操作）""" - global last_ctrl, task_step - print("🔄 开始【往复运动任务】：前后往复移动（按ESC停止）") - last_ctrl = np.zeros(10) - start_pos = np.array([0.0, 0.0, 0.1]) # 起始位置 - - while viewer.is_alive and task_step < 2000: - # 生成往复轨迹（正弦函数实现平滑往复） - cycle = np.sin(task_step * 0.01) # -1~1的周期变化 + """往复运动任务（无转圈）""" + global task_step + print("🔄 开始【往复运动任务】：前后往复（无转圈）") + start_pos = np.array([0.05, 0.0, 0.10]) + + while viewer.is_alive and task_step < 2500: + # 生成往复轨迹（限制范围） + cycle = np.sin(task_step * 0.008) target_x = start_pos[0] + cycle * BACK_FORTH_DIST + # 限制X轴范围，避免超出关节 + target_x = np.clip(target_x, -0.1, 0.2) target_pos = np.array([target_x, start_pos[1], start_pos[2]]) - # 平滑控制往复运动 - ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id < 0 else data.site_xpos[ee_id].copy() - error = target_pos - ee_pos - for i in range(min(3, model.njnt)): - data.ctrl[i] = smooth_control(error[i] * SMOOTH_GAIN * 0.7, last_ctrl, i) + # 逆运动学控制往复 + ik_control(model, data, ee_id, target_pos) - # 实时反馈往复进度 - if task_step % 200 == 0: + # 实时反馈 + if task_step % 300 == 0: direction = "前" if cycle > 0 else "后" print(f"📌 往复运动：当前方向【{direction}】（位置X：{target_x:.2f}）") @@ -399,23 +390,26 @@ def back_forth_task(model, data, ee_id): task_step += 1 task_step = 0 - print("🎉 【往复运动任务】完成！机械臂完成多次平滑往复") + print("🎉 【往复运动任务】完成！（无转圈）") -# ===================== 初始化+主程序（整合所有功能） ===================== +# ===================== 初始化+主程序 ===================== def init_model_and_viewer(): if not os.path.exists(MODEL_PATH): raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - mujoco.mj_resetData(model, data) + + # 初始化关节位置（重置到中间位置，避免初始转圈） + for i in range(min(3, model.njnt)): + data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 mujoco.mj_forward(model, data) viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) viewer.cam.distance = 1.8 - viewer.cam.elevation = 12 - viewer.cam.azimuth = 50 - viewer.cam.lookat = [0.15, 0.0, 0.12] + viewer.cam.elevation = 15 # 调整视角，更清楚看关节 + viewer.cam.azimuth = 60 + viewer.cam.lookat = [0.1, 0.0, 0.1] # 兼容原有模型ID ee_id, obj_id = -1, -1 @@ -434,41 +428,42 @@ def init_model_and_viewer(): obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) if obj_id >= 0: break - # 新增：打印丰富的操作指南 - print("=" * 50) - print("✅ 多功能机械臂控制程序初始化完成！") - print("🎮 基础操作：") - print(" W/S/A/D/Q/E：移动 空格：抓取 R：释放 P：切换精准/基础模式") - print("🎯 自动任务（新增）：") - print(" Z：简易自动抓取 X：复杂多位置任务") - print(" V：画圆任务 B：往复运动任务") - print("🔧 其他：C-重置 ESC-退出") - print("=" * 50) + # 打印修复后的操作指南 + print("=" * 60) + print("✅ 机械臂控制程序（修复转圈问题）初始化完成！") + print("🔧 核心修复：逆运动学控制+关节范围限制，彻底杜绝转圈") + print("🎮 操作指南：") + print(" W/S/A/D/Q/E：移动（精准不转圈） 空格：抓取 R：释放 P：精准模式") + print(" Z：简易抓取 X：复杂任务 V：画圆 B：往复运动") + print(" C：重置 ESC：退出") + print("=" * 60) return model, data, viewer, ee_id, obj_id def main(): - global viewer, last_ctrl, task_step, current_mode - last_ctrl = np.zeros(10) + global viewer, task_step, current_mode, target_ee_pos task_step = 0 current_mode = ControlMode.MANUAL + target_ee_pos = np.array([0.0, 0.0, 0.1]) model, data, viewer, ee_id, obj_id = init_model_and_viewer() try: while viewer.is_alive: check_keyboard_input(viewer) - # 重置功能 + # 重置功能（恢复初始关节位置） if control_cmd['reset']: - mujoco.mj_resetData(model, data) + # 重置关节到中间位置 + for i in range(min(3, model.njnt)): + data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 mujoco.mj_forward(model, data) - last_ctrl = np.zeros(10) + target_ee_pos = np.array([0.0, 0.0, 0.1]) task_step = 0 current_mode = ControlMode.MANUAL - print("\n🔄 模型完全重置：位置、缓存、任务、模式均已恢复初始状态") + print("\n🔄 模型重置完成：关节回到中间位置，彻底杜绝初始转圈") control_cmd['reset'] = False - # 执行各类自动任务（新增） + # 执行自动任务 elif control_cmd['auto_simple']: auto_simple_grasp(model, data, ee_id, obj_id) control_cmd['auto_simple'] = False @@ -482,13 +477,13 @@ def main(): back_forth_task(model, data, ee_id) control_cmd['back_forth'] = False - # 手动控制（基础/精准） + # 手动控制 else: manual_control(model, data, ee_id) mujoco.mj_step(model, data) viewer.render() - time.sleep(0.005) + time.sleep(0.006) # 稍慢帧率，更稳定 except Exception as e: print(f"\n❌ 运行出错: {e}") @@ -497,7 +492,7 @@ def main(): finally: with suppress(Exception): viewer.close() - print("\n🔚 多功能机械臂程序退出（未修改robot.xml）") + print("\n🔚 机械臂程序退出（已修复转圈问题）") if __name__ == "__main__": From c78d7af109292523a0348c0b8c4e02a10b7ddef1 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Fri, 26 Dec 2025 15:57:50 +0800 Subject: [PATCH 23/26] =?UTF-8?q?=E7=A7=BB=E9=99=A4=E6=89=8B=E5=8A=A8?= =?UTF-8?q?=E6=8C=89=E9=94=AE=E4=BE=9D=E8=B5=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 514 +++++++++++----------------- 1 file changed, 204 insertions(+), 310 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 614e03d9bb..d78eff87e9 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -21,101 +21,54 @@ MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 操作模式枚举 ===================== -class ControlMode(Enum): - MANUAL = 1 # 基础手动控制 - PRECISE = 2 # 精准微调模式 - AUTO_SIMPLE = 3 # 简易自动抓取 - AUTO_COMPLEX = 4 # 复杂任务流程 - CIRCLE_TASK = 5 # 画圆任务 - BACK_FORTH = 6 # 往复运动 - - -# ===================== 核心参数（修复转圈关键） ===================== -# 控制参数（降低增益，杜绝转圈） -MANUAL_SPEED = 0.015 # 进一步降低速度，减少误差累积 -PRECISE_SPEED = 0.008 # 精准模式速度 +# ===================== 自动任务枚举（按执行顺序） ===================== +class AutoTask(Enum): + INIT_MOVE = 1 # 初始精准移动（热身） + SIMPLE_GRASP = 2 # 简易自动抓取 + COMPLEX_TASK = 3 # 复杂多位置任务 + CIRCLE_TASK = 4 # 画圆任务 + BACK_FORTH = 5 # 往复运动 + FINISH = 6 # 任务完成 + + +# ===================== 核心参数（自动运行适配） ===================== +# 控制参数（无转圈） +MANUAL_SPEED = 0.015 +PRECISE_SPEED = 0.008 GRASP_FORCE = 3.8 AUTO_LIFT_HEIGHT = 0.10 AUTO_TRANSPORT_X = -0.12 -# 逆运动学参数（核心：限制关节范围，杜绝转圈） -IK_GAIN = 1.5 # 逆运动学增益（大幅降低） +# 逆运动学参数 +IK_GAIN = 1.5 JOINT_LIMITS = np.array([ [-1.2, 1.2], # joint1范围 [-1.0, 1.0], # joint2范围 [-0.8, 0.8] # joint3范围 ]) -# 任务参数 -CIRCLE_RADIUS = 0.08 # 缩小画圆半径，避免超出关节范围 +# 自动任务参数 +CIRCLE_RADIUS = 0.08 CIRCLE_SPEED = 0.004 BACK_FORTH_DIST = 0.15 - -# ===================== 全局变量 ===================== -control_cmd = { - 'forward': 0, 'backward': 0, 'left': 0, 'right': 0, - 'up': 0, 'down': 0, 'grasp': 0, 'release': 0, - 'auto_simple': False, 'auto_complex': False, - 'circle_task': False, 'back_forth': False, - 'switch_precise': False, 'reset': False -} -current_mode = ControlMode.MANUAL -task_step = 0 -# 新增：目标位置缓存（避免突变） -target_ee_pos = np.array([0.0, 0.0, 0.1]) # 初始末端目标位置 - - -# ===================== 兼容版按键检测 ===================== -def check_keyboard_input(viewer): - global current_mode - # 重置基础指令 - for key in control_cmd.keys(): - if key not in ['auto_simple', 'auto_complex', 'circle_task', 'back_forth', 'switch_precise', 'reset']: - control_cmd[key] = 0 - - if hasattr(viewer, 'window') and viewer.window is not None: - window = viewer.window - # 基础移动按键 - control_cmd['forward'] = 1 if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS else 0 - control_cmd['backward'] = 1 if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS else 0 - control_cmd['left'] = 1 if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS else 0 - control_cmd['right'] = 1 if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS else 0 - control_cmd['up'] = 1 if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS else 0 - control_cmd['down'] = 1 if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS else 0 - # 抓取/释放 - control_cmd['grasp'] = 1 if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS else 0 - control_cmd['release'] = 1 if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS else 0 - # 多模式任务按键 - control_cmd['auto_simple'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False - control_cmd['auto_complex'] = True if glfw.get_key(window, glfw.KEY_X) == glfw.PRESS else False - control_cmd['circle_task'] = True if glfw.get_key(window, glfw.KEY_V) == glfw.PRESS else False - control_cmd['back_forth'] = True if glfw.get_key(window, glfw.KEY_B) == glfw.PRESS else False - control_cmd['switch_precise'] = True if glfw.get_key(window, glfw.KEY_P) == glfw.PRESS else False - control_cmd['reset'] = True if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS else False - # ESC退出 - if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: - glfw.set_window_should_close(window, True) - - # 切换精准模式 - if control_cmd['switch_precise']: - current_mode = ControlMode.PRECISE if current_mode != ControlMode.PRECISE else ControlMode.MANUAL - mode_name = "精准微调" if current_mode == ControlMode.PRECISE else "基础手动" - print( - f"\n🔄 切换到【{mode_name}】模式（速度：{PRECISE_SPEED if current_mode == ControlMode.PRECISE else MANUAL_SPEED}）") - control_cmd['switch_precise'] = False - else: - print("\n⚠️ 旧版mujoco-viewer，支持：Z(简易自动)、X(复杂任务)、C(重置)") - control_cmd['auto_simple'] = True - - -# ===================== 核心修复：逆运动学控制（杜绝转圈） ===================== +# 自动运行参数（新增） +TASK_DELAY = 2.0 # 任务间等待时间（秒） +AUTO_MOVE_POINTS = [ # 初始自动移动的目标点 + np.array([0.1, 0.0, 0.1]), + np.array([0.1, 0.05, 0.12]), + np.array([0.05, -0.05, 0.08]), + np.array([0.0, 0.0, 0.1]) +] + +# ===================== 全局变量（自动运行核心） ===================== +current_task = AutoTask.INIT_MOVE # 当前执行的自动任务 +task_step = 0 # 任务内部步数 +target_ee_pos = np.array([0.0, 0.0, 0.1]) # 末端目标位置 +init_move_idx = 0 # 初始移动的目标点索引 +task_finished = False # 所有任务是否完成 + + +# ===================== 核心逆运动学控制（无转圈） ===================== def ik_control(model, data, ee_id, target_pos): - """ - 逆运动学控制：让机械臂末端精准跟随目标位置，杜绝转圈 - :param model: MuJoCo模型 - :param data: MuJoCo数据 - :param ee_id: 末端ID - :param target_pos: 末端目标位置 - """ + """逆运动学控制：精准跟随目标位置，杜绝转圈""" # 1. 获取当前末端位置 current_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: @@ -124,73 +77,67 @@ def ik_control(model, data, ee_id, target_pos): except: current_pos = data.xpos[ee_id].copy() - # 2. 计算位置误差（限制误差范围，避免过大） + # 2. 计算位置误差（限制误差范围） error = target_pos - current_pos - error = np.clip(error, -0.05, 0.05) # 单次最大误差不超过0.05 + error = np.clip(error, -0.05, 0.05) - # 3. 计算关节雅可比矩阵（核心：关联末端位置和关节角度） - jacp = np.zeros((3, model.nv)) # 位置雅可比 - jacr = np.zeros((3, model.nv)) # 旋转雅可比 + # 3. 计算关节雅可比矩阵 + jacp = np.zeros((3, model.nv)) + jacr = np.zeros((3, model.nv)) if ee_id >= 0: mujoco.mj_jac(model, data, jacp, jacr, current_pos, ee_id) - # 4. 提取前3个关节的雅可比（机械臂主关节） + # 4. 提取前3个关节的雅可比 jacp_joints = jacp[:, :3] - # 5. 计算关节速度指令（伪逆求解，避免无解转圈） + # 5. 计算关节速度指令（伪逆求解） jnt_vel = np.dot(jacp_joints.T, error * IK_GAIN) - jnt_vel = np.clip(jnt_vel, -0.2, 0.2) # 限制关节速度 + jnt_vel = np.clip(jnt_vel, -0.2, 0.2) - # 6. 积分得到关节角度，并限制关节范围（核心：杜绝转圈） + # 6. 积分得到关节角度，并限制范围 for i in range(min(3, model.njnt)): - # 积分更新关节角度 data.qpos[i] += jnt_vel[i] * model.opt.timestep - # 限制关节在安全范围（彻底杜绝转圈） data.qpos[i] = np.clip(data.qpos[i], JOINT_LIMITS[i][0], JOINT_LIMITS[i][1]) - # 7. 更新关节数据，应用限制 + # 7. 更新关节数据 mujoco.mj_forward(model, data) -def manual_control(model, data, ee_id): - """手动控制（基于逆运动学，杜绝转圈）""" - global target_ee_pos - # 选择速度（基础/精准） - speed = PRECISE_SPEED if current_mode == ControlMode.PRECISE else MANUAL_SPEED +# ===================== 自动任务实现（按顺序执行） ===================== +def auto_init_move(model, data, ee_id): + """自动任务1：初始精准移动（热身）""" + global task_step, init_move_idx, current_task, target_ee_pos + # 到达当前目标点后，切换下一个目标点 + if task_step == 0: + print(f"\n🎯 开始初始自动移动：目标点 {init_move_idx + 1}/{len(AUTO_MOVE_POINTS)}") + target_ee_pos = AUTO_MOVE_POINTS[init_move_idx] - # 1. 更新目标位置（渐进式，避免突变） - target_ee_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * speed - target_ee_pos[1] += (control_cmd['left'] - control_cmd['right']) * speed - target_ee_pos[2] += (control_cmd['up'] - control_cmd['down']) * speed - - # 2. 限制目标位置在安全范围（避免超出关节可达范围） - target_ee_pos = np.clip(target_ee_pos, - np.array([-0.2, -0.15, 0.05]), - np.array([0.3, 0.15, 0.2])) - - # 3. 逆运动学控制（核心：让末端精准跟随目标，不转圈） + # 逆运动学控制移动到目标点 ik_control(model, data, ee_id, target_ee_pos) - # 4. 渐进抓取/释放 - if control_cmd['grasp']: - if model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.1, GRASP_FORCE) - if model.nu >= 5: - data.ctrl[4] = max(data.ctrl[4] - 0.1, -GRASP_FORCE) - elif control_cmd['release']: - if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.1, 0.0) - if model.nu >= 5: - data.ctrl[4] = min(data.ctrl[4] + 0.1, 0.0) + # 检查是否到达目标点（误差小于0.005） + current_pos = np.array([0.0, 0.0, 0.1]) + if ee_id >= 0: + try: + current_pos = data.site_xpos[ee_id].copy() + except: + current_pos = data.xpos[ee_id].copy() + error = np.linalg.norm(target_ee_pos - current_pos) + + if error < 0.005: + task_step = 0 + init_move_idx += 1 + if init_move_idx >= len(AUTO_MOVE_POINTS): + print("✅ 初始自动移动完成！") + time.sleep(TASK_DELAY) # 任务间等待 + current_task = AutoTask.SIMPLE_GRASP # 切换到下一个任务 + else: + task_step += 1 -# ===================== 自动任务（适配逆运动学，杜绝转圈） ===================== def auto_simple_grasp(model, data, ee_id, obj_id): - """简易自动抓取（基于逆运动学）""" - global target_ee_pos - print("🔄 开始【简易自动抓取】任务...") - # 重置目标位置 - target_ee_pos = np.array([0.0, 0.0, 0.1]) + """自动任务2：简易抓取（无需按键）""" + global task_step, current_task, target_ee_pos # 获取物体位置 obj_pos = np.array([0.2, 0.0, 0.05]) if obj_id >= 0: @@ -199,215 +146,174 @@ def auto_simple_grasp(model, data, ee_id, obj_id): except: pass - # 阶段1：移动到物体上方（安全位置） - step = 0 - while step < 1000 and viewer.is_alive: - target = obj_pos + [0, 0, 0.07] # 降低高度，避免超出范围 + # 阶段1：移动到物体上方 + if task_step < 1000: + if task_step == 0: + print("\n🎯 开始自动简易抓取任务...") + target = obj_pos + [0, 0, 0.07] ik_control(model, data, ee_id, target) - # 渐进闭合夹爪（提前准备） - if step > 800 and model.nu >= 4: + # 渐进闭合夹爪 + if task_step > 800 and model.nu >= 4: data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - # 阶段2：下降抓取 - step = 0 - while step < 800 and viewer.is_alive: - target = obj_pos + [0, 0, 0.02] # 贴近物体但不碰撞 + elif task_step < 1800: + target = obj_pos + [0, 0, 0.02] ik_control(model, data, ee_id, target) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - # 阶段3：抬升 - step = 0 - while step < 800 and viewer.is_alive: + elif task_step < 2600: target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] ik_control(model, data, ee_id, target) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - # 阶段4：搬运 - step = 0 - while step < 1000 and viewer.is_alive: + elif task_step < 3600: target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] ik_control(model, data, ee_id, target) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - # 阶段5：下放释放 - step = 0 - while step < 800 and viewer.is_alive: + elif task_step < 4400: target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] ik_control(model, data, ee_id, target) # 渐进释放 - if step > 400: + if task_step > 4000: if model.nu >= 4: data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) if model.nu >= 5: data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - # 阶段6：归位 - step = 0 - while step < 1000 and viewer.is_alive: + elif task_step < 5400: target = np.array([0.0, 0.0, 0.12]) ik_control(model, data, ee_id, target) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - - print("🎉 【简易自动抓取】任务完成！（无转圈）") + # 任务完成 + else: + print("✅ 自动简易抓取任务完成！") + task_step = 0 + time.sleep(TASK_DELAY) + current_task = AutoTask.COMPLEX_TASK # 切换到复杂任务 def auto_complex_task(model, data, ee_id, obj_id): - """复杂任务流程（多位置，无转圈）""" - global target_ee_pos - print("🔄 开始【复杂任务】：多位置抓取+放置...") - target_ee_pos = np.array([0.0, 0.0, 0.1]) - # 定义安全的目标位置（避免超出关节范围） + """自动任务3：复杂多位置抓取+放置""" + global task_step, current_task + # 定义安全的目标位置 target_positions = [ - np.array([0.18, 0.0, 0.05]), # 初始物体位置 - np.array([-0.10, 0.08, 0.05]), # 第一个放置点 - np.array([-0.10, -0.08, 0.05]), # 第二个放置点 - np.array([0.18, 0.0, 0.05]) # 回到初始位置 + np.array([0.18, 0.0, 0.05]), + np.array([-0.10, 0.08, 0.05]), + np.array([-0.10, -0.08, 0.05]), + np.array([0.18, 0.0, 0.05]) ] - - for idx, target in enumerate(target_positions): - if not viewer.is_alive: - break - print(f"📌 复杂任务阶段 {idx + 1}/{len(target_positions)}：移动到 {target[:2]} 位置") - - # 阶段1：移动到目标上方 - step = 0 - while step < 900 and viewer.is_alive: - target_above = target + [0, 0, 0.06] - ik_control(model, data, ee_id, target_above) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - - # 阶段2：下降（抓取/释放） - step = 0 - while step < 700 and viewer.is_alive: - ik_control(model, data, ee_id, target + [0, 0, 0.02]) + stage = task_step // 2300 # 每个阶段2300步 + + if stage < len(target_positions): + if task_step % 2300 == 0: + print(f"\n🎯 复杂任务阶段 {stage + 1}/{len(target_positions)}：移动到 {target_positions[stage][:2]}") + sub_step = task_step % 2300 + + # 阶段1：移动到目标上方（0-900步） + if sub_step < 900: + target = target_positions[stage] + [0, 0, 0.06] + ik_control(model, data, ee_id, target) + # 阶段2：下降（抓取/释放）（900-1600步） + elif sub_step < 1600: + target = target_positions[stage] + [0, 0, 0.02] + ik_control(model, data, ee_id, target) # 第一阶段抓取，其他阶段释放 - if idx == 0: # 抓取 + if stage == 0: if model.nu >= 4: data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) - if model.nu >= 5: data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) - elif idx in [1, 2]: # 释放 + elif stage in [1, 2]: if model.nu >= 4: data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) - if model.nu >= 5: data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - - # 阶段3：抬升 - step = 0 - while step < 700 and viewer.is_alive: - ik_control(model, data, ee_id, target + [0, 0, AUTO_LIFT_HEIGHT]) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - - # 归位 - step = 0 - while step < 900 and viewer.is_alive: - ik_control(model, data, ee_id, np.array([0.0, 0.0, 0.12])) - mujoco.mj_step(model, data) - viewer.render() - step += 1 - - print("🎉 【复杂任务】全流程完成！（无转圈）") - - -def circle_task(model, data, ee_id): - """画圆任务（限制范围，无转圈）""" - global task_step - print("🔄 开始【画圆任务】：末端画圆（无转圈）") - center = np.array([0.08, 0.0, 0.10]) # 缩小圆心范围 - - while viewer.is_alive and task_step < 2000: - # 计算圆上的目标点（限制在关节可达范围） + # 阶段3：抬升（1600-2300步） + else: + target = target_positions[stage] + [0, 0, AUTO_LIFT_HEIGHT] + ik_control(model, data, ee_id, target) + else: + # 归位（额外1000步） + if task_step < 5600: + target = np.array([0.0, 0.0, 0.12]) + ik_control(model, data, ee_id, target) + else: + print("✅ 自动复杂任务完成！") + task_step = 0 + time.sleep(TASK_DELAY) + current_task = AutoTask.CIRCLE_TASK # 切换到画圆任务 + + task_step += 1 + + +def auto_circle_task(model, data, ee_id): + """自动任务4：画圆任务""" + global task_step, current_task + center = np.array([0.08, 0.0, 0.10]) + + if task_step < 2000: + # 计算圆上目标点 angle = task_step * CIRCLE_SPEED target_x = center[0] + CIRCLE_RADIUS * np.cos(angle) target_y = center[1] + CIRCLE_RADIUS * np.sin(angle) target_pos = np.array([target_x, target_y, center[2]]) - # 限制目标位置 + # 限制范围 target_pos = np.clip(target_pos, np.array([-0.1, -0.1, 0.08]), np.array([0.2, 0.1, 0.15])) - # 逆运动学控制画圆 ik_control(model, data, ee_id, target_pos) - # 实时反馈 - if task_step % 200 == 0: - print(f"📈 画圆进度：{int(task_step / 2000 * 100)}%（角度：{int(angle * 180 / np.pi)}°）") - - mujoco.mj_step(model, data) - viewer.render() - task_step += 1 + if task_step % 400 == 0: + print(f"\n📈 自动画圆进度：{int(task_step / 2000 * 100)}%") + else: + print("✅ 自动画圆任务完成！") + task_step = 0 + time.sleep(TASK_DELAY) + current_task = AutoTask.BACK_FORTH # 切换到往复运动 - task_step = 0 - print("🎉 【画圆任务】完成！（无转圈）") + task_step += 1 -def back_forth_task(model, data, ee_id): - """往复运动任务（无转圈）""" - global task_step - print("🔄 开始【往复运动任务】：前后往复（无转圈）") +def auto_back_forth(model, data, ee_id): + """自动任务5：往复运动""" + global task_step, current_task, task_finished start_pos = np.array([0.05, 0.0, 0.10]) - while viewer.is_alive and task_step < 2500: - # 生成往复轨迹（限制范围） + if task_step < 2500: + # 生成往复轨迹 cycle = np.sin(task_step * 0.008) target_x = start_pos[0] + cycle * BACK_FORTH_DIST - # 限制X轴范围，避免超出关节 target_x = np.clip(target_x, -0.1, 0.2) target_pos = np.array([target_x, start_pos[1], start_pos[2]]) - # 逆运动学控制往复 ik_control(model, data, ee_id, target_pos) - # 实时反馈 - if task_step % 300 == 0: + if task_step % 600 == 0: direction = "前" if cycle > 0 else "后" - print(f"📌 往复运动：当前方向【{direction}】（位置X：{target_x:.2f}）") - - mujoco.mj_step(model, data) - viewer.render() - task_step += 1 + print(f"\n📌 自动往复运动：当前方向【{direction}】（X：{target_x:.2f}）") + else: + print("✅ 自动往复运动任务完成！") + task_step = 0 + time.sleep(TASK_DELAY) + current_task = AutoTask.FINISH # 所有任务完成 + task_finished = True - task_step = 0 - print("🎉 【往复运动任务】完成！（无转圈）") + task_step += 1 -# ===================== 初始化+主程序 ===================== +# ===================== 初始化+主程序（自动运行核心） ===================== def init_model_and_viewer(): + """初始化模型和Viewer，自动运行准备""" if not os.path.exists(MODEL_PATH): raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - # 初始化关节位置（重置到中间位置，避免初始转圈） + # 初始化关节到中间位置 for i in range(min(3, model.njnt)): data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 mujoco.mj_forward(model, data) viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) viewer.cam.distance = 1.8 - viewer.cam.elevation = 15 # 调整视角，更清楚看关节 + viewer.cam.elevation = 15 viewer.cam.azimuth = 60 viewer.cam.lookat = [0.1, 0.0, 0.1] @@ -428,62 +334,48 @@ def init_model_and_viewer(): obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) if obj_id >= 0: break - # 打印修复后的操作指南 + # 打印自动运行提示 print("=" * 60) - print("✅ 机械臂控制程序（修复转圈问题）初始化完成！") - print("🔧 核心修复：逆运动学控制+关节范围限制，彻底杜绝转圈") - print("🎮 操作指南：") - print(" W/S/A/D/Q/E：移动（精准不转圈） 空格：抓取 R：释放 P：精准模式") - print(" Z：简易抓取 X：复杂任务 V：画圆 B：往复运动") - print(" C：重置 ESC：退出") + print("🚀 机械臂自动运行程序启动！") + print("🔧 自动执行流程：初始移动→简易抓取→复杂任务→画圆→往复运动") + print("⏱ 任务间等待时间：{}秒".format(TASK_DELAY)) + print("💡 按ESC可随时退出程序") print("=" * 60) return model, data, viewer, ee_id, obj_id def main(): - global viewer, task_step, current_mode, target_ee_pos - task_step = 0 - current_mode = ControlMode.MANUAL - target_ee_pos = np.array([0.0, 0.0, 0.1]) + global viewer, current_task, task_step, task_finished model, data, viewer, ee_id, obj_id = init_model_and_viewer() try: - while viewer.is_alive: - check_keyboard_input(viewer) - - # 重置功能（恢复初始关节位置） - if control_cmd['reset']: - # 重置关节到中间位置 - for i in range(min(3, model.njnt)): - data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 - mujoco.mj_forward(model, data) - target_ee_pos = np.array([0.0, 0.0, 0.1]) - task_step = 0 - current_mode = ControlMode.MANUAL - print("\n🔄 模型重置完成：关节回到中间位置，彻底杜绝初始转圈") - control_cmd['reset'] = False - - # 执行自动任务 - elif control_cmd['auto_simple']: + while viewer.is_alive and not task_finished: + # 根据当前任务执行对应逻辑（自动运行核心） + if current_task == AutoTask.INIT_MOVE: + auto_init_move(model, data, ee_id) + elif current_task == AutoTask.SIMPLE_GRASP: auto_simple_grasp(model, data, ee_id, obj_id) - control_cmd['auto_simple'] = False - elif control_cmd['auto_complex']: + elif current_task == AutoTask.COMPLEX_TASK: auto_complex_task(model, data, ee_id, obj_id) - control_cmd['auto_complex'] = False - elif control_cmd['circle_task']: - circle_task(model, data, ee_id) - control_cmd['circle_task'] = False - elif control_cmd['back_forth']: - back_forth_task(model, data, ee_id) - control_cmd['back_forth'] = False - - # 手动控制 - else: - manual_control(model, data, ee_id) - + elif current_task == AutoTask.CIRCLE_TASK: + auto_circle_task(model, data, ee_id) + elif current_task == AutoTask.BACK_FORTH: + auto_back_forth(model, data, ee_id) + elif current_task == AutoTask.FINISH: + print("\n🎉 所有自动任务执行完成！") + task_finished = True + + # 仿真步进 mujoco.mj_step(model, data) viewer.render() - time.sleep(0.006) # 稍慢帧率，更稳定 + time.sleep(0.006) + + # 所有任务完成后，保持窗口5秒再退出 + if task_finished: + print("\n⏳ 所有任务完成，5秒后自动退出...") + for i in range(5): + viewer.render() + time.sleep(1) except Exception as e: print(f"\n❌ 运行出错: {e}") @@ -492,14 +384,16 @@ def main(): finally: with suppress(Exception): viewer.close() - print("\n🔚 机械臂程序退出（已修复转圈问题）") + print("\n🔚 机械臂自动运行程序退出") if __name__ == "__main__": + # 检查依赖 try: import mujoco, mujoco_viewer, glfw except ImportError as e: print(f"❌ 缺少依赖 {str(e).split()[-1]}！执行：") print(" pip install mujoco mujoco-viewer glfw numpy matplotlib") exit(1) + # 启动自动运行 main() \ No newline at end of file From 2a1ba15e567075a400125a91d1ef21bf22ccd156 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sat, 27 Dec 2025 10:50:07 +0800 Subject: [PATCH 24/26] =?UTF-8?q?=E5=89=94=E9=99=A4glfw=E4=BE=9D=E8=B5=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 439 +++++++--------------------- 1 file changed, 102 insertions(+), 337 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index d78eff87e9..86c3ba01c3 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -1,399 +1,164 @@ import mujoco import mujoco_viewer import numpy as np -import matplotlib.pyplot as plt -import matplotlib as mpl import os import warnings import time -import glfw from contextlib import suppress -from enum import Enum -# ===================== 基础配置 ===================== +# ===================== 极简配置（剔除冗余，确保自动运行） ===================== warnings.filterwarnings('ignore') -mpl.use('TkAgg') -mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] -mpl.rcParams['axes.unicode_minus'] = False - -# 路径配置 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") - -# ===================== 自动任务枚举（按执行顺序） ===================== -class AutoTask(Enum): - INIT_MOVE = 1 # 初始精准移动（热身） - SIMPLE_GRASP = 2 # 简易自动抓取 - COMPLEX_TASK = 3 # 复杂多位置任务 - CIRCLE_TASK = 4 # 画圆任务 - BACK_FORTH = 5 # 往复运动 - FINISH = 6 # 任务完成 - - -# ===================== 核心参数（自动运行适配） ===================== -# 控制参数（无转圈） -MANUAL_SPEED = 0.015 -PRECISE_SPEED = 0.008 +# 核心参数（极简+强制） GRASP_FORCE = 3.8 -AUTO_LIFT_HEIGHT = 0.10 -AUTO_TRANSPORT_X = -0.12 -# 逆运动学参数 -IK_GAIN = 1.5 -JOINT_LIMITS = np.array([ - [-1.2, 1.2], # joint1范围 - [-1.0, 1.0], # joint2范围 - [-0.8, 0.8] # joint3范围 -]) -# 自动任务参数 -CIRCLE_RADIUS = 0.08 -CIRCLE_SPEED = 0.004 -BACK_FORTH_DIST = 0.15 -# 自动运行参数（新增） -TASK_DELAY = 2.0 # 任务间等待时间（秒） -AUTO_MOVE_POINTS = [ # 初始自动移动的目标点 - np.array([0.1, 0.0, 0.1]), - np.array([0.1, 0.05, 0.12]), - np.array([0.05, -0.05, 0.08]), - np.array([0.0, 0.0, 0.1]) +IK_GAIN = 1.0 # 极低增益，确保稳定 +JOINT_LIMITS = np.array([[-1.5, 1.5], [-1.2, 1.2], [-1.0, 1.0]]) +# 自动任务参数（极简流程） +AUTO_TARGETS = [ + np.array([0.2, 0.0, 0.08]), # 物体位置 + np.array([-0.1, 0.0, 0.08]), # 放置位置 + np.array([0.0, 0.0, 0.1]) # 归位位置 ] +STEP_PER_TARGET = 800 # 每个目标点执行步数（缩短，快速看到效果） -# ===================== 全局变量（自动运行核心） ===================== -current_task = AutoTask.INIT_MOVE # 当前执行的自动任务 -task_step = 0 # 任务内部步数 -target_ee_pos = np.array([0.0, 0.0, 0.1]) # 末端目标位置 -init_move_idx = 0 # 初始移动的目标点索引 -task_finished = False # 所有任务是否完成 +# ===================== 全局变量（极简自动运行） ===================== +current_target_idx = 0 # 当前目标点索引 +task_step = 0 # 当前目标点内步数 +grasp_state = False # 抓取状态 +viewer = None # 全局viewer，确保可访问 -# ===================== 核心逆运动学控制（无转圈） ===================== -def ik_control(model, data, ee_id, target_pos): - """逆运动学控制：精准跟随目标位置，杜绝转圈""" - # 1. 获取当前末端位置 - current_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - current_pos = data.site_xpos[ee_id].copy() - except: - current_pos = data.xpos[ee_id].copy() +# ===================== 核心逆运动学控制（极简版） ===================== +def simple_ik_control(model, data, ee_id, target_pos): + """极简逆运动学：只保留核心，确保不转圈+快速响应""" + # 获取当前末端位置 + current_pos = data.site_xpos[ee_id] if ee_id >= 0 else np.array([0.0, 0.0, 0.1]) - # 2. 计算位置误差（限制误差范围） + # 计算误差并限制 error = target_pos - current_pos - error = np.clip(error, -0.05, 0.05) - - # 3. 计算关节雅可比矩阵 - jacp = np.zeros((3, model.nv)) - jacr = np.zeros((3, model.nv)) - if ee_id >= 0: - mujoco.mj_jac(model, data, jacp, jacr, current_pos, ee_id) + error = np.clip(error, -0.03, 0.03) - # 4. 提取前3个关节的雅可比 - jacp_joints = jacp[:, :3] - - # 5. 计算关节速度指令（伪逆求解） - jnt_vel = np.dot(jacp_joints.T, error * IK_GAIN) - jnt_vel = np.clip(jnt_vel, -0.2, 0.2) - - # 6. 积分得到关节角度，并限制范围 + # 简易关节控制（直接映射，快速生效） for i in range(min(3, model.njnt)): - data.qpos[i] += jnt_vel[i] * model.opt.timestep + # 直接更新关节角度（限制范围） + data.qpos[i] += error[i] * IK_GAIN * model.opt.timestep data.qpos[i] = np.clip(data.qpos[i], JOINT_LIMITS[i][0], JOINT_LIMITS[i][1]) - # 7. 更新关节数据 mujoco.mj_forward(model, data) -# ===================== 自动任务实现（按顺序执行） ===================== -def auto_init_move(model, data, ee_id): - """自动任务1：初始精准移动（热身）""" - global task_step, init_move_idx, current_task, target_ee_pos - # 到达当前目标点后，切换下一个目标点 - if task_step == 0: - print(f"\n🎯 开始初始自动移动：目标点 {init_move_idx + 1}/{len(AUTO_MOVE_POINTS)}") - target_ee_pos = AUTO_MOVE_POINTS[init_move_idx] - - # 逆运动学控制移动到目标点 - ik_control(model, data, ee_id, target_ee_pos) - - # 检查是否到达目标点（误差小于0.005） - current_pos = np.array([0.0, 0.0, 0.1]) - if ee_id >= 0: - try: - current_pos = data.site_xpos[ee_id].copy() - except: - current_pos = data.xpos[ee_id].copy() - error = np.linalg.norm(target_ee_pos - current_pos) - - if error < 0.005: - task_step = 0 - init_move_idx += 1 - if init_move_idx >= len(AUTO_MOVE_POINTS): - print("✅ 初始自动移动完成！") - time.sleep(TASK_DELAY) # 任务间等待 - current_task = AutoTask.SIMPLE_GRASP # 切换到下一个任务 - else: - task_step += 1 - - -def auto_simple_grasp(model, data, ee_id, obj_id): - """自动任务2：简易抓取（无需按键）""" - global task_step, current_task, target_ee_pos - # 获取物体位置 - obj_pos = np.array([0.2, 0.0, 0.05]) - if obj_id >= 0: - try: - obj_pos = data.xpos[obj_id].copy() - except: - pass - - # 阶段1：移动到物体上方 - if task_step < 1000: - if task_step == 0: - print("\n🎯 开始自动简易抓取任务...") - target = obj_pos + [0, 0, 0.07] - ik_control(model, data, ee_id, target) - # 渐进闭合夹爪 - if task_step > 800 and model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) - data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) - # 阶段2：下降抓取 - elif task_step < 1800: - target = obj_pos + [0, 0, 0.02] - ik_control(model, data, ee_id, target) - # 阶段3：抬升 - elif task_step < 2600: - target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] - ik_control(model, data, ee_id, target) - # 阶段4：搬运 - elif task_step < 3600: - target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] - ik_control(model, data, ee_id, target) - # 阶段5：下放释放 - elif task_step < 4400: - target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] - ik_control(model, data, ee_id, target) - # 渐进释放 - if task_step > 4000: - if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) - if model.nu >= 5: - data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) - # 阶段6：归位 - elif task_step < 5400: - target = np.array([0.0, 0.0, 0.12]) - ik_control(model, data, ee_id, target) - # 任务完成 - else: - print("✅ 自动简易抓取任务完成！") - task_step = 0 - time.sleep(TASK_DELAY) - current_task = AutoTask.COMPLEX_TASK # 切换到复杂任务 - - -def auto_complex_task(model, data, ee_id, obj_id): - """自动任务3：复杂多位置抓取+放置""" - global task_step, current_task - # 定义安全的目标位置 - target_positions = [ - np.array([0.18, 0.0, 0.05]), - np.array([-0.10, 0.08, 0.05]), - np.array([-0.10, -0.08, 0.05]), - np.array([0.18, 0.0, 0.05]) - ] - stage = task_step // 2300 # 每个阶段2300步 - - if stage < len(target_positions): - if task_step % 2300 == 0: - print(f"\n🎯 复杂任务阶段 {stage + 1}/{len(target_positions)}：移动到 {target_positions[stage][:2]}") - sub_step = task_step % 2300 - - # 阶段1：移动到目标上方（0-900步） - if sub_step < 900: - target = target_positions[stage] + [0, 0, 0.06] - ik_control(model, data, ee_id, target) - # 阶段2：下降（抓取/释放）（900-1600步） - elif sub_step < 1600: - target = target_positions[stage] + [0, 0, 0.02] - ik_control(model, data, ee_id, target) - # 第一阶段抓取，其他阶段释放 - if stage == 0: - if model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.03, GRASP_FORCE) - data.ctrl[4] = max(data.ctrl[4] - 0.03, -GRASP_FORCE) - elif stage in [1, 2]: - if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.03, 0.0) - data.ctrl[4] = min(data.ctrl[4] + 0.03, 0.0) - # 阶段3：抬升（1600-2300步） - else: - target = target_positions[stage] + [0, 0, AUTO_LIFT_HEIGHT] - ik_control(model, data, ee_id, target) - else: - # 归位（额外1000步） - if task_step < 5600: - target = np.array([0.0, 0.0, 0.12]) - ik_control(model, data, ee_id, target) - else: - print("✅ 自动复杂任务完成！") - task_step = 0 - time.sleep(TASK_DELAY) - current_task = AutoTask.CIRCLE_TASK # 切换到画圆任务 - +# ===================== 强制自动运行逻辑（核心） ===================== +def run_auto_task(model, data, ee_id, obj_id): + """强制自动运行：启动即执行，无复杂判断""" + global current_target_idx, task_step, grasp_state + + # 1. 执行当前目标点的控制 + target = AUTO_TARGETS[current_target_idx] + simple_ik_control(model, data, ee_id, target) + + # 2. 抓取/释放逻辑（极简） + if current_target_idx == 0 and task_step > STEP_PER_TARGET * 0.7: + # 到达物体位置，闭合夹爪 + if model.nu >= 4: + data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) + data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + grasp_state = True + elif current_target_idx == 1 and task_step > STEP_PER_TARGET * 0.7: + # 到达放置位置，释放夹爪 + if model.nu >= 4: + data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) + data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) + grasp_state = False + + # 3. 切换目标点（步数到即切换） task_step += 1 - - -def auto_circle_task(model, data, ee_id): - """自动任务4：画圆任务""" - global task_step, current_task - center = np.array([0.08, 0.0, 0.10]) - - if task_step < 2000: - # 计算圆上目标点 - angle = task_step * CIRCLE_SPEED - target_x = center[0] + CIRCLE_RADIUS * np.cos(angle) - target_y = center[1] + CIRCLE_RADIUS * np.sin(angle) - target_pos = np.array([target_x, target_y, center[2]]) - # 限制范围 - target_pos = np.clip(target_pos, - np.array([-0.1, -0.1, 0.08]), - np.array([0.2, 0.1, 0.15])) - # 逆运动学控制画圆 - ik_control(model, data, ee_id, target_pos) - # 实时反馈 - if task_step % 400 == 0: - print(f"\n📈 自动画圆进度：{int(task_step / 2000 * 100)}%") - else: - print("✅ 自动画圆任务完成！") + if task_step >= STEP_PER_TARGET: + print(f"✅ 完成目标点 {current_target_idx + 1}/{len(AUTO_TARGETS)}") task_step = 0 - time.sleep(TASK_DELAY) - current_task = AutoTask.BACK_FORTH # 切换到往复运动 - - task_step += 1 + current_target_idx += 1 + # 所有目标点完成，退出 + if current_target_idx >= len(AUTO_TARGETS): + print("\n🎉 所有自动任务强制完成！") + return False # 任务完成，返回False + return True # 任务继续 -def auto_back_forth(model, data, ee_id): - """自动任务5：往复运动""" - global task_step, current_task, task_finished - start_pos = np.array([0.05, 0.0, 0.10]) - if task_step < 2500: - # 生成往复轨迹 - cycle = np.sin(task_step * 0.008) - target_x = start_pos[0] + cycle * BACK_FORTH_DIST - target_x = np.clip(target_x, -0.1, 0.2) - target_pos = np.array([target_x, start_pos[1], start_pos[2]]) - # 逆运动学控制往复 - ik_control(model, data, ee_id, target_pos) - # 实时反馈 - if task_step % 600 == 0: - direction = "前" if cycle > 0 else "后" - print(f"\n📌 自动往复运动：当前方向【{direction}】（X：{target_x:.2f}）") - else: - print("✅ 自动往复运动任务完成！") - task_step = 0 - time.sleep(TASK_DELAY) - current_task = AutoTask.FINISH # 所有任务完成 - task_finished = True - - task_step += 1 - - -# ===================== 初始化+主程序（自动运行核心） ===================== -def init_model_and_viewer(): - """初始化模型和Viewer，自动运行准备""" +# ===================== 初始化+主程序（强制自动） ===================== +def init(): + """极简初始化：确保快速启动""" + global viewer + # 检查模型文件 if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") + raise FileNotFoundError(f"请确保robot.xml在当前目录：{MODEL_PATH}") + + # 加载模型 model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - # 初始化关节到中间位置 + # 初始化关节到中间位置（避免初始转圈） for i in range(min(3, model.njnt)): data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 mujoco.mj_forward(model, data) + # 初始化Viewer（强制显示） viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer.cam.distance = 1.8 - viewer.cam.elevation = 15 - viewer.cam.azimuth = 60 + viewer.cam.distance = 1.5 + viewer.cam.elevation = 20 + viewer.cam.azimuth = 70 viewer.cam.lookat = [0.1, 0.0, 0.1] - # 兼容原有模型ID - ee_id, obj_id = -1, -1 - for name in ["ee_site", "ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) - if ee_id >= 0: break - if ee_id < 0: - for name in ["ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if ee_id >= 0: break - for name in ["target_object", "object", "ball"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if obj_id >= 0: break - if obj_id < 0: - for name in ["object_geom", "ball_geom"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) - if obj_id >= 0: break + # 极简ID识别（只找关键ID） + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") - # 打印自动运行提示 - print("=" * 60) - print("🚀 机械臂自动运行程序启动！") - print("🔧 自动执行流程：初始移动→简易抓取→复杂任务→画圆→往复运动") - print("⏱ 任务间等待时间：{}秒".format(TASK_DELAY)) - print("💡 按ESC可随时退出程序") - print("=" * 60) - return model, data, viewer, ee_id, obj_id + # 打印强制启动提示 + print("=" * 50) + print("🚨 强制自动运行模式启动！") + print("📌 无需任何按键，立刻执行抓取任务") + print("🎯 目标点：物体位置→放置位置→归位") + print("=" * 50) + return model, data, ee_id, obj_id def main(): - global viewer, current_task, task_step, task_finished - model, data, viewer, ee_id, obj_id = init_model_and_viewer() - + global viewer try: - while viewer.is_alive and not task_finished: - # 根据当前任务执行对应逻辑（自动运行核心） - if current_task == AutoTask.INIT_MOVE: - auto_init_move(model, data, ee_id) - elif current_task == AutoTask.SIMPLE_GRASP: - auto_simple_grasp(model, data, ee_id, obj_id) - elif current_task == AutoTask.COMPLEX_TASK: - auto_complex_task(model, data, ee_id, obj_id) - elif current_task == AutoTask.CIRCLE_TASK: - auto_circle_task(model, data, ee_id) - elif current_task == AutoTask.BACK_FORTH: - auto_back_forth(model, data, ee_id) - elif current_task == AutoTask.FINISH: - print("\n🎉 所有自动任务执行完成！") - task_finished = True + # 初始化 + model, data, ee_id, obj_id = init() - # 仿真步进 + # 强制自动运行核心循环（无任何按键依赖） + while viewer.is_alive: + # 执行自动任务，返回False则退出 + if not run_auto_task(model, data, ee_id, obj_id): + break + + # 仿真步进（快速渲染） mujoco.mj_step(model, data) viewer.render() - time.sleep(0.006) + time.sleep(0.005) - # 所有任务完成后，保持窗口5秒再退出 - if task_finished: - print("\n⏳ 所有任务完成，5秒后自动退出...") - for i in range(5): - viewer.render() - time.sleep(1) + # 任务完成后，保持窗口3秒 + print("\n⏳ 任务完成，3秒后自动退出...") + for _ in range(3): + viewer.render() + time.sleep(1) except Exception as e: - print(f"\n❌ 运行出错: {e}") - import traceback - traceback.print_exc() + print(f"\n❌ 错误：{e}") finally: with suppress(Exception): viewer.close() - print("\n🔚 机械臂自动运行程序退出") + print("🔚 强制自动运行结束") if __name__ == "__main__": - # 检查依赖 + # 强制检查依赖并启动 try: - import mujoco, mujoco_viewer, glfw - except ImportError as e: - print(f"❌ 缺少依赖 {str(e).split()[-1]}！执行：") - print(" pip install mujoco mujoco-viewer glfw numpy matplotlib") + import mujoco, mujoco_viewer + except ImportError: + print("❌ 缺少依赖！执行：pip install mujoco mujoco-viewer numpy") exit(1) - # 启动自动运行 main() \ No newline at end of file From 80b22eb06900ab0a8f287e8dc3e94bc5be83889f Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sat, 27 Dec 2025 18:40:12 +0800 Subject: [PATCH 25/26] =?UTF-8?q?=E4=B8=B0=E5=AF=8C=E6=8A=93=E5=8F=96?= =?UTF-8?q?=E4=BB=BB=E5=8A=A1=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 274 +++++++++++++++++----------- 1 file changed, 170 insertions(+), 104 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 86c3ba01c3..7769b8570c 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -6,159 +6,225 @@ import time from contextlib import suppress -# ===================== 极简配置（剔除冗余，确保自动运行） ===================== +# ===================== 配置（已根据你的模型定制） ===================== warnings.filterwarnings('ignore') CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# 核心参数（极简+强制） -GRASP_FORCE = 3.8 -IK_GAIN = 1.0 # 极低增益，确保稳定 -JOINT_LIMITS = np.array([[-1.5, 1.5], [-1.2, 1.2], [-1.0, 1.0]]) -# 自动任务参数（极简流程） -AUTO_TARGETS = [ - np.array([0.2, 0.0, 0.08]), # 物体位置 - np.array([-0.1, 0.0, 0.08]), # 放置位置 - np.array([0.0, 0.0, 0.1]) # 归位位置 +# --- 1. 任务清单（已使用正确的物体名称 'target_object'） --- +TASK_QUEUE = [ + # 将名为 'target_object' 的物体移动到 (-0.3, 0, 0.05) + ["target_object", [-0.3, 0, 0.05]], ] -STEP_PER_TARGET = 800 # 每个目标点执行步数（缩短，快速看到效果） -# ===================== 全局变量（极简自动运行） ===================== -current_target_idx = 0 # 当前目标点索引 -task_step = 0 # 当前目标点内步数 -grasp_state = False # 抓取状态 -viewer = None # 全局viewer，确保可访问 +# --- 2. 核心控制参数 --- +IK_GAIN = 1.5 +GRASP_FORCE = -8.0 # 夹爪闭合的力（负值表示向左/右） +CLEARANCE_HEIGHT = 0.25 # 移动时的安全高度 +STEP_PER_MOVE = 1200 # 移动到一个新位置所需的步数 +STEP_PER_GRASP = 400 # 抓取/释放动作所需的步数 +# ===================== 全局状态机 ===================== +viewer = None +current_task_index = 0 +task_step = 0 + + +class TaskState: + MOVE_TO_OBJECT_ABOVE = 1 + MOVE_DOWN_TO_GRASP = 2 + GRASP_OBJECT = 3 + MOVE_UP_AFTER_GRASP = 4 + MOVE_TO_TARGET_ABOVE = 5 + MOVE_DOWN_TO_PLACE = 6 + RELEASE_OBJECT = 7 + MOVE_UP_AFTER_RELEASE = 8 + FINISHED_ALL = 9 + + +current_state = TaskState.MOVE_TO_OBJECT_ABOVE -# ===================== 核心逆运动学控制（极简版） ===================== -def simple_ik_control(model, data, ee_id, target_pos): - """极简逆运动学：只保留核心，确保不转圈+快速响应""" - # 获取当前末端位置 - current_pos = data.site_xpos[ee_id] if ee_id >= 0 else np.array([0.0, 0.0, 0.1]) - # 计算误差并限制 +# ===================== 核心功能函数 ===================== +def simple_ik_control(model, data, ee_id, target_pos): + """逆运动学控制，让末端执行器移动到目标位置""" + current_pos = data.site_xpos[ee_id] error = target_pos - current_pos - error = np.clip(error, -0.03, 0.03) - - # 简易关节控制（直接映射，快速生效） - for i in range(min(3, model.njnt)): - # 直接更新关节角度（限制范围） - data.qpos[i] += error[i] * IK_GAIN * model.opt.timestep - data.qpos[i] = np.clip(data.qpos[i], JOINT_LIMITS[i][0], JOINT_LIMITS[i][1]) - - mujoco.mj_forward(model, data) - - -# ===================== 强制自动运行逻辑（核心） ===================== -def run_auto_task(model, data, ee_id, obj_id): - """强制自动运行：启动即执行，无复杂判断""" - global current_target_idx, task_step, grasp_state - - # 1. 执行当前目标点的控制 - target = AUTO_TARGETS[current_target_idx] - simple_ik_control(model, data, ee_id, target) - - # 2. 抓取/释放逻辑（极简） - if current_target_idx == 0 and task_step > STEP_PER_TARGET * 0.7: - # 到达物体位置，闭合夹爪 - if model.nu >= 4: - data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) - data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) - grasp_state = True - elif current_target_idx == 1 and task_step > STEP_PER_TARGET * 0.7: - # 到达放置位置，释放夹爪 - if model.nu >= 4: - data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) - data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) - grasp_state = False - - # 3. 切换目标点（步数到即切换） - task_step += 1 - if task_step >= STEP_PER_TARGET: - print(f"✅ 完成目标点 {current_target_idx + 1}/{len(AUTO_TARGETS)}") - task_step = 0 - current_target_idx += 1 + error = np.clip(error, -0.05, 0.05) + + jacp = np.zeros((3, model.nv)) + mujoco.mj_jac(model, data, jacp, None, current_pos, ee_id) + jnt_vel = np.dot(jacp[:, :3].T, error * IK_GAIN) + jnt_vel = np.clip(jnt_vel, -0.5, 0.5) + + # 注意：这里控制的是关节力矩（motor），而不是直接设置角度 + for i in range(min(3, model.nu - 2)): # 减去夹爪的两个控制 + data.ctrl[i] = jnt_vel[i] * 100 # 乘以一个系数来放大控制信号 + + +def run_smart_grasp_task(model, data, ee_id): + """智能抓取任务的状态机逻辑""" + global current_task_index, task_step, current_state + + if current_task_index >= len(TASK_QUEUE): + if current_state != TaskState.FINISHED_ALL: + print("\n🎉🎉🎉 所有抓取任务已成功完成！🎉🎉🎉") + current_state = TaskState.FINISHED_ALL + return False + + obj_name, target_place_pos = TASK_QUEUE[current_task_index] + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, obj_name) + + if obj_id == -1: + print(f"❌ 错误：未在模型中找到物体 '{obj_name}'，请检查XML文件。") + current_task_index += 1 + return True + + # --- 状态机逻辑 --- + if current_state == TaskState.MOVE_TO_OBJECT_ABOVE: + if task_step == 0: + print(f"\n[任务 {current_task_index + 1}/{len(TASK_QUEUE)}] 开始处理物体: {obj_name}") + print("-> 状态: 移动到物体上方...") + target_pos = data.xpos[obj_id].copy() + target_pos[2] = CLEARANCE_HEIGHT + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + task_step = 0 + current_state = TaskState.MOVE_DOWN_TO_GRASP + + elif current_state == TaskState.MOVE_DOWN_TO_GRASP: + if task_step == 0: + print("-> 状态: 下降以抓取物体...") + target_pos = data.xpos[obj_id].copy() + target_pos[2] += 0.05 # 停在物体表面上方一点 + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: + task_step = 0 + current_state = TaskState.GRASP_OBJECT + + elif current_state == TaskState.GRASP_OBJECT: + if task_step == 0: + print("-> 状态: 正在抓取...") + # 闭合夹爪: 左爪左移(负), 右爪右移(正) + data.ctrl[3] = GRASP_FORCE + data.ctrl[4] = -GRASP_FORCE + if task_step > STEP_PER_GRASP: + task_step = 0 + current_state = TaskState.MOVE_UP_AFTER_GRASP + + elif current_state == TaskState.MOVE_UP_AFTER_GRASP: + if task_step == 0: + print("-> 状态: 抓取成功，上升...") + target_pos = data.site_xpos[ee_id].copy() + target_pos[2] = CLEARANCE_HEIGHT + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + task_step = 0 + current_state = TaskState.MOVE_TO_TARGET_ABOVE + + elif current_state == TaskState.MOVE_TO_TARGET_ABOVE: + if task_step == 0: + print(f"-> 状态: 移动到目标放置区上方 {target_place_pos[:2]}...") + target_pos = np.array(target_place_pos) + target_pos[2] = CLEARANCE_HEIGHT + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + task_step = 0 + current_state = TaskState.MOVE_DOWN_TO_PLACE + + elif current_state == TaskState.MOVE_DOWN_TO_PLACE: + if task_step == 0: + print("-> 状态: 下降以放置物体...") + target_pos = np.array(target_place_pos) + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: + task_step = 0 + current_state = TaskState.RELEASE_OBJECT + + elif current_state == TaskState.RELEASE_OBJECT: + if task_step == 0: + print("-> 状态: 正在释放物体...") + # 打开夹爪: 左右爪都回中 + data.ctrl[3] = 0 + data.ctrl[4] = 0 + if task_step > STEP_PER_GRASP: + task_step = 0 + current_state = TaskState.MOVE_UP_AFTER_RELEASE + + elif current_state == TaskState.MOVE_UP_AFTER_RELEASE: + if task_step == 0: + print("-> 状态: 释放成功，上升并准备下一个任务...") + target_pos = data.site_xpos[ee_id].copy() + target_pos[2] = CLEARANCE_HEIGHT + simple_ik_control(model, data, ee_id, target_pos) + if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + current_task_index += 1 + task_step = 0 + current_state = TaskState.MOVE_TO_OBJECT_ABOVE - # 所有目标点完成，退出 - if current_target_idx >= len(AUTO_TARGETS): - print("\n🎉 所有自动任务强制完成！") - return False # 任务完成，返回False - return True # 任务继续 + task_step += 1 + return True -# ===================== 初始化+主程序（强制自动） ===================== +# ===================== 主程序 ===================== def init(): - """极简初始化：确保快速启动""" global viewer - # 检查模型文件 if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"请确保robot.xml在当前目录：{MODEL_PATH}") + raise FileNotFoundError(f"请确保 'robot.xml' 文件在当前目录: {MODEL_PATH}") - # 加载模型 model = mujoco.MjModel.from_xml_path(MODEL_PATH) data = mujoco.MjData(model) - # 初始化关节到中间位置（避免初始转圈） - for i in range(min(3, model.njnt)): - data.qpos[i] = (JOINT_LIMITS[i][0] + JOINT_LIMITS[i][1]) / 2 - mujoco.mj_forward(model, data) - - # 初始化Viewer（强制显示） viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer.cam.distance = 1.5 - viewer.cam.elevation = 20 - viewer.cam.azimuth = 70 - viewer.cam.lookat = [0.1, 0.0, 0.1] + viewer.cam.distance = 2.0 + viewer.cam.elevation = -20 + viewer.cam.azimuth = 90 + viewer.cam.lookat = [0.2, 0.0, 0.1] - # 极简ID识别（只找关键ID） ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site") - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object") + if ee_id == -1: + raise ValueError("模型中必须包含一个名为 'ee_site' 的site。") - # 打印强制启动提示 - print("=" * 50) - print("🚨 强制自动运行模式启动！") - print("📌 无需任何按键，立刻执行抓取任务") - print("🎯 目标点：物体位置→放置位置→归位") - print("=" * 50) - return model, data, ee_id, obj_id + print("=" * 60) + print("🚀 全自动智能抓取系统启动！") + print(f"📋 任务清单: 共 {len(TASK_QUEUE)} 个物体需要处理。") + print("💡 正在连接到模型 'simple_arm'...") + print("=" * 60) + return model, data, ee_id def main(): global viewer try: - # 初始化 - model, data, ee_id, obj_id = init() + model, data, ee_id = init() - # 强制自动运行核心循环（无任何按键依赖） while viewer.is_alive: - # 执行自动任务，返回False则退出 - if not run_auto_task(model, data, ee_id, obj_id): + if not run_smart_grasp_task(model, data, ee_id): break - # 仿真步进（快速渲染） mujoco.mj_step(model, data) viewer.render() time.sleep(0.005) - # 任务完成后，保持窗口3秒 - print("\n⏳ 任务完成，3秒后自动退出...") - for _ in range(3): + print("\n⏳ 所有任务已完成，窗口将在5秒后自动关闭。") + for _ in range(5): viewer.render() time.sleep(1) except Exception as e: - print(f"\n❌ 错误：{e}") + print(f"\n❌ 程序发生错误: {e}") + import traceback + traceback.print_exc() finally: with suppress(Exception): viewer.close() - print("🔚 强制自动运行结束") + print("🔚 程序已退出。") if __name__ == "__main__": - # 强制检查依赖并启动 try: import mujoco, mujoco_viewer except ImportError: - print("❌ 缺少依赖！执行：pip install mujoco mujoco-viewer numpy") + print("❌ 缺少依赖！请运行: pip install mujoco mujoco-viewer numpy") exit(1) main() \ No newline at end of file From 0b0d2009b503757b545b1bc60470f119b457d940 Mon Sep 17 00:00:00 2001 From: YWWBZD <3118693830@qq.com> Date: Sun, 28 Dec 2025 14:21:36 +0800 Subject: [PATCH 26/26] =?UTF-8?q?=E6=A0=B8=E5=BF=83=E6=94=B9=E8=BF=9B?= =?UTF-8?q?=E4=B8=8E=E6=96=B0=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Robot_arm_grasping_task/main.py | 150 +++++++++++++++++----------- 1 file changed, 93 insertions(+), 57 deletions(-) diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 7769b8570c..7cb85e59f6 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -6,48 +6,69 @@ import time from contextlib import suppress -# ===================== 配置（已根据你的模型定制） ===================== +# ===================== 配置 ===================== warnings.filterwarnings('ignore') CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# --- 1. 任务清单（已使用正确的物体名称 'target_object'） --- +# --- 任务清单 --- TASK_QUEUE = [ - # 将名为 'target_object' 的物体移动到 (-0.3, 0, 0.05) ["target_object", [-0.3, 0, 0.05]], ] -# --- 2. 核心控制参数 --- -IK_GAIN = 1.5 -GRASP_FORCE = -8.0 # 夹爪闭合的力（负值表示向左/右） -CLEARANCE_HEIGHT = 0.25 # 移动时的安全高度 -STEP_PER_MOVE = 1200 # 移动到一个新位置所需的步数 -STEP_PER_GRASP = 400 # 抓取/释放动作所需的步数 +# --- 核心控制参数 (精细化) --- +IK_GAIN = 1.2 +CLEARANCE_HEIGHT = 0.25 +STEP_PER_MOVE = 1500 + +# --- 抓取力控制参数 (关键) --- +GRIPPER_OPEN_FORCE = 8.0 # 张开夹爪的力 +GRIPPER_GRASP_FORCE = -12.0 # 抓取时尝试闭合的力 +GRIPPER_HOLD_FORCE = -4.0 # 抓住物体后保持的力 +CONTACT_THRESHOLD = 1.0 # 接触力阈值 (N)，超过此值认为已接触 # ===================== 全局状态机 ===================== viewer = None current_task_index = 0 task_step = 0 +grasp_force_applied = 0.0 # 记录当前施加的抓取力 class TaskState: MOVE_TO_OBJECT_ABOVE = 1 - MOVE_DOWN_TO_GRASP = 2 - GRASP_OBJECT = 3 - MOVE_UP_AFTER_GRASP = 4 - MOVE_TO_TARGET_ABOVE = 5 - MOVE_DOWN_TO_PLACE = 6 - RELEASE_OBJECT = 7 - MOVE_UP_AFTER_RELEASE = 8 - FINISHED_ALL = 9 + PRE_GRASP_OPEN = 2 + MOVE_DOWN_TO_GRASP = 3 + GRASP_OBJECT = 4 + HOLD_OBJECT = 5 + MOVE_UP_AFTER_GRASP = 6 + MOVE_TO_TARGET_ABOVE = 7 + MOVE_DOWN_TO_PLACE = 8 + RELEASE_OBJECT = 9 + MOVE_UP_AFTER_RELEASE = 10 + FINISHED_ALL = 11 current_state = TaskState.MOVE_TO_OBJECT_ABOVE -# ===================== 核心功能函数 ===================== +# ===================== 辅助函数 ===================== +def get_contact_force(model, data, geom1_name, geom2_name): + """获取两个geom之间的接触力大小""" + geom1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom1_name) + geom2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom2_name) + + for i in range(data.ncon): + con = data.contact[i] + if (con.geom1 == geom1_id and con.geom2 == geom2_id) or \ + (con.geom1 == geom2_id and con.geom2 == geom1_id): + # contact force is in data.cfrc_ext, but we need to compute it + # For simplicity, we can use the normal force magnitude + return abs(con.force[0]) + return 0.0 + + def simple_ik_control(model, data, ee_id, target_pos): - """逆运动学控制，让末端执行器移动到目标位置""" + """逆运动学控制""" current_pos = data.site_xpos[ee_id] error = target_pos - current_pos error = np.clip(error, -0.05, 0.05) @@ -57,18 +78,17 @@ def simple_ik_control(model, data, ee_id, target_pos): jnt_vel = np.dot(jacp[:, :3].T, error * IK_GAIN) jnt_vel = np.clip(jnt_vel, -0.5, 0.5) - # 注意：这里控制的是关节力矩（motor），而不是直接设置角度 - for i in range(min(3, model.nu - 2)): # 减去夹爪的两个控制 - data.ctrl[i] = jnt_vel[i] * 100 # 乘以一个系数来放大控制信号 + for i in range(min(3, model.nu)): + data.ctrl[i] = jnt_vel[i] * 100 -def run_smart_grasp_task(model, data, ee_id): - """智能抓取任务的状态机逻辑""" - global current_task_index, task_step, current_state +# ===================== 复杂抓取任务逻辑 ===================== +def run_complex_grasp_task(model, data, ee_id): + global current_task_index, task_step, current_state, grasp_force_applied if current_task_index >= len(TASK_QUEUE): if current_state != TaskState.FINISHED_ALL: - print("\n🎉🎉🎉 所有抓取任务已成功完成！🎉🎉🎉") + print("\n🎉🎉🎉 所有复杂抓取任务已成功完成！🎉🎉🎉") current_state = TaskState.FINISHED_ALL return False @@ -76,45 +96,64 @@ def run_smart_grasp_task(model, data, ee_id): obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, obj_name) if obj_id == -1: - print(f"❌ 错误：未在模型中找到物体 '{obj_name}'，请检查XML文件。") + print(f"❌ 错误：未在模型中找到物体 '{obj_name}'。") current_task_index += 1 return True # --- 状态机逻辑 --- if current_state == TaskState.MOVE_TO_OBJECT_ABOVE: - if task_step == 0: - print(f"\n[任务 {current_task_index + 1}/{len(TASK_QUEUE)}] 开始处理物体: {obj_name}") - print("-> 状态: 移动到物体上方...") + if task_step == 0: print("\n-> 状态: 移动到物体上方安全高度...") target_pos = data.xpos[obj_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.01: + task_step = 0 + current_state = TaskState.PRE_GRASP_OPEN + + elif current_state == TaskState.PRE_GRASP_OPEN: + if task_step == 0: print("-> 状态: 预抓取 - 张开夹爪...") + # 张开夹爪 + data.ctrl[3] = GRIPPER_OPEN_FORCE + data.ctrl[4] = -GRIPPER_OPEN_FORCE + if task_step > 500: # 等待夹爪完全张开 task_step = 0 current_state = TaskState.MOVE_DOWN_TO_GRASP elif current_state == TaskState.MOVE_DOWN_TO_GRASP: - if task_step == 0: - print("-> 状态: 下降以抓取物体...") + if task_step == 0: print("-> 状态: 下降接近物体...") target_pos = data.xpos[obj_id].copy() - target_pos[2] += 0.05 # 停在物体表面上方一点 + target_pos[2] += 0.04 # 非常接近物体表面 simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: task_step = 0 current_state = TaskState.GRASP_OBJECT elif current_state == TaskState.GRASP_OBJECT: - if task_step == 0: - print("-> 状态: 正在抓取...") - # 闭合夹爪: 左爪左移(负), 右爪右移(正) - data.ctrl[3] = GRASP_FORCE - data.ctrl[4] = -GRASP_FORCE - if task_step > STEP_PER_GRASP: + if task_step == 0: print("-> 状态: 抓取 - 快速闭合夹爪...") + # 快速闭合夹爪 + data.ctrl[3] = GRIPPER_GRASP_FORCE + data.ctrl[4] = -GRIPPER_GRASP_FORCE + # 检测是否接触到物体 + left_contact_force = get_contact_force(model, data, "gripper_left_geom", "target_geom") + right_contact_force = get_contact_force(model, data, "gripper_right_geom", "target_geom") + if left_contact_force > CONTACT_THRESHOLD or right_contact_force > CONTACT_THRESHOLD: + print(f" [接触检测] 检测到接触力！切换到保持模式。") + grasp_force_applied = GRIPPER_HOLD_FORCE + task_step = 0 + current_state = TaskState.HOLD_OBJECT + + elif current_state == TaskState.HOLD_OBJECT: + if task_step == 0: print("-> 状态: 保持 - 施加稳定夹持力...") + # 施加稳定的保持力 + data.ctrl[3] = grasp_force_applied + data.ctrl[4] = -grasp_force_applied + # 等待一小段时间确保抓稳 + if task_step > 500: task_step = 0 current_state = TaskState.MOVE_UP_AFTER_GRASP elif current_state == TaskState.MOVE_UP_AFTER_GRASP: - if task_step == 0: - print("-> 状态: 抓取成功，上升...") + if task_step == 0: print("-> 状态: 抓取成功，上升...") target_pos = data.site_xpos[ee_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -122,9 +161,9 @@ def run_smart_grasp_task(model, data, ee_id): task_step = 0 current_state = TaskState.MOVE_TO_TARGET_ABOVE + # ... (MOVE_TO_TARGET_ABOVE, MOVE_DOWN_TO_PLACE 状态与之前类似) ... elif current_state == TaskState.MOVE_TO_TARGET_ABOVE: - if task_step == 0: - print(f"-> 状态: 移动到目标放置区上方 {target_place_pos[:2]}...") + if task_step == 0: print(f"-> 状态: 移动到目标放置区上方 {target_place_pos[:2]}...") target_pos = np.array(target_place_pos) target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -133,8 +172,7 @@ def run_smart_grasp_task(model, data, ee_id): current_state = TaskState.MOVE_DOWN_TO_PLACE elif current_state == TaskState.MOVE_DOWN_TO_PLACE: - if task_step == 0: - print("-> 状态: 下降以放置物体...") + if task_step == 0: print("-> 状态: 下降到放置位置...") target_pos = np.array(target_place_pos) simple_ik_control(model, data, ee_id, target_pos) if np.linalg.norm(data.site_xpos[ee_id] - target_pos) < 0.005: @@ -142,18 +180,16 @@ def run_smart_grasp_task(model, data, ee_id): current_state = TaskState.RELEASE_OBJECT elif current_state == TaskState.RELEASE_OBJECT: - if task_step == 0: - print("-> 状态: 正在释放物体...") - # 打开夹爪: 左右爪都回中 - data.ctrl[3] = 0 - data.ctrl[4] = 0 - if task_step > STEP_PER_GRASP: + if task_step == 0: print("-> 状态: 释放 - 打开夹爪...") + # 完全松开夹爪 + data.ctrl[3] = GRIPPER_OPEN_FORCE + data.ctrl[4] = -GRIPPER_OPEN_FORCE + if task_step > 800: # 等待夹爪完全打开 task_step = 0 current_state = TaskState.MOVE_UP_AFTER_RELEASE elif current_state == TaskState.MOVE_UP_AFTER_RELEASE: - if task_step == 0: - print("-> 状态: 释放成功，上升并准备下一个任务...") + if task_step == 0: print("-> 状态: 释放成功，上升...") target_pos = data.site_xpos[ee_id].copy() target_pos[2] = CLEARANCE_HEIGHT simple_ik_control(model, data, ee_id, target_pos) @@ -166,7 +202,7 @@ def run_smart_grasp_task(model, data, ee_id): return True -# ===================== 主程序 ===================== +# ===================== 主程序 (与之前版本类似) ===================== def init(): global viewer if not os.path.exists(MODEL_PATH): @@ -186,9 +222,9 @@ def init(): raise ValueError("模型中必须包含一个名为 'ee_site' 的site。") print("=" * 60) - print("🚀 全自动智能抓取系统启动！") + print("🚀 全自动复杂柔顺抓取系统启动！") print(f"📋 任务清单: 共 {len(TASK_QUEUE)} 个物体需要处理。") - print("💡 正在连接到模型 'simple_arm'...") + print("💡 系统将使用接触感知进行智能抓取。") print("=" * 60) return model, data, ee_id @@ -199,7 +235,7 @@ def main(): model, data, ee_id = init() while viewer.is_alive: - if not run_smart_grasp_task(model, data, ee_id): + if not run_complex_grasp_task(model, data, ee_id): break mujoco.mj_step(model, data)