updated calibrations for closer setup

calibrations/r1_tforms.yaml

 calibration:
-  pos_ET: [0.05820142961296581, -0.0011950683373783374, -0.28785640254723044]
-  pos_WR: [-0.388600716317397, -0.6901494199465785, -0.02709005886667256]
+  pos_ET: [0.0583955576756524, -0.0015291802355345105, -0.28671339352303044]
+  pos_WR: [-0.4158095419081069, -0.5001583324096827, -0.026129679110063638]
   quat_ET: [0.0, 0.0, 0.0, 1.0]
-  quat_WR: [-0.003080032488291087, -0.0018533720858902496, 0.002346173622450026, 0.9999907868981172]
+  quat_WR: [-0.002564899440212201, 0.001168166383878079, -0.010985575921713694, 0.9999356848316943]
 measurements:
   errors:
-  - [0.0004266722474761675, 0.00010198265835584404, 0.0005118088330671386]
-  - [-0.00046734804161174814, -2.3675467075694656e-05, -0.0005118135986591776]
-  - [-4.872589732518584e-05, -0.00035988400526718685, 9.63463744896309e-09]
-  - [1.8942788887976736e-05, -0.0006219327210683634, 0.0003107987203030266]
-  - [7.045888816736845e-05, 0.0009035094379977737, -0.0003108037324149014]
+  - [0.0005237267524521028, 0.00037308631621280064, 0.0007143107785768793]
+  - [-0.0008095165370946011, -0.0006551133615757543, -0.0012766680605167113]
+  - [0.0008432203104146718, 0.0004343214696079234, 0.0005623558652110006]
+  - [-0.00030442707777106337, 0.00012283384500050287, 0.00021992435046970515]
+  - [-0.0003752700209115334, 0.0012133923406908442, -0.00034228083679220533]
+  - [-2.987720303215813e-05, -0.00022686380143621676, 0.0001223584213527057]
+  - [-0.00017550149675504834, -0.0008268994789845108, -0.00044568304866050845]
+  - [0.0013509208687272345, 0.00014604043557953084, -7.235957650220115e-05]
+  - [-0.0010232755948299532, -0.000580797763645502, 0.0005180421076614455]
   pos_RE:
-  - [-0.3135962224769355, 0.6563655431280159, 0.3228361817971632]
-  - [-0.49221982314623614, 0.6495376176037508, 0.2442383931920958]
-  - [0.33150865600473467, 0.6773316314122931, 0.3170618781339123]
-  - [0.3383302293841271, 0.08602524454942267, 0.31513979644970314]
-  - [0.34165173178988745, -0.06883620908528643, 0.270715257812836]
+  - [0.6133654474476401, 0.25809738704195223, 0.3869939626573008]
+  - [0.7037354624811429, 0.07580539245216339, 0.32090493983640744]
+  - [0.4906390629878007, 0.3080058758000657, 0.3323405012115666]
+  - [0.09781788367546922, 0.23965570549843634, 0.38342688661934693]
+  - [-0.0019315193940353664, 0.37690086058686373, 0.3070929247215546]
+  - [0.21595617073648765, 0.36008203097884656, 0.3634945952185946]
+  - [0.13285911380007764, 0.7099303677582817, 0.38946193182072153]
+  - [-0.02483223940921327, 0.7438151545933889, 0.33743380666802025]
+  - [0.25433489042883756, 0.5950433213813207, 0.3208217522824333]
   quat_RE:
-  - [-0.049425945260145925, 0.006680158986739679, 0.695473277632698, 0.7168189251895385]
-  - [-0.2928146335092252, -0.23530286175211923, 0.6731405182125008, 0.6370039218048245]
-  - [-0.05549083044279985, -0.010027667748255463, 0.3656540023178714, 0.9290410993089707]
-  - [0.023245499623933522, 0.013058665507466245, -0.08696868274845652, 0.995854189238262]
-  - [0.2887045291638117, 0.028143855644489547, -0.029925459165865444, 0.9565365048567022]
+  - [0.016330684476412557, 0.01936828893478289, -0.03405703178592342, 0.9990987422243125]
+  - [0.34354396776699675, 0.18706464518674382, -0.014888098778438117, 0.9201971013032098]
+  - [-0.1074842338060294, -0.20262488717634258, 0.05713745850182319, 0.9716612606334872]
+  - [-0.018341487374695, -0.008615911255164298, 0.1979982332944536, 0.9799928854468292]
+  - [-0.3666797022646775, -0.09163848009443812, 0.33752849718015193, 0.8621037631888501]
+  - [-0.17930148716905275, 0.23905632309324903, 0.2287532914043809, 0.926485284696829]
+  - [-0.0017931099531267444, 0.02099349928771554, 0.5487700491438146, 0.8357077784172217]
+  - [-0.25434263866969303, -0.16689095776053336, 0.6182591931345615, 0.7247156687121583]
+  - [0.3004757311525831, 0.09045061276624541, 0.5268747870679409, 0.7898961832992]
 targets:
   pos_W:
