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
« prev ^ index » next coverage.py v7.9.2, created at 2025-07-14 16:02 +0200
1"""
2Jacobian computation for robotics-numpy
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"""
9import numpy as np
10from typing import Optional, Union, List, Tuple, Literal
11from ..transforms import SE3
13# Type aliases
14ArrayLike = Union[float, int, np.ndarray, List[float]]
15JointConfig = Union[List[float], np.ndarray]
18def tr2jac(T: np.ndarray) -> np.ndarray:
19 """
20 Transform from end-effector to world frame Jacobian conversion.
22 This transforms a Jacobian matrix from the end-effector frame to the world frame.
24 Args:
25 T: Homogeneous transformation matrix (4x4)
27 Returns:
28 6x6 Jacobian transformation matrix
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]
36 # Construct the 6x6 Jacobian transformation matrix
37 Jmat = np.zeros((6, 6))
38 Jmat[:3, :3] = R
39 Jmat[3:, 3:] = R
41 return Jmat
44def jacobe(
45 robot, q: JointConfig, end: Optional[int] = None, start: int = 0
46) -> np.ndarray:
47 """
48 Manipulator Jacobian in end-effector frame
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)
56 Returns:
57 The manipulator Jacobian in the end-effector frame (6xn)
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
66 if end is None:
67 end = n - 1
69 if not (0 <= start <= end < n):
70 raise ValueError(f"Invalid link range: start={start}, end={end}, n={n}")
72 # Create Jacobian matrix and initialize
73 J = np.zeros((6, end - start + 1))
75 # Get link transforms up to the end link
76 Tall = robot.fkine_all(q)
78 # End-effector transform
79 Te = Tall[end]
81 # Tool offset for computing the Jacobian at tool tip (if any)
82 Tt = robot.tool.matrix
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
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
100 # Position of the joint
101 joint_pos = Tj.t
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
110 # Displacement from joint to end-effector
111 r = ee_pos - joint_pos
113 col = j - start
115 if robot.joint_types[j] == "R": # Revolute joint
116 # Angular velocity component (rotational joints)
117 J[3:, col] = axis
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
125 # Prismatic joints don't contribute to angular velocity
126 J[3:, col] = 0
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
134 return Tr @ J
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
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)
156 Returns:
157 The manipulator Jacobian in the world frame (6xn)
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")
168 q = np.asarray(q, dtype=float)
170 # Compute forward kinematics if not provided
171 if T is None:
172 T = robot.fkine(q, end=end, start=start)
174 # Compute Jacobian in end-effector frame
175 Je = jacobe(robot, q, end=end, start=start)
177 # Transform to world frame
178 J0 = tr2jac(T.matrix) @ Je
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'")
189 return J0
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
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.
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
219 Returns:
220 Manipulability index
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)
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")
237 # Compute Jacobian if not provided
238 if J is None:
239 J = jacob0(robot, q, end=end, start=start)
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")
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))
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)
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)
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 )
284 else:
285 raise ValueError(
286 "Unknown method. Use 'yoshikawa', 'invcondition', or 'minsingular'"
287 )
289 return m
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.
299 Args:
300 J: Jacobian matrix (6xn)
301 dq_max: Maximum joint velocities (n-vector)
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
315 # Eigendecomposition gives ellipsoid axes
316 eigvals, eigvecs = np.linalg.eigh(M)
318 return eigvals, eigvecs