Forward and Inverse Kinematics

1. Forward vs Inverse Kinematics

Robotics hinges on the duality between joint and operational spaces. Forward Kinematics (FK) maps given joint angles $q = (\theta_1, \theta_2)$ to a unique end-effector pose $(x, y)$. Inverse Kinematics (IK) solves the reverse problem: given a desired target pose in space, find the actuator angles needed to reach it. While forward kinematics is a direct trigonometric evaluation with a single solution, inverse kinematics is highly non-linear and yields multiple solutions—or none at all.

2. Forward Kinematics: Trigonometric Composition

For a two-link planar arm with link lengths $L_1 = 1.00\text{ m}$ and $L_2 = 0.80\text{ m}$, the end-effector position is found by summing the Cartesian projections of each link. The elbow joint sits at $(L_1 \cos\theta_1, L_1 \sin\theta_1)$. Link 2 rotates relative to Link 1 by $\theta_2$, pointing at absolute world angle $(\theta_1 + \theta_2)$:

$x = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2)$

$y = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2)$

3. Target Feasibility and the Reach Triangle

Consider commanding the arm to grasp our labelled coffee mug positioned at $(x, y) = (1.20\text{ m}, 0.60\text{ m})$. The distance from the shoulder base to the target is:

$r = \sqrt{x^2 + y^2} = \sqrt{1.20^2 + 0.60^2} = \sqrt{1.44 + 0.36} = \sqrt{1.80} \approx 1.342\text{ m}$

Before solving trigonometric equations, we test the triangle inequality: $|L_1 - L_2| \le r \le L_1 + L_2$. Because $0.20\text{ m} \le 1.342\text{ m} \le 1.80\text{ m}$, the target is physically reachable.

4. Solving the Elbow: Law of Cosines

The base, elbow, and mug form a triangle with side lengths $L_1$, $L_2$, and $r$. By the Law of Cosines:

$r^2 = L_1^2 + L_2^2 + 2 L_1 L_2 \cos\theta_2$

$\cos\theta_2 = \frac{r^2 - L_1^2 - L_2^2}{2 L_1 L_2} = \frac{1.80 - 1.00 - 0.64}{2(1.00)(0.80)} = \frac{0.16}{1.60} = 0.10$

Taking the arccosine yields two valid solutions:

$\theta_2 = \pm\arccos(0.10) \approx \pm 84.26^\circ \; (\pm 1.4706\text{ rad})$

5. Elbow-Up vs Elbow-Down Configurations

The plus-or-minus sign in the arccosine reflects physical geometry:

• Elbow-Down ($\theta_2 = +84.26^\circ$): The forearm bends counterclockwise relative to the upper arm.
• Elbow-Up ($\theta_2 = -84.26^\circ$): The forearm bends clockwise relative to the upper arm.

Both configurations position the gripper at the exact coordinate $(1.20\text{ m}, 0.60\text{ m})$.

6. Solving the Shoulder Angle

With $\theta_2$ established, we solve for shoulder angle $\theta_1$. Let $\alpha = \text{atan2}(y, x) = \text{atan2}(0.60, 1.20) \approx 26.57^\circ$. The internal angle offset within the link triangle is $\psi = \text{atan2}(L_2 \sin\theta_2, L_1 + L_2 \cos\theta_2)$. For the elbow-down solution ($\theta_2 = +84.26^\circ$):

$\psi = \text{atan2}(0.80 \sin(84.26^\circ), 1.00 + 0.80(0.10)) = \text{atan2}(0.7960, 1.080) \approx 36.38^\circ$

$\theta_1 = \alpha - \psi = 26.57^\circ - 36.38^\circ = -9.81^\circ \; (-0.1712\text{ rad})$

7. Forward Substitution Verification

We verify consistency by plugging $(\theta_1 = -9.81^\circ, \theta_2 = +84.26^\circ)$ back into the forward kinematics equations. The sum of the angles is $\theta_1 + \theta_2 = 74.45^\circ$:

$x = 1.00 \cos(-9.81^\circ) + 0.80 \cos(74.45^\circ) = 0.9854 + 0.2145 = 1.200\text{ m}$

$y = 1.00 \sin(-9.81^\circ) + 0.80 \sin(74.45^\circ) = -0.1704 + 0.7707 = 0.600\text{ m}$

The analytical inverse kinematics calculation recovers the exact target coordinates.

8. Branch Selection Criteria

When multiple mathematical solutions exist, how does the robot pick one?

1. Obstacle Clearance: If a shelf sits on the table, the elbow-down posture collides, forcing the planner to choose elbow-up.
2. Motion Continuity: The robot selects the configuration that minimizes angular displacement from its current posture ($\min \|\Delta q\|$).
3. Joint Limits: One branch may violate mechanical stops.

9. Differential Kinematics and the Jacobian

Controlling smooth motion requires mapping joint velocities $\dot{q}$ to end-effector Cartesian velocities $\dot{x} = (\dot{x}, \dot{y})$ via the Jacobian matrix $J(q)$:

$\dot{x} = J(q) \dot{q} = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\ L_1 c_1 + L_2 c_{12} & L_2 c_{12} \end{bmatrix} \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix}$

To generate a commanded tip velocity, the controller solves $\dot{q} = J^{-1}(q) \dot{x}$.

10. Where It Breaks

The inverse velocity mapping relies on an invertible Jacobian. The determinant of the planar arm Jacobian is:

$\det(J) = L_1 L_2 \sin\theta_2$

When the arm straightens completely ($\theta_2 = 0^\circ$), $\sin\theta_2 = 0$, so $\det(J) = 0$. The Jacobian drops rank and cannot be inverted. Attempting to command outward Cartesian velocity at or near this singularity requires division by zero ($\dot{q} = J^{-1} \dot{x} \to \infty$). Motors saturate, draw peak current, and trip hardware overcurrent shutoffs.

The Quick Version

• Forward kinematics maps joint angles to a unique end-effector coordinate.
• Inverse kinematics solves Cartesian coordinates back to joint motor angles.
• Consecutive link projections sum trigonometrically along the kinematic chain.
• Reachability requires the radial distance to satisfy the triangle inequality.
• The Law of Cosines computes the elbow angle, yielding two algebraic roots.
• Elbow-up and elbow-down configurations provide two distinct postures for one target.
• Shoulder angles decompose into line-of-sight elevation minus the internal triangle angle.
• Forward substitution proves analytical inverse calculations recover exact target coordinates.
• Planners select branches to clear obstacles and minimize joint travel distance.
• Boundary singularities reduce Jacobian rank, requiring infinite motor speeds to move radially.