Skip to content

Transforms

Native spatial transform helpers for SO(3) rotations and SE(3) rigid-body transforms. Replaces scipy.spatial.transform with Pinocchio-backed operations (no SciPy dependency).

Rotation (SO3)

Lightweight SO(3) rotation helper. Canonical methods are primary; spatialmath-style shorthands are convenience aliases.

Rotation

Lightweight SO(3) rotation helper using native embodiK/Pinocchio ops.

Replaces scipy.spatial.transform.Rotation for common use cases. Canonical methods (from_matrix, as_matrix, etc.) are primary; spatialmath-style shorthands (Rx, Rz, RPY, etc.) are convenience aliases.

Source code in python/embodik/transforms.py 
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
class Rotation:
    """
    Lightweight SO(3) rotation helper using native embodiK/Pinocchio ops.

    Replaces scipy.spatial.transform.Rotation for common use cases.
    Canonical methods (from_matrix, as_matrix, etc.) are primary;
    spatialmath-style shorthands (Rx, Rz, RPY, etc.) are convenience aliases.
    """

    __slots__ = ("_R",)

    def __init__(self, R: np.ndarray):
        self._R = np.asarray(R, dtype=float).reshape(3, 3).copy()

    # -------------------------------------------------------------------------
    # Canonical constructors
    # -------------------------------------------------------------------------

    @classmethod
    def from_matrix(cls, R: np.ndarray) -> "Rotation":
        """Create from 3x3 rotation matrix."""
        return cls(np.asarray(R, dtype=float))

    @classmethod
    def from_quat(cls, q: np.ndarray) -> "Rotation":
        """Create from quaternion [x, y, z, w] (scipy/ROS convention)."""
        q = np.asarray(q, dtype=float).ravel()
        if q.shape[0] != 4:
            raise ValueError(f"Expected 4-element quaternion, got shape {q.shape}")
        R = _native.quaternion_xyzw_to_matrix(float(q[0]), float(q[1]), float(q[2]), float(q[3]))
        return cls(np.array(R))

    @classmethod
    def from_rotvec(cls, omega: np.ndarray) -> "Rotation":
        """Create from axis-angle vector (rotation axis * angle in radians)."""
        omega = np.asarray(omega, dtype=float).ravel()
        if omega.shape[0] != 3:
            raise ValueError(f"Expected 3D rotvec, got shape {omega.shape}")
        R = _native.exp3(omega)
        return cls(np.array(R))

    @classmethod
    def from_euler(cls, seq: str, angles: Union[float, np.ndarray], degrees: bool = False) -> "Rotation":
        """
        Create from Euler angles.

        Args:
            seq: 'xyz', 'x', 'y', or 'z'
            angles: Single angle (for single-axis) or [r, p, y] for 'xyz'
            degrees: If True, angles are in degrees
        """
        angles = np.atleast_1d(np.asarray(angles, dtype=float))
        if degrees:
            angles = np.deg2rad(angles)

        if seq == "xyz":
            if angles.size != 3:
                raise ValueError("For 'xyz', angles must be [r, p, y]")
            R = _native.rotation_from_rpy(float(angles[0]), float(angles[1]), float(angles[2]))
            return cls(np.array(R))
        elif seq == "x":
            return cls.Rx(float(angles.flat[0]))
        elif seq == "y":
            return cls.Ry(float(angles.flat[0]))
        elif seq == "z":
            return cls.Rz(float(angles.flat[0]))
        else:
            raise ValueError(f"Unsupported Euler sequence: {seq!r}. Use 'xyz', 'x', 'y', or 'z'.")

    @classmethod
    def identity(cls) -> "Rotation":
        """Create identity rotation."""
        return cls(np.eye(3))

    # -------------------------------------------------------------------------
    # Spatialmath-style shorthand constructors
    # -------------------------------------------------------------------------

    @classmethod
    def Rx(cls, theta: float) -> "Rotation":
        """Rotation about X-axis by theta radians."""
        return cls.from_rotvec(np.array([theta, 0.0, 0.0]))

    @classmethod
    def Ry(cls, theta: float) -> "Rotation":
        """Rotation about Y-axis by theta radians."""
        return cls.from_rotvec(np.array([0.0, theta, 0.0]))

    @classmethod
    def Rz(cls, theta: float) -> "Rotation":
        """Rotation about Z-axis by theta radians."""
        return cls.from_rotvec(np.array([0.0, 0.0, theta]))

    @classmethod
    def RPY(cls, rpy: np.ndarray, order: str = "xyz") -> "Rotation":
        """Create from roll-pitch-yaw [r, p, y] in radians."""
        rpy = np.asarray(rpy, dtype=float).ravel()
        if rpy.size != 3:
            raise ValueError("RPY must be [r, p, y]")
        if order != "xyz":
            raise ValueError("Only order='xyz' supported")
        R = _native.rotation_from_rpy(float(rpy[0]), float(rpy[1]), float(rpy[2]))
        return cls(np.array(R))

    @classmethod
    def AngVec(cls, theta: float, axis: np.ndarray) -> "Rotation":
        """Create from angle (radians) and unit axis vector."""
        axis = np.asarray(axis, dtype=float).ravel()
        if axis.size != 3:
            raise ValueError("Axis must be 3D")
        n = np.linalg.norm(axis)
        if n < 1e-12:
            return cls.identity()
        omega = (theta / n) * axis
        return cls.from_rotvec(omega)

    @classmethod
    def EulerVec(cls, omega: np.ndarray) -> "Rotation":
        """Create from axis-angle vector (alias for from_rotvec)."""
        return cls.from_rotvec(omega)

    # -------------------------------------------------------------------------
    # Outputs (canonical)
    # -------------------------------------------------------------------------

    def as_matrix(self) -> np.ndarray:
        """Return 3x3 rotation matrix."""
        return self._R.copy()

    def as_quat(self) -> np.ndarray:
        """Return quaternion [x, y, z, w] (scipy/ROS convention)."""
        q = _native.matrix_to_quaternion_xyzw(self._R)
        return np.array(q)

    def as_rotvec(self) -> np.ndarray:
        """Return axis-angle vector (rotation axis * angle in radians)."""
        return np.array(_native.log3(self._R))

    # -------------------------------------------------------------------------
    # Property aliases (spatialmath-style)
    # -------------------------------------------------------------------------

    @property
    def R(self) -> np.ndarray:
        """Rotation matrix (alias for as_matrix())."""
        return self._R.copy()

    # -------------------------------------------------------------------------
    # Operations
    # -------------------------------------------------------------------------

    def inv(self) -> "Rotation":
        """Return inverse rotation."""
        return Rotation(self._R.T)

    def __mul__(self, other: "Rotation") -> "Rotation":
        """Compose rotations: self * other."""
        if not isinstance(other, Rotation):
            return NotImplemented
        return Rotation(self._R @ other._R)

    def apply(self, v: np.ndarray) -> np.ndarray:
        """Rotate 3D vector: R @ v."""
        v = np.asarray(v, dtype=float).reshape(3)
        return (self._R @ v).reshape(3)

    def __repr__(self) -> str:
        return f"Rotation(shape=(3,3))"

