Section 5.8: Motion constraints

"Physics does not negotiate. A motion constraint is not a preference; it is the boundary of what the robot is allowed to want."

A Constrained Optimizer

Your IK solver returns a flawless joint configuration, and the simulation replays it without complaint. Then, 340 milliseconds into execution, the real arm slams into a protective stop (a safety halt the robot's firmware triggers when a commanded position, velocity, or torque exceeds a rated limit): the cubic blend between two legal waypoints (a smooth cubic-polynomial interpolation the controller runs between successive setpoints) quietly hit 2.1 rad/s on a joint rated for 1.5. This is the gap between a configuration the solver can compute and one the hardware is permitted to execute. We define the four constraint families that matter on real hardware (joint bounds, rate limits, collision margins, and task-equality constraints), connect each to the controller that enforces it, then verify them with a dense-sampling check that catches the violations endpoint-only validation misses.

The key question is concrete: when a MoveIt 2 plan satisfies \(q_{\min}\le q\le q_{\max}\) at every waypoint, will the Franka Panda's joint_trajectory_controller still trip a protective stop on the cubic blend between them, and what logged signal would tell you before the arm freezes mid-task?

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.

To see why that inspectable interface is worth the trouble, follow a single placement task from a clean IK solution to a dropped part.

From valid solution to dropped part

Consider a Universal Robots UR5 placing a circuit board in a tight enclosure. The IK solver returns a mathematically valid configuration, but the path between waypoints cuts through a cabinet wall and peaks at 3.2 rad/s on joint 2, more than double the 1.5 rad/s actuator limit. The controller receives an infeasible command, trips a safety stop, and drops the board. Constraint-unaware planning produces exactly this: plans that look correct in simulation and fail at the first motor command. A plan that ignores actuator limits is not a motion plan; it is a wish list.

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. The Technical Core section later in this page details the mechanism, projecting a candidate configuration onto the equality-constraint manifold by Newton iteration on \(h\).

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. Checking only the two endpoint knots passes the trajectory. Checking 200 interior samples catches a mid-segment peak that would trip the actuator safety stop the moment the command reaches hardware. In practice, on cubic-blend trajectories with tight velocity margins, a planner that checks only waypoints can accept a substantial fraction of trajectories that a controller running at 1 kHz would immediately abort (the exact rate depends on blend duration and margin, but the failure mode itself is structural, not incidental): the math looks clean at two points while the actuator fails somewhere in the 999 steps between them. This is the knots-only blindspot. A velocity limit imposed only at waypoints is not the limit the actuator experiences along the blend, so 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.

If the planner checks joint limits only at waypoints and the controller interpolates freely between them, who is actually enforcing the constraint?

Write motion constraints as equations or inequalities that the solver and the evaluation script both see. This makes the treatment useful to readers approaching from theory, implementation, or research: 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.