Coverage for src/robotics_numpy/kinematics/forward.py: 47%

121 statements  

« prev     ^ index     » next       coverage.py v7.9.2, created at 2025-07-14 16:02 +0200

1""" 

2Forward kinematics algorithms for robotics-numpy 

3 

4This module provides efficient forward kinematics computation for robot manipulators 

5using DH parameters. Optimized for performance with NumPy vectorization. 

6""" 

7 

8import numpy as np 

9from typing import List, Union, Optional 

10from ..transforms import SE3 

11from ..models.dh_link import DHLink 

12 

13# Type aliases 

14ArrayLike = Union[float, int, np.ndarray, List[float]] 

15JointConfig = Union[List[float], np.ndarray] 

16 

17 

18def fkine_dh(links: List[DHLink], q: JointConfig) -> SE3: 

19 """ 

20 Forward kinematics using DH parameters. 

21 

22 Computes the transformation from the base frame to the end-effector frame 

23 by multiplying individual link transformation matrices. 

24 

25 Args: 

26 links: List of DH links 

27 q: Joint configuration (n joint values) 

28 

29 Returns: 

30 SE3 transformation from base to end-effector 

31 

32 Examples: 

33 >>> links = [RevoluteDH(d=0.1, a=0.2, alpha=0), 

34 ... RevoluteDH(d=0, a=0.3, alpha=0)] 

35 >>> T = fkine_dh(links, [0, np.pi/4]) 

36 >>> print(T) 

37 """ 

38 # Import here to avoid circular imports 

39 from ..models.dh_robot import _fkine_dh_single 

40 return _fkine_dh_single(links, q) 

41 

42 

43def fkine_dh_all(links: List[DHLink], q: JointConfig) -> List[SE3]: 

44 """ 

45 Forward kinematics for all intermediate frames. 

46 

47 Computes transformations from base to each link frame, useful for 

48 visualization and intermediate pose calculations. 

49 

50 Args: 

51 links: List of DH links 

52 q: Joint configuration 

53 

54 Returns: 

55 List of SE3 transformations, one for each link frame 

56 

57 Examples: 

58 >>> links = [RevoluteDH(d=0.1, a=0.2, alpha=0), 

59 ... RevoluteDH(d=0, a=0.3, alpha=0)] 

60 >>> transforms = fkine_dh_all(links, [0, np.pi/4]) 

61 >>> print(f"Link 1 pose: {transforms[0]}") 

62 >>> print(f"End-effector pose: {transforms[1]}") 

63 """ 

64 q = np.asarray(q, dtype=float) 

65 

66 if len(q) != len(links): 

67 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})") 

68 

69 transforms = [] 

70 T = SE3() # Start with identity 

71 

72 # Accumulate transformations 

73 for i, (link, qi) in enumerate(zip(links, q)): 

74 try: 

75 T_link = link.A(qi) 

76 T = T * T_link 

77 transforms.append(T) 

78 except Exception as e: 

79 raise RuntimeError(f"Error computing transformation for link {i}: {e}") 

80 

81 return transforms 

82 

83 

84def fkine_dh_partial(links: List[DHLink], q: JointConfig, 

85 start: int = 0, end: Optional[int] = None) -> SE3: 

86 """ 

87 Forward kinematics for a partial chain of links. 

88 

89 Computes transformation from start link to end link (inclusive). 

90 

91 Args: 

92 links: List of DH links 

93 q: Joint configuration 

94 start: Starting link index (default: 0) 

95 end: Ending link index (default: last link) 

96 

97 Returns: 

98 SE3 transformation from start to end frame 

99 

100 Examples: 

101 >>> # Compute transformation from link 1 to link 3 

102 >>> T = fkine_dh_partial(links, q, start=1, end=3) 

103 """ 

104 q = np.asarray(q, dtype=float) 

105 

106 if len(q) != len(links): 

107 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})") 

108 

109 if end is None: 

110 end = len(links) - 1 

111 

112 if not (0 <= start <= end < len(links)): 

113 raise ValueError(f"Invalid range: start={start}, end={end}, n_links={len(links)}") 

114 

115 # Extract relevant links and joint values 

116 partial_links = links[start:end+1] 

117 partial_q = q[start:end+1] 

118 

119 return fkine_dh(partial_links, partial_q) 

120 

121 

122def fkine_dh_batch(links: List[DHLink], q_batch: np.ndarray) -> List[SE3]: 

