Coverage for src/robotics_numpy/models/generic.py: 84%

190 statements  

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

1""" 

2Generic robot model for robotics-numpy 

3 

4This module implements a fully generic robotic arm class using Denavit-Hartenberg 

5parameters. It allows creating robots with arbitrary number of joints and fully 

6configurable DH parameters. 

7 

8The Generic class is useful for: 

9- Educational purposes and experimentation 

10- Rapid prototyping of robot designs 

11- Testing kinematics algorithms 

12- Creating custom robot configurations 

13""" 

14 

15import numpy as np 

16from typing import Optional, List, Union 

17from .dh_robot import DHRobot 

18from .dh_link import RevoluteDH, PrismaticDH 

19 

20# Constants 

21pi = np.pi 

22deg = pi / 180 

23 

24 

25class Generic(DHRobot): 

26 """ 

27 A fully generic robotic arm class using Denavit-Hartenberg parameters. 

28 

29 The class represents a robot arm with arbitrary number of joints and fully configurable 

30 DH parameters. It inherits from DHRobot and uses standard DH parameters to define the kinematics. 

31 

32 The robot has the following key features: 

33 - Configurable number of DOFs (degrees of freedom) 

34 - Fully customizable DH parameters: a, d, alpha, offset 

35 - Support for both revolute and prismatic joints 

36 - Ready-poses defined for: 

37 - qr: "ready" position (all joints at offset values) 

38 - qz: "zero" position (all joints at 0) 

39 - Optional 3D visualization with Plotly 

40 

41 Examples: 

42 >>> # Create a 4-DOF robot 

43 >>> robot = Generic( 

44 ... dofs=4, 

45 ... a=[0, -0.5, -0.5, -0.5], 

46 ... d=[0.5, 0, 0, 0], 

47 ... alpha=[pi/2, 0, 0, 0], 

48 ... name="Custom4DOF" 

49 ... ) 

50 >>> 

51 >>> # Forward kinematics 

52 >>> T = robot.fkine(robot.qz) 

53 >>> print(f"End-effector pose: {T}") 

54 >>> 

55 >>> # Visualization (requires plotly) 

56 >>> robot.plotly(robot.qz) 

57 """ 

58 

59 def __init__( 

60 self, 

61 dofs: int, 

62 a: Optional[List[float]] = None, 

63 d: Optional[List[float]] = None, 

64 alpha: Optional[List[float]] = None, 

65 offset: Optional[List[float]] = None, 

66 qlim: Optional[List[List[float]]] = None, 

67 joint_types: Optional[List[str]] = None, 

68 name: str = "GenericDH", 

69 **kwargs, 

70 ): 

71 """ 

72 Initialize the generic robot with configurable DH parameters. 

73 

74 Args: 

75 dofs: Number of degrees of freedom 

76 a: Link lengths along common normal [dofs elements] (default: 0.1m each) 

77 d: Link offsets along previous z to common normal [dofs elements] (default: 0.1m each) 

78 alpha: Link twist angles in radians [dofs elements] (default: 0 each) 

79 offset: Joint angle offsets in radians [dofs elements] (default: 0 each) 

80 qlim: Joint limits as [[min, max], ...] [dofs pairs] (default: ±180° each) 

81 joint_types: Joint types 'R' or 'P' [dofs elements] (default: all 'R') 

82 name: Name of the robot 

83 **kwargs: Additional arguments passed to DHRobot 

84 

85 Raises: 

86 ValueError: If parameter dimensions don't match dofs 

87 """ 

88 if not isinstance(dofs, int) or dofs < 1: 

89 raise ValueError("dofs must be a positive integer") 

90 

91 self.dofs = dofs 

92 

93 # Set default values if parameters are not provided 

94 if a is None: 

95 a = [0.1] * dofs # Default link length of 0.1m 

96 if d is None: 

97 d = [0.1] * dofs # Default link offset of 0.1m 

98 if alpha is None: 

99 alpha = [0.0] * dofs # Default twist angle of 0 

