Control: From PID to Whole-Body

Chapter 3 told you which joint angles you want. Control is how you actually get there, in a world with gravity, friction, and inertia all pushing back.

Here's the gap that ruins your whole day the first time you drive a real motor: you command it to go to 90° and it doesn't. It sags to 84°, or it swings past to 97° and rings like a bell, or it just sits there humming. Chapter 3 handed you a beautiful answer: these are the joint angles that put the hand where you want it. But an angle on paper is a wish. A motor lives in a world with gravity pulling the arm down, friction gumming up the joints, and inertia that refuses to start or stop on command. The command and the reality are two different things, and control is the entire discipline of closing that gap.

That gap is this chapter's villain: the space between what you told the robot to do and what the robot actually did. The world pushes back, always, and an open-loop plan that ignores the pushback falls apart the instant reality deviates from the model. Our hero is the same one Chapter 1 promised would return: feedback, Villain #2's nemesis, the trick of constantly re-measuring and correcting instead of commanding once and hoping.

I'm assuming you've read Chapter 3, so you're comfortable with joint angles, end-effector poses, and the idea that a robot's configuration is a handful of numbers. You don't need any control theory going in. We build it from one equation, $e = \text{setpoint} - \text{measurement}$, and grow everything else out of watching simpler ideas break.

Here's the road, and every stop is a fix for the one before it. We start open-loop and watch it fail the moment the world deviates. Feedback fixes that (proportional control), but P alone settles with a permanent error against gravity. Integral action kills that error but can wind up. Derivative action damps the resulting overshoot but amplifies noise. We stack all three into PID, then admit reaction is slow and add feedforward so the controller predicts instead of only reacting. We extend a single setpoint into a whole trajectory, then stop commanding position at all and command a force–motion relationship: impedance control, the callback to Chapter 1's stiff-vs-compliant actuators. And we end by looking up at whole-body control, where hand-designing all this gets so hard it makes Chapter 7's learning look tempting.

Let's go.

Start with the simplest possible idea. You want the joint at 90°. You know (roughly) that a certain motor voltage holds it there. So you apply that voltage and walk away. No sensing, no correcting: you computed a command from a model and sent it. This is open-loop control: the controller's output depends only on the desired input and a model of the plant, never on what actually happened.

Open-loop works beautifully right up until reality deviates from your model, and then it fails silently. A microwave is open-loop: you ask for 90 seconds, it runs the magnetron for 90 seconds, and it never once checks whether your food is actually hot. That's fine, because nothing fights back; the timer is the whole job. But point the same philosophy at a robot arm and watch it crumble. Your model said "this voltage holds 90°," but today the arm is carrying a slightly heavier tool, or the joint is cold and stiff, or the battery sagged. The arm settles at 84° and the controller has no idea, because it never looks. It's confidently, permanently wrong.

The disease is that the world is never exactly your model. Loads change, friction drifts with temperature, a gust of wind hits the drone, someone leans on the arm. Open-loop has no mechanism to notice any of it. Every disturbance is invisible, so every disturbance is permanent.

The cure is embarrassingly simple to state: look at what actually happened, compare it to what you wanted, and use the difference to correct. Measure the joint angle. Subtract it from the target. That difference is the single most important object in this entire chapter:

Reading the symbols: $e$ is the error, the gap we're trying to kill. The setpoint (often written $r$, the reference) is where you want the joint, the amber command from Chapter 1's color language. The measurement (often $y$) is where the joint actually is right now, read off an encoder, the blue signal under control. Error is just how wrong you are, with a sign that tells you which way. If the setpoint is 90° and the encoder reads 84°, then $e = 90 - 84 = +6^\circ$: you're six degrees short, and the positive sign says "push up." Read 97° instead and $e = -7^\circ$: you overshot, push back down.

