Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 31 additions & 39 deletions src/Robot_arm_grasping_task/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,15 @@
import matplotlib.pyplot as plt
import time
import matplotlib as mpl
import os # 新增:用于处理路径
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系统)
Expand All @@ -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


# ===================== 工具函数 =====================
Expand All @@ -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")

Expand All @@ -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

Expand All @@ -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}")

Expand Down Expand Up @@ -118,22 +117,22 @@ 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(
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:抓取物体 ----------------
# ---------------- 阶段2:抓取物体(简化判定,避免卡停)----------------
elif phase == 2:
# 保持末端位置
target_joint_pos = ik_newton_raphson(model, data, TARGET_OBJECT_POS, data.qpos)
Expand All @@ -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:
Expand All @@ -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("📦 已到达目标位置,进入放置阶段")
Expand All @@ -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

Expand All @@ -222,7 +214,7 @@ def grasp_simulation():

# 渲染可视化
viewer.render()
time.sleep(0.0005) # 降低仿真速度,更易观察
time.sleep(0.001) # 稍降低仿真速度,便于观察

except KeyboardInterrupt:
print("\n⚠️ 仿真被手动终止")
Expand Down Expand Up @@ -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')
Expand Down
Loading