Coverage for src/robotics_numpy/models/dh_robot.py: 71%

202 statements  

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

1""" 

2DHRobot class for robotics-numpy 

3 

4This module provides the DHRobot class for robot manipulator modeling using 

5Denavit-Hartenberg parameters. Includes forward kinematics computation and 

6robot configuration management. 

7""" 

8 

9import numpy as np 

10from typing import List, Optional, Union, Dict, Any, Tuple 

11from .dh_link import DHLink, dh_check_parameters 

12from ..transforms import SE3 

13from ..kinematics.jacobian import ( 

14 jacob0 as compute_jacob0, 

15 manipulability as compute_manipulability, 

16) 

17# from ..kinematics.forward import fkine_dh # Moved to avoid circular import 

18 

19# Type aliases 

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

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

22 

23 

24class DHRobot: 

25 """ 

26 Robot manipulator model using Denavit-Hartenberg parameters. 

27 

28 This class represents a serial-link robot manipulator using DH parameters. 

29 It provides forward kinematics computation and robot configuration management. 

30 

31 Args: 

32 links: List of DH links defining the robot 

33 name: Robot name/model 

34 manufacturer: Robot manufacturer 

35 base: Base transformation (default: identity) 

36 tool: Tool transformation (default: identity) 

37 gravity: Gravity vector [x, y, z] in m/s² (default: [0, 0, -9.81]) 

38 keywords: Keywords describing robot features 

39 

40 Examples: 

41 >>> # Create a simple 2-DOF arm 

42 >>> links = [ 

43 ... RevoluteDH(d=0.1, a=0.2, alpha=0), 

44 ... RevoluteDH(d=0, a=0.3, alpha=0) 

45 ... ] 

46 >>> robot = DHRobot(links, name="2DOF Arm") 

47 >>> 

48 >>> # Forward kinematics 

49 >>> q = [0, np.pi/4] 

50 >>> T = robot.fkine(q) 

51 >>> print(T) 

52 """ 

53 

54 def __init__( 

55 self, 

56 links: List[DHLink], 

57 name: str = "DHRobot", 

58 manufacturer: str = "", 

59 base: Optional[SE3] = None, 

60 tool: Optional[SE3] = None, 

61 gravity: Optional[List[float]] = None, 

62 keywords: Tuple[str, ...] = (), 

63 ): 

64 # Validate links 

65 if not isinstance(links, list): 

66 raise TypeError("links must be a list of DHLink objects") 

67 

68 if not links: 

69 raise ValueError("Robot must have at least one link") 

70 

71 dh_check_parameters(links) 

72 

73 self._links = links.copy() 

74 self.name = str(name) 

75 self.manufacturer = str(manufacturer) 

76 self.keywords = tuple(keywords) 

77 

78 # Base and tool transformations 

79 self.base = base if base is not None else SE3() 

80 self.tool = tool if tool is not None else SE3() 

81 

82 # Gravity vector 

83 if gravity is None: 

84 self.gravity = [0.0, 0.0, -9.81] 

85 else: 

86 if len(gravity) != 3: 

87 raise ValueError("gravity must be a 3-element vector") 

88 self.gravity = [float(g) for g in gravity] 

89 

90 # Joint configurations storage 

91 self._configurations: Dict[str, np.ndarray] = {} 

92 

93 # Cache for performance 

94 self._n = len(self._links) 

95 self._joint_types = [link.joint_type for link in self._links] 

96 

97 @property 

98 def n(self) -> int: 

99 """Number of joints.""" 

100 return self._n 

101 

102 @property 

103 def links(self) -> List[DHLink]: 

104 """List of DH links (read-only).""" 

105 return self._links.copy() 

106 

107 @property 

108 def joint_types(self) -> List[str]: 

109 """Joint types as list of 'R' or 'P'.""" 

110 return self._joint_types.copy() 

111 

112 @property 

113 def qlim(self) -> np.ndarray: 

114 """ 

115 Joint limits as 2xn array. 

116 

117 Returns: 

118 Joint limits where each column is [min, max] for one joint 

119 """ 

120 limits = np.zeros((2, self.n)) 

