Phoenix odo thread readability refactor

src/main/deploy/choreo/CMtoI4.traj

@@ -0,0 +1,86 @@
+{
+ "name":"CMtoI4",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.228332996368408, "y":3.8170414450439454, "heading":1.5707963267948966, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.248758316040039, "y":5.417433738708496, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.525346382180391, "y":5.088810322172138, "heading":2.6172720946708985, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"CM.x", "val":7.228332996368408}, "y":{"exp":"CM.y", "val":3.8170414450439454}, "heading":{"exp":"CM.heading", "val":1.5707963267948966}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.248758316040039 m", "val":6.248758316040039}, "y":{"exp":"5.417433738708496 m", "val":5.417433738708496}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"I4.x", "val":5.525346382180391}, "y":{"exp":"I4.y", "val":5.088810322172138}, "heading":{"exp":"I4.heading", "val":2.6172720946708985}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.89726,1.44252],
+  "samples":[
+    {"t":0.0, "x":7.22833, "y":3.81704, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.18878, "ay":7.14909, "alpha":1.44424, "fx":[-32.71598,-9.65549,-8.05994,-27.59955], "fy":[114.85224,119.02398,119.1711,116.21458]},
+    {"t":0.03205, "x":7.22772, "y":3.82071, "heading":1.5708, "vx":-0.03809, "vy":0.22909, "omega":0.04628, "ax":-1.24366, "ay":7.13785, "alpha":1.47196, "fx":[-33.92296,-10.46352,-8.70368,-28.54266], "fy":[114.48909,118.94277,119.11561,115.97657]},
+    {"t":0.06409, "x":7.22586, "y":3.83172, "heading":1.57228, "vx":-0.07795, "vy":0.45783, "omega":0.09345, "ax":-1.3069, "ay":7.12432, "alpha":1.50396, "fx":[-35.3019,-11.38125,-9.45921,-29.64152], "fy":[114.05697,118.84351,119.04625,115.68911]},
+    {"t":0.09614, "x":7.22269, "y":3.85005, "heading":1.57527, "vx":-0.11983, "vy":0.68613, "omega":0.14164, "ax":-1.38055, "ay":7.10778, "alpha":1.54125, "fx":[-36.89687,-12.44002,-10.35149,-30.93007], "fy":[113.53406,118.71981,118.95833,115.33792]},
+    {"t":0.12818, "x":7.21815, "y":3.87568, "heading":1.57981, "vx":-0.16407, "vy":0.9139, "omega":0.19103, "ax":-1.46738, "ay":7.08719, "alpha":1.58523, "fx":[-38.76708,-13.68306,-11.41433,-32.45354], "fy":[112.88894,118.56209,118.84498,114.90279]},
+    {"t":0.16023, "x":7.21213, "y":3.90861, "heading":1.58593, "vx":-0.21109, "vy":1.14101, "omega":0.24183, "ax":-1.57121, "ay":7.06102, "alpha":1.63777, "fx":[-40.99383,-15.17155,-12.69454,-34.27341], "fy":[112.07513,118.35556,118.69584,114.35433]},
+    {"t":0.19227, "x":7.20456, "y":3.9488, "heading":1.59368, "vx":-0.26144, "vy":1.36728, "omega":0.29432, "ax":-1.69746, "ay":7.02689, "alpha":1.70154, "fx":[-43.69151,-16.9948,-14.25882,-36.47531], "fy":[111.02122,118.07634,118.49473,113.64829]},
+    {"t":0.22432, "x":7.19531, "y":3.99622, "heading":1.60312, "vx":-0.31583, "vy":1.59245, "omega":0.34884, "ax":-1.85408, "ay":6.98099, "alpha":1.78034, "fx":[-47.02526,-19.28816,-16.20532,-39.18175], "fy":[109.61293,117.68398,118.21522,112.71551]},
+    {"t":0.25636, "x":7.18424, "y":4.05083, "heading":1.61429, "vx":-0.37525, "vy":1.81616, "omega":0.40589, "ax":-2.0531, "ay":6.91686, "alpha":1.87982, "fx":[-51.24014,-22.26622,-18.6844,-42.57335], "fy":[107.65811,117.10548,117.8118,111.44273]},
+    {"t":0.28841, "x":7.17116, "y":4.11259, "heading":1.6273, "vx":-0.44104, "vy":2.03781, "omega":0.46613, "ax":-2.31357, "ay":6.82286, "alpha":2.00851, "fx":[-56.70935,-26.28858,-21.93774,-46.92532], "fy":[104.81454,116.19854,117.20076,109.63413]},
+    {"t":0.32045, "x":7.15584, "y":4.18139, "heading":1.64224, "vx":-0.51518, "vy":2.25645, "omega":0.53049, "ax":-2.66697, "ay":6.67633, "alpha":2.17939, "fx":[-64.00959,-32.00017,-26.37731,-52.67128], "fy":[100.43029,114.65648,116.21483,106.92807]},
+    {"t":0.3525, "x":7.13796, "y":4.25713, "heading":1.65924, "vx":-0.60064, "vy":2.47039, "omega":0.60033, "ax":-3.1678, "ay":6.42874, "alpha":2.41133, "fx":[-74.00754,-40.6544,-32.75808,-60.51203], "fy":[93.16958,111.72511,114.48168,102.60221]},
+    {"t":0.38454, "x":7.11709, "y":4.33959, "heading":1.67848, "vx":-0.70215, "vy":2.6764, "omega":0.6776, "ax":-3.91212, "ay":5.9636, "alpha":2.72457, "fx":[-87.75573,-54.88408,-42.5851,-71.56425], "fy":[80.12664,105.19487,111.04471,95.08072]},
+    {"t":0.41659, "x":7.09258, "y":4.42842, "heading":1.70019, "vx":-0.82752, "vy":2.86751, "omega":0.76491, "ax":-5.04694, "ay":4.96526, "alpha":3.1136, "fx":[-105.00858,-79.91337,-59.07997,-87.27624], "fy":[55.19413,87.13325,102.93488,80.6539]},
+    {"t":0.44863, "x":7.06347, "y":4.52286, "heading":1.7247, "vx":-0.98925, "vy":3.02662, "omega":0.86469, "ax":-6.50015, "ay":2.61124, "alpha":3.62599, "fx":[-117.96758,-113.54785,-88.051,-107.09951], "fy":[10.62457,30.86174,78.93395,50.97978]},
+    {"t":0.48068, "x":7.02843, "y":4.62119, "heading":1.75241, "vx":-1.19755, "vy":3.1103, "omega":0.98088, "ax":-6.8523, "ay":-1.43749, "alpha":3.6633, "fx":[-109.96245,-103.8591,-117.54148,-118.41742], "fy":[-44.04445,-55.68527,8.02362,-2.65]},
+    {"t":0.51272, "x":6.98654, "y":4.72012, "heading":1.78384, "vx":-1.41713, "vy":3.06423, "omega":1.09828, "ax":-5.39729, "ay":-4.67112, "alpha":2.29011, "fx":[-85.89602,-71.09371,-94.68344,-102.60143], "fy":[-81.84774,-94.68957,-70.70437,-59.3679]},
+    {"t":0.54477, "x":6.93836, "y":4.81591, "heading":1.81904, "vx":-1.59009, "vy":2.91455, "omega":1.17166, "ax":-3.87275, "ay":-6.0798, "alpha":1.36057, "fx":[-63.64712,-50.82836,-64.0308,-75.6986], "fy":[-100.36518,-107.32738,-99.86993,-91.51164]},
+    {"t":0.57681, "x":6.88541, "y":4.90619, "heading":1.85658, "vx":-1.71419, "vy":2.71972, "omega":1.21526, "ax":-2.85644, "ay":-6.6522, "alpha":0.77976, "fx":[-47.71151,-39.15822,-46.04749,-54.57761], "fy":[-109.0082,-112.32589,-109.62386,-105.68815]},
+    {"t":0.60886, "x":6.82901, "y":4.98993, "heading":1.89553, "vx":-1.80572, "vy":2.50655, "omega":1.24025, "ax":-2.19469, "ay":-6.91413, "alpha":0.40596, "fx":[-36.691,-31.89517,-35.31466,-40.15697], "fy":[-113.30426,-114.73126,-113.70221,-112.10139]},
+    {"t":0.6409, "x":6.77002, "y":5.0667, "heading":1.93527, "vx":-1.87605, "vy":2.28498, "omega":1.25326, "ax":-1.7437, "ay":-7.0491, "alpha":0.15354, "fx":[-28.91482,-27.03152,-28.3076,-30.20133], "fy":[-115.61155,-116.06158,-115.74995,-115.27536]},
+    {"t":0.67295, "x":6.70901, "y":5.1363, "heading":1.97543, "vx":-1.93193, "vy":2.0591, "omega":1.25818, "ax":-1.42086, "ay":-7.12545, "alpha":-0.0257, "fx":[-23.26321,-23.58402,-23.36871,-23.04823], "fy":[-116.93738,-116.87379,-116.918,-116.98092]},
+    {"t":0.70499, "x":6.64637, "y":5.19863, "heading":2.01575, "vx":-1.97746, "vy":1.83076, "omega":1.25736, "ax":-1.17988, "ay":-7.17182, "alpha":-0.15855, "fx":[-19.04181,-21.0304,-19.67486,-17.69936], "fy":[-117.74234,-117.40724,-117.64707,-117.95686]},
+    {"t":0.73704, "x":6.5824, "y":5.25361, "heading":2.05604, "vx":-2.01527, "vy":1.60094, "omega":1.25227, "ax":-0.99377, "ay":-7.20155, "alpha":-0.26045, "fx":[-15.81632,-19.07143,-16.78224,-13.56063], "fy":[-118.25329,-117.77781,-118.13292,-118.54075]},
+    {"t":0.76908, "x":6.51731, "y":5.30122, "heading":2.09617, "vx":-2.04712, "vy":1.37016, "omega":1.24393, "ax":-0.84602, "ay":-7.22143, "alpha":-0.34079, "fx":[-13.30688,-17.52465,-14.43358,-10.26705], "fy":[-118.58964,-118.04698,-118.47286,-118.90024]},
+    {"t":0.80113, "x":6.45127, "y":5.34142, "heading":2.13603, "vx":-2.07423, "vy":1.13875, "omega":1.23301, "ax":-0.72603, "ay":-7.23517, "alpha":-0.40559, "fx":[-11.32695,-16.27295,-12.47056,-7.58542], "fy":[-118.8179,-118.24977,-118.71959,-119.12474]},
+    {"t":0.83317, "x":6.38443, "y":5.37419, "heading":2.17555, "vx":-2.09749, "vy":0.9069, "omega":1.22001, "ax":-0.62673, "ay":-7.24493, "alpha":-0.45881, "fx":[-9.74819,-15.23806,-10.79093,-5.36103], "fy":[-118.97689,-118.4073,-118.90371,-119.26476]},
+    {"t":0.86522, "x":6.3169, "y":5.39954, "heading":2.21464, "vx":-2.11758, "vy":0.67474, "omega":1.20531, "ax":-0.54326, "ay":-7.25201, "alpha":-0.5032, "fx":[-8.47961,-14.36582,-9.32596,-3.4877], "fy":[-119.09019,-118.53293,-119.04396,-119.35031]},
+    {"t":0.89726, "x":6.24876, "y":5.41743, "heading":2.25326, "vx":-2.13498, "vy":0.44234, "omega":1.18918, "ax":-0.39355, "ay":-7.25772, "alpha":-0.57441, "fx":[-6.07481,-12.69427,-6.81834,-0.24479], "fy":[-119.18734,-118.67768,-119.17142,-119.35534]},
+    {"t":0.92323, "x":6.19319, "y":5.42647, "heading":2.28414, "vx":-2.1452, "vy":0.2539, "omega":1.17427, "ax":-0.13041, "ay":-7.26295, "alpha":-0.71229, "fx":[-1.61186,-9.88852,-2.61834,5.55895], "fy":[-119.30034,-118.91844,-119.31599,-119.20076]},
+    {"t":0.94919, "x":6.13745, "y":5.43062, "heading":2.31463, "vx":-2.14859, "vy":0.06532, "omega":1.15577, "ax":0.19785, "ay":-7.25538, "alpha":-0.88307, "fx":[4.07069,-6.36865,2.53879,12.74614], "fy":[-119.20269,-119.12685,-119.29184,-118.61678]},
+    {"t":0.97516, "x":6.08173, "y":5.42987, "heading":2.34464, "vx":-2.14345, "vy":-0.12306, "omega":1.13285, "ax":0.61564, "ay":-7.22302, "alpha":-1.09861, "fx":[11.52373,-1.84448,8.95822,21.77251], "fy":[-118.66327,-119.24237,-118.95074,-117.25796]},
+    {"t":1.00112, "x":6.02628, "y":5.42424, "heading":2.37405, "vx":-2.12747, "vy":-0.3106, "omega":1.10432, "ax":1.15851, "ay":-7.14245, "alpha":-1.37622, "fx":[21.6237,4.1414,17.05837,33.22061], "fy":[-117.17564,-119.13235,-118.02306,-114.49444]},
+    {"t":1.02708, "x":5.97143, "y":5.41377, "heading":2.40273, "vx":-2.09739, "vy":-0.49605, "omega":1.06859, "ax":1.87663, "ay":-6.96644, "alpha":-1.74176, "fx":[35.74034,12.33712,27.39308,47.71024], "fy":[-113.57226,-118.4939,-116.01263,-109.194]},
+    {"t":1.05305, "x":5.91761, "y":5.39854, "heading":2.43047, "vx":-2.04866, "vy":-0.67693, "omega":1.02336, "ax":2.83081, "ay":-6.59925, "alpha":-2.2354, "fx":[55.69384,23.99679,40.6164,65.50572], "fy":[-105.09698,-116.59438,-112.00978,-99.46904]},
+    {"t":1.07901, "x":5.86537, "y":5.37874, "heading":2.45704, "vx":-1.97516, "vy":-0.84827, "omega":0.96532, "ax":4.05235, "ay":-5.86044, "alpha":-2.91191, "fx":[82.09425,41.15626,57.23832,85.50492], "fy":[-85.87181,-111.55362,-104.42295,-82.82681]},
+    {"t":1.10498, "x":5.81545, "y":5.35474, "heading":2.48211, "vx":-1.86994, "vy":-1.00044, "omega":0.88972, "ax":5.41175, "ay":-4.51334, "alpha":-3.71021, "fx":[108.1113,66.25281,76.85934,104.00015], "fy":[-49.01206,-98.5181,-90.86878,-57.85353]},
+    {"t":1.13094, "x":5.76873, "y":5.32724, "heading":2.50521, "vx":-1.72943, "vy":-1.11762, "omega":0.79338, "ax":6.5298, "ay":-2.52504, "alpha":-4.07162, "fx":[118.73008,97.22262,96.8122,115.84679], "fy":[-1.50307,-67.82719,-69.13655,-27.27511]},
+    {"t":1.15691, "x":5.72602, "y":5.29737, "heading":2.52581, "vx":-1.55989, "vy":-1.18319, "omega":0.68767, "ax":7.02694, "ay":-0.26962, "alpha":-3.72695, "fx":[112.95237,117.26752,111.9932,119.03055], "fy":[37.04644,-17.18159,-40.09097,2.52865]},
+    {"t":1.18287, "x":5.68789, "y":5.26656, "heading":2.54366, "vx":-1.37744, "vy":-1.19019, "omega":0.5909, "ax":6.88052, "ay":1.65461, "alpha":-3.33785, "fx":[102.09083,114.85835,118.6269,116.05706], "fy":[61.15731,29.87295,-9.25083,26.82828]},
+    {"t":1.20884, "x":5.65445, "y":5.23621, "heading":2.559, "vx":-1.19879, "vy":-1.14722, "omega":0.50423, "ax":6.45147, "ay":2.99859, "alpha":-3.02607, "fx":[92.12077,103.15805,117.76599,110.42518], "fy":[75.50892,59.05794,17.43018,44.82831]},
+    {"t":1.2348, "x":5.62549, "y":5.20744, "heading":2.5721, "vx":-1.03128, "vy":-1.06937, "omega":0.42566, "ax":5.99444, "ay":3.89075, "alpha":-2.7283, "fx":[84.1736,92.02012,112.96913,104.30804], "fy":[84.38725,75.47308,37.77084,57.75492]},
+    {"t":1.26076, "x":5.60074, "y":5.18098, "heading":2.58315, "vx":-0.87564, "vy":-0.96835, "omega":0.35482, "ax":5.59027, "ay":4.49271, "alpha":-2.46053, "fx":[78.00157,83.32683,106.98874,98.62473], "fy":[90.19976,85.11866,52.50305,67.07722]},
+    {"t":1.28673, "x":5.57989, "y":5.15735, "heading":2.59236, "vx":-0.73049, "vy":-0.8517, "omega":0.29094, "ax":5.25158, "ay":4.91258, "alpha":-2.23487, "fx":[73.17752,76.73654,101.15281,93.64352], "fy":[94.21166,91.20232,63.11472,73.92965]},
+    {"t":1.31269, "x":5.56269, "y":5.1369, "heading":2.59992, "vx":-0.59414, "vy":-0.72414, "omega":0.23291, "ax":4.9715, "ay":5.21589, "alpha":-2.04954, "fx":[69.34879,71.68199,95.92522,89.37006], "fy":[97.10697,95.29581,70.87826,79.0865]},
+    {"t":1.33866, "x":5.54894, "y":5.11985, "heading":2.60596, "vx":-0.46505, "vy":-0.58872, "omega":0.1797, "ax":4.73947, "ay":5.44223, "alpha":-1.89785, "fx":[66.25854,67.72262,91.39027,85.72402], "fy":[99.27435,98.20073,76.69026,83.05928]},
+    {"t":1.36462, "x":5.53846, "y":5.1064, "heading":2.61063, "vx":-0.342, "vy":-0.44741, "omega":0.13042, "ax":4.54576, "ay":5.61604, "alpha":-1.77287, "fx":[63.72455,64.55279,87.4949,82.60838], "fy":[100.94615,100.35262,81.14786,86.18657]},
+    {"t":1.39059, "x":5.53111, "y":5.09668, "heading":2.61401, "vx":-0.22397, "vy":-0.30159, "omega":0.08439, "ax":4.38248, "ay":5.75284, "alpha":-1.66884, "fx":[61.61707,61.96367,84.14888,79.93336], "fy":[102.26786,102.00309,84.64582,88.69607]},
+    {"t":1.41655, "x":5.52678, "y":5.09079, "heading":2.61621, "vx":-0.11018, "vy":-0.15223, "omega":0.04106, "ax":4.24346, "ay":5.86283, "alpha":-1.58128, "fx":[59.84248,59.81091,81.26172,77.62287], "fy":[103.33434,103.3056,87.44829,90.7441]},
+    {"t":1.44252, "x":5.52535, "y":5.08881, "heading":2.61727, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}