Inverse kinematics (IK) asks the opposite question to Lesson 6.1: given a desired tool pose, which joint positions produce it? For a 7-DOF arm there is no single formula. There may be infinitely many answers (redundancy), none (the pose is out of reach), or answers the joint limits forbid. The standard tool is an iteration: measure the pose error, use the Jacobian to choose a small joint change that reduces it, repeat.
This lesson builds that iteration, damped least squares, and then does what most IK descriptions skip: it tests the solver on targets whose reachability is known independently of the solver, counts its failures, and finds out why each one failed. The result is a solver you can trust with a stated success rate, which is what Checkpoint 6 and Project 5 ask for.
[!established] What MuJoCo provides and what it does not MuJoCo computes forward kinematics (
mj_kinematics) and Jacobians (mj_jacSiteand friends) but has no inverse-kinematics function: IK is built on top of them, asmjcourse.kinematics.solve_ikdoes. Writingqposand callingmj_kinematicsposes the robot without simulating it, which is all IK needs.
The lab runs the same iteration in the page, one step per frame, on arm7. The green sphere is the target.
```lab ik {“dock”: true, “title”: “Damped least squares on a 7-DOF arm”, “height”: 320, “mode”: “pose”, “damping”: 0.01}
## Mathematics
### The error
For a target position $p^\star$ and orientation $q^\star$ (a unit quaternion), with the tool currently at $p$, $q$:
$$e_p = p^\star - p, \qquad e_r = \log\!\left(q^\star \otimes \bar q\right),$$
where $\log$ returns the rotation vector (axis times angle) of the quaternion that turns the current orientation into the target one, in world coordinates, the same frame as MuJoCo's rotational Jacobian. Taking the shorter way round (flipping the quaternion's sign when $w < 0$) keeps the angle at most $\pi$.
### From Newton to damped least squares
To first order, a joint change $\Delta q$ changes the pose by $J\Delta q$. Asking for $J\Delta q = e$ with the smallest $\|\Delta q\|$ gives the pseudo-inverse step $\Delta q = J^\top (JJ^\top)^{-1} e$. Near a singularity $JJ^\top$ is nearly singular and the step explodes. **Damped least squares** (also called Levenberg-Marquardt) asks instead for
$$\min_{\Delta q}\ \|J\Delta q - e\|^2 + \lambda^2\|\Delta q\|^2 \quad\Longrightarrow\quad \Delta q = J^\top\left(JJ^\top + \lambda^2 I\right)^{-1} e .$$
> [!derivation] What the damping does to each direction
> With $J = U\Sigma V^\top$, the step is $\Delta q = \sum_i \frac{\sigma_i}{\sigma_i^2 + \lambda^2}\, v_i\, u_i^\top e$. For $\sigma_i \gg \lambda$ the gain is $1/\sigma_i$, the pseudo-inverse. For $\sigma_i \ll \lambda$ it is $\sigma_i/\lambda^2 \to 0$ instead of $1/\sigma_i \to \infty$. The gain $\sigma/(\sigma^2 + \lambda^2)$ is largest at $\sigma = \lambda$, where it equals $1/(2\lambda)$, so **no step is ever longer than $\|e\|/(2\lambda)$**. For the 3 cm target near the singularity in output (5), that bound is 1.5 rad at $\lambda$ = 0.01 and 0.15 rad at $\lambda$ = 0.1; the measured largest steps were 0.96 and 0.097 rad. The price of damping is slower convergence: the same target took 19 iterations instead of 5.
The solver also clips each step to 0.2 rad (a trust region) and clamps every joint to its range after the step.
### Why clamping creates local minima
When a joint is pressed against its limit, clamping removes the step's component along it. If the remaining joints cannot reduce the error from there, the iteration stalls: a stationary point of the clamped update that is not a solution. Every one of the 15 failures in output (3) ended this way, with one or two joints at a limit, and every one was solved by restarting from a random configuration inside the ranges. Restarts are crude, cheap and, here, completely effective; smarter limit handling (for example, solving a small constrained least-squares problem each step) is the Challenge.
### Redundancy and the null space
A 7-DOF arm solving a 6-D task has at least one direction of joint motion that leaves the tool still. Adding $N z$ to each step, with $N$ the projector onto the null space of $J$ and $z$ a posture preference (here $z = k\,(q_\text{mid} - q)$, towards the middle of the ranges), moves the elbow without disturbing the task:
$$N = I - V_r V_r^\top \quad (V_r \text{: right singular vectors with } \sigma_i > 0), \qquad J N = 0 .$$
> [!warning] The damped projector leaks
> It is tempting to reuse the damped inverse: $N_\lambda = I - J^\top(JJ^\top + \lambda^2 I)^{-1}J$. But $J N_\lambda = \lambda^2 (JJ^\top + \lambda^2 I)^{-1} J \neq 0$: its gain in each direction is $\lambda^2\sigma_i/(\sigma_i^2+\lambda^2)$, so the posture term pushes the tool by roughly $\lambda^2/\sigma$ per unit of $z$ every iteration. With $\lambda$ = 0.01, $\sigma \approx 0.2$ and $\|z\| \approx 0.3$ rad, that is about 0.15 mm per iteration, more than the 0.1 mm tolerance, and the solver never finishes (output 4: 38 successes instead of 81). The course's own `solve_ik` used this projector until this lesson measured it; it now uses the exact one.
## Implementation
The solver is `mjcourse/kinematics.py`; `solve_ik` is one function of about forty lines, and `solve_ik_restarts` wraps it with random restarts:
```python file=src/mjcourse/kinematics.py
"""Kinematics on top of MuJoCo: Jacobians, finite-difference checks, inverse kinematics.
Shapes follow MuJoCo: a translational or rotational Jacobian is (3, nv), mapping
generalized velocities qvel (nv,) to a world-frame linear or angular velocity.
"""
from __future__ import annotations
from dataclasses import dataclass, field
import mujoco
import numpy as np
from mjcourse import spatial
def site_jacobian(model: mujoco.MjModel, data: mujoco.MjData, site: int | str) -> tuple[np.ndarray, np.ndarray]:
"""World-frame translational and rotational Jacobians (each 3 x nv) of a site.
Valid for the configuration last processed by mj_forward or mj_kinematics + mj_comPos.
"""
sid = model.site(site).id
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacSite(model, data, jacp, jacr, sid)
return jacp, jacr
def fd_site_jacobian(model: mujoco.MjModel, data: mujoco.MjData, site: int | str,
eps: float = 1e-6) -> tuple[np.ndarray, np.ndarray]:
"""Central finite-difference Jacobians of a site's position and orientation.
Perturbs each degree of freedom with mj_integratePos (so free and ball joints
are handled on the manifold) and differences the site pose. Leaves `data`
as it found it (position-dependent quantities recomputed).
"""
sid = model.site(site).id
q0 = data.qpos.copy()
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
scratch = mujoco.MjData(model)
for k in range(model.nv):
poses = []
for sign in (+1.0, -1.0):
dq = np.zeros(model.nv)
dq[k] = sign * eps
scratch.qpos[:] = q0
mujoco.mj_integratePos(model, scratch.qpos, dq, 1.0)
mujoco.mj_kinematics(model, scratch)
poses.append((scratch.site_xpos[sid].copy(), scratch.site_xmat[sid].reshape(3, 3).copy()))
(p_plus, r_plus), (p_minus, r_minus) = poses
jacp[:, k] = (p_plus - p_minus) / (2 * eps)
# world-frame angular change: log(R_minus^T R_plus) expressed in world = R log(...)
rel = spatial.mat_to_quat(r_minus.T @ r_plus)
jacr[:, k] = r_minus @ spatial.quat_to_rotvec(rel) / (2 * eps)
return jacp, jacr
@dataclass(frozen=True)
class IKResult:
qpos: np.ndarray
success: bool
iterations: int
pos_error: float # m
rot_error: float # rad
history: list[float] = field(default_factory=list)
max_step: float = 0.0 # largest joint step before step_limit clipped it (rad)
def solve_ik(model: mujoco.MjModel, site: str, target_pos: np.ndarray,
target_quat: np.ndarray | None = None, q_init: np.ndarray | None = None,
damping: float = 1e-2, max_iters: int = 200, tol_pos: float = 1e-4, tol_rot: float = 1e-3,
step_limit: float = 0.2, joints: list[int] | None = None, nullspace_gain: float = 0.0,
q_rest: np.ndarray | None = None, projector: str = "exact") -> IKResult:
"""Damped-least-squares inverse kinematics for one site.
Solves for qpos so that the site reaches target_pos (and target_quat if given),
iterating dq = J^T (J J^T + damping^2 I)^-1 e, clamping joints to their ranges.
`joints` restricts the solve to the given dof indices (default: all dofs of
hinge and slide joints). A null-space term pulls redundant arms toward q_rest,
projected with the exact null-space projector; projector="damped" builds it from
the damped inverse instead, which leaks into the task (kept to demonstrate that).
Works on a private MjData; `model` is not modified.
"""
data = mujoco.MjData(model)
if q_init is not None:
data.qpos[:] = q_init
dofs = np.array(joints if joints is not None else
[model.jnt_dofadr[j] for j in range(model.njnt) if model.jnt_type[j] in (2, 3)])
qadr = np.array([model.jnt_qposadr[model.dof_jntid[d]] for d in dofs])
lo = np.array([model.jnt_range[model.dof_jntid[d]][0] if model.jnt_limited[model.dof_jntid[d]] else -np.inf for d in dofs])
hi = np.array([model.jnt_range[model.dof_jntid[d]][1] if model.jnt_limited[model.dof_jntid[d]] else np.inf for d in dofs])
sid = model.site(site).id
history: list[float] = []
pos_err = rot_err = np.inf
max_step = 0.0
for it in range(1, max_iters + 1):
mujoco.mj_kinematics(model, data)
mujoco.mj_comPos(model, data)
e_pos = target_pos - data.site_xpos[sid]
jacp, jacr = site_jacobian(model, data, sid)
if target_quat is not None:
cur = spatial.mat_to_quat(data.site_xmat[sid].reshape(3, 3))
e_rot = spatial.quat_to_rotvec(spatial.quat_mul(target_quat, spatial.quat_conj(cur))) # world frame
err = np.r_[e_pos, e_rot]
jac = np.vstack([jacp, jacr])[:, dofs]
rot_err = float(np.linalg.norm(e_rot))
else:
err, jac, rot_err = e_pos, jacp[:, dofs], 0.0
pos_err = float(np.linalg.norm(e_pos))
history.append(pos_err)
if pos_err < tol_pos and rot_err < tol_rot:
return IKResult(data.qpos.copy(), True, it, pos_err, rot_err, history, max_step)
jjt = jac @ jac.T + damping**2 * np.eye(jac.shape[0])
dq = jac.T @ np.linalg.solve(jjt, err)
if nullspace_gain > 0 and q_rest is not None:
# Exact projector onto the null space of J, I - V_r V_r^T. Building it from the
# damped inverse instead leaks the posture term into the task (Lesson 6.3).
if projector == "damped":
null = np.eye(len(dofs)) - jac.T @ np.linalg.solve(jjt, jac)
else:
_, sv, vt = np.linalg.svd(jac)
rank = int(np.sum(sv > 1e-6 * sv[0]))
null = np.eye(len(dofs)) - vt[:rank].T @ vt[:rank]
dq += null @ (nullspace_gain * (q_rest[qadr] - data.qpos[qadr]))
norm = np.linalg.norm(dq)
max_step = max(max_step, float(norm))
if norm > step_limit:
dq *= step_limit / norm
data.qpos[qadr] = np.clip(data.qpos[qadr] + dq, lo, hi)
return IKResult(data.qpos.copy(), False, max_iters, pos_err, rot_err, history, max_step)
def solve_ik_restarts(model: mujoco.MjModel, site: str, target_pos: np.ndarray,
target_quat: np.ndarray | None = None, q_init: np.ndarray | None = None,
restarts: int = 10, seed: int = 0, **kwargs) -> tuple[IKResult, int]:
"""solve_ik from q_init, then from up to `restarts` random configurations inside the
joint ranges. Returns the first success (or the last attempt) and the number of
restarts used. Clamping at joint limits creates local minima; restarts escape them.
"""
result = solve_ik(model, site, target_pos, target_quat, q_init=q_init, **kwargs)
rng = np.random.default_rng(seed)
lo, hi = model.jnt_range[:, 0], model.jnt_range[:, 1]
used = 0
while not result.success and used < restarts:
used += 1
q0 = np.zeros(model.nq) if q_init is None else np.array(q_init, dtype=float)
limited = model.jnt_limited.astype(bool)
for j in np.flatnonzero(limited & np.isin(model.jnt_type, (2, 3))):
q0[model.jnt_qposadr[j]] = rng.uniform(lo[j], hi[j])
result = solve_ik(model, site, target_pos, target_quat, q_init=q0, **kwargs)
return result, used
def manipulability(jac: np.ndarray) -> float:
"""Yoshikawa's manipulability sqrt(det(J J^T)); zero at singular configurations."""
return float(np.sqrt(max(np.linalg.det(jac @ jac.T), 0.0)))
The experiment tests it:
INPUT: arm7.xml; 100 targets made by forward kinematics of random joint vectors inside the ranges
PROCESS: solve from home; sweep the damping; analyse and restart every failure; compare posture terms; probe a near-singular and an unreachable target
OUTPUT: printed tables
```python file=examples/l6_3_inverse_kinematics.py “"”Lesson 6.3: inverse kinematics by damped least squares, under test.
INPUT arm7.xml; targets made by forward kinematics of random joint vectors inside the ranges, so “reachable” never depends on the solver being tested PROCESS (1) solve 6-D IK from the home pose: success, iterations, time, errors; (2) sweep the damping; (3) analyse every failure and retry it from random starts; (4) add a null-space posture term with the damped and the exact projector; (5) near the singular straight-up pose and beyond the workspace: what the solver does, and what it reports OUTPUT printed tables
Run: python examples/l6_3_inverse_kinematics.py “””
import os import time
import mujoco import numpy as np
from mjcourse import kinematics, model_path, spatial, stats
N = 40 if os.environ.get(“MJC_FAST”) == “1” else 100
def reachable_targets(model, n: int, seed: int) -> list[tuple[np.ndarray, np.ndarray]]: data = mujoco.MjData(model) rng = np.random.default_rng(seed) out = [] for _ in range(n): data.qpos[:] = rng.uniform(model.jnt_range[:, 0], model.jnt_range[:, 1]) mujoco.mj_kinematics(model, data) out.append((data.site(“ee”).xpos.copy(), spatial.mat_to_quat(data.site(“ee”).xmat.reshape(3, 3)))) return out
def at_limit(model, q: np.ndarray) -> int: lo, hi = model.jnt_range[:, 0], model.jnt_range[:, 1] return int(np.sum((q - lo < 1e-9) | (hi - q < 1e-9)))
def benchmark(model, targets, home) -> list[int]: results, t0 = [], time.perf_counter() for pos, quat in targets: results.append(kinematics.solve_ik(model, “ee”, pos, quat, q_init=home)) elapsed = (time.perf_counter() - t0) / len(targets) ok = [r for r in results if r.success] lo, hi = stats.wilson_interval(len(ok), len(results)) its = np.array([r.iterations for r in ok]) print(f” success {len(ok)}/{len(results)} (95% Wilson [{lo:.2f}, {hi:.2f}]), iterations among successes: “ f”median {np.median(its):.0f}, 95th percentile {np.percentile(its, 95):.0f}; {1000 * elapsed:.2f} ms per solve”) print(f” worst error among successes: {1000 * max(r.pos_error for r in ok):.3f} mm, “ f”{np.degrees(max(r.rot_error for r in ok)):.3f} deg; joints outside their ranges: “ f”{sum(int(np.any(r.qpos < model.jnt_range[:, 0] - 1e-12) | np.any(r.qpos > model.jnt_range[:, 1] + 1e-12)) for r in results)}”) return [i for i, r in enumerate(results) if not r.success]
def damping_sweep(model, targets, home) -> None: for lam in (1e-4, 1e-3, 1e-2, 3e-2, 1e-1, 3e-1): res = [kinematics.solve_ik(model, “ee”, p, q, q_init=home, damping=lam) for p, q in targets] its = [r.iterations for r in res if r.success] print(f” damping {lam:<7g} success {sum(r.success for r in res):3d}/{len(res)}, median iterations {np.median(its):.0f}”)
def failures(model, targets, home, failed: list[int]) -> None: used = [] for i in failed: pos, quat = targets[i] r = kinematics.solve_ik(model, “ee”, pos, quat, q_init=home) retry, n = kinematics.solve_ik_restarts(model, “ee”, pos, quat, q_init=home, restarts=10, seed=i) used.append(n if retry.success else None) print(f” target {i:2d}: stopped at {1000 * r.pos_error:6.1f} mm, {np.degrees(r.rot_error):6.1f} deg with “ f”{at_limit(model, r.qpos)} joint(s) at a limit; random restarts: “ f”{‘solved after ‘ + str(n) if retry.success else ‘not solved in 10’}”) print(f” every failure ended with a joint at a limit: {all(at_limit(model, kinematics.solve_ik(model, ‘ee’, *targets[i], q_init=home).qpos) > 0 for i in failed)}; “ f”solved by restarts: {sum(u is not None for u in used)}/{len(failed)}”)
def nullspace(model, targets, home) -> None: mid = model.jnt_range.mean(axis=1) for label, gain, proj in ((“no posture term”, 0.0, “exact”), (“posture term, damped projector”, 0.1, “damped”), (“posture term, exact projector”, 0.1, “exact”)): res = [kinematics.solve_ik(model, “ee”, p, q, q_init=home, max_iters=300, nullspace_gain=gain, q_rest=mid, projector=proj) for p, q in targets] ok = [r for r in res if r.success] spread = np.mean([np.linalg.norm(r.qpos - mid) for r in ok]) print(f” {label:<32} success {len(ok):3d}/{len(res)}, mean |q - q_mid| over successes {spread:.2f} rad”)
def edge_cases(model, home) -> None: data = mujoco.MjData(model) data.qpos[:] = [0.0, 0.001, 0.0, 0.001, 0.0, 0.001, 0.0] # 1 mrad from straight up mujoco.mj_kinematics(model, data) pos = data.site(“ee”).xpos.copy() + [0.0, 0.0, -0.03] # 3 cm down: reachable for lam in (1e-6, 1e-3, 1e-2, 1e-1): r = kinematics.solve_ik(model, “ee”, pos, q_init=data.qpos.copy(), damping=lam, max_iters=300) print(f” from 1 mrad short of straight up, 3 cm down, damping {lam:<6g}: success {r.success}, “ f”{r.iterations:2d} iterations, largest unclipped step {r.max_step:7.3g} rad”) far = np.array([1.6, 0.0, 0.5]) r = kinematics.solve_ik(model, “ee”, far, q_init=home, max_iters=300) data.qpos[:] = r.qpos mujoco.mj_kinematics(model, data) reach = np.linalg.norm(data.site(“ee”).xpos - data.xpos[model.body(“link2”).id]) print(f” target 1.6 m from the base: success {r.success}, final position error {1000 * r.pos_error:.0f} mm, “ f”shoulder-to-tool distance at the end {reach:.3f} m (arm fully stretched towards the target)”)
if name == “main”: model = mujoco.MjModel.from_xml_path(str(model_path(“arm7”))) home = model.key_qpos[model.key(“home”).id].copy() targets = reachable_targets(model, N, seed=0) print(f”(1) {N} reachable 6-D targets, one start (home), damping 0.01, tolerances 0.1 mm and 0.057 deg”) failed = benchmark(model, targets, home) print(“(2) damping sweep”) damping_sweep(model, targets, home) print(“(3) the failures, one by one”) failures(model, targets, home, failed) print(“(4) a null-space term pulling towards the middle of the joint ranges (gain 0.1, up to 300 iterations)”) nullspace(model, targets, home) print(“(5) near a singularity, and out of reach”) edge_cases(model, home)
Output:
```text
(1) 100 reachable 6-D targets, one start (home), damping 0.01, tolerances 0.1 mm and 0.057 deg
success 85/100 (95% Wilson [0.77, 0.91]), iterations among successes: median 23, 95th percentile 110; 4.79 ms per solve
worst error among successes: 0.099 mm, 0.057 deg; joints outside their ranges: 0
(2) damping sweep
damping 0.0001 success 83/100, median iterations 22
damping 0.001 success 83/100, median iterations 22
damping 0.01 success 85/100, median iterations 23
damping 0.03 success 86/100, median iterations 24
damping 0.1 success 80/100, median iterations 33
damping 0.3 success 62/100, median iterations 74
(3) the failures, one by one
target 1: stopped at 1.2 mm, 0.3 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 3: stopped at 53.9 mm, 17.4 deg with 2 joint(s) at a limit; random restarts: solved after 2
target 14: stopped at 0.9 mm, 115.6 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 20: stopped at 0.3 mm, 25.8 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 28: stopped at 0.4 mm, 58.2 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 36: stopped at 106.7 mm, 12.0 deg with 2 joint(s) at a limit; random restarts: solved after 1
target 40: stopped at 18.6 mm, 5.1 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 49: stopped at 2.8 mm, 1.5 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 61: stopped at 0.5 mm, 109.2 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 66: stopped at 6.3 mm, 12.0 deg with 2 joint(s) at a limit; random restarts: solved after 2
target 75: stopped at 3.0 mm, 0.8 deg with 1 joint(s) at a limit; random restarts: solved after 3
target 81: stopped at 0.0 mm, 3.5 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 87: stopped at 187.0 mm, 64.8 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 97: stopped at 9.7 mm, 2.6 deg with 1 joint(s) at a limit; random restarts: solved after 1
target 99: stopped at 16.1 mm, 4.4 deg with 1 joint(s) at a limit; random restarts: solved after 2
every failure ended with a joint at a limit: True; solved by restarts: 15/15
(4) a null-space term pulling towards the middle of the joint ranges (gain 0.1, up to 300 iterations)
no posture term success 87/100, mean |q - q_mid| over successes 3.12 rad
posture term, damped projector success 38/100, mean |q - q_mid| over successes 2.54 rad
posture term, exact projector success 81/100, mean |q - q_mid| over successes 2.91 rad
(5) near a singularity, and out of reach
from 1 mrad short of straight up, 3 cm down, damping 1e-06 : success True, 5 iterations, largest unclipped step 80.8 rad
from 1 mrad short of straight up, 3 cm down, damping 0.001 : success True, 5 iterations, largest unclipped step 9.79 rad
from 1 mrad short of straight up, 3 cm down, damping 0.01 : success True, 5 iterations, largest unclipped step 0.957 rad
from 1 mrad short of straight up, 3 cm down, damping 0.1 : success True, 19 iterations, largest unclipped step 0.0974 rad
target 1.6 m from the base: success False, final position error 560 mm, shoulder-to-tool distance at the end 1.048 m (arm fully stretched towards the target)
Reading the output:
Position converges, orientation stalls near 120° or 180°. The orientation error is computed in a frame different from the Jacobian’s (body instead of world), or the quaternion’s sign was not fixed so the error went the long way round. Check e_r for a known 10° rotation.
The iteration oscillates and never settles. Steps are too large: damping too small near a singularity, or no step limit. Print the unclipped step and the smallest singular value each iteration.
Some targets are never reached from the start you use. Check whether a joint sits at its limit when the solver stops (output 3). Restart from random configurations, and report how many restarts were needed.
Adding a posture term made the solver fail. The null-space projector was built from the damped inverse (output 4). Build it from the singular vectors of $J$.
The solver reports success but the tool points the wrong way. Success was judged on position only. A 6-D task needs both tolerances, as in solve_ik.
Write the closed-form IK of the planar two-link arm: from the tip position, $\cos q_2 = (x^2 + z^2 - L_1^2 - L_2^2)/(2L_1L_2)$, giving an elbow-up and an elbow-down solution. Compare both with solve_ik on arm2.xml from several starting poses: which branch does the iteration find, and what decides it?
Replace clamping with a constrained step: at each iteration, solve $\min |J\Delta q - e|^2 + \lambda^2|\Delta q|^2$ subject to $q_\text{lo} \le q + \Delta q \le q_\text{hi}$ (a small box-constrained least-squares problem; scipy.optimize.lsq_linear solves it). Measure single-start success on this lesson’s 100 targets against clamping’s 85, with intervals, and the cost per solve.
[!research] IK sits between many policies and their robots Teleoperation systems and policies that output end-effector poses rely on an IK or differential-IK layer to produce joint commands. Its failure modes, local minima at joint limits, branch switches between nearby solutions, and damping-induced lag near singularities, appear in recorded demonstrations and in policy rollouts as discontinuities or sluggish motion that have nothing to do with the policy (inference from this lesson’s failure analysis). Reporting the IK layer’s own success rate and settings, measured as here, is part of reporting an end-effector-space result.
{"id": "6.3-check", "title": "Knowledge check", "questions": [
{"kind": "numeric", "q": "With damping λ = 0.02, what is the longest joint step (rad) damped least squares can take for a pose error of norm 0.05?",
"answer": 1.25, "tol": 0.001, "unit": "rad",
"explain": "<p>The largest gain is $1/(2\\lambda)$ = 25, so $\\|\\Delta q\\| \\le 0.05 \\times 25$ = 1.25 rad, whatever the pose.</p>"},
{"kind": "mcq", "q": "How should 'reachable' test targets for an IK solver be generated?",
"options": ["Sample positions in a box around the robot and keep the ones the solver reaches", "Sample joint vectors inside the ranges and take their forward kinematics", "Use the targets from the robot's datasheet", "Sample positions within the arm's total length of the base"],
"answer": 1,
"explain": "<p>Forward kinematics of a valid joint vector is reachable by construction, independently of the solver. Keeping only targets the solver reaches makes the success rate 100% by definition; a sphere of the arm's length contains unreachable poses (and ignores orientation and joint limits).</p>"},
{"kind": "mcq", "q": "A solver that converged on 87% of targets converges on 38% once a posture term is added with N = I − Jᵀ(JJᵀ + λ²I)⁻¹J. Why?",
"options": ["The posture gain is too low", "This N is not an exact null-space projector, so the posture term moves the tool every iteration", "Redundant arms have no null space", "Damping must be zero when a posture term is used"],
"answer": 1,
"explain": "<p>$JN = \\lambda^2(JJ^\\top + \\lambda^2 I)^{-1}J \\ne 0$. The leak is small per iteration but larger than a tight tolerance, so the error never reaches it. Use $I - V_rV_r^\\top$ from the SVD of $J$.</p>"},
{"kind": "mcq", "q": "Every failure of a clamped DLS solver ends with a joint at its limit, and random restarts fix them all. What kind of failure is this?",
"options": ["Unreachable targets", "Singularities", "Local minima created by the joint limits", "Numerical round-off"],
"answer": 2,
"explain": "<p>The targets are reachable by construction, and the same target converges from other starts, so the iteration was stuck at a stationary point of the clamped update.</p>"}
]}
Level 7 moves from where the arm is to how it moves: the equations of motion, the mass matrix and the bias forces, derived for the two-link arm and read out of MuJoCo for the 7-DOF one.