Skip to article frontmatterSkip to article content
Site not loading correctly?

This may be due to an incorrect BASE_URL configuration. See the MyST Documentation for reference.

Chapter 8 — Trajectory Generation

Chapters 5 to 7 answered where: forward kinematics gives the gripper pose for a set of joint angles, the Jacobian relates their velocities, and inverse kinematics finds the joint angles for a desired gripper pose. A task such as pick and place is therefore a list of poses: above the block, at the block, lifted, above the goal, at the goal. This chapter answers the question in between:

Trajectory generation: given the poses (or joint angles) a robot must pass through, and limits on how fast it may move, find the joint angles as a function of time, θ(t)\boldsymbol\theta(t), so that the robot moves smoothly, stays within its limits, and finishes quickly.

Two decisions shape every trajectory. The path is the geometric curve the robot follows: a straight line in joint space, a straight line for the gripper, or a curve through several waypoints. The timing is how fast it moves along that path: when it starts, how quickly it speeds up, and when it stops. Good timing makes the motion smooth and keeps the motors within their limits; good path choices keep the gripper away from the table and from objects.

SectionTopic
8.1Paths, trajectories, and time scaling
8.2Point-to-point polynomials: linear, cubic, quintic
8.3Trapezoidal velocity profiles: the fastest move under speed and acceleration limits
8.4Via points: moving through waypoints without stopping
8.5Straight-line trajectories in task space
8.6From a trajectory to motor commands
8.7Putting it together: a fast pick and place on the SO-ARM101

Notation (same as Chapters 5 to 7). θ\boldsymbol\theta is the vector of joint variables; a single joint is θ\theta. A trajectory runs from θ0\boldsymbol\theta_0 at t=0t = 0 to θf\boldsymbol\theta_f at t=Tt = T, and Δ=θf−θ0\Delta = \theta_f - \theta_0 is the distance one joint travels. Dots are time derivatives: θ˙\dot\theta is velocity, θ¨\ddot\theta acceleration. The formulas work in any units. The one-joint examples use degrees, because they are easier to read; the robot code works in radians and prints degrees, as in Chapter 7. In the code, a joint vector is called q.

Tools and robots from Chapters 5 to 7

The next cell collects what we reuse: the planar 2R arm (L1=1L_1 = 1, L2=0.7L_2 = 0.7) with its forward kinematics, Jacobian and closed-form IK, and the SO-ARM101 with its forward kinematics (the body frames of its MuJoCo model, Chapter 7) and the analytical position + pitch IK so101_ik of Section 7.4.3. Run the cell and move on.

Ready: planar 2R arm (fk_2R, jacobian_2R, ik_2R) and SO-ARM101 (so101.fk, so101_ik).

8.1 Paths, Trajectories, and Time Scaling

A path is a curve through the space of joint angles, written with a path parameter ss that runs from 0 at the start to 1 at the end: θ(s)\boldsymbol\theta(s), s∈[0,1]s \in [0, 1]. The simplest path is the straight line in joint space,

θ(s)=θ0+s (θf−θ0).\boldsymbol\theta(s) = \boldsymbol\theta_0 + s\,(\boldsymbol\theta_f - \boldsymbol\theta_0).

A trajectory adds timing: a time scaling s(t)s(t) says how far along the path the robot is at time tt, with s(0)=0s(0) = 0 and s(T)=1s(T) = 1. The trajectory is θ(t)=θ(s(t))\boldsymbol\theta(t) = \boldsymbol\theta(s(t)), and by the chain rule

θ˙=dθds s˙,θ¨=dθds s¨+d2θds2 s˙2.\dot{\boldsymbol\theta} = \frac{d\boldsymbol\theta}{ds}\,\dot s, \qquad \ddot{\boldsymbol\theta} = \frac{d\boldsymbol\theta}{ds}\,\ddot s + \frac{d^2\boldsymbol\theta}{ds^2}\,\dot s^2 .

On the straight line, dθ/ds=θf−θ0d\boldsymbol\theta/ds = \boldsymbol\theta_f - \boldsymbol\theta_0 and d2θ/ds2=0d^2\boldsymbol\theta/ds^2 = \mathbf{0}, so every joint moves in proportion to the distance it has to travel. Writing Δi=θf,i−θ0,i\Delta_i = \theta_{f,i} - \theta_{0,i} for the distance joint ii travels (the ii-th entry of θf−θ0\boldsymbol\theta_f - \boldsymbol\theta_0),

θ˙i=Δi s˙,θ¨i=Δi s¨.\dot\theta_i = \Delta_i\,\dot s, \qquad \ddot\theta_i = \Delta_i\,\ddot s .

All joints start together, stop together, and reach their peak speed at the same instant. Separating the path from the timing lets us design s(t)s(t) once (Sections 8.2 and 8.3) and use it for any path: a line in joint space here, a line for the gripper in Section 8.5.

Figure 8.1 — Path + timing = trajectory. (a) A straight path in joint space, parameterized by s. (b) A time scaling s(t): the cubic of Section 8.2 (blue) and constant speed (dashed). (c) The path sampled at equal times: close together at the ends, far apart in the middle.

Figure 8.1 puts the two pieces together.

The scaling law. Write the time scaling with the normalized time u=t/T∈[0,1]u = t/T \in [0, 1], as s(t)=s(u)s(t) = s(u). Then s˙=s′(u)/T\dot s = s'(u)/T and s¨=s′′(u)/T2\ddot s = s''(u)/T^2, where ′' is d/dud/du:

Stretching a trajectory in time by a factor kk divides its velocities by kk and its accelerations by k2k^2. To bring a trajectory within a velocity limit vmax⁡v_{\max} and an acceleration limit amax⁡a_{\max}, stretch it by k=max⁡ ⁣(peak speed/vmax⁡, peak acceleration/amax⁡)k = \max\!\big(\text{peak speed}/v_{\max},\ \sqrt{\text{peak acceleration}/a_{\max}}\big).

What makes a good time scaling? At rest before and after the move, so s˙=0\dot s = 0 at both ends. Velocity must be continuous: a jump in velocity needs an infinite acceleration, and the torque a motor must supply grows with acceleration, so a real joint cannot follow a jump and lags behind. Ideally the acceleration is continuous too, because a sudden step in torque shakes the arm. And it should be as fast as the motors allow. Sections 8.2 and 8.3 trade these goals against each other.

The plot below shows the 2R arm on the same straight joint-space path with two time scalings, drawn at nine equally spaced times. The path is identical; only the spacing changes.

<Figure size 1300x500 with 2 Axes>

With constant speed the tip advances equal distances in equal times, from the first instant to the last. The smooth scaling starts gently, covers most of the path in the middle of the move, and slows down into the goal.

Key Takeaways — Section 8.1

  • Path = geometry, θ(s)\boldsymbol\theta(s); trajectory = path + timing, θ(s(t))\boldsymbol\theta(s(t)).

  • On a straight joint-space path, θ˙i=Δis˙\dot\theta_i = \Delta_i\dot s and θ¨i=Δis¨\ddot\theta_i = \Delta_i\ddot s: all joints share one time scaling and finish together.

  • Scaling law: stretching the time by kk divides velocities by kk and accelerations by k2k^2.

  • Goals: start and end at rest, continuous velocity (ideally continuous acceleration), within the limits, fast.

8.2 Point-to-Point Polynomials

8.2.1 Linear interpolation

The simplest time scaling is constant speed, s(u)=us(u) = u:

θ(t)=θ0+tT Δ,θ˙=ΔT,θ¨=0  during the move,\theta(t) = \theta_0 + \frac{t}{T}\,\Delta, \qquad \dot\theta = \frac{\Delta}{T}, \qquad \ddot\theta = 0 \ \text{ during the move,}

where Δ=θf−θ0\Delta = \theta_f - \theta_0 is the distance the joint travels. (From here on, the formulas are for one joint, so we drop the index ii; for the whole arm, apply them to each joint with its own Δi\Delta_i.)

It is what a program does when it simply sends evenly spaced goal positions to a servo. Its velocity jumps from 0 to Δ/T\Delta/T at t=0t = 0 and back to 0 at t=Tt = T, so the acceleration is infinite at those two instants. The motor cannot follow: a position servo falls behind as soon as a linear move starts, trails behind the command for the whole move, and arrives late and still moving (Section 8.6 models this). Linear interpolation is fine for slow, small moves; for anything quick we need a velocity that starts and ends at zero.

