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

1""" 

2Stanford Arm robot model for robotics-numpy 

3 

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. 

7 

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""" 

14 

15import numpy as np 

16from typing import Optional 

17from .dh_robot import DHRobot 

18from .dh_link import RevoluteDH, PrismaticDH 

19 

20# Constants 

21pi = np.pi 

22deg = pi / 180 

23inch = 0.0254 # meters per inch 

24 

25 

26class Stanford(DHRobot): 

27 """ 

28 Stanford Arm manipulator model. 

29 

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) 

37 

38 The robot has a spherical wrist (joints 4-6 intersect at a point). 

39 

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 """ 

51 

52 def __init__(self): 

53 """Initialize Stanford Arm with DH parameters and dynamic properties.""" 

54 

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 ) 

69 

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 ) 

84 

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 ) 

99 

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 ) 

114 

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 ) 

129 

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 ) 

144 

145 # Create link list 

146 links = [L0, L1, L2, L3, L4, L5] 

147 

148 # Initialize parent class 

149 super().__init__( 

150 links, 

151 name="Stanford Arm", 

152 manufacturer="Victor Scheinman", 

153 keywords=("dynamics", "spherical_wrist", "prismatic"), 

154 ) 

155 

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) 

159 

160 # Extended configuration (prismatic joint extended) 

161 self.qextended = np.array([0, 0, 0.5, 0, 0, 0]) 

162 

163 # Folded configuration (arm folded up) 

164 self.qfolded = np.array([0, -pi/2, 0.3, 0, pi/2, 0]) 

165 

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) 

171 

172 def workspace_volume(self) -> float: 

173 """ 

174 Estimate workspace volume of Stanford Arm. 

175 

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 

182 

183 # Approximate as spherical shell 

184 volume = (4/3) * pi * (max_reach**3 - min_reach**3) 

185 return volume 

186 

187 def is_singular(self, q: Optional[np.ndarray] = None) -> bool: 

188 """ 

189 Check if robot is in singular configuration. 

190 

191 Args: 

192 q: Joint configuration (default: current qr) 

193 

194 Returns: 

195 True if configuration is singular 

196 """ 

197 if q is None: 

198 q = self.qr 

199 

200 q = np.asarray(q) 

201 

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) 

205 

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 

209 

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 

214 

215 return False 

216 

217 def reach(self, point: np.ndarray) -> bool: 

218 """ 

219 Check if a point is reachable by the robot. 

220 

221 Args: 

222 point: 3D point [x, y, z] in base frame 

223 

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]") 

230 

231 # Distance from base 

232 distance = np.linalg.norm(point) 

233 

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 

237 

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 

241 

242 return min_reach <= distance <= max_reach 

243 

244 def demo(self) -> None: 

245 """ 

246 Demonstrate Stanford Arm capabilities. 

247 """ 

248 print("Stanford Arm Demonstration") 

249 print("=" * 50) 

250 

251 print("\n1. Robot Description:") 

252 print(self) 

253 

254 print(f"\n2. Workspace Volume: {self.workspace_volume():.3f} m³") 

255 

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}") 

263 

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 ] 

271 

272 for point in test_points: 

273 reachable = "✓ Reachable" if self.reach(point) else "✗ Not reachable" 

274 print(f" Point {point}: {reachable}") 

275 

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}") 

280 

281 

282def create_stanford_arm() -> Stanford: 

283 """ 

284 Factory function to create Stanford Arm robot. 

285 

286 Returns: 

287 Stanford robot instance 

288 

289 Examples: 

290 >>> robot = create_stanford_arm() 

291 >>> T = robot.fkine(robot.qr) 

292 """ 

293 return Stanford() 

294 

295 

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

297 # Demo when run as script 

298 stanford = Stanford() 

299 stanford.demo()