Skip to content

GitLab

Commit 636bd99c authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files
Options

replace old greedy (#68)

parent ae9aa016
Hide whitespace changes
Inline Side-by-side
Showing with 77 additions and 246 deletions
src/greedy.py
View file @ 636bd99c
......@@ -10,40 +10,39 @@ import pickle
home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
def greedy_layer(
contours,
contour_tracker,
env,
plan=None,
finish_home=True,
col_print=False):
"""
Greedily create a plan on for n arms printer setup
"""
if n_contours is None or \
n_contours > len(contour_tracker.available_contours):
n_contours = len(contour_tracker.all_contours)
assumed that all contours are on one layer
"""
l_height = tu.get_layer_height(contour_tracker)
bound_mins, bound_maxs = tu.get_bounding_box(contour_tracker)
bound_mins[2] = 0
last_comp_print_height = 0
print(contours)
n_contours = len(contours)
env = tu.SimEnv(env_desc=env_desc, gui=gui, home=home)
plan = tu.Plan(env)
# 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_desc.n_arms
n_arms = env.numRobots
arm_done = [False]*n_arms
print_block = None # collision object for print so far
# while contours left to do
while len(contour_tracker.available_contours)>0 and n_contours>0:
# 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)
# update collision box for printed material
comp_print_height = tu.get_completed_height(contour_tracker, l_height)
if last_comp_print_height < comp_print_height:
last_comp_print_height = comp_print_height
if print_block != None:
env.del_block(print_block)
bound_maxs[2] = comp_print_height
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)])
......@@ -51,7 +50,7 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
# calculate trajectories for all available contours
dists = {}
for cid in contour_tracker.avail_contour_ids:
for cid in contours:
# initialize contour key
if cid not in c_traj:
......@@ -60,7 +59,7 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
# 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.all_contours[cid],
contour_tracker[cid],
env,
arm
)
......@@ -96,7 +95,8 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
# 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])
#contour_tracker.do_contour(key[0])
contours.remove(key[0])
contour_added = True
break
......@@ -128,23 +128,55 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
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
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
def greedy(sim_env, tracker, nth_layer=1, layers=None):
"""
plan whole print with greedy using greedy_layer
"""
if layers is None:
n_layers = len(tracker.layers)
layers = list(range(0, n_layers, nth_layer))
plan = None
fin_home = False
for i in layers:
# take arms home if final layer
if i == layers[-1]:
fin_home = True
plan = greedy_layer(
tracker.layers[i],
tracker,
sim_env,
plan=plan,
finish_home=fin_home)
return plan
......@@ -166,4 +198,5 @@ if __name__ == "__main__":
'calibrations/r2_tforms.yaml')
# Greedily plan first n contours
plan = greedy(tracker, env_desc2, n_contours=150, gui=True)
sim_env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
greedy(sim_env, tracker, layers=[1,5])
src/greedy_layer.py deleted 100644 → 0
View file @ ae9aa016
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 greedy_layer(
contours,
contour_tracker,
env,
plan=None,
finish_home=True,
col_print=False):
"""
Greedily create a plan on for n arms printer setup
assumed that all contours are on one layer
"""
print(contours)
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:
# 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
def greedy(sim_env, tracker, nth_layer=1, layers=None):
"""
plan whole print with greedy using greedy_layer
"""
if layers is None:
n_layers = len(tracker.layers)
layers = list(range(0, n_layers, nth_layer))
plan = None
fin_home = False
for i in layers:
# take arms home if final layer
if i == layers[-1]:
fin_home = True
plan = greedy_layer(
tracker.layers[i],
tracker,
sim_env,
plan=plan,
finish_home=fin_home)
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)
greedy(sim_env, tracker, layers=[1,5])
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