Jacobian Matrices for Forward and Inverse Kinematics

calendarDec 13, 2024 · 10 min read · robotics kinematics jacobian inverse-kinematics velocity  ·
Share on: linkedincopy

Jacobian matrices relate joint velocities to end-effector velocities, essential for inverse kinematics and control.

Overview

The Jacobian matrix $J$ maps joint velocities $\dot{q}$ to end-effector velocity $\dot{x}$:

$$ \dot{x} = J(q) \dot{q} $$

Where:

  • $\dot{x} = [\dot{x}, \dot{y}, \dot{z}, \omega_x, \omega_y, \omega_z]^T$ (linear and angular velocity)
  • $\dot{q} = [\dot{q}_1, \dot{q}_2, \ldots, \dot{q}_n]^T$ (joint velocities)
  • $J(q)$ is a $6 \times n$ matrix (for spatial manipulators)

Geometric Jacobian

Column-wise Construction

For joint $i$, the $i$-th column of the Jacobian is:

Revolute Joint:

$$ J_i = \begin{bmatrix} z_{i-1} \times (o_n - o_{i-1}) \\ z_{i-1} \end{bmatrix} $$

Prismatic Joint:

$$ J_i = \begin{bmatrix} z_{i-1} \\ 0 \end{bmatrix} $$

Where:

  • $z_{i-1}$: Joint axis direction
  • $o_n$: End-effector position
  • $o_{i-1}$: Joint $i$ position

Python Implementation

 1import numpy as np
 2
 3def compute_jacobian(transforms, joint_types):
 4    """
 5    Compute geometric Jacobian
 6    
 7    Args:
 8        transforms: List of 4x4 transformation matrices to each joint
 9        joint_types: List of 'revolute' or 'prismatic'
10    
11    Returns:
12        6 x n Jacobian matrix
13    """
14    n = len(joint_types)
15    J = np.zeros((6, n))
16    
17    # End-effector position
18    o_n = transforms[-1][:3, 3]
19    
20    for i in range(n):
21        # Joint i position and z-axis
22        o_i = transforms[i][:3, 3]
23        z_i = transforms[i][:3, 2]
24        
25        if joint_types[i] == 'revolute':
26            # Linear velocity component
27            J[:3, i] = np.cross(z_i, o_n - o_i)
28            # Angular velocity component
29            J[3:, i] = z_i
30        else:  # prismatic
31            # Linear velocity component
32            J[:3, i] = z_i
33            # Angular velocity component (zero)
34            J[3:, i] = 0
35    
36    return J
37
38# Example: 2-link planar arm
39def planar_arm_jacobian(theta1, theta2, L1=1.0, L2=0.8):
40    """Jacobian for 2-link planar arm"""
41    c1 = np.cos(theta1)
42    s1 = np.sin(theta1)
43    c12 = np.cos(theta1 + theta2)
44    s12 = np.sin(theta1 + theta2)
45    
46    J = np.array([
47        [-L1*s1 - L2*s12, -L2*s12],
48        [ L1*c1 + L2*c12,  L2*c12],
49        [ 1,               1      ]
50    ])
51    
52    return J
53
54# Test
55J = planar_arm_jacobian(np.pi/4, np.pi/3)
56print("Jacobian:")
57print(J)

Analytical Jacobian

Maps joint velocities to end-effector position and orientation rates (e.g., Euler angles).

Relationship to Geometric Jacobian

$$ J_a = T(x) J_g $$

Where $T(x)$ transforms angular velocity to orientation rate.

For ZYX Euler Angles

 1def euler_transformation_matrix(roll, pitch, yaw):
 2    """
 3    Transformation from angular velocity to Euler angle rates
 4    
 5    ω = T(φ, θ, ψ) * [φ̇, θ̇, ψ̇]ᵀ
 6    """
 7    cr = np.cos(roll)
 8    sr = np.sin(roll)
 9    cp = np.cos(pitch)
10    sp = np.sin(pitch)
11    
12    T = np.array([
13        [cp, 0, 1],
14        [sp*sr, cr, 0],
15        [sp*cr, -sr, 0]
16    ])
17    
18    return T
19
20def analytical_jacobian(geometric_jacobian, euler_angles):
21    """Convert geometric to analytical Jacobian"""
22    roll, pitch, yaw = euler_angles
23    
24    T = euler_transformation_matrix(roll, pitch, yaw)
25    T_inv = np.linalg.inv(T)
26    
27    # Build transformation matrix
28    T_full = np.eye(6)
29    T_full[3:, 3:] = T_inv
30    
31    J_analytical = T_full @ geometric_jacobian
32    
33    return J_analytical

