Calculus and optimization

Intermediate40 minBefore this:Python for robotics · Probability for robots

A lidar returns 60 points along a wall, each a little off. Where exactly is the wall? A neural network that picks up objects has a million weights. Which way should each one move to make it fail less? A robot arm needs its hand at a given spot. Which joint changes get it there? These are all the same question: find the values that make an error as small as possible. The tools are derivatives and a loop that follows them downhill.

You will be able to

• Explain a derivative and a gradient as the local slope and the direction of steepest increase.
• Verify an analytic gradient against a numerical one.
• Fit a line to noisy points with least squares, both in closed form and with NumPy.
• Implement gradient descent and see what a bad learning rate does.
• Read a Jacobian as the derivative of a robot's motion.

Derivatives: how fast does it change?

The derivativeA derivative measures how quickly a quantity changes when you nudge its input; it is the slope of the curve at a point. $f'(x)$ of a function tells you how much $f$ changes for a tiny nudge in $x$. If $f(x)=x^2+3x$, then $f'(x)=2x+3$, so at $x=2$ the function climbs at a rate of 7 per unit.

You do not always have a formula. A computer can estimate the slope by nudging the input both ways and dividing:

$$f'(x)\approx\frac{f(x+h)-f(x-h)}{2h}$$

for a small $h$. This central difference is accurate and takes three lines.

derivative.py

def f(x):
    return x ** 2 + 3 * x


def num_grad(f, x, h=1e-6):
    return (f(x + h) - f(x - h)) / (2 * h)


print(num_grad(f, 2.0))     # 7.0000000001, matches 2*2 + 3

Numerical gradients are slow for large problems, but they are the best way to test the formula you derived by hand. If your analytic derivative and the numerical one disagree, your formula is wrong. Professional robotics and machine learning engineers do this check constantly.

Gradients: slope in many directions

Most robotics functions have many inputs. A gradientThe gradient of a function of several variables is the vector of its partial derivatives; it points in the direction of steepest increase. collects the slope along each input into one vector:

$$\nabla L(\boldsymbol\theta)=\begin{bmatrix}\partial L/\partial\theta_1\\ \vdots\\ \partial L/\partial\theta_n\end{bmatrix}.$$

The gradient points uphill, in the direction where $L$ grows fastest. If you want to make $L$ smaller, step the other way. That is the entire idea of gradient descentGradient descent minimises a function by repeatedly stepping in the direction opposite to its gradient.:

$$\boldsymbol\theta\leftarrow\boldsymbol\theta-\alpha\,\nabla L(\boldsymbol\theta).$$

The number $\alpha$ is the learning rate, the step size. Too small and you crawl. Too large and you overshoot and blow up. You will see both.

Optimization is walking downhill in the dark

You cannot see the whole landscape of the error. You only feel the slope under your feet, which is the gradient, and take a step down. Every learning algorithm in this course, from tuning a controller to training a neural network, is a variation on that walk.

A running example: fit a line to noisy data

A lidar sees points along a straight wall. We model the wall as $y=mx+b$ and want the best $m$ and $b$. Each point leaves a residual: the gap between what the line predicts and what was measured. The best line makes the total squared residual smallest:

$$L(m,b)=\frac1N\sum_{i=1}^{N}\left(m x_i+b-y_i\right)^2 .$$

This is least squares. Squaring makes every error positive and punishes large ones more than small ones. It is also the maximum-likelihood answer when noise is Gaussian, which links straight back to the last lesson.

Write the model as a matrix equation. Stack the data into $A=[\,x\ \ 1\,]$ (a column of x values and a column of ones) and $\boldsymbol\theta=(m,b)^\top$, so the prediction is $A\boldsymbol\theta$ and

$$L=\tfrac1N\lVert A\boldsymbol\theta-\mathbf y\rVert^2,\qquad \nabla L=\tfrac2N\,A^\top(A\boldsymbol\theta-\mathbf y).$$

Setting the gradient to zero gives the normal equations, $A^\top A\,\boldsymbol\theta=A^\top\mathbf y$, which you can solve directly.

fit_line.py

import numpy as np

rng = np.random.default_rng(3)
m_true, b_true = 0.8, 1.5
x = np.linspace(0.0, 5.0, 60)
y = m_true * x + b_true + rng.normal(0.0, 0.1, size=x.size)

