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')