The moment a controller's output depends on $e$, we've closed the loop. That's closed-loop (or feedback) control, and it's the foundation everything else in this chapter stands on. The controller senses the outcome and feeds it back to the input (hence feedback), so a disturbance that would have been invisible open-loop now shows up immediately as a nonzero error the controller is duty-bound to erase. (Side note: this is the exact hero Chapter 1 named. Feedback is how a robot fights the reality gap; every controller below is just a different rule for turning $e$ into a command.)

So we'll use the error. The most natural rule imaginable: the more wrong you are, the harder you push. If you're 40° off, shove hard; if you're 2° off, nudge gently; if you're dead on, do nothing. Command in direct proportion to error. That's proportional control, the P in PID, and it's the honest first thing anyone tries.

Reading the symbols: $u$ is the control effort, the thing you actually send to the motor, a torque or a voltage. $e$ is the error we just built. $K_p$ is the proportional gain, a number you choose that sets how aggressively the controller reacts per unit of error. Big $K_p$ means a small error produces a big shove; small $K_p$ means the controller is timid. That's the whole law: effort is a scaled copy of how wrong you are.

Let's work it by hand. Say $K_p = 2$ (units: newton-meters of torque per degree of error). The arm is at 60°, target is 90°.

Feels perfect. And on a frictionless arm floating in space with nothing pulling on it, it nearly is. But now let the villain into the room. Hang gravity on that arm. Suppose holding the arm level against gravity takes a steady 8 N·m, a constant disturbance that never lets up. Watch what P control does.

The arm can only produce effort when there's error, because $u = K_p e$. So to generate the 8 N·m needed just to hold position, it needs a standing error big enough that $K_p e = 8$. With $K_p = 2$, that's $e = 8/2 = 4^\circ$. The arm settles not at 90° but at 86°, permanently four degrees short, and it parks there forever, perfectly balanced: the 4° of error produces exactly the 8 N·m that gravity is stealing. Push the gain up to $K_p = 8$ and the standing error shrinks to $8/8 = 1^\circ$, better but still not zero. Crank it higher and you buy a smaller error at the price of a twitchy, oscillation-prone controller that rings on every move.

This leftover gap has a name, steady-state error, and it's P control's fundamental, unfixable flaw. A pure proportional controller needs some error to produce any effort, so against a constant disturbance it can never fully close the gap. Zero error would mean zero command, which can't hold against gravity, so it refuses to reach zero error. It's a dog chasing a car it's structurally forbidden to catch. We need a term that can produce effort even when the instantaneous error is zero.

The problem with P is amnesia. Every instant, it looks only at the error right now and forgets that it's been four degrees short for the last ten seconds. If only the controller could notice that a small error has been stubbornly persisting and get progressively more insistent about it, accumulate its frustration until the error finally caves.

That's exactly integral control. Add up the error over time, and let that running total drive its own contribution to the command. A tiny error that persists integrates into a large accumulated value, which produces a large corrective effort, so the controller can generate the steady 8 N·m of gravity compensation even when the instantaneous error is zero, because the accumulated history is still there holding it up.

Reading the symbols: $\int e\,dt$ is the integral of the error, the running sum of all past error, area under the error-versus-time curve. If you've been 4° short for a while, this term is large and growing; if the error has been zero, it stops growing and holds whatever value it reached. $K_i$ is the integral gain, setting how fast the accumulator translates into effort. In code you never integrate symbolically. You just keep a variable and, every timestep of length $\Delta t$, do integral += e × Δt. That accumulator is the memory P was missing.

By hand, with $K_i = 0.5$ and a timestep $\Delta t = 0.1$ s, sitting at the P-controller's stuck 4° error:

That's the magic of I: it converts a persistent error into an ever-growing correction, so the only stable resting place is zero error. The integral term is what lets the controller hold a nonzero command at zero error.

