安装
上次已经复现了一个训练了,但是我们还没有深入它的控制里面,并且也没有尝试并行环境的训练的机制。
https://github.com/google-deepmind/mujoco/releases
直接官网下载最新版的即可。


这样就算是下载好了,我们通过添加环境变量来指定环境,首先是外部新建一个变量然后写入

其次是Path中添加这个对应的环境位置。

最后我们测试它的效果,进入文件夹中打开cmd

bin\simulate.exe model\humanoid\humanoid.xml
打开后出现:

常见机械臂模型的使用
官方提供了很多好用的模型,我们直接下载即可。
git clone https://github.com/google-deepmind/mujoco_menagerie.git

常用模型示例如下。
simulate.exe arx_l5\arx_l5.xml
(注意,添加了环境变量过后就能直接输入simulate.exe了)

simulate.exe booster_t1\scene.xml

simulate.exe dynamixel_2r\dynamixel_2r.xml

simulate.exe franka_emika_panda\mjx_panda.xml

simulate.exe realsense_d435i\d435i.xml

simulate.exe unitree_go2\scene.xml

simulate.exe unitree_h1\scene.xml

simulate aloha/scene.xml
(我发现也不需要exe,直接simulate 就能够启动)

仿真末端执行器与可视化
对于mujoco来说,它的程序编写主要依赖于mujoco这个python方法(比较老的版本使用的是mujoco-py)
pip3 install mujoco glfw
我们通过python程序来启动一个模型,然后获取这个模型的关节信息
import mujoco
import numpy as np
import glfw
# 定义鼠标滚轮滚动的回调函数,用于处理界面缩放
def scroll_callback(window, xoffset, yoffset):
# 使用 global 关键字声明 cam 为全局变量,以便在函数内部修改它
global cam
# 根据鼠标滚轮的垂直滚动量 yoffset 调整相机的距离,实现缩放效果
# 0.1 是缩放的比例因子,可以根据需要调整
cam.distance *= 1 - 0.1 * yoffset
def main():
# 声明 cam 为全局变量,方便在其他函数中使用
global cam
# 你的模型安装位置
model = mujoco.MjModel.from_xml_path('D:\\EI\\mujoco\\mujoco_menagerie\\franka_emika_panda\\scene.xml')
# 创建与模型对应的 MjData 实例,用于存储模拟过程中的动态数据
data = mujoco.MjData(model)
# 初始化 GLFW 库,用于创建窗口和处理输入事件
if not glfw.init():
# 如果初始化失败,直接返回
return
# 创建一个 1200x900 像素的窗口,标题为 'Panda Arm Control'
window = glfw.create_window(1200, 900, 'Panda Arm Control', None, None)
if not window:
# 如果窗口创建失败,终止 GLFW 并返回
glfw.terminate()
return
# 将当前上下文设置为新创建的窗口,以便后续的 OpenGL 操作在该窗口上进行
glfw.make_context_current(window)
# 设置鼠标滚轮事件的回调函数为 scroll_callback,当鼠标滚轮滚动时会调用该函数
glfw.set_scroll_callback(window, scroll_callback)
# 初始化相机对象,用于定义观察视角
cam = mujoco.MjvCamera()
# 初始化渲染选项对象,用于设置渲染的一些参数
opt = mujoco.MjvOption()
# 设置相机的默认参数
mujoco.mjv_defaultCamera(cam)
# 设置渲染选项的默认参数
mujoco.mjv_defaultOption(opt)
# 初始化扰动对象,用于处理用户对模型的交互操作
pert = mujoco.MjvPerturb()
# 初始化渲染上下文对象,用于管理渲染资源
con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)
# 创建一个场景对象,用于存储要渲染的几何元素
scene = mujoco.MjvScene(model, maxgeom=10000)
# 根据名称 'hand' 查找末端执行器的 body ID
end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'hand')
if end_effector_id == -1:
# 如果未找到指定名称的末端执行器,打印警告信息并终止 GLFW
print("Warning: Could not find the end effector with the given name.")
glfw.terminate()
return
# 进入主循环,直到用户关闭窗口
while not glfw.window_should_close(window):
# 获取末端执行器在世界坐标系下的位置
end_effector_pos = data.body(end_effector_id).xpos
# 打印末端执行器的位置信息
print(f"End effector ID: {end_effector_id}")
print(f"End effector position: {end_effector_pos}")
# 执行一步模拟,更新模型的状态
mujoco.mj_step(model, data)
# 定义视口的大小和位置
viewport = mujoco.MjrRect(0, 0, 1200, 900)
# 更新场景对象,将模型的最新状态反映到场景中
mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene)
# 将场景渲染到视口中
mujoco.mjr_render(viewport, scene, con)
# 交换前后缓冲区,将渲染结果显示在窗口上
glfw.swap_buffers(window)
# 处理所有待处理的事件,如鼠标、键盘事件等
glfw.poll_events()
# 终止 GLFW 库,释放相关资源
glfw.terminate()
if __name__ == "__main__":
# 当脚本作为主程序运行时,调用 main 函数
main()
这里的glfw库主要是捕捉相机的操作来改变窗口的可视化信息的,然后mujoco主要加载环境信息,注意替换对应的mujoco.MjModel.from_xml_path(‘D:\EI\mujoco\mujoco_menagerie\franka_emika_panda\scene.xml’)的路径信息。


