Wheeled bipedal balancer layout

// Parametric layout mock-up of a wheeled bipedal balancing robot:
// a body box, two five-bar legs ending in driven wheels, an optional tail,
// and a reference step to size the geometry against.
//
// The shapes stand in for volume, reach and packaging. Nothing here is a
// printable part yet. The point is to pick numbers: leg lengths, wheel
// diameter, hip spacing and actuator class, and see whether the pose you
// want is reachable, whether the servo can hold it, and whether the two hip
// actuators fit next to each other at all. Read the console: stance height,
// axle travel, hip torque, rim force, track width and the bed check are all
// echoed there.

/* [Actuators] */
// Hip actuator. Both cranks of a five-bar leg use this one.
hip_actuator = "STS3250"; // [STS3215, STS3250]
// Wheel drive actuator, one per wheel.
wheel_actuator = "STS3215"; // [STS3215, STS3250]

/* [Legs] */
// Crank length, hip pivot to elbow.
crank_length = 55; // [20:1:110]
// Coupler length, elbow to wheel axle.
link_length = 85; // [30:1:150]
// Distance between the two hip pivots of one leg. Must clear the servo case.
hip_spacing = 60; // [30:1:110]
// Hip line down to the wheel axle on the left leg. This is the pose.
leg_drop_left = 130; // [20:1:190]
// Hip line down to the wheel axle on the right leg.
leg_drop_right = 130; // [20:1:190]
// Wheel axle carried forward of the hips. Negative is aft.
foot_offset = 0; // [-60:1:60]

/* [Wheels] */
// Diameter measured over the tread.
wheel_diameter = 140; // [60:5:240]
// Width across the tyre.
wheel_width = 26; // [10:1:60]

/* [Body] */
// Fore-aft length of the electronics box.
body_length = 120; // [60:1:200]
// Width of the electronics box, between the leg planes.
body_width = 90; // [50:1:160]
// Height of the electronics box.
body_height = 70; // [30:1:140]
// All-up mass, used for the torque check. Changes no geometry.
mass_g = 1200; // [400:50:3000]

/* [Tail] */
// Fit the tail actuator and arm at the back.
show_tail = true;
// Length of the tail arm.
tail_length = 110; // [0:1:220]
// How far the tail is swung down from horizontal.
tail_angle = 35; // [-20:1:100]

/* [Terrain] */
// Height of the reference step drawn ahead of the robot. Zero hides it.
step_height = 100; // [0:5:220]

/* [Colour] */
// Colour of the body box.
color_body = "SteelBlue"; // [SteelBlue, DimGray, Silver, Gainsboro, White, Black, FireBrick, ForestGreen, Goldenrod]
// Colour of the leg links.
color_leg = "Gainsboro"; // [Gainsboro, Silver, White, DimGray, Black, SteelBlue, FireBrick, ForestGreen, Goldenrod]
// Colour of the wheels.
color_wheel = "DimGray"; // [DimGray, Black, Silver, Gainsboro, White, SteelBlue, FireBrick, ForestGreen, Goldenrod]
// Colour of the actuators.
color_actuator = "Goldenrod"; // [Goldenrod, FireBrick, SteelBlue, Silver, Gainsboro, White, DimGray, Black, ForestGreen]

/* [Hidden] */
$fn = 40;

// ---- actuator table ----
// Case size and stall torque of the two Feetech bus servos worth considering.
function act_size(n)  = n == "STS3250" ? [45.2, 24.7, 35.0] : [44.8, 24.0, 35.5];
function act_stall(n) = n == "STS3250" ? 50 : 30;  // kg.cm stall at 12 V

hip_size  = act_size(hip_actuator);
whl_size  = act_size(wheel_actuator);
hip_stall = act_stall(hip_actuator);
whl_stall = act_stall(wheel_actuator);
// Rated torque on these servos runs about a quarter of stall. Design to this.
hip_rated = hip_stall * 0.25;

wheel_r = wheel_diameter / 2;
half_hs = hip_spacing / 2;

// Lateral planes, working outward from the body side.
// Servo cases tuck against the body, the cranks and couplers run just
// outboard of them, and the wheel sits outboard of the linkage.
crank_y = body_width / 2 + 8;
servo_y = crank_y - hip_size[2] / 2 - 3;
wheel_y = crank_y + 8 + wheel_width / 2;
track_w = 2 * (wheel_y + wheel_width / 2);

reach_max = crank_length + link_length;
reach_min = abs(link_length - crank_length);

function clampv(v, lo, hi) = min(max(v, lo), hi);

// How far down the axle can go before the linkage runs out of reach,
// and how far up before it folds on itself.
drop_max = sqrt(max(pow(reach_max - 1, 2) - pow(abs(foot_offset) + half_hs, 2), 4));
drop_raw = sqrt(max(pow(reach_min + 1, 2) - pow(half_hs, 2), 0)) + 12;
drop_lo  = min(drop_raw, drop_max - 1);

dl = clampv(leg_drop_left,  drop_lo, drop_max);
dr = clampv(leg_drop_right, drop_lo, drop_max);

// ---- five-bar kinematics ----
// Both hips are fixed on the body; the axle position is what you ask for,
// and the crank angles fall out of it. Circle intersection does the work.
function circ_int(c1, r1, c2, r2, s) =
  let(dv = c2 - c1,
      d  = max(norm(dv), 0.001),
      a  = (r1*r1 - r2*r2 + d*d) / (2*d),
      h  = sqrt(max(r1*r1 - a*a, 0)),
      p  = c1 + a*dv/d,
      nv = [-dv[1], dv[0]] / d)
  p + s*h*nv;