123 """ 

124 Batch forward kinematics for multiple configurations. 

125 

126 Efficiently computes forward kinematics for multiple joint configurations. 

127 

128 Args: 

129 links: List of DH links 

130 q_batch: Batch of joint configurations (m x n array) 

131 

132 Returns: 

133 List of SE3 transformations, one for each configuration 

134 

135 Examples: 

136 >>> # Compute FK for multiple configurations 

137 >>> q_batch = np.array([[0, 0, 0], [0.1, 0.2, 0.3], [0.5, -0.5, 1.0]]) 

138 >>> transforms = fkine_dh_batch(links, q_batch) 

139 """ 

140 q_batch = np.asarray(q_batch, dtype=float) 

141 

142 if q_batch.ndim == 1: 

143 q_batch = q_batch.reshape(1, -1) 

144 

145 if q_batch.shape[1] != len(links): 

146 raise ValueError(f"Joint configuration width ({q_batch.shape[1]}) must match number of links ({len(links)})") 

147 

148 transforms = [] 

149 for i in range(q_batch.shape[0]): 

150 T = fkine_dh(links, q_batch[i]) 

151 transforms.append(T) 

152 

153 return transforms 

154 

155 

156def link_poses(links: List[DHLink], q: JointConfig, base: Optional[SE3] = None) -> List[SE3]: 

157 """ 

158 Compute poses of all link frames in world coordinates. 

159 

160 Args: 

161 links: List of DH links 

162 q: Joint configuration 

163 base: Base transformation (default: identity) 

164 

165 Returns: 

166 List of link poses in world frame 

167 

168 Examples: 

169 >>> # Get all link poses including base transformation 

170 >>> base_transform = SE3.Trans(0, 0, 0.5) # Robot on table 

171 >>> poses = link_poses(links, q, base=base_transform) 

172 """ 

173 q = np.asarray(q, dtype=float) 

174 

175 if len(q) != len(links): 

176 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})") 

177 

178 if base is None: 

179 base = SE3() 

180 

181 # Get all intermediate transformations 

182 transforms = fkine_dh_all(links, q) 

183 

184 # Apply base transformation to all poses 

185 world_poses = [base * T for T in transforms] 

186 

187 return world_poses 

188 

189 

190def joint_axes(links: List[DHLink], q: JointConfig, base: Optional[SE3] = None) -> List[np.ndarray]: 

191 """ 

192 Compute joint axis directions in world coordinates. 

193 

194 Args: 

195 links: List of DH links 

196 q: Joint configuration 

197 base: Base transformation (default: identity) 

198 

199 Returns: 

200 List of joint axis unit vectors in world frame 

201 

202 Examples: 

203 >>> # Get joint axes for visualization 

204 >>> axes = joint_axes(links, q) 

205 >>> print(f"Joint 0 axis: {axes[0]}") 

206 """ 

207 q = np.asarray(q, dtype=float) 

208 

209 if len(q) != len(links): 

210 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})") 

211 

212 if base is None: 

213 base = SE3() 

214 

215 # Get link poses 

216 poses = link_poses(links, q, base) 

217 

218 # Extract Z-axis (joint axis) from each transformation 

219 axes = [] 

220 for i, pose in enumerate(poses): 

221 # For DH convention, joint axis is always Z-axis of previous frame 

222 if i == 0: 

223 # First joint axis is Z-axis of base frame 

224 z_axis = base.R @ np.array([0, 0, 1]) 

225 else: 

226 # Joint i axis is Z-axis of frame i-1 

227 z_axis = poses[i-1].R @ np.array([0, 0, 1]) 

228 

229 axes.append(z_axis) 

230 

231 return axes 

232 

233 

234def validate_joint_config(links: List[DHLink], q: JointConfig, 

235 check_limits: bool = True, warn: bool = True) -> bool: 

236 """ 

237 Validate joint configuration against robot model. 

238 

239 Args: 

240 links: List of DH links 

241 q: Joint configuration to validate 

242 check_limits: Whether to check joint limits 

243 warn: Whether to print warnings 

244 

245 Returns: 

246 True if configuration is valid 

247 

248 Examples: 

249 >>> # Check if configuration is valid 

250 >>> valid = validate_joint_config(links, q, check_limits=True) 

251 >>> if not valid: 

252 ... print("Invalid configuration!") 

253 """ 

254 q = np.asarray(q, dtype=float) 

255 

256 # Check dimensions 

257 if len(q) != len(links): 

258 if warn: 