A = np.column_stack([x, np.ones_like(x)])        # shape (60, 2)

# 1. Library least squares (the one to use in practice)
theta_ls, *_ = np.linalg.lstsq(A, y, rcond=None)
print(theta_ls)                                   # about [0.80 1.50]

# 2. Normal equations, solved directly
theta_ne = np.linalg.solve(A.T @ A, A.T @ y)
print(theta_ne)                                   # same numbers


# 3. Gradient descent
def loss(theta):
    r = A @ theta - y
    return (r @ r) / len(y)


def grad(theta):
    r = A @ theta - y
    return 2.0 * A.T @ r / len(y)


theta = np.zeros(2)
lr = 0.02
for _ in range(5000):
    theta -= lr * grad(theta)
print(theta, loss(theta))                         # lands on the same answer

All three give the same line, about slope 0.80 and intercept 1.50. For a linear model like this one, lstsq is the right tool: exact, fast and numerically careful. Gradient descent earns its keep when there is no closed form, as with neural networks.

Check your gradient

Before trusting a hand-derived gradient, compare it with a numerical one.

grad_check.py

# continues from fit_line.py
t0 = np.array([0.3, -0.2])
h = 1e-6
num = np.array([(loss(t0 + e) - loss(t0 - e)) / (2 * h) for e in np.eye(2) * h])
print(num)             # [-16.90  -5.90]
print(grad(t0))        # the same, to about 6 digits

If these disagree, find the bug in grad before you do anything else.

What a bad learning rate does

learning_rate.py

# continues from fit_line.py
theta = np.zeros(2)
for _ in range(50):
    theta -= 0.2 * grad(theta)     # too large
print(theta)                        # huge numbers: the iteration diverged

With the step size 0.2, each step overshoots the valley, lands higher up the opposite wall, and the error grows until the numbers explode. With 0.02 it converged. Picking the learning rate is a practical skill, and a diverging loss is the usual sign it is too high.

Outliers wreck least squares

Because errors are squared, a single wild point has an enormous pull. Add two bad lidar returns to the data above (one 8 m too high, one 6 m too low) and the fitted line moves to roughly slope 0.66 and intercept 1.89, far from the truth. Real sensors produce such outliers. The fix, robust estimation such as RANSAC, comes in Module 05.

Jacobians: calculus for moving robots

A robot arm's hand position depends on its joint angles. Suppose a two-link planar arm has link lengths $l_1,l_2$ and joint angles $q_1,q_2$. The hand is at

$$x=l_1\cos q_1+l_2\cos(q_1+q_2),\qquad y=l_1\sin q_1+l_2\sin(q_1+q_2).$$

How does the hand move when the joints move a little? The matrix of all partial derivatives is the JacobianThe Jacobian is the matrix of partial derivatives that says how small changes in the inputs of a function change its outputs. $J$, and it connects the two:

$$\delta\mathbf p\approx J(\mathbf q)\,\delta\mathbf q .$$

To reach a target, you can use this like gradient descent: measure the hand's error, convert it to a joint change with the (pseudo-)inverse of $J$, and repeat. The check with a numerical Jacobian works exactly as before.

jacobian.py

import numpy as np


def fk(q, l1=1.0, l2=0.8):
    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, l1=1.0, l2=0.8):
    s1, c1 = np.sin(q[0]), np.cos(q[0])
    s12, c12 = 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]])


q = np.array([0.4, 0.9])
h = 1e-6
num = np.column_stack([(fk(q + e) - fk(q - e)) / (2 * h) for e in np.eye(2) * h])
print(np.allclose(num, jac(q), atol=1e-6))    # True

Do not worry about the arm yet. Module 02 develops forward and inverse kinematics properly. The point here is that the same habits, derive, then verify numerically, carry over.

Hands-on lab

You will fit lidar wall data three ways and compare.

1. In kinemata-workspace/foundations, create fit.py. Generate 80 points along a wall using y = 0.5 * x + 2.0 for x from 0 to 8, with Gaussian noise of standard deviation 0.05.
2. Fit with np.linalg.lstsq and print the slope and intercept. They should be near 0.5 and 2.0.
3. Fit again with gradient descent, using lr = 0.01 and 10,000 iterations. Print the result.
4. Compare the two fits using np.allclose(theta_gd, theta_ls, atol=1e-2). It should print True. If it does not, increase the iterations.
5. Check your gradient: compare grad with a central difference at a random theta, with np.allclose(num, grad(t), atol=1e-4).
6. Replace one point with y + 5 and refit. Note how far the line moves.
7. Commit fit.py with a message that says what it does.

