Robot Kinematics

What is Kinematics?

Kinematics is the study of motion without considering forces. In robotics, it answers two questions: Forward Kinematics (FK) — given joint angles, where is the tool? And Inverse Kinematics (IK) — given a target position, what joint angles do I need?

FORWARD KINEMATICS

θ₁, θ₂, θ₃ → (x, y, z)

Easy to compute. You control joints directly. Used in simulation and teleoperation.

INVERSE KINEMATICS

(x, y, z) → θ₁, θ₂, θ₃

Harder — multiple solutions exist. Used in path planning and task-space control.

Forward Kinematics

For a 2-link planar arm with link lengths L₁ and L₂ and joint angles θ₁ and θ₂:

FK Equations

x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)

(* ST implementation *)
x_tcp := L1 * COS(theta1) + L2 * COS(theta1 + theta2);
y_tcp := L1 * SIN(theta1) + L2 * SIN(theta1 + theta2);

Each joint transforms the coordinate frame. The total tool position is the sum of all transformed link vectors — this extends to 6-axis robots via Denavit-Hartenberg (DH) parameters.

Inverse Kinematics

For the same 2-link arm, we can derive closed-form IK using the law of cosines:

IK Equations (2-link planar)

(* Given target (x, y), find θ1 and θ2 *)

D := (x² + y² - L1² - L2²) / (2·L1·L2)

θ2 = ATAN2(√(1-D²), D)          (* elbow-up solution *)
   OR
θ2 = ATAN2(-√(1-D²), D)         (* elbow-down solution *)

θ1 = ATAN2(y, x) - ATAN2(L2·sin(θ2), L1 + L2·cos(θ2))

For 6-axis robots, closed-form solutions exist only for specific geometries (spherical wrist). Otherwise, iterative numerical methods (Jacobian inverse, Newton-Raphson) are used.

🤖 Interactive Simulator

Switch between FK mode (drag sliders to set joint angles) and IK mode (click the canvas to set the target position).

JOINT ANGLES

θ₁ (Joint 1)45°

θ₂ (Joint 2)-60°

L₁ (Link 1)100

L₂ (Link 2)80

Code Example

Structured Text — IK Function Block

FUNCTION_BLOCK FB_IK_2Link
VAR_INPUT
  x_target : REAL;
  y_target : REAL;
  L1       : REAL := 500.0;  (* mm *)
  L2       : REAL := 400.0;  (* mm *)
  elbow_up : BOOL := TRUE;
END_VAR
VAR_OUTPUT
  theta1 : REAL;
  theta2 : REAL;
  reachable : BOOL;
END_VAR
VAR
  D, sq : REAL;
END_VAR

D := (x_target*x_target + y_target*y_target - L1*L1 - L2*L2) / (2.0*L1*L2);

IF ABS(D) > 1.0 THEN
  reachable := FALSE;
  RETURN;
END_IF;

reachable := TRUE;
sq := SQRT(1.0 - D*D);

IF elbow_up THEN
  theta2 := ATAN2( sq, D);
ELSE
  theta2 := ATAN2(-sq, D);
END_IF;

theta1 := ATAN2(y_target, x_target)
        - ATAN2(L2*SIN(theta2), L1 + L2*COS(theta2));

END_FUNCTION_BLOCK

🎯 Quick Quiz

Q1. Forward kinematics computes:

Q2. For a 2-link arm, how many IK solutions exist for a reachable point?

Q3. What does "workspace singularity" mean?