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
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
1"""
2Forward kinematics algorithms for robotics-numpy
4This module provides efficient forward kinematics computation for robot manipulators
5using DH parameters. Optimized for performance with NumPy vectorization.
6"""
8import numpy as np
9from typing import List, Union, Optional
10from ..transforms import SE3
11from ..models.dh_link import DHLink
13# Type aliases
14ArrayLike = Union[float, int, np.ndarray, List[float]]
15JointConfig = Union[List[float], np.ndarray]
18def fkine_dh(links: List[DHLink], q: JointConfig) -> SE3:
19 """
20 Forward kinematics using DH parameters.
22 Computes the transformation from the base frame to the end-effector frame
23 by multiplying individual link transformation matrices.
25 Args:
26 links: List of DH links
27 q: Joint configuration (n joint values)
29 Returns:
30 SE3 transformation from base to end-effector
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)
43def fkine_dh_all(links: List[DHLink], q: JointConfig) -> List[SE3]:
44 """
45 Forward kinematics for all intermediate frames.
47 Computes transformations from base to each link frame, useful for
48 visualization and intermediate pose calculations.
50 Args:
51 links: List of DH links
52 q: Joint configuration
54 Returns:
55 List of SE3 transformations, one for each link frame
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)
66 if len(q) != len(links):
67 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})")
69 transforms = []
70 T = SE3() # Start with identity
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}")
81 return transforms
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.
89 Computes transformation from start link to end link (inclusive).
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)
97 Returns:
98 SE3 transformation from start to end frame
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)
106 if len(q) != len(links):
107 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})")
109 if end is None:
110 end = len(links) - 1
112 if not (0 <= start <= end < len(links)):
113 raise ValueError(f"Invalid range: start={start}, end={end}, n_links={len(links)}")
115 # Extract relevant links and joint values
116 partial_links = links[start:end+1]
117 partial_q = q[start:end+1]
119 return fkine_dh(partial_links, partial_q)
122def fkine_dh_batch(links: List[DHLink], q_batch: np.ndarray) -> List[SE3]:
123 """
124 Batch forward kinematics for multiple configurations.
126 Efficiently computes forward kinematics for multiple joint configurations.
128 Args:
129 links: List of DH links
130 q_batch: Batch of joint configurations (m x n array)
132 Returns:
133 List of SE3 transformations, one for each configuration
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)
142 if q_batch.ndim == 1:
143 q_batch = q_batch.reshape(1, -1)
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)})")
148 transforms = []
149 for i in range(q_batch.shape[0]):
150 T = fkine_dh(links, q_batch[i])
151 transforms.append(T)
153 return transforms
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.
160 Args:
161 links: List of DH links
162 q: Joint configuration
163 base: Base transformation (default: identity)
165 Returns:
166 List of link poses in world frame
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)
175 if len(q) != len(links):
176 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})")
178 if base is None:
179 base = SE3()
181 # Get all intermediate transformations
182 transforms = fkine_dh_all(links, q)
184 # Apply base transformation to all poses
185 world_poses = [base * T for T in transforms]
187 return world_poses
190def joint_axes(links: List[DHLink], q: JointConfig, base: Optional[SE3] = None) -> List[np.ndarray]:
191 """
192 Compute joint axis directions in world coordinates.
194 Args:
195 links: List of DH links
196 q: Joint configuration
197 base: Base transformation (default: identity)
199 Returns:
200 List of joint axis unit vectors in world frame
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)
209 if len(q) != len(links):
210 raise ValueError(f"Joint configuration length ({len(q)}) must match number of links ({len(links)})")
212 if base is None:
213 base = SE3()
215 # Get link poses
216 poses = link_poses(links, q, base)
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])
229 axes.append(z_axis)
231 return axes
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.
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
245 Returns:
246 True if configuration is valid
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)
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
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
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
282 return True
285def fkine_performance_test(links: List[DHLink], n_iterations: int = 10000) -> dict:
286 """
287 Performance test for forward kinematics computation.
289 Args:
290 links: List of DH links
291 n_iterations: Number of iterations to run
293 Returns:
294 Dictionary with performance statistics
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
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)
315 # Warmup
316 for _ in range(100):
317 fkine_dh(links, q)
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)
327 times = np.array(times)
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 }
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.
347 Args:
348 links: List of exactly 6 DH links
349 q: Joint configuration with 6 values
351 Returns:
352 SE3 end-effector transformation
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)}")
360 q = np.asarray(q, dtype=float)
361 if len(q) != 6:
362 raise ValueError(f"Expected 6 joint values, got {len(q)}")
364 return fkine_dh(links, q)
367def fkine_planar(links: List[DHLink], q: JointConfig) -> SE3:
368 """
369 Forward kinematics for planar manipulator (all alpha=0).
371 Args:
372 links: List of DH links (should have alpha=0 for all)
373 q: Joint configuration
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")
383 return fkine_dh(links, q)