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
133 changes: 85 additions & 48 deletions src/Robot_arm_grasping_task/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 # 新增:控制平滑因子,降低突变


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

Expand Down Expand Up @@ -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
Expand All @@ -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("✅ 抓取成功,进入搬运阶段")
Expand All @@ -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
Expand All @@ -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:
Expand All @@ -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⚠️ 仿真被手动终止")
Expand All @@ -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. 末端执行器轨迹
Expand Down Expand Up @@ -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')
Expand Down
Loading