osiu97/ESP32-WijiBoard
1
0
Fork 0
Code Issues 10 Pull Requests Packages Projects Releases Wiki Activity

fix kinematics - maybe?

Browse Source
This commit is contained in:
PROFERIS - Mi³osz Stocki
2026-07-08 11:55:09 +02:00
parent 1fe1b9fd0b
commit f898883ff2
3 changed files with 85 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gemini.md
+24 -26
View File
@@ -41,21 +41,19 @@ only BLE between the ESP32 firmware and a local web SPA.
ELBOW1 ELBOW2 ELBOW1 ELBOW2
\ / \ /
l1 (85 mm) l1 (85 mm) l1 (85 mm) l1 (85 mm)
\ / \ /
MOTOR1 (+12.9,0) MOTOR2 (-12.9,0) MOTOR1 (-12.9,0) MOTOR2 (+12.9,0)
| | | |
[===BASE (25.8 mm)===] [===BASE (25.8 mm)===]
``` ```
**⚠️ CRITICAL MOTOR CONVENTION** (source of many past bugs): **⚠️ CRITICAL MOTOR CONVENTION** (source of many past bugs):
`PositionControl.cpp` from the original project uses: The original C++ firmware has a variable swap bug where `angle2` is passed to `stepper1` and `angle1` to `stepper2`. Thus, the physical mapping is:
- `xmd = x - d2` → Motor 1 pivot is at **(+d2, 0) = (+12.9, 0)** — the RIGHT side - **Motor 1** is the **LEFT** motor, pivoting at **(-d2, 0) = (-12.9, 0)**. It uses `xpd = x + d2`.
- `xpd = x + d2` → Motor 2 pivot is at **(-d2, 0) = (-12.9, 0)** — the LEFT side - **Motor 2** is the **RIGHT** motor, pivoting at **(+d2, 0) = (+12.9, 0)**. It uses `xmd = x - d2`.
This is counter-intuitive. Motor 1 is physically on the right. **Every piece of code that **Every piece of code that computes elbow or motor-base positions must use M1 at -12.9 and M2 at +12.9 or the arms will appear visually crossed and physically collide.**
computes elbow or motor-base positions must use M1 at +12.9 and M2 at -12.9 or the arms
will appear visually crossed even for valid target positions.**
### Arm constants (from `PositionControl.cpp`) ### Arm constants (from `PositionControl.cpp`)
@@ -184,20 +182,20 @@ hamburger (`☰`) button. It contains `<button data-route="...">` nav items that
### IK (`solve(x, y)`) ### IK (`solve(x, y)`)
Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`: Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`, with motor swap applied:
```javascript ```javascript
// Motor 1 (pivot at +d2 = +12.9 mm) // Motor 1 (Left, pivot at -d2 = -12.9 mm)
const xmd = x - d2; // Δx from M1 pivot const xpd = x + d2; // Δx from M1 pivot
const s = hypot(xmd, y);
const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
const theta1 = atan2(y, xmd) - acos(cosW1);
// Motor 2 (pivot at -d2 = -12.9 mm)
const xpd = x + d2; // Δx from M2 pivot
const t = hypot(xpd, y); const t = hypot(xpd, y);
const cosW2 = (l2² - t² - l1²) / (-2·l1·t); const cosW2 = (l2² - t² - l1²) / (-2·l1·t);
const theta2 = atan2(y, xpd) + acos(cosW2); const theta1 = atan2(y, xpd) + acos(cosW2);
// Motor 2 (Right, pivot at +d2 = +12.9 mm)
const xmd = x - d2; // Δx from M2 pivot
const s = hypot(xmd, y);
const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
const theta2 = atan2(y, xmd) - acos(cosW1);
``` ```
Returns `{ theta1, theta2, reachable }` — angles in **radians**. Returns `{ theta1, theta2, reachable }` — angles in **radians**.
@@ -207,9 +205,9 @@ Returns `{ theta1, theta2, reachable }` — angles in **radians**.
Computes elbow and EE positions for visualisation: Computes elbow and EE positions for visualisation:
```javascript ```javascript
// M1 at +d2, M2 at -d2 ← critical sign convention // M1 at -d2, M2 at +d2 ← critical sign convention
elbow1 = { x: +d2 + l1·cos(theta1), y: l1·sin(theta1) } elbow1 = { x: -d2 + l1·cos(theta1), y: l1·sin(theta1) }
elbow2 = { x: -d2 + l1·cos(theta2), y: l1·sin(theta2) } elbow2 = { x: +d2 + l1·cos(theta2), y: l1·sin(theta2) }
// EE = upward circle-circle intersection of circles (elbow1, l2) and (elbow2, l2) // EE = upward circle-circle intersection of circles (elbow1, l2) and (elbow2, l2)
``` ```
@@ -294,9 +292,9 @@ const SV = {
### Motor marker positions in SVG (match FK convention) ### Motor marker positions in SVG (match FK convention)
```javascript ```javascript
// M1 at +d2 (right side), M2 at -d2 (left side) // M1 at -d2 (left side), M2 at +d2 (right side)
motor1sx = SV.wx(+IK.ARM.d2); motor1sx = SV.wx(-IK.ARM.d2);
motor2sx = SV.wx(-IK.ARM.d2); motor2sx = SV.wx(+IK.ARM.d2);
``` ```
### IK click-to-move flow ### IK click-to-move flow
@@ -419,7 +417,7 @@ To add a new section:
| **No WiFi / no server** | User requirement — BLE only, no hosting needed | | **No WiFi / no server** | User requirement — BLE only, no hosting needed |
| **`?v=Date.now()` cache-buster** | ES module imports are cached by the browser; without this, stale code runs silently after edits | | **`?v=Date.now()` cache-buster** | ES module imports are cached by the browser; without this, stale code runs silently after edits |
| **Steps as primary state** | The firmware tracks absolute step counts. Angles are derived by `stepsToRad()`. This keeps web ↔ firmware in sync. | | **Steps as primary state** | The firmware tracks absolute step counts. Angles are derived by `stepsToRad()`. This keeps web ↔ firmware in sync. |
| **M1 at +d2, M2 at -d2** | This is the PositionControl.cpp convention. Reversing it causes visual arm crossing. Verified by tracing `xmd = x - d2` (offset from +d2 pivot) and `xpd = x + d2` (offset from -d2 pivot). | | **M1 at -d2, M2 at +d2** | Corrects a hardware variable swap in C++ firmware where angle2 goes to stepper1. Verified by tracing `xmd = x - d2` to the right motor and `xpd = x + d2` to the left motor. |
| **`forward()` picks upward EE** | Two circle-circle intersections exist; the mechanism always operates in the "above" configuration. The lower solution is physically blocked by the board. | | **`forward()` picks upward EE** | Two circle-circle intersections exist; the mechanism always operates in the "above" configuration. The lower solution is physically blocked by the board. |
| **`armsCrossed()` guard** | Prevents IK moves that would drive an elbow into the mechanism housing from the wrong side. Tunable via `LIMITS.ELBOW_BOX_X_INNER` and `ELBOW_BOX_Y_MAX`. | | **`armsCrossed()` guard** | Prevents IK moves that would drive an elbow into the mechanism housing from the wrong side. Tunable via `LIMITS.ELBOW_BOX_X_INNER` and `ELBOW_BOX_Y_MAX`. |
| **Linux/BlueZ stability** | `pAdv->setMaxPreferred(0x0C)` is mandatory in firmware. Without it, Linux/ChromeOS will drop the connection immediately after the handshake. | | **Linux/BlueZ stability** | `pAdv->setMaxPreferred(0x0C)` is mandatory in firmware. Without it, Linux/ChromeOS will drop the connection immediately after the handshake. |
@@ -437,7 +435,7 @@ To add a new section:
assume a specific physical motor orientation. If the real arm moves in the wrong direction assume a specific physical motor orientation. If the real arm moves in the wrong direction
when clicking a target, negate the step delta for that motor in `moveToXY()`. when clicking a target, negate the step delta for that motor in `moveToXY()`.
- **Home position**: Stall homing is fully implemented matching the original C++ routine but adjusted to align with `PositionControl.cpp` motor mappings. Homing pushes the motors against their physical limits and sets `steps1 = 0` (0°, points right) and `steps2 = -1024` (-180°, points left). This corresponds to the arms folded OUTWARDS. - **Home position**: Stall homing is fully implemented matching the original C++ routine but adjusted to align with physical motor mappings. Homing pushes the motors against their physical limits and sets `steps1 = -1024` (-180°, points left) and `steps2 = 0` (0°, points right). This corresponds to the arms folded OUTWARDS.
- The initial web simulation starts with these coordinates, meaning the UI assumes the arm has already been homed before the browser connects. - The initial web simulation starts with these coordinates, meaning the UI assumes the arm has already been homed before the browser connects.
- Homing can be triggered individually per-motor (`HOME1`, `HOME2`) or combined (`HOMEALL`) from the web UI. - Homing can be triggered individually per-motor (`HOME1`, `HOME2`) or combined (`HOMEALL`) from the web UI.
web/js/kinematics.js
+43 -42
View File
@@ -18,15 +18,16 @@
* d2=12.9 mm * d2=12.9 mm
* *
* ── IMPORTANT: PositionControl.cpp motor convention ───────────── * ── IMPORTANT: PositionControl.cpp motor convention ─────────────
* Motor 1 pivot is at (+d2, 0) = (+12.9, 0) [right side!] * Motor 1 pivot is at (-d2, 0) = (-12.9, 0) [left side!]
* Motor 2 pivot is at (-d2, 0) = (-12.9, 0) [left side!] * Motor 2 pivot is at (+d2, 0) = (+12.9, 0) [right side!]
* *
* This is determined by how the C++ IK formulas use the offsets: * This is determined by a variable swap in the C++ firmware where
* Motor 1: xmd = x - d2 → target is measured from x = +d2 * angle2 is passed to stepper1 and angle1 to stepper2. Thus:
* Motor 2: xpd = x + d2 → target is measured from x = -d2 * Motor 1 (Left): xpd = x + d2 → target is measured from x = -d2
* Motor 2 (Right): xmd = x - d2 → target is measured from x = +d2
* *
* All FK / visualisation code MUST use this same convention or * All FK / visualisation code MUST use this exact convention or
* the arms will appear visually crossed even for valid positions. * the arms will appear visually crossed and physically collide.
* *
* Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard) * Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard)
*/ */
@@ -34,12 +35,12 @@
// ── Exact constants from PositionControl.cpp ───────────────────── // ── Exact constants from PositionControl.cpp ─────────────────────
const ARM = { const ARM = {
d: 25.8, // full motor separation (mm) d: 25.8, // full motor separation (mm)
d2: 12.9, // half separation; M1 at (+d2, 0), M2 at (-d2, 0) d2: 12.9, // half separation; M1 at (-d2, 0), M2 at (+d2, 0)
l1: 85.0, // proximal link length (mm) l1: 85.0, // proximal link length (mm)
l2: 110.0, // distal link length (mm) l2: 110.0, // distal link length (mm)
STEPS_PER_REV: 2048, STEPS_PER_REV: 2048,
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125 °/step STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125 °/step
HOME_STEPS: { m1: 0, m2: -1024 }, // M1 at 0°, M2 at -180° (arms folded outward) HOME_STEPS: { m1: -1024, m2: 0 }, // M1 (Left) at -180°, M2 (Right) at 0° (arms folded outward)
}; };
// ── WORKSPACE CONSTRAINTS ───────────────────────────────────────── // ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
@@ -62,10 +63,10 @@ const LIMITS = {
// ── Elbow exclusion zone (prevents arm crossing near the box) ─ // ── Elbow exclusion zone (prevents arm crossing near the box) ─
// Each elbow has a separate rectangular exclusion box. // Each elbow has a separate rectangular exclusion box.
// Left-side elbow (from M1 at +d2): must NOT enter this region. // Left-side elbow (from M1 at -d2): must NOT enter this region.
// Right-side elbow (from M2 at -d2): uses mirrored X limits. // Right-side elbow (from M2 at +d2): uses mirrored X limits.
// If ELBOW_BOX_X_INNER is 5, the left elbow's X must be > +5 mm // If ELBOW_BOX_X_INNER is 5, the left elbow's X must be < -5 mm
// (can never cross to the other side of the box mid-point). // (can never cross to the right side of the box mid-point).
ELBOW_BOX_X_INNER: 5, // ← TUNE: inner X margin from centre for each elbow ELBOW_BOX_X_INNER: 5, // ← TUNE: inner X margin from centre for each elbow
ELBOW_BOX_Y_MAX: 50, // ← TUNE: Y below which elbow crossing is forbidden ELBOW_BOX_Y_MAX: 50, // ← TUNE: Y below which elbow crossing is forbidden
}; };
@@ -130,19 +131,8 @@ const LOOKUP_TABLE = {
function solve(x, y) { function solve(x, y) {
const { d2, l1, l2 } = ARM; const { d2, l1, l2 } = ARM;
// ── Motor 1 (pivot at +d2, 0 = +12.9 mm) ───────────────────── // ── Motor 1 (Left, pivot at -d2, 0 = -12.9 mm) ───────────────
// xmd = x - d2 is the X component of (target – M1_pivot). // xpd = x + d2 is the X component of (target – M1_pivot).
const xmd = x - d2;
const s = Math.sqrt(xmd * xmd + y * y);
if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
const q = Math.atan2(y, xmd);
const w1 = Math.acos(cosW1);
const theta1 = q - w1;
// ── Motor 2 (pivot at -d2, 0 = -12.9 mm) ─────────────────────
// xpd = x + d2 is the X component of (target – M2_pivot).
const xpd = x + d2; const xpd = x + d2;
const t = Math.sqrt(xpd * xpd + y * y); const t = Math.sqrt(xpd * xpd + y * y);
if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false }; if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
@@ -150,7 +140,18 @@ function solve(x, y) {
if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false }; if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
const r = Math.atan2(y, xpd); const r = Math.atan2(y, xpd);
const w2 = Math.acos(cosW2); const w2 = Math.acos(cosW2);
const theta2 = r + w2; const theta1 = r + w2;
// ── Motor 2 (Right, pivot at +d2, 0 = +12.9 mm) ──────────────
// xmd = x - d2 is the X component of (target – M2_pivot).
const xmd = x - d2;
const s = Math.sqrt(xmd * xmd + y * y);
if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
const q = Math.atan2(y, xmd);
const w1 = Math.acos(cosW1);
const theta2 = q - w1;
return { theta1, theta2, reachable: true }; return { theta1, theta2, reachable: true };
} }
@@ -174,17 +175,17 @@ function solve(x, y) {
function forward(theta1, theta2) { function forward(theta1, theta2) {
const { d2, l1, l2 } = ARM; const { d2, l1, l2 } = ARM;
// ── IMPORTANT: match PositionControl.cpp motor convention ────── // ── IMPORTANT: match physical hardware convention ──────────────
// Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0). // Motor 1 pivot at (-d2, 0), Motor 2 pivot at (+d2, 0).
// Using the opposite sign here is the single most common source // Using the opposite sign here is the single most common source
// of visually-crossed arms in the SVG visualiser. // of visually-crossed arms in the SVG visualiser.
// Elbow 1 — tip of Motor 1 proximal link (motor at +d2) // Elbow 1 — tip of Motor 1 proximal link (motor at -d2, Left)
const e1x = +d2 + l1 * Math.cos(theta1); const e1x = -d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1); const e1y = l1 * Math.sin(theta1);
// Elbow 2 — tip of Motor 2 proximal link (motor at -d2) // Elbow 2 — tip of Motor 2 proximal link (motor at +d2, Right)
const e2x = -d2 + l1 * Math.cos(theta2); const e2x = +d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2); const e2y = l1 * Math.sin(theta2);
// End-effector: intersection of the two distal-link circles // End-effector: intersection of the two distal-link circles
@@ -234,9 +235,9 @@ function forward(theta1, theta2) {
* cross each other near the centre mechanism box. * cross each other near the centre mechanism box.
* *
* Physical rule: each elbow must stay on the OUTER side of the * Physical rule: each elbow must stay on the OUTER side of the
* mechanism housing. If elbow1 (from M1 at +d2) has a small * mechanism housing. If elbow1 (from M1 at -d2) has a small
* positive X at low Y, or elbow2 (from M2 at -d2) has a small * negative X at low Y, or elbow2 (from M2 at +d2) has a small
* negative X at low Y, the arm would collide with the housing. * positive X at low Y, the arm would collide with the housing.
* *
* @param {number} theta1 Motor 1 angle (rad) * @param {number} theta1 Motor 1 angle (rad)
* @param {number} theta2 Motor 2 angle (rad) * @param {number} theta2 Motor 2 angle (rad)
@@ -247,16 +248,16 @@ function armsCrossed(theta1, theta2) {
const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS; const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
// Elbow positions (same formula as forward()) // Elbow positions (same formula as forward())
const e1x = +d2 + l1 * Math.cos(theta1); const e1x = -d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1); const e1y = l1 * Math.sin(theta1);
const e2x = -d2 + l1 * Math.cos(theta2); const e2x = +d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2); const e2y = l1 * Math.sin(theta2);
// Elbow1 (from M1 on the RIGHT) must not appear far to the LEFT at low height // Elbow1 (from M1 on the LEFT) must not appear far to the RIGHT at low height
// Elbow2 (from M2 on the LEFT) must not appear far to the RIGHT at low height // Elbow2 (from M2 on the RIGHT) must not appear far to the LEFT at low height
// Both conditions together catch the "arms have swapped sides" scenario. // Both conditions together catch the "arms have swapped sides" scenario.
const e1_crossed = e1x < -XI && e1y < YM; // M1's elbow went too far left const e1_crossed = e1x > XI && e1y < YM; // M1's elbow went too far right
const e2_crossed = e2x > XI && e2y < YM; // M2's elbow went too far right const e2_crossed = e2x < -XI && e2y < YM; // M2's elbow went too far left
return e1_crossed || e2_crossed; return e1_crossed || e2_crossed;
} }
web/js/sections/stepper-test.js
+18 -18
View File
@@ -4,8 +4,8 @@
* *
* 5-bar parallel linkage visualiser + IK-based click-to-move. * 5-bar parallel linkage visualiser + IK-based click-to-move.
* *
* Mechanism geometry (from PositionControl.cpp): * Mechanism geometry (corrected from PositionControl.cpp bug):
* Motor 1 at (+12.9, 0) mm Motor 2 at (-12.9, 0) mm ← NOTE: M1 is on the RIGHT * Motor 1 at (-12.9, 0) mm Motor 2 at (+12.9, 0) mm ← NOTE: M1 is on the LEFT
* Proximal links: l1 = 85 mm Distal links: l2 = 110 mm * Proximal links: l1 = 85 mm Distal links: l2 = 110 mm
* *
* IK flow: click XY → IK.solve(x,y) → {θ1, θ2} → steps → BLE * IK flow: click XY → IK.solve(x,y) → {θ1, θ2} → steps → BLE
@@ -93,9 +93,9 @@ function setCircle(el, cx, cy) {
function renderArm(theta1, theta2, isGhost = false) { function renderArm(theta1, theta2, isGhost = false) {
const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2); const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2);
// Motor base SVG positions — M1 at +d2, M2 at -d2 (PositionControl.cpp convention) // Motor base SVG positions — M1 at -d2, M2 at +d2 (hardware convention)
const motor1sx = SV.wx(+IK.ARM.d2); const motor1sy = SV.wy(0); const motor1sx = SV.wx(-IK.ARM.d2); const motor1sy = SV.wy(0);
const motor2sx = SV.wx(-IK.ARM.d2); const motor2sy = SV.wy(0); const motor2sx = SV.wx(+IK.ARM.d2); const motor2sy = SV.wy(0);
const elbow1sx = SV.wx(e1.x); const elbow1sy = SV.wy(e1.y); const elbow1sx = SV.wx(e1.x); const elbow1sy = SV.wy(e1.y);
const elbow2sx = SV.wx(e2.x); const elbow2sy = SV.wy(e2.y); const elbow2sx = SV.wx(e2.x); const elbow2sy = SV.wy(e2.y);
const endsx = SV.wx(endX); const endsy = SV.wy(endY); const endsx = SV.wx(endX); const endsy = SV.wy(endY);
@@ -275,9 +275,9 @@ function buildZones() {
const ew = L.BOX_HALF_W * 2 * SV.SCALE; const ew = L.BOX_HALF_W * 2 * SV.SCALE;
const eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE; const eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE;
// Motor positions — M1 at +d2, M2 at -d2 // Motor positions — M1 at -d2, M2 at +d2
const m1x = SV.wx(+IK.ARM.d2); const m1x = SV.wx(-IK.ARM.d2);
const m2x = SV.wx(-IK.ARM.d2); const m2x = SV.wx(+IK.ARM.d2);
const my = SV.wy(0); const my = SV.wy(0);
// Scale ruler // Scale ruler
@@ -432,31 +432,31 @@ function buildHTML() {
font-family="monospace" style="display:none"/> font-family="monospace" style="display:none"/>
<!-- ── Real arm ──────────────────────────────────── --> <!-- ── Real arm ──────────────────────────────────── -->
<!-- Arm 1: proximal + distal (M1 at +d2) --> <!-- Arm 1: proximal + distal (M1 at -d2) -->
<line id="arm1-prox" <line id="arm1-prox"
x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}" x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}"
x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}" x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
stroke="#448aff" stroke-width="5" stroke-linecap="round"/> stroke="#448aff" stroke-width="5" stroke-linecap="round"/>
<line id="arm1-dist" <line id="arm1-dist"
x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}" x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
x2="${SV.wx(0)}" y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}" x2="${SV.wx(0)}" y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}"
stroke="#7eb8f7" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/> stroke="#7eb8f7" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/>
<!-- Arm 2: proximal + distal (M2 at -d2) --> <!-- Arm 2: proximal + distal (M2 at +d2) -->
<line id="arm2-prox" <line id="arm2-prox"
x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}" x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}"
x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}" x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
stroke="#f7a03c" stroke-width="5" stroke-linecap="round"/> stroke="#f7a03c" stroke-width="5" stroke-linecap="round"/>
<line id="arm2-dist" <line id="arm2-dist"
x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}" x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
x2="${SV.wx(0)}" y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}" x2="${SV.wx(0)}" y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}"
stroke="#ffd08a" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/> stroke="#ffd08a" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/>
<!-- Elbow joints --> <!-- Elbow joints -->
<circle id="elbow1" r="5" fill="#a0c4ff" stroke="#0a0e18" stroke-width="1.5" <circle id="elbow1" r="5" fill="#a0c4ff" stroke="#0a0e18" stroke-width="1.5"
cx="${SV.wx(+IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
<circle id="elbow2" r="5" fill="#ffd08a" stroke="#0a0e18" stroke-width="1.5"
cx="${SV.wx(-IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/> cx="${SV.wx(-IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
<circle id="elbow2" r="5" fill="#ffd08a" stroke="#0a0e18" stroke-width="1.5"
cx="${SV.wx(+IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
<!-- End effector --> <!-- End effector -->
<circle id="end-eff" r="7" <circle id="end-eff" r="7"