Mujoco xml ＜ sensor＞

< sensor>

jointpos
jointvel
jointactuatorfrc
framequat
gyro
accelerometer
framepos
framelinvel
touch
objtype="site" objname="imu" 和site="imu"的区别
python中与sensor有关的写法
- 传感器名字索引第几个id
- id索引传感器名字
- 传感器数量
- sensor中的顺序

jointpos

jointpos 是 MuJoCo 内置传感器类型之一，用于测量关节的位置

对于旋转关节（hinge）：输出一个标量,单位为弧度。(一般情况)
对于滑动关节（slide）：输出一个标量, 单位为米。
对于球关节（ball）：输出为四元数 [w, x, y, z]表示的旋转，维度为 4。
对于自由关节（free）：输出为 7维（3维位置 + 4维姿态四元数）数据 [x, y, z, qw, qx, qy, qz],表示位置和姿态。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><joint name="joint1" type="hinge" axis="0 0 1"/><geom type="capsule" size="0.05 0.1"/></body></worldbody><sensor><jointpos name="joint1_pos_sensor" joint="joint1"/></sensor>
</mujoco>
<!-- joint="joint1"：指定这个传感器测量 joint1 的位置。 -->
<!-- name="joint1_pos_sensor"：这个传感器的名称，可用于在Python中查找 -->

不涉及 site、body、geom、objtype、objname 等其他对象类型，因为它工作于关节级别

jointvel

jointvel 是用于测量关节速度的传感器。

输出的是该关节的瞬时速度：
对于旋转关节（hinge），单位是弧度/秒。
对于滑动关节（slide），单位是米/秒。
对于球关节（ball），输出的是 3维角速度。
对于自由关节（free），输出的是 6维（3维线速度 + 3维角速度）。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><joint name="joint1" type="hinge" axis="0 0 1"/><geom type="capsule" size="0.05 0.1"/></body></worldbody><sensor><jointvel name="joint1_vel_sensor" joint="joint1"/></sensor>
</mujoco>

jointvel 也不会使用 objname、objtype、site，它只针对关节，不针对几何体、site、body等

jointactuatorfrc

jointactuatorfrc 是 MuJoCo 中的一个传感器类型，用于测量作用在关节上的控制力或控制矩。
这个力/矩是驱动器（actuator）施加在关节上的效果，它只适用于那些与关节相关联的 actuator，如：motor（电机），position，velocity 控制器等。

对于旋转关节（hinge）：输出的是力矩，单位是牛顿·米（Nm）。
对于滑动关节（slide）：输出的是线性力，单位是牛顿（N）。
对于多自由度关节（ball、free）：每个自由度会有一个相应的力或力矩分量输出。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><joint name="joint1" type="hinge" axis="0 0 1"/><geom type="capsule" size="0.05 0.1"/></body></worldbody><actuator><motor joint="joint1" ctrlrange="-1 1"/></actuator><sensor><jointactuatorfrc name="joint1_frc_sensor" joint="joint1"/></sensor>
</mujoco>

# 假设仿真过程中 joint1 的驱动器作用力矩为 0.5 Nm
mj_data.sensordata = [0.5]

jointactuatorfrc 仅与关节相关，不依赖 site、body、geom 等对象。所以不会使用 objtype，不会使用objname，不会使用 site。

framequat

framequat 是用于测量某个对象（site、body、geom）的姿态（方向）的传感器。
它的输出是该对象当前的姿态，以四元数 [w, x, y, z] 的形式表示。

framequat 需要指定测量哪个对象的姿态，所以它必须使用：objtype：对象类型（site、body、geom），objname：对象的名称。不用写joint，不是作用到joint上的。
objtype 值是什么，就测量什么的姿态，比如site，body，geom。

objtype 和 objname 的用途：framequat 必须指定哪个对象的姿态。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><geom type="box" size="0.1 0.1 0.1"/><site name="imu_site" pos="0 0 0.1" size="0.01"/></body></worldbody><sensor><framequat name="imu_quat_sensor" objtype="site" objname="imu_site"/></sensor>
</mujoco>

mj_data.sensordata[start_idx : start_idx+4] = [0.707, 0, 0.707, 0]
# 表示该 site 当前的方向为绕 y 轴旋转 90 度。

gyro

gyro 是 MuJoCo 用于模拟陀螺仪的传感器。
它测量的是某个对象的角速度，即该对象在其局部坐标系下绕 x, y, z 轴的旋转速度[ωx, ωy, ωz]。单位是弧度/秒。

gyro 只支持 site，不使用 objtype 和 objname，不能对 body 或 geom 直接测量 gyro（但可以创建 site 附着在 body 或 geom 上）

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><geom type="box" size="0.1 0.1 0.1"/><site name="imu_site" pos="0 0 0.1" size="0.01"/></body></worldbody><sensor><gyro name="imu_gyro_sensor" site="imu_site"/></sensor>
</mujoco>

accelerometer

accelerometer 是 MuJoCo 中用于模拟加速度计的传感器。

它测量的是某个对象（通常是 site）在局部坐标系下的线加速度[ax, ay, az]。这个加速度包含：线性运动的加速度；重力分量（默认是带重力的加速度，除非设置排除）。

