Coverage for src/robotics_numpy/kinematics/jacobian.py: 91%

99 statements  

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

1""" 

2Jacobian computation for robotics-numpy 

3 

4This module provides functions for computing manipulator Jacobians and manipulability. 

5Jacobians relate joint velocities to end-effector velocities, and are essential for 

6velocity control, singularity analysis, and manipulability evaluation. 

7""" 

8 

9import numpy as np 

10from typing import Optional, Union, List, Tuple, Literal 

11from ..transforms import SE3 

12 

13# Type aliases 

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

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

16 

17 

18def tr2jac(T: np.ndarray) -> np.ndarray: 

19 """ 

20 Transform from end-effector to world frame Jacobian conversion. 

21 

22 This transforms a Jacobian matrix from the end-effector frame to the world frame. 

23 

24 Args: 

25 T: Homogeneous transformation matrix (4x4) 

26 

27 Returns: 

28 6x6 Jacobian transformation matrix 

29 

30 Notes: 

31 For a Jacobian Je in end-effector frame, the equivalent Jacobian J0 in world frame 

32 is J0 = tr2jac(T) @ Je 

33 """ 

34 R = T[:3, :3] 

35 

36 # Construct the 6x6 Jacobian transformation matrix 

37 Jmat = np.zeros((6, 6)) 

38 Jmat[:3, :3] = R 

39 Jmat[3:, 3:] = R 

40 

41 return Jmat 

42 

43 

44def jacobe( 

45 robot, q: JointConfig, end: Optional[int] = None, start: int = 0 

46) -> np.ndarray: 

47 """ 

48 Manipulator Jacobian in end-effector frame 

49 

50 Args: 

51 robot: DHRobot instance 

52 q: Joint coordinate vector 

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

54 start: Start link index (default: 0) 

55 

56 Returns: 

57 The manipulator Jacobian in the end-effector frame (6xn) 

58 

59 Notes: 

60 - The end-effector Jacobian gives the relationship between joint velocities 

61 and end-effector spatial velocity expressed in the end-effector frame. 

62 """ 

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

64 n = robot.n 

65 

66 if end is None: 

67 end = n - 1 

68 

69 if not (0 <= start <= end < n): 

70 raise ValueError(f"Invalid link range: start={start}, end={end}, n={n}") 

71 

72 # Create Jacobian matrix and initialize 

73 J = np.zeros((6, end - start + 1)) 

74 

75 # Get link transforms up to the end link 

76 Tall = robot.fkine_all(q) 

77 

78 # End-effector transform 

79 Te = Tall[end] 

80 

81 # Tool offset for computing the Jacobian at tool tip (if any) 

82 Tt = robot.tool.matrix 

83 

84 # Calculate the Jacobian 

85 for j in range(start, end + 1): 

86 # Joint transform 

87 if j > 0: 

88 Tj = Tall[j - 1] 

89 else: 

90 Tj = robot.base 

91 

92 # Joint type and axis 

93 if robot.joint_types[j] == "R": # Revolute joint 

94 # Revolute joint axis is the z-axis of the joint frame 

95 axis = Tj.matrix[:3, 2] # z-axis of the joint frame 

96 else: # Prismatic joint 

97 # Prismatic joint axis is the z-axis of the joint frame 

98 axis = Tj.matrix[:3, 2] # z-axis of the joint frame 

99 

100 # Position of the joint 

101 joint_pos = Tj.t 

102 

103 # Position of the end-effector considering tool transform 

104 if np.any(Tt[:3, 3] != 0): 

105 # If there's a tool offset, use it 

106 ee_pos = Te.t + Te.R @ Tt[:3, 3] 

107 else: 

108 ee_pos = Te.t 

109 

110 # Displacement from joint to end-effector 

111 r = ee_pos - joint_pos 

112 

113 col = j - start 

114 

115 if robot.joint_types[j] == "R": # Revolute joint 

116 # Angular velocity component (rotational joints) 

117 J[3:, col] = axis 

118 

119 # Linear velocity component (cross product) 

120 J[:3, col] = np.cross(axis, r) 

121 else: # Prismatic joint 

122 # Linear velocity component (prismatic joints) 

123 J[:3, col] = axis 

124 

125 # Prismatic joints don't contribute to angular velocity 

126 J[3:, col] = 0 

127 

128 # The Jacobian as computed is in the world frame 

129 # Convert to end-effector frame 

130 Tr = np.eye(6) 

131 Tr[:3, :3] = Te.R.T 

132 Tr[3:, 3:] = Te.R.T 

133 

134 return Tr @ J 

135 

136 

137def jacob0( 

138 robot, 

139 q: JointConfig = None, 

140 T=None, 

141 half: Optional[str] = None, 

142 start: int = 0, 

143 end: Optional[int] = None, 

144) -> np.ndarray: 

