Commit 732a6957 authored by Jayant Khatkar's avatar Jayant Khatkar

zoneblocking working (implement #67)

parent f00751b0
......@@ -19,7 +19,6 @@ def greedy(
col_print=False):
"""
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
"""
......
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,
finish_home=True,
col_print=False):
"""
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
if col_print:
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:
# only select from contours in their zone
if contour_tracker[cid].pos[0][1] > 0 and arm == 0 or \
contour_tracker[cid].pos[0][1] < 0 and arm == 1:
continue
# 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
if finish_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".format(arm))
retries +=1
arm += 1
if arm == n_arms:
arm = 0
# if a collision object was created, remove it
if col_print:
env.reset()
return plan
if __name__ == "__main__":
# Read gcode file
model = "flexirex_big"
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[1], tracker, sim_env, finish_home=False)
plan = zoneblocking(tracker.layers[5], tracker, sim_env, plan=plan)
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