121 for i, link in enumerate(self._links): 

122 print(f"Link {i}: qlim={link.qlim}, is_revolute={link.is_revolute()}") 

123 if link.qlim is not None: 

124 print(f"Assigning qlim for Link {i}: {link.qlim}") 

125 limits[:, i] = link.qlim 

126 else: 

127 # Default limits 

128 if link.is_revolute(): 

129 print(f"Default revolute qlim for Link {i}: [-np.pi, np.pi]") 

130 limits[:, i] = [-np.pi, np.pi] 

131 else: 

132 limits[:, i] = [-1.0, 1.0] 

133 print(f"Default prismatic qlim for Link {i}: [-1.0, 1.0]") 

134 return limits 

135 

136 def islimit(self, q: JointConfig) -> np.ndarray: 

137 """ 

138 Check if joint configuration is within limits. 

139 

140 Args: 

141 q: Joint configuration 

142 

143 Returns: 

144 Boolean array indicating which joints are at limits 

145 """ 

146 q = self._validate_q(q) 

147 limits = self.qlim 

148 

149 # Check both upper and lower limits 

150 at_lower = np.abs(q - limits[:, 0]) < 1e-6 

151 at_upper = np.abs(q - limits[:, 1]) < 1e-6 

152 

153 return at_lower | at_upper 

154 

155 def isspherical(self) -> bool: 

156 """Check if robot has spherical wrist (last 3 joints intersect).""" 

157 if self.n < 3: 

158 return False 

159 

160 # Check if last 3 joint axes intersect at a point 

161 # This is a simplified check - assumes standard spherical wrist geometry 

162 last_3_links = self._links[-3:] 

163 

164 # Spherical wrist typically has a=0 for last 3 links and specific alpha values 

165 has_zero_a = all(abs(link.a) < 1e-6 for link in last_3_links) 

166 has_correct_alphas = ( 

167 abs(abs(last_3_links[0].alpha) - np.pi / 2) < 1e-3 

168 and abs(abs(last_3_links[1].alpha) - np.pi / 2) < 1e-3 

169 and abs(last_3_links[2].alpha) < 1e-3 

170 ) 

171 

172 return has_zero_a and has_correct_alphas 

173 

174 def fkine(self, q: JointConfig, end: Optional[int] = None, start: int = 0) -> SE3: 

175 """ 

176 Forward kinematics using DH parameters. 

177 

178 Computes the forward kinematics from the base frame to the end-effector 

179 frame (or specified intermediate frame). 

180 

181 Args: 

182 q: Joint configuration (n joint values) 

183 end: End link index (default: end-effector) 

184 start: Start link index (default: 0) 

185 

186 Returns: 

187 SE3 transformation from base to end-effector (or specified frame) 

188 

189 Examples: 

190 >>> robot = DHRobot(links) 

191 >>> T = robot.fkine([0, 0, 0, 0, 0, 0]) # All joints at zero 

192 >>> T = robot.fkine([0, 0, 0], end=2) # First 3 links only 

193 """ 

194 q = self._validate_q(q) 

195 

196 if end is None: 

197 end = self.n - 1 

198 

199 if not (0 <= start <= end < self.n): 

200 raise ValueError( 

201 f"Invalid link range: start={start}, end={end}, n={self.n}" 

202 ) 

203 

204 # If only partial chain requested, adjust q 

205 if start > 0 or end < self.n - 1: 

206 q = q[start : end + 1] 

207 

208 # Compute forward kinematics using DH algorithm 

209 T = self._fkine_dh(self._links[start : end + 1], q) 

210 

211 # Apply base and tool transformations if computing full chain 

212 if start == 0: 

213 T = self.base * T 

214 if end == self.n - 1: 

215 T = T * self.tool 

216 

217 return T 

218 

219 def fkine_all(self, q: JointConfig) -> List[SE3]: 

220 """ 

221 Forward kinematics for all intermediate frames. 

222 

223 Computes transformations from base to each link frame. 

224 

225 Args: 

226 q: Joint configuration 

227 

228 Returns: 

229 List of SE3 transformations, one for each link frame 

230 

231 Examples: 

232 >>> robot = DHRobot(links) 

233 >>> transforms = robot.fkine_all([0, 0, 0, 0, 0, 0]) 

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

235 """ 

