Coverage for src/robotics_numpy/transforms/homogeneous.py: 84%
97 statements
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
1"""
2Homogeneous transformation matrices for robotics-numpy
4This module provides efficient SE(3) homogeneous transformation operations using only NumPy.
5All functions are designed to work with both single transformations and batches.
7Conventions:
8- Homogeneous transformation matrices are 4x4 NumPy arrays
9- Position vectors are 3-element arrays [x, y, z]
10- Rotation matrices are 3x3 arrays
11- SE(3) format: [[R, t], [0, 1]] where R is 3x3 rotation, t is 3x1 translation
12"""
14from typing import Optional, Union
16import numpy as np
18from .rotations import is_rotation_matrix, rotx, roty, rotz
20# Type aliases
21ArrayLike = Union[float, int, np.ndarray]
22Vector3 = np.ndarray # 3-element position vector
23Matrix4x4 = np.ndarray # 4x4 homogeneous transformation matrix
24Matrix3x3 = np.ndarray # 3x3 rotation matrix
27def transl(x: ArrayLike, y: Optional[ArrayLike] = None, z: Optional[ArrayLike] = None) -> Matrix4x4:
28 """
29 Create pure translation transformation matrix.
31 Args:
32 x: X translation or 3-element vector [x, y, z]
33 y: Y translation (if x is scalar)
34 z: Z translation (if x is scalar)
36 Returns:
37 4x4 homogeneous transformation matrix
39 Examples:
40 >>> T1 = transl(1, 2, 3) # Translation by [1, 2, 3]
41 >>> T2 = transl([1, 2, 3]) # Same as above
42 >>> T3 = transl(np.array([1, 2, 3])) # NumPy array input
43 """
44 if y is None and z is None:
45 # Single argument case - expect 3-element vector
46 pos = np.asarray(x)
47 if pos.shape == (3,):
48 x, y, z = pos
49 else:
50 raise ValueError("Single argument must be 3-element vector")
51 elif y is not None and z is not None:
52 # Three scalar arguments
53 x, y, z = float(x), float(y), float(z)
54 else:
55 raise ValueError("Must provide either 1 or 3 arguments")
57 T = np.eye(4)
58 T[0, 3] = x
59 T[1, 3] = y
60 T[2, 3] = z
61 return T
64def rotmat(R: Matrix3x3, t: Optional[Vector3] = None) -> Matrix4x4:
65 """
66 Create homogeneous transformation from rotation matrix and optional translation.
68 Args:
69 R: 3x3 rotation matrix
70 t: 3-element translation vector (default: zero)
72 Returns:
73 4x4 homogeneous transformation matrix
75 Examples:
76 >>> R = rotx(np.pi/2)
77 >>> T1 = rotmat(R) # Pure rotation
78 >>> T2 = rotmat(R, [1, 2, 3]) # Rotation + translation
79 """
80 R = np.asarray(R)
81 if R.shape != (3, 3):
82 raise ValueError("Rotation matrix must be 3x3")
84 if not is_rotation_matrix(R):
85 raise ValueError("Input is not a valid rotation matrix")
87 T = np.eye(4)
88 T[:3, :3] = R
90 if t is not None:
91 t = np.asarray(t)
92 if t.shape != (3,):
93 raise ValueError("Translation vector must be 3-element")
94 T[:3, 3] = t
96 return T
99def SE3_from_matrix(T: Matrix4x4) -> Matrix4x4:
100 """
101 Validate and return SE(3) transformation matrix.
103 Args:
104 T: 4x4 matrix to validate
106 Returns:
107 4x4 SE(3) transformation matrix
109 Raises:
110 ValueError: If T is not a valid SE(3) matrix
112 Examples:
113 >>> T = np.eye(4)
114 >>> T_valid = SE3_from_matrix(T)
115 """
116 T = np.asarray(T)
117 if not is_homogeneous(T):
118 raise ValueError("Input is not a valid SE(3) transformation matrix")
119 return T
122def homogeneous_inverse(T: Matrix4x4) -> Matrix4x4:
123 """
124 Compute inverse of homogeneous transformation matrix efficiently.
126 For SE(3) matrix T = [[R, t], [0, 1]], the inverse is [[R^T, -R^T*t], [0, 1]]
128 Args:
129 T: 4x4 homogeneous transformation matrix
131 Returns:
132 Inverse transformation matrix
134 Examples:
135 >>> T = transl(1, 2, 3) @ rotmat(rotx(np.pi/4))
136 >>> T_inv = homogeneous_inverse(T)
137 >>> np.allclose(T @ T_inv, np.eye(4)) # Should be True
138 """
139 if not is_homogeneous(T):
140 raise ValueError("Input is not a valid SE(3) transformation matrix")
142 R = T[:3, :3]
143 t = T[:3, 3]
145 T_inv = np.eye(4)
146 T_inv[:3, :3] = R.T
147 T_inv[:3, 3] = -R.T @ t
149 return T_inv
152def is_homogeneous(T: np.ndarray, tol: float = 1e-6) -> bool:
153 """
154 Check if matrix is a valid SE(3) homogeneous transformation matrix.
156 Args:
157 T: Matrix to check
158 tol: Tolerance for numerical checks
160 Returns:
161 True if T is a valid SE(3) matrix
163 Examples:
164 >>> T = np.eye(4)
165 >>> is_homogeneous(T) # True
166 >>> is_homogeneous(np.ones((4, 4))) # False
167 """
168 if T.shape != (4, 4):
169 return False
171 # Check bottom row is [0, 0, 0, 1]
172 expected_bottom = np.array([0, 0, 0, 1])
173 if not np.allclose(T[3, :], expected_bottom, atol=tol):
174 return False
176 # Check if rotation part is valid
177 R = T[:3, :3]
178 if not is_rotation_matrix(R, tol):
179 return False
181 return True
184def extract_rotation(T: Matrix4x4) -> Matrix3x3:
185 """
186 Extract 3x3 rotation matrix from homogeneous transformation.
188 Args:
189 T: 4x4 homogeneous transformation matrix
191 Returns:
192 3x3 rotation matrix
194 Examples:
195 >>> T = rotmat(rotx(np.pi/4), [1, 2, 3])
196 >>> R = extract_rotation(T)
197 """
198 if not is_homogeneous(T):
199 raise ValueError("Input is not a valid SE(3) transformation matrix")
200 return T[:3, :3].copy()
203def extract_translation(T: Matrix4x4) -> Vector3:
204 """
205 Extract translation vector from homogeneous transformation.
207 Args:
208 T: 4x4 homogeneous transformation matrix
210 Returns:
211 3-element translation vector
213 Examples:
214 >>> T = rotmat(rotx(np.pi/4), [1, 2, 3])
215 >>> t = extract_translation(T) # Returns [1, 2, 3]
216 """
217 if not is_homogeneous(T):
218 raise ValueError("Input is not a valid SE(3) transformation matrix")
219 return T[:3, 3].copy()
222def transform_point(T: Matrix4x4, p: Vector3) -> Vector3:
223 """
224 Transform a 3D point using homogeneous transformation.
226 Args:
227 T: 4x4 homogeneous transformation matrix
228 p: 3-element point coordinates
230 Returns:
231 Transformed 3D point
233 Examples:
234 >>> T = transl(1, 0, 0) # Translation by [1, 0, 0]
235 >>> p_new = transform_point(T, [0, 0, 0]) # Returns [1, 0, 0]
236 """
237 if not is_homogeneous(T):
238 raise ValueError("Input is not a valid SE(3) transformation matrix")
240 p = np.asarray(p)
241 if p.shape != (3,):
242 raise ValueError("Point must be 3-element vector")
244 # Convert to homogeneous coordinates and transform
245 p_homo = np.append(p, 1)
246 p_transformed = T @ p_homo
247 return p_transformed[:3]
250def transform_points(T: Matrix4x4, points: np.ndarray) -> np.ndarray:
251 """
252 Transform multiple 3D points using homogeneous transformation.
254 Args:
255 T: 4x4 homogeneous transformation matrix
256 points: Nx3 array of points
258 Returns:
259 Nx3 array of transformed points
261 Examples:
262 >>> T = rotx(np.pi/2)
263 >>> points = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
264 >>> transformed = transform_points(T, points)
265 """
266 if not is_homogeneous(T):
267 raise ValueError("Input is not a valid SE(3) transformation matrix")
269 points = np.asarray(points)
270 if points.ndim != 2 or points.shape[1] != 3:
271 raise ValueError("Points must be Nx3 array")
273 # Add homogeneous coordinate
274 ones = np.ones((points.shape[0], 1))
275 points_homo = np.hstack([points, ones])
277 # Transform all points
278 transformed_homo = (T @ points_homo.T).T
279 return transformed_homo[:, :3]
282# Convenience functions for common transformations
283def SE3_translation(x: ArrayLike, y: Optional[ArrayLike] = None, z: Optional[ArrayLike] = None) -> Matrix4x4:
284 """Alias for transl()."""
285 return transl(x, y, z)
288def SE3_rotation_x(angle: float) -> Matrix4x4:
289 """Create SE(3) transformation for rotation about X-axis."""
290 return rotmat(rotx(angle))
293def SE3_rotation_y(angle: float) -> Matrix4x4:
294 """Create SE(3) transformation for rotation about Y-axis."""
295 return rotmat(roty(angle))
298def SE3_rotation_z(angle: float) -> Matrix4x4:
299 """Create SE(3) transformation for rotation about Z-axis."""
300 return rotmat(rotz(angle))
303def SE3_from_rt(R: Matrix3x3, t: Vector3) -> Matrix4x4:
304 """Create SE(3) transformation from rotation matrix and translation vector."""
305 return rotmat(R, t)
308# Constants
309IDENTITY_SE3 = np.eye(4)