datetime:2025/01/10 14:45

author:nzb

该项目来源于mujoco_learning

Ray

射线测距接口

void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec,
                 const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude,
                 int* geomid, mjtNum* dist, int nray, mjtNum cutoff);

• m: mjModel
• d: mjData
• pnt: 射线起点
• vec: 射线向量
• geomgroup：检测分组，NULL代表检测所有
• flg_static: 是否检测静态物体 1检测 0不检测
• bodyexclude: -1 可用于指示包括所有主体
• geomid: 检测到的几何体id，没有为-1
• dist: 到geom表面的距离，数据是距离比例，是vec的倍数，无限远为-1
• nray：射线数量
• cutoff: 距离截断，超过截断距离的射线不检测，判断条件(pnt到geompos中心>cutoff+几何体外接球半径)

其余ratXXX参数同理

代码

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/deep_ray.xml')
d = mujoco.MjData(m)

camID = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_CAMERA, "look_box")
cam_pos = m.cam_pos[camID*3:camID*3+3]
# 单射线
box_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_GEOM, "box1")
box2_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_GEOM, "box2")
# 多射线
box_num = 5
box_idx = np.zeros((box_num, 1), dtype=np.int32)
boxs_pos = np.zeros((box_num, 3))
for i in range(box_num):
  geom_name = "box" + str(i+1)
  box_idx[i] = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
  boxs_pos[i] = d.geom_xpos[box_idx[i]]

with mujoco.viewer.launch_passive(m, d) as viewer:
  start = time.time()
  while viewer.is_running():

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

    '''--------单射线--------'''
    box_pos = d.geom_xpos[box_id]
    box2_pos = d.geom_xpos[box2_id]
    vec1 = box_pos - cam_pos # [1, 3]
    vec2 = box2_pos - cam_pos
    vec1 = vec1.reshape(3, 1) # [3, 1]
    vec2 = vec2.reshape(3, 1)
    geomid = np.zeros((1,1), dtype=np.int32)
    geomgroup = np.ones((6, 1)) # all group
    pnt = cam_pos.reshape(3, 1)
    x1 = mujoco.mj_ray(m,d,pnt,vec1,geomgroup,1,-1,geomid)
    x2 = mujoco.mj_ray(m,d,pnt,vec2,geomgroup,1,-1,geomid)
    distance1 = mujoco.mju_norm3(vec1) * x1 # 长度 * 比例系数 = 距离
    distance2 = mujoco.mju_norm3(vec2) * x2
    # print(x1, x2)
    # print(distance1, distance2)
    '''--------单射线--------'''

    '''--------多射线--------'''
    for i in range(box_num):
      boxs_pos[i] = d.geom_xpos[box_idx[i]]
    num_vec = np.zeros((box_num*3,1))
    for i in range(box_num):
      for j in range(3):
        num_vec[i*3+j] = boxs_pos[i][j] - cam_pos[0][j]
    geomid = np.zeros((box_num,1), dtype=np.int32)
    geomgroup = np.ones((6, 1))
    dist = np.zeros((box_num,1))
    dist_ratio = np.zeros((box_num,1))
    pnt = cam_pos.reshape(3, 1)
    mujoco.mj_multiRay(m,d,pnt,num_vec,geomgroup,1,-1,geomid,dist_ratio,box_num,999)
    for i in range(box_num):
      if geomid[i] == -1:
        dist[i] = -1.0
        continue
      dist[i] = mujoco.mju_norm3(num_vec[i*3:i*3+3]) * dist_ratio[i]
    print(dist)
    '''--------多射线--------'''


    # 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)

<mujoco>
    <compiler angle="radian" meshdir="meshes" autolimits="true" />
    <!-- density="1.225" viscosity="1.8e-5" -->
    <option timestep="0.01" gravity="0 0 -9.81" integrator="implicitfast" />
    <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" />
    </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_box" mode="targetbody" target="box1" pos="0 0 0.2" />

        <body name="box1" pos="0 1 1">
            <freejoint />
            <geom name="box1" type="box" mass="1" size="0.2 0.2 0.2" rgba=".5 .4 .3 1" />
        </body>

        <body name="box2" pos="2 0 1">
            <freejoint />
            <geom name="box2" type="box" mass="1" size="0.2 0.2 0.2" rgba=".3 .4 .5 1" />
        </body>

        <body name="box3" pos="2 0 2">
            <freejoint />
            <geom name="box3" type="box" mass="1" size="0.2 0.2 0.2" rgba=".6 .5 .4 1" />
        </body>

        <body name="box4" pos="0 -2 1">
            <freejoint />
            <geom name="box4" type="box" mass="1" size="0.2 0.2 0.2" rgba=".3 .2 .1 1" />
        </body>

        <body name="box5" pos="-1 0 1">
            <freejoint />
            <geom name="box5" type="box" mass="1" size="0.2 0.2 0.2" rgba=".6 .7 .8 1" />
        </body>
    </worldbody>

</mujoco>