236 q = self._validate_q(q) 

237 

238 transforms = [] 

239 T = self.base 

240 

241 for i in range(self.n): 

242 T = T * self._links[i].A(q[i]) 

243 transforms.append(T) 

244 

245 # Apply tool transformation to last frame 

246 transforms[-1] = transforms[-1] * self.tool 

247 

248 return transforms 

249 

250 def _fkine_dh(self, links, q): 

251 """Internal forward kinematics computation.""" 

252 return _fkine_dh_single(links, q) 

253 

254 def jacob0( 

255 self, 

256 q: JointConfig = None, 

257 T=None, 

258 half: Optional[str] = None, 

259 start: int = 0, 

260 end: Optional[int] = None, 

261 ) -> np.ndarray: 

262 """ 

263 Manipulator Jacobian in world frame 

264 

265 Args: 

266 q: Joint coordinate vector 

267 T: Forward kinematics if known, SE3 matrix 

268 half: Return half Jacobian: 'trans' or 'rot' 

269 start: Start link index (default: 0) 

270 end: End link index (default: end-effector) 

271 

272 Returns: 

273 The manipulator Jacobian in the world frame (6xn) 

274 

275 Notes: 

276 - End-effector spatial velocity v = (vx, vy, vz, wx, wy, wz)^T 

277 is related to joint velocity by v = J0(q) * dq 

278 - This is the geometric Jacobian as described in texts by 

279 Corke, Spong et al., Siciliano et al. 

280 """ 

281 if q is None: 

282 raise ValueError("Joint configuration q must be provided") 

283 

284 q = self._validate_q(q) 

285 

286 # Call the implementation from the jacobian module 

287 return compute_jacob0(self, q, T, half, start, end) 

288 

289 def manipulability( 

290 self, 

291 q: Optional[JointConfig] = None, 

292 J: Optional[np.ndarray] = None, 

293 end: Optional[int] = None, 

294 start: int = 0, 

295 method: str = "yoshikawa", 

296 axes: Union[str, List[bool]] = "all", 

297 **kwargs, 

298 ) -> float: 

299 """ 

300 Manipulability measure 

301 

302 Computes the manipulability index for the robot at the joint configuration q. 

303 It indicates dexterity, that is, how well conditioned the robot is for motion 

304 with respect to the 6 degrees of Cartesian motion. The value is zero if the 

305 robot is at a singularity. 

306 

307 Args: 

308 q: Joint coordinates (one of J or q required) 

309 J: Jacobian in base frame if already computed (one of J or q required) 

310 end: End link index (default: end-effector) 

311 start: Start link index (default: 0) 

312 method: Method to use ('yoshikawa', 'asada', 'minsingular', 'invcondition') 

313 axes: Task space axes to consider: 'all', 'trans', 'rot', or a boolean list 

314 

315 Returns: 

316 Manipulability index 

317 

318 Notes: 

319 Supported measures: 

320 - 'yoshikawa': Volume of the velocity ellipsoid, distance from singularity 

321 - 'invcondition': Inverse condition number of Jacobian, isotropy of velocity ellipsoid 

322 - 'minsingular': Minimum singular value of the Jacobian, distance from singularity 

323 - 'asada': Isotropy of the task-space acceleration ellipsoid (requires inertial parameters) 

324 

325 The 'all' axes option includes rotational and translational dexterity, but this 

326 involves adding different units. It can be more useful to look at the translational 

327 and rotational manipulability separately. 

328 """ 

329 if q is not None: 

330 q = self._validate_q(q) 

331 

332 # Call the implementation from the jacobian module 

333 return compute_manipulability(self, q, J, end, start, method, axes, **kwargs) 

334 

335 def addconfiguration(self, name: str, q: JointConfig) -> None: 

