Math for Robotics

A robot runs on a handful of maths tools: frames, rotations, Jacobians, filters, feedback and least squares. Here you meet each one by reaching a two-link arm toward a cup.

Learn the five questions a robot asks every moment, and the maths tool that answers each. Then see where to go deeper.

Drag the cup, or use the two sliders. The arm bends to reach it. Switch on Frames to see the coordinate axes the robot keeps, and Speed ellipse to see where the hand can move fastest. Then we build every piece from zero.

You need high-school algebra and a little trigonometry (sine and cosine of an angle). We build the rest from zero.

Built on Lynch and Park, Modern Robotics (Cambridge University Press, 2017), and Thrun, Burgard and Fox, Probabilistic Robotics (MIT Press, 2005). All arm sizes, noise levels and gains here are illustrative toy numbers.

Reach for the cup

0.30
0.55

Toy robot: link lengths L1 = 0.40 m and L2 = 0.30 m, a camera 0.06 m ahead of and 0.04 m to the side of the hand, drawn from above. The speed ellipse is drawn at one third of true size. Real arms have other sizes; the maths is the same.

Chapter 0

One Reach, Five Questions

Follow one small robot as it reaches for a cup, and meet the five questions that need maths

A small robot sits at the edge of a table. It has one arm: two stiff bars joined by a hinge, with the first bar hinged to the base. A camera sits on the end of the arm, near the hand. On the table stands a cup. Your job is to make the arm pick up the cup.

You might think this is a job for code alone. It is not. Before one motor turns, the robot needs numbers. It must know where the cup is, how its own arm is bent, and how to move. Each of those needs a small piece of maths.

This lesson shows you those pieces. We use one robot for all of them, so the pieces fit together like parts of one machine. The robot has two links (the rigid bars) joined at two joints (the hinges). The first link is 0.40 m long. The second is 0.30 m long. These are toy sizes, chosen to keep the arithmetic clean.

The five questions

Watch the robot for one reach. It asks the same five questions, in this order, on every reach it ever makes.

Where is the cup? The camera sees the cup as a spot in its own picture. The robot needs the spot as a position it can use, measured from the base. That is a question about coordinates (numbers that name a place), and the answer comes from vectors and transforms.

How is the arm posed? The motors report two angles, one per joint. The robot needs the position of its hand. Going from joint angles to hand position is called forward kinematics.

How fast does the hand move when a joint moves? If joint one turns a little, the hand slides sideways. If joint two turns, it slides a different way. The rule that links joint speeds to hand speed is the Jacobian.

How sure am I? Wheels slip and cameras blur. Every number the robot holds is a little wrong. The tool for handling doubt is probability, and its robot form is the Kalman filter.

How do I get there? The robot now knows where the cup is and where the hand is. It must find joint angles that put the hand on the cup, then make the motors hit those angles. The first part is inverse kinematics. The second is feedback control. Both lean on one more tool: finding the best answer by optimization.

Ask the robot a question

The robot is in its start pose. Press each question. The drawing shows what the robot needs to know, and the read-out names the tool and the chapter that answers it.

Toy robot, drawn from above. The start pose has joint angles 10 degrees and 85 degrees. The cup is 0.30 m right of and 0.55 m ahead of the base. The four hand positions in the route view are the exact steps of the Newton loop in Chapter 6.

Why these five, and why in this order

Each question feeds the next. Without a place for the cup, there is no target. Without the hand position, you cannot compare hand and target. Without the Jacobian, you do not know which way to turn the joints. Without a measure of doubt, you trust a bad number. Without feedback, the arm drifts off course.

Nothing here is exotic. The maths is mostly the maths you already know: angles, sine and cosine, a little algebra, and a slope. What is new is how the pieces connect. A robot textbook uses hundreds of pages to connect them. This lesson uses ten short chapters, and gives you a map.

What this lesson does, and does not do

Each chapter teaches one tool from zero. You see the idea in plain words, then the formula, then a worked example with every number. Then you run it on the toy arm. At the end of each chapter a short line points to a deeper lesson on this site that already goes further.

This lesson does not replace those deeper lessons. It does not derive the full three-dimensional theory, and it does not prove the filters are optimal. It gives you the first real step on each road, so that when you open a deeper lesson, you know why it exists.

A note on honesty. Every length, noise level and gain in this lesson is a toy number picked to teach. A real arm has different sizes, and a real camera has different noise. The formulas stay the same. Only the numbers change.

The road

  1. Vectors and frames: a place is a list of numbers measured from a chosen origin.
  2. Rotations: turning a frame, and why the order of turns matters.
  3. Transforms: chaining camera, wrist and base into one answer.
  4. Forward kinematics: from joint angles to hand position.
  5. The Jacobian: from joint speeds to hand speed, and where it fails.
  6. Inverse kinematics: from a target to joint angles, by formula and by loop.
  7. Uncertainty: noise, the Gaussian, and fusing two doubtful readings.
  8. Dynamics and feedback: motion as an equation, and a controller that steers it.
  9. Least squares: the best answer when the data disagree.
  10. Field guide: one table, one study order, and the lessons to take next.
The robot knows its two joint angles and wants the position of its hand. Which question is that, and which tool answers it?

Chapter 1

Vectors and Frames

Name a place with numbers, and see why the same cup has different numbers for different observers

Ask the camera where the cup is, and it says "ahead, a bit to the left." Ask the base, and it says "to the right and far in front." Both are correct. They disagree because each one measures from its own spot, facing its own way.

A robot hears many such answers at once. Before it can combine them, it needs one clear way to write down a place. That way is a vector and a frame.

A place is a list of numbers

A vector is a list of numbers that says how far to go in each direction. Think of walking directions in a city grid: "3 blocks east, then 5 blocks north." The list (3, 5) is a vector. In a drawing of the table from above, our cup sits 0.30 m to the right and 0.55 m forward of the base. We write it as p = (0.30, 0.55). In code this is an array of shape (2,).

The list means nothing alone. "3 blocks east" needs a starting corner and a meaning for "east." Those two choices make a frame: an origin (the starting point) and axes (the directions the numbers count along). Our base frame has its origin at the base of the arm. Its x-axis points right. Its y-axis points forward.

The numbers in a vector are its coordinates. They depend on the frame. Move the origin or turn the axes, and the coordinates change. The cup does not move. Only the way we write it down changes.

Lengths and the dot product

How far is the cup from the base? The two coordinates are the legs of a right triangle. The distance is the long side. Pythagoras gives its length: the square root of 0.30 squared plus 0.55 squared. That is the square root of 0.0900 plus 0.3025, which is 0.6265 m. We write this length as |p|.

Now a second tool. The dot product of two vectors multiplies their matching coordinates and adds the results. It also equals the product of the two lengths and the cosine of the angle between them.

a · b
The dot product of vectors a and b. It is one number, not a vector.
axbx + ayby
The recipe in coordinates: multiply the x parts, multiply the y parts, add.
|a| |b| cos θ
The recipe in geometry: the two lengths times the cosine of the angle θ between them.

Here is the picture to keep. If b has length 1, then a · b is the length of the shadow of a on the line through b. Shine a light straight down onto that line, and the shadow is the dot product. When a and b are at right angles, the shadow is zero. The dot product is zero.

Changing frames with the dot product

Here is the payoff. Suppose a second frame sits at the same origin but is turned 30 degrees. Its x-axis is a vector of length 1 with coordinates (cos 30°, sin 30°) = (0.866, 0.500). Call it u. Its y-axis is v = (−0.500, 0.866). The coordinates of the cup in the turned frame are its shadows on these two axes.

p · u = 0.30 × 0.866 + 0.55 × 0.500 = 0.2598 + 0.2750 = 0.535

