diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000000..359bb5307e
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,3 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000000..105ce2da2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000000..2ebefb79c4
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000000..5fdb6d5018
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/nn2.iml b/.idea/nn2.iml
new file mode 100644
index 0000000000..0437ed9c9f
--- /dev/null
+++ b/.idea/nn2.iml
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000000..35eb1ddfbb
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/Robot_arm_grasp/README.md b/src/Robot_arm_grasp/README.md
new file mode 100644
index 0000000000..c23571d758
--- /dev/null
+++ b/src/Robot_arm_grasp/README.md
@@ -0,0 +1 @@
+机械臂抓取
\ No newline at end of file
diff --git a/src/Robot_arm_grasp/main.py b/src/Robot_arm_grasp/main.py
new file mode 100644
index 0000000000..690a69e2e4
--- /dev/null
+++ b/src/Robot_arm_grasp/main.py
@@ -0,0 +1,130 @@
+import numpy as np
+import math
+import time
+import mujoco as mj
+from mujoco import viewer
+
+# ====================== 全局配置参数 ======================
+# MuJoCo模型路径
+MUJOCO_MODEL_PATH = "robot_arm.xml"
+# 机械臂关节名
+JOINT_NAMES = ["joint0", "joint1", "joint2", "joint3"]
+# 目标物体名
+TARGET_BODY_NAME = "target_ball"
+# 仿真目标位置(可调整)
+SIM_TARGET_POS = np.array([1.5, 1.0, 0.5])
+# 全局标志:是否已执行移动逻辑
+has_moved = False
+
+# ====================== MuJoCo机械臂控制类(适配原生UI)=====================
+class MuJoCoArmController:
+ def __init__(self, model_path):
+ # 加载模型和数据
+ self.model = mj.MjModel.from_xml_path(model_path)
+ self.data = mj.MjData(self.model)
+ # 获取关节和目标物体索引
+ self.joint_ids = [mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_JOINT, name) for name in JOINT_NAMES]
+ self.target_body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_BODY, TARGET_BODY_NAME)
+ # 设置目标物体初始位置
+ self.set_target_pos(SIM_TARGET_POS)
+ print("✅ MuJoCo机械臂控制器初始化完成")
+
+ def set_target_pos(self, pos):
+ """设置目标物体位置"""
+ self.data.body(TARGET_BODY_NAME).xpos = pos
+ print(f"🎯 目标物体位置已设置为:{np.round(pos, 3)}")
+
+ def get_joint_angles(self):
+ """获取当前关节角度"""
+ return [self.data.joint(name).qpos[0] for name in JOINT_NAMES]
+
+ def set_joint_target(self, target_angles, kp=200, kd=10):
+ """PD控制关节到目标角度"""
+ current_angles = np.array(self.get_joint_angles())
+ current_vel = np.array([self.data.joint(name).qvel[0] for name in JOINT_NAMES])
+ torque = kp * (target_angles - current_angles) - kd * current_vel
+ for i, joint_id in enumerate(self.joint_ids):
+ self.data.ctrl[joint_id] = torque[i]
+
+ def inverse_kinematics(self, target_pos):
+ """数值逆运动学求解关节角度(兼容版本,改用手动映射避免mj_inverse问题)"""
+ print("ℹ️ 使用兼容版逆运动学(坐标映射角度)")
+ # 手动坐标到角度的映射(可根据模型调整比例)
+ angle0 = target_pos[0] * np.pi / 4 # joint0:绕z轴旋转
+ angle1 = target_pos[1] * np.pi / 4 # joint1:绕x轴旋转
+ angle2 = -target_pos[1] * np.pi / 4 # joint2:绕x轴旋转
+ angle3 = 0.0 # joint3:固定角度
+ return np.array([angle0, angle1, angle2, angle3])
+
+ def move_to_target(self, target_pos):
+ """移动机械臂到目标位置"""
+ global has_moved
+ if has_moved:
+ return
+ print(f"\n📢 开始移动到目标位置:{np.round(target_pos, 3)}")
+ target_angles = self.inverse_kinematics(target_pos)
+ # 逐步逼近目标角度
+ current_angles = np.array(self.get_joint_angles())
+ step = 0.02
+ while np.linalg.norm(current_angles - target_angles) > 0.02:
+ current_angles = np.clip(current_angles + step * np.sign(target_angles - current_angles), -np.pi, np.pi)
+ self.set_joint_target(current_angles)
+ mj.mj_step(self.model, self.data)
+ time.sleep(0.01)
+ self.set_joint_target(target_angles)
+ self.close_gripper()
+ has_moved = True
+ print("✅ 机械臂已到达目标位置并完成抓取")
+
+ def close_gripper(self):
+ """闭合夹爪(模拟抓取)"""
+ print("🤖 夹爪闭合,抓取目标")
+ # 目标物体随夹爪移动(兼容:检查arm4是否存在)
+ body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_BODY, "arm4")
+ if body_id != -1:
+ gripper_pos = self.data.body("arm4").xpos
+ self.data.body(TARGET_BODY_NAME).xpos = gripper_pos + np.array([0, 0.1, 0])
+ time.sleep(0.5)
+
+# ====================== 主程序(兼容所有MuJoCo版本,移除callback参数)=====================
+def main():
+ global has_moved
+ # 初始化控制器
+ arm_controller = MuJoCoArmController(MUJOCO_MODEL_PATH)
+ # 设置目标物体位置
+ arm_controller.set_target_pos(SIM_TARGET_POS)
+
+ # 打印操作说明
+ print("\n=====================================")
+ print("📋 操作说明:")
+ print(" - 程序启动后会自动移动机械臂到目标位置并抓取")
+ print(" - 在Viewer中可通过鼠标调整视角(拖拽/滚轮)")
+ print(" - 按ESC键或关闭窗口退出")
+ print("=====================================\n")
+
+ # 先执行机械臂移动逻辑(在启动Viewer前完成核心运动)
+ arm_controller.move_to_target(SIM_TARGET_POS)
+
+ # 启动MuJoCo原生可视化Viewer(兼容版本:仅传入model和data,不使用callback)
+ # 方式1:使用viewer.launch(简单启动,部分版本支持)
+ try:
+ viewer.launch(arm_controller.model, arm_controller.data)
+ except Exception as e:
+ # 方式2:如果launch报错,改用手动循环(最兼容的方式)
+ print(f"⚠️ 简易启动失败,使用手动循环模式:{e}")
+ # 创建viewer实例
+ v = viewer.Viewer(arm_controller.model, arm_controller.data)
+ while True:
+ # 持续步进到下一帧
+ mj.mj_step(arm_controller.model, arm_controller.data)
+ # 更新viewer画面
+ v.sync()
+ # 短暂休眠,控制帧率
+ time.sleep(0.01)
+ # 检查是否关闭窗口
+ if not v.is_running():
+ break
+ v.close()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/Robot_arm_grasp/robot_arm.xml b/src/Robot_arm_grasp/robot_arm.xml
new file mode 100644
index 0000000000..6b0123d1ff
--- /dev/null
+++ b/src/Robot_arm_grasp/robot_arm.xml
@@ -0,0 +1,75 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file