Section 5.1: Position, velocity, acceleration; twists

"Position says where. Velocity says how fast. A twist says both at once, in the language the Lie group already spoke."

Section 5.1

Tell a robot arm to "move forward" while its wrist is mid-rotation, and a single mislabeled velocity can send the gripper curving off in a direction no one commanded, with no error message to explain why. That failure has one root cause and one cure, both of which live in how you write down motion: position says where the body is, velocity says how that pose is changing, acceleration says how the velocity is changing, and a twist fuses the angular and linear parts into a single object that advances the whole pose at once. Figure 5.1A previews the relationship: position, velocity, and acceleration curves for a single joint, alongside the corresponding twist that fuses linear and angular velocity in the body frame. The object of study comes first, then its role in the agent loop, then a compact implementation that tests it.

The key question is practical: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions?

Theory

The practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.

The first thing that interface must make visible is the motion variable itself. That choice drives the rest of this section. Why unify angular and linear velocity into a single six-dimensional object at all? Consider a robot arm reaching toward a moving target: the wrist must simultaneously rotate to align the gripper and translate to close the gap. Track rotation in one variable and translation in another, and the controller must synchronize two separate integrators that evolve on different mathematical spaces. That split makes composition (chaining frames from base to end-effector) error-prone and numerically fragile. The twist packages both into one element of the Lie algebra \(\mathfrak{se}(3)\), so a single matrix exponential advances the full rigid-body pose without singularities at any orientation. This is not mathematical elegance for its own sake: it is the reason that composing a hundred joint frames in Pinocchio stays numerically stable while a naive Euler-angle chain drifts.

For kinematics, the first interface is not force or torque, it is motion state. A pose \(T\in SE(3)\), where \(SE(3)\) is the special Euclidean group of all rigid-body positions-and-orientations in 3D, tells where a rigid body is; a twist \(V=[\omega; v]\) tells how that pose is changing; and an acceleration tells how the twist is changing. The frame matters: a spatial twist is expressed in the world frame, while a body twist is expressed in the moving body frame. Figure 5.1B lays out how the pose, the twist, and the acceleration connect, and how the frame label picks the integration rule.

Before the body-versus-spatial trap below can be stated precisely, the compact contract that defines a twist's action on a pose needs to be on the table. The compact contract is \(\dot T=\widehat V_s T\) for a spatial twist and \(\dot T=T\widehat V_b\) for a body twist. The hat operator turns the 6-vector twist into a matrix in the Lie algebra \(\mathfrak{se}(3)\), so angular velocity and linear velocity update the same rigid transform instead of living in separate bookkeeping systems. To see the scale of the difference, integrate a six-joint arm chain with Euler angles at 1 kHz. In practice it can accumulate on the order of 1 mm of end-effector drift per second, and the exact figure depends on joint configuration and angular velocity magnitude. Integrate the same chain via the matrix-exponential twist update and it typically accumulates less than 1 micrometer over the same interval, a factor of roughly a thousand, from exactly the same sensor data. Put another way, a 30-second pick-and-place run with Euler-angle integration typically drifts the gripper tip by roughly the width of a finger; the twist integrator drifts it by the width of a human hair.

A twist is not merely a notational convenience: it is the smallest object that lets you advance a rigid body's full pose in one step, with no auxiliary bookkeeping and no singularity lurking at any orientation.

Worked Example

The example below integrates a constant body twist over one timestep, advances the pose with the matrix exponential on \(SE(3)\), and confirms that the result is still a valid rigid transform. It also shows the body-versus-spatial trap: applying the same twist on the wrong side of \(T\) moves the end effector in a different direction.

import numpy as np

def hat_so3(w):
    return np.array([[0, -w[2], w[1]],
                     [w[2], 0, -w[0]],
                     [-w[1], w[0], 0]])

def exp_se3(V, dt):
    """Matrix exponential of a twist V=[wx,wy,wz,vx,vy,vz] over dt -> 4x4 in SE(3)."""
    w, v = np.asarray(V[:3]) * dt, np.asarray(V[3:]) * dt
    th = np.linalg.norm(w)
    T = np.eye(4)
    if th < 1e-12:               # pure translation limit
        T[:3, 3] = v
        return T
    K = hat_so3(w / th)
    R = np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * K @ K          # Rodrigues
    G = np.eye(3) + (1 - np.cos(th)) / th**2 * (th * K) \
        + (th - np.sin(th)) / th**3 * (th * K) @ (th * K)
    T[:3, :3] = R
    T[:3, 3] = G @ v
    return T