-  - [-0.685, 0, 0]
-  - [-0.685, 0, 0]
-  - [0, 0, 0]
-  - [0, -0.6, 0]
-  - [0, -0.6, 0]
+  - [0.25125, -0.2515, 0.07]
+  - [0.25125, -0.2515, 0.07]
+  - [0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]

calibrations/r2_bed.yaml

+0:
+- 0.621985621123414
+- 0.4862575850122889
+- -0.0027796427967576587
+1:
+- 0.6383800371721462
+- -0.0037814159024361907
+- -0.0012593655951667893
+2:
+- 0.62935949457467
+- -0.20023351774534384
+- -0.0009218748201228755
+3:
+- -0.018326037985723964
+- -0.15585651096330455
+- -0.0010746781480876905
+4:
+- -0.29423323943875057
+- 0.11026249385092629
+- -0.0006805476556519391
+5:
+- -0.44159202477283466
+- 0.46282628602061976
+- 0.015576523602617492
+6:
+- 0.0812603999438149
+- 0.5696710849374528
+- -0.005132850294642027
+7:
+- 0.35921927828832523
+- 0.38487635803820996
+- -0.0018439638305856043

calibrations/r2_tforms.yaml

 calibration:
-  pos_ET: [0.0927495889203254, -0.005301959710349507, -0.362914215798195]
-  pos_WR: [0.43825905034128454, 0.9029120235503049, -0.08109206895088138]
+  pos_ET: [0.09774254069673029, -0.0030706908774323655, -0.36443659535104705]
+  pos_WR: [0.4362519130725449, 0.4876751568474799, -0.08336129867916599]
   quat_ET: [0.0, 0.0, 0.0, 1.0]
-  quat_WR: [-0.0009222169090642875, 0.0019453652652404298, -0.9999975933172429, -0.00042240937468334685]
+  quat_WR: [0.0008431908611797105, 0.000107512398679271, 0.9999341190193541, 0.011447056007724494]
 measurements:
   errors:
-  - [-0.0002476915647423583, -0.00025844160198763497, -0.0004483503915942849]
-  - [0.00018312012330701233, -0.00011994988829863837, 0.00044835028728239246]
-  - [0.0004974513862038843, 0.0007910495009358766, 2.6758832649687037e-10]
-  - [-0.00023735500543059507, -0.00023746658634649087, 0.00021717701379939114]
-  - [-0.00019552493931201953, -0.00017519142437383284, -0.00021717717712432805]
+  - [-0.0008930794645337636, 0.0003780440726432466, -0.00018276693952376522]
+  - [-0.00040028957448517977, 0.0004850758729920135, 0.0006177189472589967]
+  - [0.0014270788364738884, -0.0008782711391923104, -0.00043495205082580424]
+  - [-0.00014400387919338842, -1.5133735371541945e-05, -1.1759279944678624e-05]
+  - [0.00041458508714187214, -0.0002395061584320457, 0.00020784484223600186]
+  - [-0.00038915442870135486, -0.0010409342568877267, -0.0001960857536916355]
+  - [0.0004139729189288066, 0.0006571276799571257, -0.0005221848538618373]
+  - [-1.9011559600323746e-05, 0.00023776166408623345, 0.0003294278930152106]
+  - [-0.00041009793663510097, 0.00041583600029115875, 0.00019275719504896482]
   pos_RE:
-  - [-0.27877740206836166, 0.822502524654037, 0.45162436405930095]
-  - [-0.516186610086276, 0.7906287815697902, 0.3506565207158536]
-  - [0.3903377117933912, 0.8319403549381098, 0.4483468877422762]
-  - [0.35899111173245196, 0.28104726952078074, 0.4473359284070865]
-  - [0.42405609638070413, 0.08004711766112144, 0.3994518779549408]
+  - [0.15028780986748264, 0.644206893622099, 0.5166863936818833]
+  - [0.30862399759293635, 0.5820721847642297, 0.46282636106169167]
+  - [0.11220124193179981, 0.4891423235238311, 0.42705841226267205]
+  - [0.5858930525017407, 0.6836438109297134, 0.5128472787722008]
+  - [0.7001677633743908, 0.5249397556804738, 0.4498753761526805]
+  - [0.4350732272998422, 0.8273548102922252, 0.4379531725460367]
+  - [0.568826706055076, 0.24042763113523066, 0.5122472015495493]
+  - [0.5310826089978657, 0.41129339338225357, 0.45878033431032356]
+  - [0.7881969131673572, 0.21429218562571958, 0.5119651744332394]
   quat_RE:
