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

added proper kinematics

Browse Source
This commit is contained in:
PROFERIS - Mi³osz Stocki
2026-07-06 15:23:35 +02:00
parent 67b4eda6aa
commit ee8a2d97ed
5 changed files with 810 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/js/kinematics.js
+145 -80
View File
@@ -8,52 +8,65 @@
* / \
* L2 (110) L2 (110)
* / \
* ELBOW1 ELBOW2
* ELBOW1 ELBOW2
* \ /
* L1 (85) L1 (85)
* \ /
* MOTOR1 (-d2,0) MOTOR2 (+d2,0)
* MOTOR1 (+d2,0) MOTOR2 (-d2,0)
* | |
* [===BASE===] (d=25.8 mm wide)
* d2=12.9 mm
*
* Both motors are mounted in the centre mechanism box.
* Motor 1 is at (-d2, 0), Motor 2 is at (+d2, 0).
* Each motor drives a proximal arm (l1). The distal arms (l2)
* connect the elbows to the shared end-effector.
* ── IMPORTANT: PositionControl.cpp motor convention ─────────────
* Motor 1 pivot is at (+d2, 0) = (+12.9, 0) [right side!]
* Motor 2 pivot is at (-d2, 0) = (-12.9, 0) [left side!]
*
* IK: given target (x, y), solve θ1 and θ2 independently:
* Motor 1 sees the target at (x - d2, y) from its pivot.
* Motor 2 sees the target at (x + d2, y) from its pivot.
* Each uses the standard 2-link IK (law of cosines).
* This is determined by how the C++ IK formulas use the offsets:
* Motor 1: xmd = x - d2 → target is measured from x = +d2
* Motor 2: xpd = x + d2 → target is measured from x = -d2
*
* All FK / visualisation code MUST use this same convention or
* the arms will appear visually crossed even for valid positions.
*
* Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard)
*/
// ── Exact constants from PositionControl.cpp ─────────────────────
const ARM = {
d: 25.8, // full motor separation (mm)
d2: 12.9, // half motor separation — motor 1 at (-d2,0), motor 2 at (+d2,0)
l1: 85.0, // proximal link length (mm)
l2: 110.0, // distal link length (mm)
STEPS_PER_REV: 2048,
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125°
d: 25.8, // full motor separation (mm)
d2: 12.9, // half separation; M1 at (+d2, 0), M2 at (-d2, 0)
l1: 85.0, // proximal link length (mm)
l2: 110.0, // distal link length (mm)
STEPS_PER_REV: 2048,
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125 °/step
};
// ── Workspace limits ──────────────────────────────────────────────
// ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
// All values are in mm. Tune these to match real hardware.
// They are exported so the visualiser can draw the zones.
const LIMITS = {
// Outer bounding box (the board surface)
X_MIN: -150,
X_MAX: 150,
Y_MIN: -30, // numbers sit slightly below Y=0
Y_MAX: 145,
// ── Outer board boundary ───────────────────────────────────────
// The EE cannot be requested outside this rectangle.
X_MIN: -150, // ← TUNE: left edge of board
X_MAX: 150, // ← TUNE: right edge of board
Y_MIN: -30, // ← TUNE: bottom (numbers reach ~44 mm; sign coords go lower)
Y_MAX: 145, // ← TUNE: top (highest letter is ~128 mm)
// Centre exclusion zone (the motor/mechanism box)
// Motors are at ±12.9 mm; box is a bit larger to account for the housing
BOX_HALF_W: 22, // ±22 mm in X (tune to real hardware)
BOX_HALF_H: 22, // 0..22 mm in Y (box sits above the base line)
BOX_Y_MIN: -5,
BOX_Y_MAX: 22,
// ── Centre mechanism exclusion box ────────────────────────────
// Rectangular zone centred on (0, 0) where the motor housing sits.
// The EE and both elbows must stay outside this area.
BOX_HALF_W: 30, // ← TUNE: half-width in X (motors at ±12.9, housing wider)
BOX_Y_MIN: -10, // ← TUNE: bottom of housing
BOX_Y_MAX: 40, // ← TUNE: top of housing
// ── Elbow exclusion zone (prevents arm crossing near the box) ─
// Each elbow has a separate rectangular exclusion box.
// Left-side elbow (from M1 at +d2): must NOT enter this region.
// 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
// (can never cross to the other side of the box mid-point).
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
};
// ── Letter / number position lookup table ────────────────────────
@@ -116,22 +129,26 @@ const LOOKUP_TABLE = {
function solve(x, y) {
const { d2, l1, l2 } = ARM;
// ── Motor 1 (left pivot at -d2, 0) ───────────────────────────
const xmd = x - d2; // target X relative to left motor
const s = Math.sqrt(xmd * xmd + y * y); // distance: left motor → target
// ── Motor 1 (pivot at +d2, 0 = +12.9 mm) ─────────────────────
// xmd = 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 q = Math.atan2(y, xmd);
const w1 = Math.acos(cosW1);
const theta1 = q - w1;
// ── Motor 2 (right pivot at +d2, 0) ──────────────────────────
const xpd = x + d2; // target X relative to right motor
const t = Math.sqrt(xpd * xpd + y * y); // distance: right motor → target
// ── 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 t = Math.sqrt(xpd * xpd + y * y);
if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
const r = Math.atan2(y, xpd);
const w2 = Math.acos(cosW2);
const r = Math.atan2(y, xpd);
const w2 = Math.acos(cosW2);
const theta2 = r + w2;
return { theta1, theta2, reachable: true };
@@ -156,81 +173,129 @@ function solve(x, y) {
function forward(theta1, theta2) {
const { d2, l1, l2 } = ARM;
// Elbow 1 (tip of motor 1's proximal link)
const e1x = -d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1);
// ── IMPORTANT: match PositionControl.cpp motor convention ──────
// Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0).
// Using the opposite sign here is the single most common source
// of visually-crossed arms in the SVG visualiser.
// Elbow 2 (tip of motor 2's proximal link)
const e2x = d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2);
// Elbow 1 — tip of Motor 1 proximal link (motor at +d2)
const e1x = +d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1);
// End-effector: intersection of circle(elbow1, l2) and circle(elbow2, l2)
// Use the same approach as the IK: each distal link points from its elbow to the EE.
// For visualisation accuracy, reconstruct EE by reversing the IK:
// From IK: theta1 = q - w1 → q = atan2(y, x - d2)
// We know theta1 and the elbow position, so EE is at l2 along some direction.
// Simplest: use circle-circle intersection of the two elbow-radius-l2 circles.
const dx = e2x - e1x;
const dy = e2y - e1y;
// Elbow 2 — tip of Motor 2 proximal link (motor at -d2)
const e2x = -d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2);
// End-effector: intersection of the two distal-link circles
// (radius l2, centred on each elbow). Pick the "upward" solution.
const dx = e2x - e1x;
const dy = e2y - e1y;
const dist = Math.sqrt(dx * dx + dy * dy);
if (dist < 1e-6 || dist > 2 * l2) {
// Degenerate / unreachable — just average the two elbows
// Degenerate / unreachable — fall back to midpoint
return {
elbow1: { x: e1x, y: e1y },
elbow2: { x: e2x, y: e2y },
endX: (e1x + e2x) / 2,
endY: (e1y + e2y) / 2,
valid: false,
};
}
const a = dist / 2;
const h = Math.sqrt(l2 * l2 - a * a);
const a = dist / 2;
const h = Math.sqrt(Math.max(0, l2 * l2 - a * a));
const mx = (e1x + e2x) / 2;
const my = (e1y + e2y) / 2;
// Two intersection candidates — pick the one with higher Y (the "up" configuration)
const px1 = mx + h * (dy / dist);
const py1 = my - h * (dx / dist);
const px2 = mx - h * (dy / dist);
const py2 = my + h * (dx / dist);
// Two intersection candidates
const px1 = mx + h * ( dy / dist);
const py1 = my + h * (-dx / dist);
const px2 = mx - h * ( dy / dist);
const py2 = my - h * (-dx / dist);
const { endX, endY } = py1 > py2
? { endX: px1, endY: py1 }
: { endX: px2, endY: py2 };
// Always pick the candidate with higher Y (EE above the elbow line)
const useFirst = py1 >= py2;
const endX = useFirst ? px1 : px2;
const endY = useFirst ? py1 : py2;
return {
elbow1: { x: e1x, y: e1y },
elbow2: { x: e2x, y: e2y },
endX, endY,
valid: true,
};
}
// ── Arm-crossing check ────────────────────────────────────────────
/**
* Returns true if the two proximal arm segments geometrically
* cross each other near the centre mechanism box.
*
* Physical rule: each elbow must stay on the OUTER side of the
* 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, the arm would collide with the housing.
*
* @param {number} theta1 Motor 1 angle (rad)
* @param {number} theta2 Motor 2 angle (rad)
* @returns {boolean} true = arms will cross / collide
*/
function armsCrossed(theta1, theta2) {
const { d2, l1 } = ARM;
const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
// Elbow positions (same formula as forward())
const e1x = +d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1);
const e2x = -d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2);
// Elbow1 (from M1 on the RIGHT) must not appear far to the LEFT at low height
// Elbow2 (from M2 on the LEFT) must not appear far to the RIGHT at low height
// 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 e2_crossed = e2x > XI && e2y < YM; // M2's elbow went too far right
return e1_crossed || e2_crossed;
}
// ── Workspace check ───────────────────────────────────────────────
/**
* Check whether a point is inside the valid workspace.
* Check whether a target point is safe to move to.
*
* Order of checks (fail-fast):
* 1. Board outer boundary
* 2. Mechanism housing exclusion box
* 3. IK geometric reachability
* 4. Elbow-crossing guard (prevents physically impossible arm configs)
*
* @param {number} x
* @param {number} y
* @returns {{ ok: boolean, reason: string }}
*/
function checkWorkspace(x, y) {
// Outer bounding box
if (x < LIMITS.X_MIN || x > LIMITS.X_MAX)
return { ok: false, reason: `X=${x.toFixed(1)} outside board limits [${LIMITS.X_MIN}, ${LIMITS.X_MAX}]` };
if (y < LIMITS.Y_MIN || y > LIMITS.Y_MAX)
return { ok: false, reason: `Y=${y.toFixed(1)} outside board limits [${LIMITS.Y_MIN}, ${LIMITS.Y_MAX}]` };
const L = LIMITS;
// Centre exclusion zone (mechanism box)
if (
x > -LIMITS.BOX_HALF_W && x < LIMITS.BOX_HALF_W &&
y > LIMITS.BOX_Y_MIN && y < LIMITS.BOX_Y_MAX
) {
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism exclusion zone` };
}
// 1. Board boundary
if (x < L.X_MIN || x > L.X_MAX)
return { ok: false, reason: `X=${x.toFixed(1)} mm outside board (${L.X_MIN}…${L.X_MAX})` };
if (y < L.Y_MIN || y > L.Y_MAX)
return { ok: false, reason: `Y=${y.toFixed(1)} mm outside board (${L.Y_MIN}…${L.Y_MAX})` };
// IK reachability
const { reachable } = solve(x, y);
if (!reachable) return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is outside arm reach` };
// 2. Centre mechanism exclusion box
if (x > -L.BOX_HALF_W && x < L.BOX_HALF_W &&
y > L.BOX_Y_MIN && y < L.BOX_Y_MAX)
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism housing` };
// 3. IK geometric reachability
const { theta1, theta2, reachable } = solve(x, y);
if (!reachable)
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is geometrically unreachable` };
// 4. Elbow-crossing guard
if (armsCrossed(theta1, theta2))
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) would cross elbows near the mechanism box` };
return { ok: true, reason: '' };
}
@@ -256,6 +321,6 @@ function radToDeg(rad) {
export default {
ARM, LIMITS, LOOKUP_TABLE,
solve, forward, checkWorkspace, lookup,
solve, forward, armsCrossed, checkWorkspace, lookup,
stepsToRad, radToSteps, stepsToDeg, radToDeg,
};