8.2.2 Cubic polynomials

Take one joint. We want it to start at θ0\theta_0 with velocity v0v_0 and to reach θf\theta_f with velocity vfv_f at time TT. Four conditions need four free coefficients, so we use a cubic:

θ(t)=a0+a1t+a2t2+a3t3,θ˙(t)=a1+2a2t+3a3t2,θ¨(t)=2a2+6a3t.\theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3, \qquad \dot\theta(t) = a_1 + 2a_2 t + 3a_3 t^2, \qquad \ddot\theta(t) = 2a_2 + 6a_3 t .

The four conditions are four linear equations in the coefficients:

[100001001TT2T3012T3T2]⏟M[a0a1a2a3]=[θ0v0θfvf]←θ(0)=θ0←θ˙(0)=v0←θ(T)=θf←θ˙(T)=vf\underbrace{\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & T & T^2 & T^3 \\ 0 & 1 & 2T & 3T^2 \end{bmatrix}}_{\mathbf{M}} \begin{bmatrix} a_0 \\ a_1 \\ a_2 \\ a_3 \end{bmatrix} = \begin{bmatrix} \theta_0 \\ v_0 \\ \theta_f \\ v_f \end{bmatrix} \qquad \begin{aligned} &\leftarrow \theta(0) = \theta_0 \\ &\leftarrow \dot\theta(0) = v_0 \\ &\leftarrow \theta(T) = \theta_f \\ &\leftarrow \dot\theta(T) = v_f \end{aligned}

The first two rows give a0=θ0a_0 = \theta_0 and a1=v0a_1 = v_0. Substituting them into the last two rows leaves two equations in a2a_2 and a3a_3, with the solution

a2=3Δ−(2v0+vf) TT2,a3=−2Δ+(v0+vf) TT3\boxed{a_2 = \frac{3\Delta - (2v_0 + v_f)\,T}{T^2}, \qquad a_3 = \frac{-2\Delta + (v_0 + v_f)\,T}{T^3}}

Rest to rest (v0=vf=0v_0 = v_f = 0): a0=θ0a_0 = \theta_0, a1=0a_1 = 0, a2=3Δ/T2a_2 = 3\Delta/T^2, a3=−2Δ/T3a_3 = -2\Delta/T^3. To see the time scaling inside this cubic, substitute the coefficients and factor out Δ\Delta:

θ(t)=θ0+3ΔT2 t2−2ΔT3 t3=θ0+Δ[3(tT)2−2(tT)3].\theta(t) = \theta_0 + \frac{3\Delta}{T^2}\,t^2 - \frac{2\Delta}{T^3}\,t^3 = \theta_0 + \Delta\left[3\left(\frac{t}{T}\right)^2 - 2\left(\frac{t}{T}\right)^3\right].

With the normalized time u=t/Tu = t/T (from 0 at the start to 1 at the end), the bracket depends on uu alone. Comparing with the straight path of Section 8.1, θ=θ0+s Δ\theta = \theta_0 + s\,\Delta, the bracket is the time scaling:

s(u)=3u2−2u3,s′(u)=6u(1−u),s′′(u)=6−12u,s(u) = 3u^2 - 2u^3, \qquad s'(u) = 6u(1 - u), \qquad s''(u) = 6 - 12u ,

where ′' is d/dud/du. It satisfies s(0)=0s(0) = 0, s(1)=1s(1) = 1 and s′(0)=s′(1)=0s'(0) = s'(1) = 0, and it is the same for every joint and every move; only Δ\Delta and TT change. To get back to time, use the chain rule with du/dt=1/Tdu/dt = 1/T (Section 8.1):

θ˙=ΔT s′(u),θ¨=ΔT2 s′′(u).\dot\theta = \frac{\Delta}{T}\,s'(u), \qquad \ddot\theta = \frac{\Delta}{T^2}\,s''(u).

The velocity is a parabola, zero at both ends, with its peak at the middle: θ˙max⁡=1.5 Δ/T\dot\theta_{\max} = 1.5\,\Delta/T, 50% faster than linear interpolation, because the cubic makes up for its slow start and finish. The acceleration falls linearly from +6Δ/T2+6\Delta/T^2 to −6Δ/T2-6\Delta/T^2: finite everywhere, but it still jumps from 0 to 6Δ/T26\Delta/T^2 at the start and back to 0 at the end. The velocity is continuous; the acceleration is not.

We keep v0v_0 and vfv_f in the formulas because Section 8.4 strings cubics together through waypoints without stopping, which needs nonzero velocities at the joins. The cell codes the formulas and checks them against solving Ma=b\mathbf{M}\mathbf{a} = \mathbf{b} numerically.

formulas        : [-30.    10.    48.75 -17.5 ]
np.linalg.solve : [-30.    10.    48.75 -17.5 ]

Rest to rest, Δ = 90°, T = 2 s: peak velocity 67.50°/s (1.5 Δ/T = 67.50), acceleration at the ends ±135.0°/s² (6 Δ/T² = 135.0)

Explore the end velocities. The sliders set v0v_0, vfv_f and TT for a 90° move. With small end velocities the cubic is an S-curve. Make v0v_0 large and the joint overshoots θf\theta_f and comes back; make it negative and the joint first moves the wrong way. Large end velocities in a short time are exactly what makes multi-segment trajectories wander (Section 8.4).

Loading...

8.2.3 Quintic polynomials

To remove the jump in acceleration we also fix the accelerations at both ends, which makes six conditions, so we need a quintic (degree 5):

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5,\theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5,

with θ(0)=θ0\theta(0) = \theta_0, θ˙(0)=v0\dot\theta(0) = v_0, θ¨(0)=α0\ddot\theta(0) = \alpha_0, and θ(T)=θf\theta(T) = \theta_f, θ˙(T)=vf\dot\theta(T) = v_f, θ¨(T)=αf\ddot\theta(T) = \alpha_f. As before, the conditions at t=0t = 0 give the first coefficients directly (a0=θ0a_0 = \theta_0, a1=v0a_1 = v_0, a2=α0/2a_2 = \alpha_0/2), and the three conditions at t=Tt = T form a 3×33\times3 linear system for a3,a4,a5a_3, a_4, a_5. For rest to rest with zero end accelerations the solution is

a3=10ΔT3,a4=−15ΔT4,a5=6ΔT5⟺s(u)=10u3−15u4+6u5.a_3 = \frac{10\Delta}{T^3},\quad a_4 = -\frac{15\Delta}{T^4},\quad a_5 = \frac{6\Delta}{T^5} \qquad\Longleftrightarrow\qquad s(u) = 10u^3 - 15u^4 + 6u^5 .

Now the velocity and the acceleration start and end at zero. The price is a higher peak speed, θ˙max⁡=158Δ/T=1.875 Δ/T\dot\theta_{\max} = \tfrac{15}{8}\Delta/T = 1.875\,\Delta/T, at the middle of the move. The peak acceleration is 103Δ/T2≈5.77 Δ/T2\tfrac{10}{\sqrt3}\Delta/T^2 \approx 5.77\,\Delta/T^2, a little lower than the cubic’s, and it occurs at u=12∓36u = \tfrac12 \mp \tfrac{\sqrt3}{6} instead of at the ends.

The pattern continues: the derivative of acceleration, the jerk, now jumps at the ends, and a degree-7 polynomial with eight conditions makes the jerk start and end at zero too. Each step makes the start gentler and the middle faster.

The general solution is easiest to compute numerically from the 6×66\times6 system:

quintic, rest to rest : [  0.      0.      0.    112.5   -84.375  16.875]
closed form           : [  0.      0.      0.    112.5   -84.375  16.875]

8.2.4 Comparing the profiles, and choosing TT

The plot shows the three rest-to-rest time scalings in normalized form: ss, s′s' and s′′s'' against u=t/Tu = t/T. Multiply the velocity axis by Δ/T\Delta/T and the acceleration axis by Δ/T2\Delta/T^2 to get a joint’s actual values.

<Figure size 1600x400 with 3 Axes>
profile     peak velocity   peak acceleration   continuous up to
linear         1.000 Δ/T            infinite   position
cubic          1.500 Δ/T          6.000 Δ/T²   velocity
quintic        1.875 Δ/T          5.773 Δ/T²   acceleration