Attributes

R property 

R

Rotation matrix (alias for as_matrix()).

Functions

from_matrix classmethod 

from_matrix(R)

Create from 3x3 rotation matrix.

Source code in python/embodik/transforms.py 
31
32
33
34
@classmethod
def from_matrix(cls, R: np.ndarray) -> "Rotation":
    """Create from 3x3 rotation matrix."""
    return cls(np.asarray(R, dtype=float))

from_quat classmethod 

from_quat(q)

Create from quaternion [x, y, z, w] (scipy/ROS convention).

Source code in python/embodik/transforms.py 
36
37
38
39
40
41
42
43
@classmethod
def from_quat(cls, q: np.ndarray) -> "Rotation":
    """Create from quaternion [x, y, z, w] (scipy/ROS convention)."""
    q = np.asarray(q, dtype=float).ravel()
    if q.shape[0] != 4:
        raise ValueError(f"Expected 4-element quaternion, got shape {q.shape}")
    R = _native.quaternion_xyzw_to_matrix(float(q[0]), float(q[1]), float(q[2]), float(q[3]))
    return cls(np.array(R))

from_rotvec classmethod 

from_rotvec(omega)

Create from axis-angle vector (rotation axis * angle in radians).

Source code in python/embodik/transforms.py 
45
46
47
48
49
50
51
52
@classmethod
def from_rotvec(cls, omega: np.ndarray) -> "Rotation":
    """Create from axis-angle vector (rotation axis * angle in radians)."""
    omega = np.asarray(omega, dtype=float).ravel()
    if omega.shape[0] != 3:
        raise ValueError(f"Expected 3D rotvec, got shape {omega.shape}")
    R = _native.exp3(omega)
    return cls(np.array(R))

from_euler classmethod 

from_euler(seq, angles, degrees=False)

Create from Euler angles.

