32 lines
880 B
Python
32 lines
880 B
Python
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]]) |