p · v = 0.30 × (−0.500) + 0.55 × 0.866 = −0.1500 + 0.4763 = 0.326

So the same cup is (0.535, 0.326) in the turned frame. Check the length: 0.535 squared plus 0.326 squared is 0.2862 plus 0.1063, and the square root of 0.3925 is 0.6265 m. The distance did not change. Turning the axes cannot change how far away the cup is.

One cup, two frames

The blue axes are the base frame. The purple axes are a second frame that you can tilt. Move the cup with the two sliders, or drag it with a mouse. Then drag the tilt slider. Watch the dashed shadows: they are the dot products.

0.30
0.55
30°

The cup starts at (0.30, 0.55) m, the lesson's running example. The tilted frame shares the base origin, so only the axes differ. Distances are computed in both frames to show that they match.

The same thing in code

This is the whole idea in four lines of NumPy. The dot product @ does the shadow, one axis at a time.

pythonimport numpy as np

p = np.array([0.30, 0.55])                 # shape (2,), metres, in the base frame
a = np.deg2rad(30)
u = np.array([np.cos(a), np.sin(a)])           # tilted x-axis, a unit vector
v = np.array([-np.sin(a), np.cos(a)])          # tilted y-axis, also length 1
p_tilt = np.array([p @ u, p @ v])              # shape (2,) -> [0.5348, 0.3263]
print(np.linalg.norm(p), np.linalg.norm(p_tilt))   # 0.6265 and 0.6265

Here is why a robot cares. The camera reports the cup in the camera frame. The planner wants it in the base frame. Every hop between frames is a change like this one. Chapter 2 adds the missing piece: how to write the turn as a matrix. Chapter 3 adds the move of the origin.

Go deeper: Frames and Transforms builds frames for whole robots, and Maths for ML covers vectors and dot products at length.

You turn a frame's axes 30 degrees but keep its origin. What happens to the distance from the origin to the cup, as computed from the new coordinates?

Chapter 2

Rotations

Turn a frame with a small grid of numbers, and learn why turning in a different order gives a different result

The robot's hand points one way. The camera is bolted to the hand and points the same way. The cup is off to one side. To aim the hand, the robot must say how far to turn. It needs a precise way to write "turn by this much."

A rotation turns something around a fixed point without stretching it. Lengths stay the same. Only the direction changes. This chapter writes a rotation as a small grid of numbers called a matrix.

Derive the rotation matrix from a circle

Start with the unit circle: all points exactly 1 m from the origin. Take the point (1, 0) on the x-axis and swing it counterclockwise by an angle θ. Where does it land? By the definition of sine and cosine, it lands at (cos θ, sin θ). So after the turn, the x-axis arrow is (cos θ, sin θ).

Now take the y-axis arrow (0, 1). It also swings by θ. It was 90 degrees ahead of the x-axis arrow, and it still is. So it lands at (−sin θ, cos θ). Check at θ = 90°: the x arrow lands at (0, 1), and the y arrow lands at (−1, 0). That is correct.

Any vector is a mix of these two arrows: p = px (1, 0) + py (0, 1). A rotation turns both arrows and keeps the mix. So the turned vector is px times the turned x arrow, plus py times the turned y arrow. Write the two turned arrows as the columns of a matrix, and you have the rotation matrix.

p′
The vector after the turn, written in the same frame as before.
R(θ)
The rotation matrix for the angle θ. A 2 by 2 grid of numbers.
p
The vector before the turn, a column of two numbers.
cos θ, sin θ
Row one is (cos θ, −sin θ). Row two is (sin θ, cos θ). The columns are the turned x and y arrows.

To multiply a matrix by a vector, take the dot product of each row with the vector. That is the same dot product as in Chapter 1. Try it on the cup with θ = 30°, where cos is 0.866 and sin is 0.500.

x′ = 0.866 × 0.30 − 0.500 × 0.55 = 0.2598 − 0.2750 = −0.015

y′ = 0.500 × 0.30 + 0.866 × 0.55 = 0.1500 + 0.4763 = 0.626

The cup swings from (0.30, 0.55) to (−0.015, 0.626). Its length is still 0.6265 m. Drag the angle and watch the point ride the circle.

Swing the arrows around the circle

The warm arrow is the x axis and the teal arrow is the y axis. Drag the angle. Their tips are the columns of the matrix below.

30°

The cup at (0.30, 0.55) m is drawn at its rotated place. The circle is the set of points 1 m from the origin, drawn smaller than the table to fit.

Two facts that save you work

First, undoing a turn is easy. To undo a turn of θ, turn by −θ. Because cos(−θ) = cos θ and sin(−θ) = −sin θ, this swaps the signs of the sine terms. That is the transpose of the matrix: flip it over its main diagonal. So the inverse of R is RT. No hard algebra is needed.

Second, this explains Chapter 1. The coordinates of the cup in a frame turned by 30 degrees were (0.535, 0.326). Compute R(30°)T p, and you get the same numbers. Describing the cup in a turned frame is the same as turning the cup the other way.

In two dimensions, the order of turns does not matter. Turn 20 degrees, then 40, and you get 60. Everything lives on one circle. In three dimensions that is no longer true, and it causes real trouble.

Order matters in 3D

Hold your hand flat, palm down, fingers pointing away from you. Turn it 90 degrees about the line of your fingers (a roll). Then turn it 90 degrees about the vertical (a yaw). Note where your thumb points. Now start again and do the same two turns in the opposite order. Your thumb ends up somewhere else.

The matrices agree. Let Rx be a 90 degree turn about the x-axis, and Rz a 90 degree turn about the z-axis. Take the arrow a = (1, 0, 0). Rx sends (x, y, z) to (x, −z, y). Rz sends (x, y, z) to (−y, x, z). Turn about x first, then z:

Rz Rx a: (1, 0, 0) → (1, 0, 0) → (0, 1, 0)

Rx Rz a: (1, 0, 0) → (0, 1, 0) → (0, 0, 1)

Two different answers. Matrix products are not commutative, which means AB is not the same as BA. Try it below.

Same two turns, two orders

The three arrows are a tiny frame fixed to a hand. Choose an order, then drag the slider from 0 to 2. The first half of the slider does the first turn. The second half does the second.

2.00

Turns are about the fixed world axes, 90 degrees each. The picture is a simple tilted view of a right-handed frame: x to the right, y into the page, z up.

An honest note on Euler angles and quaternions

In three dimensions, a rotation matrix has nine numbers. Many people prefer fewer. Euler angles use three: roll, pitch and yaw, one turn about each axis in a set order. They are easy to read and to type. They also fail. At a pitch of 90 degrees, two of the axes line up, and a change in roll looks the same as a change in yaw. This is gimbal lock: one degree of freedom disappears, and the angles jump.

A quaternion uses four numbers of total length 1. It has no gimbal lock, it is cheap to multiply, and it blends smoothly between two rotations. It is hard to read by eye. Most robot software stores rotations as quaternions or matrices, and shows Euler angles to humans. This lesson stays in the plane, where one angle is enough. The deeper lessons below treat the full 3D case.

Go deeper: Rotations and Transforms covers 3D rotations, Euler angles and quaternions in detail. Lie Groups shows how rotations form a smooth family you can differentiate.

You swap the order of two 90 degree turns about different axes in 3D. What do you get?

Chapter 3

Transforms and Chaining Frames

Pack a turn and a move into one matrix, then chain the camera, wrist and base frames into a single answer

The wrist camera sees the cup 0.127 m straight ahead and 0.012 m to the left. That is a good measurement, but it is useless to the planner. The planner thinks in base coordinates. Between the two sit the camera mount, the wrist, the elbow and the shoulder.

The robot needs one tool that crosses a frame boundary, and that chains cleanly when there are many boundaries. That tool is the homogeneous transform.

