Commit 3bfe6a08 authored by Jayant Khatkar's avatar Jayant Khatkar

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

parent 1e41aa27
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)
......@@ -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:
......
......@@ -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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment