Commit f00751b0 authored by Jayant Khatkar's avatar Jayant Khatkar

greedy working for multiple layers end to end (#68)

parent 3bfe6a08
......@@ -10,7 +10,13 @@ import pickle
home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def zoneblocking(contours, contour_tracker, env, plan=None):
def greedy(
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
......@@ -18,6 +24,7 @@ def zoneblocking(contours, contour_tracker, env, plan=None):
assumed that all contours are on one layer
"""
print(contours)
n_contours = len(contours)
# continue from a previous plan if provided
......@@ -30,9 +37,10 @@ def zoneblocking(contours, contour_tracker, env, plan=None):
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)
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:
......@@ -121,25 +129,29 @@ def zoneblocking(contours, contour_tracker, env, plan=None):
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()
#env.del_block(print_block)
return plan
......@@ -161,5 +173,5 @@ if __name__ == "__main__":
# 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)
plan = greedy(tracker.layers[1], tracker, sim_env, finish_home=False)
plan = greedy(tracker.layers[5], tracker, sim_env, plan=plan)
......@@ -226,11 +226,31 @@ class SimEnv:
return self.block
def del_block(self, block_id):
def reset(self):
"""
NOTE: this does not work:
pybullet api removes a link in the arms instead
remove a block
"""
p.removeBody(block_id)
p.resetSimulation()
# Load Table
self.table = p.loadURDF(str(urdf_folder/"table.urdf"),
[0,0,0],
p.getQuaternionFromEuler([0,0,0])
)
# Load Arms
self.numRobots = self.env_desc.n_arms
self.robots = []
for i in range(self.numRobots):
self.robots.append(p.loadURDF(str(urdf_folder/"ur5e_cap.urdf"),
self.env_desc.arm_poss[i],
self.env_desc.arm_quats[i],
useFixedBase = True
))
for i in range(p.getNumJoints(self.robots[-1])):
p.changeDynamics(self.robots[-1],i,0,0)
return
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