datetime:2025/01/10 14:45
author:nzb

该项目来源于mujoco_learning

可视化及渲染

可视化配置

mjtVisFlag对应mjvOption.flags中的设置,在simulate中对应如下配置

可以使用opt.flags[mjtVisFlag::mjVIS_XXX]=true开启功能

typedef enum mjtVisFlag_ {        // flags enabling model element visualization
  mjVIS_CONVEXHULL    = 0,        // mesh convex hull
  mjVIS_TEXTURE,                  // textures
  mjVIS_JOINT,                    // joints
  mjVIS_CAMERA,                   // cameras
  mjVIS_ACTUATOR,                 // actuators
  mjVIS_ACTIVATION,               // activations
  mjVIS_LIGHT,                    // lights
  mjVIS_TENDON,                   // tendons
  mjVIS_RANGEFINDER,              // rangefinder sensors
  mjVIS_CONSTRAINT,               // point constraints
  mjVIS_INERTIA,                  // equivalent inertia boxes
  mjVIS_SCLINERTIA,               // scale equivalent inertia boxes with mass
  mjVIS_PERTFORCE,                // perturbation force
  mjVIS_PERTOBJ,                  // perturbation object
  mjVIS_CONTACTPOINT,             // contact points
  mjVIS_ISLAND,                   // constraint islands
  mjVIS_CONTACTFORCE,             // contact force
  mjVIS_CONTACTSPLIT,             // split contact force into normal and tangent
  mjVIS_TRANSPARENT,              // make dynamic geoms more transparent
  mjVIS_AUTOCONNECT,              // auto connect joints and body coms
  mjVIS_COM,                      // center of mass
  mjVIS_SELECT,                   // selection point
  mjVIS_STATIC,                   // static bodies
  mjVIS_SKIN,                     // skin
  mjVIS_FLEXVERT,                 // flex vertices
  mjVIS_FLEXEDGE,                 // flex edges
  mjVIS_FLEXFACE,                 // flex element faces
  mjVIS_FLEXSKIN,                 // flex smooth skin (disables the rest)
  mjVIS_BODYBVH,                  // body bounding volume hierarchy
  mjVIS_FLEXBVH,                  // flex bounding volume hierarchy
  mjVIS_MESHBVH,                  // mesh bounding volume hierarchy
  mjVIS_SDFITER,                  // iterations of SDF gradient descent

  mjNVISFLAG                      // number of visualization flags
} mjtVisFlag;

mjvOption.label使用mjtLabel显示对应标签

mjvOption.frame使用mjtFrame显示对应坐标系

mjvOption中还可以设置分组的可视化和bvh树深度

完整定义:

struct mjvOption_ {                  // abstract visualization options
  int      label;                    // what objects to label (mjtLabel)
  int      frame;                    // which frame to show (mjtFrame)
  mjtByte  geomgroup[mjNGROUP];      // geom visualization by group
  mjtByte  sitegroup[mjNGROUP];      // site visualization by group
  mjtByte  jointgroup[mjNGROUP];     // joint visualization by group
  mjtByte  tendongroup[mjNGROUP];    // tendon visualization by group
  mjtByte  actuatorgroup[mjNGROUP];  // actuator visualization by group
  mjtByte  flexgroup[mjNGROUP];      // flex visualization by group
  mjtByte  skingroup[mjNGROUP];      // skin visualization by group
  mjtByte  flags[mjNVISFLAG];        // visualization flags (indexed by mjtVisFlag)
  int      bvh_depth;                // depth of the bounding volume hierarchy to be visualized
  int      flex_layer;               // element layer to be visualized for 3D flex
};
typedef struct mjvOption_ mjvOption;

场景渲染

mjtRndFlag作用于mjvScene.flags,对应simulate中如下配置

