Coverage for src/robotics_numpy/models/dh_link.py: 73%
110 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"""
2DH Link classes for robotics-numpy
4This module provides DH (Denavit-Hartenberg) link representations for robot modeling.
5Supports both revolute and prismatic joints with standard DH parameters.
7The DH parameters are:
8- a: link length (distance along x_{i-1} from z_{i-1} to z_i)
9- alpha: link twist (angle about x_{i-1} from z_{i-1} to z_i)
10- d: link offset (distance along z_i from x_{i-1} to x_i)
11- theta: joint angle (angle about z_i from x_{i-1} to x_i)
13For revolute joints: theta is the joint variable, d/a/alpha are constants
14For prismatic joints: d is the joint variable, theta/a/alpha are constants
15"""
17import numpy as np
18from typing import Optional, Union, List, Tuple
19from ..transforms import SE3, rotmat, transl, rotz, rotx
21# Type aliases
22ArrayLike = Union[float, int, np.ndarray, List[float]]
25class DHLink:
26 """
27 Base class for DH (Denavit-Hartenberg) link representation.
29 This class represents a single link in a robot manipulator using
30 standard DH parameters. It can represent both revolute and prismatic joints.
32 Args:
33 d: Link offset (distance along z_i from x_{i-1} to x_i)
34 a: Link length (distance along x_{i-1} from z_{i-1} to z_i)
35 alpha: Link twist (angle about x_{i-1} from z_{i-1} to z_i)
36 theta: Joint angle (angle about z_i from x_{i-1} to x_i)
37 joint_type: Type of joint ('R' for revolute, 'P' for prismatic)
38 qlim: Joint limits [min, max] in radians (revolute) or meters (prismatic)
39 m: Link mass (kg)
40 r: Center of mass position [x, y, z] in link frame (m)
41 I: Inertia tensor [Ixx, Iyy, Izz, Ixy, Iyz, Ixz] (kg⋅m²)
42 Jm: Motor inertia (kg⋅m²)
43 G: Gear ratio
44 B: Viscous friction coefficient
45 Tc: Coulomb friction coefficients [positive, negative]
47 Examples:
48 >>> # Revolute joint
49 >>> link1 = DHLink(d=0.1, a=0.2, alpha=np.pi/2, theta=0, joint_type='R')
50 >>>
51 >>> # Prismatic joint
52 >>> link2 = DHLink(d=0, a=0, alpha=0, theta=np.pi/2, joint_type='P')
53 >>>
54 >>> # With joint limits
55 >>> link3 = DHLink(d=0.1, a=0, alpha=0, theta=0, joint_type='R',
56 ... qlim=[-np.pi, np.pi])
57 """
59 def __init__(
60 self,
61 d: float = 0.0,
62 a: float = 0.0,
63 alpha: float = 0.0,
64 theta: float = 0.0,
65 joint_type: str = 'R',
66 qlim: Optional[List[float]] = None,
67 m: Optional[float] = None,
68 r: Optional[List[float]] = None,
69 I: Optional[List[float]] = None,
70 Jm: Optional[float] = None,
71 G: Optional[float] = None,
72 B: Optional[float] = None,
73 Tc: Optional[List[float]] = None,
74 ):
75 # DH parameters
76 self.d = float(d)
77 self.a = float(a)
78 self.alpha = float(alpha)
79 self.theta = float(theta)
81 # Joint type validation
82 if joint_type not in ['R', 'P', 'revolute', 'prismatic']:
83 raise ValueError("joint_type must be 'R', 'P', 'revolute', or 'prismatic'")
85 self.joint_type = joint_type.upper()[0] # Normalize to 'R' or 'P'
87 # Joint limits
88 if qlim is not None:
89 if len(qlim) != 2:
90 raise ValueError("qlim must be a 2-element list [min, max]")
91 if qlim[0] >= qlim[1]:
92 raise ValueError("qlim[0] must be less than qlim[1]")
93 self.qlim = [float(qlim[0]), float(qlim[1])]
94 else:
95 self.qlim = None
97 # Dynamic parameters (optional)
98 self.m = float(m) if m is not None else None
100 if r is not None:
101 if len(r) != 3:
102 raise ValueError("r (center of mass) must be a 3-element list [x, y, z]")
103 self.r = [float(x) for x in r]
104 else:
105 self.r = None
107 if I is not None:
108 if len(I) != 6:
109 raise ValueError("I (inertia) must be a 6-element list [Ixx, Iyy, Izz, Ixy, Iyz, Ixz]")
110 self.I = [float(x) for x in I]
111 else:
112 self.I = None
114 self.Jm = float(Jm) if Jm is not None else None
115 self.G = float(G) if G is not None else None
116 self.B = float(B) if B is not None else None
118 if Tc is not None:
119 if len(Tc) != 2:
120 raise ValueError("Tc (Coulomb friction) must be a 2-element list [positive, negative]")
121 self.Tc = [float(Tc[0]), float(Tc[1])]
122 else:
123 self.Tc = None
125 def is_revolute(self) -> bool:
126 """Check if this is a revolute joint."""
127 return self.joint_type == 'R'
129 def is_prismatic(self) -> bool:
130 """Check if this is a prismatic joint."""
131 return self.joint_type == 'P'
133 def A(self, q: float) -> SE3:
134 """
135 Compute link transformation matrix for given joint value.
137 Computes the homogeneous transformation matrix from link frame {i-1}
138 to link frame {i} using standard DH convention.
140 The transformation is: T = Tz(d) * Rz(theta) * Tx(a) * Rx(alpha)
142 Args:
143 q: Joint value (angle in radians for revolute, distance in meters for prismatic)
145 Returns:
146 SE3 transformation matrix from frame {i-1} to frame {i}
148 Examples:
149 >>> link = DHLink(d=0.1, a=0.2, alpha=np.pi/2, theta=0, joint_type='R')
150 >>> T = link.A(np.pi/4) # 45 degree rotation
151 >>> print(T)
152 """
153 q = float(q)
155 # Determine actual DH parameters based on joint type
156 if self.is_revolute():
157 d = self.d
158 theta = self.theta + q # Joint variable is added to fixed offset
159 a = self.a
160 alpha = self.alpha
161 else: # Prismatic
162 d = self.d + q # Joint variable is added to fixed offset
163 theta = self.theta
164 a = self.a
165 alpha = self.alpha
167 # Check joint limits if specified
168 if self.qlim is not None:
169 if not (self.qlim[0] <= q <= self.qlim[1]):
170 print(f"Warning: Joint value {q:.3f} outside limits {self.qlim}")
172 # Build transformation using DH convention: Tz(d) * Rz(theta) * Tx(a) * Rx(alpha)
173 T = SE3.Trans(0, 0, d) * SE3.Rz(theta) * SE3.Trans(a, 0, 0) * SE3.Rx(alpha)
175 return T
177 def copy(self) -> 'DHLink':
178 """Create a copy of this DH link."""
179 return DHLink(
180 d=self.d,
181 a=self.a,
182 alpha=self.alpha,
183 theta=self.theta,
184 joint_type=self.joint_type,
185 qlim=self.qlim.copy() if self.qlim else None,
186 m=self.m,
187 r=self.r.copy() if self.r else None,
188 I=self.I.copy() if self.I else None,
189 Jm=self.Jm,
190 G=self.G,
191 B=self.B,
192 Tc=self.Tc.copy() if self.Tc else None,
193 )
195 def __str__(self) -> str:
196 """String representation of DH link."""
197 joint_name = "Revolute" if self.is_revolute() else "Prismatic"
199 # Format DH parameters
200 params = f"d={self.d:.3f}, a={self.a:.3f}, α={self.alpha:.3f}, θ={self.theta:.3f}"
202 # Add joint limits if present
203 limits = ""
204 if self.qlim:
205 if self.is_revolute():
206 limits = f", qlim=[{np.degrees(self.qlim[0]):.1f}°, {np.degrees(self.qlim[1]):.1f}°]"
207 else:
208 limits = f", qlim=[{self.qlim[0]:.3f}, {self.qlim[1]:.3f}]m"
210 # Add mass if present
211 mass = f", m={self.m:.2f}kg" if self.m is not None else ""
213 return f"{joint_name}DH({params}{limits}{mass})"
215 def __repr__(self) -> str:
216 """Detailed representation of DH link."""
217 return self.__str__()
220class RevoluteDH(DHLink):
221 """
222 Revolute DH link - joint variable is theta.
224 Convenience class for creating revolute joints where theta is the joint variable.
226 Args:
227 d: Link offset (constant)
228 a: Link length (constant)
229 alpha: Link twist (constant)
230 theta: Joint angle offset (added to joint variable)
231 qlim: Joint angle limits [min, max] in radians
232 **kwargs: Additional parameters passed to DHLink
234 Examples:
235 >>> # Simple revolute joint
236 >>> link = RevoluteDH(d=0.1, a=0.2, alpha=np.pi/2)
237 >>>
238 >>> # With joint limits
239 >>> link = RevoluteDH(d=0.1, a=0, alpha=0, qlim=[-np.pi, np.pi])
240 """
242 def __init__(
243 self,
244 d: float = 0.0,
245 a: float = 0.0,
246 alpha: float = 0.0,
247 theta: float = 0.0,
248 qlim: Optional[List[float]] = None,
249 **kwargs
250 ):
251 super().__init__(
252 d=d,
253 a=a,
254 alpha=alpha,
255 theta=theta,
256 joint_type='R',
257 qlim=qlim,
258 **kwargs
259 )
262class PrismaticDH(DHLink):
263 """
264 Prismatic DH link - joint variable is d.
266 Convenience class for creating prismatic joints where d is the joint variable.
268 Args:
269 theta: Joint angle (constant)
270 a: Link length (constant)
271 alpha: Link twist (constant)
272 d: Link offset (added to joint variable)
273 qlim: Joint displacement limits [min, max] in meters
274 **kwargs: Additional parameters passed to DHLink
276 Examples:
277 >>> # Simple prismatic joint
278 >>> link = PrismaticDH(theta=0, a=0, alpha=0)
279 >>>
280 >>> # With joint limits
281 >>> link = PrismaticDH(theta=np.pi/2, a=0, alpha=0, qlim=[0, 0.5])
282 """
284 def __init__(
285 self,
286 theta: float = 0.0,
287 a: float = 0.0,
288 alpha: float = 0.0,
289 d: float = 0.0,
290 qlim: Optional[List[float]] = None,
291 **kwargs
292 ):
293 super().__init__(
294 d=d,
295 a=a,
296 alpha=alpha,
297 theta=theta,
298 joint_type='P',
299 qlim=qlim,
300 **kwargs
301 )
304def dh_check_parameters(links: List[DHLink]) -> bool:
305 """
306 Validate DH parameters for a chain of links.
308 Args:
309 links: List of DH links
311 Returns:
312 True if parameters are valid
314 Raises:
315 ValueError: If parameters are invalid
316 """
317 if not links:
318 raise ValueError("Empty link list")
320 if not all(isinstance(link, DHLink) for link in links):
321 raise ValueError("All elements must be DHLink instances")
323 # Check for reasonable parameter ranges
324 for i, link in enumerate(links):
325 # Check alpha is in reasonable range
326 if abs(link.alpha) > 2 * np.pi:
327 raise ValueError(f"Link {i}: alpha ({link.alpha}) should be in [-2π, 2π]")
329 # Check theta is in reasonable range (for fixed part)
330 if abs(link.theta) > 2 * np.pi:
331 raise ValueError(f"Link {i}: theta offset ({link.theta}) should be in [-2π, 2π]")
333 # Check joint limits are reasonable
334 if link.qlim is not None:
335 if link.is_revolute():
336 if abs(link.qlim[0]) > 4 * np.pi or abs(link.qlim[1]) > 4 * np.pi:
337 print(f"Warning: Link {i} has very large joint limits (>720°)")
338 else: # Prismatic
339 if abs(link.qlim[0]) > 10 or abs(link.qlim[1]) > 10:
340 print(f"Warning: Link {i} has very large displacement limits (>10m)")
342 return True
345# Utility functions for common DH parameter patterns
346def create_6dof_revolute_arm(
347 lengths: Optional[List[float]] = None,
348 offsets: Optional[List[float]] = None
349) -> List[DHLink]:
350 """
351 Create a standard 6-DOF revolute arm with common DH parameters.
353 Args:
354 lengths: Link lengths [a1, a2, a3, a4, a5, a6] (default: [0, 0.3, 0.3, 0, 0, 0])
355 offsets: Link offsets [d1, d2, d3, d4, d5, d6] (default: [0.3, 0, 0, 0.3, 0, 0.1])
357 Returns:
358 List of 6 DH links representing a typical 6-DOF arm
360 Examples:
361 >>> links = create_6dof_revolute_arm()
362 >>> robot = DHRobot(links)
363 """
364 if lengths is None:
365 lengths = [0, 0.3, 0.3, 0, 0, 0] # Typical arm lengths
366 if offsets is None:
367 offsets = [0.3, 0, 0, 0.3, 0, 0.1] # Typical arm offsets
369 if len(lengths) != 6 or len(offsets) != 6:
370 raise ValueError("lengths and offsets must have 6 elements each")
372 # Standard 6-DOF arm DH parameters
373 alphas = [np.pi/2, 0, np.pi/2, -np.pi/2, np.pi/2, 0]
374 joint_limits = [
375 [-np.pi, np.pi], # Base rotation
376 [-np.pi/2, np.pi/2], # Shoulder
377 [-np.pi, np.pi], # Elbow
378 [-np.pi, np.pi], # Wrist roll
379 [-np.pi/2, np.pi/2], # Wrist pitch
380 [-np.pi, np.pi], # Wrist yaw
381 ]
383 links = []
384 for i in range(6):
385 link = RevoluteDH(
386 d=offsets[i],
387 a=lengths[i],
388 alpha=alphas[i],
389 theta=0,
390 qlim=joint_limits[i]
391 )
392 links.append(link)
394 return links