Skip to content
Merged
Show file tree
Hide file tree
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
43 changes: 43 additions & 0 deletions src/Embodied_Human_Arm_Modeling/arm_model.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
<!-- arm_model.xml -->
<mujoco model="仿人机械臂">
<option timestep="0.01"/>

<worldbody>
<!-- 世界坐标系中的基础光源 -->
<light pos="0 0 2" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 .9 .9 1"/>

<!-- 躯干基座 -->
<body name="torso" pos="0 0 0">
<geom type="box" size=".1 .1 .1" rgba=".7 .5 .3 1"/>
<joint type="free"/> <!-- 基座可以自由移动,方便观察 -->

<!-- 上臂 -->
<body name="upper_arm" pos="0 0 .15">
<geom type="capsule" fromto="0 0 0 0 0 .3" size="0.05" rgba="0.2 0.6 0.8 1"/>
<joint name="shoulder_pitch" type="hinge" pos="0 0 0" axis="1 0 0" range="-90 90"/>
<joint name="shoulder_yaw" type="hinge" pos="0 0 0" axis="0 1 0" range="-45 45"/>

<!-- 前臂 -->
<body name="forearm" pos="0 0 .3">
<geom type="capsule" fromto="0 0 0 0 0 .25" size="0.04" rgba="0.8 0.2 0.2 1"/>
<joint name="elbow" type="hinge" pos="0 0 0" axis="1 0 0" range="-120 0"/>

<!-- 简化手掌(作为一个整体) -->
<body name="hand" pos="0 0 .25">
<geom type="box" size=".04 .08 .02" rgba=".9 .9 .2 1"/>
<joint name="wrist" type="hinge" pos="0 0 0" axis="0 1 0" range="-30 30"/>
</body>
</body>
</body>
</body>
</worldbody>

<actuator>
<!-- 为每个关节定义位置伺服电机 -->
<motor name="motor_shoulder_pitch" joint="shoulder_pitch" gear="50"/>
<motor name="motor_shoulder_yaw" joint="shoulder_yaw" gear="30"/>
<motor name="motor_elbow" joint="elbow" gear="40"/>
<motor name="motor_wrist" joint="wrist" gear="10"/>
</actuator>
</mujoco>
59 changes: 59 additions & 0 deletions src/Embodied_Human_Arm_Modeling/simulate_arm.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
import os
import sys
os.environ['MUJOCO_GL'] = 'egl'
# 关键设置:获取当前脚本文件的绝对目录,并设置为工作目录
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
os.chdir(SCRIPT_DIR)
# simulate_arm.py
import mujoco
import mujoco.viewer
import numpy as np
import time

def main():
# 1. 加载模型
model = mujoco.MjModel.from_xml_path('arm_model.xml')
data = mujoco.MjData(model)

# 2. 设置初始关节位置(例如,让肘部稍微弯曲)
data.qpos[model.joint('elbow').id] = -np.radians(45) # -45度

# 3. 使用 viewer 进行交互式仿真
print("正在启动仿真查看器...")
print("提示:")
print(" - 在查看器窗口按 [Space] 暂停/继续物理仿真。")
print(" - 在左侧‘关节’面板,可以拖拽滑块实时驱动电机,观察手臂运动。")

with mujoco.viewer.launch(model, data) as viewer:
# 设置一个简单的摆动目标
target_positions = np.array([0.3, 0.1, -0.5, 0.0]) # 对应四个电机的目标位置
time_per_cycle = 5.0 # 摆动周期(秒)
start_time = time.time()

# 保持查看器窗口开启
while viewer.is_running():
step_start = time.time()

# 计算基于时间的周期性目标
t = time.time() - start_time
phase = 2 * np.pi * t / time_per_cycle
current_target = target_positions * np.sin(phase)

# 将目标位置设置为电机的控制信号(简单的位置控制)
data.ctrl[:] = current_target

# 执行一步物理仿真
mujoco.mj_step(model, data)

# 同步查看器
viewer.sync()

# 粗略的时间同步,避免占用过多CPU
time_until_next_step = model.opt.timestep - (time.time() - step_start)
if time_until_next_step > 0:
time.sleep(min(time_until_next_step, model.opt.timestep))

print("仿真结束。")

if __name__ == "__main__":
main()
Loading