Lab 03-02: Inverse Kinematics Solver

Objectives

Implement analytical IK for a 2-DOF planar arm
Handle multiple solutions and singularities
Implement numerical IK using Jacobian
Validate IK solutions with forward kinematics

Materials

Type	Name	Version	Tier
software	Python	3.10+	required
software	NumPy	1.24+	required
software	SciPy	1.10+	recommended
software	Matplotlib	3.5+	required

Background

Inverse kinematics (IK) computes joint angles that achieve a desired end-effector position. Unlike FK, IK often has multiple solutions or no solution.

Instructions

Step 1: Analytical IK for 2-DOF Arm

import numpy as np
import matplotlib.pyplot as plt

L1 = 0.5  # Link 1 length
L2 = 0.4  # Link 2 length

def inverse_kinematics_2dof(x, y, l1=L1, l2=L2, elbow_up=True):
    """
    Analytical inverse kinematics for 2-DOF planar arm.

    Args:
        x, y: Desired end-effector position
        l1, l2: Link lengths
        elbow_up: If True, return elbow-up solution; else elbow-down

    Returns:
        (theta1, theta2): Joint angles in radians, or None if unreachable
    """
    # Check if point is reachable
    dist = np.sqrt(x**2 + y**2)
```python
```python
    if dist > l1 + l2 or dist < abs(l1 - l2):

        return None  # Point is outside workspace

    # Law of cosines to find theta2
    cos_theta2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)

    # Clamp to valid range (handles numerical errors)
    cos_theta2 = np.clip(cos_theta2, -1, 1)

    if elbow_up:
        theta2 = np.arccos(cos_theta2)
    else:
        theta2 = -np.arccos(cos_theta2)

    # Find theta1
    k1 = l1 + l2 * np.cos(theta2)
    k2 = l2 * np.sin(theta2)

    theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)

    return theta1, theta2

# Test IK
target_x, target_y = 0.6, 0.4

# Get both solutions
solution_up = inverse_kinematics_2dof(target_x, target_y, elbow_up=True)
solution_down = inverse_kinematics_2dof(target_x, target_y, elbow_up=False)

print(f"Target position: ({target_x}, {target_y})")
print(f"Elbow-up solution:   θ1={np.degrees(solution_up[0]):.1f}°, θ2={np.degrees(solution_up[1]):.1f}°")
print(f"Elbow-down solution: θ1={np.degrees(solution_down[0]):.1f}°, θ2={np.degrees(solution_down[1]):.1f}°")

Verification: Both solutions should reach the same target point when verified with FK.

Step 2: Verify IK Solutions

def forward_kinematics_2dof(theta1, theta2, l1=L1, l2=L2):
    """Forward kinematics for verification."""
    x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
    return x, y

# Verify both solutions
for name, solution in [("Elbow-up", solution_up), ("Elbow-down", solution_down)]:
    if solution is not None:
        x_fk, y_fk = forward_kinematics_2dof(solution[0], solution[1])
        error = np.sqrt((x_fk - target_x)**2 + (y_fk - target_y)**2)
        print(f"{name}: FK result ({x_fk:.4f}, {y_fk:.4f}), Error: {error:.6f}")

# Visualize both solutions
fig, axes = plt.subplots(1, 2, figsize=(12, 6))

for ax, (name, solution) in zip(axes, [("Elbow-Up", solution_up), ("Elbow-Down", solution_down)]):
    if solution is None:
        continue

    theta1, theta2 = solution

    # Compute positions
    elbow_x = L1 * np.cos(theta1)
    elbow_y = L1 * np.sin(theta1)
    end_x = elbow_x + L2 * np.cos(theta1 + theta2)
    end_y = elbow_y + L2 * np.sin(theta1 + theta2)

    # Plot arm
    ax.plot([0, elbow_x], [0, elbow_y], 'b-', linewidth=3)
    ax.plot([elbow_x, end_x], [elbow_y, end_y], 'r-', linewidth=3)
    ax.plot(0, 0, 'ko', markersize=10)
    ax.plot(elbow_x, elbow_y, 'ko', markersize=8)
    ax.plot(end_x, end_y, 'g^', markersize=12)

    # Plot target
    ax.plot(target_x, target_y, 'mx', markersize=15, markeredgewidth=3, label='Target')

    ax.set_xlim(-1, 1)
    ax.set_ylim(-0.5, 1)
    ax.set_aspect('equal')
    ax.grid(True, alpha=0.3)
    ax.set_title(f"{name} Solution\nθ1={np.degrees(theta1):.1f}°, θ2={np.degrees(theta2):.1f}°")
    ax.legend()

plt.tight_layout()
plt.savefig('ik_solutions.png', dpi=150)
plt.show()

Expected: Two different arm configurations reaching the same target point.

Step 3: Handle Singularities and Unreachable Points

def test_ik_robustness():
    """Test IK with various target points."""
    test_points = [
        (0.6, 0.4, "Normal point"),
        (0.9, 0.0, "At outer boundary"),
        (0.1, 0.0, "At inner boundary"),
        (1.0, 0.0, "Outside workspace"),
        (0.0, 0.0, "At origin (inside hole)"),
        (0.0, 0.9, "On y-axis"),
    ]

    print("IK Robustness Test:")
    print("-" * 60)

    for x, y, description in test_points:
        solution = inverse_kinematics_2dof(x, y)

        if solution is None:
            status = "UNREACHABLE"
            angles = "N/A"
        else:
            # Verify solution
            x_fk, y_fk = forward_kinematics_2dof(solution[0], solution[1])
            error = np.sqrt((x_fk - x)**2 + (y_fk - y)**2)

