Skip to content
Tutorials Center
My learning

Topics

Lesson 38 of 46 ยท Robotics

Forward and Inverse Kinematics

Learn coordinate frames, rotation matrices and homogeneous transforms, then solve forward and inverse kinematics of a 2-link robot arm in Python with a workspace plot.

A robot arm is told what to do in the language of places: put the gripper at the cup. Its motors, however, understand joint angles: turn this joint by 40 degrees. Kinematics is the geometry that translates between the two, without worrying about forces or masses. It has two directions. Forward kinematics (FK) answers "if the joints are at these angles, where is the hand?" Inverse kinematics (IK) asks the opposite: "which joint angles put the hand here?"

Forward kinematics is a plain calculation with exactly one answer. Inverse kinematics is a search problem in disguise: there may be several answers, one, or none. This lesson builds the tools (coordinate frames, rotation matrices, homogeneous transforms), then solves a two-link planar arm in closed form and with an iterative numerical method. It follows the robotics basics lesson, and the results feed straight into path planning.

Coordinate frames

A frame is an origin plus a set of axes. The point "1 m ahead of the robot" has coordinates (1, 0) in the robot's frame, but in the world frame, the same physical point may be (2, 2) because the robot stands somewhere else and faces another way. Robotics is full of frames: the world, the robot base, each link of an arm, the hand, a camera. Almost every bug in a first robot program is a point used in the wrong frame.

To convert a point from one frame to another you need two pieces of information: how the second frame is rotated and how it is shifted.

Rotation matrices

In two dimensions, rotating a vector counter-clockwise by an angle theta is a multiplication by

TEXT
R(theta) = | cos(theta)  -sin(theta) |
           | sin(theta)   cos(theta) |

Rotation matrices have lovely properties: the columns are perpendicular unit vectors, the determinant is 1, and the inverse is simply the transpose. Rotating by a and then by b is the same as rotating by a + b, so in 2D the order does not matter. In three dimensions rotations are 3x3 matrices (one per axis: roll, pitch, yaw) and order does matter: turning 90 degrees about x and then about y is a different final orientation than y then x.

Homogeneous transforms

A rotation followed by a translation is not a matrix multiplication of a 2-vector, which is awkward. The standard trick is homogeneous coordinates: add a 1 to the end of every point, (x, y, 1), and pack rotation and translation into one 3x3 matrix (4x4 in 3D):

TEXT
T = | R   t |      p_world = T * p_robot
    | 0   1 |

Now a whole chain of frames collapses into a product of matrices, and the inverse transform moves points back. Try it: a robot at (2, 1) facing up sees a cup 1 m straight ahead.

Python Rotations and homogeneous transforms with NumPy

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The robot faces up, so "ahead" is the world's +y direction, and the cup lands at (2, 2). The last two lines show why order matters. In a product of matrices, the rightmost matrix acts first: A translates the origin to (1, 0) and then rotates that point about the world origin, ending at (0, 1); B rotates the origin (which stays put) and then translates it to (1, 0).

Read a chain like T_world_hand = T_world_base @ T_base_link1 @ T_link1_hand from the right: it takes a point in the hand frame, converts it to the link frame, then to the base frame, then to the world. Naming matrices T_to_from makes frame mistakes visible at a glance.

Take the simplest interesting arm: two rigid links of lengths L1 and L2 moving in a plane. The first joint (the "shoulder") sits at the origin with angle t1, measured from the x axis. The second joint (the "elbow") has angle t2, measured relative to the first link. Then:

TEXT
elbow = (L1 cos t1,  L1 sin t1)
hand  = (L1 cos t1 + L2 cos(t1 + t2),  L1 sin t1 + L2 sin(t1 + t2))

This is the transform chain in disguise: rotate by t1, move out along the link by L1, rotate by t2, move out by L2. Note the sum t1 + t2: the second link's absolute direction is the sum of the two joint angles, because each joint rotates everything beyond it. For longer arms you repeat the pattern once per joint, and robotics textbooks standardize it with Denavit-Hartenberg parameters, four numbers per joint that generate each link's matrix in a consistent way.

graph LR
    J["Joint angles<br/>t1, t2"] -- "forward kinematics<br/>(one answer)" --> H["Hand position<br/>x, y"]
    H -- "inverse kinematics<br/>(0, 1, 2 or more answers)" --> J

Forward kinematics is a function; inverse kinematics may have several solutions, one, or none.

Inverse kinematics in closed form

Given a target (x, y) for the hand, the shoulder, elbow and hand form a triangle with sides L1, L2 and the distance d = sqrt(x^2 + y^2) from the shoulder to the target. The law of cosines gives the elbow angle directly:

TEXT
cos(t2) = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2)

Three things follow immediately:

  1. If that value lies outside the interval from -1 to 1, no triangle exists and the target is unreachable: either too far (d > L1 + L2) or too close (d < |L1 - L2|).
  2. Otherwise t2 has two solutions, +arccos and -arccos: the elbow can bend either way, like a left or right arm. Both put the hand in the same place.
  3. With t2 known, the shoulder angle is the direction to the target minus a correction for the elbow's bend: t1 = atan2(y, x) - atan2(L2 sin t2, L1 + L2 cos t2).

