"""Forward kinematics: planar 2-link arm, DH tables, URDF chains and the SO-101. Conventions: angles in radians, lengths in metres, 4x4 homogeneous transforms acting on column vectors. URDF rpy angles are fixed-axis roll about x, pitch about y, yaw about z: R = Rz(yaw) Ry(pitch) Rx(roll). Contents -------- planar_2link_fk elbow and tip position of a planar 2-link arm dh_standard one link transform, standard (distal) DH convention dh_modified one link transform, modified (proximal, Craig) DH convention fk_dh product of DH link transforms Joint, joint_transform URDF-style joint: fixed origin, then rotation about an axis chain_fk frames of every link in a serial chain of joints parse_urdf_joints read elements of a URDF string into Joint objects SO101_JOINTS the SO-101 follower arm chain from its official URDF so101_fk base-to-gripper-frame pose of the SO-101 screw_axes, poe_fk product-of-exponentials form of the same chain fit_poly_fk, poly_fk a "learned" polynomial FK model fitted by least squares SO-101 data source: so101_new_calib.urdf in TheRobotStudio/SO-ARM100, https://github.com/TheRobotStudio/SO-ARM100/blob/141aea7bcac2744d92998e30268c413202a56f96/Simulation/SO101/so101_new_calib.urdf (Apache License 2.0). In this "new calibration" model each joint's zero is the middle of its range. Run this file to see a worked example: python fk.py """ from __future__ import annotations import itertools import xml.etree.ElementTree as ET from dataclasses import dataclass import numpy as np from numpy.typing import NDArray Array = NDArray[np.float64] # --------------------------------------------------------------------------- # Small helpers # --------------------------------------------------------------------------- def skew(w: Array) -> Array: wx, wy, wz = np.asarray(w, dtype=float) return np.array([[0.0, -wz, wy], [wz, 0.0, -wx], [-wy, wx, 0.0]]) def rot_axis(axis: Array, angle: float) -> Array: """Rodrigues' formula for a rotation of `angle` about `axis`.""" k = np.asarray(axis, dtype=float) k = k / np.linalg.norm(k) K = skew(k) return np.eye(3) + np.sin(angle) * K + (1.0 - np.cos(angle)) * (K @ K) def rpy_to_R(roll: float, pitch: float, yaw: float) -> Array: return rot_axis([0, 0, 1], yaw) @ rot_axis([0, 1, 0], pitch) @ rot_axis([1, 0, 0], roll) def make_T(R: Array, p: Array) -> Array: T = np.eye(4) T[:3, :3] = R T[:3, 3] = p return T def Rz_T(theta: float) -> Array: return make_T(rot_axis([0, 0, 1], theta), np.zeros(3)) def Rx_T(alpha: float) -> Array: return make_T(rot_axis([1, 0, 0], alpha), np.zeros(3)) def trans_T(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> Array: return make_T(np.eye(3), [x, y, z]) # --------------------------------------------------------------------------- # Planar 2-link arm # --------------------------------------------------------------------------- def planar_2link_fk(q1: float, q2: float, l1: float, l2: float) -> tuple[Array, Array]: """Elbow and tip (x, y). q1 from the x axis, q2 relative to link 1.""" elbow = np.array([l1 * np.cos(q1), l1 * np.sin(q1)]) tip = elbow + np.array([l2 * np.cos(q1 + q2), l2 * np.sin(q1 + q2)]) return elbow, tip # --------------------------------------------------------------------------- # Denavit-Hartenberg # --------------------------------------------------------------------------- def dh_standard(theta: float, d: float, a: float, alpha: float) -> Array: """Standard DH: A_i = Rz(theta_i) Tz(d_i) Tx(a_i) Rx(alpha_i).""" return Rz_T(theta) @ trans_T(z=d) @ trans_T(x=a) @ Rx_T(alpha) def dh_modified(alpha_prev: float, a_prev: float, theta: float, d: float) -> Array: """Modified DH: A_i = Rx(alpha_{i-1}) Tx(a_{i-1}) Rz(theta_i) Tz(d_i).""" return Rx_T(alpha_prev) @ trans_T(x=a_prev) @ Rz_T(theta) @ trans_T(z=d) def fk_dh(table: list[tuple[float, float, float]], q: Array, modified: bool = False) -> Array: """Base-to-tool transform from a DH table of revolute joints. Standard rows are (d, a, alpha); modified rows are (alpha_prev, a_prev, d). The joint variable q_i is added to theta_i, whose offset here is zero. """ T = np.eye(4) for row, qi in zip(table, q): if modified: alpha_prev, a_prev, d = row T = T @ dh_modified(alpha_prev, a_prev, qi, d) else: d, a, alpha = row T = T @ dh_standard(qi, d, a, alpha) return T # --------------------------------------------------------------------------- # URDF-style joint chains # --------------------------------------------------------------------------- @dataclass(frozen=True) class Joint: name: str xyz: tuple[float, float, float] # origin translation in the parent link frame rpy: tuple[float, float, float] # origin rotation, fixed-axis roll, pitch, yaw axis: tuple[float, float, float] = (0.0, 0.0, 1.0) kind: str = "revolute" # "revolute" or "fixed" lower: float = -np.pi upper: float = np.pi def joint_transform(j: Joint, q: float = 0.0) -> Array: """Parent-link to child-link transform: T(origin) @ Rot(axis, q).""" T = make_T(rpy_to_R(*j.rpy), j.xyz) if j.kind == "revolute": T = T @ make_T(rot_axis(j.axis, q), np.zeros(3)) return T def chain_fk(joints: list[Joint], q: Array) -> list[Array]: """Frames of the base and every child link, expressed in the base frame. `q` holds one value per revolute joint, in chain order. """ frames = [np.eye(4)] qi = iter(np.asarray(q, dtype=float)) for j in joints: angle = next(qi) if j.kind == "revolute" else 0.0 frames.append(frames[-1] @ joint_transform(j, angle)) return frames def parse_urdf_joints(urdf_text: str, names: list[str]) -> list[Joint]: """Read the named elements of a URDF, in the order given.""" root = ET.fromstring(urdf_text) by_name = {el.get("name"): el for el in root.iter("joint") if el.get("type") is not None} out = [] for n in names: el = by_name[n] org = el.find("origin") xyz = tuple(float(v) for v in (org.get("xyz", "0 0 0") if org is not None else "0 0 0").split()) rpy = tuple(float(v) for v in (org.get("rpy", "0 0 0") if org is not None else "0 0 0").split()) ax = el.find("axis") axis = tuple(float(v) for v in ax.get("xyz").split()) if ax is not None else (1.0, 0.0, 0.0) lim = el.find("limit") lower = float(lim.get("lower")) if lim is not None else -np.pi upper = float(lim.get("upper")) if lim is not None else np.pi out.append(Joint(n, xyz, rpy, axis, el.get("type"), lower, upper)) return out # SO-101 follower arm, base_link -> gripper_frame_link, copied from so101_new_calib.urdf # at commit 141aea7 (see module docstring). The jaw joint ("gripper") is not in this chain. SO101_JOINTS: list[Joint] = [ Joint("shoulder_pan", (0.0388353, -8.97657e-09, 0.0624), (3.14159, 4.18253e-17, -3.14159), lower=-1.91986, upper=1.91986), Joint("shoulder_lift", (-0.0303992, -0.0182778, -0.0542), (-1.5708, -1.5708, 0.0), lower=-1.74533, upper=1.74533), Joint("elbow_flex", (-0.11257, -0.028, 1.73763e-16), (-3.63608e-16, 8.74301e-16, 1.5708), lower=-1.69, upper=1.69), Joint("wrist_flex", (-0.1349, 0.0052, 3.62355e-17), (4.02456e-15, 8.67362e-16, -1.5708), lower=-1.65806, upper=1.65806), Joint("wrist_roll", (5.55112e-17, -0.0611, 0.0181), (1.5708, 0.0486795, 3.14159), lower=-2.74385, upper=2.84121), Joint("gripper_frame_joint", (-0.0079, -0.000218121, -0.0981274), (0.0, 3.14159, 0.0), axis=(0.0, 0.0, 0.0), kind="fixed", lower=0.0, upper=0.0), ] SO101_ARM_NAMES = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"] def so101_fk(q: Array) -> Array: """Pose of gripper_frame_link in base_link for the five arm joint angles (rad).""" return chain_fk(SO101_JOINTS, q)[-1] # --------------------------------------------------------------------------- # Product of exponentials # --------------------------------------------------------------------------- def exp_twist(S: Array, theta: float) -> Array: """exp([S] theta) for a unit-rotation screw axis S = (w, v), |w| = 1.""" w, v = np.asarray(S[:3], dtype=float), np.asarray(S[3:], dtype=float) W = skew(w) R = np.eye(3) + np.sin(theta) * W + (1.0 - np.cos(theta)) * (W @ W) G = np.eye(3) * theta + (1.0 - np.cos(theta)) * W + (theta - np.sin(theta)) * (W @ W) return make_T(R, G @ v) def screw_axes(joints: list[Joint]) -> tuple[list[Array], Array]: """Space-frame screw axes of the revolute joints and the home pose M at q = 0.""" frames = chain_fk(joints, np.zeros(sum(j.kind == "revolute" for j in joints))) S_list = [] for j, T in zip(joints, frames[1:]): if j.kind != "revolute": continue w = T[:3, :3] @ (np.asarray(j.axis) / np.linalg.norm(j.axis)) # axis in base frame p = T[:3, 3] # a point on the axis S_list.append(np.concatenate((w, -np.cross(w, p)))) return S_list, frames[-1] def poe_fk(S_list: list[Array], M: Array, q: Array) -> Array: """T(q) = exp([S1] q1) ... exp([Sn] qn) M.""" T = np.eye(4) for S, qi in zip(S_list, q): T = T @ exp_twist(S, qi) return T @ M # --------------------------------------------------------------------------- # A learned model: polynomial regression of the tip position # --------------------------------------------------------------------------- def poly_features(q: Array, degree: int) -> Array: """All monomials q1^i q2^j with i + j <= degree, for q of shape (N, 2).""" q = np.atleast_2d(q) cols = [np.ones(len(q))] for total in range(1, degree + 1): for i, j in itertools.product(range(total + 1), repeat=2): if i + j == total: cols.append(q[:, 0] ** i * q[:, 1] ** j) return np.column_stack(cols) def fit_poly_fk(q: Array, xy: Array, degree: int) -> Array: """Least-squares coefficients mapping joint angles (N, 2) to tip (N, 2).""" coef, *_ = np.linalg.lstsq(poly_features(q, degree), xy, rcond=None) return coef def poly_fk(coef: Array, q: Array, degree: int) -> Array: return poly_features(q, degree) @ coef def main() -> None: np.set_printoptions(precision=4, suppress=True) deg = np.pi / 180.0 # Planar 2-link worked example (lengths rounded from the SO-101 upper arm and forearm) l1, l2 = 0.116, 0.135 elbow, tip = planar_2link_fk(30 * deg, 45 * deg, l1, l2) print(f"2-link, q = (30, 45) deg: elbow {elbow}, tip {tip} m") # Same arm from a standard DH table: rows (d, a, alpha) T = fk_dh([(0.0, l1, 0.0), (0.0, l2, 0.0)], [30 * deg, 45 * deg]) print(f"DH table gives tip {T[:2, 3]} m") # SO-101 from its URDF for q in (np.zeros(5), np.array([30, -20, 40, 10, 0]) * deg): Tq = so101_fk(q) print(f"SO-101 q = {np.round(q / deg).astype(int)} deg -> gripper frame at {Tq[:3, 3]} m") # Product of exponentials gives the same pose S_list, M = screw_axes(SO101_JOINTS) q = np.array([30, -20, 40, 10, 0]) * deg print("POE matches URDF chain:", np.allclose(poe_fk(S_list, M, q), so101_fk(q))) # A learned FK model: polynomial regression on noisy samples rng = np.random.default_rng(seed=3) q_train = rng.uniform(0, np.pi / 2, size=(500, 2)) xy_train = np.array([planar_2link_fk(a, b, l1, l2)[1] for a, b in q_train]) xy_train += rng.normal(0.0, 0.001, size=xy_train.shape) # 1 mm measurement noise q_in = rng.uniform(0, np.pi / 2, size=(2000, 2)) q_out = rng.uniform(np.pi / 2, np.pi, size=(2000, 2)) true_in = np.array([planar_2link_fk(a, b, l1, l2)[1] for a, b in q_in]) true_out = np.array([planar_2link_fk(a, b, l1, l2)[1] for a, b in q_out]) for degree in (2, 4, 6): coef = fit_poly_fk(q_train, xy_train, degree) e_in = np.linalg.norm(poly_fk(coef, q_in, degree) - true_in, axis=1) e_out = np.linalg.norm(poly_fk(coef, q_out, degree) - true_out, axis=1) print(f"poly degree {degree}: mean error inside training range {1000 * e_in.mean():6.2f} mm, " f"outside {1000 * e_out.mean():8.1f} mm") if __name__ == "__main__": main()