A frame change has two parts

To move a point from the camera frame to the wrist frame, do two things. First turn it, because the axes point differently. Second shift it, because the origins sit in different places. In symbols, with R for the turn and t for the shift:

pB
The point written in the parent frame, here the base frame.
R
The rotation from Chapter 2. It turns the child frame's axes to match the parent's.
pC
The same point written in the child frame, here the camera frame.
t
The position of the child frame's origin, written in the parent frame.

This works, but chaining is clumsy. Two hops give R1(R2 p + t2) + t1, and three hops get worse. We want a single multiplication per hop.

The trick: add a 1

Append a 1 to the end of every point. The point (0.127, 0.012) becomes (0.127, 0.012, 1). This extra 1 is the key to homogeneous coordinates. Now build a 3 by 3 matrix: put R in the top-left, t in the top-right, and the row (0, 0, 1) at the bottom.

T = [ R t ] = [ cos φ −sin φ tx ] [ 0 1 ] [ sin φ cos φ ty ] [ 0 0 1 ]

Multiply T by the extended point. The top two rows give R p + t. The bottom row gives 1. So one matrix multiplication does both the turn and the shift. Modern Robotics (Lynch and Park, 2017) calls these matrices SE(2). The name means the set of rigid motions of the plane.

Chaining by multiplying

Now chaining is easy. Name each transform by its two frames: TBW takes wrist coordinates to base coordinates, and TWC takes camera coordinates to wrist coordinates. Chain them by multiplying. The inner letters cancel like fractions: B W times W C leaves B C.

pB = TBW · TWC · pC

Read the product from the right. The point starts in camera coordinates. The nearest matrix moves it to wrist coordinates. The next moves it to base coordinates. Here are the numbers for our start pose, where the joints are at 10 and 85 degrees.

The hand is at (0.368, 0.368) m and points at 10 + 85 = 95 degrees. Its transform is:

TBW = [ −0.0872 −0.9962 0.3678 ] cos 95° = −0.0872 [ 0.9962 −0.0872 0.3683 ] sin 95° = 0.9962 [ 0 0 1 ]

The camera sits 0.06 m ahead of the hand and 0.04 m to its side, with the same heading. So TWC is a pure shift by (0.06, 0.04), with R equal to the identity (no turn). Now follow the cup, which the camera sees at pC = (0.1269, 0.0117):

pW = TWC pC = (0.1269 + 0.06, 0.0117 + 0.04) = (0.1869, 0.0517)

xB = −0.0872 × 0.1869 − 0.9962 × 0.0517 + 0.3678 = 0.300

yB = 0.9962 × 0.1869 − 0.0872 × 0.0517 + 0.3683 = 0.550

The cup is at (0.300, 0.550) m in the base frame. This is the number the planner needed, and the cup was never given in base coordinates. The chain built it from three small pieces.

Chain the frames

Bend the two joints. The camera's view of the cup changes, but the chain always returns the same base coordinates. The matrices update live.

10°
85°

Toy robot as in the hero. The camera is fixed to the hand at (0.06, 0.04) m with the same heading. The cup stays at (0.30, 0.55) m. The wrong order is TWC TBW pC.

The inverse, and the failure when you get the order wrong

Sometimes you need the opposite: base to camera. Undo the shift first, then undo the turn. The result has a neat form, because the inverse of R is RT.

T−1 = [ RT −RT t ] [ 0 1 ]

For the camera: subtract the camera origin (0.3227, 0.4246) from the cup, giving (−0.0227, 0.1254), then apply R(95°)T. You get (0.1269, 0.0117) again. The loop closes.

Now the failure. Suppose a tired engineer writes the product in the wrong order, TWC TBW pC. The code runs. No error appears. The symptom is that the hand lands 0.106 m from the cup, mostly to the right, and knocks it over. The measurement: the computed cup position is (0.405, 0.534) instead of (0.300, 0.550). The fix: read the frame letters. Inner letters must match, as in B W times W C. If they do not touch, the order is wrong. Press the red button above to see this.

pythonimport numpy as np

def T(theta, tx, ty):                          # a 3x3 planar transform
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, tx], [s, c, ty], [0, 0, 1.0]])

T_BW = T(np.deg2rad(95), 0.3678, 0.3683)    # wrist in base
T_WC = T(0.0, 0.06, 0.04)                   # camera in wrist
p_C  = np.array([0.1269, 0.0117, 1.0])         # shape (3,): x, y, then the 1

p_B = T_BW @ T_WC @ p_C                         # [0.3000, 0.5500, 1.0]
T_CB = np.linalg.inv(T_BW @ T_WC)               # base-to-camera
print(T_CB @ p_B)                                # back to [0.1269, 0.0117, 1.0]

Go deeper: Rigid-Body Transforms (SE(2) and SE(3)) builds the 2D and 3D cases step by step. Frames and Transforms shows how a whole robot keeps its frame tree.

You have T_BW (wrist in base) and T_WC (camera in wrist). Which product turns a point seen by the camera into base coordinates?

Chapter 4

Forward Kinematics

Turn two joint angles into a hand position, and map the ring of places the hand can reach

The two motors report their angles: 10 degrees at the shoulder, 85 degrees at the elbow. Where is the hand? You cannot see it. The robot has to work it out, and it has to do so thousands of times a second.

The rule that turns joint angles into the position of the hand is forward kinematics. The word "kinematics" means the study of motion without asking what pushes it. We care only about geometry here, not forces.

Build the formula from two triangles

Define the angles. The shoulder angle θ1 is measured from the base x-axis to the first link. The elbow angle θ2 is measured from the first link to the second. That makes it a relative angle: it says how much the second link bends away from the first.

The elbow is the end of the first link. Its position is a length-L1 step in the direction θ1: (L1 cos θ1, L1 sin θ1). That is the same unit-circle fact as Chapter 2.

The hand is the end of the second link, which starts at the elbow. Its heading is not θ2. It is the first link's heading plus the bend: θ1 + θ2. Walking along it for L2 gives the second step. Add the two steps.

x, y
The hand position in the base frame, in metres.
L1 cos θ1, L1 sin θ1
The first step: from the base to the elbow along link 1.
L2 cos(θ1 + θ2)
The x part of the second step. Link 2 points at the sum of the two angles.
L1 sin θ1
The y part of the first step.
L2 sin(θ1 + θ2)
The y part of the second step.

A worked example

Take the start pose, θ1 = 10° and θ2 = 85°, with L1 = 0.40 m and L2 = 0.30 m. The second link points at 10 + 85 = 95 degrees.

Elbow = (0.40 cos 10°, 0.40 sin 10°) = (0.3939, 0.0695)

Second step = (0.30 cos 95°, 0.30 sin 95°) = (−0.0261, 0.2989)

Hand = (0.3939 − 0.0261, 0.0695 + 0.2989) = (0.3678, 0.3683) m

That is the point from Chapter 3. The hand is 0.5205 m from the base. The second step has a negative x part because the forearm points a little past straight up.

There is a second way to see the same answer. Each joint is a turn of θ, then a walk of L along the new x-axis. Chain the pieces as in Chapter 3: T = Rot(θ1) Shift(L1) Rot(θ2) Shift(L2). Multiply them out, and the last column is the formula above. The formula is the transform chain, written out for two joints. A 6-joint arm just has a longer chain.

The workspace: where the hand can go

Not every point is reachable. The farthest the hand can be is when the arm is straight: L1 + L2 = 0.70 m. The nearest is when the arm is folded back on itself: |L1 − L2| = 0.10 m. Between those, the distance depends on the elbow bend alone. By the law of cosines, r2 = L12 + L22 + 2 L1 L2 cos θ2. At 85 degrees, r2 = 0.16 + 0.09 + 0.24 × 0.0872 = 0.2709, so r = 0.5205 m. That matches.

