mangavalkandClaude Opus 5.5
91b5b99fd5
hardware/openscad/ rebuilds the broken Fusion models as OpenSCAD: body (top/bottom plate, standoffs, battery bay, switch), legs (coxa bracket, femur link, tibia, horn adapters, TPU socks, servo cables), the whole robot, and the remote (shell, lid, Nextion 3.5", JH-D400X-R4 sticks, 18650 UPS board, IMU, Teensy 4.0, USB-C breakout). Every part was checked by slicing against the original STEP solids; the original's bugs are fixed and listed in PLANNING.md. The robot now uses a hexagon leg layout (corner legs 60 deg round from the mid legs; the corner coxa brackets need turning on their splines). Physical limits from a collision sweep of the model are in Config.h: per-leg coxa limits and LEG_CLEARANCE_MM 56 (a leg is 55.2 mm wide at the joints); the sim API exports them (version 7). sim_smoke passes. Unity loads the model (export_unity.py -> Resources/OpenScadModel, OpenScadModel.cs builds a new rig in place of the old scene's robot). The PCB model's mounting holes now use the exact EasyEDA grid. README shows renders of both. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
90 lines
3.9 KiB
C++
90 lines
3.9 KiB
C++
#include "Kinematics.h"
|
|
#include "Config.h"
|
|
|
|
// Standard analytical 3-DOF leg IK: coxa handles yaw in the horizontal plane,
|
|
// femur/tibia form a 2-link planar problem solved with the law of cosines.
|
|
//
|
|
// The elbow direction (which way tibia_rad's sign comes out) and the overall
|
|
// roll/pitch sign convention both depend on which way your leg is physically
|
|
// mounted - bench-check this on one leg with known foot positions before
|
|
// trusting all six.
|
|
JointAngles Kinematics::solve(int i, const FootTarget& target) {
|
|
JointAngles out{};
|
|
|
|
// Body-frame target -> leg-local frame: translate to the coxa pivot, then
|
|
// rotate by -mount angle so the leg's local +X points along its resting
|
|
// outward direction.
|
|
float dx = target.x - LEG_MOUNT_X_MM[i];
|
|
float dy = target.y - LEG_MOUNT_Y_MM[i];
|
|
float a = radians(-LEG_MOUNT_ANGLE_DEG[i]);
|
|
float local_x = dx * cos(a) - dy * sin(a);
|
|
float local_y = dx * sin(a) + dy * cos(a);
|
|
float local_z = target.z;
|
|
|
|
float coxa_rad = atan2(local_y, local_x);
|
|
|
|
// Horizontal distance from the femur pivot (after the coxa segment) to the foot.
|
|
float horiz = sqrt(local_x * local_x + local_y * local_y) - COXA_LEN_MM;
|
|
float dist = sqrt(horiz * horiz + local_z * local_z);
|
|
|
|
out.valid = true;
|
|
float max_reach = FEMUR_LEN_MM + TIBIA_LEN_MM;
|
|
float min_reach = fabs(FEMUR_LEN_MM - TIBIA_LEN_MM);
|
|
if (dist > max_reach) { dist = max_reach * 0.999f; out.valid = false; }
|
|
if (dist < min_reach) { dist = min_reach * 1.001f; out.valid = false; }
|
|
|
|
// Law of cosines: knee (tibia) angle from the femur/tibia/foot-distance triangle.
|
|
float cos_tibia = (FEMUR_LEN_MM * FEMUR_LEN_MM + TIBIA_LEN_MM * TIBIA_LEN_MM - dist * dist)
|
|
/ (2.0f * FEMUR_LEN_MM * TIBIA_LEN_MM);
|
|
cos_tibia = constrain(cos_tibia, -1.0f, 1.0f);
|
|
float tibia_rad = -(PI - acos(cos_tibia)); // knee-down convention; flip sign if yours mounts the other way
|
|
|
|
// Femur angle: elevation to the foot (Z up, so a foot below the hip gives a
|
|
// negative elevation), plus the triangle's femur-side angle.
|
|
float elevation = atan2(local_z, horiz);
|
|
float cos_femur_tri = (FEMUR_LEN_MM * FEMUR_LEN_MM + dist * dist - TIBIA_LEN_MM * TIBIA_LEN_MM)
|
|
/ (2.0f * FEMUR_LEN_MM * dist);
|
|
cos_femur_tri = constrain(cos_femur_tri, -1.0f, 1.0f);
|
|
float femur_rad = elevation + acos(cos_femur_tri);
|
|
|
|
out.coxa_deg = degrees(coxa_rad);
|
|
out.femur_deg = degrees(femur_rad);
|
|
out.tibia_deg = degrees(tibia_rad);
|
|
|
|
if (out.coxa_deg < COXA_MIN_DEG[i] || out.coxa_deg > COXA_MAX_DEG[i]) out.valid = false;
|
|
if (out.femur_deg < FEMUR_MIN_DEG || out.femur_deg > FEMUR_MAX_DEG) out.valid = false;
|
|
if (out.tibia_deg < TIBIA_MIN_DEG || out.tibia_deg > TIBIA_MAX_DEG) out.valid = false;
|
|
|
|
return out;
|
|
}
|
|
|
|
void Kinematics::jointPositions(int i, const JointAngles& a, FootTarget out[4]) {
|
|
float yaw = radians(LEG_MOUNT_ANGLE_DEG[i] + a.coxa_deg);
|
|
float femur = radians(a.femur_deg);
|
|
float tibia = radians(a.femur_deg + a.tibia_deg); // tibia angle is relative to the femur
|
|
float cx = cos(yaw), cy = sin(yaw);
|
|
float r_knee = COXA_LEN_MM + FEMUR_LEN_MM * cos(femur);
|
|
float z_knee = FEMUR_LEN_MM * sin(femur);
|
|
float r_foot = r_knee + TIBIA_LEN_MM * cos(tibia);
|
|
float z_foot = z_knee + TIBIA_LEN_MM * sin(tibia);
|
|
float mx = LEG_MOUNT_X_MM[i], my = LEG_MOUNT_Y_MM[i];
|
|
out[0] = { mx, my, 0 };
|
|
out[1] = { mx + cx * COXA_LEN_MM, my + cy * COXA_LEN_MM, 0 };
|
|
out[2] = { mx + cx * r_knee, my + cy * r_knee, z_knee };
|
|
out[3] = { mx + cx * r_foot, my + cy * r_foot, z_foot };
|
|
}
|
|
|
|
static float g_stance_height_mm = STANCE_HEIGHT_MM;
|
|
|
|
void Kinematics::setStanceHeight(float mm) { g_stance_height_mm = mm; }
|
|
float Kinematics::stanceHeight() { return g_stance_height_mm; }
|
|
|
|
FootTarget Kinematics::neutralStance(int i) {
|
|
float a = radians(LEG_MOUNT_ANGLE_DEG[i]);
|
|
FootTarget t;
|
|
t.x = LEG_MOUNT_X_MM[i] + cos(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
|
|
t.y = LEG_MOUNT_Y_MM[i] + sin(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
|
|
t.z = -g_stance_height_mm;
|
|
return t;
|
|
}
|