```python
```python
            if error < 1e-6:

                status = "OK"
            else:
                status = f"ERROR: {error:.6f}"

            angles = f"({np.degrees(solution[0]):.1f}°, {np.degrees(solution[1]):.1f}°)"

        print(f"({x:.1f}, {y:.1f}) {description:25} -> {status:15} {angles}")

test_ik_robustness()

Analysis: Points at boundaries should be solvable; points outside should return None.

Step 4: Implement Numerical IK with Jacobian

def jacobian_2dof(theta1, theta2, l1=L1, l2=L2):
    """
    Compute the Jacobian matrix for 2-DOF arm.

    Returns:
        2x2 Jacobian matrix relating joint velocities to end-effector velocities
    """
    j11 = -l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2)
    j12 = -l2 * np.sin(theta1 + theta2)
    j21 = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    j22 = l2 * np.cos(theta1 + theta2)

    return np.array([[j11, j12], [j21, j22]])

def inverse_kinematics_numerical(x_target, y_target, theta_init=None,
                                  max_iter=100, tol=1e-6, l1=L1, l2=L2):
    """
    Numerical IK using Jacobian pseudoinverse (damped least squares).

    Args:
        x_target, y_target: Desired end-effector position
        theta_init: Initial joint angles (random if None)
        max_iter: Maximum iterations
        tol: Convergence tolerance

    Returns:
        Joint angles or None if failed
    """
    # Initialize
    if theta_init is None:
        theta = np.random.uniform(-np.pi, np.pi, 2)
    else:
        theta = np.array(theta_init)

    damping = 0.01  # Damping factor for numerical stability

    for i in range(max_iter):
        # Current end-effector position
        x_curr, y_curr = forward_kinematics_2dof(theta[0], theta[1], l1, l2)

        # Error
        error = np.array([x_target - x_curr, y_target - y_curr])

```python
```python
        if np.linalg.norm(error) < tol:

            return theta, i  # Converged

        # Jacobian
        J = jacobian_2dof(theta[0], theta[1], l1, l2)

        # Damped least squares (more stable than pseudoinverse)
        JtJ = J.T @ J + damping**2 * np.eye(2)
        delta_theta = np.linalg.solve(JtJ, J.T @ error)

        # Update
        theta = theta + delta_theta

    return None, max_iter  # Did not converge

# Test numerical IK
target_x, target_y = 0.6, 0.4

# Run from multiple initial conditions
print("Numerical IK Results:")
print("-" * 50)

np.random.seed(42)
for trial in range(5):
    result, iterations = inverse_kinematics_numerical(target_x, target_y)

    if result is not None:
        x_fk, y_fk = forward_kinematics_2dof(result[0], result[1])
        error = np.sqrt((x_fk - target_x)**2 + (y_fk - target_y)**2)
        print(f"Trial {trial+1}: θ=({np.degrees(result[0]):.1f}°, {np.degrees(result[1]):.1f}°), "
              f"Iterations: {iterations}, Error: {error:.2e}")
    else:
        print(f"Trial {trial+1}: Failed to converge")

Observation: Numerical IK may find different solutions depending on initial guess. Understanding: This demonstrates why IK is challenging - multiple solutions exist.

Step 5: Compare Analytical and Numerical Solutions

# Generate test grid
test_points = []
for x in np.linspace(-0.7, 0.7, 10):
    for y in np.linspace(-0.7, 0.7, 10):
```python
```python
        if abs(L1 - L2) < np.sqrt(x**2 + y**2) < L1 + L2:

            test_points.append((x, y))

# Compare methods
analytical_times = []
numerical_times = []
errors = []

import time

for x, y in test_points:
    # Analytical
    t0 = time.time()
    sol_analytical = inverse_kinematics_2dof(x, y)
    analytical_times.append(time.time() - t0)

    # Numerical (with good initial guess from analytical)
    t0 = time.time()
    if sol_analytical:
        sol_numerical, _ = inverse_kinematics_numerical(x, y, theta_init=sol_analytical)
    else:
        sol_numerical, _ = inverse_kinematics_numerical(x, y)
    numerical_times.append(time.time() - t0)

    # Compare
    if sol_analytical and sol_numerical is not None:
        x1, y1 = forward_kinematics_2dof(*sol_analytical)
        x2, y2 = forward_kinematics_2dof(*sol_numerical)
        errors.append(np.sqrt((x1-x2)**2 + (y1-y2)**2))

print(f"\nPerformance Comparison ({len(test_points)} points):")
print(f"Analytical: {np.mean(analytical_times)*1000:.3f} ms average")
print(f"Numerical:  {np.mean(numerical_times)*1000:.3f} ms average")
print(f"Speedup:    {np.mean(numerical_times)/np.mean(analytical_times):.1f}x slower")
print(f"Max FK error between solutions: {max(errors):.2e}")

Key Insight: Analytical IK is much faster but only available for simple geometries. Numerical IK is general but slower and may converge to different solutions.

Expected Outcomes

Working analytical IK with multiple solution handling
Proper handling of unreachable points and singularities
Numerical IK implementation using Jacobian
Comparison between analytical and numerical approaches

Rubric

Criterion	Points	Description
Analytical IK	30	Correct implementation with both solutions
Robustness	20	Proper handling of edge cases
Numerical IK	25	Working Jacobian-based solver
Verification	15	FK verification of all solutions
Analysis	10	Comparison and insights

Total: 100 points

Objectives​

Materials​

Background​

Instructions​

Step 1: Analytical IK for 2-DOF Arm​

Step 2: Verify IK Solutions​

Step 3: Handle Singularities and Unreachable Points​

Step 4: Implement Numerical IK with Jacobian​

Step 5: Compare Analytical and Numerical Solutions​

Expected Outcomes​

Rubric​