Mujoco入门：手把手教你Ubuntu安装、机械臂模型与末端位置追踪

mdkir MuJoCoBincd MuJoCoBinwget https://github.com/google-deepmind/mujoco/releases/download/3.3.0/mujoco-3.3.0-linux-x86_64.tar.gztar xvzf mujoco-3.3.0-linux-x86_64.tar.gz

echo"export PATH=$PATH:/home/dar/MuJoCoBin/mujoco-3.3.0/bin" >> ~/.bashrcsource ~/.bashrcsimulate ~/MuJoCoBin/mujoco-3.3.0/model/humanoid/humanoid.xml

pip3 install mujoco glfw

import mujocoimport numpy as npimport glfw# 定义鼠标滚轮滚动的回调函数，用于处理界面缩放defscroll_callback(window, xoffset, yoffset):# 使用 global 关键字声明 cam 为全局变量，以便在函数内部修改它global cam# 根据鼠标滚轮的垂直滚动量 yoffset 调整相机的距离，实现缩放效果# 0.1 是缩放的比例因子，可以根据需要调整cam.distance *= 1 - 0.1 * yoffsetdefmain():# 声明 cam 为全局变量，方便在其他函数中使用global cam# 从指定的 XML 文件路径加载 MuJoCo 模型model = mujoco.MjModel.from_xml_path('/home/dar/MuJoCoBin/mujoco_menagerie/franka_emika_panda/scene.xml')# 创建与模型对应的 MjData 实例，用于存储模拟过程中的动态数据data = mujoco.MjData(model)# 初始化 GLFW 库，用于创建窗口和处理输入事件ifnot glfw.init():# 如果初始化失败，直接返回return# 创建一个 1200x900 像素的窗口，标题为 'Panda Arm Control'window = glfw.create_window(1200, 900, 'Panda Arm Control', None, None)ifnot 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 IDend_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'hand')print(f"End effector ID: {end_effector_id}")if end_effector_id == -1:# 如果未找到指定名称的末端执行器，打印警告信息并终止 GLFWprint("Warning: Could not find the end effector with the given name.")glfw.terminate()return# 进入主循环，直到用户关闭窗口whilenot glfw.window_should_close(window):# 获取末端执行器在世界坐标系下的位置end_effector_pos = data.body(end_effector_id).xpos# 打印末端执行器的位置信息，方便调试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()

data.qpos[:7] = new_q

import mujocoimport numpy as npimport glfwdefscroll_callback(window, xoffset, yoffset):global cam# 调整相机的缩放比例cam.distance *= 1 - 0.1 * yoffsetdeflimit_angle(angle):while angle > np.pi:angle -= 2 * np.piwhile angle < -np.pi:angle += 2 * np.pireturn angledefmain():global cam# 加载模型model = mujoco.MjModel.from_xml_path('/home/dar/MuJoCoBin/mujoco_menagerie/franka_emika_panda/scene.xml')data = mujoco.MjData(model)# 打印所有 body 的 ID 和名称print("All bodies in the model:")for i in range(model.nbody):body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)print(f"ID: {i}, Name: {body_name}")# 初始化 GLFWifnot glfw.init():returnwindow = glfw.create_window(1200, 900, 'Panda Arm Control', None, None)ifnot window:glfw.terminate()returnglfw.make_context_current(window)# 设置鼠标滚轮回调函数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)# 找到末端执行器的 body idend_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'hand')print(f"End effector ID: {end_effector_id}")if end_effector_id == -1:print("Warning: Could not find the end effector with the given name.")glfw.terminate()return# 初始关节角度initial_q = data.qpos[:7].copy()whilenot glfw.window_should_close(window):# 获取当前末端执行器位置mujoco.mj_forward(model, data)end_effector_pos = data.body(end_effector_id).xposinitial_q[0] = initial_q[0] + 0.1initial_q[0] = limit_angle(initial_q[0])new_q = initial_q# 设置关节目标位置data.qpos[:7] = new_q# 模拟一步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()