Skip to content

2D 平面机械臂抓取演示 #4751

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.

Sign up for GitHub

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

Merged
donghaiwang merged 29 commits into from
Dec 27, 2025
Merged

2D 平面机械臂抓取演示 #4751

Changes from all commits
Commits
Show all changes
29 commits
Select commit Hold shift + click to select a range
c2b7030
提交选题
gongjh0916 Dec 19, 2025
fc8d277
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 19, 2025
3d9f4a3
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 19, 2025
208f44c
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 21, 2025
ef8b457
添加.gitignore，忽略无用本地文件
gongjh0916 Dec 21, 2025
7563e69
添加初始代码
gongjh0916 Dec 21, 2025
4795836
控制机械臂升降模拟
gongjh0916 Dec 22, 2025
a42d8a5
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 22, 2025
2aa5834
机械臂升降模拟
gongjh0916 Dec 22, 2025
0d1343a
初始机械臂升降控制
gongjh0916 Dec 22, 2025
32ea667
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 22, 2025
e8b85bc
Merge branch 'main' of https://github.com/gongjh0916/nn
gongjh0916 Dec 22, 2025
723a4e7
机械臂关节伸展代码优化
gongjh0916 Dec 23, 2025
9c99f7d
Merge branch 'main' into main
gongjh0916 Dec 23, 2025
b9cb5c1
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 24, 2025
e5d7fd7
mujoco的机械臂运行
gongjh0916 Dec 24, 2025
b7bc07a
mujoco机械臂运行
gongjh0916 Dec 24, 2025
f1539d7
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 25, 2025
615d15f
旋转关节多自由度
gongjh0916 Dec 25, 2025
68f6be2
Merge branch 'main' of https://github.com/gongjh0916/nn
gongjh0916 Dec 25, 2025
4c32426
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 26, 2025
d138710
带实时末端位置反馈的 SCARA 型机械臂演示
gongjh0916 Dec 26, 2025
1cb4e68
Merge branch 'main' of https://github.com/gongjh0916/nn
gongjh0916 Dec 26, 2025
5fe20d1
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 26, 2025
0dc7604
7自由度协作机械臂模型
gongjh0916 Dec 26, 2025
45aa3fd
Merge branch 'main' of https://github.com/gongjh0916/nn
gongjh0916 Dec 26, 2025
4002fce
Merge branch 'OpenHUTB:main' into main
gongjh0916 Dec 27, 2025
06a0154
2D 平面机械臂抓取演示
gongjh0916 Dec 27, 2025
d770c5a
Merge branch 'main' of https://github.com/gongjh0916/nn
gongjh0916 Dec 27, 2025
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
268 changes: 121 additions & 147 deletions src/Mechanical_arm_grasping/main.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,230 +1,204 @@
# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误）
# MuJoCo 3.4.0 轻量版2D平面机械臂抓取（无传感器，零XML错误）
import mujoco
import mujoco.viewer
import time
import numpy as np


def collaborative_robot_arm_demo():
# 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑
cobot_xml = """
<mujoco model="7-DOF Collaborative Robot Arm">
def simple_2d_robot_arm_demo():
# 纯2D平面模型，仅保留MuJoCo 3.4.0原生支持标签
robot_2d_xml = """
<mujoco model="2D Simple Robot Arm">
<compiler angle="radian" inertiafromgeom="true"/>
<option timestep="0.005" gravity="0 0 -9.81"/>
<visual/>
<visual>
<global azimuth="90" elevation="-90"/> <!-- 2D平面视角 -->
</visual>
<asset>
<material name="red" rgba="0.8 0.2 0.2 1"/>
<material name="lightblue" rgba="0.4 0.7 0.9 1"/>
<material name="blue" rgba="0.2 0.4 0.8 1"/>
<material name="gray" rgba="0.5 0.5 0.5 1"/>
<material name="green" rgba="0.2 0.8 0.2 1"/>
<material name="yellow" rgba="0.8 0.8 0.2 1"/>
<material name="orange" rgba="0.9 0.5 0.2 1"/>
</asset>

