Section 5.1: Position, velocity, acceleration; twists

A Careful Control Loop

This section develops the technical contract for Position, velocity, acceleration; twists into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.

The key question in Position, velocity, acceleration; twists 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

For Position, velocity, acceleration; twists, 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.

For kinematics, the first interface is not force or torque, it is motion state. A pose $T\in SE(3)$ 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.

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.

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.

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

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.

Connect twists to the frame where velocity is expressed, and do not mix body and spatial velocity conventions. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

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, 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.