function hipA()    = [-half_hs, 0];
function hipB()    = [ half_hs, 0];
function footP(d)  = [foot_offset, -d];
function elbowA(d) = circ_int(hipA(), crank_length, footP(d), link_length, -1);
function elbowB(d) = circ_int(hipB(), crank_length, footP(d), link_length,  1);

function to3(p, y) = [p[0], y, p[1]];
function cross2(a, b) = a[0]*b[1] - a[1]*b[0];

// ---- static hip torque ----
// Each coupler is a two-force member, so the vertical load at the axle
// resolves along the two links, and each crank sees that force at its own
// moment arm. Returns the worse of the two cranks, in N.m.
function hip_torque(d) =
  let(F  = footP(d),
      P1 = elbowA(d),
      P2 = elbowB(d),
      u1 = (F - P1) / max(norm(F - P1), 0.001),
      u2 = (F - P2) / max(norm(F - P2), 0.001),
      Fv = mass_g / 1000 * 9.81 / 2,
      det = u1[0]*u2[1] - u1[1]*u2[0],
      dd = abs(det) < 0.001 ? 0.001 : det,
      f1 = ( Fv * u2[0]) / dd,
      f2 = (-Fv * u1[0]) / dd,
      t1 = abs(cross2(P1 - hipA(), -f1 * u1)) / 1000,
      t2 = abs(cross2(P2 - hipB(), -f2 * u2)) / 1000)
  max(t1, t2);

ground_z     = -max(dl, dr) - wheel_r;
stance_h     = -ground_z;
total_h      = stance_h + body_height;
lift_range   = drop_max - drop_lo;
hip_need     = hip_torque(dl) * 10.197;          // kg.cm
rim_force_kg = whl_stall / (wheel_r / 10);       // kg.cm over cm
hip_gap      = hip_spacing - hip_size[0];
bed_max      = max(body_length, wheel_diameter);

echo(str("stance height, hip line to ground: ", round(stance_h), " mm"));
echo(str("overall height: ", round(total_h), " mm, track width: ", round(track_w), " mm"));
echo(str("axle travel: ", round(drop_lo), " to ", round(drop_max), " mm, so ", round(lift_range), " mm of lift on one leg"));
echo(str("hip torque needed in this pose: ", round(hip_need*10)/10, " kg.cm per crank"));
echo(str("hip actuator ", hip_actuator, ": ", hip_stall, " kg.cm stall, about ", hip_rated, " kg.cm rated"));
echo(str("headroom on rated torque: ", round(hip_rated/max(hip_need,0.01)*100)/100, "x"));
echo(hip_gap < 5
  ? str("WARNING hip_spacing ", hip_spacing, " mm does not clear the ", hip_size[0], " mm servo case -- the two hip actuators collide")
  : str("hip actuator cases clear each other by ", round(hip_gap), " mm"));
echo(str("wheel rim force at stall: ", round(rim_force_kg*10)/10, " kg against a ", mass_g/1000, " kg robot"));
echo(str("step the wheel can mount unaided: about ", round(wheel_r*0.5), " mm"));
echo(str("largest single dimension: ", round(bed_max), " mm", bed_max > 180 ? "  -- over the A1 mini bed" : "  -- fits the A1 mini bed"));

// ---- geometry ----
module bar3(p, q, t) {
  hull() {
    translate(p) sphere(d = t, $fn = 20);
    translate(q) sphere(d = t, $fn = 20);
  }
}

module servo_box(p, y, sz) {
  translate([p[0], y, p[1]]) rotate([90, 0, 0]) cube(sz, center = true);
}

module leg(d, side) {
  cy = side * crank_y;
  sy = side * servo_y;
  wy = side * wheel_y;

  A  = hipA();
  B  = hipB();
  F  = footP(d);
  P1 = elbowA(d);
  P2 = elbowB(d);

  // Hip actuators sit against the body side, cranks on their output shafts.
  color(color_actuator) {
    servo_box(A, sy, hip_size);
    servo_box(B, sy, hip_size);
    servo_box(F, sy, whl_size);
  }

  color(color_leg) {
    bar3(to3(A,  cy), to3(P1, cy), 13);
    bar3(to3(B,  cy), to3(P2, cy), 13);
    bar3(to3(P1, cy), to3(F,  cy), 11);
    bar3(to3(P2, cy), to3(F,  cy), 11);
    // Axle stub out to the wheel.
    bar3(to3(F, cy), to3(F, wy), 9);
  }

  color(color_wheel)
    translate(to3(F, wy)) rotate([90, 0, 0])
      cylinder(h = wheel_width, d = wheel_diameter, center = true);
}

module body() {
  color(color_body)
    translate([0, 0, body_height / 2])
      cube([body_length, body_width, body_height], center = true);
}

module tail() {
  base = [-body_length / 2, 0, body_height * 0.35];
  tip  = [base[0] - tail_length * cos(tail_angle), 0,
          base[2] - tail_length * sin(tail_angle)];
  color(color_actuator) translate(base) rotate([90, 0, 0]) cube(whl_size, center = true);
  color(color_leg) bar3(base, tip, 11);
  color(color_wheel) translate(tip) sphere(d = 20, $fn = 24);
}

module terrain() {
  color("Gainsboro")
    translate([0, 0, ground_z - 3])
      cube([700, track_w + 140, 6], center = true);
  if (step_height > 0)
    color("Silver")
      translate([body_length / 2 + wheel_r + 190, 0, ground_z + step_height / 2])
        cube([300, track_w + 140, step_height], center = true);
}

body();
leg(dl, -1);
leg(dr,  1);
if (show_tail && tail_length > 0) tail();
terrain();