Trajectories: getting there smoothly rather than instantly

Where you are. You can build a controller that drives a joint to a commanded angle. This lesson is about where that commanded angle comes from, which turns out to matter as much as the controller does.

The lift that does not throw you on the floor

You press 8. The doors close, and for about two seconds the floor firms up under your feet, as if you had gained a little weight. Then it goes quiet. Somewhere near the top the floor softens for a moment, your knees unload slightly, and the doors open.

None of that is the motor being weak. A lift motor could reach full speed in a tenth of a second. It would also put every passenger on the floor and spill every coffee in the car, so nobody builds one that does. The ride you actually get was designed: ease in, hold a steady speed, ease out. The only time you consciously notice a lift is when one of those three parts is done badly.

Your robot has no passengers. It has something worse: a gearbox with backlash in it, a motor with a hard torque limit, a link that flexes, and a block sitting loose in its gripper. It gets the same treatment, for less sentimental reasons.

The idea in one paragraph

A controller answers exactly one question: given the error right now, how hard do I push? It never asks whether the setpoint it was handed was a reasonable thing to want. Hand it a step - be at 90 degrees, now - and you have implicitly asked for infinite speed and infinite acceleration, so the controller does the only thing available to it: maximum torque, saturation, and whatever overshoot follows from that. A trajectory is the missing piece. Instead of one setpoint you feed the controller a sequence of setpoints, one per control tick, that walks from where the arm is to where you want it at a speed and an acceleration the machine can actually deliver. Building one takes two decisions: which space you interpolate in, joint angles or tool position, because those give genuinely different paths through the room; and what velocity profile you follow along the way.

A step is a wish, not a command

Take the PID controller you just wrote, with the gains from the last lesson, and command a 90 degree step. Then command the same 90 degrees spread over 1.5 seconds. Same controller, same plant, same gains; only the shape of the setpoint changes.

The step asks for four times what the motor has, gets three quarters of it thrown away by the clip, and drives the joint through the target at 335 degrees per second. The profile never asks for anything the motor cannot deliver, so the controller stays in the regime where its gains actually mean something.

Which space do you move through?

You want to move the tool from A to B. There are two obvious ways to fill in the middle, and they produce different motions.

Joint space. Interpolate each joint angle independently from its start value to its end value. It is trivially cheap, no inverse kinematics is needed anywhere in the middle, and if both endpoints are reachable then every configuration in between is too, because you never left the space of valid joint angles.

Cartesian space. Interpolate the tool pose, then solve inverse kinematics at every point to find the joint angles that produce it. You get the straight line, which is what welding, wiping a surface, inserting a peg and carrying a full glass all require.

They are not close.

On the two-link arm with links of 1.00 and 0.70, take the tool from (1.45, 0.35) to (-1.45, 0.35). The two configurations have the same elbow angle, −58.33°, because both points are the same distance from the base. So interpolating joint angles means sweeping the shoulder through 152.86° with the elbow held still, and the tool traces a circle of radius 1.4916.

You can check the gap by hand. The circle has radius 1.4916 and the straight line sits at height 0.35, so the arc bulges above it by 1.4916 − 0.35 = 1.1416 metres. The path is also 3.98 metres long against the straight line’s 2.90, thirty-seven percent further travelled for the same two endpoints.

When the straight line is not available

The last two rows of that table are where real systems break.

The same arm has a hole in the middle of its workspace: it cannot reach anything closer than $|L_1 - L_2| = 0.30$ to its own base, because the elbow cannot fold tighter than fully folded. Now take two points that are both comfortably reachable, (0.25, 0.30) and (0.25, -0.30), each 0.39 from the base. Interpolate between them in Cartesian space and 277 of 501 sampled points on that straight line, fifty-five percent of the path, have no joint solution whatsoever. The line passes 0.25 from the base, and there is nothing the solver can do about it.

The joint-space route between the same two points is fine. The elbow angle happens to be the same at both ends again, so sweeping the shoulder carries the tool round an arc of radius 0.39, safely outside the hole, and every point on it is reachable by construction.

There is a second, quieter version of the same problem. A straight line can stay inside the workspace and still pass close to a singularity, where the Jacobian loses a direction. Near one, a perfectly modest tool speed demands enormous joint speed, so the trajectory is geometrically fine and physically impossible. A path validator that checks reachability but not conditioning will pass it, and the arm will hit a velocity limit halfway through and fall behind the trajectory it was given.

How fast along the way

Having chosen the path, you choose the timing. The workhorse is the trapezoidal velocity profile: accelerate at the acceleration limit, cruise at the speed limit, decelerate at the acceleration limit.

Two lines of arithmetic size it. The ramps take $v_{\max}/a_{\max}$ seconds each, and the total is

