Skip to content
Snippets Groups Projects
Commit 150269b2 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

Merge branch 'feature/decmcts' into feature/hardware-plans

parents 11a4147a 86a9395a
Branches
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
src/decmcts.py
+ 10
− 20
View file @ 150269b2
......@@ -11,7 +11,8 @@ import pickle
import copy
home = [0, -2.1, 0, -pi/2, -pi/2, 0]
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
SAFE_TIME = 2
def avail_contours(data, state, robot_id):
......@@ -56,6 +57,7 @@ class CombinedSchedule:
self.start_times = [[0] for i in range(self.n)]
self.end_times = [[0] for i in range(self.n)]
self.t_mul = joint2time_mul
self.safe_time = SAFE_TIME # safety buffer for Collisions
def planLenOrder(self):
......@@ -122,11 +124,11 @@ class CombinedSchedule:
reversed(self.col_table[ct_r, ct])]
else:
self.col_table[ct, ct_r] = \
tu.col_free_offsets(ct, ct_r, self.data["env"])
tu.col_free_offsets(ct, ct_r, self.data["env"], safe_time=self.safe_time)
ao = copy.deepcopy(self.col_table[ct, ct_r])
# apply t_diff and add allowed offset for when other robot is done
allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1])] + \
allowed_offsets.append([(-1e6, t_0 - self.end_times[r][-1]-self.safe_time)] + \
[(a+t_diff, b+t_diff) for a,b in ao if a+t_diff<=0])
# easy for n = 2
......@@ -256,7 +258,7 @@ def decmcts(contour_tracker,
# save calculated trajectories for contours between loops
c_traj = {}
if c_traj_save_file:
if c_traj_save_file and False: # NOTE: force skipping loading of save file
try:
with open(c_traj_save_file, 'rb') as f:
c_traj = pickle.load(f)
......@@ -284,11 +286,13 @@ def decmcts(contour_tracker,
c_traj[cid] = [None]*n_arms
if c_traj[cid][arm] is None:
c_traj[cid][arm] = tu.contour_trajs(
multiple_trajs = tu.contour_trajs(
contour_tracker.all_contours[cid],
env,
arm
)
if len(multiple_trajs)>0:
c_traj[cid][arm] = [tu.get_closest_traj(multiple_trajs,home)]
update_savefile = True
# Save file so don't have to recalculate on future runs
......@@ -469,21 +473,7 @@ if __name__ == "__main__":
# Read gcode file
model = "flexirex"
pkl = "gcode/" + model + ".pkl"
try:
with open(pkl, 'rb') as f:
tracker = pickle.load(f)
except:
contours = gc.decode_gcode("gcode/" + model + ".gcode")
# 5mm safety, max 5 layers ahead
print("\nCalculating dependency graph - this could take some time...")
tracker = gc.DependencyGraph(contours, 5, 5)
with open(pkl, 'wb') as f:
pickle.dump(tracker, f)
print("Dependency graph completed and saved to " + pkl)
tracker = tu.get_tracker(model)
# 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
......
src/gcode/flexirex_big.gcode 0 → 100644
+ 50203
− 0
View file @ 150269b2
This diff is collapsed.
src/gen_cal_test.py deleted 100644 → 0
+ 0
− 76
View file @ 11a4147a
"""
Generate Plan fo rtesting calibration
"""
import gcode2contour as gc
import utils as tu
import pybullet as p
import time
from math import pi
import numpy as np
import warnings
import pickle
import copy
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def plan(points, env, gui=False):
"""
Create plan which simply goines to certain points
"""
plan = tu.Plan(env)
n_arms = 1
arm = 0
# Got to each point
for p in points:
# Go to 5cm above the point
p_above = copy.deepcopy(p)
p_above[2] = p[2] + 0.05
jabove = tu.get_closest(env.ik_pos(p_above,0),home, max_diff=100)
go2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([go2above], arm)
# Go to the point
jpoint = tu.get_closest(env.ik_pos(p,0),home, max_diff=100)
go2point = plan.plan_to(jpoint, arm)
plan.appendTrajectory([go2point], arm)
# go to 5cm above point again
back2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([back2above], arm)
# try going to home pos
gohome = plan.plan_to(env.home, arm)
if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
print("done")
else:
raise Exception("cannot plan back to home position")
return plan
if __name__ == "__main__":
# 1arm real calibration
env_desc1 = tu.read_env('calibrations/r1_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc1, gui=True, home=home)
# Greedily plan first n contours
points = [
[ 0 , 0 , 0],
[-0.215, 0.215, 0],
[ 0 , 0.215, 0],
[ 0.215, 0.215, 0],
[ 0.215, 0 , 0],
[ 0.215, -0.215, 0],
[ 0 , -0.215, 0],
[-0.215, -0.215, 0],
[-0.215, 0 , 0],
]
plan1 = plan(points, env, gui=True)
plan1.save("cal_test.plan")
src/greedy.py
+ 2
− 16
View file @ 150269b2
......@@ -7,7 +7,7 @@ import warnings
import pickle
home = [0, -2.1, 0, -pi/2, -pi/2, 0]
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
......@@ -133,21 +133,7 @@ if __name__ == "__main__":
# Read gcode file
model = "flexirex"
pkl = "gcode/" + model + ".pkl"
try:
with open(pkl, 'rb') as f:
tracker = pickle.load(f)
except:
contours = gc.decode_gcode("gcode/" + model + ".gcode")
# 5mm safety, max 5 layers ahead
print("\nCalculating dependency graph - this could take some time...")
tracker = gc.DependencyGraph(contours, 5, 5)
with open(pkl, 'wb') as f:
pickle.dump(tracker, f)
print("Dependency graph completed and saved to " + pkl)
tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,0,1],[0,0,0,1]],
......
src/test_decmcts.py 0 → 100644
+ 84
− 0
View file @ 150269b2
"""
Test Functions in decmcts.py
"""
from decmcts import *
SAFE_TIME = 2
def test_reward(contour_tracker, n_contours=None, n_arms=2, gui=False):
"""
test _reward function
"""
# Parse inputs
if n_contours is None:
#n_contours = len(contour_tracker.all_contours)
n_contours = len(contour_tracker.available_contours)
env = tu.SimEnv(gui=gui, home=home) # pyBullet env for collision checking
#plan = tu.Plan(env, [env.home, env.home]) # planner for 2 arms
#plan for these contours
contours = list(range(n_contours))
c_traj = {}
for arm in range(n_arms):
for cid in contours:
if cid not in c_traj:
c_traj[cid] = [None]*n_arms
c_traj[cid][arm] = tu.contour_trajs(
contour_tracker.all_contours[cid],
env,
arm
)
# random order of trajectories
np.random.seed(1)
arm1_plan = deque([c_traj[c][0][0] for c in contours if len(c_traj[c][0])>0])
np.random.shuffle(arm1_plan)
arm2_plan = deque([c_traj[c][1][0] for c in contours if len(c_traj[c][1])>0])
np.random.shuffle(arm2_plan)
data = {"env": env,
"collisions": {}}
s = time.time()
p,r = createCombinedSchedule(data, {0:arm1_plan, 1:arm2_plan})
e = time.time()
print(e-s)
print("orange horizonatl line means looking for start time above" + \
", black line means looking for a start time below\n")
i,j = 1, 1
robot = 1
while i<len(p.start_times[0]) and j<len(p.start_times[1]):
# run for a pair of trajectories
ct_r0, ct_r1 = p.trajs[0][i-1], p.trajs[1][j-1]
t_wait = p.start_times[1][j] - p.start_times[0][i]
tu.vis_col_region(ct_r0, ct_r1, env, res=1, chosen_time=t_wait, safe_time = SAFE_TIME)
# increment ct we're up to
if p.end_times[0][i] <= p.end_times[1][j]:
i +=1
robot = 0
else:
j +=1
robot = 1
if input('enter for next, or "q" for quit')=='q':
break
return data, r, p, c_traj
if __name__ == "__main__":
# Read gcode file
model = "flexirex_big"
tracker = tu.get_tracker(model)
# run test test_reward
data, r, p, c_traj = test_reward(tracker, n_contours=50, n_arms=2, gui=False)
src/test_plan_gen.py deleted 100644 → 0
+ 0
− 179
View file @ 11a4147a
import gcode2contour as gc
import utils as tu
import pybullet as p
import time
from math import pi
import numpy as np
import warnings
import pickle
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def plan(contour_tracker, env, n_contours=None, gui=False):
"""
Create Toy plans for a single armed printer
"""
if n_contours is None:
#n_contours = len(contour_tracker.all_contours)
n_contours = len(contour_tracker.available_contours)
# Collision object for printed material
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_block = None # collision object for print so far
# plan 1
plan = tu.Plan(env)
# save calculated trajectories for contours between loops
c_traj = {}
n_arms = 1
# choose robot with the shortest plan
arm = 0
# For plan 1 (no contour execution, just go to origin
if n_contours == 0:
go_origin = plan.plan_to(env.ik_pos((0,0,0.01),0)[4], arm)
if go_origin is not None and plan.appendTrajectory([go_origin], arm)==0:
print("0 contours requested, going to origin")
else:
print("COULD NOT GO TO ORIGIN")
# while contours left to do
while n_contours > 0 and len(contour_tracker.avail_contour_ids) > 0:
# 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
print_block = env.add_block(bound_mins, bound_maxs)
j_last = plan.trajs[arm][-1].positions[-1]
# calculate trajectories for all available contours
dists = {}
for cid in contour_tracker.avail_contour_ids:
# 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.all_contours[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
warnings.warn("Contours left but impossible for all arms\n" + \
"Leaving greedy prematurely")
break
# 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])
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")
else:
warnings.warn("cannot do any more contours")
break
# try going to home pos
gohome = plan.plan_to(env.home, arm)
if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
print("no contour possible, returning to home pose")
print(n_contours, "left")
else:
raise Exception("cannot plan back to home position")
return plan
def remove_shorts(contours, min_extrude=5):
"""
removes contours which i dont like
"""
return [c for c in contours if c.Ext[-1] >= min_extrude]
if __name__ == "__main__":
# Read gcode file
model = "flexirex"
pkl = "gcode/" + model + ".pkl"
try:
with open(pkl, 'rb') as f:
tracker = pickle.load(f)
except:
contours = gc.decode_gcode("gcode/" + model + ".gcode", max_wp_dist=20)
contours = remove_shorts(contours)
# 5mm safety, max 5 layers ahead
print("\nCalculating dependency graph - this could take some time...")
tracker = gc.DependencyGraph(contours, 5, 5)
with open(pkl, 'wb') as f:
pickle.dump(tracker, f)
print("Dependency graph completed and saved to " + pkl)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,0,1],[0,0,0,1]],
[[0.9, -0.25, 0],[-0.5, -0.25, 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')
# 1arm real calibration
env_desc1 = tu.read_env('calibrations/r1_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc1, gui=True, home=home)
# Greedily plan first n contours
plan1 = plan(tracker, env, n_contours=0, gui=True)
plan1.save("origin.plan")
plan2 = plan(tracker, env, n_contours=1, gui=True)
plan2.save("flexirex1.plan")
plan3 = plan(tracker, env, n_contours=200, gui=True)
plan3.save("flexirex200.plan")
#plan3, env = plan(tracker, env_desc1, n_contours=25, gui=True)
src/utils/planning_tools.py
+ 111
− 5
View file @ 150269b2
......@@ -7,6 +7,9 @@ from random import uniform
from .motion_planners.rrt_connect import birrt
from math import pi
from bisect import bisect_left
import gcode2contour as gc
import pickle
import matplotlib.pyplot as plt
class JTrajectory:
......@@ -49,6 +52,16 @@ joint_max = np.array([ pi]*6)
joint_range = joint_max - joint_min
def get_closest_traj(traj_list, js):
"""
Get the closest trajectory to a particular joint configuration
"""
js = np.array(js)
traj_starts = np.array([t.positions[0] for t in traj_list])
diffs = np.linalg.norm(traj_starts-js, axis=1)
return traj_list[np.argmin(diffs)]
def get_closest(sols, js, max_diff=1, joint_range=joint_range):
"""
Get the closest solution
......@@ -148,7 +161,18 @@ def joints_at_time(traj, t):
return (t-t1)/(t2-t1)*(j2-j1) + j1
def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2):
def safe_ranges(time_ranges, safety_dist):
"""
Times range are not sufficiently safe, add a buffer to make them safer
"""
safe_range = []
for trange in time_ranges:
if abs(trange[1] - trange[0]) > 2*safety_dist:
safe_range.append((trange[0] + safety_dist, trange[1] - safety_dist))
return safe_range
def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2, safe_time=0):
"""
Find time offsets for two trajectories can be executed simultaneously
......@@ -157,7 +181,7 @@ def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2):
"""
n = t1.time[-1]
m = t2.time[-1]
n_offsets = int((n + m)*freq) +1
n_offsets = int((n + m)*freq) + 1
allowed_offsets = [True] * n_offsets
offset_vals = np.append(np.arange(-m, n, 1/freq), n)
......@@ -180,7 +204,7 @@ def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2):
if not any(allowed_offsets):
return []
elif all(allowed_offsets):
return [(-m, n)]
return safe_ranges([(-m, n)], safe_time)
# get start and end of allowed ranges from bool list
s_inds = []
......@@ -195,8 +219,66 @@ def col_free_offsets(t1, t2, sim_env, freq=1, min_range=2):
if len(e_inds)<len(s_inds):
e_inds.append(i)
return [(offset_vals[s], offset_vals[e]) for s,e in zip(s_inds, e_inds)
if (offset_vals[e]-offset_vals[s]) > min_range]
return safe_ranges([(offset_vals[s], offset_vals[e]) for s,e in zip(s_inds, e_inds)
if (offset_vals[e]-offset_vals[s]) > min_range], safe_time)
def col_region(t1, t2, sim_env, freq=1):
"""
returns a numpy array of collision occurance checked at some frequency
"""
n = t1.time[-1]
m = t2.time[-1]
# timesteps along offset paths
jointss = [sim_env.home] * sim_env.numRobots
t1_steps = np.arange(0,n, 1/freq)
t2_steps = np.arange(0,m, 1/freq)
cols = np.empty((len(t1_steps),len(t2_steps)))
for i,t2_t in enumerate(t2_steps):
for j,t1_t in enumerate(t1_steps):
jointss[t1.robot] = joints_at_time(t1, t1_t)
jointss[t2.robot] = joints_at_time(t2, t2_t)
cols[j,i] = sim_env.colCheck(jointss)
return cols
def vis_col_region(t1, t2, sim_env, res=1, chosen_time=None, safe_time=0):
"""
Visualise collsion region between a pair of trajectories and
their collision free offsets
"""
col_table = col_region(t1, t2, sim_env, freq=1)
ranges = col_free_offsets(t1, t2, sim_env, safe_time=safe_time)
# Main plot
fig, ax = plt.subplots()
im = ax.imshow(np.flip(col_table, axis=0),
cmap='RdYlGn_r', vmin=0, vmax=1,
extent=[0,col_table.shape[1],0,col_table.shape[0]])
# plot valid time ranges
if len(ranges) == 0:
print("whole region blocked")
for r in ranges:
ax.plot([0,col_table.shape[1]],[r[0],r[0]+col_table.shape[1]],
linewidth=1,
color='black')
ax.plot([0,col_table.shape[1]],[r[1],r[1]+col_table.shape[1]],
linewidth=1,
color='black')
# plot the chosen wait time if any
if chosen_time is not None:
ax.plot([0,col_table.shape[1]],[chosen_time, chosen_time + col_table.shape[1]],
linewidth=2,
color = 'blue')
plt.show()
return
def _diff(p1,p2):
......@@ -330,3 +412,27 @@ def get_completed_height(c_tracker, l_height=0, mm2m=True):
if mm2m:
return m/1000 - l_height
return m - l_height
def get_tracker(model):
"""
Get contour tracker object for a model
"""
# Read gcode file
pkl = "gcode/" + model + ".pkl"
try:
with open(pkl, 'rb') as f:
tracker = pickle.load(f)
except:
contours = gc.decode_gcode("gcode/" + model + ".gcode")
# 5mm safety, max 5 layers ahead
print("\nCalculating dependency graph - this could take some time...")
tracker = gc.DependencyGraph(contours, 5, 5)
with open(pkl, 'wb') as f:
pickle.dump(tracker, f)
print("Dependency graph completed and saved to " + pkl)
return tracker
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