Commit 22db64cc authored by Jayant Khatkar's avatar Jayant Khatkar

add gcodes, use home2, update gcode2contour used (#66,...

add gcodes, use home2, update gcode2contour used (#66, twins-controller#19)
parent 83627ec2
......@@ -4,7 +4,7 @@ certifi==2020.11.8
cycler==0.10.0
Cython==0.29.21
decorator==4.4.2
gcode2contour @ git+https://code.research.uts.edu.au/bigprint/gcode2contour.git@01eb49aee19574572fafdb7d8134c3638783fc75
gcode2contour @ git+https://code.research.uts.edu.au/bigprint/gcode2contour.git@415b9dd3fff7c2ecd85558ebcfe5f90f1fda43a6
ipython==7.19.0
ipython-genutils==0.2.0
jedi==0.17.2
......
......@@ -511,7 +511,7 @@ def decmcts(contour_tracker,
if __name__ == "__main__":
# Read gcode file
model = "CuteOcto_x2"
model = "Kitchen_sponge_holder_small-ascii"
tracker = tu.get_tracker(model)
# 3arms simulation
......@@ -528,8 +528,8 @@ if __name__ == "__main__":
# decmcts plan first n contours
cs, plan = decmcts(tracker,
env,
n_layers=2,
mcts_nodes=200
n_layers=1,
mcts_nodes=1000
)
visualiseCombinedSchedule(cs)
plan.visualise()
This diff is collapsed.
This diff is collapsed.
......@@ -14,6 +14,7 @@ import copy
home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
home2 = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def plan(points, env, gui=False):
......@@ -27,23 +28,26 @@ def plan(points, env, gui=False):
# 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
print("-----")
try:
# 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)
jabove = tu.get_closest(env.ik_pos(p_above,0), home2, 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)
# Go to the point
jpoint = tu.get_closest(env.ik_pos(p,0), home2, 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)
except:
print("Could not do point {}".format(p))
# try going to home pos
gohome = plan.plan_to(env.home, arm)
......@@ -57,7 +61,7 @@ def plan(points, env, gui=False):
if __name__ == "__main__":
# 1arm real calibration
env_desc1 = tu.read_env('calibrations/r1_tforms.yaml')
env_desc1 = tu.read_env('calibrations/r2_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc1, gui=True, home=home)
# Greedily plan first n contours
......
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