Skip to content

7自由度协作机械臂模型 #4701

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 26 commits into from
Dec 27, 2025
Merged

7自由度协作机械臂模型 #4701

Changes from all commits
Commits
Show all changes
26 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
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
272 changes: 157 additions & 115 deletions src/Mechanical_arm_grasping/main.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,188 +1,230 @@
# MuJoCo 3.4.0 SCARA型机械臂（末端反馈+目标跟随）演示
# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误）
import mujoco
import mujoco.viewer
import time
import numpy as np


def scara_robot_arm_demo():
# 1. 内置SCARA机械臂XML模型（工业常用构型）
scara_xml = """
<mujoco model="SCARA Robot Arm">
def collaborative_robot_arm_demo():
# 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑
cobot_xml = """
<mujoco model="7-DOF Collaborative Robot Arm">
<compiler angle="radian" inertiafromgeom="true"/>
<option timestep="0.005" gravity="0 0 -9.81"/>
<visual/>
<asset>
<material name="red" rgba="0.8 0.2 0.2 1"/>
<material name="darkblue" rgba="0.1 0.1 0.6 1"/>
<material name="lightblue" rgba="0.4 0.7 0.9 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="cyan" rgba="0.2 0.8 0.8 1"/>
<material name="orange" rgba="0.9 0.5 0.2 1"/>
</asset>

<!-- 世界体定义 -->
<worldbody>
<camera name="fixed_camera" pos="2.0 2.0 1.5" xyaxes="1 0 0 0 1 0"/>
<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="moving_target" pos="0.8 0.6 0.3">
<geom name="target_geom" type="sphere" size="0.05" pos="0 0 0" material="cyan"/>
<!-- 易碎目标：橙色球体（低刚度，防止夹碎） -->
<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"/>
<joint name="target_joint" type="free"/>
</body>
<!-- SCARA机械臂（工业构型：旋转1+旋转2+升降+旋转夹爪） -->
<!-- 7自由度协作机械臂（UR5e构型） -->
<body name="base" pos="0 0 0">
<geom name="base_geom" type="cylinder" size="0.25 0.15" pos="0 0 0" material="darkblue"/>
<geom name="base_geom" type="cylinder" size="0.2 0.2" pos="0 0 0" material="lightblue"/>
<joint name="base_joint" type="free"/>
<!-- 关节1：水平旋转（绕Z轴，基座旋转） -->
<body name="joint1_link" pos="0 0 0.15">
<geom name="joint1_geom" type="cylinder" size="0.15 0.2" pos="0 0 0.1" material="darkblue"/>
<joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.08"/>
<!-- 关节2：水平旋转（绕Z轴，大臂旋转） -->
<body name="joint2_link" pos="0.5 0 0.1">
<geom name="joint2_geom" type="cylinder" size="0.12 0.4" pos="0.2 0 0" material="darkblue"/>
<joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.08"/>
<!-- 关节3：垂直升降（Z轴，小臂升降） -->
<body name="joint3_link" pos="0.4 0 0">
<geom name="joint3_geom" type="cylinder" size="0.1 0.3" pos="0 0 0.15" material="darkblue"/>
<joint name="joint3" type="slide" axis="0 0 1" pos="0 0 0" range="0 0.8" damping="0.08"/>
<!-- 关节4：夹爪旋转（绕Z轴，末端旋转） -->
<body name="joint4_link" pos="0 0 0.15">
<geom name="joint4_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="darkblue"/>
<!-- 关节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"/>
<!-- 末端夹爪 -->
<body name="gripper_base" pos="0 0 0">
<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.05 0.05" pos="0 0 0" material="red"/>
<joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
</body>
<!-- 右夹爪 -->
<body name="right_gripper" pos="0 -0.1 0">
<geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" 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.5" 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>
<!-- 末端位置标记（绿色小球，用于反馈） -->
<geom name="end_effector_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<!-- 执行器配置（高精度位置控制） -->

