Inverse Kinematics

3 min read•1 page

Inverse Kinematics (IK) calculates the required joint angles to reach a specific target position. For simple systems like a 2-bone planar arm, we can use exact analytical geometry (Law of Cosines) to find the solution instantly.

Analytical IK:

1. Calculate distance from base to target. 2. Use Law of Cosines to find the elbow angle (Joint 2). 3. Use atan2 and Law of Cosines to find the shoulder angle (Joint 1).

python

1# Analytical IK for a 2-bone planar arm
2import math
3
4def ik_2_bone(target_x, target_y, L1, L2):
5    # Distance to target
6    d_sq = target_x**2 + target_y**2
7    d = math.sqrt(d_sq)
8    
9    # Check if reachable
10    if d > L1 + L2 or d < abs(L1 - L2):
11        return None # Unreachable
12        
13    # Law of Cosines for Joint 2
14    cos_q2 = (d_sq - L1**2 - L2**2) / (2 * L1 * L2)
15    q2 = math.acos(cos_q2)
16    
17    # Angles for Joint 1
18    alpha = math.atan2(target_y, target_x)
19    beta = math.acos((d_sq + L1**2 - L2**2) / (2 * L1 * d))
20    q1 = alpha - beta
21    
22    return q1, q2

Target X

2.50

Target Y

1.50

2 min read•1 page

For arms with many DOFs, analytical solutions become too complex. We switch to numerical methods. The Jacobian matrix maps how changes in each joint angle (velocities) affect the position of the End Effector.

Jacobian Matrix:

1. Find position and rotational axis for each joint. 2. Calculate vector from each joint to End Effector. 3. Cross product of axis and vector gives the derivative (column in J).

python

1# Jacobian Matrix Construction
2# The Jacobian relates joint velocities to end-effector velocities.
3
4def build_jacobian(joint_positions, joint_axes, end_effector_pos):
5    """Computes the 3xN Jacobian for a 3D robot arm."""
6    import numpy as np
7    
8    J = np.zeros((3, len(joint_positions)))
9    
10    for i in range(len(joint_positions)):
11        # Vector from joint to end effector
12        r_i = end_effector_pos - joint_positions[i]
13        
14        # Cross product of rotation axis and vector r_i
15        # gives the linear velocity induced by this joint
16        J[:, i] = np.cross(joint_axes[i], r_i)
17        
18    return J

Joint 1

0.50

Joint 2

0.50

2 min read•1 page

To move the arm towards the target, we need the inverse of the Jacobian to map spatial error back to joint angle changes. A fast and stable approximation is the Jacobian Transpose.

Jacobian Transpose:

1. Calculate error vector from end effector to target. 2. Transpose the Jacobian matrix (swap rows and columns). 3. Multiply J-transpose by the error vector. 4. Scale by a small step size and apply to joint angles.

python

1# Jacobian Transpose IK Solver
2def step_jacobian_transpose(joints, target_pos, step_size=0.05):
3    """Updates joint angles using the Jacobian transpose."""
4    import numpy as np
5    
6    # 1. Forward Kinematics to get end effector
7    end_effector = calc_fk(joints)
8    
9    # 2. Compute error vector to target
10    error_vector = target_pos - end_effector
11    
12    # 3. Build Jacobian matrix J
13    J = build_jacobian(joints, ...)
14    
15    # 4. Transpose J and multiply by error to get angle deltas
16    delta_angles = np.dot(J.T, error_vector) * step_size
17    
18    # 5. Apply update
19    for i in range(len(joints)):
20        joints[i] += delta_angles[i]

Solver Step

0.50

2 min read•1 page

Cyclic Coordinate Descent (CCD) is a heuristic IK method. It solves the problem by adjusting one joint at a time. For a given joint, it calculates the angle required to point the end effector directly at the target.

CCD Step:

1. Find vector from current joint to end effector. 2. Find vector from current joint to target. 3. Calculate angle and axis to rotate effector vector onto target vector. 4. Apply rotation to the joint.

python

1# Cyclic Coordinate Descent (CCD) Single Step
2def ccd_step(joint_pos, effector_pos, target_pos):
3    """Adjusts a single joint to point the effector at the target."""
4    import math
5    import numpy as np
6    
7    # Vector from joint to effector
8    v_effector = effector_pos - joint_pos
9    v_effector /= np.linalg.norm(v_effector)
10    
11    # Vector from joint to target
12    v_target = target_pos - joint_pos
13    v_target /= np.linalg.norm(v_target)
14    
15    # Calculate angle between them using dot product
16    cos_angle = np.dot(v_effector, v_target)
17    angle = math.acos(max(-1.0, min(1.0, cos_angle)))
18    
19    # Cross product gives the rotation axis and direction
20    axis = np.cross(v_effector, v_target)
21    
22    return angle, axis

