explainer
Differential inverse kinematics: turn a tip-velocity command into a joint step
Calculate damped joint rates for a robot tip-velocity command, measure the resulting speed and direction error, and compare an instantaneous prediction with one finite joint step.
What you will learn
- Convert a Cartesian tip-velocity command into joint rates using a position Jacobian.
- Explain how damping trades velocity accuracy for smaller joint rates.
- Separate instantaneous velocity error from finite-step linearization error.
- Form a position-feedback velocity command with consistent units.
- Identify the assumptions behind one ideal held-rate joint step.
Before you start
Command a robot's tip to move diagonally at (0.5, 0.5) m/s. The arm must translate that request into joint rotation. Its current pose determines the conversion, and a damped solution can change both the speed and direction of the resulting tip motion.
Differential inverse kinematics solves this local velocity problem. We will calculate the joint rates, hold them for one interval, and check the endpoint through forward kinematics. This separates an instantaneous velocity fit from the motion that follows a finite joint step.
Start with a velocity command
Let q contain joint angles in radians, and let p(q) give the tool's position in world coordinates. At the current q, the position Jacobian J relates joint rates to tool velocity:
ṗ = J(q)q̇
Given a commanded tool velocity v_cmd, differential IK chooses q̇ to achieve or approximate that command. A controller that performs this conversion repeatedly uses a form of resolved-rate control. Each new measurement of q supplies the next Jacobian.
Russ Tedrake's Robotic Manipulation notes introduce differential IK through this velocity map. They also explain the pseudoinverse solution and why physical constraints require more than an unconstrained velocity calculation.
Numerical inverse kinematics can use similar linear algebra to update a candidate configuration. Its update has units of radians. Here q̇ has units of rad/s, and an explicit interval converts rates into angle changes. A numerical search can run while a robot stays still; a velocity command describes intended motion over time.
Use the position Jacobian in matching coordinates
Use the same planar arm as the singularities lesson: L₁ = 2 m and L₂ = 1 m. The shoulder stays at the world origin. Shoulder angle θ₁ is absolute; elbow angle θ₂ is relative to link 1. Both increase counterclockwise, with +z out of the page.
For q = (θ₁, θ₂), the world position is:
pₓ = 2 cos θ₁ + cos(θ₁ + θ₂)
pᵧ = 2 sin θ₁ + sin(θ₁ + θ₂)
Writing s₁₂ = sin(θ₁ + θ₂) and c₁₂ = cos(θ₁ + θ₂), differentiation gives:
J = [−pᵧ, −s₁₂;
pₓ, c₁₂]
The semicolon separates rows. J uses m/rad, q̇ uses rad/s, and Jq̇ uses m/s. Both rows describe world-axis components, so the command must use those same axes.
This task controls the tip's position. A tool twist also includes angular velocity and depends on a reference-frame convention. The space and body Jacobian lesson explains those differences. We do not prescribe an independent tool orientation in this two-output experiment.
Solve a damped velocity problem
For an invertible square J, q̇ = J⁻¹v_cmd matches the command exactly. The pseudoinverse extends the calculation to non-square or singular maps, selecting a minimum-norm least-squares solution. Near rank loss, exact velocity matching can require very large joint rates.
Damped least squares adds a penalty on those rates:
minimize ‖Jq̇ − v_cmd‖₂² + λ²‖q̇‖₂²
q̇ = Jᵀ(JJᵀ + λ²I)⁻¹v_cmd
Positive λ gives a unique minimizer. Samuel Buss derives this penalized objective and its equivalent matrix formulas in section 5 of his IK survey. The same linear operator applies to a velocity request when we use velocity units on its right-hand side.
Here λ uses m/rad, making both objective terms have units m²/s². The two joints receive equal Euclidean weighting, as do the two Cartesian outputs. Mixed rotational and translational joints would require a deliberate choice of scaling.
The damped answer generally leaves a residual even at a regular pose. It minimizes the combined objective, so it can accept additional velocity error to reduce joint rates. Damping does not enforce a chosen per-joint speed limit; joint limits in inverse kinematics introduces explicit bounds.
Measure the change in speed and direction
Calculate the produced instantaneous velocity and its residual:
v_out = Jq̇, r_v = v_cmd − v_out
The residual norm has units m/s. It includes any unavailable velocity component and any mismatch introduced by damping. For positive damping, the residual generally lacks the orthogonality property of an undamped least-squares projection.
Using the singular value decomposition, a nonzero singular value σ has damped inverse gain σ/(σ² + λ²). Multiplying back through J gives an output gain σ²/(σ² + λ²). Buss develops this comparison in section 6 of the same survey.
Different singular directions can shrink by different amounts. A command that combines them can therefore change direction as well as speed. For fixed J, raising λ decreases the joint-rate norm and increases the velocity-residual norm, with non-strict changes in degenerate cases.
At our regular pose q = (0°, 90°), command (0.5, 0.5) m/s and λ = 1 m/rad give v_out = (2/11, 3/11) m/s. The requested components were equal, but the produced components are unequal. The direction changes by approximately 11.309932°.
The experiment reports the unsigned angle between command and produced velocity. A zero vector has no direction. To avoid assigning an angle to roundoff, the numerical comparison treats norms at or below 10⁻¹⁰ m/s as zero; it preserves the calculated rates, endpoints, and rank.
An exactly straight arm still lacks its radial velocity direction, regardless of positive damping.
Compare three endpoints after one interval
Now freeze the computed joint rates q̇₀ for an interval Δt. The proposed joint update is:
q_next = q₀ + Δt q̇₀
This is an explicit Euler step for a state-dependent velocity law evaluated at q₀. Under our narrower assumption that ideal joints hold q̇₀ constant throughout the interval, it is also the exact joint-angle update for that hold. We evaluate the nonlinear forward map at q_next to find the resulting tool position.
Three endpoints answer different questions:
| Endpoint | Formula | Meaning |
|---|---|---|
| Command | p₀ + Δt v_cmd | Straight displacement requested by the command |
| Linearized | p₀ + Δt J(q₀)q̇₀ | First-order prediction from the produced initial velocity |
| After joint step | p(q₀ + Δt q̇₀) | Nonlinear endpoint under the ideal held-rate model |
The linearization error compares the final two rows. For fixed bounded rates and a smooth forward map, it is O(Δt²) locally. Halving a sufficiently small interval therefore reduces its leading quadratic term by about four. The damping mismatch between the first two rows scales linearly with Δt.
These errors are vectors. The norm of their sum need not equal the sum of their norms. A smaller curvature error can coexist with a larger error from the commanded endpoint when stronger damping makes the arm follow the command less closely.
Calculate one command and its joint step
Start at q₀ = (0°, 90°), so p₀ = (2, 1) m and J = [−1, −1; 2, 0]. Use v_cmd = (0.5, 0.5) m/s and λ = 0.2 m/rad.
The equivalent damped normal equation is:
[5.04, 1; 1, 1.04]q̇ = (0.5, −0.5)
Its determinant is 4.2416. Solving gives q̇ = (1.02/4.2416, −3.02/4.2416), approximately (0.240475, −0.711995) rad/s. Multiplying by J yields v_out ≈ (0.471520, 0.480951) m/s.
The residual is approximately (0.028480, 0.019049) m/s, with norm 0.034263 m/s. This error exists before choosing any time interval.
For Δt = 0.2 s, compare:
| Result | World position in meters |
|---|---|
| Command endpoint | (2.100000, 1.100000) |
| Linearized endpoint | (2.094304, 1.096190) |
| Tool after joint step | (2.091852, 1.091710) |
The linearization error is 0.005108 m. The error from the command endpoint is 0.011624 m, combining velocity mismatch and finite-step curvature.
Increase only the interval to 0.8 s. The joint rates and initial velocity residual stay identical. The nonlinear endpoint becomes (2.331438, 1.312085) m, and the linearization error grows to 0.085892 m. Holding the initial Jacobian's solution longer does not keep the tip moving along its initial tangent.
Inspect damping and hold time separately
Compare Low damping and High damping to inspect the rate-versus-accuracy tradeoff. Compare the default with Longer interval to change the finite-step error while keeping the velocity solution fixed.
Straight arm, Near straight (0.1°), and Folded arm remain numerically well defined because λ stays positive. Their position ranks still follow the geometry. A positive regularization term in the solver does not change the robot's physical task rank.
The first plot shows both arm poses in world coordinates. The second subtracts the initial tool position and enlarges the displacement comparison. Both use equal horizontal and vertical scales, but their zoom levels differ. Endpoint markers and displacement segments do not trace the tool's complete path.
Zero interval retains the velocity calculation while producing no displacement. Zero command gives zero joint rates for every interval. Blocked command asks a straight arm for outward radial velocity; the selected rates are zero, while its command endpoint advances for a positive interval.
The model assumes exact geometry and ideal joint-rate tracking. A real velocity controller must also account for timing, measurement error, mechanical limits, collisions, and lower-level actuator behavior. The sample intervals are teaching controls, not recommended robot-controller update periods.
Add position feedback with explicit assumptions
A reference path supplies a desired position p_ref(t) and velocity v_ref(t). The trajectory time scaling lesson shows how a timing law turns a chosen joint path into positions, velocities, and accelerations. Position error can add a correction to the velocity command:
e = p_ref − p, v_cmd = v_ref + Ke
Use matching world coordinates. For a scalar gain K, units of s⁻¹ convert meter-valued error into m/s. For example, v_ref = (0.1, 0) m/s, e = (0.02, −0.01) m, and K = 2 s⁻¹ produce v_cmd = (0.14, −0.02) m/s.
If the tool achieves this velocity exactly in continuous time, then ė = −Ke. A positive scalar K gives exponential decay in that ideal model. With a velocity residual r_v, the actual relation becomes ė = −Ke + r_v. Damping, lost task directions, finite sampling, and tracking errors can therefore change the response.
Modern Robotics develops task-space feedforward and feedback control with velocity inputs. Full-pose versions also need compatible rotation errors and frame conversions. Our experiment accepts v_cmd directly and evaluates one hold; it does not simulate a feedback loop.
A learned policy could supply v_ref or a reference position, but the same Jacobian, units, and constraints still apply. With extra joints, kinematic redundancy creates room for secondary objectives while preserving an attainable primary instantaneous task.
Reproduce the calculation in Python
This Python 3 example uses only the standard library. It fixes the shoulder at zero, computes damped joint rates, and evaluates the forward map after one held command. The straight preset uses its exact Jacobian.
from math import cos, sin, radians, hypot
def step(elbow_degrees, damping, dt, command=(0.5, 0.5)):
q = (0.0, radians(elbow_degrees))
def point(q):
a, b = q
return (2*cos(a) + cos(a+b), 2*sin(a) + sin(a+b))
p = point(q)
j = ((-p[1], -sin(sum(q))), (p[0], cos(sum(q))))
if elbow_degrees == 0:
j = ((0.0, 0.0), (3.0, 1.0))
a = sum(x*x for x in j[0]) + damping*damping
b = sum(j[0][i]*j[1][i] for i in range(2))
c = sum(x*x for x in j[1]) + damping*damping
determinant = a*c - b*b
z = ((c*command[0] - b*command[1]) / determinant,
(a*command[1] - b*command[0]) / determinant)
rates = tuple(sum(j[k][i]*z[k] for k in range(2)) for i in range(2))
produced = tuple(sum(j[k][i]*rates[i] for i in range(2))
for k in range(2))
q_next = tuple(q[i] + dt*rates[i] for i in range(2))
p_next = point(q_next)
linear = tuple(p[i] + dt*produced[i] for i in range(2))
error = hypot(*(p_next[i] - linear[i] for i in range(2)))
return rates, produced, p_next, error
def pair(values):
return '(' + ', '.join(f'{0.0 if abs(x) < 5e-7 else x:.6f}'
for x in values) + ')'
for name, elbow, damping, dt in [
('default', 90, 0.2, 0.2), ('high damping', 90, 1, 0.2),
('long hold', 90, 0.2, 0.8), ('straight', 0, 0.2, 0.2)
]:
rates, produced, p_next, error = step(elbow, damping, dt)
print(f'{name}: rates={pair(rates)} rad/s')
print(f' produced={pair(produced)} m/s')
print(f' endpoint={pair(p_next)} m; linearization error={error:.6f} m')
Expected output:
default: rates=(0.240475, -0.711995) rad/s
produced=(0.471520, 0.480951) m/s
endpoint=(2.091852, 1.091710) m; linearization error=0.005108 m
high damping: rates=(0.136364, -0.318182) rad/s
produced=(0.181818, 0.272727) m/s
endpoint=(2.035612, 1.053878) m; linearization error=0.001006 m
long hold: rates=(0.240475, -0.711995) rad/s
produced=(0.471520, 0.480951) m/s
endpoint=(2.331438, 1.312085) m; linearization error=0.085892 m
straight: rates=(0.149402, 0.049801) rad/s
produced=(0.000000, 0.498008) m/s
endpoint=(2.998314, 0.099582) m; linearization error=0.001686 m
The high-damping case has less curvature error than the default while following the commanded velocity less accurately. The numbers use six-decimal rounding; the calculation retains floating-point precision internally.
Try it yourself
Exercise 1. Consider a standalone local position Jacobian J = diag(2, 1) m/rad, command v_cmd = (1, 1) m/s, and damping λ = 1 m/rad. Find the joint rates, produced velocity, and velocity residual. For Δt = 0.1 s, find the difference between command and linearized endpoints.
Show the damped-velocity solution
The matrix JJᵀ + λ²I = diag(5, 2). The damped rates are q̇ = (2/5, 1/2) rad/s. Multiplying by J gives v_out = (4/5, 1/2) m/s, so r_v = (1/5, 1/2) m/s.
The command endpoint minus the linearized endpoint is Δt r_v = (0.02, 0.05) m. The produced vector has a different direction from the equal-component command. A local Jacobian alone does not specify the nonlinear endpoint; that requires the forward map and a joint update.
Exercise 2. At one instant, let v_ref = (0.1, 0) m/s, e = (0.02, −0.01) m, and K = 2 s⁻¹. Compute v_cmd. If the velocity residual is r_v = (0.01, 0.02) m/s, what is the instantaneous error derivative ė? Does the ideal equation ė = −Ke still hold?
Show the feedback-and-residual solution
The correction is Ke = (0.04, −0.02) m/s, giving v_cmd = (0.14, −0.02) m/s. Subtracting the residual gives v_out = (0.13, −0.04) m/s.
Therefore ė = v_ref − v_out = (−0.03, 0.04) m/s. This also equals −Ke + r_v. The ideal residual-free prediction would be (−0.04, 0.02) m/s, so it does not describe this case. A feedback gain alone does not guarantee that a damped or constrained velocity solve achieves the desired error dynamics.
Sources and further study
- Russ Tedrake, Robotic Manipulation: Basic Pick and Place. The differential IK sections connect Cartesian velocity commands, pseudoinverses, and physical constraints.
- Samuel R. Buss: Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods. The author's 2009 survey, hosted by Carnegie Mellon, derives the damped objective in section 5 and its singular-value gains in section 6.
- Lynch and Park, Modern Robotics: Motion Control with Velocity Inputs, Part 3. Develops task-space feedforward and feedback commands with matching frame conventions.