moving okke robot from general package

schpin_koke/src/Koke.hs

+#!/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;
+
+toteTipPos = 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",toteTipPos) ] ["tibia.scad"],
+        collision_models = [
+            scadModel "tibia_collision" [("tipOffset", toteTipPos)] ["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
+    }
+
+toteBody 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 = toteTipPos
+        },
+        pose = legPose pos
+    }
+
+tote :: Walker
+tote = Walker walker_name legs (toteBody legs)
+    where
+        walker_name = "Tote"
+        legs = [leg RR, leg RL, leg FR, leg FL]
+
+main :: IO ()
+main = do
+   mapM_ (uncurry renderSCAD) $ getSCAD tote
+   mapM_ (uncurry renderSTL) $ getSTLRenderList tote
+   writeXML (getURDF tote) "urdf/tote.urdf"

schpin_koke/src/scad/body.scad

+
+include<dynamixel.scad>;
+
+body_t = 3;
+screw_d = 3;
+
+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_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 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 body(positions) {
+    $fn = 32;
+    body_bot(positions);
+
+    body_top(positions);
+}

schpin_koke/src/scad/coxa.scad

+include<dynamixel.scad>;
+
+screw_d = 3;
+
+module coxa_holder(femurOffset) render() difference() {
+    $fn = 32;
+
+    space_r = sqrt(2) * XL320_dim[2] / 2;
+
+    translate([ femurOffset[0] / 2, 0, 0 ]) cube(
+        [ femurOffset[0], XL320_hole_space, XL320_hole_space ], center = true);
+
+    for (p = [ [ 0, 0, 0 ], femurOffset ])
+        translate(p) cube([ XL320_hole_x_offset * 2, 100, 100 ], center = true);
+
+    XL320_aligned_hole_pos() {
+        for (y = XL320_aligned_hole_offsets)
+            translate([ 0, y, 0 ])
+                cylinder(d = screw_d, h = 100, center = true);
+    }
+    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);
+    }
+}
+
+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);
+}
+
+module coxa(femurOffset) {
+    rotate([ 90, 0, 0 ]) {
+        XL320();
+        XL320_horn();
+    }
+    color("red") coxa_holder(femurOffset);
+    translate(femurOffset) rotate([ 0, 180, 0 ]) {
+        XL320();
+        XL320_horn();
+    }
+}

schpin_koke/src/scad/dynamixel.scad

+XL320_hole_d = 4;
+XL320_hole_space = 18;
+XL320_axis_d = 3;
+XL320_aligned_hole_offsets = [ -6, 0, 6 ];
+XL320_hole_x_offset = 22.5;
+XL320_dim = [ 36, 24, 24 ];
+XL320_horn_h = 3;
+XL320_horn_d = 18;
+XL320_axis_offset = 9;
+
+function dist(x) = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
+
+module tube(d1, d2, t) {
+    difference() {
+        cylinder(d = d2, h = t);
+        cylinder(d = d1, h = t + 1);
+    }
+}
+
+module XL320_aligned_hole_pos() { translate([ 33 - 9, 0, 0 ]) children(); }
+
+module XL320() color("gray") render() difference() {
+    $fn = 32;
+    dim = XL320_dim;
+    r = 4;
+    hole_d = XL320_hole_d;
+
+    translate([ dim[0] / 2 - XL320_axis_offset, 0, 0 ]) hull() {
+        cube([ dim[0] - r * 2, dim[1], dim[2] ], center = true);
+        cube([ dim[0], dim[1], dim[2] - r * 2 ], center = true);
+    }
+    rotate([ 90, 0, 0 ]) cylinder(d = XL320_axis_d, h = 100, center = true);
+
+    translate([ XL320_hole_x_offset + 5, 0, 0 ])
+        cube([ 10, XL320_hole_space, 100 ], center = true);
+    XL320_aligned_hole_pos() {
+        for (z = XL320_aligned_hole_offsets)
+            translate([ 0, 0, z ]) rotate([ 90, 0, 0 ])
+                cylinder(d = hole_d, h = 100, center = true);
+    }
+}
+
+module XL320_horn_hole_pos() {
+    for (a = [0:90:360])
+        rotate([ 0, 0, a ]) translate([ 12 / 2, 0, 0 ]) children();
+}
+
+module XL320_horn() color("gray") render() {
+    $fn = 32;
+    translate([ 0, -12, 0 ]) rotate([ 90, 0, 0 ]) difference() {
+        cylinder(d = XL320_horn_d, h = XL320_horn_h);
+
+        XL320_horn_hole_pos()
+            cylinder(d = XL320_hole_d, h = 100, center = true);
+
+        cylinder(d = XL320_axis_d, h = 100, center = true);
+    }
+}
+
+module XL320_horn_adapter(h = 2) {
+    t = 2;
+    screw_d = 3;
+
+    tube(XL320_horn_d - t * 2, XL320_horn_d, h);
+    tube(XL320_horn_d - t * 2 - screw_d * 2 - t * 2,
+         XL320_horn_d - screw_d * 2 - t * 2, h);
+
+    XL320_horn_hole_pos() {
+        cylinder(d = XL320_hole_d, h = t * 2, center = true);
+        tube(screw_d, screw_d + t * 2, h);
+    }
+}

