explainer
Joint limits in inverse kinematics: solve a bounded velocity step
Turn physical joint ranges and speed limits into bounds on a local inverse-kinematics command. Compare a constrained least-squares solution with clipping, and check the resulting finite arm position.
What you will learn
- Convert current joint positions and a step duration into allowed joint rates.
- Solve a small regularized least-squares problem with box constraints.
- Explain why independent clipping can waste the remaining motion freedom.
- Compare the same objective for bounded and clipped commands.
- Distinguish a feasible local command from a complete motion plan.
Before you start
A joint can reach its stop while the tool still has somewhere to go. Cutting off that joint's command changes the tool motion. A constrained solve can adjust the other joint at the same time.
This lesson solves one velocity step for a two-link arm. You will derive the allowed rates, calculate the best bounded command for a stated objective, and compare its endpoint with a clipped command.
Keep the physical joint coordinates
Use a fixed-base planar arm with links of 2 m and 1 m. The shoulder angle θ₁ measures rotation from the world x-axis. The elbow angle θ₂ measures rotation relative to the first link; both angles increase counterclockwise.
Our example has these physical ranges:
- Shoulder: −90° ≤ θ₁ ≤ 90°.
- Elbow: −120° ≤ θ₂ ≤ 120°.
- Each joint's speed: at most 1 rad/s in either direction.
These are teaching values for a synthetic arm. A real robot's limits come from its mechanism and controller. A cable, housing or transmission can restrict travel even when two angle values describe the same link orientation.
Keep the physical angle when checking a limit. Wrapping a 190° reading to −170° changes its coordinate value and can hide a travel violation. This model starts within its ranges and never wraps its joint angles.
Specify one local velocity task
Let q = (θ₁, θ₂), in radians. World x points right and y points up. The requested tool velocity v = (vₓ, vᵧ) uses those axes and units of m/s.
Write u = q̇ for the two joint rates, in rad/s. The position Jacobian gives the instantaneous relation ṗ = J(q)u. For this arm:
pₓ = 2 cos θ₁ + cos(θ₁ + θ₂)
pᵧ = 2 sin θ₁ + sin(θ₁ + θ₂)
J =
[−pᵧ, −sin(θ₁ + θ₂);
pₓ, cos(θ₁ + θ₂)]
The task requests tool translation in the plane. Tool orientation remains free. Review differential inverse kinematics for the distinction between a velocity command and a position target.
We choose u at the current q, then hold it constant for a duration h. The resulting joint coordinates are q_next = q + hu. The actual tool endpoint is p(q_next); its local linear prediction is p(q) + hJu.
Derive allowed rates from position limits
For joint i, require q_min,i ≤ q_i + hu_i ≤ q_max,i. Subtract q_i and divide by positive h. Intersect the resulting interval with the speed limit s_i:
ℓᵢ = max(−sᵢ, (q_min,ᵢ − qᵢ)/h)
rᵢ = min(sᵢ, (q_max,ᵢ − qᵢ)/h)
ℓᵢ ≤ uᵢ ≤ rᵢ
At an upper position limit, r_i becomes zero. Motion back into the range remains available. Far from either stop, the speed limit can set both bounds.
For example, 5° of remaining shoulder travel equals π/36 rad. Over 0.5 s, the largest allowed positive rate is π/18 ≈ 0.174533 rad/s. A 1 rad/s speed limit alone would allow the shoulder to cross its stop.
Russ Tedrake's manipulation notes introduce position and velocity constraints in a local optimization using a time step. Here, constant joint rates make q + hu the exact joint update in the ideal model. The tool's linearized position still carries approximation error.
Put the bounds inside the optimization
Choose the allowed rate pair that minimizes a regularized tracking cost:
F(u) = ‖Ju − v‖₂² + λ²‖u‖₂²
minimize F(u), subject to ℓ ≤ u ≤ r
The first term measures squared tool-velocity error. The second penalizes large joint rates, as in damped least squares. With J in m/rad, choose λ in m/rad so the whole cost uses m²/s².
Positive λ makes this quadratic strictly convex, including at a singular Jacobian. The nonempty box therefore has one minimizing rate pair. Zero rates are always feasible because the current position starts inside the limits.
To make a fair comparison, first solve the same objective without bounds. Then clip each component into its allowed interval. Clipping gives a feasible command, but it generally misses the bounded optimum because the Jacobian couples the joint effects.
The guarantee is F(bounded) ≤ F(clipped). It concerns the combined cost. The bounded solution need not minimize velocity error alone; changes in rate norm also affect the decision.
Solve the two-variable box problem
Define H = JᵀJ + λ²I and g = Jᵀv. Expanding the squares gives F(u) = uᵀHu − 2gᵀu + vᵀv. The unconstrained stationary equation is Hu = g.
For two joints, we can inspect every possible location of the box minimum:
- Use the unconstrained solution if it lies inside the box.
- Fix u₁ at each of its two bounds and minimize over u₂.
- Fix u₂ at each bound and minimize over u₁.
- Check the four corners and choose the smallest cost.
On an edge u₁ = a, setting the derivative with respect to u₂ to zero gives u₂ = (g₂ − H₂₁a)/H₂₂. Clip this one-dimensional minimizer to the edge's interval. The corresponding formula applies when u₂ is fixed.
This enumeration covers the interior and the entire boundary. Positive damping makes both edge denominators positive. Boyd and Vandenberghe's convex optimization text, sections 3.1.4 and 4.4, supplies the quadratic and convexity background.
Geometrically, ordinary clipping finds the nearest point in the box under Euclidean distance in rate coordinates. Our cost measures distance from the unconstrained solution through H. Off-diagonal entries tilt the cost contours, so a coordinate-wise nearest point can have a larger cost.
Recompute the free joint near a stop
Start at q = (85°, −90°) and request v = (−1, 0) m/s. Use h = 0.5 s and λ = 0.2 m/rad. The tool starts at approximately (1.170506, 1.905234) m.
The allowed rate intervals are shoulder [−1, 0.174533] and elbow [−1, 1] rad/s. Solving the unconstrained problem gives u₀ ≈ (0.487693, −0.552739) rad/s. Its shoulder rate exceeds the upper bound.
Independent clipping gives u_clip ≈ (0.174533, −0.552739). For the bounded solve, fix the shoulder at its upper bound and recompute the elbow. At this pose H₂₁ = 1, H₂₂ = 1.04 and g₂ ≈ −0.087156, so:
u₂ ≈ (−0.087156 − 0.174533)/1.04
u₂ ≈ −0.251624 rad/s
That rate lies inside the elbow interval. Comparing all candidates confirms the bounded optimum. Its cost is 0.422628 m²/s², compared with 0.516925 m²/s² after clipping.
After the half-second step, the bounded command gives q_next ≈ (90°, −97.208489°). The exact tool endpoint is approximately (0.992096, 1.874520) m. The shoulder reaches its stop while the elbow uses a different rate from the clipped command.
Compare the two finite updates
Start with the upper-limit preset and compare both rate pairs. Reduce h to 0.1 s: the shoulder has enough remaining travel for the unconstrained optimum, so the two methods agree. Changing h changes the position-derived bounds even though J and the requested velocity stay the same.
Choose the lower-limit preset to see the mirrored calculation. Then choose the outward request at the upper stop. At q = (90°, −90°), the x-velocity is −2u₁; producing the requested −1 m/s would require positive shoulder motion, which the bound forbids.
The optimum for that local request is zero motion, with cost 1. Clipping leaves an unnecessary elbow rate and raises the cost to about 1.231225. This example shows how an active physical constraint can block an instantaneous request even when the position Jacobian has full rank.
Separate local feasibility from a motion plan
The optimality claim covers this quadratic at the current posture. It does not establish global reachability or find a complete route to a target. A different posture or a path that first moves away from the requested direction can change the available options.
The robot workspaces lesson derives the full reachable position set for a separate model with unrestricted shoulder rotation and a symmetric elbow limit. It shows how a joint range changes the positions the arm can reach.
With constant ideal rates, every intermediate joint value lies on the segment between q and q_next. Both ends lie inside the coordinate box, so the whole segment respects these joint-position limits. This statement assumes perfect execution of that prescribed rate.
The model omits acceleration, braking distance, tracking delay, torque and collision constraints. A real controller needs those checks and suitable margins. Drake's joint-limit constraint illustrates another design: it attenuates a rate near a stop using configurable distance margins.
More joints can create kinematic redundancy. Extra freedom can help with posture goals, but a null-space command still needs limit checks. Likewise, a learned policy's preferred velocity remains a request; this small optimization checks only its stated geometric and speed constraints.
For repeated control, measure the new posture, rebuild J and the bounds, then solve again. For orientation tasks, use a frame-consistent angular or pose-error formulation. The two-row position model here leaves orientation unconstrained.
Reproduce the bounded solve in Python
This standard-library example reproduces the default experiment. It checks the unconstrained candidate, each edge minimum and each corner. It holds the chosen joint rates constant for one half-second; it does not simulate motor dynamics.
from math import cos, sin, radians, degrees
q = tuple(map(radians, (85, -90)))
v = (-1.0, 0.0)
h, lam = 0.5, 0.2
q_min = tuple(map(radians, (-90, -120)))
q_max = tuple(map(radians, (90, 120)))
def position(q):
return (2 * cos(q[0]) + cos(sum(q)),
2 * sin(q[0]) + sin(sum(q)))
p = position(q)
J = ((-p[1], -sin(sum(q))), (p[0], cos(sum(q))))
lo = tuple(max(-1, (q_min[i] - q[i]) / h) for i in range(2))
hi = tuple(min(1, (q_max[i] - q[i]) / h) for i in range(2))
a = sum(row[0] ** 2 for row in J) + lam ** 2
b = sum(row[0] * row[1] for row in J)
c = sum(row[1] ** 2 for row in J) + lam ** 2
g = tuple(sum(J[k][i] * v[k] for k in range(2)) for i in range(2))
det = a * c - b * b
free = ((c * g[0] - b * g[1]) / det,
(a * g[1] - b * g[0]) / det)
def clip(value, i):
return max(lo[i], min(hi[i], value))
def cost(u):
error = [sum(row[i] * u[i] for i in range(2)) - v[k]
for k, row in enumerate(J)]
return sum(e * e for e in error) + lam ** 2 * sum(x * x for x in u)
clipped = tuple(clip(free[i], i) for i in range(2))
candidates = []
if all(lo[i] <= free[i] <= hi[i] for i in range(2)):
candidates.append(free)
for x in (lo[0], hi[0]):
candidates.append((x, clip((g[1] - b * x) / c, 1)))
for y in (lo[1], hi[1]):
candidates.append((clip((g[0] - b * y) / a, 0), y))
candidates.extend((x, y) for x in (lo[0], hi[0]) for y in (lo[1], hi[1]))
bounded = min(candidates, key=cost)
next_q = tuple(q[i] + h * bounded[i] for i in range(2))
assert all(q_min[i] - 1e-12 <= next_q[i] <= q_max[i] + 1e-12
for i in range(2))
def pair(values):
return "(" + ", ".join(f"{x:.6f}" for x in values) + ")"
print("Unconstrained:", pair(free))
print("Clipped: ", pair(clipped))
print("Bounded: ", pair(bounded))
print(f"Clipped cost: {cost(clipped):.6f}")
print(f"Bounded cost: {cost(bounded):.6f}")
print("Next degrees: ", pair(map(degrees, next_q)))
print("Next tool: ", pair(position(next_q)))
Expected output:
Unconstrained: (0.487693, -0.552739)
Clipped: (0.174533, -0.552739)
Bounded: (0.174533, -0.251624)
Clipped cost: 0.516925
Bounded cost: 0.422628
Next degrees: (90.000000, -97.208489)
Next tool: (0.992096, 1.874520)
Try it yourself
1. Change the time horizon. A joint sits at 85°, with an upper limit of 90° and a speed limit of 1 rad/s. Find its positive rate bound for h = 0.25 s. Does a requested rate of 0.4 rad/s fit, and what angle would that rate reach?
Show solution
The remaining travel is π/36 rad. Dividing by 0.25 s gives π/9 ≈ 0.349066 rad/s, which is below the speed limit. A 0.4 rad/s command exceeds this position-derived rate bound.
It would move by 0.1 rad, about 5.729578°, ending at 90.729578°. Using the bound reaches exactly 90° in the ideal constant-rate model.
2. Reoptimize the free coordinate. Use J with rows (1, 1) and (0, 1), request v = (2, 0), and λ = 1. The unconstrained solution is (0.8, 0.4). If u₁ lies in [−1, 0.2] and u₂ in [−1, 1], compare clipping with the optimum on the upper-u₁ edge.
Show solution
Here H has rows (2, 1) and (1, 3), while g = (2, 2). Clipping gives (0.2, 0.4). On the edge u₁ = 0.2, the free-coordinate minimum is u₂ = (2 − 0.2)/3 = 0.6.
At (0.2, 0.6), the derivative along the edge is zero. The derivative in u₁ is negative, so increasing u₁ would lower cost, but its upper bound prevents that move. These conditions certify the unique box minimum for this strictly convex quadratic.
The bounded cost is 2.2, compared with 2.32 after clipping. Both rate pairs obey the box; recomputing the second rate improves the combined objective.
Sources and further study
- Russ Tedrake, Robotic Manipulation: differential inverse kinematics with constraints. A local quadratic-program formulation with joint velocity, position and acceleration constraints.
- Stephen Boyd and Lieven Vandenberghe, Convex Optimization. Sections 3.1.4 and 4.4 cover positive-definite quadratics and quadratic programming; section 5.5.3 develops optimality conditions.
- Drake: JointVelocityLimitConstraint. An implementation reference for reducing allowed velocity near joint-position limits, with configurable margins.