But integral has a nasty failure mode, and it's the one that bites everyone. Imagine the arm is physically blocked (someone's holding it, or it's slammed against a hard stop) so the error stays large and refuses to shrink no matter what. The integrator doesn't know it's blocked. It just keeps doing its job: accumulating, accumulating, accumulating, building up a monstrous stored value. Then you let go. The controller now has a gigantic accumulated command that takes ages to "unwind," so the arm rockets past the target, massively overshoots, and has to slowly bleed off all that stored-up integral before it settles. This is integral windup, and it turns a helpful memory into a loaded spring. The standard fixes are clamping the integrator to a sane range and anti-windup logic that stops accumulating whenever the actuator is saturated (already commanding as hard as it can). You can run this exact experiment in Fig 4.3: block the arm, watch the I-term climb, and release. Keep windup in mind: it's the price of memory, and it's the flaw the derivative term is not going to fix, because we've got a different problem to solve first.

P and I together will get you to the target with zero steady-state error. But how they get there is often ugly. A well-tuned aggressive P (or the ratcheting energy of I) tends to build up momentum charging at the setpoint, blow right through it, swing back, overshoot the other way, and oscillate, ringing back and forth before it finally settles. The controller is enthusiastic but reckless: it's still flooring the accelerator when it should already be braking, because it only knows how far off it is, not how fast it's closing.

What's missing is anticipation of the approach. A good driver eases off the gas before reaching the stop sign, judging not just distance but closing speed. The signal that captures closing speed is the rate of change of the error: how fast $e$ is shrinking. If the error is large but collapsing quickly, you're about to arrive and should back off now to avoid overshoot. That's derivative control: push against the rate of change of the error, applying a braking force proportional to how fast you're closing the gap.

Reading the symbols: $de/dt$ is the derivative of the error, its instantaneous rate of change, positive if the gap is opening, negative if it's closing fast. $K_d$ is the derivative gain, setting how strongly the controller reacts to that rate. Because $de/dt$ is negative while you're rushing toward the target (error shrinking), the $K_d$ term produces effort opposing the motion, a velocity-dependent brake that grows with speed. It's pure damping. It contributes nothing at steady state (a parked error isn't changing, so $de/dt = 0$) and nothing to the target itself. Its whole job is to smooth the approach, killing overshoot and calming oscillation.

Worked quickly, with $K_d = 0.3$. Estimate $de/dt$ as the change in error over one timestep divided by $\Delta t$:

So D is the calming influence. But (you saw this coming) it has its own flaw, and it's a bad one. Derivative control amplifies noise. Real encoder measurements are jittery: even a perfectly still joint reads 90.00, 90.03, 89.98, 90.02 as sensor noise dances on the last digit. That jitter is tiny in position but huge in rate, because you're dividing small meaningless wiggles by a small $\Delta t$. A 0.03° blip over a 0.001 s timestep looks like a 30 °/s velocity spike to the derivative term, which then slams the motor with a nonsense correction. Crank $K_d$ up and the arm buzzes and screeches, chasing noise. The standard defenses are low-pass filtering the derivative (smoothing the rate estimate before you use it) and computing the rate on the measurement rather than the error to avoid a spike every time the setpoint jumps. But the core tension is permanent: D damps real motion beautifully and amplifies fake motion just as beautifully, and it can't tell them apart.

Notice the pattern the whole chapter has been building. P reacts to error but leaves a steady-state gap. I closes the gap but can wind up. D damps the overshoot but amplifies noise. Each term fixes the previous one's flaw and introduces its own. So we don't pick one; we blend all three.

Stack the three terms and you get the single most-used control law on Earth:

Read it as three simultaneous perspectives on the same error signal, each answering a different question:

Present, past, future: one signal, three lenses, three gains to blend them. That's the whole thing. And it's genuinely ancient by software standards: the theory was worked out by Nicolas Minorsky in 1922 for automatic ship steering. He derived the three-term law by watching how a skilled helmsman actually held a course: hard over for a big heading error (P), a little extra to counter a steady crosswind (I), and easing the wheel back as the bow swung toward the mark (D). A century later, PID still runs an estimated 90-plus percent of the industrial control loops on the planet: thermostats, cruise control, drone attitude, 3D-printer heaters, hard-drive heads. It is the workhorse, and understanding it is most of practical control.

