2-Link Arm: Inverse Kinematics

move cursor to target, click to flip elbow

A planar two-link manipulator anchored at the canvas center, with link lengths $L_{1}$ and $L_{2}$ . The end effector tracks the cursor via closed-form inverse kinematics. Given a target $(x, y)$ relative to the shoulder with $r^{2} = x^{2} + y^{2}$ , the elbow angle comes from the law of cosines: $θ_{2} = \pm arccos (\frac{r ^{2} - L _{1}^{2} - L _{2}^{2}}{2 L _{1} L _{2}})$ , and the shoulder angle is $θ_{1} = atan2 (y, x) - atan2 (L_{2} sin θ_{2}, L_{1} + L_{2} cos θ_{2})$ . The $\pm$ branch picks between two physically valid postures — 'elbow-up' and 'elbow-down' — drawn simultaneously, the non-primary at half alpha. The end effector is reachable iff $r \in [∣ L_{1} - L_{2} ∣, L_{1} + L_{2}]$ (the dashed annulus); outside it, the arm stretches collinearly and tints red. Click anywhere to swap which branch is primary, exposing the configuration-space discontinuity at the workspace boundary where both branches coincide.

idle

169 lines · vanilla

view source

// 2-Link planar arm with closed-form inverse kinematics.
// Anchored at canvas center. End effector tracks cursor.
// Given target (x, y) with r^2 = x^2 + y^2:
//   theta2 = +/- acos((r^2 - L1^2 - L2^2) / (2 L1 L2))
//   theta1 = atan2(y, x) - atan2(L2 sin(theta2), L1 + L2 cos(theta2))
// Two valid configs: "elbow up" (theta2 > 0) and "elbow down" (theta2 < 0).
// Both rendered at half alpha; primary is drawn fully. Click to toggle which
// is primary. When out of reachable annulus [|L1-L2|, L1+L2], arm stretches
// collinear toward the target and tints red.

let W = 0, H = 0;
let cx = 0, cy = 0;
let L1 = 0, L2 = 0;
let primaryUp = true; // true = elbow-up primary; false = elbow-down
let buttonRect = null; // { x, y, w, h }

function recompute(width, height) {
  W = width;
  H = height;
  cx = W * 0.5;
  cy = H * 0.55;
  const m = Math.min(W, H);
  L1 = 0.45 * m;
  L2 = 0.35 * m;
  buttonRect = { x: 12, y: 12, w: 160, h: 28 };
}

function init({ width, height, ctx }) {
  recompute(width, height);
  ctx.fillStyle = "#0a0c14";
  ctx.fillRect(0, 0, W, H);
}

// Solve IK. Returns { theta1, theta2, reachable }.
// If unreachable, returns the collinear-clamped angle in theta1 and theta2 = 0.
function solveIK(dx, dy, elbowUp) {
  const r2 = dx * dx + dy * dy;
  const r = Math.sqrt(r2);
  const rMin = Math.abs(L1 - L2);
  const rMax = L1 + L2;

  if (r > rMax + 1e-6 || r < rMin - 1e-6) {
    // unreachable: collinear stretch toward target
    const theta1 = Math.atan2(dy, dx);
    // For r > rMax: arm fully extended, theta2 = 0 (collinear out).
    // For r < rMin: arm fully folded, theta2 = pi (collinear back), but the
    // visual is least ugly if we point both links toward the target — use
    // theta2 = 0 anchored along theta1, which lands beyond rMin but inside
    // rMax. Either way, end effector won't reach.
    return { theta1, theta2: 0, reachable: false };
  }

  // cos(theta2) clamp guards floating noise.
  let c2 = (r2 - L1 * L1 - L2 * L2) / (2 * L1 * L2);
  if (c2 > 1) c2 = 1;
  if (c2 < -1) c2 = -1;
  const s2mag = Math.sqrt(Math.max(0, 1 - c2 * c2));
  const theta2 = (elbowUp ? +1 : -1) * Math.atan2(s2mag, c2);
  const theta1 = Math.atan2(dy, dx) - Math.atan2(L2 * Math.sin(theta2), L1 + L2 * Math.cos(theta2));
  return { theta1, theta2, reachable: true };
}