which says the move takes as long as cruising the whole distance would, plus one ramp’s worth of penalty for having to start and stop. For a 90 degree move with $v_{\max} = 1.5$ rad/s and $a_{\max} = 3$ rad/s², that is 1.5708/1.5 + 1.5/3 = 1.5472 s: half a second speeding up, 0.55 s at speed, half a second slowing down.

If the move is short enough, there is no cruise at all. When $d\,a_{\max} < v_{\max}^{2}$ the joint is still accelerating when it has to start braking, the profile is a triangle, and $T = 2\sqrt{d/a_{\max}}$. With the same 90 degrees and a 3 rad/s speed limit the profile never gets near that limit: it peaks at 2.17 rad/s and the move takes 1.4472 s.

The bill for that speed is at the corners. Acceleration jumps instantly from zero to $a_{\max}$ at the start, and instantly to zero at the end of the ramp. That instantaneous change in acceleration is jerk, and infinite jerk is what excites a real arm’s structural flex and makes it ring after it stops. The fixes are to round the corners, giving an S-curve, or to use a polynomial whose acceleration starts and ends at zero.

Here is what those choices actually cost, for the same distance in the same total time:

Read it the other way round and it becomes a decision. Hold the limits fixed instead of the time, at $v_{\max} = 1.5$ and $a_{\max} = 3$, and ask how long each profile needs for the 90 degree move: the trapezoid takes 1.547 s, the cubic 1.773 s, and the quintic 1.964 s. Smoothness is not free. It costs 15% and 27% of your cycle time respectively, which on a machine doing one move every two seconds all day is the whole argument.

Stopping at every corner is slow

Real tasks are lists of waypoints, not single moves. Run the profile above between each consecutive pair and the arm comes to a complete stop at every waypoint, which is correct, safe, and slow: each stop costs you a full deceleration and acceleration ramp.

The standard remedy is blending. Instead of passing exactly through an intermediate waypoint, the trajectory cuts the corner within a tolerance you specify, and the arm carries its speed through. You trade geometric precision at the waypoint, which usually does not matter because the waypoint was just somewhere on the way, for cycle time, which usually does.

Which gives the shape the milestone project needs, and it is worth writing down now:

• Transit above the workspace: joint space, blended, fast. Nobody cares what path the tool takes through empty air.
• Descend onto the object: Cartesian, straight down, slow. Here the path is the point, because approaching a grasp from the side knocks the object over.
• Lift: Cartesian, straight up, slow, for the same reason in reverse.
• Carry to the bin: joint space again, blended.
• Every waypoint on that list is validated before anything moves.

Check yourself

Do this

Two experiments, about thirty minutes, both on code you already have.

1. Give your controller something reasonable to chase. In code/pid_pendulum.py, with your PID implementation in place, add a moving setpoint and use it:

def setpoint(t, T=1.5):
    s = min(t / T, 1.0)
    return TARGET * (10 * s ** 3 - 15 * s ** 4 + 6 * s ** 5)

then change tau = ctrl.update(TARGET - th) to tau = ctrl.update(setpoint(t) - th). Measure peak speed by adding print(np.degrees(np.abs(np.diff(ths))).max() / DT) after each run.

Predict the numbers before you run it. You should get 335.4°/s for the step and 102.3°/s for the profile, and the peak angle should stop exceeding the target entirely: 92.64° becomes 89.52°. Then try T = 1.0 and T = 2.5 and watch peak speed move to 160.9 and 60.8. One caution when reading the printed settling value: the profiled runs are still creeping up the last half degree at five seconds because the integral is catching up on the lag it built during the move, so set T_END = 8.0 before comparing final angles.

2. Measure the bow yourself. Using solutions/ik_2link.py, take A = (1.45, 0.35) and B = (-1.45, 0.35), solve both with ik_analytic, interpolate the joint angles in 200 steps, and run fk on each. Compare that path against the straight line between the two tool positions.

Confirm three things: the maximum gap is 1.1416, it occurs exactly at the midpoint, and it equals $|A| - 0.35$ to full precision. The third one is the point of the exercise. The deviation is not an empirical curiosity; it is the sagitta of a circular arc, and you could have predicted it before writing any code. Then repeat with A = (1.2, 0.45) and B = (1.2, -0.45) and check whether your closed-form prediction still holds, and if not, work out what is different about that pair.

What you can now do

You can explain why a step command is a violent thing to hand a controller, and quote what it costs in torque demand and peak speed. You can choose between joint-space and Cartesian interpolation on the merits, predict the arc that joint-space interpolation traces when both endpoints sit at the same radius, and say why validating two endpoints tells you nothing about the path between them. You can size a trapezoidal profile from a speed and an acceleration limit, recognise when it degenerates to a triangle, and put a number on what smoothing it costs you in cycle time.