The whole art of PID is tuning (choosing the three gains), and the intuition lives on one axis, borrowed from how a mass-spring-damper responds to a step:

A workable tuning recipe, roughly: raise $K_p$ until the response is snappy but just starting to overshoot; add $K_d$ to damp that overshoot back out; then add just enough $K_i$ to erase any leftover steady-state error, watching for windup. There are formal methods (Ziegler–Nichols is the classic, from 1942), but honestly most real tuning is this informed twiddling, guided by watching the step response and knowing which knob fixes which symptom.

PID is purely reactive. Every correction it makes is a response to an error that has already happened: the arm has to sag before the controller notices and pushes back up. That's a structural lag: feedback is always a step behind reality, cleaning up messes rather than preventing them. Against a disturbance you can see coming, like gravity, which you know is there before the arm ever moves, reacting to it after the fact is leaving free performance on the table.

Here's the insight. You often have a model of the predictable part of the world. You know the arm has mass. You know the gravitational torque needed to hold it at any angle; it's just physics, computable from the geometry Chapter 3 gave you. So why wait for gravity to cause an error and then react? Compute the effort you know you'll need and add it in preemptively. That's feedforward: a model-based command injected in anticipation, so feedback only has to clean up the small residual the model didn't predict.

Reading it: $u_{\text{ff}}$ is the feedforward term, computed from a model of the plant; for an arm, most commonly the gravity-compensation torque, the exact torque needed to hold the arm up against gravity at its current pose. The feedback block is our familiar PID, now demoted to a residual cleaner: it only has to correct the difference between what the model predicted and what actually happened (friction, an unmodeled load, a gust). Prediction does the heavy lifting; feedback mops up.

The canonical case: gravity compensation. For a single joint holding a link of mass $m$ whose center of mass sits a distance $L$ from the pivot, the gravitational torque trying to pull it down is $\tau_g = m g L \cos\theta$, where $g = 9.81$ m/s² and $\theta$ is measured from horizontal. That $\cos\theta$ is the geometry: gravity's torque is maximal when the arm is horizontal ($\cos 0^\circ = 1$, full lever arm) and zero when it hangs straight down ($\cos 90^\circ = 0$, no lever arm). Work a number: $m = 2$ kg, $L = 0.3$ m, arm horizontal ($\theta = 0$). Then $\tau_g = 2 \times 9.81 \times 0.3 \times 1 = 5.9$ N·m. Feed that 5.9 N·m in as $u_{\text{ff}}$ and the arm holds itself level with zero error before feedback has done anything at all: you've cancelled gravity outright instead of tolerating a standing error to fight it.

This is why the classic robot-arm controller is PD plus gravity compensation, not PID. Once feedforward cancels gravity, the constant disturbance that created the steady-state error in the first place, you no longer need the integral term to grind that error out, so you drop I (dodging windup entirely) and keep PD purely to damp and correct the leftover residual. Prediction handles the known; feedback handles the surprise. It's the difference between a controller that understands its own body and one that only ever reacts to being wrong.

Everything so far chased a single setpoint: hold 90°, hold this pose. But real robots rarely want to jump to a target; they want to move smoothly along a path: sweep the arm through a welding seam, follow a drawn curve, carry a full cup without sloshing. That's a whole time-varying reference, a trajectory, a function $r(t)$ giving the desired position (and usually velocity and acceleration) at every instant, not just a lone endpoint.

The fix is small and elegant: make the setpoint a moving target. Instead of a fixed $r$, feed the controller $r(t)$, the desired position right now, and recompute the error against it every timestep: $e(t) = r(t) - y(t)$. The controller is now chasing a point that's itself gliding along the path, and its job is to stay glued to it. This pairs perfectly with feedforward: since you planned the trajectory, you know the desired velocity and acceleration at every instant, so you can feed those forward, commanding the motor the acceleration the plan calls for (mass times desired acceleration, plus the gravity term), and let PID clean up only the tracking error. Feedforward the plan, feed back the deviation.