function drawArm(ctx, sol, alpha, isPrimary, unreachable) {
  const baseR = unreachable ? 235 : 90;
  const baseG = unreachable ? 70 : 170;
  const baseB = unreachable ? 95 : 240;

  const j1x = cx + L1 * Math.cos(sol.theta1);
  const j1y = cy + L1 * Math.sin(sol.theta1);
  const eeAng = sol.theta1 + sol.theta2;
  const eex = j1x + L2 * Math.cos(eeAng);
  const eey = j1y + L2 * Math.sin(eeAng);

  ctx.lineCap = "round";
  ctx.lineJoin = "round";

  // upper arm
  ctx.strokeStyle = `rgba(${baseR}, ${baseG}, ${baseB}, ${alpha})`;
  ctx.lineWidth = isPrimary ? 9 : 6;
  ctx.beginPath();
  ctx.moveTo(cx, cy);
  ctx.lineTo(j1x, j1y);
  ctx.stroke();

  // forearm
  const fR = unreachable ? 235 : 140;
  const fG = unreachable ? 110 : 210;
  const fB = unreachable ? 130 : 255;
  ctx.strokeStyle = `rgba(${fR}, ${fG}, ${fB}, ${alpha})`;
  ctx.lineWidth = isPrimary ? 7 : 5;
  ctx.beginPath();
  ctx.moveTo(j1x, j1y);
  ctx.lineTo(eex, eey);
  ctx.stroke();

  // elbow joint
  ctx.fillStyle = `rgba(230, 230, 250, ${alpha})`;
  ctx.beginPath();
  ctx.arc(j1x, j1y, isPrimary ? 7 : 5, 0, Math.PI * 2);
  ctx.fill();

  // end effector
  ctx.fillStyle = unreachable
    ? `rgba(255, 120, 130, ${alpha})`
    : `rgba(255, 220, 130, ${alpha})`;
  ctx.beginPath();
  ctx.arc(eex, eey, isPrimary ? 6 : 4, 0, Math.PI * 2);
  ctx.fill();

  return { eex, eey };
}

function pointInRect(x, y, r) {
  return r && x >= r.x && x <= r.x + r.w && y >= r.y && y <= r.y + r.h;
}