336 """ 

337 Add a named joint configuration. 

338 

339 Args: 

340 name: Configuration name 

341 q: Joint configuration 

342 

343 Examples: 

344 >>> robot.addconfiguration("home", [0, 0, 0, 0, 0, 0]) 

345 >>> robot.addconfiguration("ready", robot.qr) 

346 """ 

347 q = self._validate_q(q) 

348 self._configurations[name] = q.copy() 

349 

350 # Also set as attribute for convenience 

351 setattr(self, name, q.copy()) 

352 

353 def configurations(self) -> List[str]: 

354 """List all configuration names.""" 

355 return list(self._configurations.keys()) 

356 

357 def getconfig(self, name: str) -> np.ndarray: 

358 """ 

359 Get a named configuration. 

360 

361 Args: 

362 name: Configuration name 

363 

364 Returns: 

365 Joint configuration array 

366 

367 Raises: 

368 KeyError: If configuration doesn't exist 

369 """ 

370 if name not in self._configurations: 

371 raise KeyError(f"Configuration '{name}' not found") 

372 return self._configurations[name].copy() 

373 

374 def teach(self, q: Optional[JointConfig] = None) -> None: 

375 """ 

376 Simple teach interface - print current pose. 

377 

378 Args: 

379 q: Joint configuration (default: all zeros) 

380 """ 

381 if q is None: 

382 q = np.zeros(self.n) 

383 else: 

384 q = self._validate_q(q) 

385 

386 T = self.fkine(q) 

387 print(f"Joint configuration: {q}") 

388 print(f"End-effector pose:\n{T}") 

389 print(f"Position: {T.t}") 

390 print(f"RPY angles: {np.degrees(T.rpy())} degrees") 

391 

392 def _validate_q(self, q: JointConfig) -> np.ndarray: 

393 """Validate and convert joint configuration to numpy array.""" 

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

395 

396 if q.shape == (): 

397 q = np.array([q]) # Handle scalar input 

398 

399 if q.shape != (self.n,): 

400 raise ValueError(f"q must have {self.n} elements, got {q.shape}") 

401 

402 return q 

403 

404 def __len__(self) -> int: 

405 """Number of joints.""" 

406 return self.n 

407 

408 def __getitem__(self, index: int) -> DHLink: 

409 """Get link by index.""" 

410 return self._links[index] 

411 

412 def __str__(self) -> str: 

413 """String representation of robot.""" 

414 joint_str = "".join(link.joint_type for link in self._links) 

415 

416 header = f"{self.name}" 

417 if self.manufacturer: 

418 header += f" (by {self.manufacturer})" 

419 

420 header += f", {self.n} joints ({joint_str})" 

421 

422 if self.keywords: 

423 header += f", {', '.join(self.keywords)}" 

424 

425 # Create table of DH parameters 

426 lines = [header, ""] 

427 

428 # Table header 

429 lines.append( 

430 "┌─────┬──────────┬─────────┬─────────┬─────────┬─────────────────┐" 

431 ) 

432 lines.append( 

433 "│link │ link │ joint │ θ │ d │ a │" 

434 ) 

435 lines.append( 

436 "├─────┼──────────┼─────────┼─────────┼─────────┼─────────────────┤" 

437 ) 

438 

439 # Table rows 

440 for i, link in enumerate(self._links): 

441 joint_type = "R" if link.is_revolute() else "P" 

442 

443 # Format values 

444 theta_str = f"{link.theta:7.3f}" if not link.is_revolute() else f"q{i}" 

445 d_str = f"{link.d:7.3f}" if not link.is_prismatic() else f"q{i}" 

446 a_str = f"{link.a:7.3f}" 

447 alpha_str = f"{link.alpha:7.3f}" 

448 

449 lines.append( 

450 f"│{i:4d} │ link{i:<4d} │ {joint_type:>1s} │{theta_str:>8s} │" 

451 f"{d_str:>8s} │{a_str:>8s} α={alpha_str:>6s}│" 

452 ) 

453 

454 lines.append( 

455 "└─────┴──────────┴─────────┴─────────┴─────────┴─────────────────┘" 

456 ) 

457 

458 # Add configurations if any 

459 if self._configurations: 

460 lines.append("") 

461 lines.append("Named configurations:") 