schpin_koke/src/scad/femur.scad

+
+include<dynamixel.scad>;
+
+screw_d = 3;
+femur_t = 2;
+
+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 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_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();
+            }
+    }
+
+    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_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 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 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);
+}
+
+module femur(tibiaOffset) color("purple") {
+    //    rotate([ 0, 180, 0 ]) XL320();
+    //  XL320_horn();
+
+    $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);
+    }
+
+    // translate(tibiaOffset) {
+    //  XL320();
+    // XL320_horn();
+    //}
+}

schpin_koke/src/scad/skeletons.scad

+include <gen/skeleton.scad>;
+include <gen/col_skeleton.scad>;

schpin_koke/src/scad/tibia.scad

+include<dynamixel.scad>;
+
+module tibia_base(tipOffset) color("yellow") render() difference() {
+    t = 2;
+    a = XL320_hole_space;
+    screw_d = 3;
+
+    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);
+        }
+
+        translate(tipOffset) sphere(d = t);
+    }
+    difference() {
+        union() {
+            hull() {
+                sphere(d = a);
+                translate([ -t * 4, 0, 0 ]) translate(tipOffset) sphere(d = t);
+            }
+
+            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);
+    }
+
+    XL320_aligned_hole_pos() {
+        for (z = XL320_aligned_hole_offsets)
+            translate([ 0, 0, z ]) rotate([ 90, 0, 0 ])
+                cylinder(d = screw_d, h = 100, center = true);
+    }
+}
+
+module tibia_collision(tipOffset) {
+    z = XL320_dim[2];
+    h = XL320_horn_h;
+    y = XL320_dim[1];
+    x_min = -XL320_axis_offset;
+    x_max = x_min + XL320_dim[0];
+
+    tip_x = dist(tipOffset) + 3;
+
+    polyhedron(points =
+                   [
+                       [ x_min, -y / 2 - h, -z / 2 ], [ x_min, y / 2, -z / 2 ],
+                       [ x_min, -y / 2 - h, z / 2 ], [ x_min, y / 2, z / 2 ],
+                       [ x_max, -y / 2 - h, -z / 2 ], [ x_max, y / 2, -z / 2 ],
+                       [ x_max, -y / 2 - h, z / 2 ], [ x_max, y / 2, z / 2 ],
+                       [ tip_x, 0, 0 ]
+                   ],
+               faces = [
+                   [ 0, 1, 3, 2 ], [ 0, 1, 5, 4 ], [ 0, 2, 6, 4 ],
+                   [ 1, 3, 7, 5 ], [ 2, 3, 7, 6 ], [ 8, 4, 6 ], [ 8, 4, 5 ],
+                   [ 8, 6, 7 ], [ 8, 7, 5 ]
+               ]);
+}
+
+module tibia(tipOffset) {
+    tibia_base(tipOffset);
+    XL320();
+    XL320_horn();
+}