100 if offset is None: 

101 offset = [0.0] * dofs # Default joint offset of 0 

102 if qlim is None: 

103 qlim = [[-180 * deg, 180 * deg]] * dofs # Default ±180 degrees 

104 if joint_types is None: 

105 joint_types = ["R"] * dofs # Default to all revolute joints 

106 

107 # Validate input dimensions 

108 if len(a) != dofs: 

109 raise ValueError(f"Length of 'a' ({len(a)}) must equal dofs ({dofs})") 

110 if len(d) != dofs: 

111 raise ValueError(f"Length of 'd' ({len(d)}) must equal dofs ({dofs})") 

112 if len(alpha) != dofs: 

113 raise ValueError( 

114 f"Length of 'alpha' ({len(alpha)}) must equal dofs ({dofs})" 

115 ) 

116 if len(offset) != dofs: 

117 raise ValueError( 

118 f"Length of 'offset' ({len(offset)}) must equal dofs ({dofs})" 

119 ) 

120 if len(qlim) != dofs: 

121 raise ValueError(f"Length of 'qlim' ({len(qlim)}) must equal dofs ({dofs})") 

122 if len(joint_types) != dofs: 

123 raise ValueError( 

124 f"Length of 'joint_types' ({len(joint_types)}) must equal dofs ({dofs})" 

125 ) 

126 

127 # Validate joint types 

128 for i, jtype in enumerate(joint_types): 

129 if jtype not in ["R", "P"]: 

130 raise ValueError(f"joint_types[{i}] must be 'R' or 'P', got '{jtype}'") 

131 

132 # Validate qlim format 

133 for i, limit in enumerate(qlim): 

134 if len(limit) != 2: 

135 raise ValueError(f"qlim[{i}] must have exactly 2 elements [min, max]") 

136 if limit[0] >= limit[1]: 

137 raise ValueError( 

138 f"qlim[{i}] min ({limit[0]}) must be less than max ({limit[1]})" 

139 ) 

140 

141 # Store parameters 

142 self._a = list(a) 

143 self._d = list(d) 

144 self._alpha = list(alpha) 

145 self._offset = list(offset) 

146 self._qlim = [list(limit) for limit in qlim] 

147 self._joint_types = list(joint_types) 

148 

149 # Default dynamic properties (can be customized later) 

150 r = [ 

151 [0.0, 0.0, 0.0] for _ in range(dofs) 

152 ] # Position of COM with respect to link frame 

153 I = [ 

154 [0.0] * 6 for _ in range(dofs) 

155 ] # Inertia tensor [Ixx, Iyy, Izz, Ixy, Iyz, Ixz] 

156 m = [0.0] * dofs # mass of link 

157 Jm = [0.0] * dofs # actuator inertia 

158 G = [1.0] * dofs # gear ratio 

159 B = [0.0] * dofs # actuator viscous friction coefficient 

160 Tc = [ 

161 [0.0, 0.0] for _ in range(dofs) 

162 ] # Coulomb friction coefficient for direction [-,+] 

163 

164 # Create links based on joint types 

165 links = [] 

166 for i in range(dofs): 

167 if joint_types[i] == "R": 

168 # Revolute joint 

169 link = RevoluteDH( 

170 d=d[i], 

171 a=a[i], 

172 alpha=alpha[i], 

173 theta=offset[i], # theta is the offset for revolute joints 

174 r=r[i], 

175 I=I[i], 

176 m=m[i], 

177 Jm=Jm[i], 

178 G=G[i], 

179 B=B[i], 

180 Tc=Tc[i], 

181 qlim=qlim[i], 

182 ) 

183 else: 

184 # Prismatic joint 

185 link = PrismaticDH( 

186 theta=offset[i], # For prismatic, offset is the fixed angle 

187 a=a[i], 

188 alpha=alpha[i], 

189 d=0.0, # d is variable for prismatic joints 

190 r=r[i], 

191 I=I[i], 

192 m=m[i], 

193 Jm=Jm[i], 

194 G=G[i], 

195 B=B[i], 

196 Tc=Tc[i], 

197 qlim=qlim[i], 

198 ) 

