[Bug] Adjoint matrix block order inconsistent with twist (v, ω) definition

[ht-Leung]

🧠 Summary

The current implementation of construct_adjoint_matrix() defines the adjoint transformation matrix according to the twist ordering (ω, v), while the action and observation vectors in the environment are organized as (v, ω).

This mismatch causes incorrect coupling between linear and angular velocity components when the translation vector p is non-zero (e.g., when the end-effector is far from the robot base).

---

📐 Mathematical Background

For a homogeneous transform:

$$ T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} $$

The adjoint operator is defined as:

$$ \mathrm{Ad}_T=\begin{bmatrix} R & 0\\ [\mathbf p]_\times R & R \end{bmatrix} $$

which acts on twists ordered as:

$$ V=\begin{bmatrix} \boldsymbol{\omega} \\ \mathbf v \end{bmatrix} $$

However, if the twist is represented as (v, ω), the equivalent form should be:

$$ \mathrm{Ad}_T=\begin{bmatrix} R & [\mathbf p]_\times R\\ 0 & R \end{bmatrix} $$

---

⚙️ Current Behavior

adjoint_matrix[:3, :3] = rotation
adjoint_matrix[3:, 3:] = rotation
adjoint_matrix[3:, :3] = skew_matrix @ rotation

This corresponds to the (ω, v) convention, but both the action and tcp_vel in the environment are defined as (v, ω).

---

💥 Impact

• Linear and angular velocity components become cross-coupled under translation.
• Errors may cancel out for small motions but accumulate under large displacements or rotations.
• Leads to inaccurate policy learning and physical inconsistency in twist transformations.

---

✅ Expected Fix

Move the cross term [p]_× R to the upper-right block:

adjoint_matrix[:3, 3:] = skew_matrix @ rotation

Thus, for twists

$$ V = \begin{bmatrix} \mathbf v \\ \boldsymbol{\omega} \end{bmatrix}, $$

we have

$$ V_b=\mathrm{Ad}_T V_e,\quad \mathrm{Ad}_T=\begin{bmatrix} R & [\mathbf p]_\times R\\ 0 & R \end{bmatrix} $$

---

🧪 Verification

• Verified against Modern Robotics (Lynch & Park, 2017, §3.3.2) .
• Numerical validation using random SE(3) samples confirms correctness only when block order matches twist order.

---