import numpy as np


def _skew(v: np.ndarray) -> np.ndarray:
    x, y, z = v
    return np.array([[0, -z,  y],
                     [z,  0, -x],
                     [-y, x,  0]], dtype=float)


def adjoint_SE3(T: np.ndarray) -> np.ndarray:
    """Adjoint(T) for SE(3): 6x6, spatial 顺序([ω; v])."""
    R = T[:3, :3]
    p = T[:3, 3]
    Ad = np.block([[R, np.zeros((3, 3))],
                   [_skew(p) @ R, R]])
    return Ad


def spatial_to_geometric(J_spatial: np.ndarray) -> np.ndarray:
    """把 Pinocchio 的 [ω; v] 排列变成 [v; ω]。"""
    return np.vstack([J_spatial[3:, :], J_spatial[:3, :]])


def wrench_geo_to_spa(F_geo: np.ndarray) -> np.ndarray:
    """[f; τ] -> [τ; f]"""
    return np.hstack([F_geo[3:], F_geo[:3]])


def wrench_spa_to_geo(F_spa: np.ndarray) -> np.ndarray:
    """[τ; f] -> [f; τ]"""
    return np.hstack([F_spa[3:], F_spa[:3]])