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

1""" 

2Homogeneous transformation matrices for robotics-numpy 

3 

4This module provides efficient SE(3) homogeneous transformation operations using only NumPy. 

5All functions are designed to work with both single transformations and batches. 

6 

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""" 

13 

14from typing import Optional, Union 

15 

16import numpy as np 

17 

18from .rotations import is_rotation_matrix, rotx, roty, rotz 

19 

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 

25 

26 

27def transl(x: ArrayLike, y: Optional[ArrayLike] = None, z: Optional[ArrayLike] = None) -> Matrix4x4: 

28 """ 

29 Create pure translation transformation matrix. 

30 

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) 

35 

36 Returns: 

37 4x4 homogeneous transformation matrix 

38 

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") 

56 

57 T = np.eye(4) 

58 T[0, 3] = x 

59 T[1, 3] = y 

60 T[2, 3] = z 

61 return T 

62 

63 

64def rotmat(R: Matrix3x3, t: Optional[Vector3] = None) -> Matrix4x4: 

65 """ 

66 Create homogeneous transformation from rotation matrix and optional translation. 

67 

68 Args: 

69 R: 3x3 rotation matrix 

70 t: 3-element translation vector (default: zero) 

71 

72 Returns: 

73 4x4 homogeneous transformation matrix 

74 

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") 

83 

84 if not is_rotation_matrix(R): 

85 raise ValueError("Input is not a valid rotation matrix") 

86 

87 T = np.eye(4) 

88 T[:3, :3] = R 

89 

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 

95 

96 return T 

97 

98 

99def SE3_from_matrix(T: Matrix4x4) -> Matrix4x4: 

100 """ 

101 Validate and return SE(3) transformation matrix. 

102 

103 Args: 

104 T: 4x4 matrix to validate 

105 

106 Returns: 

107 4x4 SE(3) transformation matrix 

108 

109 Raises: 

110 ValueError: If T is not a valid SE(3) matrix 

111 

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 

120 

121 

122def homogeneous_inverse(T: Matrix4x4) -> Matrix4x4: 

123 """ 

124 Compute inverse of homogeneous transformation matrix efficiently. 

125 

126 For SE(3) matrix T = [[R, t], [0, 1]], the inverse is [[R^T, -R^T*t], [0, 1]] 

127 

128 Args: 

129 T: 4x4 homogeneous transformation matrix 

130 

131 Returns: 

132 Inverse transformation matrix 

133 

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") 

141 

142 R = T[:3, :3] 

143 t = T[:3, 3] 

144 

145 T_inv = np.eye(4) 

146 T_inv[:3, :3] = R.T 

147 T_inv[:3, 3] = -R.T @ t 

148 

149 return T_inv 

150 

151 

152def is_homogeneous(T: np.ndarray, tol: float = 1e-6) -> bool: 

153 """ 

154 Check if matrix is a valid SE(3) homogeneous transformation matrix. 

155 

156 Args: 

157 T: Matrix to check 

158 tol: Tolerance for numerical checks 

159 

160 Returns: 

161 True if T is a valid SE(3) matrix 

162 

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 

170 

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 

175 

176 # Check if rotation part is valid 

177 R = T[:3, :3] 

178 if not is_rotation_matrix(R, tol): 

179 return False 

180 

181 return True 

182 

183 

184def extract_rotation(T: Matrix4x4) -> Matrix3x3: 

185 """ 

186 Extract 3x3 rotation matrix from homogeneous transformation. 

187 

188 Args: 

189 T: 4x4 homogeneous transformation matrix 

190 

191 Returns: 

192 3x3 rotation matrix 

193 

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() 

201 

202 

203def extract_translation(T: Matrix4x4) -> Vector3: 

204 """ 

205 Extract translation vector from homogeneous transformation. 

206 

207 Args: 

208 T: 4x4 homogeneous transformation matrix 

209 

210 Returns: 

211 3-element translation vector 

212 

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() 

220 

221 

222def transform_point(T: Matrix4x4, p: Vector3) -> Vector3: 

223 """ 

224 Transform a 3D point using homogeneous transformation. 

225 

226 Args: 

227 T: 4x4 homogeneous transformation matrix 

228 p: 3-element point coordinates 

229 

230 Returns: 

231 Transformed 3D point 

232 

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") 

239 

240 p = np.asarray(p) 

241 if p.shape != (3,): 

242 raise ValueError("Point must be 3-element vector") 

243 

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] 

248 

249 

250def transform_points(T: Matrix4x4, points: np.ndarray) -> np.ndarray: 

251 """ 

252 Transform multiple 3D points using homogeneous transformation. 

253 

254 Args: 

255 T: 4x4 homogeneous transformation matrix 

256 points: Nx3 array of points 

257 

258 Returns: 

259 Nx3 array of transformed points 

260 

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") 

268 

269 points = np.asarray(points) 

270 if points.ndim != 2 or points.shape[1] != 3: 

271 raise ValueError("Points must be Nx3 array") 

272 

273 # Add homogeneous coordinate 

274 ones = np.ones((points.shape[0], 1)) 

275 points_homo = np.hstack([points, ones]) 

276 

277 # Transform all points 

278 transformed_homo = (T @ points_homo.T).T 

279 return transformed_homo[:, :3] 

280 

281 

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) 

286 

287 

288def SE3_rotation_x(angle: float) -> Matrix4x4: 

289 """Create SE(3) transformation for rotation about X-axis.""" 

290 return rotmat(rotx(angle)) 

291 

292 

293def SE3_rotation_y(angle: float) -> Matrix4x4: 

294 """Create SE(3) transformation for rotation about Y-axis.""" 

295 return rotmat(roty(angle)) 

296 

297 

298def SE3_rotation_z(angle: float) -> Matrix4x4: 

299 """Create SE(3) transformation for rotation about Z-axis.""" 

300 return rotmat(rotz(angle)) 

301 

302 

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) 

306 

307 

308# Constants 

309IDENTITY_SE3 = np.eye(4)