test r2 calibration, add first parse of real plan for first print (#19)

src/KeyboardController.py

@@ -55,7 +55,7 @@ class KeyboardController(Controller):
     """
 
     def __init__(self,
-        homeConf_deg=np.array([-90,-90,45,-45,-90,-45]),
+        homeConf_deg=np.array([0,-90,45,-45,-90,-45]),
         waitForExtruder=True,
         timeScale=1.0,
         disableExtruder=False,
@@ -595,7 +595,7 @@ class KeyboardController(Controller):
                 if priors:
                     # calculate IK for next point and move there
                     mat = np.identity(4)
-                    p[2] += 0.01 # go 1cm above bed for safety
+                    #p[2] += 0.01 # go 1cm above bed for safety
                     mat[:3,3] = p
                     mat = world2armT.dot(mat).dot(_vert_or[arm]).dot(tip2armT)
                     q = Rotation.from_dcm(mat[:3,:3]).as_quat()
@@ -662,14 +662,14 @@ class KeyboardController(Controller):
 
 
 if __name__=="__main__":
-    con = KeyboardController(disableExtruder=True)
+    #con = KeyboardController(disableExtruder=True)
     #con = KeyboardController()
     #con1 = KeyboardController(robot=1)
-    #con2 = KeyboardController(robot=2, disableExtruder=True)
+    con2 = KeyboardController(robot=2, disableExtruder=True)
 
     print('Collecting configrations...')
-    configuations = con.prompt_configurations()
+    #configuations = con.prompt_configurations()
     #configuations = con1.prompt_configurations()
-    #configuations = con2.prompt_configurations()
+    configuations = con2.prompt_configurations()
     print('Your configurations:')
     print(configuations)

src/main1.py

@@ -83,7 +83,7 @@ def execute_plan(plan,
 
 if __name__ == '__main__':
     plan1 = load_plan('origin')
-    calibration_test = load_plan('r1_cal_test')
+    calibration_test = load_plan('r2_cal_test')
     plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing

src/main2.py

@@ -22,7 +22,6 @@ def execute_plan(plan,
         r1_con,
         r2_con,
         contours=None,
-        contour_speed=0.2,
         move_speed=1,
         retry_time=0.1,
         max_traj_retries=20):
@@ -84,7 +83,7 @@ def execute_plan(plan,
 
         # Extruding with trajectory
         if t.contour is not None and contours is not None:
-            traj2send = speed_multiplier(t, 1/contour_speed)
+            traj2send = speed_multiplier(t, 1/move_speed)
             while not controllers[arm].exec_ctraj(
                     traj2send,
                     contour=contours[t.contour],
@@ -117,7 +116,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('test_rotation2')
+    plan2arms = load_plan('octox2')
 
     r1_con = Controller(disable_extruder=True, robot=1)
     r2_con = Controller(disable_extruder=True, robot=2)