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

1""" 

2DH Link classes for robotics-numpy 

3 

4This module provides DH (Denavit-Hartenberg) link representations for robot modeling. 

5Supports both revolute and prismatic joints with standard DH parameters. 

6 

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) 

12 

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

16 

17import numpy as np 

18from typing import Optional, Union, List, Tuple 

19from ..transforms import SE3, rotmat, transl, rotz, rotx 

20 

21# Type aliases 

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

23 

24 

25class DHLink: 

26 """ 

27 Base class for DH (Denavit-Hartenberg) link representation. 

28 

29 This class represents a single link in a robot manipulator using 

30 standard DH parameters. It can represent both revolute and prismatic joints. 

31 

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] 

46 

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

58 

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) 

80 

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

84 

85 self.joint_type = joint_type.upper()[0] # Normalize to 'R' or 'P' 

86 

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 

96 

97 # Dynamic parameters (optional) 

98 self.m = float(m) if m is not None else None 

99 

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 

106 

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 

113 

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 

117 

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 

124 

125 def is_revolute(self) -> bool: 

126 """Check if this is a revolute joint.""" 

127 return self.joint_type == 'R' 

128 

129 def is_prismatic(self) -> bool: 

130 """Check if this is a prismatic joint.""" 

131 return self.joint_type == 'P' 

132 

133 def A(self, q: float) -> SE3: 

134 """ 

135 Compute link transformation matrix for given joint value. 

136 

137 Computes the homogeneous transformation matrix from link frame {i-1} 

138 to link frame {i} using standard DH convention. 

139 

140 The transformation is: T = Tz(d) * Rz(theta) * Tx(a) * Rx(alpha) 

141 

142 Args: 

143 q: Joint value (angle in radians for revolute, distance in meters for prismatic) 

144 

145 Returns: 

146 SE3 transformation matrix from frame {i-1} to frame {i} 

147 

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) 

154 

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 

166 

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

171 

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) 

174 

175 return T 

176 

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 ) 

194 

195 def __str__(self) -> str: 

196 """String representation of DH link.""" 

197 joint_name = "Revolute" if self.is_revolute() else "Prismatic" 

198 

199 # Format DH parameters 

200 params = f"d={self.d:.3f}, a={self.a:.3f}, α={self.alpha:.3f}, θ={self.theta:.3f}" 

201 

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" 

209 

210 # Add mass if present 

211 mass = f", m={self.m:.2f}kg" if self.m is not None else "" 

212 

213 return f"{joint_name}DH({params}{limits}{mass})" 

214 

215 def __repr__(self) -> str: 

216 """Detailed representation of DH link.""" 

217 return self.__str__() 

218 

219 

220class RevoluteDH(DHLink): 

221 """ 

222 Revolute DH link - joint variable is theta. 

223 

224 Convenience class for creating revolute joints where theta is the joint variable. 

225 

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 

233 

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

241 

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 ) 

260 

261 

262class PrismaticDH(DHLink): 

263 """ 

264 Prismatic DH link - joint variable is d. 

265 

266 Convenience class for creating prismatic joints where d is the joint variable. 

267 

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 

275 

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

283 

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 ) 

302 

303 

304def dh_check_parameters(links: List[DHLink]) -> bool: 

305 """ 

306 Validate DH parameters for a chain of links. 

307 

308 Args: 

309 links: List of DH links 

310 

311 Returns: 

312 True if parameters are valid 

313 

314 Raises: 

315 ValueError: If parameters are invalid 

316 """ 

317 if not links: 

318 raise ValueError("Empty link list") 

319 

320 if not all(isinstance(link, DHLink) for link in links): 

321 raise ValueError("All elements must be DHLink instances") 

322 

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

328 

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

332 

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

341 

342 return True 

343 

344 

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. 

352 

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

356 

357 Returns: 

358 List of 6 DH links representing a typical 6-DOF arm 

359 

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 

368 

369 if len(lengths) != 6 or len(offsets) != 6: 

370 raise ValueError("lengths and offsets must have 6 elements each") 

371 

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 ] 

382 

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) 

393 

394 return links