[BUG] Proprioception is not working

[kgd-al]

Describe the bug
Active hinge sensors are returning nonsensical values taken from data.qpos

To Reproduce
Actually look at the initially perceived values of the sensors to see that they are not zero.

Expected behavior
That each hinge sensor returns the local position of its associated hinge

Screenshots

Not a screenshot, but a log excerpt:

[at time 0]

Mujoco data
0.0 [0.0, 0.0, 0.075, 0.9238795325112867, 0.0, 0.0, -0.3826834323650897, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
mbs1/pos + mbs1/quat [0. 0. 0.075] [ 0.92387953 0. 0. -0.38268343]
mbs1/ [0.0, 0.0, 0.075, 0.9238795325112867, 0.0, 0.0, -0.3826834323650897]
mbs1/mbs1_joint0 [0.0]
mbs1/mbs1_link0_joint1 [0.0]
mbs1/mbs1_joint1 [0.0]
mbs1/mbs1_link1_joint1 [0.0]
mbs1/mbs1_joint2 [0.0]
mbs1/mbs1_link2_joint1 [0.0]
mbs1/mbs1_joint3 [0.0]
mbs1/mbs1_link3_joint1 [0.0]
Revolve proprioceptive data
0.0 [0.0, 0.9238795325112867, 0.0, 0.0, 0.075, 0.0, -0.3826834323650897, 0.0]

[at about 13 second]

Mujoco data
12.999999999998234 [2.38491741502192, 0.2660076918162429, 0.13800461478869996, 0.573907200087368, 0.0021039460130971056, -0.006762895980071553, -0.8188897131708365, 0.9485355502456285, 0.7055231558294652, 1.0483350231498312, 0.2623087102921389, 1.0391867093059712, 0.576184293755307, -0.5851401730696333, 0.5983306565426767]
mbs1/pos + mbs1/quat [2.3849174 0.26600769 0.1380046 ] [ 0.57390726 0.00210396 -0.00676301 -0.81888967]
mbs1/ [2.38491741502192, 0.2660076918162429, 0.13800461478869996, 0.573907200087368, 0.0021039460130971056, -0.006762895980071553, -0.8188897131708365]
mbs1/mbs1_joint0 [0.9485355502456285]
mbs1/mbs1_link0_joint1 [0.7055231558294652]
mbs1/mbs1_joint1 [1.0483350231498312]
mbs1/mbs1_link1_joint1 [0.2623087102921389]
mbs1/mbs1_joint2 [1.0391867093059712]
mbs1/mbs1_link2_joint1 [0.576184293755307]
mbs1/mbs1_joint3 [-0.5851401730696333]
mbs1/mbs1_link3_joint1 [0.5983306565426767]
Revolve proprioceptive data
12.999999999998234 [0.2660076918162429, 0.573907200087368, -0.006762895980071553, 0.9485355502456285, 0.13800461478869996, 0.0021039460130971056, -0.8188897131708365, 0.7055231558294652]

---

What we can see is that the hinges use the y and z position of the free joint, plus the 4 elements of its quaternion and only the actual positions of the first two hinges

Platform:

• OS: Kubuntu 22.04
• Python 10