The set of reachable points is a flat ring, called the workspace, between 0.10 m and 0.70 m from the base. The shoulder angle swings the whole ring around. In this toy, both joints turn freely, so the ring is full. Real joints have stops, and the ring has bites taken out.

Bend the arm, find the hand

Drag the two joints. The warm trail records where the hand has been. The shaded ring is everywhere the hand can ever go.

10°
85°

Toy arm: L1 = 0.40 m, L2 = 0.30 m, joints free to turn through 360 degrees. The ring runs from |L1 minus L2| = 0.10 m to L1 + L2 = 0.70 m.

Failure mode: degrees fed to a function that wants radians

Here is the classic bug. NumPy's cos and sin expect radians, a way of measuring angles where a half turn is π = 3.14159. A programmer types np.cos(10) meaning ten degrees. NumPy reads ten radians, which is more than one and a half turns.

The symptom: the hand drifts to the wrong side of the robot, and a reach pushes the cup off the table. The measurement: the computed hand position is (−0.117, −0.013) m, but the true hand is at (0.368, 0.368) m. The error is 0.616 m. The fix is one line: convert with np.deg2rad at the boundary, and keep every angle in radians inside the code.

pythonimport numpy as np

L1, L2 = 0.40, 0.30                               # metres (toy sizes)

def fk(theta1, theta2):                             # radians in, metres out
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    return np.array([x, y])                         # shape (2,)

print(fk(np.deg2rad(10), np.deg2rad(85)))       # [0.3678, 0.3683]  correct
print(fk(10, 85))                                # [-0.1166, -0.0126]  the radians bug

Notice what forward kinematics gives you: one answer for each pair of angles. The reverse is not true. Many angle pairs can reach the same hand position. Chapter 6 deals with that.

Go deeper: Inverse Kinematics starts from forward kinematics for longer arms. Rigid-Body Transforms gives the chain-of-transforms view.

The elbow bends by θ2 = 85 degrees relative to link 1, and the shoulder is at θ1 = 10 degrees. In what direction does link 2 point, measured from the base x-axis?

Chapter 5

Derivatives and the Jacobian

Learn how a small joint nudge becomes hand motion, and see why a straight arm gets stuck

The camera shows the cup 5 cm to the right of the hand. The arm must slide the hand 5 cm. Which joint should it turn, and by how much? Forward kinematics tells you where the hand is. It does not tell you which way a turn will push it.

For that you need the speed of the hand for a given speed of the joints. The rule is the Jacobian. It is built from derivatives, so we start there.

A derivative is a slope

Drive a car. Your position changes with time, and your speed is how fast. Speed is a derivative: the change in one quantity per small change in another. On a graph of position against time, speed is the slope of the line.

For the arm, a joint angle plays the part of time, and the hand position plays the part of distance. If turning joint 1 by 1 degree moves the hand 6 mm, the slope is 6 mm per degree. A steeper slope means a small turn makes a big move.

The hand has two coordinates and two joints, so we have four slopes. Each one answers: if I nudge only this joint, how much does this coordinate change? A slope measured with the other variables held fixed is a partial derivative.

The four slopes in one matrix

Take the hand formula from Chapter 4. Differentiate each coordinate with respect to each angle. The rules you need are that the slope of sine is cosine, and the slope of cosine is minus sine. For a nudge to θ1, both terms move, because θ1 appears in both. For a nudge to θ2, only the second link moves.

J
The Jacobian: a 2 by 2 grid of slopes. Column 1 is what joint 1 does to the hand. Column 2 is what joint 2 does.
θ12
Short for θ1 + θ2, the heading of link 2.
v
The velocity of the hand: two numbers, in metres per second.
J ω
The matrix times the joint speeds. This gives the hand velocity.
ω
The joint speeds (ω1, ω2), in radians per second.

Look at column 1. It equals (−y, x), the hand's own position turned by 90 degrees. That makes sense. Turning joint 1 spins the whole arm about the base, so the hand moves at a right angle to the line from the base.

A worked example

At the start pose (10°, 85°), the hand is at (0.3678, 0.3683) and the second link's heading has sin 95° = 0.9962 and cos 95° = −0.0872. So:

J = [ −0.3683 −0.2989 ] column 1 = (−y, x) [ 0.3678 −0.0261 ] column 2 = L2 (−sin θ12, cos θ12)

Let joint 1 turn at ω1 = 0.50 rad/s and joint 2 at ω2 = −0.20 rad/s. Multiply:

vx = −0.3683 × 0.50 + (−0.2989) × (−0.20) = −0.1842 + 0.0598 = −0.124 m/s

vy = 0.3678 × 0.50 + (−0.0261) × (−0.20) = 0.1839 + 0.0052 = 0.189 m/s

The hand moves up and to the left at 0.226 m/s. As a check, nudge joint 1 by half a degree and recompute the forward kinematics. The hand moves (−0.00323, 0.00320) m. The Jacobian predicts (−0.00321, 0.00321). They agree because the nudge is small. The Jacobian is the best straight-line approximation to the hand's motion, near this pose.

The speed ellipse

Now imagine all the joint speeds with length 1, the circle of "unit effort." Each one gives a hand velocity. Together they trace an ellipse (a stretched circle). The long axis shows the direction in which the hand can move fastest. The short axis is the weakest direction. The hero shows this as the yellow ellipse.

Turn the joints, move the hand

Warm arrows show each joint's own push on the hand. The teal arrow is their sum. Drag the joint speeds. Then drag joint 2 toward 0 and watch the ellipse flatten.

85°
0.50
−0.20

Joint 1 is fixed at 10 degrees. Speeds are in rad/s. The ellipse is drawn at one third of true size to fit: its two half-widths are the hand speeds, in m/s, along the longest and shortest directions for one unit of joint speed. The determinant is L1 L2 sin(theta2).

Singularity: the straight arm

The determinant of a 2 by 2 matrix tells you whether it is invertible, which means whether you can undo it. For a grid with rows (a, b) and (c, d), the determinant is a d minus b c. If it is zero, two columns point the same way, and the grid squashes the plane onto a line. You cannot undo a squash.

Work it out for J. Here a = −L1 sin θ1 − L2 sin θ12, b = −L2 sin θ12, c = L1 cos θ1 + L2 cos θ12, and d = L2 cos θ12. The L22 terms cancel. What is left is L1 L2 (sin θ12 cos θ1 − cos θ12 sin θ1). This is the sine of a difference, sin(θ12 − θ1) = sin θ2. One clean result: det J = L1 L2 sin θ2.

At θ2 = 85 degrees, det J = 0.12 × 0.9962 = 0.1195. Healthy. At θ2 = 0, the determinant is zero. The arm is stretched straight. Both columns of J now point the same way, at a right angle to the arm. Nothing you do with the joints can move the hand along the arm. This is a kinematic singularity: a pose where the hand loses a direction of motion. Modern Robotics, section 5.3, treats singularities for general arms.

The trouble starts before you reach it. Take the pose (40°, 5°), nearly straight, and ask the hand to move outward along the arm at 0.1 m/s. The symptom: the joints spin wildly and the arm jerks. The measurement: solving for joint speeds gives 7.27 rad/s, against 0.46 rad/s for the same hand move at an elbow bend of 85 degrees. That is 16 times more. The fix: stay away from straight poses when you plan, or use the damped step of Chapter 6.

pythonimport numpy as np

def jacobian(t1, t2, L1=0.40, L2=0.30):
    s1, c1 = np.sin(t1), np.cos(t1)
    s12, c12 = np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1*s1 - L2*s12, -L2*s12],
                     [ L1*c1 + L2*c12,  L2*c12]])        # shape (2, 2)

