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
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
1"""
2Generic robot model for robotics-numpy
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.
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"""
15import numpy as np
16from typing import Optional, List, Union
17from .dh_robot import DHRobot
18from .dh_link import RevoluteDH, PrismaticDH
20# Constants
21pi = np.pi
22deg = pi / 180
25class Generic(DHRobot):
26 """
27 A fully generic robotic arm class using Denavit-Hartenberg parameters.
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.
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
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 """
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.
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
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")
91 self.dofs = dofs
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
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 )
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}'")
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 )
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)
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 [-,+]
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)
201 # Initialize parent class
202 super().__init__(
203 links,
204 name=name,
205 manufacturer="generic",
206 keywords=("configurable", "educational", "generic"),
207 **kwargs,
208 )
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 )
215 # Zero pose: all joint values at 0
216 self.qz = np.zeros(dofs)
218 # Add configurations
219 self.addconfiguration("qr", self.qr)
220 self.addconfiguration("qz", self.qz)
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 }
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.
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]
256 Raises:
257 ValueError: If parameter dimensions don't match dofs
258 """
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))
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")
289 def workspace_radius(self) -> float:
290 """
291 Estimate maximum workspace radius.
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
308 def summary(self) -> str:
309 """
310 Generate a summary string of the robot configuration.
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 ]
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"
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"
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 )
354 return "\n".join(lines)
356 def demo(self) -> None:
357 """
358 Demonstrate Generic robot capabilities.
359 """
360 print("Generic Robot Demonstration")
361 print("=" * 50)
363 print(f"\n{self.summary()}")
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 )
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}")
379 def __str__(self) -> str:
380 """String representation of the robot."""
381 return self.summary()
383 def __repr__(self) -> str:
384 """Detailed representation of the robot."""
385 return f"Generic(dofs={self.dofs}, name='{self.name}')"
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).
399 Args:
400 q: Joint configuration to visualize (default: qz)
401 **kwargs: Additional arguments for visualization
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 )
414 if q is None:
415 q = self.qz
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)
426 # Extract positions for plotting
427 positions = np.array([T.t for T in link_poses])
429 # Create 3D plot
430 fig = go.Figure()
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 )
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
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
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]
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 )
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 )
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")
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 )
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 )
537 if is_show:
538 fig.show()
540 return fig
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.
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
554 Returns:
555 Generic robot instance
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)
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]
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
593 return Generic(
594 dofs=dofs, a=link_lengths, alpha=alpha, d=d, name=f"Generic{dofs}DOF", **kwargs
595 )
598if __name__ == "__main__": # pragma: no cover
599 # Demo when run as script
600 print("Creating various generic robots...\n")
602 # Simple 3-DOF robot
603 robot3 = create_generic_robot(3, name="Simple3DOF")
604 robot3.demo()
606 print("\n" + "=" * 60 + "\n")
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()