<!-- 执行器配置（仅保留位置/速度控制，无传感器依赖） -->
<actuator>
<position name="joint1_act" joint="joint1" kp="1500" kv="150"/>
<position name="joint2_act" joint="joint2" kp="1500" kv="150"/>
<position name="joint3_act" joint="joint3" kp="1500" kv="150"/>
<position name="joint4_act" joint="joint4" kp="1500" kv="150"/>
<position name="left_grip_act" joint="left_grip_joint" kp="800" kv="80"/>
<position name="right_grip_act" joint="right_grip_joint" kp="800" kv="80"/>
<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"/>
</actuator>
</mujoco>
"""

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

# 3. 获取执行器索引
joint1_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act")
joint2_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
joint3_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act")
joint4_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act")
# 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"),
}
# 夹爪执行器索引
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. 获取末端执行器（绿色标记）的ID（用于位置反馈）
end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "end_effector_marker")
target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_geom")

# 5. 控制函数（平滑控制+末端反馈）
def smooth_set_joint(joint_idx, target_val, duration, viewer):
start_val = data.ctrl[joint_idx]
# 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取）
def smooth_joint_control(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_val = start_val + t * (target_val - start_val)
data.ctrl[joint_idx] = current_val
# 实时打印末端位置
print_end_effector_position(data, end_effector_id, target_id)
# 步进仿真
current_angle = start_angle + t * (target_angle - start_angle)
data.ctrl[idx] = current_angle
# 打印关节状态（无力度）
print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="")
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)

def smooth_set_gripper(target, duration, viewer):
start_left = data.ctrl[left_grip_idx]
start_right = data.ctrl[right_grip_idx]
target_right = -target
def safe_grasp(viewer):
"""安全抓取（通过时间控制闭合，模拟力控效果）"""
print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）")
grip_speed = -0.1 # 低速闭合，避免夹碎
start_time = time.time()
# 闭合1.5秒后停止（模拟力控阈值）
while time.time() - start_time < 1.5 and viewer.is_running():
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="")
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✅ 抓取完成（已停止闭合，防止夹碎）！")

def release_gripper(duration, viewer):
"""放松夹爪"""
print("\n🔧 开始放松夹爪")
start_time = time.time()
while (time.time() - start_time) < duration and viewer.is_running():
t = (time.time() - start_time) / duration
data.ctrl[left_grip_idx] = start_left + t * (target - start_left)
data.ctrl[right_grip_idx] = start_right + t * (target_right - start_right)
print_end_effector_position(data, end_effector_id, target_id)
data.ctrl[left_grip_idx] = 0.2 # 张开速度
data.ctrl[right_grip_idx] = -0.2
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.001)
print("✅ 夹爪已完全张开")

def print_end_effector_position(data, ee_id, tar_id):
# 获取末端和目标的位置
ee_pos = data.geom_xpos[ee_id]
tar_pos = data.geom_xpos[tar_id]
# 计算距离
distance = np.linalg.norm(ee_pos - tar_pos)
# 实时刷新打印（不换行）
print(
f"\r末端位置(X:{ee_pos[0]:.2f}, Y:{ee_pos[1]:.2f}, Z:{ee_pos[2]:.2f}) | 目标位置(X:{tar_pos[0]:.2f}, Y:{tar_pos[1]:.2f}, Z:{tar_pos[2]:.2f}) | 距离:{distance:.3f} m",
end="")

# 6. SCARA机械臂目标跟随流程
scara_steps = [
("关节1旋转对准目标", joint1_idx, 0.785, 2.5), # 45°旋转
("关节2旋转调整姿态", joint2_idx, -0.523, 2.0), # -30°旋转
("关节3升降接近目标", joint3_idx, 0.3, 1.8), # 下降接近目标
("关节4旋转校准方向", joint4_idx, 1.047, 2.0), # 60°旋转校准
("夹紧夹爪模拟抓取", "gripper", -0.4, 1.2), # 夹紧夹爪
("关节3升降抬升目标", joint3_idx, 0.6, 1.8), # 抬升
("关节1反向旋转归位", joint1_idx, 0.0, 2.5), # 归位旋转
("关节2反向旋转归位", joint2_idx, 0.0, 2.0), # 归位旋转
("关节3下降放置目标", joint3_idx, 0.3, 1.8), # 下降放置
("放松夹爪完成操作", "gripper", 0.0, 1.2), # 放松夹爪
("关节3升降归位", joint3_idx, 0.0, 1.8), # 最终归位
("关节4旋转归位", joint4_idx, 0.0, 2.0), # 最终归位
# 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°旋转
]

# 7. 启动仿真
# 6. 启动仿真（纯3.4.0原生逻辑）
with mujoco.viewer.launch_passive(model, data) as viewer:
print("\n📌 开始SCARA机械臂目标跟随流程...")
print("\n📌 开始7自由度协作机械臂抓取流程...")
print("-" * 60)

for step_name, joint_or_grip, target, duration in scara_steps:
# 第一步：关节运动对准目标
for step_name, joint_name, target_angle, duration in cobot_steps:
print(f"\n\n🔧 {step_name}")
if joint_or_grip == "gripper":
smooth_set_gripper(target, duration, viewer)
else:
smooth_set_joint(joint_or_grip, target, duration, viewer)
smooth_joint_control(joint_name, target_angle, duration, viewer)

# 第二步：安全抓取（模拟力控）
safe_grasp(viewer=viewer)

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

# 第四步：归位（关节1旋转回原位）
print("\n\n🔧 旋转归位")
smooth_joint_control("joint1", 0.0, 3.0, viewer)

# 第五步：下放目标
print("\n\n🔧 下放目标")
smooth_joint_control("joint2", 0.785, 2.5, viewer) # 45°俯仰下放

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

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

print("\n\n🎉 SCARA机械臂末端反馈+目标跟随演示完毕！")
print("\n\n🎉 7自由度协作机械臂抓取演示完毕！")


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