"""Inverse kinematics for serial arms: analytic 2-link IK and numerical IK. Contents -------- fk_planar tip position of a planar chain of revolute joints ik_2link closed-form IK of the planar 2-link arm (elbow up or down) jacobian_planar analytic 2 x n position Jacobian of a planar chain numerical_jacobian central-difference Jacobian of any forward-kinematics function manipulability Yoshikawa manipulability sqrt(det(J J^T)) ik_solve iterative IK: Jacobian transpose, pseudo-inverse or damped least squares resolved_rate_step one step of resolved-rate (Cartesian velocity) control Run this file to see a worked example: python ik.py """ from __future__ import annotations from dataclasses import dataclass, field from typing import Callable, Literal import numpy as np from numpy.typing import NDArray FloatArray = NDArray[np.float64] FkFunction = Callable[[FloatArray], FloatArray] JacFunction = Callable[[FloatArray], FloatArray] # --------------------------------------------------------------------------- # Planar chain: forward kinematics and analytic Jacobian # --------------------------------------------------------------------------- def joint_positions_planar(q: FloatArray, lengths: FloatArray) -> FloatArray: """Return the (n+1) x 2 array of joint positions, base first, tip last.""" q = np.asarray(q, dtype=float) lengths = np.asarray(lengths, dtype=float) angles = np.cumsum(q) steps = np.column_stack((lengths * np.cos(angles), lengths * np.sin(angles))) return np.vstack((np.zeros(2), np.cumsum(steps, axis=0))) def fk_planar(q: FloatArray, lengths: FloatArray) -> FloatArray: """Tip position (x, y) of a planar chain with joint angles q (rad).""" return joint_positions_planar(q, lengths)[-1] def jacobian_planar(q: FloatArray, lengths: FloatArray) -> FloatArray: """Analytic 2 x n position Jacobian of a planar revolute chain. Column i is the tip velocity caused by joint i turning at 1 rad/s: z x (p_tip - p_i), with z the axis out of the plane. """ p = joint_positions_planar(q, lengths) r = p[-1] - p[:-1] # vectors from each joint to the tip return np.vstack((-r[:, 1], r[:, 0])) # z x r = (-r_y, r_x) # --------------------------------------------------------------------------- # Analytic IK for the planar 2-link arm # --------------------------------------------------------------------------- def ik_2link(x: float, y: float, l1: float, l2: float, elbow: Literal["up", "down"] = "down") -> tuple[float, float]: """Closed-form IK of a planar 2-link arm. elbow="down" returns the solution with q2 >= 0, elbow="up" the one with q2 <= 0. (For a target to the right of the base, q2 >= 0 puts the elbow below the line from base to tip.) Raises ValueError if (x, y) is outside the annulus |l1 - l2| <= r <= l1 + l2. """ r2 = x * x + y * y c2 = (r2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2) tol = 1e-12 if c2 > 1.0 + tol or c2 < -1.0 - tol: raise ValueError(f"target ({x:.4f}, {y:.4f}) is out of reach " f"(r = {np.sqrt(r2):.4f}, reach {abs(l1 - l2):.4f} to {l1 + l2:.4f})") c2 = float(np.clip(c2, -1.0, 1.0)) s2 = np.sqrt(1.0 - c2 * c2) if elbow == "up": s2 = -s2 elif elbow != "down": raise ValueError("elbow must be 'up' or 'down'") q2 = np.arctan2(s2, c2) q1 = np.arctan2(y, x) - np.arctan2(l2 * s2, l1 + l2 * c2) return float(q1), float(q2) # --------------------------------------------------------------------------- # General tools: numerical Jacobian, manipulability # --------------------------------------------------------------------------- def numerical_jacobian(fk: FkFunction, q: FloatArray, eps: float = 1e-6) -> FloatArray: """Central-difference Jacobian d fk / d q, shape (m, n). Error is O(eps^2).""" q = np.asarray(q, dtype=float) f0 = np.asarray(fk(q), dtype=float) jac = np.empty((f0.size, q.size)) for i in range(q.size): dq = np.zeros_like(q) dq[i] = eps jac[:, i] = (np.asarray(fk(q + dq)) - np.asarray(fk(q - dq))) / (2.0 * eps) return jac def manipulability(jac: FloatArray) -> float: """Yoshikawa manipulability w = sqrt(det(J J^T)). Zero at a singularity.""" jac = np.asarray(jac, dtype=float) return float(np.sqrt(max(np.linalg.det(jac @ jac.T), 0.0))) def dls_step(jac: FloatArray, err: FloatArray, lam: float) -> FloatArray: """Damped least-squares step dq = J^T (J J^T + lam^2 I)^-1 e.""" m = jac.shape[0] return jac.T @ np.linalg.solve(jac @ jac.T + lam * lam * np.eye(m), err) # --------------------------------------------------------------------------- # Iterative IK # --------------------------------------------------------------------------- @dataclass class IKResult: q: FloatArray converged: bool iterations: int errors: list[float] = field(default_factory=list) # |e| before each step step_norms: list[float] = field(default_factory=list) # |dq| of each step def ik_solve(fk: FkFunction, target: FloatArray, q0: FloatArray, *, method: Literal["dls", "pinv", "transpose"] = "dls", jac: JacFunction | None = None, lam: float = 0.05, alpha: float = 1.0, tol: float = 1e-6, max_iter: int = 200, q_min: FloatArray | None = None, q_max: FloatArray | None = None, max_step: float | None = None) -> IKResult: """Iterative inverse kinematics for any chain. fk maps joint vector q (n,) to task vector x (m,), e.g. tip position. jac analytic Jacobian function; if None a central-difference Jacobian of fk is used. method "transpose": dq = alpha J^T e (alpha is the gain) "pinv": dq = J^+ e "dls": dq = J^T (J J^T + lam^2 I)^-1 e q_min, q_max joint limits; q is clipped to them after every step. max_step optional cap on |dq| per iteration (rad). """ target = np.asarray(target, dtype=float) q = np.asarray(q0, dtype=float).copy() jac_fn: JacFunction = jac if jac is not None else (lambda qq: numerical_jacobian(fk, qq)) result = IKResult(q=q, converged=False, iterations=0) for k in range(max_iter): err = target - np.asarray(fk(q), dtype=float) err_norm = float(np.linalg.norm(err)) result.errors.append(err_norm) if err_norm < tol: result.converged = True result.iterations = k break jmat = jac_fn(q) if method == "transpose": dq = alpha * jmat.T @ err elif method == "pinv": dq = np.linalg.pinv(jmat) @ err elif method == "dls": dq = dls_step(jmat, err, lam) else: raise ValueError(f"unknown method {method!r}") step = float(np.linalg.norm(dq)) if max_step is not None and step > max_step: dq *= max_step / step result.step_norms.append(float(np.linalg.norm(dq))) q = q + dq if q_min is not None or q_max is not None: q = np.clip(q, q_min if q_min is not None else -np.inf, q_max if q_max is not None else np.inf) else: result.iterations = max_iter result.q = q return result def resolved_rate_step(q: FloatArray, xdot: FloatArray, jac: FloatArray, dt: float, lam: float = 0.0) -> FloatArray: """Integrate one step of resolved-rate control: qdot = J^# xdot. lam = 0 uses the pseudo-inverse; lam > 0 uses damped least squares. """ xdot = np.asarray(xdot, dtype=float) if lam > 0.0: qdot = dls_step(jac, xdot, lam) else: qdot = np.linalg.pinv(jac) @ xdot return np.asarray(q, dtype=float) + dt * qdot # --------------------------------------------------------------------------- # Worked example # --------------------------------------------------------------------------- def main() -> None: l1, l2 = 0.30, 0.20 lengths = np.array([l1, l2]) x, y = 0.35, 0.20 for elbow in ("down", "up"): q1, q2 = ik_2link(x, y, l1, l2, elbow=elbow) tip = fk_planar(np.array([q1, q2]), lengths) print(f"elbow {elbow:4s}: q1 = {np.degrees(q1):7.2f} deg, q2 = {np.degrees(q2):7.2f} deg, " f"FK -> ({tip[0]:.4f}, {tip[1]:.4f})") q = np.array(ik_2link(x, y, l1, l2, "down")) jmat = jacobian_planar(q, lengths) print("J =\n", np.round(jmat, 4)) print(f"det J = {np.linalg.det(jmat):.5f}, l1*l2*sin(q2) = {l1 * l2 * np.sin(q[1]):.5f}") # Redundant 3-link chain solved numerically with DLS lengths3 = np.array([0.20, 0.15, 0.10]) res = ik_solve(lambda qq: fk_planar(qq, lengths3), np.array([0.25, 0.15]), np.array([0.1, 0.1, 0.1]), method="dls", lam=0.01, max_step=0.3, jac=lambda qq: jacobian_planar(qq, lengths3)) print(f"3-link DLS: converged={res.converged} in {res.iterations} iterations, " f"q = {np.round(np.degrees(res.q), 2)} deg") if __name__ == "__main__": main()