J = jacobian(np.deg2rad(10), np.deg2rad(85))
v = J @ np.array([0.5, -0.2])                         # [-0.1244, 0.1891] m/s
print(np.linalg.det(J))                             # 0.1195 = 0.4*0.3*sin(85 deg)
print(np.linalg.svd(J)[1])                          # the two ellipse semi-axes

Go deeper: Jacobians and Linearization builds the Jacobian for any function. Eigenvalues explains the ellipse axes.

The determinant of the 2-link Jacobian is L1 L2 sin(theta2). What happens to the hand when theta2 is near 0, and why does it matter?

Chapter 6

Inverse Kinematics

Find the joint angles that put the hand on the cup, first by a formula and then by a loop that always works

The cup stands at (0.30, 0.55) m. The motors do not take positions. They take angles. So the robot must run forward kinematics backward: given a place, find the angles that reach it.

This job is called inverse kinematics, or IK. It is harder than the forward direction, because the answer may be none, one, two, or a whole family.

A formula for the two-link arm

Draw the triangle with the base, the elbow and the cup at its corners. Its sides are known: L1, L2, and the distance r from base to cup. The elbow angle is fixed by that triangle alone. The law of cosines gives it:

cos θ2
The cosine of the elbow bend. Take the arccos to get the angle, with either sign.
x2 + y2 − L12 − L22
The squared distance to the cup, minus the squared link lengths.
2 L1 L2
A fixed number for this arm: 0.24 here.
θ1
The shoulder angle: aim at the cup, then correct for the elbow's bend.
atan2(y, x)
The angle of the line from the base to the cup, measured from the x-axis. It handles all four quadrants.
atan2(…)
The extra angle that link 1 sits away from that line, because link 2 pulls the hand to one side.

Work it for the cup. The squared distance is 0.30 squared plus 0.55 squared, which is 0.0900 + 0.3025 = 0.3925. Subtract 0.16 and 0.09:

cos θ2 = (0.3925 − 0.16 − 0.09) / 0.24 = 0.1425 / 0.24 = 0.594, so θ2 = ±53.58°

There are two answers: bend the elbow one way, or bend it the other. The two bends give mirror-image arms that reach the same cup. Robot books call these elbow-up and elbow-down when the arm is seen from the side. From above, we call them elbow right and elbow left.

For θ2 = +53.58°: atan2(0.55, 0.30) = 61.39°, and atan2(0.3 sin 53.58°, 0.4 + 0.3 × 0.594) = atan2(0.2414, 0.5781) = 22.66°. So θ1 = 61.39 − 22.66 = 38.73°. For the other sign, the correction flips: θ1 = 61.39 + 22.66 = 84.05° with θ2 = −53.58°. Run forward kinematics on either pair and you land on (0.300, 0.550).

Two more facts. If the right side of the formula for cos θ2 is bigger than 1 or smaller than −1, there is no angle. The cup is out of the workspace of Chapter 4, and the arm cannot reach it. And which of the two answers should the robot take? Usually the one nearest the current pose, so the arm does not swing across the table, or the one that clears an obstacle.

When there is no formula: the Newton loop

For most arms, no formula exists. The fix is a loop that creeps toward the answer. It rests on one idea: over a small move, the arm is nearly a straight-line machine, and Chapter 5 told us the line.

Here is the loop. Let e be the error, the gap between the cup and the hand. Treat the arm as exactly linear, and ask: which joint change would close the gap in one go? Solve J Δθ = e. That is two equations in two unknowns, the two joint changes, and any pair of equations like it has a short solution. Take that step. The hand will not land exactly, because the arm is only nearly linear. But it will land closer. Repeat. This is the Newton-Raphson method; Modern Robotics, section 6.2, derives it from a first-order Taylor expansion.

e
The error: a vector from the hand to the cup.
xcup − f(θ)
The cup position minus the hand position from forward kinematics.
Δθ
The change in both joint angles for this step.
J−1
The inverse of the Jacobian at the current pose. It exists when the determinant is not zero.
θ ← θ + Δθ
Update the angles, then recompute everything.

Start at the start pose (10°, 85°), with the hand 0.194 m from the cup. The loop runs like this:

step joints (deg) hand (m) error (m) 0 10.00, 85.00 0.3678, 0.3683 0.1939 1 36.87, 64.87 0.2589, 0.5337 0.0442 2 38.07, 54.69 0.3005, 0.5463 0.0037 3 38.72, 53.59 0.3000, 0.5500 0.00003

Look at the last column. The error falls from 0.19 to 0.044 to 0.0037 to 0.00003 m. Each step makes the error roughly the square of the one before. After three steps, the hand is 0.03 mm from the cup. And the loop found the elbow-right answer, 38.73 and 53.58 degrees, with no formula.

Solve it by formula or by loop

Choose a method and a start. Press Step to run one iteration, or Run to run until it settles. Faded arms are earlier steps. You can also drag the cup.

0.10

Toy arm as before, cup at (0.30, 0.55) m unless you drag it. The nearly straight start is 30 and 3 degrees. The damped step is Δθ = JT(J JT + λ2 I)−1 e.

Failure mode: Newton near a singularity

Start the arm nearly straight, at (30°, 3°). The symptom: the first step throws the elbow around the table, and the arm never settles. The measurement: Δθ comes out as (−406°, +1007°), because J is almost singular and J−1 is huge. After seven steps the hand is still 0.75 m from the cup. Try it with the Newton button above.

The fix is damping. Instead of demanding the exact step, ask for a gentle one, and pay a penalty for large joint motion. Near a singularity, J JT has an almost zero entry, and inverting it gives a huge number. Adding λ2 keeps that entry away from zero, so the inverse stays small. The damped least squares step uses Δθ = JT(J JT + λ2I)−1 e. The number λ is the damping. At λ = 0.1, the first step is (18.5°, 15.9°). The error then falls 0.35, 0.084, 0.043, 0.020, 0.007, 0.0025, 0.0008, 0.0003 m. It is slower than clean Newton, but it never blows up.

pythonimport numpy as np
# fk(theta) and jacobian(t1, t2) as in Chapters 4 and 5

def ik_step(theta, target, lam=0.1):                # theta: shape (2,), radians
    e = target - fk(*theta)                          # shape (2,), metres
    J = jacobian(*theta)                             # shape (2, 2)
    dtheta = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), e)
    return theta + dtheta, np.linalg.norm(e)

theta = np.deg2rad([10.0, 85.0])
for k in range(12):
    theta, err = ik_step(theta, np.array([0.30, 0.55]))
print(np.rad2deg(theta), err)                     # about [38.7, 53.6], error under 1 mm

Go deeper: Inverse Kinematics covers longer arms and redundant joints, and Jacobians and Linearization explains why the loop converges.

An IK loop starts with the arm almost straight, and the first Newton step asks for joint changes of hundreds of degrees. What is the cause, and what is the fix?

Chapter 7

Uncertainty and Fusion

Describe doubt with a bell curve, then blend two doubtful readings into one better guess

The robot sits on a small wheeled base. It drives 0.50 m toward the table. The wheel sensor says "0.50 m." The wrist camera then looks at a mark on the floor and says "you are at 0.46 m." Which number is right?

Neither, probably. Wheels slip a little. Cameras blur a little. The robot holds two guesses that disagree, and it must still decide where it is. The maths of doing this well is the maths of uncertainty.

Noise, and the bell curve

Measure the same distance ten times with a cheap tape measure, and you get ten slightly different numbers. The scatter is noise. The values cluster around a middle value and thin out on both sides. Plot how often each value appears, and you get a bell shape.

