compute collisions in height field frame

[thowell]

for hfield<>geom collisions, transform the geom pose into the hfield frame. follows the mujoco implementation https://github.com/google-deepmind/mujoco/blob/310ac0c5ac0c5fd0391b0e7ea2575314b2b4dca2/src/engine/engine_collision_convex.c#L1204

[louislelay]

Hello @thowell and @kbayes, I just tested your changes with mjlab and it seems to have resolved our issues with hfield. Thanks for the reactivity!

[kevinzakka]

I just launched a job and so far, no NaNs.