这个scene.xml文件主要包含了:
<mujoco model="panda scene">
<include file="panda.xml"/>
<statistic center="0.3 0 0.4" extent="1"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="120" elevation="-20"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
</worldbody>
</mujoco>
导入的内容主要是panda.xml
这里面只需要关注的是,带body的是主要的关节的部分.
<mujoco model="panda">
<compiler angle="radian" meshdir="assets" autolimits="true"/>
<option integrator="implicitfast"/>
<default>
<default class="panda">
<material specular="0.5" shininess="0.25"/>
<joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
<default class="finger">
<joint axis="0 1 0" type="slide" range="0 0.04"/>
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="collision">
<geom type="mesh" group="3"/>
<default class="fingertip_pad_collision_1">
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
</default>
<default class="fingertip_pad_collision_2">
<geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
</default>
<default class="fingertip_pad_collision_3">
<geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
</default>
<default class="fingertip_pad_collision_4">
<geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
</default>
<default class="fingertip_pad_collision_5">
<geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
</default>
</default>
</default>
</default>
<asset>
<material class="panda" name="white" rgba="1 1 1 1"/>
<material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
<material class="panda" name="black" rgba="0.25 0.25 0.25 1"/>
<material class="panda" name="green" rgba="0 1 0 1"/>
<material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>
<!-- Collision meshes -->
<mesh name="link0_c" file="link0.stl"/>
<mesh name="link1_c" file="link1.stl"/>
<mesh name="link2_c" file="link2.stl"/>
<mesh name="link3_c" file="link3.stl"/>
<mesh name="link4_c" file="link4.stl"/>
<mesh name="link5_c0" file="link5_collision_0.obj"/>
<mesh name="link5_c1" file="link5_collision_1.obj"/>
<mesh name="link5_c2" file="link5_collision_2.obj"/>
<mesh name="link6_c" file="link6.stl"/>
<mesh name="link7_c" file="link7.stl"/>
<mesh name="hand_c" file="hand.stl"/>
<!-- Visual meshes -->
<mesh file="link0_0.obj"/>
<mesh file="link0_1.obj"/>
<mesh file="link0_2.obj"/>
<mesh file="link0_3.obj"/>
<mesh file="link0_4.obj"/>
<mesh file="link0_5.obj"/>
<mesh file="link0_7.obj"/>
<mesh file="link0_8.obj"/>
<mesh file="link0_9.obj"/>
<mesh file="link0_10.obj"/>
<mesh file="link0_11.obj"/>
<mesh file="link1.obj"/>
<mesh file="link2.obj"/>
<mesh file="link3_0.obj"/>
<mesh file="link3_1.obj"/>
<mesh file="link3_2.obj"/>
<mesh file="link3_3.obj"/>
<mesh file="link4_0.obj"/>
<mesh file="link4_1.obj"/>
<mesh file="link4_2.obj"/>
<mesh file="link4_3.obj"/>
<mesh file="link5_0.obj"/>
<mesh file="link5_1.obj"/>
<mesh file="link5_2.obj"/>
<mesh file="link6_0.obj"/>
<mesh file="link6_1.obj"/>
<mesh file="link6_2.obj"/>
<mesh file="link6_3.obj"/>
<mesh file="link6_4.obj"/>
<mesh file="link6_5.obj"/>
<mesh file="link6_6.obj"/>
<mesh file="link6_7.obj"/>
<mesh file="link6_8.obj"/>
<mesh file="link6_9.obj"/>
<mesh file="link6_10.obj"/>
<mesh file="link6_11.obj"/>
<mesh file="link6_12.obj"/>
<mesh file="link6_13.obj"/>
<mesh file="link6_14.obj"/>
<mesh file="link6_15.obj"/>
<mesh file="link6_16.obj"/>
<mesh file="link7_0.obj"/>
<mesh file="link7_1.obj"/>
<mesh file="link7_2.obj"/>
<mesh file="link7_3.obj"/>
<mesh file="link7_4.obj"/>
<mesh file="link7_5.obj"/>
<mesh file="link7_6.obj"/>
<mesh file="link7_7.obj"/>
<mesh file="hand_0.obj"/>
<mesh file="hand_1.obj"/>
<mesh file="hand_2.obj"/>
<mesh file="hand_3.obj"/>
<mesh file="hand_4.obj"/>
<mesh file="finger_0.obj"/>
<mesh file="finger_1.obj"/>
</asset>
<worldbody>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="link0" childclass="panda">
<inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
<geom mesh="link0_0" material="off_white" class="visual"/>
<geom mesh="link0_1" material="black" class="visual"/>
<geom mesh="link0_2" material="off_white" class="visual"/>
<geom mesh="link0_3" material="black" class="visual"/>
<geom mesh="link0_4" material="off_white" class="visual"/>
<geom mesh="link0_5" material="black" class="visual"/>
<geom mesh="link0_7" material="white" class="visual"/>
<geom mesh="link0_8" material="white" class="visual"/>
<geom mesh="link0_9" material="black" class="visual"/>
<geom mesh="link0_10" material="off_white" class="visual"/>
<geom mesh="link0_11" material="white" class="visual"/>
<geom mesh="link0_c" class="collision"/>
<body name="link1" pos="0 0 0.333">
<inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
<joint name="joint1"/>
<geom material="white" mesh="link1" class="visual"/>
<geom mesh="link1_c" class="collision"/>
<body name="link2" quat="1 -1 0 0">
<inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
<joint name="joint2" range="-1.7628 1.7628"/>
<geom material="white" mesh="link2" class="visual"/>
<geom mesh="link2_c" class="collision"/>
<body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
<joint name="joint3"/>
<inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
<geom mesh="link3_0" material="white" class="visual"/>
<geom mesh="link3_1" material="white" class="visual"/>
<geom mesh="link3_2" material="white" class="visual"/>
<geom mesh="link3_3" material="black" class="visual"/>
<geom mesh="link3_c" class="collision"/>
<body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
<inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
<joint name="joint4" range="-3.0718 -0.0698"/>
<geom mesh="link4_0" material="white" class="visual"/>
<geom mesh="link4_1" material="white" class="visual"/>
<geom mesh="link4_2" material="black" class="visual"/>
<geom mesh="link4_3" material="white" class="visual"/>
<geom mesh="link4_c" class="collision"/>
<body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
<inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
<joint name="joint5"/>
<geom mesh="link5_0" material="black" class="visual"/>
<geom mesh="link5_1" material="white" class="visual"/>
<geom mesh="link5_2" material="white" class="visual"/>
<geom mesh="link5_c0" class="collision"/>
<geom mesh="link5_c1" class="collision"/>
<geom mesh="link5_c2" class="collision"/>
<body name="link6" quat="1 1 0 0">
<inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
<joint name="joint6" range="-0.0175 3.7525"/>
<geom mesh="link6_0" material="off_white" class="visual"/>
<geom mesh="link6_1" material="white" class="visual"/>
<geom mesh="link6_2" material="black" class="visual"/>
<geom mesh="link6_3" material="white" class="visual"/>
<geom mesh="link6_4" material="white" class="visual"/>
<geom mesh="link6_5" material="white" class="visual"/>
<geom mesh="link6_6" material="white" class="visual"/>
<geom mesh="link6_7" material="light_blue" class="visual"/>
<geom mesh="link6_8" material="light_blue" class="visual"/>
<geom mesh="link6_9" material="black" class="visual"/>
<geom mesh="link6_10" material="black" class="visual"/>
<geom mesh="link6_11" material="white" class="visual"/>
<geom mesh="link6_12" material="green" class="visual"/>
<geom mesh="link6_13" material="white" class="visual"/>
<geom mesh="link6_14" material="black" class="visual"/>
<geom mesh="link6_15" material="black" class="visual"/>
<geom mesh="link6_16" material="white" class="visual"/>
<geom mesh="link6_c" class="collision"/>
<body name="link7" pos="0.088 0 0" quat="1 1 0 0">
<inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
<joint name="joint7"/>
<geom mesh="link7_0" material="white" class="visual"/>
<geom mesh="link7_1" material="black" class="visual"/>
<geom mesh="link7_2" material="black" class="visual"/>
<geom mesh="link7_3" material="black" class="visual"/>
<geom mesh="link7_4" material="black" class="visual"/>
<geom mesh="link7_5" material="black" class="visual"/>
<geom mesh="link7_6" material="black" class="visual"/>
<geom mesh="link7_7" material="white" class="visual"/>
<geom mesh="link7_c" class="collision"/>
<body name="hand" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834">
<inertial mass="0.73" pos="-0.01 0 0.03" diaginertia="0.001 0.0025 0.0017"/>
<geom mesh="hand_0" material="off_white" class="visual"/>
<geom mesh="hand_1" material="black" class="visual"/>
<geom mesh="hand_2" material="black" class="visual"/>
<geom mesh="hand_3" material="white" class="visual"/>
<geom mesh="hand_4" material="off_white" class="visual"/>
<geom mesh="hand_c" class="collision"/>
<body name="left_finger" pos="0 0 0.0584">
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
<joint name="finger_joint1" class="finger"/>
<geom mesh="finger_0" material="off_white" class="visual"/>
<geom mesh="finger_1" material="black" class="visual"/>
<geom mesh="finger_0" class="collision"/>
<geom class="fingertip_pad_collision_1"/>
<geom class="fingertip_pad_collision_2"/>
<geom class="fingertip_pad_collision_3"/>
<geom class="fingertip_pad_collision_4"/>
<geom class="fingertip_pad_collision_5"/>
</body>
<body name="right_finger" pos="0 0 0.0584" quat="0 0 0 1">
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
<joint name="finger_joint2" class="finger"/>
<geom mesh="finger_0" material="off_white" class="visual"/>
<geom mesh="finger_1" material="black" class="visual"/>
<geom mesh="finger_0" class="collision"/>
<geom class="fingertip_pad_collision_1"/>
<geom class="fingertip_pad_collision_2"/>
<geom class="fingertip_pad_collision_3"/>
<geom class="fingertip_pad_collision_4"/>
<geom class="fingertip_pad_collision_5"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<tendon>
<fixed name="split">
<joint joint="finger_joint1" coef="0.5"/>
<joint joint="finger_joint2" coef="0.5"/>
</fixed>
</tendon>
<equality>
<joint joint1="finger_joint1" joint2="finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
</equality>
<actuator>
<general class="panda" name="actuator1" joint="joint1" gainprm="4500" biasprm="0 -4500 -450"/>
<general class="panda" name="actuator2" joint="joint2" gainprm="4500" biasprm="0 -4500 -450"
ctrlrange="-1.7628 1.7628"/>
<general class="panda" name="actuator3" joint="joint3" gainprm="3500" biasprm="0 -3500 -350"/>
<general class="panda" name="actuator4" joint="joint4" gainprm="3500" biasprm="0 -3500 -350"
ctrlrange="-3.0718 -0.0698"/>
<general class="panda" name="actuator5" joint="joint5" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
<general class="panda" name="actuator6" joint="joint6" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"
ctrlrange="-0.0175 3.7525"/>
<general class="panda" name="actuator7" joint="joint7" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
<!-- Remap original ctrlrange (0, 0.04) to (0, 255): 0.04 * 100 / 255 = 0.01568627451 -->
<general class="panda" name="actuator8" tendon="split" forcerange="-100 100" ctrlrange="0 255"
gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
</actuator>
<keyframe>
<key name="home" qpos="0 0 0 -1.57079 0 1.57079 -0.7853 0.04 0.04" ctrl="0 0 0 -1.57079 0 1.57079 -0.7853 255"/>
</keyframe>
<contact>
<exclude body1="link0" body2="link1"/>
</contact>
</mujoco>
一个模型我们最关注的是link和joint,这里一共有9个joint,7 个机械臂旋转关节 和 2 个手指滑动关节.
为了得到更加详细的内容,我们打印更多的相关的信息.
import mujoco
import numpy as np
import glfw
# 全局变量:相机和鼠标状态
cam = None
mouse_button_left = False
mouse_button_right = False
last_x, last_y = 0.0, 0.0
# 鼠标滚轮缩放回调
def scroll_callback(window, xoffset, yoffset):
global cam
cam.distance *= 1 - 0.1 * yoffset
cam.distance = max(cam.distance, 0.1) # 限制最小距离
# 鼠标按键回调(左键旋转/右键平移)
def mouse_button_callback(window, button, action, mods):
global mouse_button_left, mouse_button_right
if button == glfw.MOUSE_BUTTON_LEFT:
mouse_button_left = (action == glfw.PRESS)
elif button == glfw.MOUSE_BUTTON_RIGHT:
mouse_button_right = (action == glfw.PRESS)
# 鼠标移动回调(处理视角旋转和平移)
def cursor_pos_callback(window, xpos, ypos):
global last_x, last_y, cam, mouse_button_left, mouse_button_right
if last_x == 0.0 and last_y == 0.0:
last_x, last_y = xpos, ypos
return
dx = xpos - last_x
dy = ypos - last_y
last_x, last_y = xpos, ypos
# 左键旋转视角(方位角和仰角)
if mouse_button_left:
sensitivity = 0.005
cam.azimuth += dx * sensitivity
cam.elevation -= dy * sensitivity
cam.elevation = np.clip(cam.elevation, -np.pi/2 + 0.01, np.pi/2 - 0.01)
# 右键平移视角(瞄准点)
if mouse_button_right:
sensitivity = 0.001 * cam.distance
ax = np.cos(cam.azimuth)
az = np.sin(cam.azimuth)
cam.lookat[0] -= dx * sensitivity * ax
cam.lookat[1] += dx * sensitivity * az
cam.lookat[2] += dy * sensitivity
def main():
global cam, last_x, last_y
# 加载模型(替换为你的模型路径)
model = mujoco.MjModel.from_xml_path(
'D:\\EI\\mujoco\\mujoco_menagerie\\franka_emika_panda\\scene.xml'
)
data = mujoco.MjData(model)
# 初始化GLFW
if not glfw.init():
return
window = glfw.create_window(1200, 900, 'Panda Joint States', None, None)
if not window:
glfw.terminate()
return
glfw.make_context_current(window)
# 设置回调函数(鼠标交互)
glfw.set_scroll_callback(window, scroll_callback)
glfw.set_mouse_button_callback(window, mouse_button_callback)
glfw.set_cursor_pos_callback(window, cursor_pos_callback)
# 初始化相机和渲染参数
cam = mujoco.MjvCamera()
opt = mujoco.MjvOption()
mujoco.mjv_defaultCamera(cam)
mujoco.mjv_defaultOption(opt)
# 调整初始相机位置(适配Panda机械臂)
cam.distance = 1.5
cam.azimuth = 90 # 方位角(度)
cam.elevation = -30 # 仰角(度)
cam.lookat = [0.5, 0, 0.5] # 瞄准点(机械臂大致中心)
# 初始化渲染资源
con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)
scene = mujoco.MjvScene(model, maxgeom=10000)
pert = mujoco.MjvPerturb()
# 获取所有关节信息(名称、ID、对应的子身体ID)
joint_info = [] # 存储 (关节ID, 关节名称, 子身体ID)
for joint_id in range(model.njnt):
joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id)
child_body_id = model.jnt_bodyid[joint_id]
joint_info.append((joint_id, joint_name, child_body_id))
print(f"检测到 {len(joint_info)} 个关节:{[info[1] for info in joint_info]}\n")
# 主循环
while not glfw.window_should_close(window):
print("="*50)
print(f"仿真时间:{data.time:.2f}s")
for idx, (joint_id, joint_name, child_body_id) in enumerate(joint_info):
# 关节角度/位移
joint_angle = data.qpos[joint_id]
# 关节对应子身体位置
joint_pos = data.body(child_body_id).xpos
# 关节类型判断(仅用数值,无枚举引用)
joint_type = "旋转关节 (rad)" if model.jnt_type[joint_id] == 1 else "滑动关节 (m)"
# 打印信息
print(f"关节 {idx+1}:{joint_name}(ID: {joint_id})")
print(f" 类型:{joint_type}")
print(f" 角度/位移:{joint_angle:.4f}")
print(f" 子身体位置:[{joint_pos[0]:.4f}, {joint_pos[1]:.4f}, {joint_pos[2]:.4f}]\n")
# 单步仿真
mujoco.mj_step(model, data)
# 渲染
viewport = mujoco.MjrRect(0, 0, 1200, 900)
mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene)
mujoco.mjr_render(viewport, scene, con)
# 刷新窗口
glfw.swap_buffers(window)
glfw.poll_events()
glfw.terminate()
if __name__ == "__main__":
main()

