send extrusion speed per point

Author: tetov

src/clay_3dp_print/__init__.py

@@ -0,0 +1,2 @@
+from .print_frame import PrintFrame  # noqa: F401
+from .print_layer import PrintLayer  # noqa: F401

src/clay_3dp_print/__main__.py

@@ -1,75 +1,120 @@
 import sys
+import typing
 
 from compas import json_load
 import compas.geometry
 from compas_fab.backends import RosClient
+from compas_fab.backends.ros.messages import ROSmsg
 import compas_rrc as rrc
+from clay_3dp_print import PrintLayer
 
 # IP = "192.168.8.30" # cook
 IP = "localhost"
 
 WOBJ = "wobj_pallet_markers"
 TOOL = "t_erratic_t25"
 
-DRY_RUN = False
-
 SPEED = 75
 SPEED_PRINT = 50
 
+EXTRUSION_FACTOR_DO = "ao_printPtSpd"
+
+# negative means move towards the sky
+Z_ADJUSTMENT = -3
+
 
-def get_set_extruder(state: int):
-    return rrc.SetDigital("do_extrudeRelSpd", state)
+def get_set_extruder(speed_factor: float):
+    return rrc.SetAnalog(EXTRUSION_FACTOR_DO, speed_factor)
 
 
-def get_start_extrude():
-    if not DRY_RUN:
-        return get_set_extruder(1)
+def get_start_extrude(speed_factor=1):
+    return get_set_extruder(speed_factor)
 
 
 def get_stop_extrude():
-    if not DRY_RUN:
-        return get_set_extruder(0)
+    return get_set_extruder(0)
 
 
-def construct_cmds(frames, travel):
-    last_was_t = True
+def construct_cmds(layers: list[PrintLayer]) -> typing.Generator[ROSmsg]:
+    first_print_frame = layers[0].pop(0)
 
-    cmds = []
-    for i, (lframes, ltravel) in enumerate(zip(frames, travel)):
-        lcmds = []
+    first_print_frame.translate_frame_in_local_Z(Z_ADJUSTMENT)
 
-        lcmds.append(rrc.PrintText(f"Layer {i}"))
+    yield rrc.MoveToFrame(
+        first_print_frame, SPEED, rrc.Zone.FINE, motion_type=rrc.Motion.JOINT
+    )
 
-        x, y, z = lframes[0].point
+    for i, layer in enumerate(layers):
+        yield rrc.PrintText(f"Layer {i}")
+
+        x, y, z = layer[0].point
         first_pt_str = f"{x:.2f},{y:.2f},{z:.2f}"
 
-        lcmds.append(rrc.PrintText(f"First frame: {first_pt_str}"))
+        yield rrc.PrintText(f"First frame: {first_pt_str}")
 
-        for f, t in zip(lframes, ltravel, strict=True):
-            if not DRY_RUN:
-                if not t and last_was_t:
-                    lcmds.append(get_start_extrude())
+        for print_frame in layer:
+            yield get_set_extruder(print_frame.extrusion_factor)
 
-                if t and not last_was_t:
-                    lcmds.append(get_stop_extrude())
+            print_frame.translate_frame_in_local_Z(Z_ADJUSTMENT)
 
-            lcmds.append(
-                rrc.MoveToFrame(
-                    f,
-                    SPEED if t else SPEED_PRINT,
-                    rrc.Zone.Z1,
-                    motion_type=rrc.Motion.LINEAR,
-                )
-            )
-            last_was_t = t
+            if print_frame.is_travel():
+                # TODO: Add retraction
+                pass
 
