[Fix] Correct adjoint matrix block order to match (v, ω) twist definition

[ht-Leung]

Summary

This PR fixes the adjoint matrix definition to match the twist ordering (v, ω) used in the environment.

Details

• Moved the cross term [p]_× R to the upper-right block.
• Verified correctness against Modern Robotics (Lynch & Park, 2017, §3.3.2).
• Numerical tests with random SE(3) samples confirm the correction.

Related Issue

Fixes #36

[jianlanluo]

Thanks for the PR, I think you are right, however the real issue is that we used delta pose control for low-level controller in Franka (https://github.com/rail-berkeley/serl_franka_controllers) rather than twist as in this code.

So I think the fix would be:

1. use your PR + a robot running actual twist control
2. revise this code to adapt for delta pose control as in the current robot controller (we'll fix this later)

[ht-Leung]

Got it, thanks for clarifying — that makes sense!
Based on my understanding, this pipeline does not directly affect the low-level control of the robot. The adjoint transformation mainly serves as a geometric mapping between different coordinate spaces (e.g., from the SpaceMouse expert frame to the robot frame), rather than altering the control law itself. During data collection, the expert actions (a_expert) are transformed via Adj⁻¹ for frame consistency, and the policy outputs (a_policy) are later mapped back through Adj before being executed by the robot. Therefore, the adjoint operation ensures mathematical and geometric consistency (or approximate consistency) between expert and policy actions, while the actual control behavior is still determined by the low-level controller (twist or delta pose).
Could you please confirm if my understanding is correct?

[jianlanluo]

Yes, however it does make a difference if you switch to a different type of controller, sorry for the confusion, this is mostly a problem on our side.
We fixed this:#39
so right now, the codebase is assuming doing delta pose control (it's still typed as twist, that's a problem on our side), and the franka controller is accepting delta pose control, so it's consistent now.

In your PR, the codebase will be doing twist control, then you would need to perform twist control on the low level

It will make a significant difference if the robot's movement involves rotation

[ht-Leung]

Thanks for the explanation — that clears it up!
So the main difference shows up when the controller interprets commands as delta poses instead of twists, especially when rotation is involved.

My PR just fixes the adjoint matrix definition to match the (v, ω) convention, so it doesn’t change how control is actually applied. Once the controller side is unified, this should stay consistent geometrically.

Appreciate the context. I’ll stay in sync with #39 then.

Also, really appreciate the great work from Prof. Luo and the HIL-SERL team — the real-world RL system is impressively robust and well-engineered. The stability and performance of your setup are honestly remarkable!