199 links.append(link) 

200 

201 # Initialize parent class 

202 super().__init__( 

203 links, 

204 name=name, 

205 manufacturer="generic", 

206 keywords=("configurable", "educational", "generic"), 

207 **kwargs, 

208 ) 

209 

210 # Ready pose: joint angles at offset values for revolute, 0 for prismatic 

211 self.qr = np.array( 

212 [offset[i] if joint_types[i] == "R" else 0.0 for i in range(dofs)] 

213 ) 

214 

215 # Zero pose: all joint values at 0 

216 self.qz = np.zeros(dofs) 

217 

218 # Add configurations 

219 self.addconfiguration("qr", self.qr) 

220 self.addconfiguration("qz", self.qz) 

221 

222 @property 

223 def dh_parameters(self) -> dict: 

224 """Return DH parameters as a dictionary.""" 

225 return { 

226 "a": self._a.copy(), 

227 "d": self._d.copy(), 

228 "alpha": self._alpha.copy(), 

229 "offset": self._offset.copy(), 

230 "qlim": [limit.copy() for limit in self._qlim], 

231 "joint_types": self._joint_types.copy(), 

232 } 

233 

234 def set_dynamic_properties( 

235 self, 

236 m: Optional[List[float]] = None, 

237 r: Optional[List[List[float]]] = None, 

238 I: Optional[List[List[float]]] = None, 

239 Jm: Optional[List[float]] = None, 

240 G: Optional[List[float]] = None, 

241 B: Optional[List[float]] = None, 

242 Tc: Optional[List[List[float]]] = None, 

243 ) -> None: 

244 """ 

245 Set dynamic properties for the robot links. 

246 

247 Args: 

248 m: Mass of each link [dofs elements] 

249 r: Position of COM with respect to link frame [dofs x 3] 

250 I: Inertia tensor of each link [dofs x 6] as [Ixx, Iyy, Izz, Ixy, Iyz, Ixz] 

251 Jm: Actuator inertia [dofs elements] 

252 G: Gear ratio [dofs elements] 

253 B: Viscous friction coefficient [dofs elements] 

254 Tc: Coulomb friction coefficient [dofs x 2] as [negative, positive] 

255 

256 Raises: 

257 ValueError: If parameter dimensions don't match dofs 

258 """ 

259 

260 def _validate_and_set(param, param_name, expected_shape, attr_name): 

261 if param is not None: 

262 if isinstance(expected_shape, int): 

263 if len(param) != expected_shape: 

264 raise ValueError( 

265 f"{param_name} must have length {expected_shape}" 

266 ) 

267 for i, link in enumerate(self.links): 

268 setattr(link, attr_name, param[i]) 

269 else: 

270 if len(param) != expected_shape[0]: 

271 raise ValueError( 

272 f"{param_name} must have {expected_shape[0]} elements" 

273 ) 

274 for i, p in enumerate(param): 

275 if len(p) != expected_shape[1]: 

276 raise ValueError( 

277 f"{param_name}[{i}] must have {expected_shape[1]} elements" 

278 ) 

279 setattr(self.links[i], attr_name, list(p)) 

280 

281 _validate_and_set(m, "m", self.dofs, "m") 

282 _validate_and_set(r, "r", (self.dofs, 3), "r") 

283 _validate_and_set(I, "I", (self.dofs, 6), "I") 

284 _validate_and_set(Jm, "Jm", self.dofs, "Jm") 

285 _validate_and_set(G, "G", self.dofs, "G") 

286 _validate_and_set(B, "B", self.dofs, "B") 

287 _validate_and_set(Tc, "Tc", (self.dofs, 2), "Tc") 

288 

289 def workspace_radius(self) -> float: 

290 """ 

291 Estimate maximum workspace radius. 

292 

293 Returns: 

294 Maximum reach from base in meters 

295 """ 