-        cmds.append(lcmds)
-    return cmds
+            yield rrc.MoveToFrame(
+                print_frame,
+                SPEED if print_frame.is_travel() else SPEED_PRINT,
+                rrc.Zone.Z1,
+                motion_type=rrc.Motion.LINEAR,
+                feedback_level=rrc.FeedbackLevel.DONE,
+            )
 
 
-def robot_program(
-    frames: list[list[compas.geometry.Frame]], travel: list[list[bool]] | None
+def process_with_batches(
+    abb: rrc.AbbClient, cmd_generator: typing.Generator[ROSmsg], batch_size: int = 100
 ):
+    futures = []
+    commands_sent = 0
+
+    # Send the first batch
+    for _ in range(batch_size):
+        try:
+            cmd = next(cmd_generator)
+
+            feedback = abb.send(cmd)
+
+            if feedback:
+                futures.append(feedback)
+
+            commands_sent += 1
+        except StopIteration:
+            break
+
+    while futures:
+        completed = []
+        for f in futures:
+            try:
+                f.result(timeout=3)
+                completed.append(f)
+            except rrc.TimeoutException:
+                continue
+
+        # Remove completed futures
+        for f in completed:
+            futures.remove(f)
+
+        # Add more commands to maintain the buffer
+        for _ in range(len(completed)):
+            try:
+                cmd = next(cmd_generator)
+                futures.append(abb.send(cmd))
+                commands_sent += 1
+            except StopIteration:
+                break
+
+
+def robot_program(layers: list[PrintLayer]):
+    breakpoint()
     # Define robot joints
     robot_joints_start_position = robot_joints_end_position = [100, 20, 25, 5, -40, -10]
 
@@ -81,19 +126,7 @@ def robot_program(
     override = 100  # Unit [%]
     max_tcp = 250  # Unit [mm/s]
 
-    z_adjustment_mm = -3  # negative means raise up printbed
-
-    if z_adjustment_mm != 0:
-        for layer in frames:
-            for frame in layer:
-                v = frame.normal.copy()
-                v.scale(z_adjustment_mm)
-                frame.point.translate(v)
-
-    first_frame = frames[0].pop(0)
-    _ = travel[0].pop(0)
-
-    cmds = construct_cmds(frames, travel)
+    cmd_generator = construct_cmds(layers)
 
     with RosClient(host=IP, port=9090) as ros:
         abb = rrc.AbbClient(ros, "/rob1")
@@ -102,8 +135,7 @@ def robot_program(
         abb.send(rrc.SetAcceleration(acc, ramp))
         abb.send(rrc.SetMaxSpeed(override, max_tcp))
 
-        if not DRY_RUN:
-            abb.send_and_wait(get_stop_extrude())  # reset signal
+        abb.send(get_stop_extrude())  # reset signal
 
         abb.send(rrc.SetTool(TOOL))
         abb.send(rrc.SetWorkObject(WOBJ))
@@ -117,21 +149,13 @@ def robot_program(
         abb.send(rrc.Stop())
 
         # Move robot to start position
-        abb.send_and_wait(
+        abb.send(
             rrc.MoveToJoints(
                 robot_joints_start_position, external_axis_dummy, SPEED, rrc.Zone.FINE
             )
         )
 
-        abb.send(rrc.MoveToFrame(first_frame, SPEED, rrc.Zone.FINE))
-
-        for lcmds in cmds:
-            abb.send_and_wait(lcmds.pop(0))
-            for c in lcmds:
-                abb.send(c)
-
-        if not DRY_RUN:
-            abb.send_and_wait((get_stop_extrude()))
+        process_with_batches(abb, cmd_generator)
 
         # Move robot to end position
         abb.send(
@@ -144,24 +168,31 @@ def robot_program(
         abb.send_and_wait(rrc.PrintText("Print finished."))
 
 
+def load_json_from_arg1():
+    filepath = sys.argv[1]
+    data = json_load(filepath)
+    frames = data["frames"]
+    extrusion_factors = data["travel"]
+
+    # put all in one "layer"
+    if isinstance(frames[0], compas.geometry.Frame):
+        if len(extrusion_factors) == len(frames) - 1:
+            # TODO: Fix me
+            extrusion_factors.append(True)
+
+        frames = [frames]
+        extrusion_factors = [extrusion_factors]
+
+    return [
+        PrintLayer.from_frames_and_factors(f, ef)
+        for f, ef in zip(frames, extrusion_factors, strict=True)
+    ]
+
+
 def main():
     if len(sys.argv) > 1:
-        filepath = sys.argv[1]
-        data = json_load(filepath)
-        frames = data["frames"]
-        travel = data["travel"]
-
-        # put all in one "layer"
-        if isinstance(frames[0], compas.geometry.Frame):
-            frames = [frames]
-            travel = [travel]
-
-        print(len(frames))
-        print(len(travel))
-        print(len(frames[0]))
-        print(len(travel[0]))
-
-        robot_program(frames, travel)
+        print_layers = load_json_from_arg1()
+        robot_program(print_layers)
     else:
         print("Usage: clay_3dp_print <filepath>")
 

src/clay_3dp_print/print_frame.py

@@ -0,0 +1,50 @@
+from dataclasses import dataclass
+
+import compas.geometry
+from compas.tolerance import TOL
+
+
+@dataclass
+class PrintFrame(compas.geometry.Frame):
+    """Frame and extrusion value"""
+
+    def __init__(self, frame: compas.geometry.Frame, extrusion_factor: float):
+        super().__init__(
+            frame.point, xaxis=frame.xaxis, yaxis=frame.yaxis, name=frame.name
+        )
+        self._extrusion_factor: float = extrusion_factor
+
+    DATASCHEMA = {
+        "type": "object",
+        "properties": {
+            "point": compas.geometry.Point.DATASCHEMA,
+            "xaxis": compas.geometry.Vector.DATASCHEMA,
+            "yaxis": compas.geometry.Vector.DATASCHEMA,
+            "extrusion_factor": "number",
+        },
+        "required": ["point", "xaxis", "yaxis", "extrusion_factor"],
+    }
+
+    @property
+    def __data__(self):
+        return super().__data__ | {"extrusion_factor": self.extrusion_factor}
+
+    @property
+    def extrusion_factor(self) -> float:
+        return float(self._extrusion_factor)
+
+    @extrusion_factor.setter
+    def extrusion_factor(self, value):
+        self._extrusion_factor = value
+
+    def is_travel(self) -> bool:
+        if not isinstance(self.extrusion_factor, bool):
+            return TOL.is_close(self.extrusion_factor, 0)
+        # TODO: Clean this up when I only set numbers not bools
+        return bool(self.extrusion_factor)
+
+    def translate_frame_in_local_Z(self, signed_distance: float) -> None:
+        """Translates along local Z axis in place."""
+        v = self.normal.copy()
+        v.scale(signed_distance)
+        self.point.translate(v)

src/clay_3dp_print/print_layer.py

@@ -0,0 +1,15 @@
+import typing
+from collections import UserList
+
+from clay_3dp_print.print_frame import PrintFrame
+
+
+class PrintLayer(UserList):
+    def __init__(self, print_frames: list[PrintFrame] | None = None):
+        self.data = print_frames or []
+
+    @classmethod
+    def from_frames_and_factors(cls, frames, extrusion_factors) -> typing.Self:
+        return cls(
+            [PrintFrame(f, ef) for f, ef in zip(frames, extrusion_factors, strict=True)]
+        )