<!-- 世界体定义 -->
<!-- 2D世界体定义（限制在X-Y平面运动） -->
<worldbody>
<camera name="fixed_camera" pos="2.5 2.0 2.0" xyaxes="1 0 0 0 1 0"/>
<!-- 地面 -->
<geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
<!-- 易碎目标：橙色球体（低刚度，防止夹碎） -->
<body name="fragile_target" pos="0.7 0.5 0.1">
<geom name="target_geom" type="sphere" size="0.07" pos="0 0 0" material="orange" solref="0.02 1.0" solimp="0.9 0.95 0.01"/>
<!-- 固定相机（2D视角） -->
<camera name="2d_camera" pos="0 2 1" xyaxes="1 0 0 0 0 1"/>
<!-- 地面（2D平面） -->
<geom name="floor" type="plane" size="3 3 0.1" pos="0 0 -0.1" material="gray"/>
<!-- 抓取目标：黄色立方体（2D平面放置） -->
<body name="target" pos="1.2 0 0.1">
<geom name="target_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="yellow"/>
<joint name="target_joint" type="free"/>
</body>
<!-- 7自由度协作机械臂（UR5e构型） -->
<!-- 2自由度平面机械臂 -->
<body name="base" pos="0 0 0">
<geom name="base_geom" type="cylinder" size="0.2 0.2" pos="0 0 0" material="lightblue"/>
<geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="blue"/>
<joint name="base_joint" type="free"/>
<!-- 关节1：基座旋转（Z轴） -->
<body name="joint1_link" pos="0 0 0.2">
<geom name="joint1_geom" type="cylinder" size="0.12 0.3" pos="0 0 0.15" material="lightblue"/>
<joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
<!-- 关节2：大臂俯仰（Y轴） -->
<body name="joint2_link" pos="0 0 0.3">
<geom name="joint2_geom" type="cylinder" size="0.1 0.4" pos="0 0 0.2" material="lightblue"/>
<joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
<!-- 关节3：小臂俯仰（Y轴） -->
<body name="joint3_link" pos="0 0 0.4">
<geom name="joint3_geom" type="cylinder" size="0.09 0.4" pos="0 0 0.2" material="lightblue"/>
<joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
<!-- 关节4：腕部旋转（Z轴） -->
<body name="joint4_link" pos="0 0 0.4">
<geom name="joint4_geom" type="cylinder" size="0.08 0.3" pos="0 0 0.15" material="lightblue"/>
<joint name="joint4" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
<!-- 关节5：腕部俯仰（Y轴） -->
<body name="joint5_link" pos="0 0 0.3">
<geom name="joint5_geom" type="cylinder" size="0.07 0.3" pos="0 0 0.15" material="lightblue"/>
<joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
<!-- 关节6：腕部偏摆（Z轴） -->
<body name="joint6_link" pos="0 0 0.3">
<geom name="joint6_geom" type="cylinder" size="0.06 0.2" pos="0 0 0.1" material="lightblue"/>
<joint name="joint6" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
<!-- 关节7：末端旋转（Y轴） -->
<body name="joint7_link" pos="0 0 0.2">
<geom name="joint7_geom" type="cylinder" size="0.05 0.2" pos="0 0 0.1" material="lightblue"/>
<joint name="joint7" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
<!-- 夹爪（简化版，无传感器） -->
<body name="gripper_base" pos="0 0 0.2">
<geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
<!-- 左夹爪 -->
<body name="left_gripper" pos="0 0.1 0">
<geom name="left_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
<joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.6 0" damping="0.03"/>
</body>
<!-- 右夹爪 -->
<body name="right_gripper" pos="0 -0.1 0">
<geom name="right_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
<joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.6" damping="0.03"/>
</body>
<!-- 末端标记 -->
<geom name="end_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
</body>
</body>
</body>
</body>

<!-- 关节1：基座旋转（Z轴，2D平面旋转） -->
<body name="arm1" pos="0 0 0.1">
<geom name="arm1_geom" type="cylinder" size="0.08 0.6" pos="0 0 0.3" material="blue"/>
<joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.03"/>

<!-- 关节2：大臂俯仰（Z轴，2D平面摆动） -->
<body name="arm2" pos="0 0 0.6">
<geom name="arm2_geom" type="cylinder" size="0.06 0.5" pos="0 0 0.25" material="blue"/>
<joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.03"/>

<!-- 简易夹爪（2D平面抓取） -->
<body name="gripper_base" pos="0 0 0.5">
<geom name="gripper_base_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>

<!-- 左夹爪 -->
<body name="left_grip" pos="0 0.08 0">
<geom name="left_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
<joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.08 0" range="-0.5 0" damping="0.02"/>
</body>

<!-- 右夹爪 -->
<body name="right_grip" pos="0 -0.08 0">
<geom name="right_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
<joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.08 0" range="0 0.5" damping="0.02"/>
</body>
</body>
</body>
</body>
</body>
</worldbody>

