Robot arms pass goods from belt to belt; native beam primitive
Deploy web Studio / deploy (push) Successful in 1m3s
Deploy web Studio / deploy (push) Successful in 1m3s
- Robot arm (category Industry): picks goods off the end of the nearest conveyor within its reach and puts them on the start of another conveyor within reach, or on a pallet a quarter turn on (pick / put: auto or a side). One move per item coming in (cycle 0) or a set cycle; waits while the line before it stands. The belt it feeds carries its goods at its pace and starts its loop when they land (feeds carry a time offset `ph`), so arm and belts stay in step. - Drawn as a 16-pose flipbook: two-link inverse kinematics per pose, each pose shown for a sixteenth of the cycle by a stepped CSS animation (iso-flip; pose 0 alone when unanimated). Poses pointing away from the viewer are drawn before the base, the others after it. - New sprite method beam(a, b, n, w, mat): a square bar between two 3D points, six faces culled and shaded like face(). Native, so the factory demo renders in 4.7 ms instead of 18.8 ms. - Factory demo: a palletiser at the end of the carton line, a loading robot passing drums from the incline onto the loading belt into the truck. MCP instructions on robot arms. Checks: 70 Rust tests (arms and belts in step, waiting arm, beam faces from every side), Studio 109/109, web 90/90.
This commit is contained in:
grabowski committed
1 parent
18dd324543
commit
f49798583e
14 files changed
+460
-31
No files matched your search
@@ -297,6 +297,14 @@ pub fn register_sprite(e: &mut Engine) {
|
||||
ok(s)
|
||||
},
|
||||
);
|
||||
e.register_fn(
|
||||
"beam",
|
||||
|s: &mut SpriteH, a: D, b: D, nn: D, w: D, mat: &str| -> RR<SpriteH> {
|
||||
s.0.borrow_mut()
|
||||
.beam(p3(&a)?, p3(&b)?, p3(&nn)?, n(&w)?, &classes(mat));
|
||||
ok(s)
|
||||
},
|
||||
);
|
||||
e.register_fn("face", |s: &mut SpriteH, p: D, mat: &str| -> RR<SpriteH> {
|
||||
s.0.borrow_mut().face(pts3(&p)?, &classes(mat), None, false);
|
||||
ok(s)
|
||||
|
||||
@@ -643,6 +643,95 @@ fn conveyors_carry_what_the_producer_makes() {
|
||||
assert_eq!(row("belt3", "Speed"), "0.8 m/s");
|
||||
}
|
||||
|
||||
fn robot_doc(belt1_running: bool) -> Doc {
|
||||
let mut doc = Doc::default();
|
||||
for (id, kind, x, y, rot, props) in [
|
||||
("press1", "producer", -6.0, -4.0, 0.0, json!({"rate": 15})),
|
||||
(
|
||||
"belt1",
|
||||
"conveyor",
|
||||
-1.4,
|
||||
-4.0,
|
||||
0.0,
|
||||
json!({"w": 6, "running": belt1_running}),
|
||||
),
|
||||
("robot1", "robot_arm", 3.2, -4.0, 0.0, json!({})),
|
||||
("belt2", "conveyor", 3.2, 1.0, 90.0, json!({"w": 6})),
|
||||
(
|
||||
"robot2",
|
||||
"robot_arm",
|
||||
3.2,
|
||||
5.8,
|
||||
0.0,
|
||||
json!({"colour": "mach2"}),
|
||||
),
|
||||
(
|
||||
"robot3",
|
||||
"robot_arm",
|
||||
-6.0,
|
||||
5.0,
|
||||
0.0,
|
||||
json!({"running": false, "pick": "+x", "put": "-y", "goods": "drum"}),
|
||||
),
|
||||
] {
|
||||
let mut o = obj(id, kind, x, y, props);
|
||||
o.rot = rot;
|
||||
doc.objects.push(o);
|
||||
}
|
||||
doc
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn robot_arms_pass_goods_from_belt_to_belt() {
|
||||
let r = Runtime::new(&Bundle::default()).unwrap();
|
||||
let doc = robot_doc(true);
|
||||
for view in 0..4u8 {
|
||||
let b = r.build("site", view, &cfg(), &doc).unwrap();
|
||||
assert!(b.scene.warnings.is_empty(), "{:?}", b.scene.warnings);
|
||||
let out = render(&b.scene, view, &RenderOpts::default());
|
||||
assert_eq!(out.cycles, 0, "view {view}: depth-sort cycles");
|
||||
assert!(!out.html.contains("NaN"));
|
||||
// two running arms with 16 poses each, one shown at a time; the stopped one stands still
|
||||
assert_eq!(
|
||||
out.html.matches("iso-a iso-flip").count(),
|
||||
32,
|
||||
"view {view}"
|
||||
);
|
||||
assert_eq!(
|
||||
out.html.matches("iso-flip iso-pose0").count(),
|
||||
2,
|
||||
"view {view}"
|
||||
);
|
||||
}
|
||||
let b = r.build("site", 1, &cfg(), &doc).unwrap();
|
||||
let out = render(&b.scene, 1, &RenderOpts::default());
|
||||
let row = |id: &str, k: &str| {
|
||||
out.info[id]
|
||||
.rows
|
||||
.iter()
|
||||
.find(|r| r.0 == k)
|
||||
.map(|r| r.1.clone())
|
||||
.unwrap_or_default()
|
||||
};
|
||||
assert_eq!(row("robot1", "Moves"), "cartons from belt1 to belt2");
|
||||
assert_eq!(row("robot1", "Cycle"), "4 s");
|
||||
// the belt after the arm carries its goods at its pace, starting its loop when they land
|
||||
assert_eq!(
|
||||
row("belt2", "Carries"),
|
||||
"15 cartons a minute, 2 m apart (from robot1)"
|
||||
);
|
||||
assert!(out
|
||||
.html
|
||||
.contains("animation-duration:4.0s;animation-delay:-1.75s"));
|
||||
assert_eq!(row("robot2", "Moves"), "cartons from belt2 to a pallet");
|
||||
assert_eq!(row("robot3", "Cycle"), "4 s (stopped)");
|
||||
// a stopped belt before it: the arm waits
|
||||
let b = r.build("site", 1, &cfg(), &robot_doc(false)).unwrap();
|
||||
let out = render(&b.scene, 1, &RenderOpts::default());
|
||||
assert_eq!(out.html.matches("iso-a iso-flip").count(), 0);
|
||||
assert!(out.info["robot1"].rows[1].1.contains("waiting"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn site_walls_join_at_corners() {
|
||||
let r = Runtime::new(&Bundle::default()).unwrap();
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
// isokit/industry: conveyors and producers (machines that make goods at a
|
||||
// set rate). Goods flow from a producer onto the conveyor starting at its
|
||||
// output and on along the conveyors that start where another one ends: each
|
||||
// belt carries them at its own speed, so the gap between them is speed / rate.
|
||||
// isokit/industry: conveyors, producers (machines that make goods at a set
|
||||
// rate) and robot arms. Goods flow from a producer onto the conveyor starting
|
||||
// at its output and on along the conveyors that start where another one ends:
|
||||
// each belt carries them at its own speed, so the gap between them is
|
||||
// speed / rate. A robot arm picks them off the end of a belt in its reach and
|
||||
// puts them on the start of another belt, or on a pallet.
|
||||
|
||||
fn goods_kinds() { ["carton", "crate", "drum", "sack", "part"] }
|
||||
|
||||
@@ -67,10 +69,12 @@ fn conveyor_ext(o, e) {
|
||||
0.0
|
||||
}
|
||||
|
||||
/// What reaches conveyor o at its start: #{ rate, item, running, from }
|
||||
/// (rate in goods per second) from the producer whose output side it starts
|
||||
/// at, or else from the conveyor ending there (followed back up to 12 belts);
|
||||
/// () when nothing feeds it.
|
||||
/// What reaches conveyor o at its start: #{ rate, item, running, from, ph }
|
||||
/// (rate in goods per second; ph: seconds into the loop at which goods land,
|
||||
/// so a belt after a robot arm runs in step with it) from the producer whose
|
||||
/// output side it starts at, a robot arm putting goods on it, or else the
|
||||
/// conveyor ending there (followed back up to 12 belts and arms); () when
|
||||
/// nothing feeds it.
|
||||
fn conveyor_feed(o, hops) {
|
||||
if hops > 12 { return (); }
|
||||
let p = o.p;
|
||||
@@ -78,10 +82,19 @@ fn conveyor_feed(o, hops) {
|
||||
for m in objects_at(["producer"], a[0], a[1]) {
|
||||
if (m.z - o.z).abs() > 0.5 { continue; }
|
||||
if local_of(m, a[0], a[1])[0] > m.p.w / 2.0 - 1.0 {
|
||||
return #{ rate: m.p.rate / 60.0, item: m.p.product, running: m.p.running, from: m.id };
|
||||
return #{ rate: m.p.rate / 60.0, item: m.p.product, running: m.p.running, from: m.id, ph: 0.0 };
|
||||
}
|
||||
}
|
||||
let s = world_of(o, -p.w / 2.0, 0.0);
|
||||
for m in objects_of("robot_arm") {
|
||||
let dx = m.x - s[0];
|
||||
let dy = m.y - s[1];
|
||||
if dx * dx + dy * dy > (m.p.reach + 1.0) * (m.p.reach + 1.0) { continue; }
|
||||
let t = arm_targets(m, hops + 1);
|
||||
if t.put.belt == o.id {
|
||||
return #{ rate: 1.0 / t.cycle, item: t.kind, running: t.running, from: m.id, ph: t.ph + t.cycle * 9.0 / 16.0 };
|
||||
}
|
||||
}
|
||||
let zs = o.z + belt_z(p, -p.w / 2.0);
|
||||
for c in objects_of("conveyor") {
|
||||
if c.id == o.id { continue; }
|
||||
@@ -91,9 +104,9 @@ fn conveyor_feed(o, hops) {
|
||||
if (c.z + belt_z(c.p, c.p.w / 2.0) - zs).abs() > 0.3 { continue; }
|
||||
let f = conveyor_feed(c, hops + 1);
|
||||
if type_of(f) == "map" {
|
||||
return #{ rate: f.rate, item: f.item, running: f.running && c.p.running, from: c.id };
|
||||
return #{ rate: f.rate, item: f.item, running: f.running && c.p.running, from: c.id, ph: f.ph };
|
||||
}
|
||||
return #{ rate: c.p.speed / c.p.gap, item: c.p.item, running: c.p.running, from: c.id };
|
||||
return #{ rate: c.p.speed / c.p.gap, item: c.p.item, running: c.p.running, from: c.id, ph: 0.0 };
|
||||
}
|
||||
()
|
||||
}
|
||||
@@ -173,11 +186,15 @@ fn prefab_conveyor(o) {
|
||||
if moving {
|
||||
let per = step / p.speed;
|
||||
let v = dv(step, 0.0, p.rise * step / p.w);
|
||||
s.begin("a ride", `--dx:${v[0]}px;--dy:${v[1]}px;animation-duration:${per}s`);
|
||||
// the loop starts when goods land (ph seconds in)
|
||||
let ph = if fed { feed.ph } else { 0.0 };
|
||||
let late = ((per - ph) % per + per) % per;
|
||||
let at = `animation-duration:${per}s;animation-delay:-${late}s`;
|
||||
s.begin("a ride", `--dx:${v[0]}px;--dy:${v[1]}px;${at}`);
|
||||
for i in order {
|
||||
let u = u0 + i * step;
|
||||
let fade = if cnt == 1 { "rideio" } else if i == 0 { "ridein" } else if i == cnt - 1 { "rideout" } else { "" };
|
||||
if fade != "" { s.begin("a " + fade, `animation-duration:${per}s`); }
|
||||
if fade != "" { s.begin("a " + fade, at); }
|
||||
s.goods(kind, u, 0.0, belt_z(p, u) + lift, k);
|
||||
if fade != "" { s.end(); }
|
||||
}
|
||||
@@ -251,3 +268,192 @@ fn prefab_producer(o) {
|
||||
s.box(cx, -D, L, D, ct, p.h, mat);
|
||||
s.cyl(px, 0.0, 0.2, p.h, p.h + stroke + 0.15, "press");
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------- robot arms
|
||||
|
||||
fn v3(a, b, k) { [a[0] + b[0] * k, a[1] + b[1] * k, a[2] + b[2] * k] }
|
||||
/// Angle (radians) of the local plan point (x, y) seen from the origin.
|
||||
fn plan_angle(x, y) {
|
||||
let r = (x * x + y * y).sqrt();
|
||||
if r < 1e-9 { return 0.0; }
|
||||
let a = clamp(x / r, -1.0, 1.0).acos();
|
||||
if y < 0.0 { -a } else { a }
|
||||
}
|
||||
|
||||
fn side_angle(side) {
|
||||
if side == "+y" { PI() / 2.0 } else if side == "-x" { PI() } else if side == "-y" { -PI() / 2.0 } else { 0.0 }
|
||||
}
|
||||
|
||||
/// Where robot arm o picks and puts goods: #{ pick, put } each #{ r, a, z,
|
||||
/// belt } (distance, angle and height in o's frame; the belt id or ""), the
|
||||
/// kind of goods, its cycle (s: Cycle, else one per item coming in) and
|
||||
/// whether it runs (it waits while the line before it stands).
|
||||
fn arm_targets(o, hops) {
|
||||
let p = o.p;
|
||||
let pick = ();
|
||||
let put = ();
|
||||
let kind = "carton";
|
||||
let rate = 0.0;
|
||||
let fed = true;
|
||||
let ph = 0.0;
|
||||
let best = 1e9;
|
||||
if p.pick == "auto" {
|
||||
for c in objects_of("conveyor") {
|
||||
let e = world_of(c, c.p.w / 2.0 - 0.35, 0.0);
|
||||
let l = local_of(o, e[0], e[1]);
|
||||
let d = (l[0] * l[0] + l[1] * l[1]).sqrt();
|
||||
if d < 0.6 || d > p.reach + 0.4 || d >= best { continue; }
|
||||
best = d;
|
||||
pick = #{ r: d, a: plan_angle(l[0], l[1]), z: c.z + belt_z(c.p, c.p.w / 2.0) - o.z, belt: c.id };
|
||||
let f = if hops > 12 { () } else { conveyor_feed(c, hops + 1) };
|
||||
if type_of(f) == "map" { kind = f.item; rate = f.rate; fed = f.running && c.p.running; ph = f.ph; } else { kind = c.p.item; rate = c.p.speed / c.p.gap; fed = c.p.running; ph = 0.0; }
|
||||
}
|
||||
}
|
||||
if type_of(pick) != "map" {
|
||||
let side = if p.pick == "auto" { "-x" } else { p.pick };
|
||||
pick = #{ r: p.reach * 0.75, a: side_angle(side), z: 0.8, belt: "" };
|
||||
kind = p.goods;
|
||||
}
|
||||
if kind == "none" { kind = p.goods; }
|
||||
best = 1e9;
|
||||
if p.put == "auto" {
|
||||
for c in objects_of("conveyor") {
|
||||
if c.id == pick.belt { continue; }
|
||||
let e = world_of(c, -c.p.w / 2.0 + 0.35, 0.0);
|
||||
let l = local_of(o, e[0], e[1]);
|
||||
let d = (l[0] * l[0] + l[1] * l[1]).sqrt();
|
||||
if d < 0.6 || d > p.reach + 0.4 || d >= best { continue; }
|
||||
best = d;
|
||||
put = #{ r: d, a: plan_angle(l[0], l[1]), z: c.z + belt_z(c.p, -c.p.w / 2.0) - o.z, belt: c.id };
|
||||
}
|
||||
}
|
||||
if type_of(put) != "map" {
|
||||
let a = if p.put == "auto" { pick.a + PI() / 2.0 } else { side_angle(p.put) };
|
||||
put = #{ r: p.reach * 0.75, a: a, z: 0.14, belt: "" };
|
||||
}
|
||||
let cycle = if p.cycle > 0.0 { p.cycle } else if rate > 0.0 { 1.0 / rate } else { 4.0 };
|
||||
#{ pick: pick, put: put, kind: kind, cycle: clamp(cycle, 1.5, 120.0), running: p.running && fed, ph: ph }
|
||||
}
|
||||
|
||||
/// One pose of the arm (links al long, shoulder 1.2 m up): the gripper's tip
|
||||
/// at distance r, angle a, height zt, holding an item of kind unless kind is
|
||||
/// "". The parts are drawn outer ones last when the arm points at the viewer.
|
||||
fn arm_pose(r, a, zt, kind, k, mat, al) {
|
||||
let zs = 1.2;
|
||||
let a1 = al;
|
||||
let a2 = al;
|
||||
let wr = r;
|
||||
let wz = zt + 0.32;
|
||||
let dz = wz - zs;
|
||||
let d = fmin((wr * wr + dz * dz).sqrt(), a1 + a2 - 0.01);
|
||||
let phi = if d < 1e-6 { 0.0 } else { let q = clamp(wr / d, -1.0, 1.0).acos(); if dz < 0.0 { -q } else { q } };
|
||||
let al = clamp((a1 * a1 + d * d - a2 * a2) / (2.0 * a1 * d), -1.0, 1.0).acos();
|
||||
let sh = phi + al;
|
||||
let er = a1 * sh.cos();
|
||||
let ez = zs + a1 * sh.sin();
|
||||
let ca = a.cos();
|
||||
let sa = a.sin();
|
||||
let n = [-sa, ca, 0.0];
|
||||
let S = [0.0, 0.0, zs];
|
||||
let E = [ca * er, sa * er, ez];
|
||||
let W = [ca * wr, sa * wr, wz];
|
||||
let T = [ca * wr, sa * wr, zt];
|
||||
let parts = [0, 1, 2];
|
||||
if !vis(ca, sa) { parts.reverse(); }
|
||||
for part in parts {
|
||||
if part == 0 {
|
||||
this.beam(S, E, n, 0.26, mat);
|
||||
this.beam(v3(E, n, -0.17), v3(E, n, 0.17), [ca, sa, 0.0], 0.3, "robotb");
|
||||
} else if part == 1 {
|
||||
this.beam(E, W, n, 0.2, mat);
|
||||
} else {
|
||||
if kind != "" { this.goods(kind, T[0], T[1], zt - goods_h(kind) * k, k); }
|
||||
this.beam(W, T, n, 0.12, "robotb");
|
||||
this.beam(v3(T, n, -0.16), v3(T, n, 0.16), [ca, sa, 0.0], 0.07, "grip");
|
||||
}
|
||||
}
|
||||
this
|
||||
}
|
||||
|
||||
fn meta_robot_arm() {
|
||||
#{ label: "Robot arm", category: "Industry", icon: "⟟",
|
||||
help: "Picks goods off the end of the nearest conveyor in its reach and puts them on the start of another one, or on a pallet beside it (Pick from / Put on: auto, or a side of the arm). With Cycle 0 it keeps pace with the goods.",
|
||||
props: [
|
||||
#{ key: "reach", type: "float", def: 2.0, min: 1.0, max: 4.0, step: 0.1, label: "Reach (m)" },
|
||||
#{ key: "pick", type: "choice", def: "auto", choices: ["auto", "+x", "+y", "-x", "-y"], label: "Pick from" },
|
||||
#{ key: "put", type: "choice", def: "auto", choices: ["auto", "+x", "+y", "-x", "-y"], label: "Put on" },
|
||||
#{ key: "cycle", type: "float", def: 0.0, min: 0.0, max: 60.0, step: 0.5, label: "Cycle (s)" },
|
||||
#{ key: "goods", type: "choice", def: "carton", choices: goods_kinds(), label: "Goods" },
|
||||
#{ key: "running", type: "bool", def: true, label: "Running" },
|
||||
#{ key: "colour", type: "choice", def: "robot", choices: ["robot", "mach2", "mach3", "mach1"], label: "Colour" },
|
||||
] }
|
||||
}
|
||||
fn prefab_robot_arm(o) {
|
||||
let p = o.p;
|
||||
let t = arm_targets(o, 0);
|
||||
let run = t.running;
|
||||
let k = 0.9;
|
||||
let gh = goods_h(t.kind) * k;
|
||||
let pk = t.pick;
|
||||
let pt = t.put;
|
||||
if pt.belt == "" { pt.z = 0.14 + gh; }
|
||||
let cycle = t.cycle;
|
||||
let al = (p.reach + 0.6) / 2.0;
|
||||
let da = pt.a - pk.a;
|
||||
while da > PI() { da -= 2.0 * PI(); }
|
||||
while da < -PI() { da += 2.0 * PI(); }
|
||||
let pick_from = if pk.belt == "" { "its " + p.pick + " side" } else { pk.belt };
|
||||
let put_on = if pt.belt == "" { "a pallet" } else { pt.belt };
|
||||
let stopped = if p.running { if run { "" } else { " (waiting: the line before it stands)" } } else { " (stopped)" };
|
||||
info_row("Moves", `${t.kind}s from ${pick_from} to ${put_on}`);
|
||||
info_row("Cycle", `${num1(cycle)} s${stopped}`);
|
||||
// the poses over one cycle: [turn from pick to put (0..1), lift (0..1), holding]
|
||||
let F = [[0.0, 0.0, true], [0.0, 0.5, true], [0.0, 1.0, true], [0.2, 1.0, true],
|
||||
[0.45, 1.0, true], [0.7, 1.0, true], [0.9, 1.0, true], [1.0, 1.0, true],
|
||||
[1.0, 0.4, true], [1.0, 0.0, false], [1.0, 0.6, false], [0.8, 1.0, false],
|
||||
[0.55, 1.0, false], [0.3, 1.0, false], [0.1, 1.0, false], [0.0, 0.5, false]];
|
||||
let N = F.len();
|
||||
let ptx = pt.r * pt.a.cos();
|
||||
let pty = pt.r * pt.a.sin();
|
||||
// poses pointing away from the viewer go behind the base, the others in front
|
||||
let behind = [];
|
||||
let front = [];
|
||||
for i in 0..N {
|
||||
if !run && i != 2 { continue; }
|
||||
let te = F[i][0] * F[i][0] * (3.0 - 2.0 * F[i][0]);
|
||||
let a = pk.a + da * te;
|
||||
if vis(a.cos(), a.sin()) { front.push(i); } else { behind.push(i); }
|
||||
}
|
||||
let s = sprite();
|
||||
// a pallet with a layer of goods where it puts them, when not on a belt
|
||||
if pt.belt == "" {
|
||||
s.box(ptx - 0.6, pty - 0.5, ptx + 0.6, pty + 0.5, 0.0, 0.14, "crate");
|
||||
let layer = [[ptx - 0.3, pty - 0.25], [ptx + 0.3, pty - 0.25], [ptx - 0.3, pty + 0.25], [ptx + 0.3, pty + 0.25]];
|
||||
layer.sort(|a, b| if depth(a[0], a[1]) < depth(b[0], b[1]) { -1 } else { 1 });
|
||||
for q in layer { s.goods(t.kind, q[0], q[1], 0.14, k * 0.85); }
|
||||
}
|
||||
for side in [0, 1] {
|
||||
if side == 1 {
|
||||
s.box(-0.45, -0.45, 0.45, 0.45, 0.0, 0.25, "robotb");
|
||||
s.cyl(0.0, 0.0, 0.33, 0.25, 0.95, p.colour);
|
||||
s.cyl(0.0, 0.0, 0.27, 0.95, 1.2, "robotb");
|
||||
}
|
||||
for i in if side == 0 { behind } else { front } {
|
||||
let f = F[i];
|
||||
let te = f[0] * f[0] * (3.0 - 2.0 * f[0]);
|
||||
let a = pk.a + da * te;
|
||||
let r = pk.r + (pt.r - pk.r) * te;
|
||||
let z = pk.z + (pt.z - pk.z) * te;
|
||||
if run {
|
||||
let cls = if i == 0 { "a flip pose0" } else { "a flip" };
|
||||
// pose 0 (the grip) when the last item on the pick belt goes (t.ph seconds in)
|
||||
let delay = (((N - i) % N) * cycle / N - t.ph % cycle + cycle) % cycle;
|
||||
s.begin(cls, `animation-duration:${cycle}s;animation-delay:-${delay}s`);
|
||||
}
|
||||
// put down on a pallet: the item stays there until the next one comes
|
||||
if !f[2] && pt.belt == "" { s.goods(t.kind, ptx, pty, pt.z - gh, k); }
|
||||
s.arm_pose(r, a, z + gh + f[1] * 0.5, if f[2] { t.kind } else { "" }, k, p.colour, al);
|
||||
if run { s.end(); }
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user