145 """ 

146 Manipulator Jacobian in world frame 

147 

148 Args: 

149 robot: DHRobot instance 

150 q: Joint coordinate vector 

151 T: Forward kinematics if known (SE3 matrix) 

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

153 start: Start link index (default: 0) 

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

155 

156 Returns: 

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

158 

159 Notes: 

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

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

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

163 Corke, Spong et al., Siciliano et al. 

164 """ 

165 if q is None: 

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

167 

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

169 

170 # Compute forward kinematics if not provided 

171 if T is None: 

172 T = robot.fkine(q, end=end, start=start) 

173 

174 # Compute Jacobian in end-effector frame 

175 Je = jacobe(robot, q, end=end, start=start) 

176 

177 # Transform to world frame 

178 J0 = tr2jac(T.matrix) @ Je 

179 

180 # Return top or bottom half if requested 

181 if half is not None: 

182 if half == "trans": 

183 J0 = J0[:3, :] 

184 elif half == "rot": 

185 J0 = J0[3:, :] 

186 else: 

187 raise ValueError("half must be 'trans' or 'rot'") 

188 

189 return J0 

190 

191 

192def manipulability( 

193 robot, 

194 q: Optional[JointConfig] = None, 

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

196 end: Optional[int] = None, 

197 start: int = 0, 

198 method: str = "yoshikawa", 

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

200 **kwargs, 

201) -> float: 

202 """ 

203 Manipulability measure 

204 

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

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

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

208 robot is at a singularity. 

209 

210 Args: 

211 robot: DHRobot instance 

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

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

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

215 start: Start link index (default: 0) 

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

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

218 

219 Returns: 

220 Manipulability index 

221 

222 Notes: 

223 Supported measures: 

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

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

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

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

228 

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

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

231 and rotational manipulability separately. 

232 """ 

233 # Need either joint configuration or Jacobian 

234 if q is None and J is None: 

235 raise ValueError("Either q or J must be provided") 

236 

237 # Compute Jacobian if not provided 

238 if J is None: 

239 J = jacob0(robot, q, end=end, start=start) 

240 

241 # Select axes to use 

242 if isinstance(axes, str): 

243 if axes == "all": 

244 J_used = J 

245 elif axes == "trans": 

246 J_used = J[:3, :] 

247 elif axes == "rot": 

248 J_used = J[3:, :] 

249 else: 

250 raise ValueError("axes must be 'all', 'trans', 'rot', or a boolean list") 

251 elif isinstance(axes, list) and all(isinstance(x, bool) for x in axes): 

252 if len(axes) != 6: 

253 raise ValueError("Boolean list for axes must have 6 elements") 

254 # Select rows based on boolean mask 

255 J_used = J[np.array(axes), :] 

256 else: 

257 raise ValueError("axes must be 'all', 'trans', 'rot', or a boolean list") 

258 

259 # Compute manipulability based on selected method 

260 if method == "yoshikawa": 

261 # Yoshikawa's measure - square root of determinant of J*J^T 

262 m = np.sqrt(np.linalg.det(J_used @ J_used.T)) 

263 

264 elif method == "invcondition": 

265 # Inverse condition number - ratio of minimum to maximum singular value 

266 s = np.linalg.svd(J_used, compute_uv=False) 

267 if np.max(s) == 0: 

268 m = 0 # At singularity 

269 else: 

270 m = np.min(s) / np.max(s) 

271 

272 elif method == "minsingular": 

273 # Minimum singular value - distance from singularity 

274 s = np.linalg.svd(J_used, compute_uv=False) 

275 m = np.min(s) 

276 

277 elif method == "asada": 

278 # Asada's method - uses robot dynamics (requires mass/inertia data) 

279 # Not fully implemented without robot dynamics 

280 raise NotImplementedError( 

281 "Asada's method requires robot dynamics implementation" 

282 ) 

283 

284 else: 

285 raise ValueError( 

286 "Unknown method. Use 'yoshikawa', 'invcondition', or 'minsingular'" 

287 ) 

288 

289 return m 

290 

291 

292# Velocity ellipsoid functions 

293def joint_velocity_ellipsoid( 

294 J: np.ndarray, dq_max: np.ndarray = None 

295) -> Tuple[np.ndarray, np.ndarray]: 

296 """ 

297 Compute joint velocity ellipsoid axes. 

298 

299 Args: 

300 J: Jacobian matrix (6xn) 

301 dq_max: Maximum joint velocities (n-vector) 

302 

303 Returns: 

304 Tuple of (eigenvalues, eigenvectors) representing the ellipsoid 

305 """ 

306 if dq_max is None: 

307 # Unit joint velocity sphere 

308 M = J @ J.T 

309 else: 

310 # Normalize by maximum joint velocities 

311 dq_max = np.asarray(dq_max) 

312 Wq = np.diag(1.0 / dq_max**2) 

313 M = J @ Wq @ J.T 

314 

315 # Eigendecomposition gives ellipsoid axes 

316 eigvals, eigvecs = np.linalg.eigh(M) 

317 

318 return eigvals, eigvecs