Choosing TT from the motor limits. Suppose a joint may move at most vmax⁡v_{\max} and accelerate at most amax⁡a_{\max}. A rest-to-rest polynomial with peak values cv Δ/Tc_v\,\Delta/T and ca Δ/T2c_a\,\Delta/T^2 stays within both limits if

T ≥ Tmin⁡=max⁡ ⁣(cv ∣Δ∣vmax⁡, ca ∣Δ∣amax⁡),(cv,ca)=(1.5, 6) for the cubic, (1.875, 5.77) for the quintic.T \ \ge\ T_{\min} = \max\!\left(\frac{c_v\,|\Delta|}{v_{\max}},\ \sqrt{\frac{c_a\,|\Delta|}{a_{\max}}}\right), \qquad (c_v, c_a) = (1.5,\ 6) \text{ for the cubic},\ (1.875,\ 5.77) \text{ for the quintic}.

Several joints. All joints share one time scaling (Section 8.1), so the joint that needs the most time sets TT for everyone; the others move more slowly than their limits allow, and all arrive together.

Example limits. For the SO-ARM101 we use vmax⁡=180∘/sv_{\max} = 180^\circ/\text{s} and amax⁡=720∘/s2a_{\max} = 720^\circ/\text{s}^2 for every arm joint throughout this chapter. These are illustrative values. The real limits depend on the servos, the supply voltage, the load, and how firmly the gripper holds the block, and are best measured on the robot: the pan joint of one 12 V SO-ARM101, for example, reached about 200°/s and 2,000°/s². The example move goes from 6 cm above a pick spot at (x,y)=(20,−12)(x, y) = (20, -12) cm to 6 cm above a place spot at (20,0)(20, 0) cm, with the gripper pointing straight down.

joint             Δ (°)   T_min cubic (s)   T_min quintic (s)
shoulder_pan      -36.7             0.553               0.542
shoulder_lift     -20.4             0.412               0.404
elbow_flex         24.0             0.447               0.439
wrist_flex         -3.6             0.174               0.171
wrist_roll          0.0             0.000               0.000

All joints together with one cubic: T = 0.553 s, set by shoulder_pan

shoulder_pan has the farthest to go, so it sets TT; the other joints move only a few degrees and use a small part of their limits. Its Tmin⁡T_{\min} comes from the acceleration limit, not the speed limit: for moves shorter than cavmax⁡2/(cv2amax⁡)c_a v_{\max}^2 / (c_v^2 a_{\max}) (120° for the cubic with these limits) the square root wins.

Key Takeaways — Section 8.2

  • Linear interpolation: constant speed Δ/T\Delta/T, but velocity jumps at both ends (infinite acceleration).

  • Cubic: four conditions (θ0,v0,θf,vf)(\theta_0, v_0, \theta_f, v_f); a2=[3Δ−(2v0+vf)T]/T2a_2 = [3\Delta - (2v_0 + v_f)T]/T^2, a3=[−2Δ+(v0+vf)T]/T3a_3 = [-2\Delta + (v_0 + v_f)T]/T^3. Rest to rest: s=3u2−2u3s = 3u^2 - 2u^3, peak velocity 1.5Δ/T1.5\Delta/T, acceleration ±6Δ/T2\pm 6\Delta/T^2 at the ends (it jumps there).

  • Quintic: six conditions, adds the end accelerations; rest to rest s=10u3−15u4+6u5s = 10u^3 - 15u^4 + 6u^5, peak velocity 1.875Δ/T1.875\Delta/T, acceleration continuous.

  • Choose TT from the limits: Tmin⁡=max⁡(cv∣Δ∣/vmax⁡, ca∣Δ∣/amax⁡)T_{\min} = \max(c_v|\Delta|/v_{\max},\ \sqrt{c_a|\Delta|/a_{\max}}); with several joints the slowest joint sets TT.

8.3 Trapezoidal Velocity Profiles

8.3.1 Accelerate, cruise, decelerate

The polynomials of Section 8.2 never use their full speed or acceleration for long: the cubic reaches its peak speed only for an instant, and its acceleration falls steadily. A trapezoidal velocity profile uses both limits as much as possible. It has three phases:

  1. accelerate at constant aa for a time tb=v/at_b = v/a, until the speed reaches vv;

  2. cruise at constant speed vv;

  3. decelerate at −a-a for tbt_b, coming to rest at θf\theta_f.

The velocity is a trapezoid and the position is a parabola, a straight line, and a parabola, so the profile is also called linear segment with parabolic blends (LSPB). The area under the velocity is the distance travelled, Δ=v (T−tb)\Delta = v\,(T - t_b), so

T=∣Δ∣v+va,tb=va.T = \frac{|\Delta|}{v} + \frac{v}{a}, \qquad t_b = \frac{v}{a}.

During the move (for Δ>0\Delta > 0):