# Pose at t: rotated 30 deg about z, sitting at (1, 0, 0)
a = np.deg2rad(30)
T = np.array([[np.cos(a), -np.sin(a), 0, 1.0],
              [np.sin(a),  np.cos(a), 0, 0.0],
              [0, 0, 1, 0.0],
              [0, 0, 0, 1.0]])

V_body = [0, 0, 1.0, 0.5, 0, 0]   # 1 rad/s yaw, 0.5 m/s forward in BODY frame
dt = 0.1
T_body_next = T @ exp_se3(V_body, dt)     # body twist: post-multiply
T_spatial_next = exp_se3(V_body, dt) @ T  # WRONG side: spatial interpretation

R = T_body_next[:3, :3]
print("orthonormal?", np.allclose(R.T @ R, np.eye(3)))   # R^T R = I
print("det(R) =", round(np.linalg.det(R), 6))            # 1.0 -> proper rotation
print("body-frame next position   :", np.round(T_body_next[:3, 3], 4))
print("spatial-frame next position:", np.round(T_spatial_next[:3, 3], 4))
# The two positions differ: forward-in-body is not forward-in-world.

The orthonormality check and unit determinant prove the integrated pose stayed in \(SE(3)\); the two distinct positions show why the body-versus-spatial label must travel with every twist in the log. The same label discipline that the hand-built exponential makes obvious is exactly what a production library can quietly default away from, as the next steps show.

Practical Recipe

1. Fix the frame convention first and write it in a header comment: spatial (world-aligned) or body (end-effector-aligned). On a Franka Panda, pin.getFrameVelocity defaults to LOCAL (body); ROS 2 tf2 publishes twists in LOCAL_WORLD_ALIGNED (spatial). Mixing the two silently misaligns your velocity command by the full wrist rotation, typically 30 to 90 degrees on any non-trivial pick-and-place posture.
2. Verify forward kinematics at a known "zero" posture before running any trajectory. On a seven-degrees-of-freedom (DOF) arm such as the Franka Panda or the Kinova Gen3, the zero posture is documented in the URDF; confirm the end-effector origin matches the manufacturer datasheet within 0.1 mm before trusting any computed twist.
3. Sanity-check the twist magnitude against physical limits. Franka Panda joint velocity limits are 2.175 rad/s per joint; Spot's body twist saturates at roughly 1.6 m/s linear and 1.0 rad/s yaw. A computed twist that exceeds these limits by more than 10% indicates a frame error or a timestep mismatch, not a planning decision.
4. Test the body-versus-spatial trap explicitly: command a 0.1 m/s forward body-frame velocity at a 45-degree yaw, then log the world-frame Cartesian arc. The path should curve; a straight-line path means the spatial and body twist were silently swapped.
5. For sim-to-real transfer, compare Pinocchio twist predictions against MuJoCo mj_objectVelocity on the same robot model at three postures. Discrepancies larger than 5% in any axis typically trace to differing joint-angle conventions (DH vs. URDF) or a missing offset frame defined in the URDF but absent in the analytic model.

Production Pattern

Position, velocity, acceleration; twists sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Tie every twist to the frame its velocity lives in, and never mix body and spatial conventions. That discipline gives the idea an intuitive role, a formal interface, a runnable check, and a reproducible failure mode, which is what makes it serve practitioners, builders, and researchers at once.

Use this recipe when turning Position, velocity, acceleration; twists into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Write the joint vector, frame target, velocity convention, and constraint set before solving.
2. Check forward kinematics on a known posture, then perturb one joint and inspect the end-effector delta.
3. Compare an analytic or numerical Jacobian with Pinocchio, Robotics Toolbox, or Drake on the same robot model.
4. Log residual error, joint-limit distance, manipulability (a scalar measure of how far the arm's current posture is from a singularity, where small joint motions produce almost no end-effector motion), and solver iteration count in one artifact.
5. Treat singularities and infeasible targets as design signals, not as solver annoyances.

Kinematic failures often arrive as a plausible pose with an impossible motion. Inspect frame choice, timestamp alignment, angular units, and whether the twist is body-frame or spatial-frame before blaming the planner. For this section, first reproduce one tiny rigid-body motion by hand, then rerun it through Pinocchio, Robotics Toolbox for Python, Drake, or ROS 2 tf2. If the two disagree, inspect conventions and timing before changing the model.