Inverse Kinematics with Jacobian

Jacobian Inverse Method

For square Jacobian ($n = 6$):

$$ \dot{q} = J^{-1}(q) \dot{x} $$

 1def inverse_kinematics_step(current_joints, target_pose, current_pose, 
 2                            robot, dt=0.01, gain=1.0):
 3    """
 4    Single IK iteration using Jacobian inverse
 5    
 6    Args:
 7        current_joints: Current joint angles
 8        target_pose: Desired end-effector pose (position + orientation)
 9        current_pose: Current end-effector pose
10        robot: Robot object with compute_jacobian method
11        dt: Time step
12        gain: Convergence gain
13    
14    Returns:
15        Updated joint angles
16    """
17    # Compute pose error
18    position_error = target_pose[:3] - current_pose[:3]
19    orientation_error = target_pose[3:] - current_pose[3:]
20    
21    error = np.concatenate([position_error, orientation_error])
22    
23    # Desired end-effector velocity
24    v_desired = gain * error
25    
26    # Compute Jacobian
27    J = robot.compute_jacobian(current_joints)
28    
29    # Compute joint velocities
30    try:
31        q_dot = np.linalg.solve(J, v_desired)
32    except np.linalg.LinAlgError:
33        print("Warning: Singular Jacobian, using pseudoinverse")
34        q_dot = np.linalg.pinv(J) @ v_desired
35    
36    # Update joints
37    new_joints = current_joints + q_dot * dt
38    
39    return new_joints

Jacobian Pseudoinverse (Redundant/Underconstrained)

For non-square Jacobian:

$$ \dot{q} = J^+ \dot{x} $$

Where $J^+ = J^T(JJ^T)^{-1}$ is the Moore-Penrose pseudoinverse.

 1def inverse_kinematics_pseudoinverse(current_joints, target_pose, 
 2                                     current_pose, robot, 
 3                                     max_iterations=100, tolerance=1e-3):
 4    """
 5    IK using Jacobian pseudoinverse
 6    
 7    Works for redundant and underconstrained manipulators
 8    """
 9    joints = current_joints.copy()
10    
11    for iteration in range(max_iterations):
12        # Current pose
13        T = robot.forward_kinematics(joints)
14        current_pos = T[:3, 3]
15        
16        # Position error
17        error = target_pose[:3] - current_pos
18        error_norm = np.linalg.norm(error)
19        
20        if error_norm < tolerance:
21            print(f"Converged in {iteration} iterations")
22            return joints, True
23        
24        # Compute Jacobian (position only)
25        J = robot.compute_jacobian(joints)[:3, :]
26        
27        # Pseudoinverse
28        J_pinv = np.linalg.pinv(J)
29        
30        # Update
31        delta_q = J_pinv @ error
32        joints = joints + 0.1 * delta_q  # Step size 0.1
33    
34    print(f"Did not converge after {max_iterations} iterations")
35    return joints, False

Damped Least Squares (Levenberg-Marquardt)

Handles singularities better than pure pseudoinverse.

$$ \dot{q} = J^T(JJ^T + \lambda^2 I)^{-1} \dot{x} $$

 1def damped_least_squares_ik(current_joints, target_pose, robot,
 2                            max_iterations=100, tolerance=1e-3,
 3                            lambda_damping=0.01):
 4    """
 5    IK using damped least squares
 6    
 7    Args:
 8        lambda_damping: Damping factor (larger = more stable, slower convergence)
 9    """
10    joints = current_joints.copy()
11    
12    for iteration in range(max_iterations):
13        # Current pose
14        T = robot.forward_kinematics(joints)
15        current_pos = T[:3, 3]
16        
17        # Error
18        error = target_pose[:3] - current_pos
19        error_norm = np.linalg.norm(error)
20        
21        if error_norm < tolerance:
22            return joints, True
23        
24        # Jacobian
25        J = robot.compute_jacobian(joints)[:3, :]
26        
27        # Damped least squares
28        JJT = J @ J.T
29        damping_matrix = lambda_damping**2 * np.eye(JJT.shape[0])
30        
31        delta_q = J.T @ np.linalg.solve(JJT + damping_matrix, error)
32        
33        # Update with adaptive step size
34        alpha = 0.5  # Step size
35        joints = joints + alpha * delta_q
36    
37    return joints, False

Singularity Analysis

Detecting Singularities

 1def analyze_singularities(robot, joints):
 2    """Analyze manipulator singularities"""
 3    J = robot.compute_jacobian(joints)
 4    
 5    # Singular value decomposition
 6    U, s, Vt = np.linalg.svd(J)
 7    
 8    # Condition number
 9    condition_number = s[0] / s[-1] if s[-1] > 1e-10 else np.inf