296 # Simple estimate: sum of all link lengths and max extensions 

297 max_reach = 0.0 

298 for i, link in enumerate(self.links): 

299 max_reach += abs(self._a[i]) # Link length 

300 if link.is_prismatic(): 

301 # Add maximum prismatic extension 

302 max_reach += max(abs(self._qlim[i][0]), abs(self._qlim[i][1])) 

303 else: 

304 # Add link offset for revolute joints 

305 max_reach += abs(self._d[i]) 

306 return max_reach 

307 

308 def summary(self) -> str: 

309 """ 

310 Generate a summary string of the robot configuration. 

311 

312 Returns: 

313 Multi-line string describing the robot 

314 """ 

315 lines = [ 

316 f"Generic Robot: {self.name}", 

317 f"DOFs: {self.dofs}", 

318 f"Joint types: {' '.join(self._joint_types)}", 

319 f"Max reach: {self.workspace_radius():.3f} m", 

320 "", 

321 "DH Parameters:", 

322 "Link | a | d | alpha | offset | type | qlim ", 

323 "-----+--------+--------+--------+--------+------+------------", 

324 ] 

325 

326 for i in range(self.dofs): 

327 alpha_deg = self._alpha[i] * 180 / pi 

328 offset_deg = ( 

329 self._offset[i] * 180 / pi 

330 if self._joint_types[i] == "R" 

331 else self._offset[i] 

332 ) 

333 offset_unit = "°" if self._joint_types[i] == "R" else "m" 

334 

335 qlim_min = ( 

336 self._qlim[i][0] * 180 / pi 

337 if self._joint_types[i] == "R" 

338 else self._qlim[i][0] 

339 ) 

340 qlim_max = ( 

341 self._qlim[i][1] * 180 / pi 

342 if self._joint_types[i] == "R" 

343 else self._qlim[i][1] 

344 ) 

345 qlim_unit = "°" if self._joint_types[i] == "R" else "m" 

346 

347 lines.append( 

348 f" {i:2d} | {self._a[i]:6.3f} | {self._d[i]:6.3f} | " 

349 f"{alpha_deg:6.1f} | {offset_deg:6.1f}{offset_unit} | " 

350 f" {self._joint_types[i]} | " 

351 f"[{qlim_min:5.1f},{qlim_max:5.1f}]{qlim_unit}" 

352 ) 

353 

354 return "\n".join(lines) 

355 

356 def demo(self) -> None: 

357 """ 

358 Demonstrate Generic robot capabilities. 

359 """ 

360 print("Generic Robot Demonstration") 

361 print("=" * 50) 

362 

363 print(f"\n{self.summary()}") 

364 

365 print(f"\n\nConfigurations:") 

366 for config_name in ["qr", "qz"]: 

367 q = getattr(self, config_name) 

368 T = self.fkine(q) 

369 print(f" {config_name}: {q}") 

370 print( 

371 f" End-effector position: [{T.t[0]:.3f}, {T.t[1]:.3f}, {T.t[2]:.3f}]" 

372 ) 

373 

374 print(f"\nJoint limits check:") 

375 test_q = np.zeros(self.dofs) 

376 within_limits = not np.any(self.islimit(test_q)) 

377 print(f" Zero config within limits: {within_limits}") 

378 

379 def __str__(self) -> str: 

380 """String representation of the robot.""" 

381 return self.summary() 

382 

383 def __repr__(self) -> str: 

384 """Detailed representation of the robot.""" 

385 return f"Generic(dofs={self.dofs}, name='{self.name}')" 

386 

387 def plotly( 

388 self, 

389 q: Optional[List[float]] = None, 

390 frame_size: float = 0.1, # Size of coordinate frames in meters 

391 is_show: bool = True, # Whether to display the plot immediately 

392 show_z_axis: bool = True, 

393 show_all_axes: bool = False, 

394 **kwargs, 

395 ) -> "plotly.graph_objects.Figure": 