The check: steps 2, 3 and 4 agree, the gradient check prints True, and the outlier visibly drags the line.

Exercises

Analytic versus numericalBeginner

Derive the derivative of $g(x)=\sin(x)\,x^2$ and check it at $x=1.5$ with num_grad.

Show solutionHide solution

By the product rule, $g'(x)=\cos(x)\,x^2+2x\sin(x)$. In code:

import numpy as np
g = lambda x: np.sin(x) * x ** 2
dg = lambda x: np.cos(x) * x ** 2 + 2 * x * np.sin(x)
print(dg(1.5), num_grad(g, 1.5))    # both about 3.1516

They agree to many decimals.

Find the stable learning rateIntermediate

Minimise $f(x)=x^2$ with gradient descent starting at $x=5$. Try learning rates 0.1, 0.9, 1.0 and 1.1 for 20 steps. What happens to each?

Show solutionHide solution

The gradient is $2x$, so each step multiplies $x$ by $(1-2\alpha)$. For 0.1 the factor is 0.8, so $x$ shrinks smoothly. For 0.9 the factor is $-0.8$: it zigzags across zero but still shrinks. For 1.0 the factor is $-1$, so $x$ bounces between 5 and -5 forever. For 1.1 the factor is $-1.2$ and $x$ grows, so it diverges. Descent converges only if $\lvert1-2\alpha\rvert<1$.

Fit a circle by least squaresAdvanced

A set of points lies roughly on a circle. The equation $x^2+y^2=2ax+2by+c$ is linear in the unknowns $a$, $b$, $c$, where the centre is $(a,b)$ and the radius is $\sqrt{c+a^2+b^2}$. Fit a circle to the points generated from centre $(2,-1)$, radius 3 with a little noise.

Show solutionHide solution

import numpy as np

rng = np.random.default_rng(0)
t = rng.uniform(0, 2 * np.pi, 100)
x = 2 + 3 * np.cos(t) + rng.normal(0, 0.02, 100)
y = -1 + 3 * np.sin(t) + rng.normal(0, 0.02, 100)

A = np.column_stack([2 * x, 2 * y, np.ones_like(x)])
rhs = x ** 2 + y ** 2
(a, b, c), *_ = np.linalg.lstsq(A, rhs, rcond=None)
print(a, b, np.sqrt(c + a ** 2 + b ** 2))    # about 2.0, -1.0, 3.0

The trick is to rewrite a nonlinear shape so that the unknowns appear linearly, which turns the problem into ordinary least squares.

Check your understanding

1What does the gradient of a loss point toward?

AThe minimumBThe direction of steepest increaseCThe direction of steepest decreaseDThe previous step

2Why compare an analytic gradient with a numerical one?

ANumerical gradients are always more accurateBTo catch mistakes in the hand-derived formulaCTo make training fasterDBecause analytic gradients do not exist

3Gradient descent on a smooth loss produces rapidly growing numbers. What is the most likely cause?

ALearning rate too smallBLearning rate too largeCToo many data pointsDThe loss is convex

4Why can a single outlier badly distort a least-squares fit?

ALeast squares ignores noiseBErrors are squared, so large errors dominate the totalCNumPy cannot handle itDOutliers are always correct

0 of 4 answered

Takeaways

• A derivative is local slope, and a gradient is the vector of slopes that points uphill.
• Gradient descent steps against the gradient; the learning rate sets the step size and a too-large one diverges.
• Least squares fits a model by minimising squared residuals, and for linear models np.linalg.lstsq solves it exactly.
• Always verify a hand-derived gradient with a numerical central difference.
• Squared error is sensitive to outliers, so real systems add robust methods.
• A Jacobian is the derivative of a robot's motion, and it links joint changes to hand changes.

Go deeper

• 3Blue1Brown: Essence of Calculus: visual intuition for derivatives and the chain rule.
• NumPy lstsq reference: details and options of the least-squares solver.
• Modern Robotics, chapter 6: inverse kinematics: a preview of Jacobian-based methods from Module 02.
• Convex Optimization (Boyd and Vandenberghe): the free standard text, for later reading.

Next: Linux, Git and tooling