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

add gcodes, use home2, update gcode2contour used (#66, twins-controller#19)

requirements.txt

@@ -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

src/decmcts.py

@@ -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()

src/gen_cal_test.py

@@ -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