typedef enum mjtRndFlag_ {        // flags enabling rendering effects
  mjRND_SHADOW        = 0,        // shadows
  mjRND_WIREFRAME,                // wireframe
  mjRND_REFLECTION,               // reflections
  mjRND_ADDITIVE,                 // additive transparency
  mjRND_SKYBOX,                   // skybox
  mjRND_FOG,                      // fog
  mjRND_HAZE,                     // haze
  mjRND_SEGMENT,                  // segmentation with random color
  mjRND_IDCOLOR,                  // segmentation with segid+1 color
  mjRND_CULL_FACE,                // cull backward faces

  mjNRNDFLAG                      // number of rendering flags
} mjtRndFlag;

图像分割

mjRND_SEGMENT and mjRND_IDCOLOR

mjtRndFlagmjRND_SEGMENT是随机颜色分割物体,

mjRND_IDCOLOR是通过设置mjvGeom.segid固定物体分割颜色 随机分割效果:

mujoco源码中segid映射到rgba见src/render/render_gl3.c如下

使用segid分割:

单/双目渲染

mjtStereo作用于mjvScene.stereo

分别是单目,四缓冲,并排,可以直接使用mjSTEREO_SIDEBYSIDE显示双目,mjSTEREO_QUADBUFFERED则需要更好一些的GPU

typedef enum mjtStereo_ {         // type of stereo rendering
  mjSTEREO_NONE       = 0,        // no stereo; use left eye only
  mjSTEREO_QUADBUFFERED,          // quad buffered; revert to side-by-side if no hardware support
  mjSTEREO_SIDEBYSIDE             // side-by-side
} mjtStereo;

演示:

代码

import time
import math

import mujoco
import mujoco.viewer
import cv2
import glfw
import numpy as np

m = mujoco.MjModel.from_xml_path('../../API-MJCF/vis_cfg.xml')
d = mujoco.MjData(m)

glfw.init()
glfw.window_hint(glfw.VISIBLE,glfw.FALSE)
window = glfw.create_window(1920,1080,"mujoco",None,None)
glfw.make_context_current(window)
#创建相机
camera = mujoco.MjvCamera()
camID = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_CAMERA, "look_bunny")#look_bunny bunny_eyes
camera.fixedcamid = camID
camera.type = mujoco.mjtCamera.mjCAMERA_FIXED 
scene = mujoco.MjvScene(m, maxgeom=2000)
context = mujoco.MjrContext(m, mujoco.mjtFontScale.mjFONTSCALE_150)

def get_image(w,h,stereo=mujoco.mjtStereo.mjSTEREO_NONE):
    before_stereo = scene.stereo
    scene.stereo = stereo
    viewport = mujoco.MjrRect(0, 0, w, h)
    # 更新场景
    mujoco.mjv_updateScene(
        m, d, mujoco.MjvOption(), 
        None, camera, mujoco.mjtCatBit.mjCAT_ALL, scene
    )
    '''--------设置分割颜色--------'''
    for i in range(scene.ngeom):
        geom = scene.geoms[i]
        if geom.objid == bunny_id and geom.objtype == mujoco.mjtObj.mjOBJ_GEOM:
            r = 254
            g = 0
            b = 255
            geom.segid = b << 16 | g << 8 | r
    '''--------设置分割颜色--------'''
    # 渲染到缓冲区
    mujoco.mjr_render(viewport, scene, context)
    scene.stereo = before_stereo
    # 读取 RGB 数据(格式为 HWC, uint8)
    rgb = np.zeros((h, w, 3), dtype=np.uint8)
    mujoco.mjr_readPixels(rgb, None, viewport, context)
    cv_image = cv2.cvtColor(np.flipud(rgb), cv2.COLOR_RGB2BGR)
    return cv_image


