Trajectory generation and the real-time motion pipeline

14.1 The layers

A complete motion pipeline for the VCP6 has five layers, and it is much easier to reason about when they stay separate.

At the top is the task layer: "pick up the object at these coordinates". This is application logic and it lives on the host.

Below that is the path layer, which turns a task into a sequence of poses in space — waypoints, and the type of motion between them.

Below that is the trajectory layer, which adds time. It takes the geometric path and produces a schedule: at time t, the tool should be here, moving at this velocity. This is where speed limits, acceleration limits, and smoothing live.

Below that is the kinematics layer, which converts each sampled point on the trajectory into six joint angles via IK, and then into microstep targets.

And at the bottom is the step generation layer: the fixed-rate ISR from Chapter 3, consuming segments and producing pulses.

The boundary that matters most is between the kinematics layer and the step generator, since that is where soft real time meets hard real time. Everything above it can take variable time and occasionally be slow; everything below it must not miss a deadline. The ring buffer between them absorbs the difference, and its depth is the margin — enough segments buffered that a hiccup upstream does not starve the ISR.

14.2 Joint-space versus Cartesian moves

There are two ways to get from pose A to pose B, and a controller wants both.

A joint-space move interpolates each joint angle linearly from start to finish, scaled so that all joints start and stop together. It is simple, it is fast, it never encounters a singularity, and it uses the arm's full speed capability. What it does not do is move the tool in any predictable path — the tool traces a curve through space that depends on the geometry, and that curve can swing surprisingly wide. Use joint-space moves when the path does not matter, which is most of the time.

A Cartesian move interpolates the tool pose itself, typically along a straight line for position and by spherical linear interpolation for orientation, then runs IK at each sample point. The tool follows a predictable path in space, which is what you need for approaching a part, inserting something, or dispensing along a line. The costs are that it requires IK at every sample, that it can hit singularities mid-path, and that the required joint velocities vary along the path in ways that can exceed limits.

The standard industrial pattern is to use Cartesian moves only for the short, precise segments near the workpiece, and joint-space moves for everything else. Approach a pick point with a fast joint move to a position 50 mm above it, then a slow Cartesian move straight down — faster and safer than doing the whole thing in Cartesian space.

14.3 Velocity profiles

A stepper asked to go instantly from rest to 20,000 steps per second will not go: the rotor cannot accelerate that fast, it falls out of sync with the field, and it stalls or skips. Every move needs a velocity profile.

The trapezoidal profile is the classic: accelerate at a constant rate to a cruise velocity, hold, then decelerate at a constant rate to a stop. Velocity is a trapezoid, acceleration is a square wave. It is simple, optimal in the minimum-time sense given acceleration limits, and what most CNC controllers use.

Its drawback is that acceleration changes instantaneously at the corners of the trapezoid, and the rate of change of acceleration — jerk — is therefore infinite at those points. Infinite jerk excites structural resonance. On a rigid steel machine that is tolerable; on a 3D-printed arm with belt drives and a 400 mm reach it produces a visible wobble at the tool, and the settling time eats whatever the profile gained.

The S-curve profile ramps the acceleration itself, so that acceleration rises smoothly to its maximum, holds, and falls smoothly back. Jerk is bounded. Moves take slightly longer in theory, but on a compliant structure they often finish sooner in practice, because there is nothing to wait for at the end. For the VCP6 I would use S-curve profiles from the start.

The profile applies to the move as a whole, not per joint. Compute the time-optimal profile subject to every joint's velocity and acceleration limits — that is, find the joint that is the binding constraint, profile for that, and scale all the others to match — so that all joints start and stop together and the path is preserved.

14.4 The segment queue

The interface between the planner and the ISR should be a queue of small, fixed-duration segments. Each segment says: for the next N ticks, axis 1 gets this step increment, axis 2 gets this one, and so on, with these direction bits.

Segment duration is a design choice. Something in the region of 1 to 10 milliseconds works well: short enough that velocity changes are smooth, long enough that the planner is not overwhelmed. At 5 ms per segment and a queue depth of 200, you have a full second of buffered motion, which is comfortably more than any upstream hiccup will consume.

Make the queue a lock-free single-producer, single-consumer ring buffer: only the planner writes, only the ISR reads, the head and tail indices are volatile, and no locking is required.

Decide what happens when the queue runs dry. Stopping instantly is a step change in velocity and loses position; decelerating at the configured limit and flagging an underrun does not. Log the underruns — if they happen during normal operation, the planner is too slow or the queue too shallow.

14.5 Emergency stop

Every motion system needs a stop path that nothing else can block.

A controlled stop decelerates at maximum rate and keeps position tracking valid. This is what a software stop command, a soft limit violation, or a communication watchdog timeout should trigger. The arm stops in a controlled way and still knows where it is.

An immediate stop kills the driver enable lines. Position tracking is lost — the arm will coast and may drop under gravity — and re-homing is required. This is what a hardware emergency stop button does, and it should work whether or not the firmware is running: a physical contactor in the motor supply, not a Teensy input pin. The most likely reason you need one is that the software has gone wrong, which is exactly when a software-only stop will not help.

Wire both. Use the controlled stop for everything you can, and keep the immediate stop as the thing that works when nothing else does.