396 """ 

397 Visualize the robot using Plotly (if available). 

398 

399 Args: 

400 q: Joint configuration to visualize (default: qz) 

401 **kwargs: Additional arguments for visualization 

402 

403 Raises: 

404 ImportError: If Plotly is not installed 

405 """ 

406 try: 

407 import plotly.graph_objects as go 

408 import plotly.express as px 

409 except ImportError: 

410 raise ImportError( 

411 "Plotly is required for visualization. Install with: pip install plotly" 

412 ) 

413 

414 if q is None: 

415 q = self.qz 

416 

417 # Compute forward kinematics for all links 

418 link_poses = [] 

419 for i in range(self.n + 1): # Include base 

420 if i == 0: 

421 T = self.base # Base frame 

422 else: 

423 T = self.fkine(q, end=i - 1) 

424 link_poses.append(T) 

425 

426 # Extract positions for plotting 

427 positions = np.array([T.t for T in link_poses]) 

428 

429 # Create 3D plot 

430 fig = go.Figure() 

431 

432 # Plot robot links 

433 fig.add_trace( 

434 go.Scatter3d( 

435 x=positions[:, 0], 

436 y=positions[:, 1], 

437 z=positions[:, 2], 

438 mode="lines", 

439 name=f"{self.name} Links", 

440 line=dict(width=15, color="#E1706E"), 

441 ) 

442 ) 

443 

444 # Add coordinate frames at each joint 

445 colors = ["#F84752", "#BBDA55", "#8EC1E1"] # X, Y, Z axis colors 

446 directions = ["X", "Y", "Z"] 

447 arrow_length = frame_size * 0.2 # Length of the cone arrowhead 

448 

449 for i, T in enumerate(link_poses): 

450 origin = T.t 

451 for axis_idx in range(3): # X, Y, Z axes 

452 if (axis_idx < 2 and show_all_axes) or (axis_idx == 2 and show_z_axis): 

453 direction = T.R[:, axis_idx] / np.linalg.norm(T.R[:, axis_idx]) 

454 end_point = origin + frame_size * direction 

455 

456 # Determine color for Z-axis of the last frame 

457 if axis_idx == 2 and i == len(link_poses) - 1: # Last frame Z-axis 

458 axis_color = "#800080" # Purple for end-effector Z-axis 

459 else: 

460 axis_color = colors[axis_idx] 

461 

462 # Add axis line 

463 fig.add_trace( 

464 go.Scatter3d( 

465 x=[origin[0], end_point[0]], 

466 y=[origin[1], end_point[1]], 

467 z=[origin[2], end_point[2]], 

468 mode="lines", 

469 name=f"Frame {i} {directions[axis_idx]}", 

470 line=dict(width=5, color=axis_color), 

471 ) 

472 ) 

473 

474 # Add cone (arrowhead) 

475 fig.add_trace( 

476 go.Cone( 

477 x=[end_point[0]], 

478 y=[end_point[1]], 

479 z=[end_point[2]], 

480 u=[direction[0]], 

481 v=[direction[1]], 

482 w=[direction[2]], 

483 sizemode="absolute", 

484 sizeref=arrow_length, 

485 showscale=False, 

486 colorscale=[[0, axis_color], [1, axis_color]], 

487 cmin=0, 

488 cmax=1, 

489 ) 

490 ) 

491 

492 # Print configuration info 

493 print(f"\nRobot Configuration:") 

494 print(f"Joint angles: {np.array(q)}") 

495 print(f"End-effector position: {link_poses[-1].t}") 

496 rpy_rad = link_poses[-1].rpy() 

497 rpy_deg = np.array(rpy_rad) * 180 / np.pi 

498 print(f"End-effector orientation (RPY): {rpy_deg} degrees") 

499 

500 # Set maximum and minimum limits for axes 

501 max_range = np.max(np.abs(positions)) 

502 min_range = -max_range 