Parameters:

Name Type Description Default
seq str

'xyz', 'x', 'y', or 'z'

 required
angles Union[float, ndarray]

Single angle (for single-axis) or [r, p, y] for 'xyz'

 required
degrees bool

If True, angles are in degrees

 False
Source code in python/embodik/transforms.py 
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
@classmethod
def from_euler(cls, seq: str, angles: Union[float, np.ndarray], degrees: bool = False) -> "Rotation":
    """
    Create from Euler angles.

    Args:
        seq: 'xyz', 'x', 'y', or 'z'
        angles: Single angle (for single-axis) or [r, p, y] for 'xyz'
        degrees: If True, angles are in degrees
    """
    angles = np.atleast_1d(np.asarray(angles, dtype=float))
    if degrees:
        angles = np.deg2rad(angles)

    if seq == "xyz":
        if angles.size != 3:
            raise ValueError("For 'xyz', angles must be [r, p, y]")
        R = _native.rotation_from_rpy(float(angles[0]), float(angles[1]), float(angles[2]))
        return cls(np.array(R))
    elif seq == "x":
        return cls.Rx(float(angles.flat[0]))
    elif seq == "y":
        return cls.Ry(float(angles.flat[0]))
    elif seq == "z":
        return cls.Rz(float(angles.flat[0]))
    else:
        raise ValueError(f"Unsupported Euler sequence: {seq!r}. Use 'xyz', 'x', 'y', or 'z'.")

identity classmethod 

identity()

Create identity rotation.

Source code in python/embodik/transforms.py 
82
83
84
85
@classmethod
def identity(cls) -> "Rotation":
    """Create identity rotation."""
    return cls(np.eye(3))

Rx classmethod 

Rx(theta)

Rotation about X-axis by theta radians.

Source code in python/embodik/transforms.py 
91
92
93
94
@classmethod
def Rx(cls, theta: float) -> "Rotation":
    """Rotation about X-axis by theta radians."""
    return cls.from_rotvec(np.array([theta, 0.0, 0.0]))

Ry classmethod 

Ry(theta)

Rotation about Y-axis by theta radians.

Source code in python/embodik/transforms.py 
96
97
98
99
@classmethod
def Ry(cls, theta: float) -> "Rotation":
    """Rotation about Y-axis by theta radians."""
    return cls.from_rotvec(np.array([0.0, theta, 0.0]))

Rz classmethod 

Rz(theta)

Rotation about Z-axis by theta radians.

Source code in python/embodik/transforms.py 
101
102
103
104
@classmethod
def Rz(cls, theta: float) -> "Rotation":
    """Rotation about Z-axis by theta radians."""
    return cls.from_rotvec(np.array([0.0, 0.0, theta]))

RPY classmethod 

RPY(rpy, order='xyz')

Create from roll-pitch-yaw [r, p, y] in radians.

Source code in python/embodik/transforms.py 
106
107
108
109
110
111
112
113
114
115
@classmethod
def RPY(cls, rpy: np.ndarray, order: str = "xyz") -> "Rotation":
    """Create from roll-pitch-yaw [r, p, y] in radians."""
    rpy = np.asarray(rpy, dtype=float).ravel()
    if rpy.size != 3:
        raise ValueError("RPY must be [r, p, y]")
    if order != "xyz":
        raise ValueError("Only order='xyz' supported")
    R = _native.rotation_from_rpy(float(rpy[0]), float(rpy[1]), float(rpy[2]))
    return cls(np.array(R))

AngVec classmethod 

AngVec(theta, axis)

Create from angle (radians) and unit axis vector.

Source code in python/embodik/transforms.py 
117
118
119
120
121
122
123
124
125
126
127
@classmethod
def AngVec(cls, theta: float, axis: np.ndarray) -> "Rotation":
    """Create from angle (radians) and unit axis vector."""
    axis = np.asarray(axis, dtype=float).ravel()
    if axis.size != 3:
        raise ValueError("Axis must be 3D")
    n = np.linalg.norm(axis)
    if n < 1e-12:
        return cls.identity()
    omega = (theta / n) * axis
    return cls.from_rotvec(omega)

EulerVec classmethod 

EulerVec(omega)

Create from axis-angle vector (alias for from_rotvec).

Source code in python/embodik/transforms.py 
129
130
131
132
@classmethod
def EulerVec(cls, omega: np.ndarray) -> "Rotation":
    """Create from axis-angle vector (alias for from_rotvec)."""
    return cls.from_rotvec(omega)

as_matrix 

as_matrix()

