Robotic Arm Simulation

1. Kinematic Modeling

Consider a planar two-link manipulator rooted at the Cartesian origin (0, 0) with link lengths l₁, l₂ and joint rotation angles θ₁, θ₂. The angle θ₁ is measured relative to the global horizontal x-axis, while θ₂ represents the relative angle of the second link with respect to the first link.

The forward kinematics map F: ℝ² → ℝ² computes the Cartesian position (x, y)ᵖ of the end-effector from joint space coordinates:

Given a desired end-effector target position pos* = (x*, y*)ᵖ, inverse kinematics seeks joint angles θ = (θ₁, θ₂)ᵖ that satisfy the non-linear vector equation:

2. The Jacobian Matrix: Analytical vs. Finite-Difference

The Jacobian matrix J(θ) ∈ ℝ²×² is the matrix of all first-order partial derivatives of the forward kinematics function. It describes how infinitesimal variations in joint angles produce linear displacements in Cartesian space:

Differentiating the forward kinematics equations with respect to each joint angle yields the exact analytical Jacobian:

Central Finite-Difference Approximation

In more complex multi-body robotic systems (such as 6-DOF industrial manipulators or tendon-driven continuum arms), deriving and computing symbolic analytical derivatives can be cumbersome or computationally expensive. We implemented a central finite-difference approximation with perturbation step h:

3. Inverse Kinematics Solvers: Newton vs. Broyden

To iteratively find joint angles θ that drive the end-effector error d = pos* - F(θ) below a specified tolerance threshold (ε = 0.001), two primary numerical optimization strategies were implemented and evaluated:

Method 1: Newton-Raphson Method

At each iteration step k, Newton's method forms a local linear Taylor approximation of the kinematic mapping and solves the linear system for the angular correction step s_k:

Newton's method demonstrates quadratic local convergence when the initial guess is sufficiently close to the solution. However, it requires recalculating the entire Jacobian matrix at every iteration, which for high-DOF arms or complex forward kinematic models can become the dominant computational bottleneck.

Method 2: Broyden's Quasi-Newton Method

Broyden's method generalizes the 1D secant method to multiple dimensions. Rather than evaluating fresh derivatives at every iteration, Broyden begins with an initial Jacobian estimate B₀ (evaluated once at θ₀) and updates the matrix iteratively using the minimum Frobenius-norm rank-1 correction that satisfies the secant condition:

where s_k = θ_k+1 - θ_k is the joint step and y_k = f(θ_k+1) - f(θ_k) is the change in residual error.

4. Kinematic Singularities & Damped Least Squares

A kinematic singularity occurs when the manipulator loses one or more degrees of freedom, causing the Jacobian matrix to lose rank. For our 2D planar arm, the determinant of the Jacobian is:

Singularities occur when θ₂ = 0 (arm completely outstretched at the workspace boundary R = l₁ + l₂) or θ₂ = π (arm completely folded in on itself). When det(J) → 0, naive matrix inversion produces unbounded angular velocities (s_k → ∞).

To prevent numerical blow-ups when users test near-singular postures, we introduced a Damped Least Squares (Levenberg-Marquardt) regularization:

The damping parameter λ ≈ 0.05 smoothly limits step magnitudes near singular configurations, ensuring graceful degradation rather than erratic numerical oscillations.

5. Original MATLAB Implementation

Below are excerpts from the original MATLAB script developed for CMPUT 340, highlighting the forward kinematics, finite-difference approximation, and iterative inverse kinematics routine:

function [pos, J] = evalRobot2D(l, theta)
    l1 = l(1);
    l2 = l(2);
    th1 = theta(1);
    th2 = theta(2);
    
    x = l1*cos(th1) + l2*cos(th1 + th2);
    y = l1*sin(th1) + l2*sin(th1 + th2);
    pos = [x; y];

    dx_dth1 = -l1*sin(th1) - l2*sin(th1 + th2);
    dx_dth2 = -l2*sin(th1 + th2);
    dy_dth1 =  l1*cos(th1) + l2*cos(th1 + th2);
    dy_dth2 =  l2*cos(th1 + th2);

    J = [dx_dth1, dx_dth2; dy_dth1, dy_dth2];
end

function J = fdJacob2D(l, theta, h)
    dpos_dtheta1 = ( evalRobot2D(l, theta+[h;0]) - evalRobot2D(l, theta-[h;0]) ) / (2*h);
    dpos_dtheta2 = ( evalRobot2D(l, theta+[0;h]) - evalRobot2D(l, theta-[0;h]) ) / (2*h);
    J = [dpos_dtheta1, dpos_dtheta2];
end

function theta = invKin2D(l, theta0, pos, n, mode)
    tol = 0.001; % Tolerance threshold
    if mode == 0
        % Broyden's Quasi-Newton Method
        theta = theta0;
        [current_pos, J] = evalRobot2D(l, theta); % Initial Jacobian
        d = current_pos - pos;
    
        for k = 1:n
            if norm(d) < tol
                break;
            end
            s_k = -J \ d;
            theta = theta + s_k;
    
            [new_pos, ~] = evalRobot2D(l, theta);
            d_new = new_pos - pos;
            y_k = d_new - d;
    
            % Rank-1 Broyden update
            J = J + ((y_k - J * s_k) * s_k') / (s_k' * s_k);
            d = d_new;
        end
    elseif mode == 1
        % Newton's Method
        theta = theta0;
        for k = 1:n
            [current_pos, J] = evalRobot2D(l, theta);
            d = pos - current_pos;
            if norm(d) < tol
                break;
            end
            s_k = J \ d;
            theta = theta + s_k;
        end
    end
end

6. Key Conclusions & Insights

Through experimental testing of both solvers across various workspace configurations, several core insights emerged:

• Initial Guess Matters: Because the inverse kinematics mapping has multiple solutions (elbow-up vs. elbow-down configurations) and local minima outside the reachable workspace, both Newton and Broyden depend on a reasonable initial posture θ₀.
• Broyden Minimizes Function Calls: When the forward kinematics evaluation is computationally expensive, Broyden's method is vastly superior per second of CPU time, even though it requires roughly 1.5x to 2x more iteration steps than Newton's method.
• Exact Jacobians Accelerate Terminal Convergence: Once within the basin of attraction, Newton's method achieves machine precision in just a few steps, doubling the number of correct significant digits with each step due to quadratic convergence.