Position, velocity & acceleration

Position, velocity, acceleration

A joint's angle $\theta$ (theta, in radians) says where it is. Its angular velocity $\omega$ (omega, rad/s) says how fast that angle changes, and its angular acceleration $\alpha$ (alpha, rad/s²) says how fast the velocity changes. Velocity is the derivative (rate of change) of position; acceleration is the derivative of velocity.

Finite differences

A robot never sees a smooth curve. It reads its sensors at fixed moments $t_0, t_1, t_2, \dots$, one sampling period $\Delta t$ apart (10 ms here). Between two readings, the velocity is about

$$\omega_k \approx \frac{\theta_k - \theta_{k-1}}{t_k - t_{k-1}}$$

This is a finite difference. Apply it to the velocities and you get the acceleration. Going the other way, adding up $\omega\,\Delta t$ step by step, is integration: it turns velocity back into position.

Encoders count ticks

The shoulder's angle comes from an encoder, which counts whole ticks, $N = 512$ per revolution. encoder.read(1) gives the shoulder's count $n$, rounded down, so the angle is $\theta = n \cdot 2\pi / N$. One tick is $2\pi/512 \approx 0.0123$ rad (0.7°).

Because the angle moves in whole ticks, every difference is a multiple of $2\pi/(N\,\Delta t) \approx 1.23$ rad/s, even while the arm glides at 0.5 rad/s. Halve $\Delta t$ and those steps double.

Smoothing

A moving average of the last $n$ velocities cancels most of that jitter:

$$\bar\omega_k = \frac{\omega_k + \omega_{k-1} + \dots + \omega_{k-n+1}}{n}$$

The price is lag: the average describes the last $n\,\Delta t$ seconds, so it trails the truth by about half of that. Small $n$ is jumpy, large $n$ is late. The table gives each window's typical error, its RMS (root mean square: square the errors, average, take the square root).

Window $n$	1	2	5	20	40
RMS error (rad/s)	0.50	0.25	0.09	0.14	0.27

Finite differences come back in Jacobians, step 1, where they measure how the gripper moves.

Your task

The program swings the shoulder from 0.2 to 1.0 rad and back, reading the encoder every 10 ms. Write:

1. ticks_to_angle(ticks, ticks_per_rev): the angle in radians.
2. finite_difference(ts, xs): $(x_k - x_{k-1})/(t_k - t_{k-1})$ for $k = 1, 2, \dots$, one value fewer than xs.
3. moving_average(xs, n): entry $k$ is the mean of the last n values up to xs[k] (fewer at the start), so it's as long as xs.

Your smoothed velocities, stored in velocity, must have an RMS error under 0.2 rad/s. Open Plots and try N_AVG = 1, 5 and 20.