That bell is the Gaussian, also called the normal distribution. Two numbers describe it. The mean μ is the centre, the best single guess. The standard deviation σ is the typical distance from the centre. About 68 out of 100 readings fall within one σ of the mean. About 95 fall within two. The variance is σ2, the spread squared.

Why a bell? Many small independent errors add up: a bit of wheel slip, a bit of floor dust, a bit of timing jitter. A famous result says such sums look Gaussian, almost whatever the small errors are. The robot rarely checks. It assumes the bell, and gets a very good tool in return.

A belief is the robot's picture of where it is: a Gaussian with a mean (where it thinks it is) and a σ (how unsure it is). A narrow bell means "I am sure." A wide bell means "I could be anywhere in this range."

Two readings, one better guess

Back to the two numbers. The odometry belief is a Gaussian with μ = 0.50 m and σ = 0.04 m. The camera belief has μ = 0.46 m and σ = 0.02 m. The camera is twice as sharp. So the answer should sit nearer to 0.46 than to 0.50.

The fix is to weight each reading by how much we trust it. Trust is the inverse of variance. Here are the steps, which together make the 1-D Kalman filter update (Thrun, Burgard and Fox, Probabilistic Robotics, 2005, chapter 3).

K
The Kalman gain, between 0 and 1. A trust slider: 0 ignores the camera, 1 believes it fully.
σ2
The variance of the current belief. Large means unsure.
σ2 + r2
The total doubt: the belief's variance plus the camera's.
r2
The variance of the camera reading. Small means a sharp camera.
z
The camera reading itself: 0.46 m.
μ′
The new mean: the old mean, nudged toward the reading by K times the gap.
σ′2
The new variance. It is always smaller than before: each reading adds knowledge.

Put in the numbers: σ2 = 0.042 = 0.0016, and r2 = 0.022 = 0.0004.

K = 0.0016 / (0.0016 + 0.0004) = 0.80

μ′ = 0.50 + 0.80 × (0.46 − 0.50) = 0.50 − 0.032 = 0.468 m

σ′2 = (1 − 0.80) × 0.0016 = 0.00032, so σ′ = 0.0179 m

The fused guess is 0.468 m. It is closer to the camera, as it should be. And its σ of 0.018 m is better than either input, 0.040 or 0.020. Two doubtful readings together beat the best one alone.

The full loop: predict, then update

The robot keeps driving, so the loop repeats. Each cycle has two moves. Predict: shift the mean by the odometry distance, and add the odometry's own noise to the variance. Doubt grows when you move without looking. Update: blend in a camera reading with the formula above. Doubt shrinks when you look.

Drive, look, fuse

The pink bell is the robot's belief. The blue bell is the camera reading. The dashed line is the true position, which the robot never sees. Press Drive, then Look.

0.039 m
0.020 m

Toy robot. Each drive commands 0.50 m, but the wheels slip: the true move is 0.47 m. Camera noise values come from a fixed list so every visit matches. The start belief is 0.00 m with a standard deviation of 0.01 m.

Follow two cycles on paper. Start with μ = 0 and σ2 = 0.0001. Drive 0.50 m: μ = 0.50, and σ2 = 0.0001 + 0.0015 = 0.0016. Look at z = 0.46: μ = 0.468, σ2 = 0.00032. Drive again: μ = 0.968, σ2 = 0.00182, so σ = 0.043. Look at z = 0.956: K = 0.82, μ = 0.958, σ2 = 0.00033. The belief tracks the truth (0.94 m after two slipping drives) even though the odometry alone says 1.00 m.

Failure mode: a filter that stops listening

Suppose the engineer says "my odometry is perfect" and sets the added noise to zero. The belief then only ever shrinks. The gain falls each cycle: K is 1 over (n + 4) after n looks in this setup. By the twentieth look, K = 0.042. The camera now gets 4% of the vote.

The symptom: the robot slowly drifts off course and the estimate does not follow. The measurement: with a 6% wheel slip, the odometry error grows by 0.03 m per drive, but the filter pulls back only 4% of each gap. The fix: never set noise to zero. Keep a small floor, so the filter always listens a little. Real systems tune this noise with data.

pythondef predict(mu, var, move, q2):              # q2: odometry noise variance per drive
    return mu + move, var + q2

def update(mu, var, z, r2):                  # r2: camera noise variance
    K = var / (var + r2)                       # trust slider, 0 to 1
    return mu + K * (z - mu), (1 - K) * var

mu, var = 0.0, 0.0001
mu, var = predict(mu, var, 0.50, 0.0015)         # 0.50, 0.0016
mu, var = update(mu, var, 0.46, 0.0004)          # 0.468, 0.00032

Go deeper: The Bayes Filter builds beliefs without assuming a bell. The Kalman Filter does the full matrix version. The Extended Kalman Filter handles curved motion models. Barfoot, State Estimation for Robotics (Cambridge University Press, 2017), is the standard textbook.

Odometry says 0.50 m with sigma 0.04 m. The camera says 0.46 m with sigma 0.02 m. Where does the fused estimate land, and how sure is it?

Chapter 8

Dynamics and Feedback

Write the motion of one joint as an equation, step it forward in time, and steer it with a controller

The planner says: turn joint 1 to 57 degrees, which is 1.0 rad. The motor does not jump there. It pushes, the joint speeds up, it overshoots, it swings back. If the planner sends the angle and hopes, the arm wobbles, or stops short, or goes wild.

To steer the joint, the robot needs two things. A model of how the joint moves when pushed. And a rule that looks at the error and decides how hard to push. The first is dynamics. The second is feedback control.

Newton's law, as an equation about change

You know F = ma: a push makes a mass speed up. A turning joint obeys the same law in spin form: torque = inertia × angular acceleration. Torque is a twisting push. Inertia I measures how hard the joint is to spin up. Angular acceleration is how fast the spin speed changes.

Add two real effects. Friction pushes back in proportion to the speed: b times the speed. And a steady load (a cable that drags, say) pushes back with a fixed torque. We get:

I
The joint's inertia: 0.5 kg m2 in our toy.
θ̈
The angular acceleration: how fast the joint's speed changes, in rad/s2. Two dots mean "change of change."
τ
The torque the motor applies. This is what the controller chooses.
b θ̇
Friction: b = 0.2 times the speed θ̇ (one dot is the speed).
τload
A steady drag from outside. Zero, or 2 N m, in the demo.

This is an ordinary differential equation, or ODE: it ties a quantity to its own rates of change. Such an equation says how the state changes, not what the state is. To find the motion, you step through time.

Euler integration: take small steps

The simplest way to follow an ODE is Euler integration. From the current angle and speed, compute the acceleration. Move the speed by acceleration times a small time step dt. Move the angle by speed times dt. Repeat. It is "walk a little, look again" with a stopwatch.

Here are the first steps of a joint that must go from 0 to 1 rad. The stiffness is Kp = 20 N m per rad. The damping gain is Kd = 6 N m s per rad, which we meet in a moment. The time step is dt = 0.01 s. At the start the speed is 0, the error is 1, and the torque is 20 × 1 = 20 N m. So the acceleration is 20 / 0.5 = 40 rad/s2.

Step 1: angle stays 0 + 0.01 × 0 = 0. Speed = 0 + 0.01 × 40 = 0.40 rad/s.

Step 2: angle = 0 + 0.01 × 0.40 = 0.004. Torque = 20 × 0.996 − 6 × 0.40 = 17.52. Acceleration = (17.52 − 0.2 × 0.40) / 0.5 = 34.88. Speed = 0.40 + 0.3488 = 0.749.

You can do this by hand for a few steps, and a computer does it a thousand times a second.

The controller: P, then D, then I

Feedback means the command depends on the error: the target minus what you measure. The P term pushes in proportion to the error: τ = Kp e. A big error means a big push, a small error means a small one. It acts like a spring that pulls the joint to its target.