Put a number on it. Plan a sweep from 0° to 90° over 3 s with a smooth cosine ramp, $r(t) = 45(1 - \cos(\pi t / 3))$ degrees. At $t = 1.5$ s the plan says: be at 45°, moving at 47 °/s, accelerating at exactly zero (it's mid-swing, coasting through the fastest point). Feedforward hands the motor the torque for precisely that state, the gravity term at 45° plus inertia times the planned acceleration, and PID's whole job shrinks to trimming the last degree of tracking error. Chase the moving point, not the far-off endpoint, and the error never gets a chance to grow. Trajectory tracking is where feedforward really earns its keep, because now you're anticipating not just gravity but the whole intended motion. (Where those smooth $r(t)$ trajectories come from, planning a feasible, collision-free path through the configuration space of Chapter 1's Fig 1.3, is the subject of Chapter 6. Here we just track one once it exists.)

Every controller so far has been obsessed with position: reach this angle, follow this path, and resist anything that pushes you off it. That obsession is exactly what you want for cutting a straight line or holding a pose. But it's a disaster the moment the robot has to touch something. Think about a robot pressing a peg into a hole, wiping a table, or shaking a person's hand. A stiff position controller commanded to be "right there" will fight any contact to the death: if its target is a hair inside the table surface, it will push through the table (or itself) with the full authority of its gains, because to a position controller, contact is just an error to be crushed. That's how position control snaps pegs, gouges surfaces, and hurts people.

The reframe is beautiful. In a contact task you usually don't actually care about exact position; you care about a relationship between force and motion. "Push on this surface with 5 newtons, and if it moves, follow it." "Be rigid along the cut line but soft perpendicular to it, so you can slide without gouging." So stop commanding a position and start commanding that relationship between force and position directly. That's impedance control (and its close cousin admittance control): instead of a stiff position target, you make the robot behave like a chosen mechanical system, most often a virtual spring-damper, so that when the world pushes on it, it responds the way a spring of your chosen stiffness would.

Reading the symbols: $F$ is the force the robot exerts. $x$ is the actual end-effector position, $x_{\text{desired}}$ the target (the rest position of your virtual spring). $K$ is the stiffness (the virtual spring constant you get to choose) and $D$ is the damping. If you notice this looks exactly like a spring-damper from physics class, that's the entire point: the robot is emulating a spring of stiffness $K$ and a damper $D$, in software, at whatever values you dial in. Push the end-effector away from $x_{\text{desired}}$ and it pushes back with force $K$ times the displacement, gently if $K$ is small, firmly if $K$ is large, exactly like a real spring, except you set the spring constant on the fly.

The $K$ knob is the whole story, and it's the callback Chapter 1 promised. Turn $K$ high and the virtual spring is stiff: the robot resists displacement hard and behaves like a rigid position controller, precise, but unforgiving in contact. Turn $K$ low and the spring is soft: push the end-effector and it yields, following your hand, springing gently back when released, compliant. This is Chapter 1's stiff-vs-compliant actuator split, now expressed not as a hardware property but as a number you command in software, adjustable moment to moment. A robot can be stiff while moving fast through free space and go soft the instant it makes contact.

Work the number: $K = 200$ N/m, virtual rest position $x_{\text{desired}}$, and something pushes the end-effector 2 cm ($0.02$ m) off it. Restoring force: $F = 200 \times 0.02 = 4$ N. A gentle, springy resistance: you can push it around by hand and it yields like a soft spring. Now set $K = 5000$ N/m and the same 2 cm displacement demands $F = 5000 \times 0.02 = 100$ N: rigid, immovable, a position controller in all but name. Same robot, same code, one number deciding whether it's a rock or a cushion.