503 fig.update_layout( 

504 scene=dict( 

505 xaxis=dict( 

506 nticks=10, 

507 range=[-1.2 * max_range, 1.2 * max_range], 

508 title="X (m)", 

509 ), 

510 yaxis=dict( 

511 nticks=10, 

512 range=[-1.2 * max_range, 1.2 * max_range], 

513 title="Y (m)", 

514 ), 

515 zaxis=dict( 

516 nticks=10, 

517 range=[-1 * max_range, 1.2 * max_range], 

518 title="Z (m)", 

519 ), 

520 ) 

521 ) 

522 

523 # Update layout 

524 fig.update_layout( 

525 title=f"{self.name} Robot Visualization", 

526 scene=dict( 

527 xaxis_title="X (m)", 

528 yaxis_title="Y (m)", 

529 zaxis_title="Z (m)", 

530 aspectmode="cube", 

531 ), 

532 showlegend=False, 

533 width=800, 

534 height=600, 

535 ) 

536 

537 if is_show: 

538 fig.show() 

539 

540 return fig 

541 

542 

543def create_generic_robot( 

544 dofs: int, link_lengths: Optional[List[float]] = None, **kwargs 

545) -> Generic: 

546 """ 

547 Factory function to create a generic robot with sensible defaults. 

548 

549 Args: 

550 dofs: Number of degrees of freedom 

551 link_lengths: Link lengths (default: equal lengths for anthropomorphic arm) 

552 **kwargs: Additional arguments passed to Generic constructor 

553 

554 Returns: 

555 Generic robot instance 

556 

557 Examples: 

558 >>> # Simple 3-DOF arm with equal link lengths 

559 >>> robot = create_generic_robot(3, link_lengths=[0.3, 0.3, 0.2]) 

560 >>> 

561 >>> # 6-DOF arm with anthropomorphic proportions 

562 >>> robot = create_generic_robot(6) 

563 """ 

564 if link_lengths is None: 

565 if dofs <= 3: 

566 # Short arm 

567 link_lengths = [0.3] * dofs 

568 else: 

569 # Longer arm with varied link lengths 

570 link_lengths = [0.1, 0.4, 0.3] + [0.1] * (dofs - 3) 

571 

572 # Create anthropomorphic joint configuration 

573 if dofs >= 6: 

574 # 6-DOF with spherical wrist, extend pattern for more DOFs 

575 alpha = [pi / 2, 0, pi / 2, -pi / 2, pi / 2, 0] 

576 d = [0.2, 0, 0, 0.4, 0, 0.1] 

577 

578 # Extend for more than 6 DOFs 

579 if dofs > 6: 

580 # Add more joints with alternating alpha values 

581 for i in range(6, dofs): 

582 alpha.append(pi / 2 if i % 2 == 0 else 0) 

583 d.append(0.1 if i % 2 == 0 else 0) 

584 elif dofs >= 3: 

585 # Planar arm 

586 alpha = [0] * dofs 

587 d = [0.1] + [0] * (dofs - 1) 

588 else: 

589 # Simple 2-DOF 

590 alpha = [0] * dofs 

591 d = [0] * dofs 

592 

593 return Generic( 

594 dofs=dofs, a=link_lengths, alpha=alpha, d=d, name=f"Generic{dofs}DOF", **kwargs 

595 ) 

596 

597 

598if __name__ == "__main__": # pragma: no cover 

599 # Demo when run as script 

600 print("Creating various generic robots...\n") 

601 

602 # Simple 3-DOF robot 

603 robot3 = create_generic_robot(3, name="Simple3DOF") 

604 robot3.demo() 

605 

606 print("\n" + "=" * 60 + "\n") 

607 

608 # 6-DOF robot with custom parameters 

609 robot6 = Generic( 

610 dofs=6, 

611 a=[0, 0.4, 0.3, 0, 0, 0], 

612 d=[0.2, 0, 0, 0.4, 0, 0.1], 

613 alpha=[pi / 2, 0, pi / 2, -pi / 2, pi / 2, 0], 

614 name="Custom6DOF", 

615 ) 

616 robot6.demo()