with mujoco.viewer.launch_passive(m, d) as viewer:
  '''--------可视化配置--------'''
  viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CAMERA] = True
  '''--------可视化配置--------'''

  '''--------场景渲染--------'''
  # scene.flags[mujoco.mjtRndFlag.mjRND_WIREFRAME] = True
  scene.flags[mujoco.mjtRndFlag.mjRND_SEGMENT] = True #随机颜色分割
  scene.flags[mujoco.mjtRndFlag.mjRND_IDCOLOR] = True #segid
  bunny_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_GEOM, "bunny")
  '''--------场景渲染--------'''

  # Close the viewer automatically after 30 wall-seconds.
  start = time.time()
  while viewer.is_running():

    step_start = time.time()
    mujoco.mj_step(m, d)

    img = get_image(1024,640,mujoco.mjtStereo.mjSTEREO_SIDEBYSIDE)
    # img = get_image(1024,640)
    cv2.imshow("img",img)
    cv2.waitKey(1)


    # Pick up changes to the physics state, apply perturbations, update options from GUI.
    viewer.sync()

    # Rudimentary time keeping, will drift relative to wall clock.
    time_until_next_step = m.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
      time.sleep(time_until_next_step)
<?xml version="1.0" encoding="utf-8"?>
<mujoco model="inverted_pendulum">
    <compiler angle="radian" autolimits="true" />
    <option timestep="0.01" gravity="0 0 -9.81" integrator="implicitfast" density="1.225"
        viscosity="1.8e-5" />

    <visual>
        <global realtime="1" ipd="0.068" />
        <quality shadowsize="16384" numslices="28" offsamples="4" />
        <headlight diffuse="1 1 1" specular="0.5 0.5 0.5" active="1" />
        <rgba fog="0 0 0 1" haze="1 1 1 1" />
    </visual>

    <asset>
        <mesh name="tetrahedron" vertex="0 0 0 1 0 0 0 1 0 0 0 1" />
        <texture type="skybox" file="../MJCF/asset/desert.png"
            gridsize="3 4" gridlayout=".U..LFRB.D.." />
        <texture name="plane" type="2d" builtin="checker" rgb1=".1 .1 .1" rgb2=".9 .9 .9"
            width="512" height="512" mark="cross" markrgb=".8 .8 .8" />
        <material name="plane" reflectance="0.3" texture="plane" texrepeat="1 1" texuniform="true" />
        <material name="box" rgba="0 0.5 0 1" emission="0" />
        <mesh file="asset/bunny.obj" />
    </asset>

    <worldbody>
        <geom name="floor" pos="0 0 0" size="0 0 .25" type="plane" material="plane"
            condim="3" />
        <light directional="true" ambient=".3 .3 .3" pos="30 30 30" dir="0 -.5 -1"
            diffuse=".5 .5 .5" specular=".5 .5 .5" />

        <camera name="look_bunny" pos="0 -1 1" euler="0 0 0" mode="targetbody" target="bunny" ipd="0.068"/>

        <body name="bunny" pos="0 -0.2 .1" euler="1.57 0 0">
            <freejoint />
            <geom name="bunny" type="mesh" mesh="bunny"/>
            <camera name="bunny_eyes" pos="-0.075 0.13 0" euler="0 3.14 0" mode="fixed" ipd="0.068" focalpixel="1280 1024" sensorsize="2 2" resolution="1280 1024"/>
        </body>

        <body pos="-1 0 .5">
            <freejoint />
            <geom type="sphere" size="0.1" rgba=".5 0 0 1" />
        </body>
        <body pos="-0.5 0 .5">
            <freejoint />
            <geom type="box" size="0.1 0.1 0.1" material="box" />
        </body>
        <body pos="0 0 .5">
            <freejoint />
            <geom type="capsule" size="0.1 0.1" rgba="0 0 .5 1" />
        </body>
        <body pos=".5 0 .5">
            <freejoint />
            <geom type="cylinder" size="0.1 0.1" rgba=".5 .5 0 1" />
        </body>
        <body pos="1 0 .5">
            <freejoint />
            <geom type="ellipsoid" size="0.2 0.2 0.1" rgba="0 .5 .5 1" />
        </body>
        <body pos="1.5 0 .5">
            <freejoint />
            <geom type="ellipsoid" size="0.2 0.1 0.1" rgba=".5 0 .5 1" />
        </body>

        <body pos="2.0 0 .5">
            <freejoint />
            <geom type="mesh" mesh="tetrahedron" rgba=".5 .5 .5 1" />
        </body>
    </worldbody>
</mujoco>

results matching ""

    No results matching ""