function tick({ ctx, width, height, input }) {
  if (width !== W || height !== H) recompute(width, height);

  // background
  ctx.fillStyle = "#0a0c14";
  ctx.fillRect(0, 0, W, H);

  // faint grid
  ctx.strokeStyle = "rgba(120,140,180,0.06)";
  ctx.lineWidth = 1;
  ctx.beginPath();
  for (let x = 0; x < W; x += 40) { ctx.moveTo(x, 0); ctx.lineTo(x, H); }
  for (let y = 0; y < H; y += 40) { ctx.moveTo(0, y); ctx.lineTo(0 + W, y); }
  ctx.stroke();

  // reachable annulus boundary
  const rMin = Math.abs(L1 - L2);
  const rMax = L1 + L2;
  ctx.strokeStyle = "rgba(160, 200, 255, 0.18)";
  ctx.setLineDash([6, 6]);
  ctx.lineWidth = 1;
  ctx.beginPath();
  ctx.arc(cx, cy, rMax, 0, Math.PI * 2);
  ctx.stroke();
  if (rMin > 1) {
    ctx.beginPath();
    ctx.arc(cx, cy, rMin, 0, Math.PI * 2);
    ctx.stroke();
  }
  ctx.setLineDash([]);

  // target relative to shoulder
  const tx = input.mouseX - cx;
  const ty = input.mouseY - cy;

  // Process clicks. Toggle elbow only when click is NOT on the button
  // (button click also toggles, so a single handler is fine — but we want
  // both to work, so we just toggle on any click that is inside the canvas).
  for (const c of input.consumeClicks()) {
    // If user clicks the explicit button area, always toggle.
    // If user clicks elsewhere on canvas, also toggle (per spec).
    if (pointInRect(c.x, c.y, buttonRect)) {
      primaryUp = !primaryUp;
    } else {
      primaryUp = !primaryUp;
    }
  }

  const upSol = solveIK(tx, ty, true);
  const downSol = solveIK(tx, ty, false);
  const reachable = upSol.reachable && downSol.reachable;

  // Draw non-primary at half alpha first, then primary on top.
  let primaryEE;
  if (primaryUp) {
    drawArm(ctx, downSol, 0.35, false, !reachable);
    primaryEE = drawArm(ctx, upSol, 1.0, true, !reachable);
  } else {
    drawArm(ctx, upSol, 0.35, false, !reachable);
    primaryEE = drawArm(ctx, downSol, 1.0, true, !reachable);
  }

  // shoulder pivot
  ctx.fillStyle = "rgba(220, 230, 255, 0.95)";
  ctx.strokeStyle = "rgba(255, 255, 255, 0.6)";
  ctx.lineWidth = 1.5;
  ctx.beginPath();
  ctx.arc(cx, cy, 9, 0, Math.PI * 2);
  ctx.fill();
  ctx.stroke();

  // target crosshair on cursor (only meaningful inside canvas)
  if (input.mouseX >= 0 && input.mouseY >= 0 && input.mouseX <= W && input.mouseY <= H) {
    ctx.strokeStyle = reachable ? "rgba(255, 220, 130, 0.55)" : "rgba(255, 120, 130, 0.7)";
    ctx.lineWidth = 1;
    const s = 8;
    ctx.beginPath();
    ctx.moveTo(input.mouseX - s, input.mouseY);
    ctx.lineTo(input.mouseX + s, input.mouseY);
    ctx.moveTo(input.mouseX, input.mouseY - s);
    ctx.lineTo(input.mouseX, input.mouseY + s);
    ctx.stroke();
  }

  // Toggle button (top-left)
  const b = buttonRect;
  ctx.fillStyle = "rgba(30, 40, 60, 0.85)";
  ctx.fillRect(b.x, b.y, b.w, b.h);
  ctx.strokeStyle = "rgba(160, 200, 255, 0.55)";
  ctx.lineWidth = 1;
  ctx.strokeRect(b.x + 0.5, b.y + 0.5, b.w - 1, b.h - 1);
  ctx.fillStyle = "rgba(230, 240, 255, 0.95)";
  ctx.font = "13px ui-monospace, monospace";
  ctx.textBaseline = "middle";
  ctx.fillText(`primary: elbow-${primaryUp ? "up" : "down"}`, b.x + 10, b.y + b.h / 2);
  ctx.textBaseline = "alphabetic";

  // HUD: angles in degrees of the primary solution
  const sol = primaryUp ? upSol : downSol;
  const t1deg = sol.theta1 * 180 / Math.PI;
  const t2deg = sol.theta2 * 180 / Math.PI;
  const r = Math.hypot(tx, ty);

  ctx.fillStyle = "rgba(220, 230, 250, 0.92)";
  ctx.font = "13px ui-monospace, monospace";
  let hy = b.y + b.h + 18;
  ctx.fillText(`theta1 = ${t1deg.toFixed(1)} deg`, 12, hy); hy += 16;
  ctx.fillText(`theta2 = ${t2deg.toFixed(1)} deg`, 12, hy); hy += 16;
  ctx.fillText(`L1 = ${L1.toFixed(0)}  L2 = ${L2.toFixed(0)}`, 12, hy); hy += 16;
  ctx.fillText(`r = ${r.toFixed(0)} / [${rMin.toFixed(0)}, ${rMax.toFixed(0)}]`, 12, hy); hy += 16;
  if (!reachable) {
    ctx.fillStyle = "rgba(255, 130, 140, 0.95)";
    ctx.fillText("unreachable — clamped collinear", 12, hy);
  }

  // legend
  ctx.fillStyle = "rgba(200, 210, 230, 0.55)";
  ctx.fillText("click anywhere to flip elbow", W - 240, H - 14);
}

2-Link Arm: Inverse Kinematics

Comments (0)