Rotate Joint

0.00

2 min read•1 page

The full CCD algorithm iterates over all joints, usually starting from the one closest to the end effector and working backwards to the base. This process is repeated until the end effector reaches the target or a maximum number of iterations is met.

CCD Loop:

1. Start at End Effector, move to Joint N. 2. Adjust Joint N to point Effector at Target. 3. Move to Joint N-1, repeat up to Joint 0 (Base). 4. Loop over the whole chain until converged.

python

1# Full CCD Iterative Loop
2def ccd_solve(joints, target_pos, iterations=10, tolerance=0.01):
3    """Iterates backward from effector to base repeatedly."""
4    for _ in range(iterations):
5        # Iterate from the last joint (closest to effector) to the first
6        for i in range(len(joints) - 1, -1, -1):
7            effector_pos = calc_fk(joints)[-1]
8            
9            # Check convergence
10            if distance(effector_pos, target_pos) < tolerance:
11                return True
12                
13            # Perform single step on joint i
14            angle, axis = ccd_step(joints[i].pos, effector_pos, target_pos)
15            joints[i].rotate(angle, axis)
16            
17    return False

Iterations

1.00

2 min read•1 page

Pure IK algorithms can generate mathematically correct solutions that are physically impossible (e.g. arms bending backwards through themselves). By clamping joint angles at each step of the solver, we ensure realistic poses, though it may take more iterations to converge (or prevent reaching some targets entirely).

Constrained IK:

1. Calculate the theoretical angle update for the joint. 2. Calculate the proposed new angle. 3. Clamp the new angle to the joint's physical limits. 4. Apply the clamped angle and proceed to the next joint.

python

1# IK with Joint Constraints
2def ccd_step_constrained(joint_idx, joints, target_pos):
3    """Applies constraints after calculating the raw CCD update."""
4    
5    # 1. Calculate raw update angle
6    raw_angle, axis = ccd_step(joints[joint_idx], target_pos)
7    
8    # 2. Add to current angle
9    new_angle = joints[joint_idx].angle + raw_angle
10    
11    # 3. Clamp against physical limits
12    clamped_angle = clamp(new_angle, joints[joint_idx].min, joints[joint_idx].max)
13    
14    # 4. Apply clamped angle
15    joints[joint_idx].angle = clamped_angle

Iterations

1.00

2 min read•1 page

When a robot arm is fully extended (a kinematic singularity), numerical solvers like the pure Jacobian pseudo-inverse fail because the matrix becomes non-invertible (divide by zero). Methods like Damped Least Squares (DLS) add a small damping factor to maintain stability at the cost of slight accuracy.

Damped Least Squares:

1. Calculate standard Jacobian. 2. Add a damping factor times the Identity matrix before inversion. 3. The damping prevents division by zero at full extension. 4. Results in smooth behavior even when unreachable.

python

1# Damped Least Squares (DLS)
2def solve_dls(J, error, damping=0.1):
3    """Avoids singularities by adding a damping factor."""
4    import numpy as np
5    
6    # Normally we want J_inverse = J^T * (J * J^T)^-1
7    # Near singularities, det(J*J^T) approaches 0.
8    
9    # DLS adds damping^2 * Identity to the inverted part
10    I = np.eye(J.shape[0])
11    J_T = J.T
12    
13    # (J*J^T + lambda^2 * I)^-1
14    inv_term = np.linalg.inv(np.dot(J, J_T) + (damping**2) * I)
15    
16    # J^T * inv_term * error
17    pseudo_inverse = np.dot(J_T, inv_term)
18    
19    return np.dot(pseudo_inverse, error)

Target X

4.50

2 min read•1 page

In interactive applications (like games or VR), IK solvers run continuously in the background, often distributing iterations across multiple frames. This creates a fluid, organic movement as the arm smoothly tracks a dynamic target without stalling the simulation.

Continuous Tracking:

1. Target position updates dynamically (e.g. following a sine wave). 2. IK solver runs 1-2 iterations per frame. 3. The arm "chases" the target smoothly, demonstrating real-time reactivity.

python

1# Interactive IK Tracking
2def update_loop(mouse_pos, dt):
3    """Continuously solves IK towards a moving target."""
4    
5    # 1. Update target position from input
6    target = get_mouse_world_position()
7    
8    # 2. Run a few IK iterations per frame
9    # We don't need full convergence every frame if the target is moving,
10    # distributing the solver load creates a smooth tracking effect.
11    ccd_solve(joints, target, iterations=2)
12    
13    # 3. Render
14    draw_robot(joints)