<!-- 执行器配置（仅保留位置/速度控制，无传感器依赖） -->
<!-- 执行器配置（MuJoCo 3.4.0原生支持） -->
<actuator>
<position name="joint1_act" joint="joint1" kp="1200" kv="120"/>
<position name="joint2_act" joint="joint2" kp="1200" kv="120"/>
<position name="joint3_act" joint="joint3" kp="1200" kv="120"/>
<position name="joint4_act" joint="joint4" kp="1200" kv="120"/>
<position name="joint5_act" joint="joint5" kp="1200" kv="120"/>
<position name="joint6_act" joint="joint6" kp="1200" kv="120"/>
<position name="joint7_act" joint="joint7" kp="1200" kv="120"/>
<!-- 夹爪速度控制（低刚度，防止夹碎物体） -->
<velocity name="left_grip_act" joint="left_grip_joint" kv="50" ctrlrange="-0.5 0"/>
<velocity name="right_grip_act" joint="right_grip_joint" kv="50" ctrlrange="0 0.5"/>
<!-- 关节位置控制 -->
<position name="joint1_act" joint="joint1" kp="1000" kv="100"/>
<position name="joint2_act" joint="joint2" kp="1000" kv="100"/>

<!-- 夹爪速度控制（安全低速） -->
<velocity name="left_grip_act" joint="left_grip_joint" kv="40" ctrlrange="-0.4 0"/>
<velocity name="right_grip_act" joint="right_grip_joint" kv="40" ctrlrange="0 0.4"/>
</actuator>
</mujoco>
"""

# 2. 加载模型（100%兼容3.4.0，无任何传感器相关错误）
# 加载模型（确保100%兼容MuJoCo 3.4.0）
try:
model = mujoco.MjModel.from_xml_string(cobot_xml)
model = mujoco.MjModel.from_xml_string(robot_2d_xml)
data = mujoco.MjData(model)
print("✅ 7自由度协作机械臂模型加载成功，启动仿真...")
print("✅ 2D平面机械臂模型加载成功，启动仿真...")
except Exception as e:
print(f"❌ 模型加载失败：{e}")
return

# 3. 获取执行器索引（仅保留基础执行器，无传感器）
# 关节执行器索引
# 获取执行器索引
joint_idxs = {
"joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"),
"joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"),
"joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act"),
"joint4": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act"),
"joint5": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint5_act"),
"joint6": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint6_act"),
"joint7": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint7_act"),
"joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
}
# 夹爪执行器索引
left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act")
right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act")

# 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取）
def smooth_joint_control(joint_name, target_angle, duration, viewer):
"""平滑关节角度控制"""
# 核心控制函数
def smooth_joint_move(joint_name, target_angle, duration, viewer):
"""平滑移动关节到目标角度"""
idx = joint_idxs[joint_name]
start_angle = data.ctrl[idx]
start_time = time.time()

while (time.time() - start_time) < duration and viewer.is_running():
t = (time.time() - start_time) / duration
current_angle = start_angle + t * (target_angle - start_angle)
progress = (time.time() - start_time) / duration
current_angle = start_angle + progress * (target_angle - start_angle)
data.ctrl[idx] = current_angle
# 打印关节状态（无力度）
print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="")

# 打印实时状态
print(f"\r{joint_name} 当前角度：{current_angle:.2f} rad | 目标角度：{target_angle:.2f} rad", end="")

mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
print() # 换行

def safe_grasp(viewer):
"""安全抓取（通过时间控制闭合，模拟力控效果）"""
print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）")
grip_speed = -0.1 # 低速闭合，避免夹碎
def safe_gripper_close(viewer):
"""安全闭合夹爪（低速+定时，模拟力控）"""
print("\n🔧 开始闭合夹爪（安全低速）")
grip_speed = -0.3
start_time = time.time()
# 闭合1.5秒后停止（模拟力控阈值）
while time.time() - start_time < 1.5 and viewer.is_running():
close_duration = 1.2 # 闭合1.2秒后停止，防止夹碎

while (time.time() - start_time) < close_duration and viewer.is_running():
progress = (time.time() - start_time) / close_duration
data.ctrl[left_grip_idx] = grip_speed
data.ctrl[right_grip_idx] = -grip_speed
print(f"\r抓取进度：{((time.time() - start_time) / 1.5) * 100:.1f}%", end="")

print(f"\r夹爪闭合进度：{progress * 100:.1f}%", end="")

mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
# 停止闭合

# 停止夹爪运动
data.ctrl[left_grip_idx] = 0
data.ctrl[right_grip_idx] = 0
print("\n✅ 抓取完成（已停止闭合，防止夹碎）！")
print("\n✅ 夹爪闭合完成，已锁定目标")

def release_gripper(duration, viewer):
"""放松夹爪"""
print("\n🔧 开始放松夹爪")
def gripper_open(duration, viewer):
"""张开夹爪"""
print("\n🔧 开始张开夹爪")
start_time = time.time()

while (time.time() - start_time) < duration and viewer.is_running():
data.ctrl[left_grip_idx] = 0.2 # 张开速度
data.ctrl[right_grip_idx] = -0.2
data.ctrl[left_grip_idx] = 0.3
data.ctrl[right_grip_idx] = -0.3

mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
print("✅ 夹爪已完全张开")

# 5. 7自由度机械臂抓取流程
cobot_steps = [
("关节1旋转对准目标", "joint1", 0.87, 3.0), # 50°旋转
("关节2俯仰调整高度", "joint2", 0.785, 2.5), # 45°俯仰
("关节3俯仰接近目标", "joint3", -0.61, 2.5), # -35°俯仰
("关节4腕部旋转校准", "joint4", 1.047, 2.0), # 60°旋转
("关节5腕部俯仰调整", "joint5", 0.523, 2.0), # 30°俯仰
("关节6腕部偏摆校准", "joint6", 0.349, 2.0), # 20°偏摆
("关节7末端旋转对准", "joint7", 0.174, 2.0), # 10°旋转
]

# 6. 启动仿真（纯3.4.0原生逻辑）

data.ctrl[left_grip_idx] = 0
data.ctrl[right_grip_idx] = 0
print("✅ 夹爪已完全张开，目标放置完成")

# 2D机械臂抓取流程
with mujoco.viewer.launch_passive(model, data) as viewer:
print("\n📌 开始7自由度协作机械臂抓取流程...")
print("\n📌 开始2D平面机械臂抓取流程...")
print("-" * 60)

# 第一步：关节运动对准目标
for step_name, joint_name, target_angle, duration in cobot_steps:
print(f"\n\n🔧 {step_name}")
smooth_joint_control(joint_name, target_angle, duration, viewer)
# 步骤1：关节1旋转对准目标
print("\n\n🔧 步骤1：基座旋转对准目标")
smooth_joint_move("joint1", 0.0, 2.5, viewer)

# 步骤2：关节2俯仰接近目标
print("\n\n🔧 步骤2：大臂俯仰接近目标")
smooth_joint_move("joint2", -0.785, 2.5, viewer) # -45°俯仰

# 第二步：安全抓取（模拟力控）
safe_grasp(viewer=viewer)
# 步骤3：安全闭合夹爪抓取目标
safe_gripper_close(viewer)

# 第三步：抬升目标（仅调整关节2）
print("\n\n🔧 抓取成功，抬升目标")
smooth_joint_control("joint2", 1.047, 2.5, viewer) # 60°俯仰抬升
# 步骤4：抬升目标（关节2回正）
print("\n\n🔧 步骤4：抬升抓取目标")
smooth_joint_move("joint2", 0.0, 2.0, viewer)

# 第四步：归位（关节1旋转回原位）
print("\n\n🔧 旋转归位")
smooth_joint_control("joint1", 0.0, 3.0, viewer)
# 步骤5：基座旋转归位
print("\n\n🔧 步骤5：机械臂旋转归位")
smooth_joint_move("joint1", 1.57, 3.0, viewer) # 90°旋转归位

# 第五步：下放目标
print("\n\n🔧 下放目标")
smooth_joint_control("joint2", 0.785, 2.5, viewer) # 45°俯仰下放
# 步骤6：下放目标（关节2再次俯仰）
print("\n\n🔧 步骤6：下放抓取目标")
smooth_joint_move("joint2", -0.785, 2.0, viewer)

# 第六步：放松夹爪
print("\n\n🔧 放松夹爪完成放置")
release_gripper(duration=2.0, viewer=viewer)
# 步骤7：张开夹爪完成放置
gripper_open(1.5, viewer)

# 保持6秒查看最终效果
print("\n\n📌 抓取流程完成，保持可视化6秒...")
# 保持可视化5秒
print("\n\n📌 抓取流程全部完成，保持可视化5秒...")
start_hold = time.time()
while (time.time() - start_hold) < 6 and viewer.is_running():
while (time.time() - start_hold) < 5 and viewer.is_running():
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)

print("\n\n🎉 7自由度协作机械臂抓取演示完毕！")
print("\n\n🎉 2D平面机械臂抓取演示完毕！")


if __name__ == "__main__":
collaborative_robot_arm_demo()
simple_2d_robot_arm_demo()
Loading