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
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
1"""
2DHRobot class for robotics-numpy
4This module provides the DHRobot class for robot manipulator modeling using
5Denavit-Hartenberg parameters. Includes forward kinematics computation and
6robot configuration management.
7"""
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
19# Type aliases
20ArrayLike = Union[float, int, np.ndarray, List[float]]
21JointConfig = Union[List[float], np.ndarray]
24class DHRobot:
25 """
26 Robot manipulator model using Denavit-Hartenberg parameters.
28 This class represents a serial-link robot manipulator using DH parameters.
29 It provides forward kinematics computation and robot configuration management.
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
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 """
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")
68 if not links:
69 raise ValueError("Robot must have at least one link")
71 dh_check_parameters(links)
73 self._links = links.copy()
74 self.name = str(name)
75 self.manufacturer = str(manufacturer)
76 self.keywords = tuple(keywords)
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()
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]
90 # Joint configurations storage
91 self._configurations: Dict[str, np.ndarray] = {}
93 # Cache for performance
94 self._n = len(self._links)
95 self._joint_types = [link.joint_type for link in self._links]
97 @property
98 def n(self) -> int:
99 """Number of joints."""
100 return self._n
102 @property
103 def links(self) -> List[DHLink]:
104 """List of DH links (read-only)."""
105 return self._links.copy()
107 @property
108 def joint_types(self) -> List[str]:
109 """Joint types as list of 'R' or 'P'."""
110 return self._joint_types.copy()
112 @property
113 def qlim(self) -> np.ndarray:
114 """
115 Joint limits as 2xn array.
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
136 def islimit(self, q: JointConfig) -> np.ndarray:
137 """
138 Check if joint configuration is within limits.
140 Args:
141 q: Joint configuration
143 Returns:
144 Boolean array indicating which joints are at limits
145 """
146 q = self._validate_q(q)
147 limits = self.qlim
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
153 return at_lower | at_upper
155 def isspherical(self) -> bool:
156 """Check if robot has spherical wrist (last 3 joints intersect)."""
157 if self.n < 3:
158 return False
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:]
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 )
172 return has_zero_a and has_correct_alphas
174 def fkine(self, q: JointConfig, end: Optional[int] = None, start: int = 0) -> SE3:
175 """
176 Forward kinematics using DH parameters.
178 Computes the forward kinematics from the base frame to the end-effector
179 frame (or specified intermediate frame).
181 Args:
182 q: Joint configuration (n joint values)
183 end: End link index (default: end-effector)
184 start: Start link index (default: 0)
186 Returns:
187 SE3 transformation from base to end-effector (or specified frame)
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)
196 if end is None:
197 end = self.n - 1
199 if not (0 <= start <= end < self.n):
200 raise ValueError(
201 f"Invalid link range: start={start}, end={end}, n={self.n}"
202 )
204 # If only partial chain requested, adjust q
205 if start > 0 or end < self.n - 1:
206 q = q[start : end + 1]
208 # Compute forward kinematics using DH algorithm
209 T = self._fkine_dh(self._links[start : end + 1], q)
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
217 return T
219 def fkine_all(self, q: JointConfig) -> List[SE3]:
220 """
221 Forward kinematics for all intermediate frames.
223 Computes transformations from base to each link frame.
225 Args:
226 q: Joint configuration
228 Returns:
229 List of SE3 transformations, one for each link frame
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)
238 transforms = []
239 T = self.base
241 for i in range(self.n):
242 T = T * self._links[i].A(q[i])
243 transforms.append(T)
245 # Apply tool transformation to last frame
246 transforms[-1] = transforms[-1] * self.tool
248 return transforms
250 def _fkine_dh(self, links, q):
251 """Internal forward kinematics computation."""
252 return _fkine_dh_single(links, q)
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
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)
272 Returns:
273 The manipulator Jacobian in the world frame (6xn)
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")
284 q = self._validate_q(q)
286 # Call the implementation from the jacobian module
287 return compute_jacob0(self, q, T, half, start, end)
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
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.
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
315 Returns:
316 Manipulability index
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)
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)
332 # Call the implementation from the jacobian module
333 return compute_manipulability(self, q, J, end, start, method, axes, **kwargs)
335 def addconfiguration(self, name: str, q: JointConfig) -> None:
336 """
337 Add a named joint configuration.
339 Args:
340 name: Configuration name
341 q: Joint configuration
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()
350 # Also set as attribute for convenience
351 setattr(self, name, q.copy())
353 def configurations(self) -> List[str]:
354 """List all configuration names."""
355 return list(self._configurations.keys())
357 def getconfig(self, name: str) -> np.ndarray:
358 """
359 Get a named configuration.
361 Args:
362 name: Configuration name
364 Returns:
365 Joint configuration array
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()
374 def teach(self, q: Optional[JointConfig] = None) -> None:
375 """
376 Simple teach interface - print current pose.
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)
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")
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)
396 if q.shape == ():
397 q = np.array([q]) # Handle scalar input
399 if q.shape != (self.n,):
400 raise ValueError(f"q must have {self.n} elements, got {q.shape}")
402 return q
404 def __len__(self) -> int:
405 """Number of joints."""
406 return self.n
408 def __getitem__(self, index: int) -> DHLink:
409 """Get link by index."""
410 return self._links[index]
412 def __str__(self) -> str:
413 """String representation of robot."""
414 joint_str = "".join(link.joint_type for link in self._links)
416 header = f"{self.name}"
417 if self.manufacturer:
418 header += f" (by {self.manufacturer})"
420 header += f", {self.n} joints ({joint_str})"
422 if self.keywords:
423 header += f", {', '.join(self.keywords)}"
425 # Create table of DH parameters
426 lines = [header, ""]
428 # Table header
429 lines.append(
430 "┌─────┬──────────┬─────────┬─────────┬─────────┬─────────────────┐"
431 )
432 lines.append(
433 "│link │ link │ joint │ θ │ d │ a │"
434 )
435 lines.append(
436 "├─────┼──────────┼─────────┼─────────┼─────────┼─────────────────┤"
437 )
439 # Table rows
440 for i, link in enumerate(self._links):
441 joint_type = "R" if link.is_revolute() else "P"
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}"
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 )
454 lines.append(
455 "└─────┴──────────┴─────────┴─────────┴─────────┴─────────────────┘"
456 )
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}]")
466 return "\n".join(lines)
468 def __repr__(self) -> str:
469 """Detailed representation."""
470 return f"DHRobot(name='{self.name}', n={self.n}, joints='{self.joint_types}')"
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.
478 Args:
479 n_joints: Number of joints (default: 6)
480 link_length: Length of each link (default: 0.3m)
482 Returns:
483 DHRobot with simple DH parameters
484 """
485 from .dh_link import RevoluteDH
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])
500 links.append(link)
502 robot = DHRobot(links, name=f"Simple {n_joints}DOF Arm")
504 # Add some standard configurations
505 robot.addconfiguration("qz", np.zeros(n_joints))
506 robot.addconfiguration("qr", np.zeros(n_joints)) # Ready position
508 return robot
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).
517 Args:
518 n_joints: Number of joints (default: 3)
519 link_lengths: Length of each link (default: equal lengths)
521 Returns:
522 DHRobot representing planar arm
523 """
524 from .dh_link import RevoluteDH
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")
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)
542 robot = DHRobot(links, name=f"Planar {n_joints}DOF Arm")
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))
554 return robot
557def _fkine_dh_single(links, q):
558 """
559 Internal forward kinematics using DH parameters.
561 This avoids circular imports by keeping FK computation within the robot model.
562 """
563 from ..transforms import SE3
565 q = np.asarray(q, dtype=float)
567 if len(q) != len(links):
568 raise ValueError(
569 f"Joint configuration length ({len(q)}) must match number of links ({len(links)})"
570 )
572 # Start with identity transformation
573 T = SE3()
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}")
583 return T