updated calibration process with improved z axis alignment (#15)

calibrations/r1_tforms_alt.yaml

+calibration:
+  pos_ET: [0.14982225501989826, -0.048118796065005576, -0.03482924194973972]
+  pos_WR: [0.033410320051684865, -0.5779185680332759, -0.13099530024566308]
+  quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
+  quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
+measurements:
+  errors:
+  - [0.0007463740373759575, 0.0007358258676100349, 2.272630091060268e-08]
+  - [-0.0009020387968710353, 0.00016398070836140533, 1.617623791638767e-05]
+  - [-0.00032259677224483196, -0.000623335143616649, 4.8670205514383946e-05]
+  - [3.681804322736015e-05, 3.621990973879134e-05, -2.1586398841921817e-05]
+  - [0.00044144348854441295, -0.00031269134204364724, -4.3282771052338687e-05]
+  pos_RE:
+  - [0.08910742634272081, -0.7837172401184285, 0.27405864289606996]
+  - [-0.12486057160685166, -0.3528862439702894, 0.2790308995256403]
+  - [0.3056457370236582, -0.3563077645137278, 0.27594833824244247]
+  - [-0.3553141725527921, -0.56732851836896, 0.27599090687109595]
+  - [0.47642188171860617, -0.18642048697664007, 0.20344840058382788]
+  quat_RE:
+  - [0.2692242146471514, 0.6541983200550241, -0.27068386305481473, 0.6528959538598255]
+  - [0.26903425766037653, 0.6541193957881474, -0.2707136444182161, 0.6530409688409196]
+  - [0.2689585934995034, 0.6539863896443541, -0.2707389095513149, 0.653194856071048]
+  - [0.3517200864442189, 0.3547622789699167, -0.14691067603123373, 0.8537294416147239]
+  - [0.019597120228666234, 0.6766897609973048, -0.526359272046243, 0.5144442019996951]
+targets:
+  pos_W:
+  - [0, 0.215, 0]
+  - [0.215, -0.215, 0]
+  - [-0.215, -0.215, 0]
+  - [0.322, 0, 0]
+  - [-0.322, -0.322, 0]

src/main.py

@@ -35,7 +35,6 @@ def execute_plan(plan, controller):
 
     # execute trajectories in plan
     for t in plan.trajs[0]:
-        print(t.time)
         jt = JTraj2ROS(t)
         try:
             controller.exec_traj(jt)
@@ -46,12 +45,12 @@ def execute_plan(plan, controller):
 
 
 if __name__ == '__main__':
-    plan = load_plan('plans/2.plan')
+    plan1 = load_plan('plans/1.plan')
+    plan2 = load_plan('plans/2.plan')
     con = Controller(disable_extruder=True)
-    execute_plan(plan, con)
+    execute_plan(plan2, con)
 
     # go to origin and stay (to check calibration)
-    #plan1 = load_plan('plans/1.plan')
-    #plan2 = copy.deepcopy(plan1)
-    #plan2.trajs[0].pop()
-    #execute_plan(plan2, con)
+    #plan05 = copy.deepcopy(plan1)
+    #plan05.trajs[0].pop()
+    #execute_plan(plan05, con)

src/update_test_plans.sh

+# Copy new calibration files to planner
+cp ../calibrations/r1_tforms_alt.yaml ../../twins-planner/src/calibrations/r1_tforms.yaml
+
+# Create new test plans
+cd ../../twins-planner/src
+conda activate planner
+python test_plan_gen.py
+mv *.plan ../../twins-controller/src/plans
+cd ../twins-controller/src/plans