Commit fbd36424 authored by Jan Koniarik's avatar Jan Koniarik
Browse files

wololo

parent 2aa3a822
#!/usr/bin/env runhaskell
import Data.SCAD
import Data.Shared
import Data.URDF.Xml (writeXML)
import Data.Walker
import Linear.Quaternion
import Linear.V3
data LegPos = RL | RR | FR | FL deriving Eq;
kokeTipPos = V3 100 0 0
femurOffset = V3 55 0 0
tibiaOffset = V3 70 0 0
legPose :: LegPos -> Pose
legPose pos = Pose (V3 (xval*20) (yval*20) 0) $ axisAngle zAxis ( angle * pi/180 )
where
xval = if pos == FR || pos == RR then 1 else -1
yval = if pos == FL || pos == FR then 1 else -1
angle = case pos of
RL -> -90 - 45
RR -> -90 + 45
FL -> 90 + 45
FR -> 90 - 45
tibia = Part {
model = scadModel "tibia" [ ("tipOffset",kokeTipPos) ] ["tibia.scad"],
collision_models = [
scadModel "tibia_collision" [("tipOffset", kokeTipPos)] ["tibia.scad"]
],
part_weight = Nothing,
center_of_mass = Nothing
}
femur = Part {
model = scadModel "femur" [ ( "tibiaOffset", tibiaOffset)] ["femur.scad"],
collision_models = [
scadModel "femur_collision_a" [("tibiaOffset", tibiaOffset)] ["femur.scad"],
scadModel "femur_collision_b" [("tibiaOffset", tibiaOffset)] ["femur.scad"],
scadModel "femur_collision_c" [("tibiaOffset", tibiaOffset)] ["femur.scad"]
],
part_weight = Nothing,
center_of_mass = Nothing
}
coxa = Part {
model = scadModel "coxa" [ ( "femurOffset", femurOffset)] ["coxa.scad"],
collision_models = [
scadModel "coxa_collision" [ ( "femurOffset", femurOffset)] ["coxa.scad"]
],
part_weight = Nothing,
center_of_mass = Nothing
}
kokeBody legs = Body $ Part {
model = scadModel "body" [("positions", map (pos.pose) legs)] ["body.scad"],
collision_models = [
scadModel "body_collision_a" [("positions", map (pos.pose) legs)] ["body.scad"],
scadModel "body_collision_b" [("positions", map (pos.pose) legs)] ["body.scad"]
],
part_weight = Nothing,
center_of_mass = Nothing
}
leg :: LegPos -> Leg
leg pos = Leg {
leg_name = case pos of
RL -> "rl"
RR -> "rr"
FL -> "fl"
FR -> "fr"
,
chain = [
Link "coxa" noOffset coxa zAxis (-pi/2) 0 (pi/2) 50 100 8,
Link "femur" femurOffset femur yAxis (-pi/2) (-pi/8) (pi/2) 50 100 8,
Link "tibia" tibiaOffset tibia yAxis (-pi) (pi/2) pi 50 100 8
],
tip = Tip {
point = kokeTipPos
},
pose = legPose pos
}
koke :: Walker
koke = Walker walker_name legs (kokeBody legs)
where
walker_name = "Tote"
legs = [leg RR, leg RL, leg FR, leg FL]
main :: IO ()
main = do
mapM_ (uncurry renderSCAD) $ getSCAD koke
mapM_ (uncurry renderSTL) $ getSTLRenderList koke
writeXML (getURDF koke) "urdf/koke.urdf"
LX15D_dim = [ 44.17, 22.95, 26 ];
LX15D_middle_t = 1.5; // height of the middle space
LX15D_middle_a = 3.0; // side space
LX15D_axis_offset = 10.66;
LX15D_horn_d = 20;
LX15D_horn_w = 38.6;
LX15D_screw_x = 41.2;
LX15D_screw_y = 18;
LX15D_screw_clearance_d = 7;
LX15D_body_screw_d = 2;
LX15D_horn_clearance_d = 8;
LX15D_clearance_r = sqrt(pow(LX15D_dim[1] / 2, 2) + pow(LX15D_axis_offset, 2));
module LX15D() color("gray") {
dim = LX15D_dim;
translate([ dim[0] / 2 - LX15D_axis_offset, 0, 0 ]) {
cube(dim, center = true);
difference() {
cube(dim + [ -LX15D_middle_a, -LX15D_middle_a, LX15D_middle_t * 2 ],
center = true);
for (i = [ -0.5, 0.5 ], j = [ -0.5, 0.5 ])
translate([ dim[0] * i, dim[1] * j, 0 ]) cylinder(
d = 13, h = 100, center = true);
}
}
cylinder(d = LX15D_horn_d, h = LX15D_horn_w, center = true);
}
module LX15D_clearance() {
cylinder(r = LX15D_clearance_r, h = LX15D_horn_w, center = true);
}
module LX15D_screw_plane() { translate([ 0, 0, LX15D_dim[2] / 2 ]) children(); }
module LX15D_bottom_screw_pos()
translate([ LX15D_dim[0] / 2 - LX15D_axis_offset, 0, 0 ]) {
for (y = [ -0.5, 0.5 ])
translate([ LX15D_screw_x / 2, y * LX15D_screw_y, LX15D_dim[2] / 2 ])
children();
}
module LX15D_horn_screw_pos() {
for (a = [0:90:360])
rotate([ 0, 0, a ]) translate([ 14 / 2, 0, 0 ]) children();
}
module LX15D_side_plate(screw_d, holes, t) difference() {
union() {
LX15D_bottom_screw_pos() hull() {
cylinder(d = LX15D_screw_clearance_d, h = t);
translate([ t * 2, 0, 0 ])
cylinder(d = LX15D_screw_clearance_d, h = t);
}
hull() for (coord = holes) {
translate(coord + [ 0, 0, LX15D_dim[2] / 2 ])
cylinder(d = screw_d + t * 2, h = t);
}
}
for (coord = holes)
translate(coord) cylinder(d = screw_d, h = 100, center = true);
LX15D_bottom_screw_pos()
cylinder(d = LX15D_body_screw_d, h = 100, center = true);
}
include<tile.scad>;
include<LX15D.scad>;
include<dynamixel.scad>;
body_t = 3;
screw_d = 3;
t = 3;
tube_d = 4;
local_tile_t = 5;
coxa_z_space = 40;
module body_bot(positions) color("green") render() {
t = body_t;
d = 2 * dist(positions[0]);
translate([ 0, 0, -XL320_hole_space / 2 - XL320_horn_h - body_t ]) {
translate([ 0, 0, t ]) for (pos = positions) translate(pos)
rotate([ 180, 0, 0 ]) difference() {
XL320_horn_adapter(t);
XL320_horn_hole_pos() { cylinder(d = 3, h = 100, center = true); }
}
tube(d - XL320_horn_d - t, d - XL320_horn_d + t, t);
tube(d + XL320_horn_d - t, d + XL320_horn_d + t, t);
module body_collision_a(positions) {
hull() for (pos = positions) {
translate(pos)
cube([ LX15D_dim[1], LX15D_dim[1], LX15D_horn_w ], center = true);
}
}
module body_top(positions) color("green") render() {
t = 3;
d = 2 * dist(positions[0]);
translate([ 0, 0, XL320_hole_space / 2 + XL320_horn_h ]) {
for (pos = positions)
translate(pos) render() tube(screw_d, screw_d + t * 2, t);
tube(d - screw_d - t * 2, d - screw_d, t);
tube(d + screw_d, d + screw_d + t * 2, t);
module top_tiles() {
translate([ 0, 0, (coxa_z_space + t) / 2 ]) {
for (j = [-2:2])
translate([ 0, j * tile_a, 0 ]) tile_base(local_tile_t, 0);
for (i = [ -1, 0, 1 ])
translate([ i * tile_a, 0, 0 ]) tile_base(local_tile_t, 0);
}
}
module body_collision_a(positions) {
d = 2 * dist(positions[0]) + XL320_horn_d + body_t;
translate([ 0, 0, -XL320_hole_space / 2 - XL320_horn_h - body_t / 2 ])
cube([ d, d, body_t ], center = true);
}
module body_collision_b(positions) {
d = 2 * dist(positions[0]) + screw_d + body_t * 2;
translate([ 0, 0, XL320_hole_space / 2 + XL320_horn_h + body_t / 2 ])
cube([ d, d, body_t ], center = true);
module bot_tiles() {
translate([ 0, 0, -(coxa_z_space + t) / 2 ]) {
for (j = [-2:2])
translate([ 0, j * tile_a, 0 ]) tile_base(local_tile_t, 0);
for (i = [ -1, 0, 1 ])
translate([ i * tile_a, 0, 0 ]) tile_base(local_tile_t, 0);
}
}
module body(positions) {
$fn = 32;
body_bot(positions);
body_top(positions);
for (pos = positions) translate(pos) {
if (pos[1] > 0) {
rotate([ 0, 0, -90 ]) LX15D();
} else {
rotate([ 0, 0, 90 ]) LX15D();
}
}
top_tiles();
bot_tiles();
}
include<dynamixel.scad>;
include<LX15D.scad>;
screw_d = 3;
t = 2;
module coxa_holder(femurOffset) render() difference() {
$fn = 32;
z_screw_offset = 36;
main_screw_offset = LX15D_clearance_r + t;
space_r = sqrt(2) * XL320_dim[2] / 2;
module main_screw_pos() {
for (z = [ -0.5, 0.5 ])
translate([ main_screw_offset, 0, z * z_screw_offset ])
rotate([ -90, 0, 0 ]) children();
}
translate([ femurOffset[0] / 2, 0, 0 ]) cube(
[ femurOffset[0], XL320_hole_space, XL320_hole_space ], center = true);
module side_screw_pos(femurOffset) {
translate(femurOffset) rotate([ 90, 0, 180 ]) {
LX15D_bottom_screw_pos() children();
}
rotate([ -90, 0, 0 ]) LX15D_screw_plane() {
rotate([ 90, 0, 0 ]) main_screw_pos() children();
}
}
module M3_screw(l, center = false) { cylinder(d = 3, h = l, center = center); }
for (p = [ [ 0, 0, 0 ], femurOffset ])
translate(p) cube([ XL320_hole_x_offset * 2, 100, 100 ], center = true);
module side_plate(femurOffset) {
relative_screw_offset = [ femurOffset[0] - main_screw_offset, 0, 0 ];
translate(femurOffset) rotate([ 90, 0, 180 ]) LX15D_side_plate(
screw_d,
[
relative_screw_offset + [ 0, z_screw_offset / 2, 0 ],
relative_screw_offset - [ 0, z_screw_offset / 2, 0 ]
],
t);
}
cable_clearance_radius = 2;
cable_clearance_d = 3;
cable_clearance_h = LX15D_screw_y;
cable_clearance_w = cable_clearance_d / 2 + t;
cable_clearance_x = 10;
XL320_aligned_hole_pos() {
for (y = XL320_aligned_hole_offsets)
translate([ 0, y, 0 ])
cylinder(d = screw_d, h = 100, center = true);
module cable_side_plate() difference() {
union() {
rotate([ 90, 0, 180 ]) hull()
LX15D_bottom_screw_pos() for (x = [ 0, -1.5 ])
translate([ x, 0, t ]) {
cylinder(d = LX15D_screw_clearance_d,
h = cable_clearance_w - t + LX15D_middle_t);
}
translate([ LX15D_axis_offset - LX15D_dim[0], LX15D_dim[2] / 2, 0 ])
translate([ cable_clearance_x, LX15D_middle_t, 0 ]) difference() {
union() {
cube(
[
cable_clearance_d + t * 3, cable_clearance_w * 2,
cable_clearance_h
],
center = true);
}
translate([ 0, -50, 0 ]) cube(100, center = true);
difference() {
cube(
[
cable_clearance_d + cable_clearance_radius * 2,
cable_clearance_d + cable_clearance_radius * 2, 30
],
center = true);
rotate_extrude(convexity = 10) hull() for (i = [ -0.5, 0.5 ])
translate([
cable_clearance_radius + cable_clearance_d,
i * (cable_clearance_h - cable_clearance_radius * 2), 0
]) circle(r = cable_clearance_radius);
}
}
}
translate(femurOffset) rotate([ 0, 0, 180 ]) rotate([ 90, 0, 0 ])
XL320_aligned_hole_pos() {
for (z = XL320_aligned_hole_offsets)
translate([ 0, z, 0 ])
cylinder(d = screw_d, h = 100, center = true);
rotate([ 90, 0, 180 ]) LX15D_bottom_screw_pos()
cylinder(d = LX15D_body_screw_d, h = 100, center = true);
}
module holder() difference() {
a = screw_d + t * 2;
hull() {
LX15D_horn_screw_pos() translate([ 0, 0, LX15D_horn_w / 2 ])
cylinder(d = a, h = t);
translate([ main_screw_offset, 0, z_screw_offset / 2 ])
rotate([ 90, 0, 0 ])
cylinder(d = a, h = LX15D_dim[2], center = true);
}
LX15D_clearance();
LX15D_horn_screw_pos() translate([ 0, 0, LX15D_horn_w / 2 ])
cylinder(d = screw_d, h = 100, center = true);
cylinder(d = LX15D_horn_clearance_d, h = 100, center = true);
translate([ main_screw_offset, 0, z_screw_offset / 2 ]) rotate([ 90, 0, 0 ])
cylinder(d = screw_d, h = 100, center = true);
}
module coxa_holder(femurOffset) {
for (m = [ 0, 1 ]) mirror([ 0, 0, m ]) holder();
for (m = [ 0, 1 ]) mirror([ 0, m, 0 ]) side_plate(femurOffset);
translate(femurOffset) cable_side_plate();
}
module coxa_collision(femurOffset) {
dim = [
femurOffset[0] + XL320_axis_offset * 2, XL320_dim[1] + XL320_horn_h,
XL320_dim[2]
];
translate([ 0, -XL320_horn_h / 2, 0 ]) translate(femurOffset / 2)
cube(dim, center = true);
width = LX15D_dim[2] + t * 2;
translate([ femurOffset[0] / 2, 0, 0 ]) cube(
[ femurOffset[0] + width, width, LX15D_horn_w + t * 2 ], center = true);
}
$fs = 0.5;
$fa = 0.5;
module coxa(femurOffset) {
rotate([ 90, 0, 0 ]) {
XL320();
XL320_horn();
}
color("red") coxa_holder(femurOffset);
translate(femurOffset) rotate([ 0, 180, 0 ]) {
XL320();
XL320_horn();
}
translate(femurOffset) rotate([ 90, 0, 180 ]) LX15D();
}
coxa([ 55, 0, 0 ]);
include<LX15D.scad>;
include<tile.scad>;
include<dynamixel.scad>;
t = 3;
screw_d = 3;
femur_t = 2;
hole_d = 10;
module femur_screw_pos(tibiaOffset) {
translate([ sqrt(2) * XL320_dim[2] / 2 + femur_t + screw_d / 2, 0, 0 ])
children();
translate(tibiaOffset)
translate([ -sqrt(2) * XL320_dim[2] / 2 - femur_t - screw_d / 2, 0, 0 ])
children();
module collision_cube() {
translate([ 0, LX15D_horn_w / 2 + t / 2, 0 ])
cube([ LX15D_dim[1], t, LX15D_dim[1] ], center = true);
}
module scaled_tube(x, y, t) {
difference() {
scale([ x, y, t ]) cylinder(d = 1, h = 1);
translate([ 0, 0, -0.5 ]) scale([ x - t * 2, y - t * 2, t + 1 ])
cylinder(d = 1, h = 1);
module femur_collision_a(tibiaOffset) {
hull() {
collision_cube();
middle_pos(tibiaOffset) collision_cube();
}
}
module femur_front_side(tibiaOffset) difference() {
$fn = 32;
d = XL320_horn_d;
s = XL320_dim[1] / 2 + XL320_horn_h;
translate([ 0, -s, 0 ]) rotate([ 90, 0, 0 ]) {
translate([ tibiaOffset[0] / 2, 0, 0 ]) {
scaled_tube(tibiaOffset[0] + d, d, femur_t);
scaled_tube(tibiaOffset[0] - XL320_dim[2] * sqrt(2), d, femur_t);
}
femur_screw_pos(tibiaOffset)
tube(screw_d, screw_d + femur_t * 2, femur_t);
for (p = [ [ 0, 0, 0 ], tibiaOffset ]) translate(p) {
XL320_horn_adapter();
}
hull() {
middle_pos(tibiaOffset) collision_cube();
translate(tibiaOffset) collision_cube();
}
femur_screw_pos(tibiaOffset) rotate([ 90, 0, 0 ])
cylinder(d = screw_d, h = 100, center = true);
for (p = [ [ 0, 0, 0 ], tibiaOffset ])
translate(p) rotate([ 90, 0, 0 ]) XL320_horn_hole_pos()
cylinder(d = screw_d, h = 100, center = true);
}
module femur_rear_side(tibiaOffset) difference() {
$fn = 32;
d = screw_d + femur_t * 2;
s = XL320_dim[1] / 2;
translate([ 0, s, 0 ]) rotate([ -90, 0, 0 ]) {
translate([ tibiaOffset[0] / 2, 0, 0 ]) {
scaled_tube(tibiaOffset[0] + d, d * 2, femur_t);
scaled_tube(tibiaOffset[0] - XL320_dim[2] * sqrt(2), d * 2,
femur_t);
}
femur_screw_pos(tibiaOffset)
tube(screw_d, screw_d + femur_t * 2, femur_t);
for (p = [ [ 0, 0, 0 ], tibiaOffset ])
translate(p) cylinder(d = d, h = femur_t);
}
for (p = [ [ 0, 0, 0 ], tibiaOffset ])
rotate([ 90, 0, 0 ]) translate(p)
cylinder(d = screw_d, h = 100, center = true);
femur_screw_pos(tibiaOffset) rotate([ 90, 0, 0 ])
cylinder(d = screw_d, h = 100, center = true);
module femur_collision_b(tibiaOffset) {
mirror([ 0, 1, 0 ]) femur_collision_a(tibiaOffset);
}
module femur_collision_a(tibiaOffset) {
dim = [
tibiaOffset[0] + XL320_horn_d + femur_t, femur_t, XL320_horn_d + femur_t
];
translate([ 0, -XL320_dim[1] / 2 - dim[1] / 2 - XL320_horn_h, 0 ])
translate(tibiaOffset / 2) cube(dim, center = true);
module screw_pos(tibiaOffset) translate([ 0, LX15D_horn_w / 2, 0 ]) {
rotate([ -90, 0, 0 ]) LX15D_horn_screw_pos() children();
translate(tibiaOffset) rotate([ -90, 45, 0 ]) LX15D_horn_screw_pos()
children();
}
module femur_collision_b(tibiaOffset) {
dim = [
tibiaOffset[0] + screw_d + 2 * femur_t, femur_t,
screw_d * 2 + femur_t * 4
];
translate([ 0, XL320_dim[1] / 2 + dim[1] / 2, 0 ])
translate(tibiaOffset / 2) cube(dim, center = true);
module screw_center(tibiaOffset) translate([ 0, LX15D_horn_w / 2, 0 ]) {
rotate([ -90, 0, 0 ]) children();
translate(tibiaOffset) rotate([ -90, 0, 0 ]) children();
}
module femur_collision_c(tibiaOffset) {
dim = [
screw_d + femur_t * 2, XL320_dim[1] + XL320_horn_h,
screw_d + femur_t * 2
];
translate([ 0, -XL320_horn_h / 2, 0 ]) hull() femur_screw_pos(tibiaOffset)
cube(dim, center = true);
function middle_pos_f(tibiaOffset) = [ tibiaOffset[2], 0, tibiaOffset[2] ];
module middle_pos(tibiaOffset) {
translate(middle_pos_f(tibiaOffset)) children();
}
module femur(tibiaOffset) color("purple") {
// rotate([ 0, 180, 0 ]) XL320();
// XL320_horn();
module screw() { cylinder(d = screw_d + t * 2, h = t); }
$fn = 32;
femur_front_side(tibiaOffset);
femur_rear_side(tibiaOffset);
femur_screw_pos(tibiaOffset) {
translate([ 0, XL320_dim[1] / 2, 0 ]) rotate([ 90, 0, 0 ])
tube(screw_d, screw_d + femur_t * 2, XL320_dim[1] + XL320_horn_h);
module femur_plate(tibiaOffset) {
translate([ 0, LX15D_horn_w / 2 + 5 / 2, 0 ]) {
rotate([ 90, 45, 0 ]) scale([ 0.5, 0.5, 1 ]) for (i = [1:3])
translate([ 0, i * tile_a, 0 ]) tile_base(5);
translate(tibiaOffset) scale([ 0.5, 1, 0.5 ])
rotate([ 90, 0, 0 ]) for (i = [-1:-3])
translate([ i * tile_a, 0, 0 ]) tile_base(5);
}
difference() {
translate([ 0, LX15D_horn_w / 2, 0 ]) union() {
hull() { rotate([ -90, 0, 0 ]) LX15D_horn_screw_pos() screw(); }
hull() translate(tibiaOffset) rotate([ -90, 45, 0 ])
LX15D_horn_screw_pos() screw();
}
screw_pos(tibiaOffset) cylinder(d = screw_d, h = 100, center = true);
screw_center(tibiaOffset)
cylinder(d = LX15D_horn_clearance_d, h = 100, center = true);
}
}
// translate(tibiaOffset) {
// XL320();
// XL320_horn();
//}
$fa = 1.5;
$fs = 1.5;
module femur(tibiaOffset) {
color("purple") {
for (m = [ 0, 1 ]) mirror([ 0, m, 0 ]) femur_plate(tibiaOffset);
}
}
femur([ 100, 0, 45 ]);
include<dynamixel.scad>;
include<LX15D.scad>;
module tibia_base(tipOffset) color("yellow") render() difference() {
t = 2;
a = XL320_hole_space;
screw_d = 3;
t = 3;
screw_d = 3;
tube_d = 6;
hull() {
XL320_aligned_hole_pos() {
for (z = XL320_aligned_hole_offsets)
translate([ 0, 0, z ]) rotate([ 90, 0, 0 ])
cylinder(d = screw_d + t * 2, h = a, center = true);
}
screw_a = 18;
screw_x_offset = 6;
translate(tipOffset) sphere(d = t);
}
difference() {
union() {
hull() {
sphere(d = a);
translate([ -t * 4, 0, 0 ]) translate(tipOffset) sphere(d = t);
}
function screw_coord(i) = [
LX15D_dim[0] - LX15D_axis_offset + screw_x_offset, //
(floor(i / 2) - 0.5) * LX15D_dim[2], //
((i % 2) - 0.5) *
screw_a
];
rotate([ 0, 90, 0 ]) {
for (an = [0:90:360]) rotate([ 0, 0, an ]) hull() {
translate([ t, 0, tipOffset[0] ]) sphere(d = t);
translate([ sqrt(2) * a / 2 + t, 0, 0 ])
cylinder(d = a, h = 200, center = true);
}
}
}
XL320_aligned_hole_pos() for (z = [ -6, 6 ]) translate([ 0, 0, z ])
rotate([ 90, 0, 0 ])
cylinder(d = screw_d + t * 2, h = a, center = true);
function line_interp(from, to, f) = [
(to[0] - from[0]) * f + from[0],
(to[1] - from[1]) * f + from[1],
(to[2] - from[2]) * f + from[2] + sin(f * 180) * 2,
];
function line_coord(i) = screw_coord(i) + [ 0, -4 * floor(i / 2) + 2, 0 ];
module line(from, to, t) {
hull() {