This is the enabling idea for two huge domains. Contact tasks: peg-in-hole, polishing, assembly, anything where the robot must comply with a surface it can't perfectly localize; impedance control lets it press with a controlled force and slide along instead of jamming or snapping. Safety around humans: a compliant robot that yields when it bumps you is fundamentally safer than a stiff one that plows through, which is why collaborative robots ("cobots") that share space with people are almost always torque-controlled with low virtual stiffness (the exact reason Chapter 1 said torque-controlled actuators exist, and the theme Chapter 12 returns to for human-safe systems). Command a force–motion relationship, not a position, and the robot stops fighting the world and starts negotiating with it.

One joint, one setpoint: we can hand-tune that all day. But now picture a humanoid standing on one foot, reaching for a shelf while a hand rests on a wall for balance. There is no "the joint" to control. There are thirty-odd joints that must move together to satisfy several goals at once: keep the reaching hand on target, keep the wall hand where it is, and, non-negotiably, keep the whole body's center of mass balanced over the supporting foot so it doesn't topple. And it must do all this under hard constraints: the feet in contact can't slip or pull on the floor, joints can't exceed their limits, motors can't exceed their torque. This is whole-body control: coordinating every joint of a high-DoF robot to achieve multiple prioritized objectives simultaneously while respecting the physics of contact and balance.

The mature classical answer is genuinely impressive engineering. At every control tick, hundreds of times a second, the robot solves an optimization problem: find the joint torques that best achieve the prioritized tasks (balance first, then hands, then posture) subject to the contact and actuator constraints, typically as a quadratic program using the full-body dynamics model (the same physics that gave us gravity compensation, now scaled up to the whole articulated body). It's gravity compensation and feedforward and task-tracking, generalized to a whole coordinated body with a strict priority stack, and it's how the classical controllers on advanced legged robots hold balance while doing useful work.

And it works, but you can feel the wall approaching. Every task, every constraint, every priority has to be specified by hand, and the model has to be accurate, and the contacts have to be known. The moment the terrain is unmodeled, the contact is uncertain, or the behavior you want is too subtle to write down as a clean objective (a natural-looking recovery stumble, a parkour leap, a dynamic whole-body throw) hand-designing the objective stack becomes brutally hard, sometimes hopeless. That difficulty is exactly the crack that learning pours into. When you can't hand-write the controller, you can often train one: let the robot try, reward what works, and let reinforcement learning discover the whole-body coordination you couldn't specify. That's Chapter 7, and whole-body control is precisely the frontier where hand-design gets hard enough that learning starts to look not just clever but necessary.

You now own the ladder that turns a commanded angle into an achieved one, each rung a fix for the last. Open-loop commands and hopes, and fails the instant the world deviates. Feedback closes the loop on $e = \text{setpoint} - \text{measurement}$, the error signal everything hangs on. P pushes in proportion to error but parks short against a constant disturbance; I accumulates the past to erase that gap but can wind up; D damps the approach but amplifies noise; PID blends present, past, and future into the workhorse law that runs most of the loops on Earth. Feedforward stops the controller merely reacting and lets it predict, gravity compensation being the classic, giving us the PD-plus-gravity-comp arm controller. Trajectory tracking turns a setpoint into a moving target. Impedance control stops commanding position and commands a force–motion relationship, a virtual spring whose stiffness you dial from rigid to human-safe: Chapter 1's compliant actuator made software. And whole-body control scales all of it to a coordinated, balancing body, right up to the point where it gets too hard to hand-design and learning takes over.

This chapter completes the Act beat of the sense–plan–act loop. Chapter 3 said which joint angles you want; this chapter got you to them despite a world that pushes back. But everything here quietly assumed the robot knows where it is: every controller ate a measurement off a sensor and trusted it. Chapter 1 warned that every measurement lies a little and the lies pile up. So next we turn to Sense: Chapter 5 is estimation, localization, and SLAM, fighting Villain #2's drift by fusing many imperfect sensors into one trustworthy belief about where the robot and its world actually are. A controller is only ever as good as the measurement it's correcting toward. Let's go get an honest one.

Prev: Chapter 3, Kinematics · Next: Chapter 5, Estimation, Localization & SLAM