-  - [-0.0023122878877374544, 0.022722453692347426, 0.680162063612887, 0.7327058827693297]
-  - [-0.21377202756563665, -0.26784943495189395, 0.6456892443167647, 0.6823808322337853]
-  - [-0.010637238551265003, 0.004870832174276727, 0.5227218832162491, 0.8524229918044162]
-  - [-0.01670959444897823, 0.019837246026019772, 0.0824697826486496, 0.9962559952508886]
-  - [0.30280166017112836, 0.15608924402269997, -0.17370179284806214, 0.9239994532787341]
+  - [0.009139517884009988, 0.0020144390889252945, 0.6328736492724769, 0.7741985244784073]
+  - [0.23070444720905595, 0.1677513153562072, 0.5081427602746503, 0.8126659150073208]
+  - [0.20439305304249347, -0.14964696615333348, 0.5134526095052029, 0.8198754071085248]
+  - [0.005020428888226247, -0.016835614824863034, 0.35946892551343257, 0.9329916660707455]
+  - [0.28732475112376565, 0.07090386606704172, 0.30035668742025007, 0.9067540953810728]
+  - [-0.28619350582410147, -0.13999817866062705, 0.37421548831994456, 0.8708941126780471]
+  - [0.0005426635528732491, -0.022916387191523394, 0.06980742411215486, 0.9972970812416843]
+  - [-0.28270684601920576, -0.03356454212376598, 0.17110417768796005, 0.9432251168748753]
+  - [0.05435415452018546, 0.2713318266643181, -0.010350466642406752, 0.9608941323401267]
 targets:
   pos_W:
-  - [0.715, 0, 0]
-  - [0.715, 0, 0]
-  - [0, 0, 0]
-  - [0, 0.625, 0]
-  - [0, 0.625, 0]
+  - [0.25125, -0.2515, 0.07]
+  - [0.25125, -0.2515, 0.07]
+  - [0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, -0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]
+  - [-0.25125, 0.2515, 0.07]

src/bed_levelling.py

@@ -151,4 +151,4 @@ if __name__=="__main__":
     with open(calibFile_in, 'r') as file:
         calibData = yaml.load(file, Loader=yaml.FullLoader)
 
-    main(bedFile, calibData, 1) # robot 1 or 2
+    main(bedFile, calibData, 2) # robot 1 or 2

src/gohome2.py

+import numpy as np
+import pickle
+import time
+import copy
+import sys
+
+# ROS Libraries
+import rospy
+from std_msgs.msg import Bool
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+    JointTrajectory
+)
+
+## Our libraries
+from utils import load_plan, JTraj2ROS, speed_multiplier, calibration_correction, calib_corr_single
+from Controller import Controller
+#import gcode2contour as gc
+
+
+def execute_plan(plan, r1_con, r2_con):
+    """
+    move to starting positions of a plan
+    """
+    # plan to the start position of the plan
+    r1_con.group.go(calib_corr_single(plan.trajs[0][0].positions[0],"/r1/robot_description"))
+    r2_con.group.go(calib_corr_single(plan.trajs[1][0].positions[0],"/r2/robot_description"))
+    return
+
+
+if __name__ == '__main__':
+    plan2arms = load_plan('Y')
+
+    r1_con = Controller(robot=1, disable_extruder=True)
+    r2_con = Controller(robot=2, disable_extruder=True)
+    execute_plan(plan2arms, r1_con, r2_con)

src/main1.py

@@ -20,7 +20,7 @@ from Controller import Controller
 
 
 # NEED TO CHANGE
-robot_num = 0 # 0 or 1
+robot_num = 1 # 0 or 1
 robot_desc_param = "/r{}/robot_description".format(robot_num+1)
 
 
@@ -115,7 +115,7 @@ def gohome(arm):
 
 
 if __name__ == '__main__':
-    plan = load_plan('r1_sq')
+    plan = load_plan('r2_sq')
 
     # Test calibration
     con = Controller(robot=robot_num+1, disable_extruder=True) # one arm when both arms running

src/main2.py

@@ -116,10 +116,8 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    #plan2arms, contours = load_plan('octox2', contours=True)
-    #plan2arms, contours = load_plan('flatraptor_l1', contours=True)
-    plan2arms = load_plan('trace2x')
+    plan2arms = load_plan('star')
 
     r1_con = Controller(robot=1, disable_extruder=True)
     r2_con = Controller(robot=2, disable_extruder=True)
-    execute_plan(plan2arms, r1_con, r2_con, move_speed=1)#, contours=contours)
+    execute_plan(plan2arms, r1_con, r2_con)#, contours=contours)