Use atan2, never plain atan of a ratio: it keeps track of the quadrant and does not divide by zero.

Python Closed-form inverse kinematics with both solutions

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The first two lines are the two arm configurations for the same target. Feeding the angles back through forward returns exactly (1.000, 0.800), a round-trip check that is worth putting into every kinematics test. The target (2.0, 0.5) is farther than the arm's total length of 1.7, and (0.2, 0.1) is closer than |L1 - L2| = 0.3, so both are reported as unreachable. The last target is the edge case where the arm is fully stretched: cos t2 equals 1, the two solutions merge into one, and t1 and t2 are 0.

The workspace

The set of all positions the hand can reach is the arm's workspace. For our arm without joint limits it is the ring between two circles: radius L1 + L2 = 1.7 on the outside and |L1 - L2| = 0.3 on the inside. Real joints cannot turn freely (cables, collisions, motor limits), which carves pieces out of the ring. The plot below samples random joint angles within limits (shoulder from -90 to 180 degrees, elbow from -150 to 150 degrees), plots where the hand ends up, and draws the two arm configurations for one target.

Python Plot the workspace of a 2-link arm

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The farthest sampled reach is 1.700, right at the outer circle. The closest is 0.527, noticeably larger than the ideal inner radius of 0.3, because the elbow limit of 150 degrees stops the arm from folding back on itself fully. Joint limits matter: the closed-form formula would happily return a solution that the real elbow cannot reach, so a practical solver checks every solution against the limits and picks the one that is allowed (and, usually, the closest to the current pose).

Numerical inverse kinematics

Closed forms exist for common arm shapes, but a general 6- or 7-joint arm often needs a numerical method. The idea is the same as gradient descent. Near the current pose, a small change in joint angles dq changes the hand by dx = J(q) dq, where J is the Jacobian, the matrix of how each hand coordinate changes with each joint. Pick dq so that dx points toward the target, move a little, and repeat.

Inverting J fails near singularities, poses where the arm loses the ability to move in some direction (the fully stretched arm cannot extend further). The standard fix is damped least squares: dq = J^T (J J^T + lambda^2 I)^-1 * error. We also clamp the step size so one iteration never turns a joint more than 0.2 radians.

Python Jacobian-based inverse kinematics

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

Notice two things. First, the answer depends on the starting pose: starting near the first configuration converges to the angles (5.74, 83.85), and starting from the other side converges to (71.58, -83.85), the two closed-form solutions. Numerical IK finds a solution near where it begins, which is also what you usually want in motion: the arm should move smoothly, not flip to the other elbow. Second, for the unreachable target it does not crash; it stretches the arm out as far as it can and stops with a large leftover error, which your code must check for.

Always test the final error after an IK solve. Iterative solvers can end at a local minimum, run out of steps or chase an unreachable target. Treat "no solution" as a normal result, not an exceptional one.

Why this matters for planning

Kinematics tells you what one configuration looks like in the world. A planner needs more: it must decide which sequence of configurations moves the arm from here to there without hitting anything. The set of all joint-angle combinations is called the configuration space, and obstacles in the workspace turn into forbidden regions in it. That is the central idea of the next lesson.

Check your understanding

Quick check ยท 5 questions

of 5 answered You scored / 5 ยท perfect!

  1. 1. What is the difference between forward and inverse kinematics?

    Forward kinematics computes where the hand is from known joint angles (always one answer). Inverse kinematics goes the other way and may have none, one or several answers.
  2. 2. A two-link arm has L1 = 1.0 and L2 = 0.7. Which target is unreachable?

    The arm can reach at most L1 + L2 = 1.7 from the shoulder, so a point 2.0 away is out of reach. The other points are within the reachable ring between 0.3 and 1.7.
  3. 3. Why do robotics programs use homogeneous coordinates?

    Adding a 1 to each point lets a single matrix express rotate-then-translate, and composing frames is just multiplying matrices.
  4. 4. Why does a two-link arm usually have two inverse-kinematics solutions for a reachable target?

    The shoulder, elbow and hand form a triangle that can be mirrored across the line to the target, giving positive and negative elbow angles (except when the arm is fully stretched).
  5. 5. What problem does damped least squares address in numerical inverse kinematics?

    Near a singularity the plain inverse of the Jacobian explodes. Damping keeps the step bounded at the cost of slower convergence.

Summary

  • Forward kinematics maps joint angles to the hand pose; inverse kinematics maps a desired pose back to joint angles.
  • A frame is an origin with axes; rotation matrices and homogeneous transforms convert points between frames, and chains of frames become matrix products read right to left.
  • For a 2-link planar arm, the law of cosines gives cos t2; a value outside [-1, 1] means the target is unreachable, otherwise there are two elbow configurations.
  • The workspace is the ring between |L1 - L2| and L1 + L2, reduced further by joint limits.
  • Numerical IK uses the Jacobian with damping and step clamping, converges to a solution near the starting pose and must check for failure.
  • Obstacles turn into forbidden regions of joint space, which leads to planning in configuration space.

Next: Motion and Path Planning: Grids, Potential Fields and RRT.