以上就是一些导入和可视化的信息了.接下来我们看怎么控制这些关节的运动过程.
控制关节运动
要想控制关节运动,我们首先需要获取这个关节的信息,包括它是旋转关节或者滑动关节,这里控制时,我们直接通过位置进行控制,所以它的位置直接就会反映在控制量上.
import mujoco
import numpy as np
import glfw
# 全局变量:相机、关节控制参数
cam = None
# 关节目标位置(初始值为模型默认位置)
target_q = None
# 当前选中的关节(0~6对应joint1~7,7对应手指)
selected_joint = 0
# 控制步长(旋转关节:弧度/步;滑动关节:米/步)
rotate_step = 0.05
slide_step = 0.001
# 鼠标滚轮缩放回调
def scroll_callback(window, xoffset, yoffset):
global cam
cam.distance *= 1 - 0.1 * yoffset
cam.distance = max(cam.distance, 0.1)
# 键盘控制回调
def key_callback(window, key, scancode, action, mods):
global target_q, selected_joint, rotate_step, slide_step
if action != glfw.PRESS and action != glfw.REPEAT:
return
# 切换选中的关节(1~7对应joint1~7,8对应手指)
if key == glfw.KEY_1:
selected_joint = 0
print(f"选中关节:joint1(旋转关节)")
elif key == glfw.KEY_2:
selected_joint = 1
print(f"选中关节:joint2(旋转关节)")
elif key == glfw.KEY_3:
selected_joint = 2
print(f"选中关节:joint3(旋转关节)")
elif key == glfw.KEY_4:
selected_joint = 3
print(f"选中关节:joint4(旋转关节)")
elif key == glfw.KEY_5:
selected_joint = 4
print(f"选中关节:joint5(旋转关节)")
elif key == glfw.KEY_6:
selected_joint = 5
print(f"选中关节:joint6(旋转关节)")
elif key == glfw.KEY_7:
selected_joint = 6
print(f"选中关节:joint7(旋转关节)")
elif key == glfw.KEY_8:
selected_joint = 7
print(f"选中关节:手指(滑动关节)")
# 调整选中关节的目标位置
if selected_joint < 7: # 旋转关节(joint1~7)
if key == glfw.KEY_UP:
target_q[selected_joint] += rotate_step
elif key == glfw.KEY_DOWN:
target_q[selected_joint] -= rotate_step
# 限制旋转关节角度在模型定义的范围内
min_range, max_range = model.jnt_range[selected_joint]
target_q[selected_joint] = np.clip(target_q[selected_joint], min_range, max_range)
else: # 手指滑动关节(由actuator8控制)
if key == glfw.KEY_RIGHT: # 张开
target_q[7] += slide_step # finger_joint1
target_q[8] += slide_step # finger_joint2
elif key == glfw.KEY_LEFT: # 闭合
target_q[7] -= slide_step
target_q[8] -= slide_step
# 限制手指关节范围(0~0.04米,模型定义)
target_q[7] = np.clip(target_q[7], 0, 0.04)
target_q[8] = np.clip(target_q[8], 0, 0.04)
# 限制角度在[-π, π](可选,仅旋转关节)
def limit_angle(angle):
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
angle += 2 * np.pi
return angle
def main():
global cam, target_q, model
# 加载模型
model = mujoco.MjModel.from_xml_path(
'D:\\EI\\mujoco\\mujoco_menagerie\\franka_emika_panda\\scene.xml'
)
data = mujoco.MjData(model)
# 初始化关节目标位置(初始为模型默认位置)
target_q = data.qpos.copy()
# 打印关节信息(确认对应关系)
print("关节-执行器对应关系:")
for i in range(model.njnt):
joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i)
print(f"关节{i}:{joint_name},位置索引:{i}")
print(f"执行器数量:{model.nu}(0~6控制joint1~7,7控制手指)\n")
print("控制说明:")
print("1-7键:选中joint1-joint7;8键:选中手指")
print("上/下箭头:调整旋转关节角度;左/右箭头:调整手指开合\n")
# 初始化GLFW
if not glfw.init():
return
window = glfw.create_window(1200, 900, 'Panda Joint Control', None, None)
if not window:
glfw.terminate()
return
glfw.make_context_current(window)
# 设置回调(鼠标缩放+键盘控制)
glfw.set_scroll_callback(window, scroll_callback)
glfw.set_key_callback(window, key_callback)
# 初始化渲染器
cam = mujoco.MjvCamera()
opt = mujoco.MjvOption()
mujoco.mjv_defaultCamera(cam)
mujoco.mjv_defaultOption(opt)
# 调整初始视角(方便观察机械臂)
cam.distance = 1.5
cam.azimuth = 90
cam.elevation = -30
cam.lookat = [0.5, 0, 0.5]
pert = mujoco.MjvPerturb()
con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)
scene = mujoco.MjvScene(model, maxgeom=10000)
# 主循环
while not glfw.window_should_close(window):
# 更新控制信号:将目标位置映射到执行器(位置控制)
# 前7个执行器对应joint1~7
data.ctrl[:7] = target_q[:7]
# 第8个执行器控制手指(模型中通过tendon联动,映射为0~255的控制范围)
# 手指关节范围0~0.04m,对应执行器ctrl范围0~255(XML中定义)
finger_ctrl = (target_q[7] / 0.04) * 255 # 转换为0~255
data.ctrl[7] = finger_ctrl
# 单步仿真
mujoco.mj_step(model, data)
# 打印当前关节状态(可选,方便调试)
print(f"\r选中关节{selected_joint},当前位置:{target_q[selected_joint]:.4f}", end="")
# 渲染场景
viewport = mujoco.MjrRect(0, 0, 1200, 900)
mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene)
mujoco.mjr_render(viewport, scene, con)
# 刷新窗口
glfw.swap_buffers(window)
glfw.poll_events()
glfw.terminate()
if __name__ == "__main__":
main()
首先是选择要控制哪一个关节,然后控制它的实际位置是哪里.
旋转关节(joint1~7):限制在模型定义的 range 范围内(如 joint1 范围 -2.8973~2.8973 rad);
手指关节:限制在 0~0.04 m(闭合到完全张开)。


