fix kinematics - maybe?
This commit is contained in:
PROFERIS - Mi³osz Stocki
+43
-42
@@ -18,15 +18,16 @@
|
||||
* d2=12.9 mm
|
||||
*
|
||||
* ── 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!]
|
||||
* Motor 1 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:
|
||||
* Motor 1: xmd = x - d2 → target is measured from x = +d2
|
||||
* Motor 2: xpd = x + d2 → target is measured from x = -d2
|
||||
* This is determined by a variable swap in the C++ firmware where
|
||||
* angle2 is passed to stepper1 and angle1 to stepper2. Thus:
|
||||
* 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
|
||||
* the arms will appear visually crossed even for valid positions.
|
||||
* All FK / visualisation code MUST use this exact convention or
|
||||
* the arms will appear visually crossed and physically collide.
|
||||
*
|
||||
* Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard)
|
||||
*/
|
||||
@@ -34,12 +35,12 @@
|
||||
// ── Exact constants from PositionControl.cpp ─────────────────────
|
||||
const ARM = {
|
||||
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)
|
||||
l2: 110.0, // distal link length (mm)
|
||||
STEPS_PER_REV: 2048,
|
||||
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 ─────────────────────────────────────────
|
||||
@@ -62,10 +63,10 @@ const LIMITS = {
|
||||
|
||||
// ── 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).
|
||||
// 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 right 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
|
||||
};
|
||||
@@ -130,19 +131,8 @@ const LOOKUP_TABLE = {
|
||||
function solve(x, y) {
|
||||
const { d2, l1, l2 } = ARM;
|
||||
|
||||
// ── 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 theta1 = q - w1;
|
||||
|
||||
// ── Motor 2 (pivot at -d2, 0 = -12.9 mm) ─────────────────────
|
||||
// xpd = x + d2 is the X component of (target – M2_pivot).
|
||||
// ── Motor 1 (Left, pivot at -d2, 0 = -12.9 mm) ───────────────
|
||||
// xpd = x + d2 is the X component of (target – M1_pivot).
|
||||
const xpd = x + d2;
|
||||
const t = Math.sqrt(xpd * xpd + y * y);
|
||||
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 };
|
||||
const r = Math.atan2(y, xpd);
|
||||
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 };
|
||||
}
|
||||
@@ -174,17 +175,17 @@ function solve(x, y) {
|
||||
function forward(theta1, theta2) {
|
||||
const { d2, l1, l2 } = ARM;
|
||||
|
||||
// ── IMPORTANT: match PositionControl.cpp motor convention ──────
|
||||
// Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0).
|
||||
// ── IMPORTANT: match physical hardware 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 1 — tip of Motor 1 proximal link (motor at +d2)
|
||||
const e1x = +d2 + l1 * Math.cos(theta1);
|
||||
// Elbow 1 — tip of Motor 1 proximal link (motor at -d2, Left)
|
||||
const e1x = -d2 + l1 * Math.cos(theta1);
|
||||
const e1y = l1 * Math.sin(theta1);
|
||||
|
||||
// Elbow 2 — tip of Motor 2 proximal link (motor at -d2)
|
||||
const e2x = -d2 + l1 * Math.cos(theta2);
|
||||
// Elbow 2 — tip of Motor 2 proximal link (motor at +d2, Right)
|
||||
const e2x = +d2 + l1 * Math.cos(theta2);
|
||||
const e2y = l1 * Math.sin(theta2);
|
||||
|
||||
// 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.
|
||||
*
|
||||
* 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.
|
||||
* mechanism housing. If elbow1 (from M1 at -d2) has a small
|
||||
* negative X at low Y, or elbow2 (from M2 at +d2) has a small
|
||||
* positive 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)
|
||||
@@ -247,16 +248,16 @@ function armsCrossed(theta1, theta2) {
|
||||
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 e1x = -d2 + l1 * Math.cos(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);
|
||||
|
||||
// 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
|
||||
// Elbow1 (from M1 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.
|
||||
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
|
||||
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 left
|
||||
|
||||
return e1_crossed || e2_crossed;
|
||||
}
|
||||
|
||||
@@ -4,8 +4,8 @@
|
||||
*
|
||||
* 5-bar parallel linkage visualiser + IK-based click-to-move.
|
||||
*
|
||||
* Mechanism geometry (from PositionControl.cpp):
|
||||
* Motor 1 at (+12.9, 0) mm Motor 2 at (-12.9, 0) mm ← NOTE: M1 is on the RIGHT
|
||||
* 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 LEFT
|
||||
* Proximal links: l1 = 85 mm Distal links: l2 = 110 mm
|
||||
*
|
||||
* 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) {
|
||||
const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2);
|
||||
|
||||
// Motor base SVG positions — M1 at +d2, M2 at -d2 (PositionControl.cpp convention)
|
||||
const motor1sx = SV.wx(+IK.ARM.d2); const motor1sy = SV.wy(0);
|
||||
const motor2sx = SV.wx(-IK.ARM.d2); const motor2sy = SV.wy(0);
|
||||
// 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 motor2sx = SV.wx(+IK.ARM.d2); const motor2sy = SV.wy(0);
|
||||
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 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 eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE;
|
||||
|
||||
// Motor positions — M1 at +d2, M2 at -d2
|
||||
const m1x = SV.wx(+IK.ARM.d2);
|
||||
const m2x = SV.wx(-IK.ARM.d2);
|
||||
// Motor positions — M1 at -d2, M2 at +d2
|
||||
const m1x = SV.wx(-IK.ARM.d2);
|
||||
const m2x = SV.wx(+IK.ARM.d2);
|
||||
const my = SV.wy(0);
|
||||
|
||||
// Scale ruler
|
||||
@@ -432,31 +432,31 @@ function buildHTML() {
|
||||
font-family="monospace" style="display:none"/>
|
||||
|
||||
<!-- ── Real arm ──────────────────────────────────── -->
|
||||
<!-- Arm 1: proximal + distal (M1 at +d2) -->
|
||||
<!-- Arm 1: proximal + distal (M1 at -d2) -->
|
||||
<line id="arm1-prox"
|
||||
x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}"
|
||||
x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
|
||||
x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}"
|
||||
x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
|
||||
stroke="#448aff" stroke-width="5" stroke-linecap="round"/>
|
||||
<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)}"
|
||||
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"
|
||||
x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}"
|
||||
x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
|
||||
x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}"
|
||||
x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
|
||||
stroke="#f7a03c" stroke-width="5" stroke-linecap="round"/>
|
||||
<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)}"
|
||||
stroke="#ffd08a" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/>
|
||||
|
||||
<!-- Elbow joints -->
|
||||
<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)}"/>
|
||||
<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 -->
|
||||
<circle id="end-eff" r="7"
|
||||
|
||||
Reference in New Issue
Block a user