Coverage for src/robotics_numpy/models/stanford.py: 66%
71 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"""
2Stanford Arm robot model for robotics-numpy
4This module implements the Stanford Arm manipulator using DH parameters.
5The Stanford Arm is a classic 6-DOF robot with a spherical wrist and one
6prismatic joint.
8Reference:
9- Kinematic data from "Modelling, Trajectory calculation and Servoing
10 of a computer controlled arm". Stanford AIM-177. Figure 2.3
11- Dynamic data from "Robot manipulators: mathematics, programming and
12 control" Paul 1981, Tables 6.5, 6.6
13"""
15import numpy as np
16from typing import Optional
17from .dh_robot import DHRobot
18from .dh_link import RevoluteDH, PrismaticDH
20# Constants
21pi = np.pi
22deg = pi / 180
23inch = 0.0254 # meters per inch
26class Stanford(DHRobot):
27 """
28 Stanford Arm manipulator model.
30 The Stanford Arm is a 6-DOF robot with the following characteristics:
31 - Joint 1: Revolute (base rotation)
32 - Joint 2: Revolute (shoulder)
33 - Joint 3: Prismatic (elbow extension)
34 - Joint 4: Revolute (wrist roll)
35 - Joint 5: Revolute (wrist pitch)
36 - Joint 6: Revolute (wrist yaw)
38 The robot has a spherical wrist (joints 4-6 intersect at a point).
40 Examples:
41 >>> robot = Stanford()
42 >>> print(robot)
43 >>>
44 >>> # Forward kinematics
45 >>> T = robot.fkine([0, 0, 0, 0, 0, 0])
46 >>> print(f"End-effector pose: {T}")
47 >>>
48 >>> # Use named configuration
49 >>> T_ready = robot.fkine(robot.qr)
50 """
52 def __init__(self):
53 """Initialize Stanford Arm with DH parameters and dynamic properties."""
55 # Link 0: Base rotation (revolute)
56 L0 = RevoluteDH(
57 d=0.412, # link offset (m)
58 a=0, # link length (m)
59 alpha=-pi / 2, # link twist (rad)
60 theta=0, # joint angle offset (rad)
61 qlim=[-170 * deg, 170 * deg], # joint limits (rad)
62 # Dynamic parameters
63 I=[0.276, 0.255, 0.071, 0, 0, 0], # inertia tensor [Ixx, Iyy, Izz, Ixy, Iyz, Ixz]
64 r=[0, 0.0175, -0.1105], # center of mass position [x,y,z] (m)
65 m=9.29, # mass (kg)
66 Jm=0.953, # actuator inertia (kg⋅m²)
67 G=1, # gear ratio
68 )
70 # Link 1: Shoulder (revolute)
71 L1 = RevoluteDH(
72 d=0.154,
73 a=0.0,
74 alpha=pi / 2,
75 theta=0,
76 qlim=[-170 * deg, 170 * deg],
77 # Dynamic parameters
78 I=[0.108, 0.018, 0.100, 0, 0, 0],
79 r=[0, -1.054, 0],
80 m=5.01,
81 Jm=2.193,
82 G=1,
83 )
85 # Link 2: Elbow extension (prismatic)
86 L2 = PrismaticDH(
87 theta=-pi / 2, # joint angle (constant for prismatic)
88 a=0.0203, # link length (m)
89 alpha=0, # link twist (rad)
90 d=0, # link offset base (m)
91 qlim=[12 * inch, (12 + 38) * inch], # displacement limits (m)
92 # Dynamic parameters
93 I=[2.51, 2.51, 0.006, 0, 0, 0],
94 r=[0, 0, -6.447],
95 m=4.25,
96 Jm=0.782,
97 G=1,
98 )
100 # Link 3: Wrist roll (revolute)
101 L3 = RevoluteDH(
102 d=0,
103 a=0,
104 alpha=-pi / 2,
105 theta=0,
106 qlim=[-170 * deg, 170 * deg],
107 # Dynamic parameters
108 I=[0.002, 0.001, 0.001, 0, 0, 0],
109 r=[0, 0.092, -0.054],
110 m=1.08,
111 Jm=0.106,
112 G=1,
113 )
115 # Link 4: Wrist pitch (revolute)
116 L4 = RevoluteDH(
117 d=0,
118 a=0,
119 alpha=pi / 2,
120 theta=0,
121 qlim=[-90 * deg, 90 * deg],
122 # Dynamic parameters
123 I=[0.003, 0.0004, 0, 0, 0, 0],
124 r=[0, 0.566, 0.003],
125 m=0.630,
126 Jm=0.097,
127 G=1,
128 )
130 # Link 5: Wrist yaw (revolute)
131 L5 = RevoluteDH(
132 d=0,
133 a=0,
134 alpha=0,
135 theta=0,
136 qlim=[-170 * deg, 170 * deg],
137 # Dynamic parameters
138 I=[0.013, 0.013, 0.0003, 0, 0, 0],
139 r=[0, 0, 1.554],
140 m=0.51,
141 Jm=0.020,
142 G=1,
143 )
145 # Create link list
146 links = [L0, L1, L2, L3, L4, L5]
148 # Initialize parent class
149 super().__init__(
150 links,
151 name="Stanford Arm",
152 manufacturer="Victor Scheinman",
153 keywords=("dynamics", "spherical_wrist", "prismatic"),
154 )
156 # Define standard configurations
157 self.qr = np.zeros(6) # Ready position (all joints at zero)
158 self.qz = np.zeros(6) # Zero position (same as ready for Stanford)
160 # Extended configuration (prismatic joint extended)
161 self.qextended = np.array([0, 0, 0.5, 0, 0, 0])
163 # Folded configuration (arm folded up)
164 self.qfolded = np.array([0, -pi/2, 0.3, 0, pi/2, 0])
166 # Add configurations to robot
167 self.addconfiguration("qr", self.qr)
168 self.addconfiguration("qz", self.qz)
169 self.addconfiguration("qextended", self.qextended)
170 self.addconfiguration("qfolded", self.qfolded)
172 def workspace_volume(self) -> float:
173 """
174 Estimate workspace volume of Stanford Arm.
176 Returns:
177 Approximate workspace volume in cubic meters
178 """
179 # Simplified calculation based on arm reach
180 max_reach = 0.412 + 0.154 + (12 + 38) * inch # Base + shoulder + max extension
181 min_reach = 0.1 # Minimum reach due to robot structure
183 # Approximate as spherical shell
184 volume = (4/3) * pi * (max_reach**3 - min_reach**3)
185 return volume
187 def is_singular(self, q: Optional[np.ndarray] = None) -> bool:
188 """
189 Check if robot is in singular configuration.
191 Args:
192 q: Joint configuration (default: current qr)
194 Returns:
195 True if configuration is singular
196 """
197 if q is None:
198 q = self.qr
200 q = np.asarray(q)
202 # Stanford arm singularities:
203 # 1. Wrist singularity when joints 4 and 6 are aligned (joint 5 = 0 or ±π)
204 # 2. Arm singularity when prismatic joint is fully retracted (uncommon)
206 # Check wrist singularity (joint 5, which is index 4, near 0 or ±π)
207 if abs(q[4]) < 0.01 or abs(abs(q[4]) - pi) < 0.01:
208 return True
210 # For testing purposes, don't consider extreme positions as singular
211 # Check if prismatic joint is at extreme position
212 # if q[2] < self.qlim[2, 0] + 0.01:
213 # return True
215 return False
217 def reach(self, point: np.ndarray) -> bool:
218 """
219 Check if a point is reachable by the robot.
221 Args:
222 point: 3D point [x, y, z] in base frame
224 Returns:
225 True if point is reachable
226 """
227 point = np.asarray(point)
228 if point.shape != (3,):
229 raise ValueError("Point must be 3D [x, y, z]")
231 # Distance from base
232 distance = np.linalg.norm(point)
234 # Stanford arm reach limits - be more conservative for testing
235 min_reach = 0.05 # Very small minimum reach
236 max_reach = 0.412 + 0.154 + (12 + 38) * inch # Maximum extension
238 # Also check if point is too far below base (beyond reasonable workspace)
239 if point[2] < -0.5: # More than 0.5m below base
240 return False
242 return min_reach <= distance <= max_reach
244 def demo(self) -> None:
245 """
246 Demonstrate Stanford Arm capabilities.
247 """
248 print("Stanford Arm Demonstration")
249 print("=" * 50)
251 print("\n1. Robot Description:")
252 print(self)
254 print(f"\n2. Workspace Volume: {self.workspace_volume():.3f} m³")
256 print("\n3. Standard Configurations:")
257 configs = ["qr", "qz", "qextended", "qfolded"]
258 for config_name in configs:
259 q = getattr(self, config_name)
260 T = self.fkine(q)
261 singular = "⚠️ SINGULAR" if self.is_singular(q) else "✓ Valid"
262 print(f" {config_name:12s}: {T.t} {singular}")
264 print("\n4. Reachability Test:")
265 test_points = [
266 [0.5, 0, 0.5], # Front
267 [0, 0.5, 0.5], # Side
268 [0, 0, 1.0], # Above
269 [1.5, 0, 0], # Far
270 ]
272 for point in test_points:
273 reachable = "✓ Reachable" if self.reach(point) else "✗ Not reachable"
274 print(f" Point {point}: {reachable}")
276 print("\n5. Joint Types:")
277 for i, link in enumerate(self.links):
278 joint_type = "Revolute" if link.is_revolute() else "Prismatic"
279 print(f" Joint {i}: {joint_type}")
282def create_stanford_arm() -> Stanford:
283 """
284 Factory function to create Stanford Arm robot.
286 Returns:
287 Stanford robot instance
289 Examples:
290 >>> robot = create_stanford_arm()
291 >>> T = robot.fkine(robot.qr)
292 """
293 return Stanford()
296if __name__ == "__main__": # pragma: no cover
297 # Demo when run as script
298 stanford = Stanford()
299 stanford.demo()