θ(t)={θ0+12at20≤t<tbθ0+v (t−tb2)tb≤t≤T−tbθf−12a (T−t)2T−tb<t≤T\theta(t) = \begin{cases} \theta_0 + \tfrac12 a t^2 & 0 \le t < t_b \\ \theta_0 + v\,(t - \tfrac{t_b}{2}) & t_b \le t \le T - t_b \\ \theta_f - \tfrac12 a\,(T - t)^2 & T - t_b < t \le T \end{cases}

The cruise phase exists only if 2tb≤T2t_b \le T, that is v2≤a∣Δ∣v^2 \le a|\Delta|. A move too short to reach vv becomes a triangle: accelerate for half the time, decelerate for the other half, peak speed a∣Δ∣\sqrt{a|\Delta|} and T=2∣Δ∣/aT = 2\sqrt{|\Delta|/a}.

Sometimes TT is given instead (for example, to make one joint finish together with another). Fix the acceleration aa and solve Δ=a tb (T−tb)\Delta = a\,t_b\,(T - t_b) for the blend time:

tb=T−T2−4∣Δ∣/a2,possible only if aT2≥4∣Δ∣.t_b = \frac{T - \sqrt{T^2 - 4|\Delta|/a}}{2}, \qquad \text{possible only if } aT^2 \ge 4|\Delta| .

Figure 8.2 — Anatomy of a trapezoidal velocity profile. (a) Velocity: accelerate at a for tb, cruise at v, decelerate; the area under the curve is the distance Δ. The dashed curve is a cubic with the same Δ and T. (b) Position: parabola, straight line, parabola.

<Figure size 1500x380 with 3 Axes>
Δ = 90°: T = 0.750 s, cruise at 180°/s, blend time t_b = 0.250 s; check: end position 90.0000°

8.3.2 The fastest move under speed and acceleration limits

With v=vmax⁡v = v_{\max} and a=amax⁡a = a_{\max} the trapezoid is the fastest possible rest-to-rest move within the two limits. The reason: to cover Δ\Delta as quickly as possible the joint must be as fast as possible at every instant. It cannot speed up faster than amax⁡a_{\max}, cannot cruise faster than vmax⁡v_{\max}, and must still be able to stop in time, which it does only by braking at amax⁡a_{\max} at the last possible moment. The trapezoid does exactly that. This “full acceleration, full speed, full braking” structure is called bang-coast-bang.

The cell compares the shortest time of each profile for several move sizes, with the example limits of Section 8.2.4.

Shortest rest-to-rest time (s) with v_max = 180°/s, a_max = 720°/s²

 Δ (°)  trapezoid    cubic  quintic   trapezoid saves
    10      0.236    0.289    0.283    18.4% vs cubic
    20      0.333    0.408    0.400    18.4% vs cubic
    45      0.500    0.612    0.601    18.4% vs cubic
    90      0.750    0.866    0.938    13.4% vs cubic
   180      1.250    1.500    1.875    16.7% vs cubic
<Figure size 1400x400 with 2 Axes>

The trapezoid is 13% to 18% faster than the cubic over this range, and more than that against the quintic for long moves: it rides along both limits, while the polynomials touch each limit only at one instant.

Smoothness. The trapezoid’s acceleration jumps four times (at the start and end of each blend), so like the cubic it has continuous velocity but not continuous acceleration. Industrial drives often use an S-curve profile, which also limits the jerk by ramping the acceleration up and down linearly; it is a trapezoid of acceleration inside a smooth velocity profile.

Servos do this too. Sent a single goal angle, a smart servo such as the STS3215 of the SO-ARM101 runs its own trapezoidal velocity profile toward the goal. Section 8.6 discusses how that built-in profile interacts with the trajectories of this chapter.

8.3.3 Synchronizing several joints

If each joint ran its own fastest trapezoid, the joints would finish at different times and the arm would not follow the straight joint-space path. Instead, give all joints one trapezoidal time scaling s(t)s(t). Since θ˙i=Δi s˙\dot\theta_i = \Delta_i\,\dot s and θ¨i=Δi s¨\ddot\theta_i = \Delta_i\,\ddot s, every joint stays within its limits if

s˙≤s˙max⁡=min⁡ivmax⁡,i∣Δi∣,s¨≤s¨max⁡=min⁡iamax⁡,i∣Δi∣,\dot s \le \dot s_{\max} = \min_i \frac{v_{\max,i}}{|\Delta_i|}, \qquad \ddot s \le \ddot s_{\max} = \min_i \frac{a_{\max,i}}{|\Delta_i|},

and the fastest such s(t)s(t) is the trapezoid from 0 to 1 with limits s˙max⁡\dot s_{\max} and s¨max⁡\ddot s_{\max}. The joint with the largest ∣Δi∣|\Delta_i| (relative to its limits) uses its full speed and acceleration; the others scale down in proportion.

<Figure size 1400x400 with 2 Axes>
Synchronized trapezoid: T = 0.451 s   (one cubic within the same limits: 0.553 s)

On the left, the joints finish at different times, so the arm leaves the straight joint-space path between the two poses; the gripper’s pitch, −(θ2+θ3+θ4)-(\theta_2 + \theta_3 + \theta_4), changes during the move because the three joints no longer move in step. On the right, every joint follows the same profile shape, scaled by its distance, and they all arrive together, in the time the pan joint needs anyway.

Profiles(u)s(u)Peak velocityPeak accelerationContinuous up toUse
LinearuuΔ/T\Delta/Tinfinite (ends)positionslow moves only
Cubic3u2−2u33u^2 - 2u^31.5 Δ/T1.5\,\Delta/T6 Δ/T26\,\Delta/T^2 (ends)velocitysimple and smooth; also passes through points with given velocities (Section 8.4)
Quintic10u3−15u4+6u510u^3 - 15u^4 + 6u^51.875 Δ/T1.875\,\Delta/T5.77 Δ/T25.77\,\Delta/T^2accelerationgentlest start and stop
Trapezoidaccelerate, cruise, deceleratevv (e.g. vmax⁡v_{\max})aa (e.g. amax⁡a_{\max})velocityfastest within vmax⁡v_{\max}, amax⁡a_{\max}

Key Takeaways — Section 8.3

  • Trapezoid (LSPB): accelerate at aa for tb=v/at_b = v/a, cruise at vv, decelerate; T=∣Δ∣/v+v/aT = |\Delta|/v + v/a; a triangle if v2>a∣Δ∣v^2 > a|\Delta|.

  • With v=vmax⁡v = v_{\max} and a=amax⁡a = a_{\max} it is the fastest rest-to-rest move within the limits (bang-coast-bang), about 15% faster than the cubic.

  • For a given TT: tb=(T−T2−4∣Δ∣/a)/2t_b = \big(T - \sqrt{T^2 - 4|\Delta|/a}\big)/2.

  • Synchronize joints with one time scaling: s˙max⁡=min⁡ivmax⁡,i/∣Δi∣\dot s_{\max} = \min_i v_{\max,i}/|\Delta_i|, s¨max⁡=min⁡iamax⁡,i/∣Δi∣\ddot s_{\max} = \min_i a_{\max,i}/|\Delta_i|.

8.4 Via Points: Moving Through Waypoints Without Stopping

A pick and place visits a sequence of waypoints: above the block, down to the block (close the gripper), back up, over to above the goal, down (open the gripper), and up again. With a rest-to-rest move between each pair, the arm stops at every waypoint, and every stop costs time: the arm must slow down to zero and speed up again.

Only some waypoints need a stop. At the block and at the goal the arm must be at rest while the gripper closes or opens. The points above them are there to shape the path, so that the block is lifted clear of the table and lowered from above; the arm only needs to pass through or near them. These are via points. This section gives two ways to pass via points without stopping:

  • Cubic segments with nonzero velocities at the via points (Section 8.4.1): the trajectory passes exactly through every point.

  • Linear segments with parabolic blends (Section 8.4.2): the trapezoid of Section 8.3, extended to many points; the trajectory passes near the via points and rounds the corners.

Figure 8.3 — Two ways through via points. (a) Cubic segments with a velocity vk at each via point pass exactly through every point; a joint that turns back gets v = 0. (b) Linear segments with parabolic blends (shaded) start and stop exactly at the end points but miss each via point by δ = (Vk − Vk−1) tb/8.

8.4.1 Cubic segments with velocities at the via points

Let the waypoints be θ0,θ1,…,θn\theta_0, \theta_1, \dots, \theta_n for one joint: the start, n−1n - 1 via points, and the goal. Segment kk (k=0,…,n−1k = 0, \dots, n-1) runs from θk\theta_k to θk+1\theta_{k+1} in time TkT_k, with Δk=θk+1−θk\Delta_k = \theta_{k+1} - \theta_k, and each segment is a cubic of Section 8.2.2.

Count the unknowns and the conditions. nn cubic segments have 4n4n coefficients. The conditions we certainly want are:

ConditionEquations
each segment starts at θk\theta_k and ends at θk+1\theta_{k+1}2n2n
the joint starts and ends at rest2
the velocity is continuous at each via pointn−1n - 1
total3n+13n + 1

That leaves 4n−(3n+1)=n−14n - (3n + 1) = n - 1 conditions to choose: one per via point. A convenient way to see them is to name the velocity at each waypoint, v0,v1,…,vnv_0, v_1, \dots, v_n, and build segment kk as the cubic from (θk,vk)(\theta_k, v_k) to (θk+1,vk+1)(\theta_{k+1}, v_{k+1}) with the formulas of Section 8.2.2. Then the first 3n+13n + 1 conditions hold automatically: each segment hits its two waypoints, v0=vn=0v_0 = v_n = 0 gives the rest conditions, and neighbouring segments share vkv_k, so the velocity is continuous. The only unknowns left are the n−1n - 1 via velocities v1,…,vn−1v_1, \dots, v_{n-1}, and we need n−1n - 1 equations for them. The two rules below supply those equations in different ways.

Rule 1: average the slopes (one explicit formula per via point). Let mk=Δk/Tkm_k = \Delta_k / T_k be the average speed of segment kk. At via point kk,

vk={12 (mk−1+mk)if mk−1 and mk have the same sign,0otherwise.v_k = \begin{cases} \tfrac12\,(m_{k-1} + m_k) & \text{if } m_{k-1} \text{ and } m_k \text{ have the same sign,} \\ 0 & \text{otherwise.} \end{cases}

This gives the n−1n - 1 equations directly, each via velocity on its own, so nothing has to be solved. A joint that keeps moving the same way passes through at about its average speed; a joint that turns back stops at its turning point. Nothing in it asks for continuous acceleration, so the acceleration jumps at the via points.

Rule 2: continuous acceleration (the cubic spline). Use the n−1n - 1 free conditions to make the acceleration continuous at every via point. From the coefficients of Section 8.2.2, a cubic segment with end velocities v0,vfv_0, v_f has the accelerations

θ¨(0)=6Δ−(4v0+2vf) TT2,θ¨(T)=−6Δ+(2v0+4vf) TT2\ddot\theta(0) = \frac{6\Delta - (4v_0 + 2v_f)\,T}{T^2}, \qquad \ddot\theta(T) = \frac{-6\Delta + (2v_0 + 4v_f)\,T}{T^2}

at its two ends. (They follow from θ¨(t)=2a2+6a3t\ddot\theta(t) = 2a_2 + 6a_3 t: θ¨(0)=2a2\ddot\theta(0) = 2a_2 and θ¨(T)=2a2+6a3T\ddot\theta(T) = 2a_2 + 6a_3T, with a2a_2, a3a_3 from Section 8.2.2.)

At via point kk, two segments meet. The segment arriving is segment k−1k-1: it has Δk−1\Delta_{k-1} and Tk−1T_{k-1}, starts with vk−1v_{k-1} and ends with vkv_k. The segment leaving is segment kk: it has Δk\Delta_k and TkT_k, starts with vkv_k and ends with vk+1v_{k+1}. Their accelerations at the via point are

θ¨k−1(Tk−1)=−6Δk−1+(2vk−1+4vk) Tk−1Tk−12,θ¨k(0)=6Δk−(4vk+2vk+1) TkTk2.\ddot\theta_{k-1}(T_{k-1}) = \frac{-6\Delta_{k-1} + (2v_{k-1} + 4v_k)\,T_{k-1}}{T_{k-1}^2}, \qquad \ddot\theta_k(0) = \frac{6\Delta_k - (4v_k + 2v_{k+1})\,T_k}{T_k^2}.

Set them equal and split each fraction:

−6Δk−1Tk−12+2vk−1+4vkTk−1=6ΔkTk2−4vk+2vk+1Tk.-\frac{6\Delta_{k-1}}{T_{k-1}^2} + \frac{2v_{k-1} + 4v_k}{T_{k-1}} = \frac{6\Delta_k}{T_k^2} - \frac{4v_k + 2v_{k+1}}{T_k}.

Move the velocities to the left and the Δ\Delta terms to the right:

2vk−1Tk−1+4vkTk−1+4vkTk+2vk+1Tk=6Δk−1Tk−12+6ΔkTk2.\frac{2v_{k-1}}{T_{k-1}} + \frac{4v_k}{T_{k-1}} + \frac{4v_k}{T_k} + \frac{2v_{k+1}}{T_k} = \frac{6\Delta_{k-1}}{T_{k-1}^2} + \frac{6\Delta_k}{T_k^2}.

Divide by 2 and group the two vkv_k terms. This gives one equation at each via point k=1,…,n−1k = 1, \dots, n-1:

vk−1Tk−1+2(1Tk−1+1Tk)vk+vk+1Tk=3(Δk−1Tk−12+ΔkTk2)\boxed{\frac{v_{k-1}}{T_{k-1}} + 2\left(\frac{1}{T_{k-1}} + \frac{1}{T_k}\right) v_k + \frac{v_{k+1}}{T_k} = 3\left(\frac{\Delta_{k-1}}{T_{k-1}^2} + \frac{\Delta_k}{T_k^2}\right)}

That is n−1n - 1 linear equations for the n−1n - 1 unknowns v1,…,vn−1v_1, \dots, v_{n-1} (v0=vn=0v_0 = v_n = 0 are known), so the count closes: 4n4n conditions for 4n4n coefficients, and the solution is unique. Unlike Rule 1, the via velocities are coupled: each equation involves a via velocity and its two neighbours, so they must be solved together. The matrix is tridiagonal. For the five waypoints used below (n=4n = 4 segments, 16 coefficients, 3 via points):

[2(1T0+1T1)1T101T12(1T1+1T2)1T201T22(1T2+1T3)][v1v2v3]=3[Δ0/T02+Δ1/T12Δ1/T12+Δ2/T22Δ2/T22+Δ3/T32]\begin{bmatrix} 2\left(\frac{1}{T_0} + \frac{1}{T_1}\right) & \frac{1}{T_1} & 0 \\ \frac{1}{T_1} & 2\left(\frac{1}{T_1} + \frac{1}{T_2}\right) & \frac{1}{T_2} \\ 0 & \frac{1}{T_2} & 2\left(\frac{1}{T_2} + \frac{1}{T_3}\right) \end{bmatrix} \begin{bmatrix} v_1 \\ v_2 \\ v_3 \end{bmatrix} = 3\begin{bmatrix} \Delta_0/T_0^2 + \Delta_1/T_1^2 \\ \Delta_1/T_1^2 + \Delta_2/T_2^2 \\ \Delta_2/T_2^2 + \Delta_3/T_3^2 \end{bmatrix}

(the terms with v0v_0 and v4v_4 drop out because both are zero). The solution is the cubic spline through the waypoints. Among all trajectories that pass through the waypoints at the given times and start and end at rest, the spline has the smallest ∫θ¨2 dt\int \ddot\theta^2\,dt: in that sense it is the smoothest.

UnknownsEquationsHow they are found
Stop at every pointnone—v1=⋯=vn−1=0v_1 = \dots = v_{n-1} = 0
Rule 1: average slopesn−1n - 1 via velocitiesn−1n - 1, one per via pointeach computed on its own
Rule 2: cubic splinen−1n - 1 via velocitiesn−1n - 1, one per via pointone tridiagonal linear system

The cell compares stopping at every waypoint, Rule 1 and Rule 2 for one joint and five waypoints.

method                   via velocities (°/s)          peak speed  peak accel.  largest accel. jump
stop at every point      [0. 0. 0.]                        60.0°/s     240.0°/s²             400.0°/s²
Rule 1: average slopes   [40.  0.  0.]                     60.0°/s     239.8°/s²             146.2°/s²
Rule 2: cubic spline     [ 64.3 -21.4 -11.5]               65.8°/s     151.1°/s²               0.0°/s²
<Figure size 1600x400 with 3 Axes>

Stopping at every point brings the velocity to zero at each via point. Rule 1 keeps moving through 40°, where the joint continues upward, and stops at 100° and at 60°, where it turns back. The spline keeps the acceleration continuous (no jumps at the vertical lines) and has by far the lowest peak acceleration, but it runs through every via point with some velocity, even where the joint turns back, so it overshoots: it climbs past 100° before coming down, and dips below 60° before rising to 70°. We will see what that means in space shortly.

8.4.2 Linear segments with parabolic blends

The trapezoid of Section 8.3 is a straight line in time with a parabola at each end. With more waypoints, keep that idea: move along each segment at constant velocity Vk=Δk/TkV_k = \Delta_k / T_k, and around each waypoint replace the corner between two segments by a parabolic blend, a period tb,kt_{b,k} of constant acceleration (Vk−Vk−1)/tb,k(V_k - V_{k-1})/t_{b,k} centred on the waypoint time. At the first and last waypoints the velocity before (after) the motion is zero, so those blends are exactly the accelerate and decelerate phases of a trapezoid.

The trajectory starts exactly at θ0\theta_0 and stops exactly at θn\theta_n, but it does not pass through the via points. In the middle of blend kk it is off by

δk=(Vk−Vk−1) tb,k8,\delta_k = \frac{(V_k - V_{k-1})\,t_{b,k}}{8},

because during the half-blend before the waypoint it is still on the incoming line but has started to turn. With the shortest blend the acceleration limit allows, tb,k=∣Vk−Vk−1∣/amax⁡t_{b,k} = |V_k - V_{k-1}|/a_{\max}, the miss is δk=(Vk−Vk−1)2/(8amax⁡)\delta_k = (V_k - V_{k-1})^2/(8a_{\max}): a fast change of direction with a low acceleration limit cuts the corner the most.

Planning from the limits. For several joints:

  1. Cruise as fast as the slowest joint allows: Tk=max⁡i∣Δk,i∣/vmax⁡,iT_k = \max_i |\Delta_{k,i}| / v_{\max,i}.

  2. Blend as quickly as the slowest joint allows, with one blend time per waypoint for all joints: tb,k=max⁡i∣Vk,i−Vk−1,i∣/amax⁡,it_{b,k} = \max_i |V_{k,i} - V_{k-1,i}| / a_{\max,i}. Because all joints blend together, the corner of the joint-space path is replaced by one smooth curve.

  3. The two blends of a segment must not overlap, (tb,k+tb,k+1)/2≤Tk(t_{b,k} + t_{b,k+1})/2 \le T_k. If they do, the segment is too short to reach its cruise speed: lengthen TkT_k (which lowers VkV_k and shortens the blends) until they fit.

The total time is ∑kTk+(tb,0+tb,n)/2\sum_k T_k + (t_{b,0} + t_{b,n})/2. Industrial robot languages offer the same choice for every waypoint: an exact stop, or a “fly-by” point that the robot only passes near (ABB calls the allowed distance a zone, KUKA approximate positioning, Universal Robots a blend radius).

<Figure size 1400x400 with 2 Axes>
Total time 2.002 s, peak speed 90.0°/s, peak acceleration 360.0°/s²

via point  θ_k (°)  at the blend centre (°)    miss   (V_k - V_k-1) t_b / 8
        1     40.0                    40.00    0.00                    0.00
        2    100.0                    89.39  -10.61                  -10.61
        3     60.0                    65.62    5.62                    5.62

The dashed polyline is the constant-velocity reference: each waypoint is drawn at its waypoint time, tkt_k, shifted by half the first blend so the motion starts at t=0t = 0. The trajectory starts exactly at the first point and stops exactly at the last one; its first and last blends are the trapezoid’s acceleration and deceleration. In between it cruises at the 90°/s limit on the long segments and rounds every corner with a 360°/s² blend. Via point 1 is not a corner at all (both neighbouring segments run at the speed limit, upward), so it is passed exactly. At the other via points the miss is exactly (Vk−Vk−1)tb,k/8(V_k - V_{k-1})t_{b,k}/8: at the 100° peak the joint turns around more than 10° early, the price of not stopping there.

8.4.3 Comparing the methods on a 2R arm

Now the shape in space. The planar 2R arm works in a vertical plane, with its base at the origin and a table at y=0.05y = 0.05 m. It must take a block from A=(1.35,0.1)A = (1.35, 0.1) to B=(0.55,0.1)B = (0.55, 0.1), lifting it through the via points A′=(1.35,0.7)A' = (1.35, 0.7) above AA and B′=(0.55,0.7)B' = (0.55, 0.7) above BB. The waypoints are converted to joint angles with ik_2R on the elbow-up branch (elbow=-1), which keeps the elbow well above the table, and each method interpolates the joint angles. The cubic methods use the segment durations 1.0, 1.5 and 1.0 s; the blends are planned from the limits vmax⁡=2v_{\max} = 2 rad/s and amax⁡=6a_{\max} = 6 rad/s².

<Figure size 1500x550 with 2 Axes>
Time to run the same waypoints within v_max = 2.0 rad/s and a_max = 6.0 rad/s²:
  stop at every point (fastest trapezoids)    2.11 s
  stop at every point (cubics)                2.99 s
  Rule 1: average slopes                      2.99 s
  Rule 2: cubic spline                        2.65 s
  parabolic blends                            1.59 s

Three lessons:

  1. Smoothest in time is not best in space. The spline has the smoothest joint motion, yet its tip path swings far above the via points (to y≈1.09y \approx 1.09 m) and out past AA near the pick point, where a neighbouring object would be hit. Rule 1 bulges less. The parabolic blends rise no higher than stopping at every point (both reach y≈0.77y \approx 0.77 m on the carry), but they cut the corners hard: with these limits the tip passes 8.6 cm from A′A' and 17.7 cm from B′B' (the explorer below prints these distances).

  2. Even “stop at every point” is not straight. All four methods interpolate joint angles, so the vertical lifts and descents come out curved, most of all near BB, close to the base (Section 8.5 explains why and shows how to get straight lines).

  3. Not stopping is faster. Within the same limits, the blends need about a quarter less time than the fastest stop-at-every-point trapezoids (1.59 s vs 2.11 s), because they keep moving through the corners and cruise at the speed limit. The cubic methods are slower still: their hand-picked segment durations, stretched by the scaling law until the limits hold, do not use the limits well.

Whatever the method, check the path with forward kinematics before running it, as we did here: plot the gripper path and compare it with the table and the objects around it.

Explore the blends. The sliders set the limits. Lower the acceleration limit and watch the corners round off more; raise the speed limit and the corners round off more too, because faster segments need longer blends to turn.

Loading...

8.4.4 Where the robot must stop

Not stopping saves time, but some waypoints still need the arm at rest:

  • Grasp and release. The gripper closes or opens while the arm holds still; a block grabbed on the move slides or tips.

  • Contact and precise placement. Setting a block on a stack, or anything that touches, ends at rest, ideally with a short, slow final approach.

  • Every other waypoint (above the pick, above the goal, points that steer around obstacles) can be a via point.

When a waypoint becomes a via point, the path near it changes: blends cut the corner (Section 8.4.2) and cubics can bulge (Section 8.4.3). If the via point was there to give clearance, for example to lift a block 6 cm before moving sideways, raise it by the amount the corner is cut, or plan the blend with a smaller speed or a larger acceleration, and check the result with forward kinematics.

Key Takeaways — Section 8.4

  • Stop only where the task needs it (grasp, release, contact); pass every other waypoint as a via point.

  • Cubic segments with via velocities pass exactly through the points with continuous velocity. Rule 1: average the neighbouring slopes, or 0 where the joint turns back. Rule 2 (cubic spline): a tridiagonal system that also makes the acceleration continuous.

  • Linear segments with parabolic blends: cruise between points, blend around them; the miss at a via point is (Vk−Vk−1)tb/8(V_k - V_{k-1})t_b/8, or ΔV2/(8amax⁡)\Delta V^2/(8a_{\max}) with the shortest blend. Plan TkT_k from vmax⁡v_{\max} and tbt_b from amax⁡a_{\max}, with one blend time for all joints.

  • Smoothest in time is not best in space: always check the gripper path with forward kinematics.

8.5 Straight-Line Trajectories in Task Space

8.5.1 Joint space is not task space

Every trajectory so far interpolates joint angles, and the gripper follows whatever curve the forward kinematics makes of them. Section 8.4.3 showed curved “vertical” lifts on the 2R arm. On the SO-ARM101, the same happens for the two moves of a pick and place, with the gripper pointing straight down:

  • the descent from 6 cm above the pick spot down to the grasp height, and

  • the carry from 6 cm above the pick spot to 6 cm above the place spot.

Figure 8.4 — SO-ARM101 seen from above. (a) Interpolating the joints swings the gripper in an arc around the pan axis (blue); a straight line needs IK at every sample (red). (b) The same gripper speed v on lines closer to the pan axis needs a faster pan joint: θ̇1,max = v/d (Section 8.5.3).

Figure 8.4(a) shows the carry. The cell runs both moves as joint-space cubics and measures how far the gripper point strays from the straight line between the ends.

joint-space descent : the gripper point strays up to   3.0 mm from the straight line
joint-space carry   : the gripper point strays up to  11.3 mm from the straight line
<Figure size 1400x480 with 2 Axes>

The carry swings in an arc around the pan axis: interpolating the joints keeps the reach roughly constant while the pan joint turns. The descent drifts a few millimetres sideways and comes back. A few millimetres matter when the fingers must come down around a 1-inch block with a few millimetres to spare.

8.5.2 Interpolating the gripper position

To move the gripper along a straight line, interpolate the gripper position and let the inverse kinematics find the joint angles at every instant:

p(t)=p0+s(t) (pf−p0),θ(t)=IK(p(t)).\mathbf{p}(t) = \mathbf{p}_0 + s(t)\,(\mathbf{p}_f - \mathbf{p}_0), \qquad \boldsymbol\theta(t) = \mathrm{IK}\big(\mathbf{p}(t)\big).

Any time scaling of Sections 8.2 and 8.3 can be used for s(t)s(t). The orientation is interpolated the same way. For the SO-ARM101 that is just the pitch, φ(t)=φ0+s(t)(φf−φ0)\varphi(t) = \varphi_0 + s(t)(\varphi_f - \varphi_0) (constant −90∘-90^\circ here). For a full orientation, turn about one fixed axis: if (k^,α)(\hat{\mathbf{k}}, \alpha) is the axis-angle form (Section 4.3.3) of the rotation R0⊤Rf\mathbf{R}_0^\top\mathbf{R}_f from the start to the goal, use R(t)=R0 Rk^(s(t) α)\mathbf{R}(t) = \mathbf{R}_0\,\mathbf{R}_{\hat{\mathbf{k}}}\big(s(t)\,\alpha\big), which is the same as quaternion SLERP.

Three checks, at every sample, not just at the ends:

  1. Reachable: every point of the line must be inside the workspace.

  2. Within the joint limits.

  3. Same branch: the IK must keep choosing the same solution (elbow up, facing the target). Switching branches mid-line makes the joints jump.

descent  joint space    off the line   3.05 mm   peak joint speeds (°/s):    0.0    6.3   30.2   36.6
descent  straight line  off the line   0.00 mm   peak joint speeds (°/s):    0.0    9.8   32.1   36.5
carry    joint space    off the line  11.27 mm   peak joint speeds (°/s):   55.0   30.6   36.0    5.4
carry    straight line  off the line   0.00 mm   peak joint speeds (°/s):   57.7   39.6   47.8    9.3

Both straight lines are straight to within a hundredth of a millimetre (what remains is the 0.01 mm sideways offset that Section 7.4.5 removes). For the same 1-second carry the straight line asks more of the shoulder and elbow, which must pull the gripper in toward the base and push it out again instead of letting it swing around in an arc.

8.5.3 Joint speeds along a line, and choosing TT

Differentiating p(t)\mathbf{p}(t) gives the gripper velocity p˙=(pf−p0) s˙\dot{\mathbf{p}} = (\mathbf{p}_f - \mathbf{p}_0)\,\dot s, and the joint velocities follow from the inverse velocity relation of Section 6.6.1,

θ˙=J−1(θ) p˙.\dot{\boldsymbol\theta} = \mathbf{J}^{-1}(\boldsymbol\theta)\,\dot{\mathbf{p}} .

A straight-line trajectory is the resolved-rate motion of Chapter 6 with a time-scaled velocity. Near a singularity J\mathbf{J} is nearly singular, and even a slow gripper needs very fast joints (Path B in Section 6.6.1). For the SO-ARM101 there is a simple special case: a horizontal line that passes at distance dd from the pan axis, travelled at speed vv, turns the pan joint at

θ˙1=v dr2⟹∣θ˙1∣max⁡=vd  at the closest point (r=d).\dot\theta_1 = \frac{v\,d}{r^2} \quad\Longrightarrow\quad |\dot\theta_1|_{\max} = \frac{v}{d}\ \text{ at the closest point } (r = d).

Lines that pass close to the base need a fast pan joint (Figure 8.4(b)). The cell checks the formula on three parallel 20 cm lines, each run in 1 s, 3 cm above the grasp height.

 distance d (cm)  peak gripper speed (m/s)  peak pan speed (°/s)  v/d (°/s)
              20                      0.30                  85.9       85.9
              12                      0.30                 143.2      143.2
               8                      0.30                 214.9      214.9

The formula matches the simulated peaks exactly. The same gripper speed that needs 86°/s of pan speed 20 cm from the axis needs 215°/s at 8 cm, more than our 180°/s limit: that line must be run more slowly, or moved away from the base.

Choosing TT for a line. The joint speeds of a straight line change along it, so there is no closed-form Tmin⁡T_{\min} as in Section 8.2.4. The scaling law of Section 8.1 still holds, because the shape of the motion does not change when TT does: sample the trajectory once, find the peak joint speed and acceleration, and stretch it by k=max⁡(peak speed/vmax⁡, peak acceleration/amax⁡)k = \max\big(\text{peak speed}/v_{\max},\ \sqrt{\text{peak acceleration}/a_{\max}}\big).

descent : straight line (cubic timing) 0.489 s, peak joint speed  74.5°/s, peak acceleration 720.0°/s²   |   joint-space trapezoid 0.368 s
carry   : straight line (cubic timing) 0.658 s, peak joint speed  87.7°/s, peak acceleration 720.0°/s²   |   joint-space trapezoid 0.451 s

The straight line costs some time: it uses cubic timing (Section 8.3.2: a trapezoid needs about 15% less time), and its joint speeds vary along the line, so the limit is reached only at one point. Near the table, where the path matters, that is usually worth it.

8.5.4 Joint space or task space?

Joint-space pathStraight line in task space
Industrial nameMoveJ (joint move)MoveL (linear move)
Gripper patha curve (an arc around the base for a carry)a straight line
NeedsIK at the two ends onlyIK at every sample, all reachable, same branch
Joint speedsset directly by the time scalingvary along the line; large near singularities and near the pan axis
Speedfastest for a given limitslower
Use forlarge moves through free spaceapproaching, grasping, lifting, placing

Key Takeaways — Section 8.5

  • Interpolating joints moves the gripper along a curve: the SO-ARM101 carry swings in an arc around the pan axis, and a “vertical” descent drifts sideways by millimetres.

  • Straight line: p(t)=p0+s(t)(pf−p0)\mathbf{p}(t) = \mathbf{p}_0 + s(t)(\mathbf{p}_f - \mathbf{p}_0), θ(t)=IK(p(t))\boldsymbol\theta(t) = \mathrm{IK}(\mathbf{p}(t)); orientation about a fixed axis (axis-angle, SLERP). Check reachability, limits and branch at every sample.

  • Joint speeds: θ˙=J−1p˙\dot{\boldsymbol\theta} = \mathbf{J}^{-1}\dot{\mathbf{p}}; large near singularities. For the SO-ARM101 pan joint, ∣θ˙1∣max⁡=v/d|\dot\theta_1|_{\max} = v/d.

  • Choose TT by sampling once and applying the scaling law. Straight lines near objects, joint-space moves through free space.

8.6 From a Trajectory to Motor Commands

A trajectory is a function θ(t)\boldsymbol\theta(t), but a servo only accepts goal positions. Three layers turn one into the other:

LayerWhereHow oftenWhat it does
Planneryour programonce per movechooses the path and the timing (Sections 8.1 to 8.5)
Sampleryour programevery Δt\Delta t (for example 20 ms)evaluates θ(k Δt)\boldsymbol\theta(k\,\Delta t) and sends it as the goal position of each servo
Servoinside each motormuch fasterits own feedback loop drives the motor toward the latest goal

Figure 8.5 — From a trajectory to motion. The planner computes θ(t); the sampler sends θ(kΔt) as the goal every Δt and holds it; the servo follows that staircase with a lag of about tlag θ̇.

Sampling. Between samples the goal is held constant, so the servo sees a staircase that follows the trajectory. Each step is about θ˙ Δt\dot\theta\,\Delta t: at 100°/s, 2° at 50 Hz but 10° at 10 Hz. Coarser steps make the motion jerkier and add delay, because on average the goal is already Δt/2\Delta t/2 old.

Tracking. A position servo needs an error to make torque, so it lags behind a moving goal. A simple model: the servo closes the gap with a time constant tlagt_{\text{lag}}, θ˙m=(θgoal−θm)/tlag\dot\theta_m = (\theta_{\text{goal}} - \theta_m)/t_{\text{lag}}. For a goal moving at speed θ˙\dot\theta the error settles at tlag θ˙t_{\text{lag}}\,\dot\theta. The cell uses tlag=15t_{\text{lag}} = 15 ms, about what the SO-101’s simulated servos show, and a 75° move in 1 s.

trajectory   rate  peak error (°)  error at t = T (°)
linear       50 Hz            2.03                2.03
linear       10 Hz            7.52                7.51
cubic        50 Hz            3.05                0.20
cubic        10 Hz           11.12                2.11
<Figure size 1500x420 with 2 Axes>

At 50 Hz the model shows the pattern a real servo shows. The linear trajectory’s error jumps as soon as the move starts, and the motor is still 2° behind, and still moving, at t=Tt = T. The cubic’s error grows and shrinks with its speed: its peak is larger, because its peak speed is 1.5 times linear’s, but it comes in the middle of the move, and at t=Tt = T the motor is almost on target and at rest. (This first-order model leaves out the overshoot a real servo shows when a linear move starts, which makes linear’s error at the start even larger.)

At 10 Hz the errors are several times larger: just before each new goal arrives, the goal is a full sample period old, so the error grows by up to θ˙ Δt\dot\theta\,\Delta t (7.5° at 75°/s), and the motor visibly follows a staircase.

The servo’s own profile. Sent a single goal angle, an STS3215 servo moves there along its own trapezoidal velocity profile. When the sampler sends a new goal every 20 ms, each goal is only a step of a degree or two, and the built-in profile acts on those small steps; it no longer shapes the whole move, as long as its acceleration setting is high (LeRobot sets it high when it connects). Some servos also accept a move time with each goal, which gives a timed, smooth move from a single command. That is enough for a point-to-point joint move, but the profile shape is fixed by the servo, and a single command per move cannot follow a straight line in task space or pass a via point without stopping. That is why the trajectories of this chapter are streamed from the computer.

Key Takeaways — Section 8.6

  • Planner (once per move), sampler (every Δt\Delta t, sends goal positions), servo (fast internal loop).

  • Sampling holds each goal for Δt\Delta t: steps of θ˙ Δt\dot\theta\,\Delta t, and about Δt/2\Delta t/2 of extra delay.

  • A position servo lags by roughly tlag θ˙t_{\text{lag}}\,\dot\theta: smooth trajectories keep that error small at the start and the end of a move.

  • A servo’s built-in profile handles one goal at a time; streaming lets the computer choose the shape, the path and the via points.

8.7 Putting It Together: A Fast Pick and Place on the SO-ARM101

We now move a block from the pick spot, (20,−12)(20, -12) cm, to the place spot, (20,0)(20, 0) cm, with the gripper pointing straight down and a 6 cm lift, and compare three plans. Each starts and ends 6 cm above a spot, and each spends 0.5 s on closing and 0.5 s on opening the gripper.

  1. Fixed times: every move takes a fixed, generous time (1 to 2.5 s), with a hold of 0.5 to 1 s after it.

  2. Stop at every waypoint, as fast as the limits allow: a synchronized trapezoid (Section 8.3.3) for each move.

  3. Via points: the descent to the block and the final retreat are trapezoids, but lift, carry and lower form one blended trajectory through the two “above” points (Section 8.4.2), stopping only at the block and at the goal.

Figure 8.6 — Lift, carry and lower. The arm must stop at the grasp and the place, where the gripper acts; the points above them are via points. Blending through them (red) saves time but cuts the corners, so the block starts moving sideways before it is fully lifted. Times from the comparison below, with amax = 720°/s².

Figure 8.6 shows plan 3 next to stopping at every waypoint. Real limits are rarely the paper values: a block held in the gripper slips if the arm accelerates too hard. So the comparison runs for three acceleration limits.

Plan 1, fixed times: 15.0 s

a_max (°/s²)  plan 2: stop everywhere  plan 3: via points   saved   lift → carry → lower: stop vs blend
         720                   2.94 s              2.58 s     13%   1.20 s vs 0.83 s
         360                   3.75 s              3.23 s     14%   1.69 s vs 1.17 s
         180                   4.89 s              4.15 s     15%   2.40 s vs 1.66 s

Two lessons for a fast pick and place:

  • Time each move from the limits. Going from fixed, generous times to the fastest moves the limits allow is by far the largest gain: from 15 s per block to a few seconds.

  • Then remove the unnecessary stops. Blending through the two points above the block and above the goal saves about 30% of the lift-carry-lower time, and the saving grows as the acceleration limit is lowered, because each stop costs more when the arm must slow down gently.

The cost of not stopping is the corner. The cell plots the height of the gripper point against its horizontal distance from the block, for the blended plan and for the stop-everywhere plan.

a_max = 720°/s²: the gripper is 1 cm sideways from the block when only 4.6 cm above the grasp height (of the 6 cm lift)
a_max = 180°/s²: the gripper is 1 cm sideways from the block when only 4.6 cm above the grasp height (of the 6 cm lift)
<Figure size 1000x450 with 1 Axes>

The blend rounds off the top corners, here by about 1.4 cm of the 6 cm lift: the block starts moving sideways before it is fully lifted. The amount does not depend on the acceleration limit in this plan, because the planner scales the segment speeds and the blends together. If neighbouring blocks or a growing stack need the full clearance, raise the via points by the amount cut, and check again.

For a fast pick and place, this is the recipe:

  1. Compute every waypoint with IK from the block and goal positions (Chapter 7), and check it against the joint limits (the far spots need a tilted pitch, Section 7.4.4).

  2. Decide which waypoints are stops (grasp, release, placing on a stack) and which are via points.

  3. Time every move from measured speed and acceleration limits, not from guesses: trapezoids for moves that stop, blends through via points.

  4. Use straight lines (Section 8.5) where the path near a block matters; joint-space moves elsewhere.

  5. Check every gripper path with FK against the table, the waiting blocks and the stack, then simulate it before running it on the robot.

Chapter 8 Summary

  • Path vs trajectory: a trajectory is a path θ(s)\boldsymbol\theta(s) plus a time scaling s(t)s(t). On a straight joint-space path every joint uses the same s(t)s(t): θ˙i=Δis˙\dot\theta_i = \Delta_i\dot s. Stretching time by kk divides velocities by kk and accelerations by k2k^2.

  • Polynomials: linear (velocity jumps), cubic (four conditions; rest to rest 3u2−2u33u^2 - 2u^3, peak 1.5Δ/T1.5\Delta/T, ±6Δ/T2\pm6\Delta/T^2), quintic (six conditions; 10u3−15u4+6u510u^3 - 15u^4 + 6u^5, continuous acceleration). Tmin⁡=max⁡(cv∣Δ∣/vmax⁡,ca∣Δ∣/amax⁡)T_{\min} = \max(c_v|\Delta|/v_{\max}, \sqrt{c_a|\Delta|/a_{\max}}).

  • Trapezoid (LSPB): T=∣Δ∣/v+v/aT = |\Delta|/v + v/a; with vmax⁡v_{\max}, amax⁡a_{\max} the fastest rest-to-rest move. Synchronize joints with s˙max⁡=min⁡vmax⁡,i/∣Δi∣\dot s_{\max} = \min v_{\max,i}/|\Delta_i|, s¨max⁡=min⁡amax⁡,i/∣Δi∣\ddot s_{\max} = \min a_{\max,i}/|\Delta_i|.

  • Via points: cubic segments with via velocities (average-slope rule, or the spline’s tridiagonal system for continuous acceleration), or linear segments with parabolic blends (miss ΔV tb/8\Delta V\,t_b/8). Stop only where the task needs it, and check the path with FK.

  • Task space: θ(t)=IK(p(t))\boldsymbol\theta(t) = \mathrm{IK}(\mathbf{p}(t)) for straight lines; θ˙=J−1p˙\dot{\boldsymbol\theta} = \mathbf{J}^{-1}\dot{\mathbf{p}} grows near singularities (and near the pan axis, v/dv/d).

  • Execution: sample every Δt\Delta t and stream goal positions; the servo lags by about tlagθ˙t_{\text{lag}}\dot\theta.

MethodPasses through the pointsStops at the pointsContinuousChooses TT from limits
Linearyesyespositionno (infinite acceleration)
Cubic, rest to restyesyesvelocityTmin⁡T_{\min} formula
Quintic, rest to restyesyesaccelerationTmin⁡T_{\min} formula
Trapezoidyesyesvelocityyes, fastest
Cubic segments, average-slope velocitiesyesonly where a joint turns backvelocityscaling law
Cubic splineyesno (except the ends)accelerationscaling law
Parabolic blendsno (cuts corners)no (except the ends)velocityyes
Straight line in task spaceyes, along a lineyesas its s(t)s(t)sample + scaling law

Practice problems

  1. A joint moves from 10∘10^\circ to 70∘70^\circ in T=2T = 2 s, starting at rest and ending at 20∘20^\circ/s. Find the cubic coefficients by hand and check them with cubic_coefficients. Does the joint overshoot 70∘70^\circ?

  2. Show that the rest-to-rest cubic has peak velocity 1.5Δ/T1.5\Delta/T and end accelerations ±6Δ/T2\pm6\Delta/T^2, and that the rest-to-rest quintic has peak velocity 158Δ/T\tfrac{15}{8}\Delta/T.

  3. Trapezoid: a joint must move 120∘120^\circ with vmax⁡=150∘v_{\max} = 150^\circ/s and amax⁡=500∘a_{\max} = 500^\circ/s². Find TT and tbt_b. Is the profile a trapezoid or a triangle? Then find tbt_b and the cruise speed for a move of exactly T=1.5T = 1.5 s with a=500∘a = 500^\circ/s².

  4. Two joints move Δ=(90∘,−30∘)\Delta = (90^\circ, -30^\circ). Joint 1 has limits 180∘180^\circ/s and 720∘720^\circ/s²; joint 2, 40∘40^\circ/s and 360∘360^\circ/s². Which joint sets the pace of a synchronized trapezoid, and what is TT?

  5. Via points: one joint passes 0∘,50∘,20∘,60∘0^\circ, 50^\circ, 20^\circ, 60^\circ at t=0,1,2,3t = 0, 1, 2, 3 s. Find the via velocities with the average-slope rule and with the spline (solve the 2×22\times2 system by hand), and compare with via_velocities_average and via_velocities_spline.

  6. A blend turns a joint from +100∘+100^\circ/s to −60∘-60^\circ/s with amax⁡=400∘a_{\max} = 400^\circ/s². How long is the blend, and by how much does the joint miss the via point?

  7. SO-ARM101: a straight carry passes 6 cm from the pan axis with a peak gripper speed of 0.25 m/s. What pan speed does it need, and is it within 180∘180^\circ/s? What could you change?

  8. Extend Section 8.7 to stacking six blocks into a pyramid (three on the table, two on them, one on top): list the waypoints for one block, mark which are stops, and estimate the total time with the plan-3 numbers. Which waypoints need extra clearance as the pyramid grows?