A spring alone makes the joint swing forever, like a ball on a spring with almost no friction. With Kp = 20, the joint overshoots by 100% and still rings after three seconds. So add D, for derivative: push against the speed, τ = Kp e − Kd θ̇. It acts like a shock absorber.

With Kd = 2 the overshoot falls to 35%. With Kd = 6 the joint glides to the target with no overshoot, settling to within 2% in 0.86 s. Modern Robotics, section 11.2, names two numbers to measure on such a response. One is the overshoot (how far past the target it swings, as a percentage). The other is the 2% settling time (the time after which it stays within 2% of the target).

Tune one joint

The warm line is the joint angle after you command a step from 0 to 1 rad. The faint band is within 2% of the target. Switch the controller, then try the load and the time step.

20
2.0
10 ms

Toy joint: inertia 0.5 kg m2, friction 0.2 N m s/rad, explicit Euler steps, 3 s shown. In P mode Kd is off. In PID mode the integral gain is fixed at Ki = 20 N m per rad second. These numbers are illustrative.

Failure mode 1: a steady push leaves a steady error

Turn on the load. The symptom: the joint stops short of the target and sits there. The measurement: with Kp = 20 and Kd = 6, it ends at 0.90 rad, not 1.00. The error is 0.1 rad, about 5.7 degrees. This is because at rest the spring torque Kp e must balance the load: 20 e = 2, so e = 0.1.

The fix is I, for integral. Keep a running sum of the error, and add Ki times that sum to the torque. While any error remains, the sum keeps growing, and the push keeps growing, until the error is exactly zero. With Ki = 20, the joint reaches 1.00 rad, though it overshoots 15% on the way and takes 2.55 s to settle. That is the price of memory.

Failure mode 2: a step that is too long

Euler's method is exact only for tiny steps. Set Kp = 20, Kd = 2, and raise dt. At dt = 0.15 s the same controller swings to −6 rad and keeps growing. The control law is fine. The simulation, or the real control loop, is running too slowly to track the motion. The fix: shrink dt. A real robot runs its joint loop at 500 to 1000 times a second. The rule of thumb: make the step much shorter than the fastest swing.

pythonI, b, load = 0.5, 0.2, 2.0                      # inertia, friction, steady drag
Kp, Kd, Ki, dt = 20.0, 6.0, 20.0, 0.01
theta, omega, integral, target = 0.0, 0.0, 0.0, 1.0

for step in range(300):                       # 3 seconds
    e = target - theta
    integral += e * dt
    tau = Kp * e - Kd * omega + Ki * integral  # the PID law
    alpha = (tau - b * omega - load) / I       # Newton, in spin form
    theta += dt * omega                        # Euler: angle uses the old speed
    omega += dt * alpha
print(theta)                                    # about 1.01 rad: the integral removed the load error

Go deeper: Trajectory Tracking, PID and LQR builds tracking controllers from this base. Modern Robotics, chapters 8 and 11, covers full-arm dynamics and motion control.

A joint under PD control stops 0.1 rad short of its target and stays there, because a steady load pushes against it. Which term removes this error, and why?

Chapter 9

Least Squares and Optimization

Find the best answer when the data disagree, using the slope of the error to walk downhill

The robot's drawing says the links are 0.40 m and 0.30 m long. But the factory cut them a little off, and every IK answer inherits that error. The robot can measure the hand position at several poses, say with a ruler or an overhead camera. The readings are noisy. Which link lengths fit them best?

No single pose gives the answer, and the poses disagree a little. The task is to find the numbers that fit all the data as well as possible. That is least squares, the workhorse of optimization, which means choosing numbers to make a score as good as possible.

Residuals and the sum of squares

Guess two link lengths. For each reading, compute where the hand should be, using the Chapter 4 formula. The gap between that and the measured position is the residual. A good guess has small residuals.

To score the whole guess, square each residual and add them up: the sum of squared errors. Squaring does three useful things. It makes every error positive, so errors cannot cancel. It punishes big misses more than small ones. And it gives a smooth bowl-shaped score that you can slide down, which a sum of absolute values does not.

The trick: the problem is linear in the lengths

Look at the forward kinematics once more: x = L1 cos θ1 + L2 cos θ12. The angles are known from the motor encoders. The unknowns, L1 and L2, appear only multiplied by constants. So each measurement gives one equation that is linear in the unknowns. Stack all the equations into one matrix form: A l = m.

Here A has one row per measurement and one column per unknown. An x reading adds the row (cos θ1, cos θ12). A y reading adds (sin θ1, sin θ12). The vector l = (L1, L2), and m holds the measured coordinates.

C(l)
The cost: the sum of squared residuals for a guess l.
|A l − m|2
A l is what the model predicts. Subtract the measurements m, square each gap, add.
ATA
A small square grid, 2 by 2 here, however many measurements there are.
AT m
The measurements, projected onto the columns of A.

Why do the normal equations on the right give the best fit? The bottom of a bowl is where the slope is zero. The slope of C is 2AT(A l − m). Set it to zero and rearrange. You get ATA l = AT m. It is two equations in two unknowns, solved like any pair.

A worked example

Take three poses: (10°, 85°), (40°, 60°), (70°, 30°). Suppose the true lengths are 0.410 and 0.290 m (toy values), and each reading is off by a few millimetres. Six numbers result:

pose row for x row for y m_x m_y (10, 85) [0.9848 −0.0872] [0.1736 0.9962] 0.3815 0.3581 (40, 60) [0.7660 −0.1736] [0.6428 0.9848] 0.2597 0.5521 (70, 30) [0.3420 −0.1736] [0.9397 0.9848] 0.0919 0.6679

Add up the products to form ATA and ATm. The diagonal entries are each exactly 3, because cos2 + sin2 = 1 for each of three poses.

ATA = [ 3.0000 1.4532 ] ATm = [ 1.6508 ] [ 1.4532 3.0000 ] [ 1.4639 ]

Solve: L1 = 0.4101 m, L2 = 0.2893 m

The fit recovers 0.410 and 0.290 to within 0.7 mm in total. The drawing's values, 0.40 and 0.30, leave an RMS residual of 8.0 mm. The fitted values cut it to 2.9 mm, which is the noise level of the readings. That is calibration: the data corrected the model.

Walking downhill: gradient descent

For this problem you can solve the normal equations in one step. For curvy problems, like the IK of Chapter 6 or training a neural network, you cannot. The general method is to walk downhill. At the current guess, find the slope, take a step in the opposite direction, and repeat: l ← l − η × slope. That is gradient descent, and η is the step size.

Calibrate the link lengths

Colour shows the cost for each guess of (L1, L2): bright is low. The pink dot is the best fit. The teal path is gradient descent. Drag the start guess anywhere on the map, then press Step.

0.9 of limit
3.0 mm

Toy data: three poses (six readings), true lengths 0.410 m and 0.290 m, noise values from a fixed list scaled by the slider. The step size is a multiple of 1 over the largest curvature of the cost, so 1 is a safe size and anything above 2 diverges. The map spans 0.30 to 0.50 m for L1 and 0.20 to 0.40 m for L2.

Failure modes: too long a step, and a valley that is too flat

Push the step size above 2. The symptom: each step jumps over the bottom to the far wall of the bowl, higher up, and the path spirals out. The measurement: the cost grows at every step instead of shrinking. The fix: lower the step size, or let the step shrink as you go.

Now switch to "Arm nearly straight." With the same 3 mm of noise, poses near (40°, 5°) give a fit that is 27 mm off, against 0.7 mm for the spread poses. The two columns of A point almost the same way. So ATA has eigenvalues of 5.99 and 0.013. An eigenvalue says how steeply the bowl rises along one of its two main directions: 5.99 is a steep wall, and 0.013 is an almost flat floor. The bowl has become a long thin valley. The data cannot say how to split the reach between link 1 and link 2. This is the singularity of Chapter 5 again, in disguise. The fix is to collect data at varied poses.

