Pointing the Arm at a Target

# labs/kinematics.py
"""Two-link geometry for the arm: joint angles to a point, and back again."""
import math
from dataclasses import dataclass

L1_CM = 10.0                # shoulder pivot to elbow pivot
L2_CM = 7.0                 # elbow pivot to the point between the jaws
SHOULDER_ZERO_DEG = 90.0    # the shoulder angle that points the upper arm straight out

MAX_REACH_CM = L1_CM + L2_CM
MIN_REACH_CM = abs(L1_CM - L2_CM)


@dataclass(frozen=True)
class Pose:
    """Two joint angles, in the units chapter 69's config bounds."""
    shoulder_deg: float
    elbow_deg: float


def forward_kinematics(pose: Pose, l1: float = L1_CM,
                       l2: float = L2_CM) -> tuple[float, float]:
    """Where the grip point lands, in centimetres from the shoulder pivot."""
    t1 = math.radians(pose.shoulder_deg - SHOULDER_ZERO_DEG)
    t2 = math.radians(pose.elbow_deg)
    return (l1 * math.cos(t1) + l2 * math.cos(t1 + t2),
            l1 * math.sin(t1) + l2 * math.sin(t1 + t2))


def elbow_point(pose: Pose, l1: float = L1_CM) -> tuple[float, float]:
    """Where the elbow pivot sits, which is not where the grip point sits."""
    t1 = math.radians(pose.shoulder_deg - SHOULDER_ZERO_DEG)
    return l1 * math.cos(t1), l1 * math.sin(t1)


if __name__ == "__main__":
    print(f"upper arm {L1_CM:.1f} cm, forearm {L2_CM:.1f} cm, "
          f"reach {MIN_REACH_CM:.1f} to {MAX_REACH_CM:.1f} cm")
    for name, pose in (("home", Pose(90.0, 30.0)), ("straight out", Pose(90.0, 0.0)),
                       ("folded", Pose(90.0, 150.0)), ("aimed high", Pose(140.0, 60.0)),
                       ("aimed low", Pose(40.0, 90.0))):
        x, y = forward_kinematics(pose)
        ex, ey = elbow_point(pose)
        print(f"{name:<13} shoulder {pose.shoulder_deg:5.1f}  elbow {pose.elbow_deg:5.1f}"
              f"   elbow at ({ex:6.2f},{ey:6.2f})   grip at ({x:6.2f},{y:6.2f})"
              f"   {math.hypot(x, y):5.2f} cm out")

$ uv run python -m labs.kinematics
upper arm 10.0 cm, forearm 7.0 cm, reach 3.0 to 17.0 cm
home          shoulder  90.0  elbow  30.0   elbow at ( 10.00,  0.00)   grip at ( 16.06,  3.50)   16.44 cm out
straight out  shoulder  90.0  elbow   0.0   elbow at ( 10.00,  0.00)   grip at ( 17.00,  0.00)   17.00 cm out
folded        shoulder  90.0  elbow 150.0   elbow at ( 10.00,  0.00)   grip at (  3.94,  3.50)    5.27 cm out
aimed high    shoulder 140.0  elbow  60.0   elbow at (  6.43,  7.66)   grip at (  4.03, 14.24)   14.80 cm out
aimed low     shoulder  40.0  elbow  90.0   elbow at (  6.43, -7.66)   grip at ( 11.79, -3.16)   12.21 cm out

This direction is a chain of additions and no unknowns at all. Walk 10 cm along whatever direction the shoulder is pointing, turn by the elbow's bend, walk 7 cm more, and you are at the jaws. The only subtlety is the constant: a shoulder reading of 90 puts the upper arm straight out along x, so the geometry works in shoulder_deg - 90 while the servo works in the 15 to 165 band chapter 69 measured. That offset appears in exactly two functions here, and later in exactly one more.

Two rows are the useful ones. The home pose puts the jaws 16.44 cm from the shoulder, almost fully stretched out, and folding the elbow to its 150 limit pulls them back to 5.27 cm; that span is the working depth of this arm. The last row is the one to remember: an elbow pivot at y = -7.66 is 1.66 cm underneath a tabletop that sits at -6.0, and the grip point in the same row is comfortably above it. Position the fingertip correctly and you can still put the elbow through the desk.