259 print(f"Error: Joint configuration length ({len(q)}) must match number of links ({len(links)})") 

260 return False 

261 

262 # Check for NaN or infinite values 

263 if not np.all(np.isfinite(q)): 

264 if warn: 

265 print("Error: Joint configuration contains NaN or infinite values") 

266 return False 

267 

268 # Check joint limits if requested 

269 if check_limits: 

270 for i, (link, qi) in enumerate(zip(links, q)): 

271 if link.qlim is not None: 

272 qmin, qmax = link.qlim 

273 if not (qmin <= qi <= qmax): 

274 if warn: 

275 if link.is_revolute(): 

276 print(f"Warning: Joint {i} ({qi:.3f} rad = {np.degrees(qi):.1f}°) " 

277 f"outside limits [{np.degrees(qmin):.1f}°, {np.degrees(qmax):.1f}°]") 

278 else: 

279 print(f"Warning: Joint {i} ({qi:.3f} m) outside limits [{qmin:.3f}, {qmax:.3f}] m") 

280 return False 

281 

282 return True 

283 

284 

285def fkine_performance_test(links: List[DHLink], n_iterations: int = 10000) -> dict: 

286 """ 

287 Performance test for forward kinematics computation. 

288 

289 Args: 

290 links: List of DH links 

291 n_iterations: Number of iterations to run 

292 

293 Returns: 

294 Dictionary with performance statistics 

295 

296 Examples: 

297 >>> # Test FK performance 

298 >>> stats = fkine_performance_test(links, n_iterations=10000) 

299 >>> print(f"Mean time: {stats['mean_time_us']:.2f} μs") 

300 """ 

301 import time 

302 

303 # Generate random valid joint configuration 

304 q = np.zeros(len(links)) 

305 for i, link in enumerate(links): 

306 if link.qlim is not None: 

307 qmin, qmax = link.qlim 

308 q[i] = np.random.uniform(qmin, qmax) 

309 else: 

310 if link.is_revolute(): 

311 q[i] = np.random.uniform(-np.pi, np.pi) 

312 else: 

313 q[i] = np.random.uniform(-1.0, 1.0) 

314 

315 # Warmup 

316 for _ in range(100): 

317 fkine_dh(links, q) 

318 

319 # Actual timing 

320 times = [] 

321 for _ in range(n_iterations): 

322 start = time.perf_counter() 

323 T = fkine_dh(links, q) 

324 end = time.perf_counter() 

325 times.append(end - start) 

326 

327 times = np.array(times) 

328 

329 return { 

330 'mean_time_us': np.mean(times) * 1e6, 

331 'std_time_us': np.std(times) * 1e6, 

332 'min_time_us': np.min(times) * 1e6, 

333 'max_time_us': np.max(times) * 1e6, 

334 'median_time_us': np.median(times) * 1e6, 

335 'iterations': n_iterations, 

336 'total_time_s': np.sum(times), 

337 'target_position': T.t, 

338 'target_orientation_rpy': T.rpy(), 

339 } 

340 

341 

342# Convenience functions for common robot types 

343def fkine_6dof(links: List[DHLink], q: JointConfig) -> SE3: 

344 """ 

345 Forward kinematics for 6-DOF manipulator with validation. 

346 

347 Args: 

348 links: List of exactly 6 DH links 

349 q: Joint configuration with 6 values 

350 

351 Returns: 

352 SE3 end-effector transformation 

353 

354 Raises: 

355 ValueError: If not exactly 6 DOF 

356 """ 

357 if len(links) != 6: 

358 raise ValueError(f"Expected 6 links, got {len(links)}") 

359 

360 q = np.asarray(q, dtype=float) 

361 if len(q) != 6: 

362 raise ValueError(f"Expected 6 joint values, got {len(q)}") 

363 

364 return fkine_dh(links, q) 

365 

366 

367def fkine_planar(links: List[DHLink], q: JointConfig) -> SE3: 

368 """ 

369 Forward kinematics for planar manipulator (all alpha=0). 

370 

371 Args: 

372 links: List of DH links (should have alpha=0 for all) 

373 q: Joint configuration 

374 

375 Returns: 

376 SE3 end-effector transformation 

377 """ 

378 # Verify it's actually planar 

379 for i, link in enumerate(links): 

380 if abs(link.alpha) > 1e-6: 

381 print(f"Warning: Link {i} has non-zero alpha ({link.alpha}), not truly planar") 

382 

383 return fkine_dh(links, q)