Repository navigation
彻底清除idea文件 #4321
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Closed
Closed
彻底清除idea文件 #4321
Changes from all commits
Commits
Show all changes
12 commits
Select commit
Hold shift + click to select a range
a7473c9
提交选题
ywwbzd ddde325
main_robot
ywwbzd ca56bd3
增加模型文件内容
ywwbzd f0c8b2a
增加模型文件内容
ywwbzd 07c4ba2
Final commit: robot arm grasping project (complete version)
ywwbzd 4e7386a
解决冲突问题
ywwbzd 6e3e28a
修改冲突
ywwbzd cd3e042
优化README
ywwbzd fed40a4
Trigger PR refresh: sync README
ywwbzd e1c3bcc
修复文件名空格问题:重命名Robot_arm_grasping_task 同步main.py修改
ywwbzd 5dbb9c3
彻底删除.idea目录:从Git仓库中移除配置文件
ywwbzd 2be28c7
Merge branch 'main' into main
ywwbzd File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,25 @@ | ||
| # 机械臂抓取仿真项目(MuJoCo) | ||
| Robot Arm Grasping Simulation (MuJoCo) | ||
|
|
||
| ## 项目概述 | ||
| 本项目基于 MuJoCo 物理仿真引擎实现机械臂抓取任务,核心聚焦典型物体的精准抓取、姿态规划及接触力控制,可直接用于抓取算法验证、机械臂控制逻辑调试等场景。 | ||
|
|
||
| 仿真系统支持三大核心抓取场景,覆盖基础到进阶的应用需求: | ||
| - 单物体精准抓取:从固定位置抓取立方体、圆柱体等标准物体,并放置到指定目标坐标 | ||
| - 多物体无序抓取(混箱拣选):从杂乱堆放的多物体中识别并抓取指定目标,具备避障路径规划能力 | ||
| - 易碎品柔顺抓取:通过精准力控调节夹爪力度,抓取玻璃、鸡蛋等易碎物品,避免发生破损 | ||
|
|
||
| ## 核心功能 | ||
| - 逆运动学(IK)求解器:融合数值法(牛顿-拉夫逊法)与雅可比矩阵计算,实现机械臂末端执行器的精准姿态控制 | ||
| - 力反馈控制:读取 MuJoCo 力传感器数据,实时调整夹爪夹持力,保障抓取过程稳定性 | ||
| - 抓取成功判定:通过检测物体位移量和接触力阈值双重标准,自动判断抓取任务是否成功 | ||
|
|
||
| ## 技术框架 | ||
| - 物理引擎:MuJoCo(v2.3+ 版本) | ||
| - 控制算法:PID 控制、计算力矩控制(CTC)、阻抗控制 | ||
| - 评价指标:抓取成功率、轨迹跟踪均方误差(MSE)、接触力稳定性 | ||
|
|
||
| ## 快速开始 | ||
| 1. 配置 MuJoCo 环境变量,加载机械臂模型文件(robot.xml) | ||
| 2. 运行抓取演示脚本:`python grasp_demo.py` | ||
| 3. 查看仿真结果日志及可视化视频,验证抓取效果 |
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,292 @@ | ||
| 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") |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
图片文件不需要使用git管理。而且图中文件乱码