greedy simple interface for layer but can't join multiple layers (#68)

src/greedy_layer.py

+import gcode2contour as gc
+import utils as tu
+
+from math import pi
+import numpy as np
+import warnings
+import pickle
+
+
+home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
+
+
+def zoneblocking(contours, contour_tracker, env, plan=None):
+    """
+    Greedily create a plan on for n arms printer setup
+    by assigning contours to an arm in their designated region
+
+    assumed that all contours are on one layer
+    """
+
+    n_contours = len(contours)
+
+    # continue from a previous plan if provided
+    if plan is None:
+        plan = tu.Plan(env)
+
+    # save calculated trajectories for contours between loops
+    c_traj = {}
+    n_arms = env.numRobots
+    arm_done = [False]*n_arms
+
+    # update collision box for printed material
+    bound_mins, bound_maxs = tu.get_bounding_box(contour_tracker)
+    bound_maxs[2] = tracker[contours[0]].pos[0][2]/1000 - 0.001
+    print_block = env.add_block(bound_mins, bound_maxs)
+
+    # while contours left to do
+    while n_contours>0:
+        
+        # choose robot with the shortest plan
+        arm = np.argmin([plan.cumul_time[arm][-1] for arm in range(plan.n)])
+        j_last = plan.trajs[arm][-1].positions[-1]
+
+        # calculate trajectories for all available contours
+        dists = {}
+        for cid in contours:
+
+            # initialize contour key
+            if cid not in c_traj:
+                c_traj[cid] = [None]*n_arms
+
+            # if not calculated, calc and add to c_traj
+            if c_traj[cid][arm] is None:
+                c_traj[cid][arm] = tu.contour_trajs(
+                        contour_tracker[cid],
+                        env,
+                        arm
+                    )
+
+            # Calculate dists to each trajectory
+            for tid, t in enumerate(c_traj[cid][arm]):
+                dists[cid,tid] = np.linalg.norm(
+                        np.array(t.positions[0]) - j_last
+                    )
+
+        if len(dists)==0:
+            # this arm cannot do any contours right now
+            arm_done[arm]=True
+            if all(arm_done):
+                warnings.warn("Contours left but impossible for all arms\n" + \
+                        "Leaving greedy prematurely")
+                break
+        else:
+            arm_done[arm] = False
+
+        # loop through contour trajectories by closest first
+        contour_added = False
+        for key, dist in sorted(dists.items(), key=lambda x: x[1]):
+            traj = c_traj[key[0]][arm][key[1]]
+
+            # create trajectory to start of next contour
+            travel_traj = plan.plan_to(traj.positions[0], arm)
+
+            # if no travel found, try another contour trajectory
+            if travel_traj is None:
+                continue
+
+            # try adding traj + contour to planner
+            if plan.appendTrajectory([travel_traj, traj], arm) == 0:
+                # if success, break, else loop again
+                #contour_tracker.do_contour(key[0])
+                contours.remove(key[0])
+                contour_added = True
+                break
+
+        # catch edge cases for when no contour is possible
+        if contour_added:
+            n_contours -= 1
+            print("contour placed in plan,", n_contours, "to go")
+
+        elif list(plan.getLastJoint(arm)) == home:
+            # stay at home for another 10s
+            stayhome = tu.JTrajectory([home, home],[0,10], arm)
+            if plan.appendTrajectory([stayhome], arm)!=0:
+                return plan
+                #raise Exception("cannot stay at home - this should never happen, choose a better home position")
+
+        else:
+            # try going to home pos
+            gohome = plan.plan_to(env.home, arm)
+
+            # if no travel found, stay at last pos
+            if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
+                print("no contour possible, returning to home pose")
+                print(n_contours, "left")
+            else:
+                # No contour possible, cannot return home, stay put at last point for 10s
+                last_js = plan.trajs[arm][-1].positions[-1]
+                stayput = tu.JTrajectory([last_js, last_js],[0,10], arm)
+                if plan.appendTrajectory([stayput], arm)!=0:
+                    raise Exception("cannot go home or stay put - this should never happen")
+
+    # take arms home
+    arms_home=[list(plan.getLastJoint(a)) == home for a in range(n_arms)]
+    arm = 0
+    retries = 0
+    while not all(arms_home) and retries < n_arms:
+        gohome = plan.plan_to(env.home, arm)
+        if list(plan.getLastJoint(arm)) != home and \
+                gohome is not None and \
+                plan.appendTrajectory([gohome], arm)==0:
+            print("Arm {} safely returned to home pose".format(arm))
+            retries = 0
+            arms_home[arm] = True
+        else:
+            print("Failed to take arm {} home, trying another arm")
+            retries +=1
+        arm += 1
+        if arm == n_arms:
+            arm = 0
+
+    #env.del_block(print_block)
+    return plan
+
+
+if __name__ == "__main__":
+
+    # Read gcode file
+    model = "CuteOcto_x2"
+    tracker = tu.get_tracker(model)
+
+    # 3arms simulation
+    env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
+                       [[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
+                       [[0,0,0,1]]*3,
+                       [[0.15,0,0]]*3) # WARNING - also edit urdf
+
+    # 2arms real calibration
+    env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
+                            'calibrations/r2_tforms.yaml')
+
+    # Greedily plan first n contours
+    sim_env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
+    plan = zoneblocking(tracker.layers[0], tracker, sim_env)
+    #plan = zoneblocking(tracker.layers[3], tracker, sim_env, plan=plan)

src/utils/planning_tools.py

@@ -372,7 +372,7 @@ def get_bounding_box(c_tracker, mm2m = True):
             x_max = max(x_max, p[0])
             y_min = min(y_min, p[1])
             y_max = max(y_max, p[1])
-            z_min = min(z_min, p[2])
+            z_min = 0
             z_max = max(z_max, p[2])
 
     if mm2m:

src/utils/simEnv.py

@@ -228,6 +228,8 @@ class SimEnv:
 
     def del_block(self, block_id):
         """
+        NOTE: this does not work:
+         pybullet api removes a link in the arms instead
         remove a block
         """
         p.removeBody(block_id)