pythonimport numpy as np

# A: shape (2N, 2), one row per x or y reading; m: shape (2N,) measured coordinates
l_hat = np.linalg.solve(A.T @ A, A.T @ m)       # normal equations: [0.4101, 0.2893]
l_alt, *_ = np.linalg.lstsq(A, m, rcond=None)   # the same, but more stable: prefer this

# gradient descent on C(l) = |A l - m|^2, step size eta below 2 / lambda_max
lam_max = 2 * np.linalg.eigvalsh(A.T @ A).max()
eta, l = 0.9 / lam_max, np.array([0.40, 0.30])
for k in range(100):
    l = l - eta * 2 * A.T @ (A @ l - m)               # walk downhill

Go deeper: Nonlinear Least Squares handles models that are not linear in the unknowns. Maths for ML covers gradients and optimization. The Robotics Math Workbook has exercises on every tool in this lesson.

Why does the formula for the hand position lead to a linear least-squares problem when you solve for the link lengths?

Chapter 10

The Field Guide

Keep one table of the tools, match a robot's symptom to the right one, and pick the lesson to take next

You have followed one reach from start to finish. The cup sat at (0.30, 0.55) m. Frames and transforms put it in the base frame. Forward kinematics told the arm where its hand was. The Jacobian showed which way each joint pushes. Inverse kinematics found the angles. A filter fused two doubtful readings. A controller drove the joints to those angles. And least squares tuned the model that all of it rests on.

Nine tools, one robot. This last chapter packs them into a map you can keep, and shows you where to go next.

The toolkit table

ToolThe question it answersWhere it shows up in a robotLesson to take next
Vectors and framesWhere is it, measured from what?Every sensor reading and every mapFrames and Transforms
RotationsHow is it turned?Camera and IMU orientation, wrist controlRotations and Transforms, Lie Groups
Homogeneous transformsHow do I move a point from one frame to another?Camera to base, tool to wrist, sensor calibrationRigid-Body Transforms
Forward kinematicsWhere is the hand for these joint angles?Collision checks, visual servoing, simulationInverse Kinematics
The JacobianHow do joint speeds turn into hand speed?Velocity control, force control, singularity checksJacobians and Linearization
Inverse kinematicsWhich angles put the hand there?Pick and place, teleoperation, animationInverse Kinematics
Probability and Kalman filtersHow sure am I, and how do I combine readings?Localization, tracking, SLAMBayes Filter, Kalman Filter, EKF
ODEs and feedbackHow do I make the joint follow a target?Every motor loop, trajectory trackingTracking, PID and LQR
Least squaresWhat fits the data best?Calibration, mapping, learningNonlinear Least Squares, Maths for ML

One more tool hides behind the Jacobian ellipse and the fit valley: eigenvalues, which measure how stretched a matrix is. They return in the filter and the control lessons.

Read a symptom, pick a tool

Real robots rarely say "my Jacobian is singular." They say something odd. Match each symptom to the tool that explains it.

Which tool explains it?

Press a symptom. The card names the likely cause, the chapter, and the lesson to open.

Pick a symptom.Each one points back to a chapter of this lesson.

The symptoms are typical, not a full list. Several tools can cause the same symptom, so the card gives the most likely one.

The key numbers in one place

QuantityValueChapter
Cup in the base frame(0.300, 0.550) m1, 3
Cup in the camera frame, start pose(0.127, 0.012) m3
Hand at joints 10 and 85 degrees(0.368, 0.368) m4
Determinant of J at the start pose0.1195 (zero when straight)5
IK joint angles for the cup38.73 and 53.58 degrees, or 84.05 and −53.586
Newton error after 0, 1, 2, 3 steps0.194, 0.044, 0.0037, 0.00003 m6
Fused estimate and its sigma0.468 m, 0.018 m7
PD step response, Kp 20 and Kd 6No overshoot, 0.86 s to settle8
Fitted link lengths0.4101 m and 0.2893 m9

The whole lesson in code

pythonimport numpy as np
L1, L2 = 0.40, 0.30

def T(th, tx, ty):                                   # Ch 3: a planar transform
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, tx], [s, c, ty], [0, 0, 1.0]])
def fk(q):                                          # Ch 4: joint angles to hand
    return np.array([L1*np.cos(q[0]) + L2*np.cos(q[0]+q[1]), L1*np.sin(q[0]) + L2*np.sin(q[0]+q[1])])
def jac(q):                                         # Ch 5: joint speeds to hand speed
    s1, c1, s12, c12 = np.sin(q[0]), np.cos(q[0]), np.sin(q[0]+q[1]), np.cos(q[0]+q[1])
    return np.array([[-L1*s1-L2*s12, -L2*s12], [L1*c1+L2*c12, L2*c12]])
def ik(target, q, lam=0.1, n=20):                   # Ch 6: a damped loop
    for _ in range(n):
        e, J = target - fk(q), jac(q)
        q = q + J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), e)
    return q
def fuse(mu, var, z, r2):                            # Ch 7: a Kalman update
    K = var / (var + r2); return mu + K*(z - mu), (1 - K)*var
def pd(e, w, Kp=20.0, Kd=6.0):                      # Ch 8: a torque from error and speed
    return Kp*e - Kd*w
# Ch 9: l_hat = np.linalg.lstsq(A, m, rcond=None)[0]

q = ik(np.array([0.30, 0.55]), np.deg2rad([10.0, 85.0]))
print(np.rad2deg(q), fk(q))                           # about [38.7, 53.6] and [0.30, 0.55]

Where to go next, and in what order

Study in the order the robot needs the tools. Read the lessons below one at a time. Do not try to finish a whole textbook before you touch a robot.

  1. Frames and rotations: Frames and Transforms, then Rotations and Transforms. Everything else rests on these.
  2. Poses and arms: Rigid-Body Transforms, Jacobians and Linearization, then Inverse Kinematics.
  3. Doubt: Bayes Filter, Kalman Filter, then EKF.
  4. Motion: Trajectory Tracking, PID and LQR.
  5. Fitting: Nonlinear Least Squares, with Eigenvalues and Maths for ML as support. For smooth rotation groups, try Lie Groups once you are ready.
  6. Practice all of it: the Robotics Math Workbook.

References

  1. K. M. Lynch and F. C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Free preprint: hades.mech.northwestern.edu. Sections 3.1 (planar motions), 5.1 and 5.3 (Jacobian, singularity), 6.1 and 6.2 (inverse kinematics), 11.2 (error dynamics).
  2. S. Thrun, W. Burgard and D. Fox. Probabilistic Robotics. MIT Press, 2005. The Bayes and Kalman filters.
  3. T. D. Barfoot. State Estimation for Robotics. Cambridge University Press, 2017; second edition 2024. Estimation, rotations and least squares.
The takeaway. A robot is a stack of small, old ideas. A place is a list of numbers in a frame. A turn is a matrix. Chaining frames is multiplying matrices. A slope links joint speeds to hand speed. A loop inverts it. A bell curve describes doubt. An equation steps motion forward in time. And a score that you walk downhill fits the model to the world.

Now press Teach and explain one reach from memory. Say how the camera reading reaches the base frame, how the joints are found, and what could go wrong at each step. Then go back to the arm at the top. Drag the cup to the edge of the reachable ring, and say which of the nine tools you see at work.

A robot reaches a steady 10 cm to the right of every cup, though its joint controllers track perfectly. Which tool from this lesson is the first suspect?
Math for Robotics
Modern Robotics (free preprint) · Robotics Math Workbook · Back to Gleams