10    
11    # Manipulability measure (Yoshikawa)
12    manipulability = np.sqrt(np.linalg.det(J @ J.T))
13    
14    # Check singularity
15    is_singular = condition_number > 1000 or manipulability < 0.01
16    
17    return {
18        'singular_values': s,
19        'condition_number': condition_number,
20        'manipulability': manipulability,
21        'is_singular': is_singular,
22        'rank': np.sum(s > 1e-10)
23    }
24
25# Example
26analysis = analyze_singularities(robot, joints)
27print(f"Condition number: {analysis['condition_number']:.2f}")
28print(f"Manipulability: {analysis['manipulability']:.4f}")
29print(f"Singular: {analysis['is_singular']}")

Manipulability Ellipsoid

 1def plot_manipulability_ellipsoid(robot, joints):
 2    """Visualize manipulability ellipsoid"""
 3    import matplotlib.pyplot as plt
 4    from mpl_toolkits.mplot3d import Axes3D
 5    
 6    J = robot.compute_jacobian(joints)[:3, :]  # Position part
 7    
 8    # SVD
 9    U, s, Vt = np.linalg.svd(J)
10    
11    # Generate ellipsoid
12    u = np.linspace(0, 2*np.pi, 50)
13    v = np.linspace(0, np.pi, 50)
14    x = np.outer(np.cos(u), np.sin(v))
15    y = np.outer(np.sin(u), np.sin(v))
16    z = np.outer(np.ones(np.size(u)), np.cos(v))
17    
18    # Scale by singular values
19    for i in range(len(x)):
20        for j in range(len(x)):
21            point = np.array([x[i,j], y[i,j], z[i,j]])
22            scaled = U @ np.diag(s) @ point
23            x[i,j], y[i,j], z[i,j] = scaled
24    
25    # Plot
26    fig = plt.figure(figsize=(10, 8))
27    ax = fig.add_subplot(111, projection='3d')
28    ax.plot_surface(x, y, z, alpha=0.6, cmap='viridis')
29    ax.set_xlabel('X')
30    ax.set_ylabel('Y')
31    ax.set_zlabel('Z')
32    ax.set_title('Manipulability Ellipsoid')
33    plt.show()

Null Space Motion (Redundant Manipulators)

For redundant manipulators ($n > 6$), use null space for secondary objectives.

$$ \dot{q} = J^+ \dot{x} + (I - J^+ J) \dot{q}_0 $$

Where $(I - J^+ J)$ projects $\dot{q}_0$ onto the null space.

 1def ik_with_null_space(current_joints, target_velocity, 
 2                       secondary_objective, robot):
 3    """
 4    IK with null space optimization
 5    
 6    Args:
 7        current_joints: Current joint configuration
 8        target_velocity: Desired end-effector velocity
 9        secondary_objective: Secondary task joint velocities
10        robot: Robot object
11    
12    Returns:
13        Joint velocities achieving primary task + secondary objective
14    """
15    J = robot.compute_jacobian(current_joints)
16    J_pinv = np.linalg.pinv(J)
17    
18    # Primary task
19    q_dot_primary = J_pinv @ target_velocity
20    
21    # Null space projector
22    n = len(current_joints)
23    I = np.eye(n)
24    null_space_projector = I - J_pinv @ J
25    
26    # Secondary task (projected onto null space)
27    q_dot_secondary = null_space_projector @ secondary_objective
28    
29    # Combined motion
30    q_dot = q_dot_primary + q_dot_secondary
31    
32    return q_dot
33
34# Example: Joint limit avoidance as secondary task
35def joint_limit_avoidance(joints, joint_limits, gain=0.1):
36    """Generate joint velocities to avoid limits"""
37    q_dot_secondary = np.zeros_like(joints)
38    
39    for i, (q, limits) in enumerate(zip(joints, joint_limits)):
40        q_mid = (limits['max'] + limits['min']) / 2
41        q_range = limits['max'] - limits['min']
42        
43        # Gradient pushes toward middle
44        q_dot_secondary[i] = -gain * (q - q_mid) / q_range
45    
46    return q_dot_secondary

Velocity IK Example

Complete example with 3-DOF RRR manipulator.

 1class RRRManipulatorWithJacobian:
 2    def __init__(self, L1=0.5, L2=0.4, L3=0.3):
 3        self.L1 = L1
 4        self.L2 = L2
 5        self.L3 = L3
 6    
 7    def forward_kinematics(self, theta1, theta2, theta3):
 8        """Compute end-effector position"""
 9        c1 = np.cos(theta1)