Return 3x3 rotation matrix.

Source code in python/embodik/transforms.py 
138
139
140
def as_matrix(self) -> np.ndarray:
    """Return 3x3 rotation matrix."""
    return self._R.copy()

as_quat 

as_quat()

Return quaternion [x, y, z, w] (scipy/ROS convention).

Source code in python/embodik/transforms.py 
142
143
144
145
def as_quat(self) -> np.ndarray:
    """Return quaternion [x, y, z, w] (scipy/ROS convention)."""
    q = _native.matrix_to_quaternion_xyzw(self._R)
    return np.array(q)

as_rotvec 

as_rotvec()

Return axis-angle vector (rotation axis * angle in radians).

Source code in python/embodik/transforms.py 
147
148
149
def as_rotvec(self) -> np.ndarray:
    """Return axis-angle vector (rotation axis * angle in radians)."""
    return np.array(_native.log3(self._R))

inv 

inv()

Return inverse rotation.

Source code in python/embodik/transforms.py 
164
165
166
def inv(self) -> "Rotation":
    """Return inverse rotation."""
    return Rotation(self._R.T)

apply 

apply(v)

Rotate 3D vector: R @ v.

Source code in python/embodik/transforms.py 
174
175
176
177
def apply(self, v: np.ndarray) -> np.ndarray:
    """Rotate 3D vector: R @ v."""
    v = np.asarray(v, dtype=float).reshape(3)
    return (self._R @ v).reshape(3)

SO3 Alias

SO3 is an alias for Rotation (spatialmath-style):

from embodik import SO3
R = SO3.Rx(np.pi/2)
R = SO3.RPY([roll, pitch, yaw], order='xyz')

SE3

Rigid-body transform (rotation + translation). Supports composition, point transforms, and spatialmath-style property aliases.

SE3

Attributes

rotation property 

rotation

(self) -> numpy.ndarray[dtype=float64, shape=(3, 3), order='F']

translation property 

translation

(self) -> numpy.ndarray[dtype=float64, shape=(3), order='C']

R property 

R

Rotation matrix (alias for rotation)

t property 

t

Translation vector (alias for translation)

A property 

A

4x4 homogeneous matrix (alias for homogeneous())

Functions

Rt 

Rt(*args, **kwargs)

Rt(R: numpy.ndarray[dtype=float64, shape=(3, 3), order='F'], t: numpy.ndarray[dtype=float64, shape=(3), order='C']) -> embodik._embodik_impl.SE3

Create SE3 from rotation matrix and translation (spatialmath-style)

homogeneous method descriptor 

homogeneous()

homogeneous(self) -> numpy.ndarray[dtype=float64, shape=(4, 4), order='F']

Get 4x4 homogeneous transformation matrix

inverse method descriptor 

inverse()

inverse(self) -> embodik._embodik_impl.SE3

act method descriptor 

act()

act(self, point: numpy.ndarray[dtype=float64, shape=(3), order='C']) -> numpy.ndarray[dtype=float64, shape=(3), order='C']

Transform 3D point from local to world frame: p_world = R @ p + t

actInv method descriptor 

actInv()

actInv(self, point: numpy.ndarray[dtype=float64, shape=(3), order='C']) -> numpy.ndarray[dtype=float64, shape=(3), order='C']

Transform 3D point from world to local frame: p_local = R.T @ (p - t)

SE3 Composition

from embodik import Rt, SE3
T1 = Rt(R=np.eye(3), t=[1, 0, 0])
T2 = Rt(R=np.eye(3), t=[0, 1, 0])
T_composed = T1 * T2  # Pinocchio: ^A M_B * ^B M_C = ^A M_C

SE3 Point Transforms

p_world = T.act(p_local)   # p_world = R @ p_local + t
p_local = T.actInv(p_world)  # p_local = R.T @ (p_world - t)

Low-Level Utilities

Native C++ functions exposed for advanced use:

Function Description
log3(R) Axis-angle from 3×3 rotation matrix
exp3(omega) 3×3 rotation matrix from axis-angle
matrix_to_quaternion_wxyz(R) Matrix → [w,x,y,z] quaternion
matrix_to_quaternion_xyzw(R) Matrix → [x,y,z,w] quaternion
quaternion_wxyz_to_matrix(w,x,y,z) [w,x,y,z] → 3×3 matrix
quaternion_xyzw_to_matrix(x,y,z,w) [x,y,z,w] → 3×3 matrix
rotation_from_rpy(r,p,y) Roll-pitch-yaw → 3×3 matrix