Trajectories and moving smoothly

Inverse kinematics tells you the joint angles for a target pose. It says nothing about how to get there. If you simply write the final angles to the servos, each one slews at full speed, the arm jerks, the gears take a beating and the tip swings through some uncontrolled path. A trajectory is the plan for the motion: a position (and velocity) for every joint at every instant between the start and the goal.

Point-to-point motion

The simplest task is to move from joint angles q0 to q1 in a time T, starting and ending at rest. You need a function q(t) for 0 <= t <= T that satisfies q(0) = q0 and q(T) = q1. There are infinitely many; the differences are in how the motion starts, how it ends and how much the joints are asked to accelerate.

Linear interpolation

The first idea is a straight line in time:

q(t) = q0 + (q1 - q0) * t / T

With q0 = 0 and q1 = 90 degrees over T = 2 s, the velocity is constant at 90 / 2 = 45 degrees/s. It is simple, but the velocity jumps from 0 to 45 degrees/s in an instant at t = 0, and from 45 back to 0 at t = T. A jump in velocity is an infinite acceleration, which means an infinite force on an ideal arm and a violent jolt on a real one. A hobby servo will often hide this because its internal controller smooths the step, but it still draws a current spike and shakes the frame.

Cubic polynomial with zero end velocities

To remove the jumps, require the velocity to be zero at both ends. A cubic has four coefficients, exactly enough for four conditions:

q(t) = a0 + a1*t + a2*t^2 + a3*t^3

Conditions: q(0) = q0, q(T) = q1, qdot(0) = 0, qdot(T) = 0. Write D = q1 - q0.

1. q(0) = a0 = q0.
2. qdot(0) = a1 = 0.
3. q(T) = q0 + a2*T^2 + a3*T^3 = q1, so a2*T^2 + a3*T^3 = D.
4. qdot(T) = 2*a2*T + 3*a3*T^2 = 0, so a2 = -1.5*a3*T.

Substitute step 4 into step 3: -1.5*a3*T^3 + a3*T^3 = D, so a3 = -2*D / T^3 and then a2 = 3*D / T^2.

q(t) = q0 + D * (3*s^2 - 2*s^3) with s = t / T

The same example, D = 90 degrees and T = 2 s: at t = 0.5 s, s = 0.25 and 3*0.0625 - 2*0.015625 = 0.15625, so q = 14.1 degrees, compared with 22.5 degrees for the straight line. The cubic starts slowly, speeds up and then eases in. At the midpoint the velocity peaks:

qdot(T/2) = 1.5 * D / T = 1.5 * 45 = 67.5 degrees/s

That is 1.5 times the average velocity, the price of starting and stopping smoothly in the same time. The peak acceleration is at the ends: qdot2(0) = 6*D/T^2 = 6*90/4 = 135 degrees/s^2, finite, unlike the straight line. If your servo's top speed is lower than the peak, make T longer. The peak velocity falls as 1/T and acceleration as 1/T^2.

Joint space versus Cartesian space

Interpolating the joint angles is simple and never hits a singularity on the way, but the tip does not move in a straight line. Take L1 = 100 mm and L2 = 80 mm. Pose A has t1 = 0, t2 = 90 degrees, so the tip is at (100, 80) mm. Pose B has t1 = 90, t2 = 90 degrees, so the tip is at (-80, 100) mm. Halfway in joint space, t1 = 45, t2 = 90:

x = 100*cos 45 + 80*cos 135 = 70.71 - 56.57 = 14.1 mm y = 100*sin 45 + 80*sin 135 = 70.71 + 56.57 = 127.3 mm

The straight line between A and B has its midpoint at ((100 - 80)/2, (80 + 100)/2) = (10, 90) mm. The joint-space path bulges outward by about 37 mm. If the task is drawing, gluing or pushing past an obstacle, that matters.

To get a straight tip path, interpolate the Cartesian position, and call IK at every step. Each point on the line p(t) = pA + (pB - pA)*s(t) becomes a pair of joint angles. The price: IK at every tick, a need to check that every point on the line is reachable, and the risk of running into a singularity, where joint speeds spike. Rule of thumb: use joint space for free moves, and Cartesian space when the path itself matters.

import numpy as np

L1, L2 = 100.0, 80.0

def ik(x, y):
    D = (x*x + y*y - L1**2 - L2**2) / (2*L1*L2)
    if abs(D) > 1: raise ValueError("unreachable")
    s2 = np.sqrt(1 - D*D)                    # elbow-down
    return (np.arctan2(y, x) - np.arctan2(L2*s2, L1 + L2*D),
            np.arctan2(s2, D))

def ease(s):                                 # cubic with zero end velocities
    return 3*s**2 - 2*s**3

pA, pB = np.array([100.0, 80.0]), np.array([-80.0, 100.0])
T, dt = 2.0, 0.02
for k in range(int(T/dt) + 1):
    s = ease(k*dt / T)
    p = pA + (pB - pA) * s                   # straight line in Cartesian space
    t1, t2 = ik(*p)
    if k % 25 == 0:
        print(round(k*dt, 2), p.round(1), np.degrees([t1, t2]).round(1))

Sending angles to servos at a fixed rate

A hobby servo receives a pulse every 20 ms (50 Hz), so there is no point updating faster than that. You also cannot use delay() to wait between updates: it blocks everything else, including reading sensors and running other joints. Use millis() and a schedule instead:

#include <Servo.h>

Servo shoulder;
const unsigned long PERIOD_MS = 20;     // 50 Hz update rate
const float T = 2.0;                    // move duration, s
const float Q0 = 30.0, Q1 = 120.0;      // start and end angles, degrees

unsigned long tStart, tNext;

float cubic(float q0, float q1, float s) {
  return q0 + (q1 - q0) * (3*s*s - 2*s*s*s);
}

void writeAngle(Servo &sv, float deg) {
  // writeMicroseconds keeps sub-degree resolution; 544 to 2400 us = 0 to 180 deg
  sv.writeMicroseconds((int)(544 + deg * (1856.0 / 180.0)));
}

void setup() {
  shoulder.attach(9);
  writeAngle(shoulder, Q0);
  tStart = millis();
  tNext = tStart;
}

void loop() {
  unsigned long now = millis();
  if ((long)(now - tNext) >= 0) {       // time for the next update
    tNext += PERIOD_MS;                 // schedule from the plan, not from now
    float s = (now - tStart) / (T * 1000.0);
    if (s > 1.0) s = 1.0;
    writeAngle(shoulder, cubic(Q0, Q1, s));
  }
  // other work can run here without being blocked
}

Two details matter. tNext += PERIOD_MS instead of tNext = now + PERIOD_MS stops the schedule drifting, because small delays in one iteration are not added to every later one. And (long)(now - tNext) >= 0 is safe when millis() wraps around after about 49 days, since unsigned subtraction handles the overflow. Calling write() with an integer would work too, but at the slow start of a cubic it repeats the same degree for many ticks, so the motion looks steppy. Microseconds fix that.