accelerometer 不使用 objtype 和 objname。只能通过 site 定位测量点。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><geom type="box" size="0.1 0.1 0.1"/><site name="imu_site" pos="0 0 0.1" size="0.01"/></body></worldbody><sensor><accelerometer name="imu_acc_sensor" site="imu_site"/></sensor>
</mujoco>

mj_data.sensordata[start_idx : start_idx+3] = [0.0, 0.0, 9.81]

framepos

framepos 是 MuJoCo 用于测量某个对象（site、body、geom）在世界坐标系下的位置的传感器。它输出的是该对象参考系的三维平移位置，单位是米（m）。适用于跟踪某物体的位置，比如 IMU 位置、物体质心位置等。表示该对象（site、body、geom）在世界坐标系下的 x, y, z 位置。不能用于不能用于关节joint。
对于body就是测量某个刚体的质心位置。

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><geom type="box" size="0.1 0.1 0.1"/><site name="imu_site" pos="0 0 0.1" size="0.01"/></body></worldbody><sensor><framepos name="imu_pos_sensor" objtype="site" objname="imu_site"/></sensor>
</mujoco>

mj_data.sensordata[start_idx : start_idx+3] = [0.0, 0.0, 0.6]

framelinvel

framelinvel 是 MuJoCo 中用于测量某个对象（site、body、geom）的线速度的传感器。
它输出的是该对象在世界坐标系下的线速度分量[vx, vy, vz]。常用于模拟 IMU 中的速度传感器，或者跟踪物体运动状态。

objtype=“site”：指定测量对象类型，可以是 site、body 或 geom。不能用于关节joint

<mujoco><worldbody><body name="link1" pos="0 0 0.5"><geom type="box" size="0.1 0.1 0.1"/><site name="imu_site" pos="0 0 0.1" size="0.01"/></body></worldbody><sensor><framelinvel name="imu_linvel_sensor" objtype="site" objname="imu_site"/></sensor>
</mujoco>

mj_data.sensordata[start_idx : start_idx+3] = [0.1, 0.2, 0.0]

touch

touch 是 MuJoCo 中用于检测对象是否发生接触的传感器。
它输出的是一个标量：0 表示当前没有接触；非0 表示有接触发生（输出接触力的模长也就是大小）。

测量某个对象（site、body、geom）在世界坐标系下的接触以及接触力的传感器。

<mujoco><worldbody><body name="finger" pos="0 0 0.1"><geom name="finger_geom" type="sphere" size="0.01"/></body><body name="table" pos="0 0 0"><geom name="table_geom" type="box" size="0.2 0.2 0.01"/></body></worldbody><sensor><touch name="finger_touch_sensor" objtype="geom" objname="finger_geom"/></sensor>
</mujoco>

mj_data.sensordata[start_idx] = 5.3
# 表示 finger_geom 正在接触某物体，接触力模长为 5.3 牛顿。

objtype=“site” objname=“imu” 和site="imu"的区别

site是定义一个对象的属性。

site="imu"直接绑定到某个 site 上测量局部数据。是传感器“装在哪儿”测局部值。

objtype=“site” objname="imu"测量某个对象（site/body/geom）的世界坐标下状态。是传感器“测谁”的全局状态。

这也使得一些传感器会使用一个特定写法，比如像机器人质心imu会写objtype=“site” objname="imu"表示全局的方向。机器人的陀螺仪gyro会用site="imu"表示机器人自己的旋转角速度。

python中与sensor有关的写法

传感器名字索引第几个id

# 模型，类型是传感器，传感器名字
sensor_id = mujoco.mj_name2id(mj_model, mujoco.mjtObj.mjOBJ_SENSOR, "imu_gyro_sensor")

id索引传感器名字

sensor_name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i)
# 得到imu这种

传感器数量

<sensor><accelerometer site="box_site" name="accel_sensor"/><gyro site="box_site" name="gyro_sensor"/>
</sensor>

print("Number of sensors:", model.nsensor)
# Number of sensors: 2

sensor中的顺序

在代码中要找到xml中sensor的数据需要注意xml中传感器的顺序

   <sensor><jointpos name="joint1_pos" joint="joint1" /><jointpos name="joint2_pos" joint="joint2" /><framequat name="imu_quat" objtype="site" objname="imu" /><gyro name="imu_gyro" site="imu" /><accelerometer name="imu_acc" site="imu" /></sensor>

mj_model = mujoco.MjModel.from_xml_path(config.ROBOT_SCENE)
mj_data = mujoco.MjData(mj_model)print("sensordata 内容:", mj_data.sensordata)

输出是一个数组这样，[0.1, 0.2, 0.707, 0, 0.707, 0, 0.01, 0.02, 0.03, 9.8, 0, 0]按索引来是这样的顺序：
sensordata[0] = 0.1    # joint1_pos
sensordata[1] = 0.2    # joint2_pos
sensordata[2] = 0.707  # imu_quat (w)
sensordata[3] = 0.0    # imu_quat (x)
sensordata[4] = 0.707  # imu_quat (y)
sensordata[5] = 0.0    # imu_quat (z)
sensordata[6] = 0.01   # imu_gyro (x)
sensordata[7] = 0.02   # imu_gyro (y)
sensordata[8] = 0.03   # imu_gyro (z)
sensordata[9] = 9.8    # imu_acc (x)
sensordata[10] = 0.0   # imu_acc (y)
sensordata[11] = 0.0   # imu_acc (z)