# labs/ik_walkthrough.py
import math

from labs.kinematics import L1_CM, L2_CM, Pose, forward_kinematics

x, y = 12.0, 5.0
dist = math.hypot(x, y)
cos_elbow = (dist ** 2 - L1_CM ** 2 - L2_CM ** 2) / (2 * L1_CM * L2_CM)
bend = math.acos(cos_elbow)

k1 = L1_CM + L2_CM * math.cos(bend)      # where the jaws land with the shoulder at zero
k2 = L2_CM * math.sin(bend)
aim = math.atan2(y, x) - math.atan2(k2, k1)

print(f"target ({x:.1f}, {y:.1f}) is {dist:.2f} cm from the shoulder")
print(f"cos(bend) = ({dist ** 2:.0f} - {L1_CM ** 2:.0f} - {L2_CM ** 2:.0f}) "
      f"/ {2 * L1_CM * L2_CM:.0f} = {cos_elbow:+.4f}   bend = {math.degrees(bend):.2f} deg")
print(f"shoulder at zero would put the grip at ({k1:.2f}, {k2:.2f}), "
      f"{math.hypot(k1, k2):.2f} cm out at {math.degrees(math.atan2(k2, k1)):.2f} deg")
print(f"the target sits at {math.degrees(math.atan2(y, x)):.2f} deg, "
      f"so the shoulder turns {math.degrees(aim):+.2f} deg from its zero")
print(f"shoulder = {math.degrees(aim) + 90.0:.2f} deg,  elbow = {math.degrees(bend):.2f} deg")
gx, gy = forward_kinematics(Pose(math.degrees(aim) + 90.0, math.degrees(bend)))
print(f"forward kinematics on that pose: ({gx:.2f}, {gy:.2f})")

$ uv run python -m labs.ik_walkthrough
target (12.0, 5.0) is 13.00 cm from the shoulder
cos(bend) = (169 - 100 - 49) / 140 = +0.1429   bend = 81.79 deg
shoulder at zero would put the grip at (11.00, 6.93), 13.00 cm out at 32.20 deg
the target sits at 22.62 deg, so the shoulder turns -9.58 deg from its zero
shoulder = 80.42 deg,  elbow = 81.79 deg
forward kinematics on that pose: (12.00, 5.00)

The middle two lines are the whole trick for the shoulder, and they are less clever than they look. Once the elbow's bend is fixed the arm is a rigid bent stick, so bolt it to the shoulder at the shoulder's zero and ask where the jaws land: at k1 = l1 + l2·cos(bend) across and k2 = l2·sin(bend) up, which for this target is (11.00, 6.93). That point is 13.00 cm from the shoulder, the same 13.00 the target is, because the stick did not change length. It is merely pointing 32.20 degrees off, when the target lies at 22.62. Rotate the whole stick by the difference, -9.58 degrees, and the jaws swing onto the target. Add the 90 that the shoulder calls zero and the servo wants 80.42.

math.atan2 and not math.atan, both times. Plain atan(y / x) divides by zero on any target directly above the shoulder and gives the same answer for a point in front and a point behind, since dividing two negatives loses both signs. atan2 is handed the two numbers separately and keeps them, so it can tell 22.62 degrees from 157.38.

# labs/kinematics.py — added below elbow_point