462 for name, config in self._configurations.items(): 

463 config_str = ", ".join(f"{val:6.2f}" for val in config) 

464 lines.append(f" {name}: [{config_str}]") 

465 

466 return "\n".join(lines) 

467 

468 def __repr__(self) -> str: 

469 """Detailed representation.""" 

470 return f"DHRobot(name='{self.name}', n={self.n}, joints='{self.joint_types}')" 

471 

472 

473# Utility functions for robot creation 

474def create_simple_arm(n_joints: int = 6, link_length: float = 0.3) -> DHRobot: 

475 """ 

476 Create a simple n-DOF revolute arm for testing. 

477 

478 Args: 

479 n_joints: Number of joints (default: 6) 

480 link_length: Length of each link (default: 0.3m) 

481 

482 Returns: 

483 DHRobot with simple DH parameters 

484 """ 

485 from .dh_link import RevoluteDH 

486 

487 links = [] 

488 for i in range(n_joints): 

489 if i == 0: 

490 # Base joint 

491 link = RevoluteDH(d=0.3, a=0, alpha=np.pi / 2, qlim=[-np.pi, np.pi]) 

492 elif i == n_joints - 1: 

493 # Last joint 

494 link = RevoluteDH(d=0, a=0, alpha=0, qlim=[-np.pi, np.pi]) 

495 else: 

496 # Intermediate joints 

497 alpha = np.pi / 2 if i % 2 == 1 else 0 

498 link = RevoluteDH(d=0, a=link_length, alpha=alpha, qlim=[-np.pi, np.pi]) 

499 

500 links.append(link) 

501 

502 robot = DHRobot(links, name=f"Simple {n_joints}DOF Arm") 

503 

504 # Add some standard configurations 

505 robot.addconfiguration("qz", np.zeros(n_joints)) 

506 robot.addconfiguration("qr", np.zeros(n_joints)) # Ready position 

507 

508 return robot 

509 

510 

511def create_planar_arm( 

512 n_joints: int = 3, link_lengths: Optional[List[float]] = None 

513) -> DHRobot: 

514 """ 

515 Create a planar n-DOF arm (all joints parallel, in same plane). 

516 

517 Args: 

518 n_joints: Number of joints (default: 3) 

519 link_lengths: Length of each link (default: equal lengths) 

520 

521 Returns: 

522 DHRobot representing planar arm 

523 """ 

524 from .dh_link import RevoluteDH 

525 

526 if link_lengths is None: 

527 link_lengths = [0.3] * n_joints 

528 elif len(link_lengths) != n_joints: 

529 raise ValueError(f"link_lengths must have {n_joints} elements") 

530 

531 links = [] 

532 for i in range(n_joints): 

533 link = RevoluteDH( 

534 d=0, 

535 a=link_lengths[i], 

536 alpha=0, # All joints parallel 

537 theta=0, 

538 qlim=[-np.pi, np.pi], 

539 ) 

540 links.append(link) 

541 

542 robot = DHRobot(links, name=f"Planar {n_joints}DOF Arm") 

543 

544 # Add configurations 

545 robot.addconfiguration("qz", np.zeros(n_joints)) 

546 robot.addconfiguration("qstraight", np.zeros(n_joints)) 

547 if n_joints >= 3: 

548 robot.addconfiguration( 

549 "qfold", np.array([0, np.pi / 2, -np.pi / 2] + [0] * (n_joints - 3)) 

550 ) 

551 else: 

552 robot.addconfiguration("qfold", np.zeros(n_joints)) 

553 

554 return robot 

555 

556 

557def _fkine_dh_single(links, q): 

558 """ 

559 Internal forward kinematics using DH parameters. 

560 

561 This avoids circular imports by keeping FK computation within the robot model. 

562 """ 

563 from ..transforms import SE3 

564 

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

566 

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

568 raise ValueError( 

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

570 ) 

571 

572 # Start with identity transformation 

573 T = SE3() 

574 

575 # Multiply transformation matrices for each link 

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

577 try: 

578 T_link = link.A(qi) 

579 T = T * T_link 

580 except Exception as e: 

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

582 

583 return T