Merge branch 'choreolib' into main

blair-robot-project · Jan 18, 2025 · f67ff7d · f67ff7d
2 parents a33f895 + 812340d
commit f67ff7d
Show file tree

Hide file tree

Showing 42 changed files with 1,167 additions and 4,830 deletions.
diff --git a/gradlew b/gradlew
diff --git a/src/main/deploy/choreo/E_rightStation.traj b/src/main/deploy/choreo/E_rightStation.traj
@@ -0,0 +1,87 @@
+{
+ "name":"E_rightStation",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.000988006591797, "y":2.773613691329956, "heading":2.0852470875746736, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.3086029589176178, "y":0.4841791093349457, "heading":0.8139619227060371, "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":"5.000988006591797 m", "val":5.000988006591797}, "y":{"exp":"2.773613691329956 m", "val":2.773613691329956}, "heading":{"exp":"2.0852470875746736 rad", "val":2.0852470875746736}, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"0.3086029589176178 m", "val":0.3086029589176178}, "y":{"exp":"0.4841791093349457 m", "val":0.4841791093349457}, "heading":{"exp":"0.8139619227060371 rad", "val":0.8139619227060371}, "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,1.33399],
+  "samples":[
+    {"t":0.0, "x":5.00099, "y":2.77361, "heading":2.08525, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.56352, "ay":-5.15399, "alpha":-2.87458, "fx":[-138.96052,-138.80031,-136.58525,-136.68004], "fy":[-64.728,-65.08992,-69.61716,-69.41289]},
+    {"t":0.02565, "x":4.99751, "y":2.77192, "heading":2.08525, "vx":-0.27099, "vy":-0.13222, "omega":-0.07374, "ax":-10.56284, "ay":-5.15365, "alpha":-2.87289, "fx":[-138.95069,-138.7909,-136.57725,-136.67169], "fy":[-64.72524,-65.08702,-69.61135,-69.40702]},
+    {"t":0.05131, "x":4.98708, "y":2.76683, "heading":2.08336, "vx":-0.54197, "vy":-0.26443, "omega":-0.14744, "ax":-10.5621, "ay":-5.15329, "alpha":-2.87117, "fx":[-138.93815,-138.78289,-136.5706,-136.66028], "fy":[-64.72624,-65.07915,-69.60106,-69.40532]},
+    {"t":0.07696, "x":4.96971, "y":2.75835, "heading":2.07957, "vx":-0.81292, "vy":-0.39663, "omega":-0.2211, "ax":-10.56129, "ay":-5.1529, "alpha":-2.86942, "fx":[-138.92278,-138.77616,-136.56522,-136.6457], "fy":[-64.73099,-65.06632,-69.58624,-69.40771]},
+    {"t":0.10261, "x":4.94538, "y":2.74648, "heading":2.0739, "vx":-1.08386, "vy":-0.52882, "omega":-0.29471, "ax":-10.56041, "ay":-5.15247, "alpha":-2.86762, "fx":[-138.90444,-138.77057,-136.56101,-136.62789], "fy":[-64.73947,-65.04853,-69.56678,-69.41405]},
+    {"t":0.12827, "x":4.9141, "y":2.73122, "heading":2.06634, "vx":-1.35477, "vy":-0.661, "omega":-0.36828, "ax":-10.55944, "ay":-5.152, "alpha":-2.86578, "fx":[-138.88294,-138.76593,-136.55785,-136.60673], "fy":[-64.75168,-65.02579,-69.54255,-69.4242]},
+    {"t":0.15392, "x":4.87587, "y":2.71257, "heading":2.05689, "vx":-1.62566, "vy":-0.79317, "omega":-0.44179, "ax":-10.55838, "ay":-5.15148, "alpha":-2.86388, "fx":[-138.85806,-138.76201,-136.55564,-136.58212], "fy":[-64.76765,-64.99813,-69.51337,-69.43793]},
+    {"t":0.17958, "x":4.83069, "y":2.69052, "heading":2.04556, "vx":-1.89652, "vy":-0.92532, "omega":-0.51526, "ax":-10.55719, "ay":-5.1509, "alpha":-2.86193, "fx":[-138.82952,-138.75854,-136.55421,-136.55391], "fy":[-64.78739,-64.96557,-69.47904,-69.45499]},
+    {"t":0.20523, "x":4.77856, "y":2.66509, "heading":2.03234, "vx":-2.16735, "vy":-1.05746, "omega":-0.58868, "ax":-10.55588, "ay":-5.15026, "alpha":-2.85992, "fx":[-138.79698,-138.75519,-136.55338,-136.52194], "fy":[-64.81093,-64.92816,-69.4393,-69.47509]},
+    {"t":0.23088, "x":4.71949, "y":2.63627, "heading":2.01724, "vx":-2.43814, "vy":-1.18958, "omega":-0.66205, "ax":-10.5544, "ay":-5.14954, "alpha":-2.85784, "fx":[-138.76003,-138.75155,-136.5529,-136.486], "fy":[-64.83828,-64.88592,-69.39386,-69.49784]},
+    {"t":0.25654, "x":4.65347, "y":2.60406, "heading":2.00026, "vx":-2.7089, "vy":-1.32169, "omega":-0.73536, "ax":-10.55273, "ay":-5.14872, "alpha":-2.85568, "fx":[-138.71816,-138.74711,-136.55247,-136.44579], "fy":[-64.86942,-64.83889,-69.34235,-69.52281]},
+    {"t":0.28219, "x":4.5805, "y":2.56846, "heading":1.98139, "vx":-2.97962, "vy":-1.45377, "omega":-0.80862, "ax":-10.55084, "ay":-5.1478, "alpha":-2.85344, "fx":[-138.67075,-138.74125,-136.55166,-136.40094], "fy":[-64.90432,-64.78707,-69.28434,-69.54947]},
+    {"t":0.30784, "x":4.50059, "y":2.52947, "heading":1.96065, "vx":-3.25029, "vy":-1.58583, "omega":-0.88182, "ax":-10.54866, "ay":-5.14674, "alpha":-2.8511, "fx":[-138.61699,-138.73315,-136.54993,-136.35094], "fy":[-64.94286,-64.73042,-69.21933,-69.57717]},
+    {"t":0.3335, "x":4.41374, "y":2.48709, "heading":1.93802, "vx":-3.5209, "vy":-1.71786, "omega":-0.95496, "ax":-10.54613, "ay":-5.1455, "alpha":-2.84866, "fx":[-138.55588,-138.7218,-136.5465,-136.29508], "fy":[-64.98485,-64.66884,-69.14668,-69.60512]},
+    {"t":0.35915, "x":4.31995, "y":2.44133, "heading":1.91353, "vx":-3.79144, "vy":-1.84986, "omega":-1.02804, "ax":-10.54317, "ay":-5.14406, "alpha":-2.84611, "fx":[-138.48608,-138.70583,-136.54032,-136.23239], "fy":[-65.02995,-64.60213,-69.06563,-69.63234]},
+    {"t":0.3848, "x":4.21921, "y":2.39218, "heading":1.88715, "vx":-4.06191, "vy":-1.98182, "omega":-1.10105, "ax":-10.53964, "ay":-5.14233, "alpha":-2.84344, "fx":[-138.40579,-138.68344,-136.5299,-136.16144], "fy":[-65.07758,-64.52991,-68.97517,-69.65757]},
+    {"t":0.41046, "x":4.11154, "y":2.33965, "heading":1.85891, "vx":-4.33229, "vy":-2.11374, "omega":-1.174, "ax":-10.53537, "ay":-5.14025, "alpha":-2.84063, "fx":[-138.3125,-138.65209,-136.51306,-136.08016], "fy":[-65.12686,-64.45155,-68.87397,-69.67917]},
+    {"t":0.43611, "x":3.99694, "y":2.28373, "heading":1.82879, "vx":-4.60256, "vy":-2.24561, "omega":-1.24687, "ax":-10.5301, "ay":-5.13768, "alpha":-2.83766, "fx":[-138.2026,-138.60814,-136.48658,-135.98543], "fy":[-65.17633,-64.36592,-68.76019,-69.69489]},
+    {"t":0.46176, "x":3.8754, "y":2.22443, "heading":1.7968, "vx":-4.8727, "vy":-2.37741, "omega":-1.31967, "ax":-10.52342, "ay":-5.13442, "alpha":-2.83453, "fx":[-138.07062,-138.54611,-136.44545,-135.87233], "fy":[-65.22366,-64.27113,-68.63111,-69.70152]},
+    {"t":0.48742, "x":3.74693, "y":2.16176, "heading":1.76295, "vx":-5.14266, "vy":-2.50913, "omega":-1.39238, "ax":-10.5147, "ay":-5.13016, "alpha":-2.83121, "fx":[-137.90779,-138.45728,-136.38154,-135.73278], "fy":[-65.26491,-64.16384,-68.48249,-69.69414]},
+    {"t":0.51307, "x":3.61155, "y":2.0957, "heading":1.72723, "vx":-5.4124, "vy":-2.64073, "omega":-1.46501, "ax":-10.50281, "ay":-5.12436, "alpha":-2.82768, "fx":[-137.69913,-138.32678,-136.28074,-135.55269], "fy":[-65.29309,-64.03794,-68.30714,-69.66466]},
+    {"t":0.53873, "x":3.46924, "y":2.02627, "heading":1.68965, "vx":-5.68184, "vy":-2.77219, "omega":-1.53755, "ax":-10.48566, "ay":-5.116, "alpha":-2.82389, "fx":[-137.41648,-138.12682,-136.11639,-135.3052], "fy":[-65.2949,-63.88144,-68.09173,-69.59837]},
+    {"t":0.56438, "x":3.32003, "y":1.95347, "heading":1.6502, "vx":-5.95083, "vy":-2.90344, "omega":-1.61, "ax":-10.45877, "ay":-5.10288, "alpha":-2.81977, "fx":[-136.99981,-137.79831,-135.8315,-134.93257], "fy":[-65.24164,-63.66803,-67.80789,-69.46451]},
+    {"t":0.59003, "x":3.16393, "y":1.87731, "heading":1.6089, "vx":-6.21914, "vy":-3.03434, "omega":-1.68233, "ax":-10.41054, "ay":-5.07935, "alpha":-2.81514, "fx":[-136.29399,-137.18895,-135.27852,-134.28489], "fy":[-65.05891,-63.32856,-67.38248,-69.18463]},
+    {"t":0.61569, "x":3.00096, "y":1.79779, "heading":1.56574, "vx":-6.4862, "vy":-3.16465, "omega":-1.75455, "ax":-10.29888, "ay":-5.02487, "alpha":-2.80924, "fx":[-134.73625,-135.74667,-133.92164,-132.81704], "fy":[-64.47647,-62.61018,-66.55014,-68.47592]},
+    {"t":0.64134, "x":2.83118, "y":1.71495, "heading":1.52073, "vx":-6.75041, "vy":-3.29355, "omega":-1.82662, "ax":-9.75895, "ay":-4.76144, "alpha":-2.79161, "fx":[-127.45393,-128.68825,-127.11297,-125.80233], "fy":[-61.15422,-59.31449,-63.00897,-64.89382]},
+    {"t":0.66699, "x":2.6548, "y":1.6289, "heading":1.47387, "vx":-7.00076, "vy":-3.4157, "omega":-1.89823, "ax":9.75895, "ay":4.76143, "alpha":2.80263, "fx":[127.38606,128.70083,127.17982,125.7907], "fy":[61.28544,59.25997,62.883,64.94253]},
+    {"t":0.69265, "x":2.47841, "y":1.54284, "heading":1.42518, "vx":-6.75041, "vy":-3.29355, "omega":-1.82634, "ax":10.29888, "ay":5.02486, "alpha":2.81256, "fx":[134.52546,135.79285,134.1287,132.77451], "fy":[64.90669,62.48026,66.13913,68.58642]},
+    {"t":0.7183, "x":2.30863, "y":1.46, "heading":1.37832, "vx":-6.4862, "vy":-3.16465, "omega":-1.75418, "ax":10.41054, "ay":5.07934, "alpha":2.81671, "fx":[135.94583,137.27043,135.62045,134.20958], "fy":[65.77515,63.1221,66.69866,69.35857]},
+    {"t":0.74395, "x":2.14566, "y":1.38049, "heading":1.33332, "vx":-6.21914, "vy":-3.03434, "omega":-1.68193, "ax":10.45877, "ay":5.10288, "alpha":2.82047, "fx":[136.52015,137.91359,136.30253,134.82589], "fy":[66.23154,63.38865,66.8629,69.69891]},
+    {"t":0.76961, "x":1.98956, "y":1.30432, "heading":1.29018, "vx":-5.95083, "vy":-2.90344, "omega":-1.60957, "ax":10.48566, "ay":5.116, "alpha":2.82403, "fx":[136.81138,138.274,136.71056,135.16893], "fy":[66.54575,63.53344,66.8975,69.88973]},
+    {"t":0.79526, "x":1.84035, "y":1.23152, "heading":1.24888, "vx":-5.68184, "vy":-2.77219, "omega":-1.53712, "ax":10.50281, "ay":5.12436, "alpha":2.82743, "fx":[136.97478,138.50388,136.99195,135.38873], "fy":[66.79187,63.62577,66.87589,70.00932]},
+    {"t":0.82092, "x":1.69804, "y":1.16209, "heading":1.20945, "vx":-5.4124, "vy":-2.64073, "omega":-1.46459, "ax":10.5147, "ay":5.13016, "alpha":2.83069, "fx":[137.07048,138.66226,137.20362,135.54304], "fy":[66.99839,63.69202,66.82657,70.0884]},
+    {"t":0.84657, "x":1.56266, "y":1.09604, "heading":1.17188, "vx":-5.14266, "vy":-2.50913, "omega":-1.39197, "ax":10.52342, "ay":5.13442, "alpha":2.83381, "fx":[137.12669,138.77694,137.37218,135.65871], "fy":[67.17854,63.74416,66.76301,70.14174]},
+    {"t":0.87222, "x":1.43419, "y":1.03336, "heading":1.13617, "vx":-4.8727, "vy":-2.37741, "omega":-1.31927, "ax":10.5301, "ay":5.13768, "alpha":2.8368, "fx":[137.1584,138.86282,137.51173,135.74982], "fy":[67.33928,63.78824,66.69238,70.17747]},
+    {"t":0.89788, "x":1.31265, "y":0.97406, "heading":1.10233, "vx":-4.60256, "vy":-2.24561, "omega":-1.2465, "ax":10.53537, "ay":5.14025, "alpha":2.83968, "fx":[137.17436,138.92864,137.63043,135.8244], "fy":[67.48462,63.82749,66.6189,70.20057]},
+    {"t":0.92353, "x":1.19805, "y":0.91814, "heading":1.07035, "vx":-4.33229, "vy":-2.11374, "omega":-1.17365, "ax":10.53964, "ay":5.14234, "alpha":2.84244, "fx":[137.17996,138.97995,137.73334,135.88733], "fy":[67.61705,63.86372,66.54518,70.21434]},
+    {"t":0.94918, "x":1.09038, "y":0.86561, "heading":1.04024, "vx":-4.06191, "vy":-1.98183, "omega":-1.10073, "ax":10.54317, "ay":5.14406, "alpha":2.84509, "fx":[137.17875,139.02042,137.82379,135.94168], "fy":[67.73818,63.89789,66.4729,70.22112]},
+    {"t":0.97484, "x":0.98964, "y":0.81646, "heading":1.012, "vx":-3.79144, "vy":-1.84986, "omega":-1.02775, "ax":10.54613, "ay":5.1455, "alpha":2.84764, "fx":[137.17313,139.05266,137.90402,135.98946], "fy":[67.84915,63.93052,66.40323,70.22265]},
+    {"t":1.00049, "x":0.89585, "y":0.7707, "heading":0.98564, "vx":-3.5209, "vy":-1.71786, "omega":-0.95469, "ax":10.54866, "ay":5.14674, "alpha":2.8501, "fx":[137.16478,139.07853,137.97566,136.03205], "fy":[67.95078,63.96182,66.33695,70.22028]},
+    {"t":1.02614, "x":0.809, "y":0.72832, "heading":0.96115, "vx":-3.25029, "vy":-1.58583, "omega":-0.88158, "ax":10.55084, "ay":5.1478, "alpha":2.85247, "fx":[137.15491,139.09947,138.03986,136.07036], "fy":[68.04369,63.99184,66.27461,70.21511]},
+    {"t":1.0518, "x":0.72909, "y":0.68934, "heading":0.93853, "vx":-2.97962, "vy":-1.45377, "omega":-0.8084, "ax":10.55273, "ay":5.14872, "alpha":2.85475, "fx":[137.14441,139.11656,138.09753,136.10504], "fy":[68.12837,64.02053,66.2166,70.20803]},
+    {"t":1.07745, "x":0.65612, "y":0.65374, "heading":0.91779, "vx":-2.7089, "vy":-1.32169, "omega":-0.73517, "ax":10.5544, "ay":5.14954, "alpha":2.85695, "fx":[137.13394,139.13067,138.14934,136.13655], "fy":[68.20521,64.04776,66.16319,70.19978]},
+    {"t":1.1031, "x":0.5901, "y":0.62152, "heading":0.89893, "vx":-2.43814, "vy":-1.18958, "omega":-0.66188, "ax":10.55588, "ay":5.15026, "alpha":2.85907, "fx":[137.12399,139.14249,138.19582,136.16519], "fy":[68.27456,64.07338,66.11458,70.191]},
+    {"t":1.12876, "x":0.53103, "y":0.5927, "heading":0.88195, "vx":-2.16735, "vy":-1.05746, "omega":-0.58853, "ax":10.55719, "ay":5.1509, "alpha":2.86113, "fx":[137.11496,139.1526,138.23742,136.19121], "fy":[68.33669,64.09723,66.0709,70.18223]},
+    {"t":1.15441, "x":0.4789, "y":0.56727, "heading":0.86686, "vx":-1.89652, "vy":-0.92532, "omega":-0.51513, "ax":10.55838, "ay":5.15148, "alpha":2.86312, "fx":[137.10713,139.16146,138.27448,136.21477], "fy":[68.39184,64.11911,66.03224,70.17393]},
+    {"t":1.18007, "x":0.43372, "y":0.54523, "heading":0.85364, "vx":-1.62566, "vy":-0.79317, "omega":-0.44168, "ax":10.55944, "ay":5.152, "alpha":2.86504, "fx":[137.10074,139.16946,138.30729,136.23598], "fy":[68.44022,64.13889,65.99866,70.16649]},
+    {"t":1.20572, "x":0.39549, "y":0.52657, "heading":0.84231, "vx":-1.35477, "vy":-0.661, "omega":-0.36819, "ax":10.56041, "ay":5.15247, "alpha":2.86691, "fx":[137.09597,139.17693,138.33608,136.25494], "fy":[68.48201,64.15639,65.97021,70.16027]},
+    {"t":1.23137, "x":0.36422, "y":0.51131, "heading":0.83286, "vx":-1.08386, "vy":-0.52882, "omega":-0.29464, "ax":10.56129, "ay":5.1529, "alpha":2.86873, "fx":[137.09296,139.18414,138.36105,136.27173], "fy":[68.51737,64.17149,65.94692,70.15553]},
+    {"t":1.25703, "x":0.33989, "y":0.49944, "heading":0.82531, "vx":-0.81292, "vy":-0.39663, "omega":-0.22105, "ax":10.5621, "ay":5.15329, "alpha":2.87049, "fx":[137.09186,139.19131,138.38238,136.28638], "fy":[68.54642,64.18407,65.92881,70.15252]},
+    {"t":1.28268, "x":0.32251, "y":0.49096, "heading":0.81963, "vx":-0.54197, "vy":-0.26443, "omega":-0.14741, "ax":10.56284, "ay":5.15365, "alpha":2.87221, "fx":[137.09274,139.19864,138.40019,136.29897], "fy":[68.56927,64.19405,65.9159,70.15144]},
+    {"t":1.30833, "x":0.31208, "y":0.48588, "heading":0.81585, "vx":-0.27099, "vy":-0.13222, "omega":-0.07373, "ax":10.56352, "ay":5.15399, "alpha":2.87389, "fx":[137.09572,139.20628,138.4146,136.30953], "fy":[68.58601,64.20136,65.90822,70.15243]},
+    {"t":1.33399, "x":0.3086, "y":0.48418, "heading":0.81396, "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":[]
+}