moveit+mujoco联合仿真
安装moveit
对于机械臂来说,它的逆运动学计算是非常常见的需求,所以我们这里也联动一下,通过moveit来进行逆运动学解算,并且同时控制mujoco中的模型.
对于moveit的ros1 noetic版本的安装,参考之前写的https://blog.youkuaiyun.com/hooksten/article/details/154026427?spm=1011.2124.3001.6209
安装mujoco
不过要想使用联合仿真,就需要在linux下使用了.基本原理是类似的,不过我们的bashrc需要重新进行配置.
下载了对应的压缩包之后,向环境变量中添加以下内容:

export PATH=$PATH:/home/cyun/app/mujoco-3.3.7-linux-x86_64/mujoco-3.3.7/bin
即可使用.
联合仿真
接下来我们启动一个节点,控制mujoco模型并且订阅相关的ros话题的消息.
import rospy
from sensor_msgs.msg import JointState
import mujoco
import numpy as np
import glfw
class JointStateSubscriber:
def __init__(self):
# ROS 1 节点初始化(替换 rclpy Node)
rospy.init_node("joint_state_subscriber", anonymous=True)
# ROS 1 订阅者(替换 rclpy Subscription)
self.subscription = rospy.Subscriber(
"/joint_states",
JointState,
self.callback,
queue_size=10
)
# --------------------------
# MuJoCo 3.3.7 初始化(核心适配)
# --------------------------
# 加载模型(确保路径正确)
self.model = mujoco.MjModel.from_xml_path(
'/home/cyun/app/mujoco-3.3.7-linux-x86_64/mujoco_menagerie/franka_emika_panda/scene.xml'
)
self.data = mujoco.MjData(self.model)
# 打印所有 Body 信息(MuJoCo 3.3.7 接口不变)
rospy.loginfo("All bodies in the model:")
for i in range(self.model.nbody):
body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, i)
rospy.loginfo(f"Body ID: {i}, Name: {body_name}")
# --------------------------
# GLFW 窗口初始化(保持交互功能)
# --------------------------
if not glfw.init():
rospy.logerr("GLFW initialization failed!")
return
self.window = glfw.create_window(1200, 900, 'Panda Arm Control (ROS1)', None, None)
if not self.window:
glfw.terminate()
rospy.logerr("GLFW window creation failed!")
return
glfw.make_context_current(self.window)
# 鼠标滚轮缩放回调
glfw.set_scroll_callback(self.window, self.scroll_callback)
# --------------------------
# MuJoCo 3.3.7 渲染器初始化(关键API适配)
# --------------------------
self.cam = mujoco.MjvCamera() # 相机对象
self.opt = mujoco.MjvOption() # 渲染选项
mujoco.mjv_defaultCamera(self.cam)
mujoco.mjv_defaultOption(self.opt)
# 适配 MuJoCo 3.3.7:渲染上下文用 MjrContext(非 MjContext)
self.con = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150.value)
self.scene = mujoco.MjvScene(self.model, maxgeom=10000) # 场景对象
self.pert = mujoco.MjvPerturb() # 扰动对象(用于交互)
# 调整初始相机视角(适配 Panda 机械臂,3.3.7 用 lookat 而非 target)
self.cam.distance = 1.5 # 相机距离模型的距离
self.cam.azimuth = 90.0 # 方位角(度)
self.cam.elevation = -30.0 # 仰角(度,负值向下看)
self.cam.lookat = [0.5, 0, 0.5] # 相机瞄准点(模型中心)
# --------------------------
# 末端执行器与关节初始化
# --------------------------
# 查找末端执行器 Body ID(名称为 'hand',需与模型一致)
self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'hand')
if self.end_effector_id == -1:
rospy.logwarn("Could not find end effector 'hand'!")
glfw.terminate()
return
rospy.loginfo(f"End effector ID: {self.end_effector_id}")
# 初始关节角度(从 MuJoCo 模型默认状态获取)
self.initial_q = self.data.qpos[:7].copy()
rospy.loginfo(f"Initial joint positions: {self.initial_q}")
# 初始化 ROS 1 定时器(替换 rclpy Timer,周期 0.01s = 100Hz)
self.timer = rospy.Timer(
rospy.Duration(0.01),
self.timer_callback1
)
# 关节位置缓存(避免未订阅到消息时的空值错误)
self.positions = None
# ROS 1 关闭回调(确保 GLFW 资源清理)
rospy.on_shutdown(self.shutdown_hook)
# --------------------------
# 辅助函数(角度限制、鼠标交互)
# --------------------------
def scroll_callback(self, window, xoffset, yoffset):
# 鼠标滚轮缩放:调整相机距离
self.cam.distance *= 1 - 0.1 * yoffset
self.cam.distance = max(self.cam.distance, 0.1) # 限制最小距离
def limit_angle(self, angle):
# 限制角度在 [-π, π] 范围内
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
angle += 2 * np.pi
return angle
# --------------------------
# ROS 1 回调函数(订阅关节状态)
# --------------------------
def callback(self, msg: JointState):
# 过滤 Panda 机械臂的 7 个关节(名称含 "panda_joint",需与话题匹配)
panda_joint_indices = [i for i, name in enumerate(msg.name) if "panda_joint" in name]
if len(panda_joint_indices) == 7: # 确保只获取 7 个臂关节
self.positions = [msg.position[i] for i in panda_joint_indices]
# 打印关节状态(ROS 1 用 rospy.loginfo 替代 get_logger)
for i, idx in enumerate(panda_joint_indices):
rospy.loginfo(f"{msg.name[idx]}: {self.positions[i]:.4f}")
else:
rospy.logwarn(f"Received {len(panda_joint_indices)} joint(s), expected 7!")
# --------------------------
# 定时器回调(MuJoCo 仿真与渲染)
# --------------------------
def timer_callback1(self, event):
# 检查窗口是否需要关闭
if glfw.window_should_close(self.window):
rospy.signal_shutdown("Window closed by user")
return
# 确保已订阅到关节状态(避免空值赋值)
if self.positions is None:
rospy.logdebug("Waiting for joint states...")
# 用初始角度暂代(避免模型静止)
new_q = self.initial_q
self.initial_q[0] = self.limit_angle(self.initial_q[0] + 0.01) # 轻微旋转
else:
# 使用 ROS 订阅到的关节角度
new_q = self.positions
# --------------------------
# MuJoCo 3.3.7 仿真与渲染(核心步骤)
# --------------------------
# 1. 设置关节目标位置
self.data.qpos[:7] = new_q # 前 7 个为臂关节
# 2. 单步仿真(更新模型状态)
mujoco.mj_step(self.model, self.data)
# 3. 计算末端执行器位置(MuJoCo 3.3.7 用 data.body(id).xpos)
mujoco.mj_forward(self.model, self.data) # 确保状态更新
end_effector_pos = self.data.body(self.end_effector_id).xpos
rospy.logdebug(f"End effector pos: {end_effector_pos}")
# 4. 更新渲染场景
viewport = mujoco.MjrRect(0, 0, 1200, 900) # 视口大小
mujoco.mjv_updateScene(
self.model, self.data, self.opt, self.pert,
self.cam, mujoco.mjtCatBit.mjCAT_ALL.value, self.scene
)
# 5. 渲染到窗口
mujoco.mjr_render(viewport, self.scene, self.con)
# 6. 交换缓冲区(显示渲染结果)
glfw.swap_buffers(self.window)
# 7. 处理 GLFW 事件(鼠标、键盘)
glfw.poll_events()
# --------------------------
# ROS 1 关闭钩子(清理资源)
# --------------------------
def shutdown_hook(self):
rospy.loginfo("Shutting down... Cleaning GLFW resources")
glfw.terminate() # 确保 GLFW 窗口关闭
def main():
try:
# 初始化节点并阻塞运行
subscriber = JointStateSubscriber()
rospy.spin() # ROS 1 自旋(替代 rclpy.spin)
except rospy.ROSInterruptException:
rospy.loginfo("Node interrupted by user")
finally:
# 额外保险:确保 GLFW 清理
if 'subscriber' in locals() and hasattr(subscriber, 'window'):
glfw.terminate()
if __name__ == "__main__":
main()
注意修改对应的xml的位置,我们这里主要对机械臂的7个关节进行控制。启动过后再运行moveit:
roslaunch panda_moveit_config demo.launch

设置终端位置,进行规划并且执行,然后可以发现是同步进行的:
b站效果链接

后续我们将进一步学习相关知识,然后通过学习的范式来进行训练。
1015

被折叠的 条评论
为什么被折叠?



