使用mujoco加载模型和控制、以及训练(二)

安装

上次已经复现了一个训练了,但是我们还没有深入它的控制里面,并且也没有尝试并行环境的训练的机制。
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站效果链接
在这里插入图片描述
后续我们将进一步学习相关知识,然后通过学习的范式来进行训练。

在人形机器人控制领域,零矩点(ZMP)原理是一种重要的控制理论。在MuJoCo中对humanoid模型进行基于ZMP的步态控制,具有一定的实际意义。 基于模型的技术里,基于ZMP原理的技术在人形机器人的基本运动任务(如行走、跑步跳跃等)中取得了显著成功,但这类方法通常难以推广或适应新的环境 [^1]。传统的控制方法中,ZMP控制是通过调节重心位置来维持平衡 [^2]。 在MuJoCo环境下对humanoid模型应用基于ZMP的步态控制,首先要在MuJoCo中构建精确的humanoid动力学模型,利用该模型模拟humanoid的运动。然后依据ZMP原理,通过计算调节模型的重心位置,使其在运动过程中满足ZMP的要求,以此实现稳定的步态。例如,在行走过程中,不断根据当前的运动状态传感器反馈,动态调整关节的运动,让ZMP始终保持在支撑面内,从而保证humanoid模型能够稳定行走。 以下是一个简单的伪代码示例,展示在MuJoCo中实现基于ZMP的步态控制的基本思路: ```python import mujoco # 加载MuJoCo模型 model = mujoco.MjModel.from_xml_path('humanoid.xml') data = mujoco.MjData(model) # 模拟循环 for _ in range(1000): # 计算当前ZMP current_zmp = calculate_zmp(data) # 判断ZMP是否在支撑面内 if not is_zmp_in_support_area(current_zmp, data): # 若不在,调整关节运动 adjust_joint_motion(data) # 进行一步模拟 mujoco.mj_step(model, data) def calculate_zmp(data): # 实现ZMP计算逻辑 pass def is_zmp_in_support_area(zmp, data): # 判断ZMP是否在支撑面内的逻辑 pass def adjust_joint_motion(data): # 调整关节运动的逻辑 pass ```
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

白云千载尽

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值