Section 5.8: Motion constraints

A Careful Control Loop

This section develops the technical contract for Motion constraints 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 Motion constraints 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 Motion constraints, 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.

Motion constraints turn "move there" into "move there while staying legal." Some constraints are hard safety limits, such as joint bounds, collision clearance, and maximum speed. Others are task constraints, such as keeping a camera pointed at an object or keeping a carried cup upright. A planner that ignores the distinction may find a mathematically short path that the robot must reject at execution time.

A useful constraint is stated in a form that both the solver and the evaluator can check: equality constraints $h(q)=0$, inequality constraints $g(q)\le 0$, velocity limits $\dot q_{\min}\le \dot q\le \dot q_{\max}$, and acceleration or jerk limits for smooth execution. The same definitions should appear in the logs, not only inside the solver setup.

Worked Example

The example clamps a candidate joint configuration into its bounds (constraint projection), then samples velocity along a cubic interpolation between two waypoints. The endpoint speeds are zero, yet the mid-segment speed peaks well above the limit. Checking only the knots accepts a trajectory the actuator cannot execute.

import numpy as np

q_min, q_max = np.array([-2.0, -2.0]), np.array([2.0, 2.0])
qd_max = 1.5            # rad/s per joint velocity limit

# Constraint projection: clamp an out-of-bounds candidate into joint limits
candidate = np.array([2.4, -0.3])
clamped = np.clip(candidate, q_min, q_max)
print("candidate clamped:", candidate, "->", clamped)

# Cubic (Hermite) blend q0 -> q1 over duration T, zero endpoint velocity.
# Endpoint speed is 0, but interior speed peaks at 1.5 * |q1-q0| / T.
q0, q1, T = np.array([0.0, 0.0]), np.array([1.2, 0.0]), 1.0

def q_of_t(t):
    s = t / T
    return q0 + (q1 - q0) * (3*s**2 - 2*s**3)

def speed(t, h=1e-5):
    return np.max(np.abs((q_of_t(t + h) - q_of_t(t - h)) / (2*h)))

v_endpoints = max(speed(1e-4), speed(T - 1e-4))
ts = np.linspace(0, T, 200)
v_dense = max(speed(t) for t in ts[1:-1])

print(f"speed at endpoints : {v_endpoints:.3f} rad/s -> ok? {v_endpoints <= qd_max}")
print(f"peak speed (dense) : {v_dense:.3f} rad/s -> ok? {v_dense <= qd_max}")
# Endpoints pass; the dense check catches the mid-segment overspeed.
assert v_dense > v_endpoints, "dense check must reveal the interior peak"

The dense re-sample is the cheap guard that turns a "looks fine at the knots" plan into a verified one. A velocity limit imposed only at waypoints is not the limit the actuator experiences along the blend, which is why the evaluation script must sample the same trajectory the controller will track.

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

Motion constraints 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.

Write motion constraints as equations or inequalities that the solver and the evaluation script both see. 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 Motion constraints 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 which constraints were enforced during planning, which were checked only after planning, and which were hidden inside a controller. For this section, first reproduce one constrained update by hand, then rerun it through Drake, MoveIt 2, Pinocchio, Robotics Toolbox for Python, or a small NumPy projection. If the two disagree, inspect conventions and timing before changing the model.