10        s1 = np.sin(theta1)
11        c12 = np.cos(theta1 + theta2)
12        s12 = np.sin(theta1 + theta2)
13        c123 = np.cos(theta1 + theta2 + theta3)
14        s123 = np.sin(theta1 + theta2 + theta3)
15        
16        x = c1 * (self.L2*np.cos(theta2) + self.L3*np.cos(theta2 + theta3))
17        y = s1 * (self.L2*np.cos(theta2) + self.L3*np.cos(theta2 + theta3))
18        z = self.L1 + self.L2*np.sin(theta2) + self.L3*np.sin(theta2 + theta3)
19        
20        return np.array([x, y, z])
21    
22    def compute_jacobian(self, theta1, theta2, theta3):
23        """Compute 3x3 Jacobian (position only)"""
24        c1 = np.cos(theta1)
25        s1 = np.sin(theta1)
26        s2 = np.sin(theta2)
27        c2 = np.cos(theta2)
28        s23 = np.sin(theta2 + theta3)
29        c23 = np.cos(theta2 + theta3)
30        
31        L2c2_L3c23 = self.L2*c2 + self.L3*c23
32        
33        J = np.array([
34            [-s1*L2c2_L3c23, -c1*(self.L2*s2 + self.L3*s23), -c1*self.L3*s23],
35            [ c1*L2c2_L3c23, -s1*(self.L2*s2 + self.L3*s23), -s1*self.L3*s23],
36            [ 0,              self.L2*c2 + self.L3*c23,       self.L3*c23    ]
37        ])
38        
39        return J
40    
41    def inverse_kinematics(self, target_pos, initial_guess=None,
42                          max_iter=100, tol=1e-3):
43        """Numerical IK using Jacobian"""
44        if initial_guess is None:
45            joints = np.array([0.0, np.pi/4, 0.0])
46        else:
47            joints = initial_guess.copy()
48        
49        for i in range(max_iter):
50            current_pos = self.forward_kinematics(*joints)
51            error = target_pos - current_pos
52            
53            if np.linalg.norm(error) < tol:
54                return joints, True
55            
56            J = self.compute_jacobian(*joints)
57            
58            # Damped least squares
59            lambda_damping = 0.01
60            JJT = J @ J.T
61            delta_q = J.T @ np.linalg.solve(
62                JJT + lambda_damping**2 * np.eye(3), error
63            )
64            
65            joints = joints + 0.5 * delta_q
66        
67        return joints, False
68
69# Example usage
70robot = RRRManipulatorWithJacobian()
71
72# Forward kinematics
73joints = np.array([np.pi/6, np.pi/4, np.pi/6])
74pos = robot.forward_kinematics(*joints)
75print(f"Forward kinematics: {pos}")
76
77# Jacobian
78J = robot.compute_jacobian(*joints)
79print(f"\nJacobian:\n{J}")
80
81# Inverse kinematics
82target = np.array([0.3, 0.2, 0.8])
83solution, success = robot.inverse_kinematics(target)
84if success:
85    print(f"\nIK solution: {np.degrees(solution)} degrees")
86    print(f"Verification: {robot.forward_kinematics(*solution)}")

Numerical Jacobian (Finite Differences)

When analytical Jacobian is difficult to derive.

 1def numerical_jacobian(robot, joints, epsilon=1e-6):
 2    """
 3    Compute Jacobian using finite differences
 4    
 5    Args:
 6        robot: Robot with forward_kinematics method
 7        joints: Current joint configuration
 8        epsilon: Finite difference step size
 9    """
10    n = len(joints)
11    
12    # Current position
13    T0 = robot.forward_kinematics(*joints)
14    pos0 = T0[:3, 3]
15    
16    J = np.zeros((3, n))
17    
18    for i in range(n):
19        # Perturb joint i
20        joints_plus = joints.copy()
21        joints_plus[i] += epsilon
22        
23        # Forward kinematics
24        T_plus = robot.forward_kinematics(*joints_plus)
25        pos_plus = T_plus[:3, 3]
26        
27        # Finite difference
28        J[:, i] = (pos_plus - pos0) / epsilon
29    
30    return J
31
32# Verify against analytical Jacobian
33J_analytical = robot.compute_jacobian(*joints)
34J_numerical = numerical_jacobian(robot, joints)
35
36print("Analytical Jacobian:")
37print(J_analytical)
38print("\nNumerical Jacobian:")
39print(J_numerical)
40print("\nDifference:")
41print(np.abs(J_analytical - J_numerical))

Further Reading

Related Snippets