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)