def solve(x: float, y: float, l1: float = L1_CM,
          l2: float = L2_CM) -> list[Pose]:
    """Every pose that puts the grip point on (x, y). Empty if no triangle closes."""
    dist = math.hypot(x, y)
    if dist > l1 + l2 or dist < abs(l1 - l2):
        return []
    cos_elbow = (dist ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
    cos_elbow = max(-1.0, min(1.0, cos_elbow))       # pin float drift into acos's domain
    elbow = math.acos(cos_elbow)
    poses = []
    for bend in (elbow, -elbow):
        k1 = l1 + l2 * math.cos(bend)
        k2 = l2 * math.sin(bend)
        aim = math.atan2(y, x) - math.atan2(k2, k1)
        poses.append(Pose(math.degrees(aim) + SHOULDER_ZERO_DEG, math.degrees(bend)))
    return poses

# labs/ik_branches.py
import math

from labs.kinematics import MAX_REACH_CM, MIN_REACH_CM, forward_kinematics, solve

for tx, ty in ((12.0, 5.0), (11.0, -2.0), (16.0, 8.0), (2.0, 0.0)):
    dist = math.hypot(tx, ty)
    poses = solve(tx, ty)
    if not poses:
        print(f"({tx:5.1f},{ty:5.1f}) {dist:6.2f} cm   no triangle closes "
              f"({MIN_REACH_CM:.1f} to {MAX_REACH_CM:.1f} cm)")
        continue
    for pose in poses:
        gx, gy = forward_kinematics(pose)
        print(f"({tx:5.1f},{ty:5.1f}) {dist:6.2f} cm   shoulder {pose.shoulder_deg:7.2f}"
              f"   elbow {pose.elbow_deg:7.2f}   back through FK ({gx:6.2f},{gy:6.2f})")

$ uv run python -m labs.ik_branches
( 12.0,  5.0)  13.00 cm   shoulder   80.42   elbow   81.79   back through FK ( 12.00,  5.00)
( 12.0,  5.0)  13.00 cm   shoulder  144.82   elbow  -81.79   back through FK ( 12.00,  5.00)
( 11.0, -2.0)  11.18 cm   shoulder   41.61   elbow   99.87   back through FK ( 11.00, -2.00)
( 11.0, -2.0)  11.18 cm   shoulder  117.78   elbow  -99.87   back through FK ( 11.00, -2.00)
( 16.0,  8.0)  17.89 cm   no triangle closes (3.0 to 17.0 cm)
(  2.0,  0.0)   2.00 cm   no triangle closes (3.0 to 17.0 cm)

Two rows per target, and the fourth column proves both of them. Negating the bend mirrors the elbow across the straight line from the shoulder to the target, and because the mirror image of a triangle is still that triangle, the jaws land in the same place. The sign flips inside k2, which flips the sign of the correction, which swings the shoulder up instead of down. One target, two honest answers, and nothing in the trigonometry that prefers either.

The two rejected targets fail before any trigonometry runs, and that ordering is the point of the guard. A target 17.89 cm out gives a cosine of +1.2214, and 2.00 cm in gives -1.0357. Both are outside the interval math.acos accepts, so without the guard the run ends in ValueError: math domain error from somewhere deep inside a solver, for a target whose problem was obvious from its distance alone. The clamp on the next line is a second, quieter defence: a target sitting exactly on the 17.0 cm boundary can round to 1.0000000002 on the way through the arithmetic, and pinning it back to 1.0 costs nothing and keeps a legal target from raising.

# labs/arm_reach.py
"""A solved pose is a proposal. This module decides whether the arm can hold it."""
import math

from labs.arm_config import JOINTS
from labs.kinematics import (MAX_REACH_CM, MIN_REACH_CM, Pose, elbow_point,
                             forward_kinematics, solve)

TABLE_Y_CM = -6.0        # the tabletop, measured down from the shoulder pivot
CLEARANCE_CM = 0.5       # how close to it anything is allowed to come
FLOOR_Y_CM = TABLE_Y_CM + CLEARANCE_CM


def blocked_because(pose: Pose) -> str | None:
    """The first reason this arm cannot hold this pose, or None if it can."""
    for name, degrees in (("shoulder", pose.shoulder_deg), ("elbow", pose.elbow_deg)):
        cfg = JOINTS[name]
        if not cfg.min_deg <= degrees <= cfg.max_deg:
            return (f"{name} {degrees:.1f} is outside its "
                    f"{cfg.min_deg:.0f}-{cfg.max_deg:.0f} band")
    for label, (_, height) in (("elbow pivot", elbow_point(pose)),
                               ("grip point", forward_kinematics(pose))):
        if height < FLOOR_Y_CM:
            return f"{label} at y={height:.2f} is below the keep-out line at y={FLOOR_Y_CM:.2f}"
    return None


def plan_reach(x: float, y: float) -> tuple[Pose | None, list[str]]:
    """A pose the arm can hold that puts the grip point on (x, y), and why not."""
    poses = solve(x, y)
    if not poses:
        dist = math.hypot(x, y)
        edge = "past full stretch" if dist > MAX_REACH_CM else "inside the fold radius"
        return None, [f"{dist:.2f} cm from the shoulder is {edge} "
                      f"({MIN_REACH_CM:.1f} to {MAX_REACH_CM:.1f} cm)"]
    refused = []
    for pose in poses:
        reason = blocked_because(pose)
        if reason is None:
            return pose, refused
        refused.append(reason)
    return None, refused

Nothing in this module knows any trigonometry and nothing in labs/kinematics.py knows this arm exists. That split is deliberate. The solver answers a question about two lengths and a point, which would have the same answer on any planar arm anywhere; blocked_because answers a question about printed brackets, a servo's travel and a particular desk. The band numbers are read straight out of the JOINTS config from chapter 69, so recalibrating a joint changes what is reachable without a line of this file being touched.

# labs/arm_reach.py — continued

if __name__ == "__main__":
    print(f"tabletop at y={TABLE_Y_CM:.1f} cm, nothing allowed below y={FLOOR_Y_CM:.1f} cm")
    for tx, ty in ((12.0, 5.0), (3.0, 12.0), (11.0, -2.0),
                   (-5.0, 12.0), (16.0, 8.0), (2.0, 0.0)):
        pose, refused = plan_reach(tx, ty)
        head = f"({tx:6.1f},{ty:6.1f})  {math.hypot(tx, ty):5.2f} cm"
        if pose is None:
            print(f"{head}  ->  no pose this arm can hold")
            for reason in refused:
                print(f"{'':>21}   rejected: {reason}")
        else:
            gx, gy = forward_kinematics(pose)
            print(f"{head}  ->  shoulder {pose.shoulder_deg:6.2f}  "
                  f"elbow {pose.elbow_deg:6.2f}   FK ({gx:6.2f},{gy:6.2f})")

$ uv run python -m labs.arm_reach
tabletop at y=-6.0 cm, nothing allowed below y=-5.5 cm
(  12.0,   5.0)  13.00 cm  ->  shoulder  80.42  elbow  81.79   FK ( 12.00,  5.00)
(   3.0,  12.0)  12.37 cm  ->  shoulder 131.51  elbow  88.36   FK (  3.00, 12.00)
(  11.0,  -2.0)  11.18 cm  ->  no pose this arm can hold
                        rejected: elbow pivot at y=-7.48 is below the keep-out line at y=-5.50
                        rejected: elbow -99.9 is outside its 0-150 band
(  -5.0,  12.0)  13.00 cm  ->  no pose this arm can hold
                        rejected: shoulder 170.4 is outside its 15-165 band
                        rejected: shoulder 234.8 is outside its 15-165 band
(  16.0,   8.0)  17.89 cm  ->  no pose this arm can hold
                        rejected: 17.89 cm from the shoulder is past full stretch (3.0 to 17.0 cm)
(   2.0,   0.0)   2.00 cm  ->  no pose this arm can hold
                        rejected: 2.00 cm from the shoulder is inside the fold radius (3.0 to 17.0 cm)

Read the third target slowly, because it is the one that changes how you think about a solved pose. Both answers put the jaws on (11.0, -2.0) and both are perfectly good trigonometry. The first bends the elbow 99.87 degrees, well inside the 0 to 150 band, and drops the elbow pivot to y = -7.48, roughly a centimetre and a half inside the desk. The second holds the elbow pivot up at y = 4.66 and clears everything, and asks the elbow for -99.87 degrees. The elbow servo's band starts at 0, so the joint that would keep the arm off the table is the joint that cannot bend that way at all. A point 11 cm out and 2 cm down is not reachable on this arm, and no amount of better solving will change that.

The fourth target fails differently: a point up and behind the shoulder needs 170.4 degrees, and the printed bracket takes everything past 165. Its mirror answer wants 234.8, which is not even a servo angle. Between them the six rows cover every way a point can be refused: too far, too close, past a joint stop, and into the furniture.