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