diff --git a/Choreo.chor b/Choreo.chor new file mode 100644 index 00000000..73260322 --- /dev/null +++ b/Choreo.chor @@ -0,0 +1,1489 @@ +{ + "version": "v0.2", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.0236966824644549, + "motorMaxVelocity": 5427.200000000001, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "SW1S": { + "waypoints": [ + { + "x": 1.5, + "y": 7, + "heading": -2.18, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.34, + "y": 7, + "heading": -3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 2.9, + "y": 7, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 26 + }, + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.5, + "y": 7, + "heading": -2.18, + "angularVelocity": -7.014080895465295e-20, + "velocityX": -6.1070488035186446e-18, + "velocityY": 1.704040905172389e-18, + "timestamp": 0 + }, + { + "x": 1.5109255821948424, + "y": 6.999825295018549, + "heading": -2.1957306466432525, + "angularVelocity": -0.3925464436026101, + "velocityX": 0.2726396779571315, + "velocityY": -0.004359631279235028, + "timestamp": 0.040073338835740574 + }, + { + "x": 1.5328085169368726, + "y": 6.999520158674122, + "heading": -2.226854341802715, + "angularVelocity": -0.7766683801176959, + "velocityX": 0.5460721611375935, + "velocityY": -0.007614447742360675, + "timestamp": 0.08014667767148115 + }, + { + "x": 1.5656875968313502, + "y": 6.999154789467847, + "heading": -2.2729635858312136, + "angularVelocity": -1.1506214697382335, + "velocityX": 0.8204726845759085, + "velocityY": -0.009117513461358586, + "timestamp": 0.12022001650722172 + }, + { + "x": 1.6095978948873755, + "y": 6.998820636587558, + "heading": -2.333703386104559, + "angularVelocity": -1.515715985691008, + "velocityX": 1.0957484285503496, + "velocityY": -0.008338533548678785, + "timestamp": 0.1602933553429623 + }, + { + "x": 1.664560942869615, + "y": 6.998614187753099, + "heading": -2.408894144312653, + "angularVelocity": -1.8763287610322779, + "velocityX": 1.3715614815010924, + "velocityY": -0.005151775231561494, + "timestamp": 0.20036669417870287 + }, + { + "x": 1.7305843869075594, + "y": 6.998608834089527, + "heading": -2.4985652376560035, + "angularVelocity": -2.2376746222944734, + "velocityX": 1.6475653378566024, + "velocityY": -0.00013359664377947739, + "timestamp": 0.24044003301444344 + }, + { + "x": 1.8076797623531138, + "y": 6.998823522335968, + "heading": -2.602788739978155, + "angularVelocity": -2.6008190320587983, + "velocityX": 1.9238570502341217, + "velocityY": 0.005357383554156006, + "timestamp": 0.280513371850184 + }, + { + "x": 1.8959026263569365, + "y": 6.999202422024261, + "heading": -2.7212110856967926, + "angularVelocity": -2.9551404789105975, + "velocityX": 2.2015351494781537, + "velocityY": 0.009455156453139688, + "timestamp": 0.3205867106859246 + }, + { + "x": 1.9954182043717335, + "y": 6.999616761540192, + "heading": -2.852071562900106, + "angularVelocity": -3.2655246856175077, + "velocityX": 2.4833363254985996, + "velocityY": 0.010339530669704458, + "timestamp": 0.3606600495216652 + }, + { + "x": 2.10657810793121, + "y": 6.999879512490144, + "heading": -2.9896460539129763, + "angularVelocity": -3.4330678453477326, + "velocityX": 2.7739117026192326, + "velocityY": 0.006556752134792622, + "timestamp": 0.4007333883574058 + }, + { + "x": 2.2257531407250775, + "y": 7, + "heading": -3.090048550833243, + "angularVelocity": -2.505468719038884, + "velocityX": 2.9739232181838897, + "velocityY": 0.00300667509514831, + "timestamp": 0.4408067271931464 + }, + { + "x": 2.34, + "y": 7, + "heading": -3.14, + "angularVelocity": -1.2465008062235836, + "velocityX": 2.8509443583715566, + "velocityY": 2.66121515722134e-18, + "timestamp": 0.480880066028887 + }, + { + "x": 2.3902685794468983, + "y": 7, + "heading": -3.154884405334106, + "angularVelocity": -0.8176183004169806, + "velocityX": 2.7613135741148205, + "velocityY": 1.0875590759442405e-18, + "timestamp": 0.49908465529325463 + }, + { + "x": 2.4384926520059382, + "y": 7, + "heading": -3.1644927701307046, + "angularVelocity": -0.52779904325617, + "velocityX": 2.6490063499218826, + "velocityY": -2.8339734979486245e-20, + "timestamp": 0.5172892445576223 + }, + { + "x": 2.4844766569844676, + "y": 7, + "heading": -3.1704799601608893, + "angularVelocity": -0.3288835547586147, + "velocityX": 2.525956741497459, + "velocityY": -4.102095677617762e-20, + "timestamp": 0.5354938338219899 + }, + { + "x": 2.5281300977466628, + "y": 7, + "heading": -3.173869974980369, + "angularVelocity": -0.18621759437953955, + "velocityX": 2.3979360439421336, + "velocityY": -2.8092935749629884e-20, + "timestamp": 0.5536984230863575 + }, + { + "x": 2.5694076425029193, + "y": 7, + "heading": -3.1753354183161275, + "angularVelocity": -0.08049856629415185, + "velocityX": 2.2674252166208264, + "velocityY": -2.386752349644547e-20, + "timestamp": 0.5719030123507252 + }, + { + "x": 2.608284712434112, + "y": 7, + "heading": -3.1753501196100573, + "angularVelocity": -0.0008075597704278253, + "velocityX": 2.1355642451809587, + "velocityY": -2.3063509187477393e-20, + "timestamp": 0.5901076016150928 + }, + { + "x": 2.644747196375146, + "y": 7, + "heading": -3.1742675406474747, + "angularVelocity": 0.059467365446420554, + "velocityX": 2.0029281304577187, + "velocityY": -2.3158354576494492e-20, + "timestamp": 0.6083121908794604 + }, + { + "x": 2.6787867109477963, + "y": 7, + "heading": -3.1723631059987043, + "angularVelocity": 0.10461288750417592, + "velocityX": 1.8698315066780489, + "velocityY": -2.337204487809121e-20, + "timestamp": 0.6265167801438281 + }, + { + "x": 2.7103982151264825, + "y": 7, + "heading": -3.1698586440576513, + "angularVelocity": 0.13757310888402063, + "velocityX": 1.7364579732956862, + "velocityY": -2.352312118224177e-20, + "timestamp": 0.6447213694081957 + }, + { + "x": 2.7395787166189542, + "y": 7, + "heading": -3.166937416369599, + "angularVelocity": 0.1604665530011168, + "velocityX": 1.6029200696983334, + "velocityY": -2.3601907781176607e-20, + "timestamp": 0.6629259586725633 + }, + { + "x": 2.766326526340967, + "y": 7, + "heading": -3.1637538745889637, + "angularVelocity": 0.1748757818375146, + "velocityX": 1.4692893826704234, + "velocityY": -2.3634202608032735e-20, + "timestamp": 0.681130547936931 + }, + { + "x": 2.790640807171386, + "y": 7, + "heading": -3.160440289451799, + "angularVelocity": 0.18201921993659123, + "velocityX": 1.3356127115709844, + "velocityY": -2.3643590270339423e-20, + "timestamp": 0.6993351372012986 + }, + { + "x": 2.812521289813831, + "y": 7, + "heading": -3.1571114286194844, + "angularVelocity": 0.18285833225752388, + "velocityX": 1.2019212476971914, + "velocityY": -2.3644570679392813e-20, + "timestamp": 0.7175397264656662 + }, + { + "x": 2.831968088068395, + "y": 7, + "heading": -3.1538679639272003, + "angularVelocity": 0.17816741949969286, + "velocityX": 1.068236034999556, + "velocityY": -2.3644058515684063e-20, + "timestamp": 0.7357443157300338 + }, + { + "x": 2.8489815755787715, + "y": 7, + "heading": -3.150799020167611, + "angularVelocity": 0.16858077460703824, + "velocityX": 0.9345713469996556, + "velocityY": -2.3644262680482202e-20, + "timestamp": 0.7539489049944015 + }, + { + "x": 2.8635623018508554, + "y": 7, + "heading": -3.147984125285486, + "angularVelocity": 0.15462556398536811, + "velocityX": 0.8009368440200342, + "velocityY": -2.3645061677621432e-20, + "timestamp": 0.7721534942587691 + }, + { + "x": 2.8757109340551876, + "y": 7, + "heading": -3.1454947317261395, + "angularVelocity": 0.13674538453989185, + "velocityX": 0.6673389895212279, + "velocityY": -2.3645586230834433e-20, + "timestamp": 0.7903580835231367 + }, + { + "x": 2.8854282161557525, + "y": 7, + "heading": -3.1433954232633634, + "angularVelocity": 0.11531754066459125, + "velocityX": 0.533782001859519, + "velocityY": -2.3645144712534832e-20, + "timestamp": 0.8085626727875044 + }, + { + "x": 2.892714939914606, + "y": 7, + "heading": -3.1417448863967854, + "angularVelocity": 0.09066597672756352, + "velocityX": 0.40026850664056873, + "velocityY": -2.3643698574082317e-20, + "timestamp": 0.826767262051872 + }, + { + "x": 2.8975719241751396, + "y": 7, + "heading": -3.140596702327834, + "angularVelocity": 0.06307113290374677, + "velocityX": 0.26679999147468597, + "velocityY": -2.3642052138431793e-20, + "timestamp": 0.8449718513162396 + }, + { + "x": 2.9, + "y": 7, + "heading": -3.14, + "angularVelocity": 0.03277757708062409, + "velocityX": 0.13337712758027845, + "velocityY": -2.3641921174305024e-20, + "timestamp": 0.8631764405806073 + }, + { + "x": 2.9, + "y": 7, + "heading": -3.14, + "angularVelocity": 1.1349626131054217e-19, + "velocityX": -5.87186181590248e-18, + "velocityY": 3.5234181128249e-19, + "timestamp": 0.8813810298449749 + }, + { + "x": 2.896686389644809, + "y": 6.991124257977168, + "heading": -3.14, + "angularVelocity": 7.138015320386443e-20, + "velocityX": -0.09225587401594994, + "velocityY": -0.2471139482570088, + "timestamp": 0.9172986382415722 + }, + { + "x": 2.890059169018056, + "y": 6.973372774155508, + "heading": -3.14, + "angularVelocity": 7.138015366262094e-20, + "velocityX": -0.18451174570356052, + "velocityY": -0.49422789027739433, + "timestamp": 0.9532162466381695 + }, + { + "x": 2.8801183382192987, + "y": 6.946745548801693, + "heading": -3.14, + "angularVelocity": 7.138015292840098e-20, + "velocityX": -0.2767676146193387, + "velocityY": -0.7413418248732287, + "timestamp": 0.9891338550347668 + }, + { + "x": 2.8668638973690532, + "y": 6.911242582238536, + "heading": -3.14, + "angularVelocity": 7.138015343140458e-20, + "velocityX": -0.369023480179741, + "velocityY": -0.9884557504814493, + "timestamp": 1.025051463431364 + }, + { + "x": 2.8502958466161945, + "y": 6.866863874864807, + "heading": -3.14, + "angularVelocity": 7.138015303703884e-20, + "velocityX": -0.4612793415952677, + "velocityY": -1.2355696649873245, + "timestamp": 1.0609690718279614 + }, + { + "x": 2.8304141861492957, + "y": 6.813609427185614, + "heading": -3.14, + "angularVelocity": 7.138015321902843e-20, + "velocityX": -0.5535351977606193, + "velocityY": -1.4826835654302304, + "timestamp": 1.0968866802245587 + }, + { + "x": 2.807218916214953, + "y": 6.751479239861482, + "heading": -3.14, + "angularVelocity": 7.138015311568457e-20, + "velocityX": -0.6457910470603585, + "velocityY": -1.729797447483103, + "timestamp": 1.132804288621156 + }, + { + "x": 2.7807100371494347, + "y": 6.6804733137931285, + "heading": -3.14, + "angularVelocity": 7.138015326356826e-20, + "velocityX": -0.7380468869979013, + "velocityY": -1.9769113044586646, + "timestamp": 1.1687218970177533 + }, + { + "x": 2.75088754943846, + "y": 6.60059165028159, + "heading": -3.14, + "angularVelocity": 7.138015312468868e-20, + "velocityX": -0.8303027134122785, + "velocityY": -2.2240251252114605, + "timestamp": 1.2046395054143506 + }, + { + "x": 2.7177514538453025, + "y": 6.511834251371347, + "heading": -3.14, + "angularVelocity": 7.138015316229824e-20, + "velocityX": -0.9225585185759843, + "velocityY": -2.471138889042815, + "timestamp": 1.240557113810948 + }, + { + "x": 2.681301751743851, + "y": 6.414201120742459, + "heading": -3.14, + "angularVelocity": 7.138015315767601e-20, + "velocityX": -1.0148142854885556, + "velocityY": -2.7182525504157744, + "timestamp": 1.2764747222075452 + }, + { + "x": 2.641538446339825, + "y": 6.307692266981674, + "heading": -3.14, + "angularVelocity": 7.138015319545813e-20, + "velocityX": -1.1070699631490775, + "velocityY": -2.9653659727207433, + "timestamp": 1.3123923306041425 + }, + { + "x": 2.5984615536601745, + "y": 6.192307733018326, + "heading": -3.14, + "angularVelocity": 7.138015317958231e-20, + "velocityX": -1.1993251945949435, + "velocityY": -3.212478199807885, + "timestamp": 1.3483099390007398 + }, + { + "x": 2.558698248256148, + "y": 6.08579887925754, + "heading": -3.14, + "angularVelocity": 7.13801531830952e-20, + "velocityX": -1.1070699631490775, + "velocityY": -2.9653659727207433, + "timestamp": 1.384227547397337 + }, + { + "x": 2.5222485461546964, + "y": 5.988165748628653, + "heading": -3.14, + "angularVelocity": 7.138015317499089e-20, + "velocityX": -1.0148142854885558, + "velocityY": -2.7182525504157744, + "timestamp": 1.4201451557939344 + }, + { + "x": 2.489112450561539, + "y": 5.899408349718409, + "heading": -3.14, + "angularVelocity": 7.138015312927702e-20, + "velocityX": -0.9225585185759843, + "velocityY": -2.471138889042815, + "timestamp": 1.4560627641905317 + }, + { + "x": 2.4592899628505647, + "y": 5.819526686206871, + "heading": -3.14, + "angularVelocity": 7.138015322817121e-20, + "velocityX": -0.8303027134122783, + "velocityY": -2.2240251252114605, + "timestamp": 1.491980372587129 + }, + { + "x": 2.4327810837850468, + "y": 5.748520760138518, + "heading": -3.14, + "angularVelocity": 7.138015316283442e-20, + "velocityX": -0.7380468869979014, + "velocityY": -1.9769113044586648, + "timestamp": 1.5278979809837263 + }, + { + "x": 2.409585813850704, + "y": 5.686390572814386, + "heading": -3.14, + "angularVelocity": 7.138015332876638e-20, + "velocityX": -0.6457910470603584, + "velocityY": -1.729797447483103, + "timestamp": 1.5638155893803236 + }, + { + "x": 2.3897041533838053, + "y": 5.633136125135193, + "heading": -3.14, + "angularVelocity": 7.138015307743406e-20, + "velocityX": -0.5535351977606193, + "velocityY": -1.4826835654302304, + "timestamp": 1.599733197776921 + }, + { + "x": 2.373136102630946, + "y": 5.588757417761465, + "heading": -3.14, + "angularVelocity": 7.13801531645015e-20, + "velocityX": -0.46127934159526773, + "velocityY": -1.2355696649873245, + "timestamp": 1.6356508061735182 + }, + { + "x": 2.359881661780701, + "y": 5.553254451198307, + "heading": -3.14, + "angularVelocity": 7.138015317270443e-20, + "velocityX": -0.3690234801797411, + "velocityY": -0.9884557504814494, + "timestamp": 1.6715684145701155 + }, + { + "x": 2.3499408309819434, + "y": 5.526627225844492, + "heading": -3.14, + "angularVelocity": 7.138015308841649e-20, + "velocityX": -0.2767676146193387, + "velocityY": -0.7413418248732287, + "timestamp": 1.7074860229667128 + }, + { + "x": 2.3433136103551906, + "y": 5.508875742022832, + "heading": -3.14, + "angularVelocity": 7.138015351011194e-20, + "velocityX": -0.18451174570356052, + "velocityY": -0.49422789027739433, + "timestamp": 1.7434036313633101 + }, + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 7.138015274700303e-20, + "velocityX": -0.09225587401594994, + "velocityY": -0.2471139482570088, + "timestamp": 1.7793212397599074 + }, + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 1.7845038344079976e-20, + "velocityX": 1.0123024300783969e-18, + "velocityY": 1.3576731951648499e-18, + "timestamp": 1.8152388481565047 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "b7d15dc7-4c16-4ce7-810e-bef2c2444c3e", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "d322e311-572c-4b37-b9a3-fc4fe4e47d46", + "direction": 0 + }, + { + "scope": [ + 2, + 3 + ], + "type": "StraightLine", + "uuid": "e964b72d-29a0-4798-a6f0-c1bbeb2bdeed", + "direction": 0 + }, + { + "scope": [ + 1, + 2 + ], + "type": "StraightLine", + "uuid": "9eccf6a1-301d-4b1c-80de-5563161da94f", + "direction": 0 + }, + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity", + "uuid": "3fd8a618-f4f7-4239-bcde-a02c66fd3caa", + "direction": 0 + }, + { + "scope": [ + 3 + ], + "type": "StopPoint", + "uuid": "80a7dfe5-4af6-4cbe-b5ea-5678d5d023c5", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "W2S": { + "waypoints": [ + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 2.9, + "y": 5.5, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 29 + }, + { + "x": 1.7, + "y": 4, + "heading": -3.7507561014580326, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 2.8438778833914306e-22, + "velocityX": -5.403956856695926e-22, + "velocityY": -1.764496225422152e-21, + "timestamp": 0 + }, + { + "x": 2.345219454149142, + "y": 5.500680620832891, + "heading": -3.14015789834957, + "angularVelocity": -0.005897779171567437, + "velocityX": 0.19495572975659137, + "velocityY": 0.025422377009582938, + "timestamp": 0.026772509613639375 + }, + { + "x": 2.3556590020609622, + "y": 5.502036692864336, + "heading": -3.1404865067656247, + "angularVelocity": -0.01227409834927267, + "velocityX": 0.38993535019600484, + "velocityY": 0.050651659146463954, + "timestamp": 0.05354501922727875 + }, + { + "x": 2.371319433607059, + "y": 5.504061744738199, + "heading": -3.14100186046821, + "angularVelocity": -0.019249361005830575, + "velocityX": 0.5849444737193447, + "velocityY": 0.07563922482755309, + "timestamp": 0.08031752884091813 + }, + { + "x": 2.392201748509867, + "y": 5.50674744096323, + "heading": -3.1417246098705878, + "angularVelocity": -0.026995952669661743, + "velocityX": 0.7799909386219801, + "velocityY": 0.10031544535007665, + "timestamp": 0.1070900384545575 + }, + { + "x": 2.4183072518555147, + "y": 5.510082641059692, + "heading": -3.1426823480926416, + "angularVelocity": -0.03577319556048744, + "velocityX": 0.9750861507711613, + "velocityY": 0.12457554949432209, + "timestamp": 0.13386254806819686 + }, + { + "x": 2.4496377167718046, + "y": 5.514051697670015, + "heading": -3.143913819915893, + "angularVelocity": -0.04599762372011175, + "velocityX": 1.1702475923411, + "velocityY": 0.14825119750076746, + "timestamp": 0.16063505768183622 + }, + { + "x": 2.48619568337043, + "y": 5.518631029455087, + "heading": -3.145477394985239, + "angularVelocity": -0.058402260076100644, + "velocityX": 1.3655039115197647, + "velocityY": 0.17104604129973955, + "timestamp": 0.18740756729547559 + }, + { + "x": 2.527985060150862, + "y": 5.523781042535177, + "heading": -3.1474710387900213, + "angularVelocity": -0.07446607858410415, + "velocityX": 1.5609062199810606, + "velocityY": 0.19236198452856787, + "timestamp": 0.21418007690911495 + }, + { + "x": 2.5750124033881825, + "y": 5.529421473768213, + "heading": -3.1500933109792566, + "angularVelocity": -0.09794644682467382, + "velocityX": 1.7565534167690278, + "velocityY": 0.21067995919826019, + "timestamp": 0.2409525865227543 + }, + { + "x": 2.62728643823873, + "y": 5.535303378399583, + "heading": -3.1539624386682226, + "angularVelocity": -0.14451867773331506, + "velocityX": 1.9525265133873122, + "velocityY": 0.21969941242910496, + "timestamp": 0.26772509613639367 + }, + { + "x": 2.678521434738675, + "y": 5.537289220441695, + "heading": -3.171534912348283, + "angularVelocity": -0.6563625873574372, + "velocityX": 1.9137166160113463, + "velocityY": 0.07417466912029727, + "timestamp": 0.29449760575003303 + }, + { + "x": 2.724629958364521, + "y": 5.538116660747324, + "heading": -3.1901492341368907, + "angularVelocity": -0.6952774341007215, + "velocityX": 1.7222338992963062, + "velocityY": 0.030906340779074025, + "timestamp": 0.3212701153636724 + }, + { + "x": 2.7655630513066685, + "y": 5.537994392383535, + "heading": -3.209278140929457, + "angularVelocity": -0.7144980828700698, + "velocityX": 1.5289225228737546, + "velocityY": -0.004566936964587362, + "timestamp": 0.34804262497731175 + }, + { + "x": 2.8013067248822057, + "y": 5.536995150249463, + "heading": -3.228739854250013, + "angularVelocity": -0.7269289880333657, + "velocityX": 1.3350886447091723, + "velocityY": -0.03732343917295602, + "timestamp": 0.3748151345909511 + }, + { + "x": 2.8318544687583866, + "y": 5.535155887776591, + "heading": -3.248442340989224, + "angularVelocity": -0.7359222958005035, + "velocityX": 1.1410115942443573, + "velocityY": -0.06869966616553647, + "timestamp": 0.4015876442045905 + }, + { + "x": 2.857202537781273, + "y": 5.5324989630903465, + "heading": -3.268330011391853, + "angularVelocity": -0.7428392291060215, + "velocityX": 0.9467946557379203, + "velocityY": -0.09924077811861721, + "timestamp": 0.42836015381822984 + }, + { + "x": 2.8773485037250652, + "y": 5.529039358763453, + "heading": -3.288365650396495, + "angularVelocity": -0.7483661148611586, + "velocityX": 0.7524870187563222, + "velocityY": -0.1292222648089432, + "timestamp": 0.4551326634318692 + }, + { + "x": 2.8922906661715295, + "y": 5.524787814300188, + "heading": -3.308522599399848, + "angularVelocity": -0.7528972552159968, + "velocityX": 0.5581158681833853, + "velocityY": -0.15880261225486758, + "timestamp": 0.48190517304550856 + }, + { + "x": 2.9020277685918865, + "y": 5.519752404878458, + "heading": -3.3287808224995312, + "angularVelocity": -0.7566800196184369, + "velocityX": 0.3636977840656549, + "velocityY": -0.1880813377003599, + "timestamp": 0.5086776826591479 + }, + { + "x": 2.9065588448949846, + "y": 5.513939423347603, + "heading": -3.349124711283007, + "angularVelocity": -0.7598797825479614, + "velocityX": 0.16924361475585362, + "velocityY": -0.21712501423080047, + "timestamp": 0.5354501922727873 + }, + { + "x": 2.9058831293071905, + "y": 5.507353911633989, + "heading": -3.3695417632753553, + "angularVelocity": -0.7626125562000761, + "velocityX": -0.025239157536812964, + "velocityY": -0.24598036600421022, + "timestamp": 0.5622227018864268 + }, + { + "x": 2.9, + "y": 5.5, + "heading": -3.390021738749093, + "angularVelocity": -0.7649628581440181, + "velocityX": -0.21974515621028465, + "velocityY": -0.27468144526285576, + "timestamp": 0.5889952115000662 + }, + { + "x": 2.8873503491496635, + "y": 5.48418793643708, + "heading": -3.4159685355180973, + "angularVelocity": -0.7684647926624162, + "velocityX": -0.37464398416870504, + "velocityY": -0.4683049802108812, + "timestamp": 0.6227596689179652 + }, + { + "x": 2.869470623566405, + "y": 5.461838279458007, + "heading": -3.442033568339415, + "angularVelocity": -0.7719665830465854, + "velocityX": -0.5295428077508573, + "velocityY": -0.6619285096885713, + "timestamp": 0.6565241263358642 + }, + { + "x": 2.846360823424834, + "y": 5.432951029281044, + "heading": -3.4682168314242467, + "angularVelocity": -0.7754682019842498, + "velocityX": -0.6844416261615902, + "velocityY": -0.8555520327019875, + "timestamp": 0.6902885837537632 + }, + { + "x": 2.8180209489344645, + "y": 5.397526186168083, + "heading": -3.4945183177858, + "angularVelocity": -0.7789696139944609, + "velocityX": -0.8393404383671771, + "velocityY": -1.049175547958971, + "timestamp": 0.7240530411716622 + }, + { + "x": 2.7844510003513503, + "y": 5.355563750439189, + "heading": -3.5209380188430024, + "angularVelocity": -0.7824707718595512, + "velocityX": -0.9942392429892433, + "velocityY": -1.2427990537365536, + "timestamp": 0.7578174985895613 + }, + { + "x": 2.7456509779955414, + "y": 5.307063722494428, + "heading": -3.5474759238290736, + "angularVelocity": -0.7859716108455241, + "velocityX": -1.1491380381323866, + "velocityY": -1.4364225476654828, + "timestamp": 0.7915819560074603 + }, + { + "x": 2.701620882278521, + "y": 5.252026102848154, + "heading": -3.5741320188666457, + "angularVelocity": -0.7894720388262817, + "velocityX": -1.3040368210886484, + "velocityY": -1.63004602636081, + "timestamp": 0.8253464134253593 + }, + { + "x": 2.6523607137489367, + "y": 5.190450892186173, + "heading": -3.6009062854336653, + "angularVelocity": -0.792971918240461, + "velocityX": -1.4589355877956627, + "velocityY": -1.823669484744578, + "timestamp": 0.8591108708432583 + }, + { + "x": 2.5978704731749236, + "y": 5.122338091468658, + "heading": -3.6277986976154084, + "angularVelocity": -0.7964710301397292, + "velocityX": -1.6138343317528592, + "velocityY": -2.0172929146910734, + "timestamp": 0.8928753282611573 + }, + { + "x": 2.5381501617087743, + "y": 5.047687702135971, + "heading": -3.6548092166363553, + "angularVelocity": -0.7999689936266586, + "velocityX": -1.7687330415826792, + "velocityY": -2.2109163019783487, + "timestamp": 0.9266397856790564 + }, + { + "x": 2.4731997812712154, + "y": 4.9664997265890225, + "heading": -3.6819377781703686, + "angularVelocity": -0.8034650519700735, + "velocityX": -1.9236316945264456, + "velocityY": -2.4045396181580565, + "timestamp": 0.9604042430969554 + }, + { + "x": 2.403019335704405, + "y": 4.8787741696305105, + "heading": -3.709184254492685, + "angularVelocity": -0.8069573274964867, + "velocityX": -2.0785302336772222, + "velocityY": -2.598162792096527, + "timestamp": 0.9941687005148544 + }, + { + "x": 2.32760883653786, + "y": 4.784511045672329, + "heading": -3.736548266378328, + "angularVelocity": -0.8104383715384815, + "velocityX": -2.233428431358973, + "velocityY": -2.791785539198716, + "timestamp": 1.0279331579327533 + }, + { + "x": 2.249157766758394, + "y": 4.686447208447997, + "heading": -3.738325324519509, + "angularVelocity": -0.05263102910812686, + "velocityX": -2.323480836919306, + "velocityY": -2.9043510461491318, + "timestamp": 1.0616976153506523 + }, + { + "x": 2.1759367495246877, + "y": 4.594920936905864, + "heading": -3.7399833295422438, + "angularVelocity": -0.049105039723097314, + "velocityX": -2.1685826704530755, + "velocityY": -2.710728338066344, + "timestamp": 1.0954620727685513 + }, + { + "x": 2.107945796889999, + "y": 4.509932246112504, + "heading": -3.7415226557241197, + "angularVelocity": -0.045590135295933004, + "velocityX": -2.0136841470061912, + "velocityY": -2.5171051837577383, + "timestamp": 1.1292265301864504 + }, + { + "x": 2.045184912871734, + "y": 4.431481141089672, + "heading": -3.7429434277034694, + "angularVelocity": -0.042078922275138625, + "velocityX": -1.8587855045760215, + "velocityY": -2.3234818807200264, + "timestamp": 1.1629909876043494 + }, + { + "x": 1.9876540994785168, + "y": 4.359567624348151, + "heading": -3.7442457077566362, + "angularVelocity": -0.038569553689213554, + "velocityX": -1.7038868026565464, + "velocityY": -2.129858503320682, + "timestamp": 1.1967554450222484 + }, + { + "x": 1.9353533579154936, + "y": 4.294191697394372, + "heading": -3.74542953323058, + "angularVelocity": -0.0350612912060703, + "velocityX": -1.5489880650443397, + "velocityY": -1.9362350813054243, + "timestamp": 1.2305199024401474 + }, + { + "x": 1.888282688986081, + "y": 4.235353361232607, + "heading": -3.746494929013771, + "angularVelocity": -0.03155376584332283, + "velocityX": -1.3940893036373676, + "velocityY": -1.7426116295467093, + "timestamp": 1.2642843598580464 + }, + { + "x": 1.8464420932641423, + "y": 4.183052616580183, + "heading": -3.747441912878439, + "angularVelocity": -0.02804676683967347, + "velocityX": -1.2391905252343365, + "velocityY": -1.54898815654292, + "timestamp": 1.2980488172759455 + }, + { + "x": 1.8098315711800703, + "y": 4.137289463975094, + "heading": -3.7482704981506734, + "angularVelocity": -0.024540162514063414, + "velocityX": -1.0842917340843754, + "velocityY": -1.355364667605469, + "timestamp": 1.3318132746938445 + }, + { + "x": 1.7784511230686133, + "y": 4.098063903835773, + "heading": -3.748980695193332, + "angularVelocity": -0.021033865104619943, + "velocityX": -0.9293929330202054, + "velocityY": -1.1617411662752564, + "timestamp": 1.3655777321117435 + }, + { + "x": 1.7523007491975677, + "y": 4.065375936496967, + "heading": -3.7495725122955377, + "angularVelocity": -0.0175278131936343, + "velocityX": -0.7744941240247116, + "velocityY": -0.9681176550308892, + "timestamp": 1.3993421895296425 + }, + { + "x": 1.7313804497860397, + "y": 4.039225562232557, + "heading": -3.7500459562385835, + "angularVelocity": -0.014021962123839892, + "velocityX": -0.6195953085399809, + "velocityY": -0.7744941356749758, + "timestamp": 1.4331066469475415 + }, + { + "x": 1.7156902250166162, + "y": 4.019612781270777, + "heading": -3.750401032673127, + "angularVelocity": -0.010516278409242411, + "velocityX": -0.46469648764757093, + "velocityY": -0.5808706095594635, + "timestamp": 1.4668711043654405 + }, + { + "x": 1.7052300750437936, + "y": 4.006537593804749, + "heading": -3.7506377463802734, + "angularVelocity": -0.007010736296354315, + "velocityX": -0.30979766217944427, + "velocityY": -0.3872470777243053, + "timestamp": 1.5006355617833396 + }, + { + "x": 1.6999999999999975, + "y": 4.0000000000000036, + "heading": -3.7507561014580326, + "angularVelocity": -0.0035053155539891035, + "velocityX": -0.15489883278928215, + "velocityY": -0.19362354098660264, + "timestamp": 1.5344000192012386 + }, + { + "x": 1.7, + "y": 4, + "heading": -3.7507561014580326, + "angularVelocity": 1.6035161704104545e-21, + "velocityX": 7.414988771169268e-14, + "velocityY": -1.1459521982628032e-13, + "timestamp": 1.5681644766191376 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "ef635949-df4f-43d7-936d-58edbc41bd02" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "71984ce6-18aa-44ff-9c4d-d8b16c8b6c57" + }, + { + "scope": [ + 1, + 2 + ], + "type": "StraightLine", + "uuid": "351da888-c462-4732-b1af-be0a93c655b8" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "W3S": { + "waypoints": [ + { + "x": 1.7, + "y": 4, + "heading": 2.5324292057215536, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.7, + "y": 4, + "heading": 3.143185307179586, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.7, + "y": 2.8759732246398926, + "heading": 2.299843907718641, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.7, + "y": 4, + "heading": 2.5324292057215536, + "angularVelocity": 9.836427314098696e-18, + "velocityX": 2.3386198211048914e-17, + "velocityY": 4.209145509360342e-19, + "timestamp": 0 + }, + { + "x": 1.726680446907769, + "y": 4, + "heading": 2.5507389446586037, + "angularVelocity": 0.30101879304795914, + "velocityX": 0.438636288254118, + "velocityY": -4.559667670299124e-19, + "timestamp": 0.06082589977675328 + }, + { + "x": 1.7800391101374584, + "y": 4, + "heading": 2.58742303958277, + "angularVelocity": 0.6030999139905887, + "velocityX": 0.8772359048625102, + "velocityY": -4.737105509825422e-19, + "timestamp": 0.12165179955350656 + }, + { + "x": 1.8600710399111293, + "y": 4, + "heading": 2.642613112610323, + "angularVelocity": 0.9073449505798018, + "velocityX": 1.3157541453144361, + "velocityY": -5.466125636924237e-19, + "timestamp": 0.18247769933025984 + }, + { + "x": 1.966768076798484, + "y": 4, + "heading": 2.7165199563471956, + "angularVelocity": 1.2150554945706835, + "velocityX": 1.7541382417532945, + "velocityY": -6.537740639145266e-19, + "timestamp": 0.24330359910701313 + }, + { + "x": 2.100119086392268, + "y": 4, + "heading": 2.809427430936263, + "angularVelocity": 1.5274328029470334, + "velocityX": 2.1923392844717964, + "velocityY": -7.370999188984064e-19, + "timestamp": 0.3041294988837664 + }, + { + "x": 2.2601117640496655, + "y": 4, + "heading": 2.921646905870359, + "angularVelocity": 1.8449291394637872, + "velocityX": 2.630338034370403, + "velocityY": -5.778823319330962e-19, + "timestamp": 0.3649553986605197 + }, + { + "x": 2.4013515665186476, + "y": 4, + "heading": 3.0051161246453697, + "angularVelocity": 1.3722644314788166, + "velocityX": 2.3220339195494386, + "velocityY": -6.643266058953634e-19, + "timestamp": 0.42578129843727297 + }, + { + "x": 2.515959575579507, + "y": 4, + "heading": 3.0690228968495292, + "angularVelocity": 1.0506506675560594, + "velocityX": 1.8841975125942059, + "velocityY": -5.253201411023923e-19, + "timestamp": 0.48660719821402626 + }, + { + "x": 2.6039369458093504, + "y": 4, + "heading": 3.1133381061427943, + "angularVelocity": 0.7285582203767191, + "velocityX": 1.4463800873074064, + "velocityY": -5.128797583419745e-19, + "timestamp": 0.5474330979907795 + }, + { + "x": 2.665283787360706, + "y": 4, + "heading": 3.138058308493628, + "angularVelocity": 0.40640915206282496, + "velocityX": 1.0085644729682035, + "velocityY": -5.254834134379204e-19, + "timestamp": 0.6082589977675328 + }, + { + "x": 2.7, + "y": 4, + "heading": 3.143185307179586, + "angularVelocity": 0.08428973029097245, + "velocityX": 0.5707472107523717, + "velocityY": -6.37300209887089e-17, + "timestamp": 0.669084897544286 + }, + { + "x": 2.7133851131067677, + "y": 3.977058292663326, + "heading": 3.131348732424157, + "angularVelocity": -0.17128774031052524, + "velocityX": 0.19369672604995564, + "velocityY": -0.33199073968173415, + "timestamp": 0.7381883531371755 + }, + { + "x": 2.7007159790989697, + "y": 3.9311730899537207, + "heading": 3.1018591119354357, + "angularVelocity": -0.42674595990012093, + "velocityX": -0.18333575215041678, + "velocityY": -0.6640073541374039, + "timestamp": 0.8072918087300649 + }, + { + "x": 2.66199484115675, + "y": 3.862341706815855, + "heading": 3.054718737972702, + "angularVelocity": -0.6821710080767611, + "velocityX": -0.5603357691638745, + "velocityY": -0.9960628241807469, + "timestamp": 0.8763952643229543 + }, + { + "x": 2.597225247659028, + "y": 3.770561041115099, + "heading": 2.989905863580779, + "angularVelocity": -0.9379107576676369, + "velocityX": -0.9372844373850924, + "velocityY": -1.3281631853901548, + "timestamp": 0.9454987199158438 + }, + { + "x": 2.506412179488274, + "y": 3.655828456407957, + "heading": 2.907352350125962, + "angularVelocity": -1.1946365452673091, + "velocityX": -1.3141610269737936, + "velocityY": -1.6603016987236956, + "timestamp": 1.014602175508733 + }, + { + "x": 2.3895616650846327, + "y": 3.5181430948085093, + "heading": 2.8069247500054657, + "angularVelocity": -1.4532934606482912, + "velocityX": -1.6909503786622855, + "velocityY": -1.992452626576132, + "timestamp": 1.0837056311016224 + }, + { + "x": 2.246679523391877, + "y": 3.357507378978086, + "heading": 2.68842324173939, + "angularVelocity": -1.7148420039290893, + "velocityX": -2.067655524077823, + "velocityY": -2.324568495970117, + "timestamp": 1.1528090866945118 + }, + { + "x": 2.090491509437375, + "y": 3.2199148203355916, + "heading": 2.5775020951989167, + "angularVelocity": -1.6051461621820693, + "velocityX": -2.2602055514869797, + "velocityY": -1.9911096696716033, + "timestamp": 1.221912542287401 + }, + { + "x": 1.960331483425684, + "y": 3.1052621757182224, + "heading": 2.4849774188469222, + "angularVelocity": -1.3389298054169396, + "velocityX": -1.8835530712188058, + "velocityY": -1.6591448811266607, + "timestamp": 1.2910159978802904 + }, + { + "x": 1.8562006631201746, + "y": 3.0135445544840174, + "heading": 2.4109267688757035, + "angularVelocity": -1.071591128618647, + "velocityX": -1.5068829686377219, + "velocityY": -1.3272508653263182, + "timestamp": 1.3601194534731798 + }, + { + "x": 1.7781008483685374, + "y": 2.944758434687115, + "heading": 2.3553834808519016, + "angularVelocity": -0.8037700509556753, + "velocityX": -1.1301868203624321, + "velocityY": -0.995407815799475, + "timestamp": 1.429222909066069 + }, + { + "x": 1.7260336446967814, + "y": 2.8989016512903865, + "heading": 2.3183561927552425, + "angularVelocity": -0.5358239726007007, + "velocityX": -0.7534674384313609, + "velocityY": -0.6635960966451635, + "timestamp": 1.4983263646589584 + }, + { + "x": 1.7000000000000002, + "y": 2.875973224639892, + "heading": 2.299843907718641, + "angularVelocity": -0.2678923199651603, + "velocityX": -0.3767343394579128, + "velocityY": -0.3317985541171716, + "timestamp": 1.5674298202518477 + }, + { + "x": 1.7, + "y": 2.8759732246398926, + "heading": 2.299843907718641, + "angularVelocity": 9.076953787892685e-18, + "velocityX": -1.1997312285823622e-16, + "velocityY": 1.5409057838224231e-16, + "timestamp": 1.636533275844737 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "0206ccc5-d7d5-4a96-a88c-d93a79cc0d76" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "3b8706c0-f4cc-4675-b0bb-ba918daf3417" + }, + { + "scope": [ + 0, + 1 + ], + "type": "StraightLine", + "uuid": "dc3f97c5-610c-4db2-bd95-f84502c7434a" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + } +} \ No newline at end of file diff --git a/TEST b/TEST deleted file mode 100644 index 8b137891..00000000 --- a/TEST +++ /dev/null @@ -1 +0,0 @@ - diff --git a/TEST again b/TEST again deleted file mode 100644 index 8b137891..00000000 --- a/TEST again +++ /dev/null @@ -1 +0,0 @@ - diff --git a/TEST again again b/TEST again again deleted file mode 100644 index 8b137891..00000000 --- a/TEST again again +++ /dev/null @@ -1 +0,0 @@ - diff --git a/TESTTTTT b/TESTTTTT deleted file mode 100644 index 6e9f0da1..00000000 --- a/TESTTTTT +++ /dev/null @@ -1 +0,0 @@ -h diff --git a/TESTTTTTTTTTTTT b/TESTTTTTTTTTTTT deleted file mode 100644 index 8b137891..00000000 --- a/TESTTTTTTTTTTTT +++ /dev/null @@ -1 +0,0 @@ - diff --git a/goodsmile.chor b/goodsmile.chor deleted file mode 100644 index 52b31c8f..00000000 --- a/goodsmile.chor +++ /dev/null @@ -1,169 +0,0 @@ -{ - "version": "v0.2", - "robotConfiguration": { - "mass": 66.67917930278274, - "rotationalInertia": 6, - "motorMaxTorque": 1.0236966824644549, - "motorMaxVelocity": 5427.200000000001, - "gearing": 6.75, - "wheelbase": 0.5460997051061592, - "trackWidth": 0.5460997051061592, - "bumperLength": 0.7873995748042296, - "bumperWidth": 0.7873995748042296, - "wheelRadius": 0.050799972568014815 - }, - "paths": { - "StraightPath": { - "waypoints": [ - { - "x": 0.7868605852127075, - "y": 1.7450958490371704, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.9511260986328125, - "y": 1.7450958490371704, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7868605852127075, - "y": 1.7450958490371704, - "heading": 0, - "angularVelocity": -4.4492197356624413e-41, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.8590027745865342, - "y": 1.7450958490371704, - "heading": 4.657766863245244e-22, - "angularVelocity": 4.9536318880564336e-21, - "velocityX": 0.7672471831093439, - "velocityY": 1.2958530337319892e-34, - "timestamp": 0.0940273108354252 - }, - { - "x": 1.0032871512367538, - "y": 1.7450958490371704, - "heading": 1.403641448356225e-21, - "angularVelocity": 9.974386655514963e-21, - "velocityX": 1.5344943439120444, - "velocityY": -3.121891213060774e-34, - "timestamp": 0.1880546216708504 - }, - { - "x": 1.2197137116676435, - "y": 1.7450958490371704, - "heading": 2.7920190511467688e-21, - "angularVelocity": 1.4765684328282012e-20, - "velocityX": 2.301741467537005, - "velocityY": 3.707033245034656e-34, - "timestamp": 0.2820819325062756 - }, - { - "x": 1.5082824488877573, - "y": 1.7450958490371704, - "heading": 4.5069164071667e-21, - "angularVelocity": 1.8238289714603687e-20, - "velocityX": 3.0689885168064825, - "velocityY": 4.7213688126860205e-34, - "timestamp": 0.3761092433417008 - }, - { - "x": 1.8689933419227598, - "y": 1.7450958490371704, - "heading": 6.529263648732127e-21, - "angularVelocity": 2.150808314782629e-20, - "velocityX": 3.836235343009545, - "velocityY": 5.789081007414333e-34, - "timestamp": 0.470136554177126 - }, - { - "x": 2.2297042349577625, - "y": 1.7450958490371704, - "heading": 2.803134639856034e-21, - "angularVelocity": -3.9628156567184644e-20, - "velocityX": 3.836235343009545, - "velocityY": -8.692522666586612e-34, - "timestamp": 0.5641638650125512 - }, - { - "x": 2.518272972177876, - "y": 1.7450958490371704, - "heading": 1.1776876468750482e-21, - "angularVelocity": -1.728696651707287e-20, - "velocityX": 3.068988516806483, - "velocityY": -1.0003811410512837e-33, - "timestamp": 0.6581911758479764 - }, - { - "x": 2.734699532608766, - "y": 1.7450958490371704, - "heading": 3.973012037331865e-22, - "angularVelocity": -8.299571861552746e-21, - "velocityX": 2.301741467537005, - "velocityY": -1.1177157729079989e-33, - "timestamp": 0.7522184866834016 - }, - { - "x": 2.8789839092589857, - "y": 1.7450958490371704, - "heading": 9.127912197331218e-23, - "angularVelocity": -3.2546083802962352e-21, - "velocityX": 1.5344943439120444, - "velocityY": -2.0250344955726205e-33, - "timestamp": 0.8462457975188269 - }, - { - "x": 2.9511260986328125, - "y": 1.7450958490371704, - "heading": -1.0858594213851783e-41, - "angularVelocity": -9.707724081768977e-22, - "velocityX": 0.7672471831093439, - "velocityY": -1.069387939985581e-33, - "timestamp": 0.9402731083542522 - }, - { - "x": 2.9511260986328125, - "y": 1.7450958490371704, - "heading": 2.453784248073241e-41, - "angularVelocity": 3.764408456751764e-40, - "velocityX": -3.382441800768739e-40, - "velocityY": 0, - "timestamp": 1.0343004191896774 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "60ea8831-d5a7-4b01-baa5-513216666085" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "88aa503c-a5c9-4f49-bea6-a1744dede821" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - } -} \ No newline at end of file diff --git a/goodsmile2.chor b/goodsmile2.chor deleted file mode 100644 index b3a4f3b5..00000000 --- a/goodsmile2.chor +++ /dev/null @@ -1,421 +0,0 @@ -{ - "version": "v0.2", - "robotConfiguration": { - "mass": 66.67917930278274, - "rotationalInertia": 6, - "motorMaxTorque": 1.0236966824644549, - "motorMaxVelocity": 5427.200000000001, - "gearing": 6.75, - "wheelbase": 0.5460997051061592, - "trackWidth": 0.5460997051061592, - "bumperLength": 0.7873995748042296, - "bumperWidth": 0.7873995748042296, - "wheelRadius": 0.050799972568014815 - }, - "paths": { - "TestPath": { - "waypoints": [ - { - "x": 0.7673627138137817, - "y": 3.2464332580566406, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.3856873512268066, - "y": 3.1684415340423584, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 1.1378226280212402, - "y": 1.9790704250335693, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 - }, - { - "x": 2.2297043800354004, - "y": 1.862083077430725, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 1.0793288946151733, - "y": 0.9456824064254761, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7673627138137817, - "y": 3.2464332580566406, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.8313219702670532, - "y": 3.261713385574597, - "heading": 3.533650609102648e-18, - "angularVelocity": 3.979021166800338e-17, - "velocityX": 0.7202108891214888, - "velocityY": 0.17206132209410366, - "timestamp": 0.0888062891293223 - }, - { - "x": 0.9594752138713047, - "y": 3.2912553658203234, - "heading": 3.671497818522258e-18, - "angularVelocity": 1.5517087282187453e-18, - "velocityX": 1.4430649547567311, - "velocityY": 0.33265639782049516, - "timestamp": 0.1776125782586446 - }, - { - "x": 1.1522255018116965, - "y": 3.3331054579829322, - "heading": 1.2612772723019954e-17, - "angularVelocity": 1.0068204495400854e-16, - "velocityX": 2.170457631216723, - "velocityY": 0.47125144595882906, - "timestamp": 0.2664188673879669 - }, - { - "x": 1.4103534961588797, - "y": 3.382028136962027, - "heading": 2.378933917940323e-17, - "angularVelocity": 1.2585235830092183e-16, - "velocityX": 2.906640924628416, - "velocityY": 0.5508920534643025, - "timestamp": 0.3552251565172892 - }, - { - "x": 1.7238538224608133, - "y": 3.395480293589944, - "heading": 2.9151676321528394e-17, - "angularVelocity": 6.038195737762248e-17, - "velocityX": 3.5301590616565566, - "velocityY": 0.15147752213034935, - "timestamp": 0.4440314456466115 - }, - { - "x": 1.9809621494666982, - "y": 3.375106325581054, - "heading": 3.1298902665080175e-17, - "angularVelocity": 2.4178931499768006e-17, - "velocityX": 2.89515899747691, - "velocityY": -0.22942032832169598, - "timestamp": 0.5328377347759338 - }, - { - "x": 2.1775487153774615, - "y": 3.3290149751984806, - "heading": 1.7949675457552524e-17, - "angularVelocity": -1.5031784256628876e-16, - "velocityX": 2.213655900243467, - "velocityY": -0.5190099804213014, - "timestamp": 0.6216440239052561 - }, - { - "x": 2.312605513389427, - "y": 3.2597211271439366, - "heading": 1.0435986091861083e-17, - "angularVelocity": -8.460659225837193e-17, - "velocityX": 1.5208021789326576, - "velocityY": -0.7802808644891939, - "timestamp": 0.7104503130345784 - }, - { - "x": 2.385687351226806, - "y": 3.1684415340423584, - "heading": 0, - "angularVelocity": -1.1751277770389067e-16, - "velocityX": 0.8229353861363937, - "velocityY": -1.0278505497251267, - "timestamp": 0.7992566021639007 - }, - { - "x": 2.39802018138135, - "y": 3.059861903828739, - "heading": -8.971793294610122e-18, - "angularVelocity": -1.0412721485646132e-16, - "velocityX": 0.14313910934234023, - "velocityY": -1.2602128927311516, - "timestamp": 0.8854163540899043 - }, - { - "x": 2.351733996293172, - "y": 2.9314027927936044, - "heading": -8.22228309257207e-18, - "angularVelocity": 8.701816868292406e-18, - "velocityX": -0.537213537140077, - "velocityY": -1.4909410503319676, - "timestamp": 0.9715761060159078 - }, - { - "x": 2.2467498134917507, - "y": 2.783298646799663, - "heading": 6.361873667551736e-19, - "angularVelocity": 1.0281745008234488e-16, - "velocityX": -1.2184828816013207, - "velocityY": -1.7189481478408888, - "timestamp": 1.0577358579419114 - }, - { - "x": 2.0829130728854173, - "y": 2.6160174824363893, - "heading": 1.451869152796759e-17, - "angularVelocity": 1.6112838511948148e-16, - "velocityX": -1.9015461041561337, - "velocityY": -1.9415232822976558, - "timestamp": 1.143895609867915 - }, - { - "x": 1.8597870079627992, - "y": 2.4309555582539106, - "heading": 2.540851142484414e-17, - "angularVelocity": 1.2639457014376757e-16, - "velocityX": -2.5896785905056863, - "velocityY": -2.1478929551827464, - "timestamp": 1.2300553617939185 - }, - { - "x": 1.600006221993929, - "y": 2.295770745776511, - "heading": 2.7033858084873666e-17, - "angularVelocity": 1.8862499864961428e-17, - "velocityX": -3.0151060113341863, - "velocityY": -1.5690018767564016, - "timestamp": 1.316215113719922 - }, - { - "x": 1.397520808532318, - "y": 2.184008562485583, - "heading": 1.991688644871111e-17, - "angularVelocity": -8.260435096889218e-17, - "velocityX": -2.3501160220610267, - "velocityY": -1.2971507089049148, - "timestamp": 1.4023748656459256 - }, - { - "x": 1.252891763187085, - "y": 2.0942468844258686, - "heading": 7.319675515282265e-18, - "angularVelocity": -1.4621043904092617e-16, - "velocityX": -1.6786149229487735, - "velocityY": -1.041805205498046, - "timestamp": 1.4885346175719292 - }, - { - "x": 1.1662966862808535, - "y": 2.026012882216162, - "heading": 3.721123316316183e-18, - "angularVelocity": -4.17697656090747e-17, - "velocityX": -1.0050525328222646, - "velocityY": -0.7919475240752721, - "timestamp": 1.5746943694979327 - }, - { - "x": 1.1378226280212402, - "y": 1.9790704250335691, - "heading": 0, - "angularVelocity": -4.319295160350896e-17, - "velocityX": -0.3304798077588347, - "velocityY": -0.5448304589773546, - "timestamp": 1.6608541214239363 - }, - { - "x": 1.1661502780554174, - "y": 1.9533563874123017, - "heading": -4.541174473529744e-18, - "angularVelocity": -5.3551930315835353e-17, - "velocityX": 0.33404737752043334, - "velocityY": -0.3032269469035916, - "timestamp": 1.7456554159172775 - }, - { - "x": 1.2508475814997149, - "y": 1.9480841237655542, - "heading": -1.088487624461785e-17, - "angularVelocity": -7.480794436746658e-17, - "velocityX": 0.9987737092009069, - "velocityY": -0.062171971314293334, - "timestamp": 1.8304567104106186 - }, - { - "x": 1.3919481486764127, - "y": 1.9631606983898189, - "heading": -2.4436837957428723e-17, - "angularVelocity": -1.5980979431139914e-16, - "velocityX": 1.6638963829723745, - "velocityY": 0.17778708110147448, - "timestamp": 1.9152580049039598 - }, - { - "x": 1.589551494740232, - "y": 1.9983080890650415, - "heading": -3.77560389720726e-17, - "angularVelocity": -1.5706512623296957e-16, - "velocityX": 2.3301925666012955, - "velocityY": 0.41446761963922935, - "timestamp": 2.000059299397301 - }, - { - "x": 1.8338166061674905, - "y": 1.9958021393306062, - "heading": -2.5692727806323102e-17, - "angularVelocity": 1.4225490924619824e-16, - "velocityX": 2.880440833879005, - "velocityY": -0.02955084288474224, - "timestamp": 2.0848605938906424 - }, - { - "x": 2.021989020039336, - "y": 1.9721063118101656, - "heading": -2.0568548109141354e-17, - "angularVelocity": 6.042699735689612e-17, - "velocityX": 2.218980441205407, - "velocityY": -0.279427662741262, - "timestamp": 2.1696618883839838 - }, - { - "x": 2.153963188558974, - "y": 1.9275020986870228, - "heading": -7.160463800154364e-18, - "angularVelocity": 1.5811317114788486e-16, - "velocityX": 1.556275400113002, - "velocityY": -0.5259850499663564, - "timestamp": 2.254463182877325 - }, - { - "x": 2.2297043800354004, - "y": 1.862083077430725, - "heading": 0, - "angularVelocity": 8.443971747672219e-17, - "velocityX": 0.8931607934436495, - "velocityY": -0.7714389461376743, - "timestamp": 2.3392644773706666 - }, - { - "x": 2.241642122651777, - "y": 1.7577725520899423, - "heading": 1.09215011181075e-17, - "angularVelocity": 1.1061115628178402e-16, - "velocityX": 0.12091699674436268, - "velocityY": -1.0565578306529544, - "timestamp": 2.4379912316672145 - }, - { - "x": 2.1770696443739364, - "y": 1.6260530315126709, - "heading": 2.966300916987114e-17, - "angularVelocity": 1.898177424403562e-16, - "velocityX": -0.6540524777622911, - "velocityY": -1.3341826287869905, - "timestamp": 2.5367179859637625 - }, - { - "x": 2.0354835831990714, - "y": 1.4683724994171197, - "heading": 3.7407730059877466e-17, - "angularVelocity": 7.842694480695285e-17, - "velocityX": -1.4341204892006811, - "velocityY": -1.5971408481480822, - "timestamp": 2.6354447402603105 - }, - { - "x": 1.8156233245797035, - "y": 1.2888238692984357, - "heading": 5.370156680643209e-17, - "angularVelocity": 1.6501104231487417e-16, - "velocityX": -2.226957223328312, - "velocityY": -1.818642083270466, - "timestamp": 2.7341714945568585 - }, - { - "x": 1.5235882590431191, - "y": 1.1466360697607068, - "heading": 3.939999146803209e-17, - "angularVelocity": -1.448227110599334e-16, - "velocityX": -2.9580134343658933, - "velocityY": -1.4402154770671953, - "timestamp": 2.8328982488534065 - }, - { - "x": 1.302028360814128, - "y": 1.0449243534386055, - "heading": 2.679743341818561e-17, - "angularVelocity": -1.2763045565525862e-16, - "velocityX": -2.2441728162985854, - "velocityY": -1.0302345807287014, - "timestamp": 2.9316250031499544 - }, - { - "x": 1.1536834387307633, - "y": 0.9784919942389271, - "heading": 1.1689536588822448e-17, - "angularVelocity": -1.5301578380329193e-16, - "velocityX": -1.5025807659115946, - "velocityY": -0.6728911496218141, - "timestamp": 3.0303517574465024 - }, - { - "x": 1.0793288946151731, - "y": 0.945682406425476, - "heading": 0, - "angularVelocity": -1.1839793532548193e-16, - "velocityX": -0.7531346963367315, - "velocityY": -0.3323272201710826, - "timestamp": 3.1290785117430504 - }, - { - "x": 1.0793288946151731, - "y": 0.945682406425476, - "heading": 0, - "angularVelocity": 0, - "velocityX": 1.228842240194809e-30, - "velocityY": 2.0852631765546578e-30, - "timestamp": 3.2278052660395984 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "8b0b78e0-fb39-4ec1-8161-cc5a24e3e917" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "2c17db2c-096f-4ae0-bc10-6ef89ad026c4" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - } -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SW1S.traj b/src/main/deploy/choreo/SW1S.traj new file mode 100644 index 00000000..e51e9174 --- /dev/null +++ b/src/main/deploy/choreo/SW1S.traj @@ -0,0 +1,553 @@ +{ + "samples": [ + { + "x": 1.5, + "y": 7, + "heading": -2.18, + "angularVelocity": -7.014080895465295e-20, + "velocityX": -6.1070488035186446e-18, + "velocityY": 1.704040905172389e-18, + "timestamp": 0 + }, + { + "x": 1.5109255821948424, + "y": 6.999825295018549, + "heading": -2.1957306466432525, + "angularVelocity": -0.3925464436026101, + "velocityX": 0.2726396779571315, + "velocityY": -0.004359631279235028, + "timestamp": 0.040073338835740574 + }, + { + "x": 1.5328085169368726, + "y": 6.999520158674122, + "heading": -2.226854341802715, + "angularVelocity": -0.7766683801176959, + "velocityX": 0.5460721611375935, + "velocityY": -0.007614447742360675, + "timestamp": 0.08014667767148115 + }, + { + "x": 1.5656875968313502, + "y": 6.999154789467847, + "heading": -2.2729635858312136, + "angularVelocity": -1.1506214697382335, + "velocityX": 0.8204726845759085, + "velocityY": -0.009117513461358586, + "timestamp": 0.12022001650722172 + }, + { + "x": 1.6095978948873755, + "y": 6.998820636587558, + "heading": -2.333703386104559, + "angularVelocity": -1.515715985691008, + "velocityX": 1.0957484285503496, + "velocityY": -0.008338533548678785, + "timestamp": 0.1602933553429623 + }, + { + "x": 1.664560942869615, + "y": 6.998614187753099, + "heading": -2.408894144312653, + "angularVelocity": -1.8763287610322779, + "velocityX": 1.3715614815010924, + "velocityY": -0.005151775231561494, + "timestamp": 0.20036669417870287 + }, + { + "x": 1.7305843869075594, + "y": 6.998608834089527, + "heading": -2.4985652376560035, + "angularVelocity": -2.2376746222944734, + "velocityX": 1.6475653378566024, + "velocityY": -0.00013359664377947739, + "timestamp": 0.24044003301444344 + }, + { + "x": 1.8076797623531138, + "y": 6.998823522335968, + "heading": -2.602788739978155, + "angularVelocity": -2.6008190320587983, + "velocityX": 1.9238570502341217, + "velocityY": 0.005357383554156006, + "timestamp": 0.280513371850184 + }, + { + "x": 1.8959026263569365, + "y": 6.999202422024261, + "heading": -2.7212110856967926, + "angularVelocity": -2.9551404789105975, + "velocityX": 2.2015351494781537, + "velocityY": 0.009455156453139688, + "timestamp": 0.3205867106859246 + }, + { + "x": 1.9954182043717335, + "y": 6.999616761540192, + "heading": -2.852071562900106, + "angularVelocity": -3.2655246856175077, + "velocityX": 2.4833363254985996, + "velocityY": 0.010339530669704458, + "timestamp": 0.3606600495216652 + }, + { + "x": 2.10657810793121, + "y": 6.999879512490144, + "heading": -2.9896460539129763, + "angularVelocity": -3.4330678453477326, + "velocityX": 2.7739117026192326, + "velocityY": 0.006556752134792622, + "timestamp": 0.4007333883574058 + }, + { + "x": 2.2257531407250775, + "y": 7, + "heading": -3.090048550833243, + "angularVelocity": -2.505468719038884, + "velocityX": 2.9739232181838897, + "velocityY": 0.00300667509514831, + "timestamp": 0.4408067271931464 + }, + { + "x": 2.34, + "y": 7, + "heading": -3.14, + "angularVelocity": -1.2465008062235836, + "velocityX": 2.8509443583715566, + "velocityY": 2.66121515722134e-18, + "timestamp": 0.480880066028887 + }, + { + "x": 2.3902685794468983, + "y": 7, + "heading": -3.154884405334106, + "angularVelocity": -0.8176183004169806, + "velocityX": 2.7613135741148205, + "velocityY": 1.0875590759442405e-18, + "timestamp": 0.49908465529325463 + }, + { + "x": 2.4384926520059382, + "y": 7, + "heading": -3.1644927701307046, + "angularVelocity": -0.52779904325617, + "velocityX": 2.6490063499218826, + "velocityY": -2.8339734979486245e-20, + "timestamp": 0.5172892445576223 + }, + { + "x": 2.4844766569844676, + "y": 7, + "heading": -3.1704799601608893, + "angularVelocity": -0.3288835547586147, + "velocityX": 2.525956741497459, + "velocityY": -4.102095677617762e-20, + "timestamp": 0.5354938338219899 + }, + { + "x": 2.5281300977466628, + "y": 7, + "heading": -3.173869974980369, + "angularVelocity": -0.18621759437953955, + "velocityX": 2.3979360439421336, + "velocityY": -2.8092935749629884e-20, + "timestamp": 0.5536984230863575 + }, + { + "x": 2.5694076425029193, + "y": 7, + "heading": -3.1753354183161275, + "angularVelocity": -0.08049856629415185, + "velocityX": 2.2674252166208264, + "velocityY": -2.386752349644547e-20, + "timestamp": 0.5719030123507252 + }, + { + "x": 2.608284712434112, + "y": 7, + "heading": -3.1753501196100573, + "angularVelocity": -0.0008075597704278253, + "velocityX": 2.1355642451809587, + "velocityY": -2.3063509187477393e-20, + "timestamp": 0.5901076016150928 + }, + { + "x": 2.644747196375146, + "y": 7, + "heading": -3.1742675406474747, + "angularVelocity": 0.059467365446420554, + "velocityX": 2.0029281304577187, + "velocityY": -2.3158354576494492e-20, + "timestamp": 0.6083121908794604 + }, + { + "x": 2.6787867109477963, + "y": 7, + "heading": -3.1723631059987043, + "angularVelocity": 0.10461288750417592, + "velocityX": 1.8698315066780489, + "velocityY": -2.337204487809121e-20, + "timestamp": 0.6265167801438281 + }, + { + "x": 2.7103982151264825, + "y": 7, + "heading": -3.1698586440576513, + "angularVelocity": 0.13757310888402063, + "velocityX": 1.7364579732956862, + "velocityY": -2.352312118224177e-20, + "timestamp": 0.6447213694081957 + }, + { + "x": 2.7395787166189542, + "y": 7, + "heading": -3.166937416369599, + "angularVelocity": 0.1604665530011168, + "velocityX": 1.6029200696983334, + "velocityY": -2.3601907781176607e-20, + "timestamp": 0.6629259586725633 + }, + { + "x": 2.766326526340967, + "y": 7, + "heading": -3.1637538745889637, + "angularVelocity": 0.1748757818375146, + "velocityX": 1.4692893826704234, + "velocityY": -2.3634202608032735e-20, + "timestamp": 0.681130547936931 + }, + { + "x": 2.790640807171386, + "y": 7, + "heading": -3.160440289451799, + "angularVelocity": 0.18201921993659123, + "velocityX": 1.3356127115709844, + "velocityY": -2.3643590270339423e-20, + "timestamp": 0.6993351372012986 + }, + { + "x": 2.812521289813831, + "y": 7, + "heading": -3.1571114286194844, + "angularVelocity": 0.18285833225752388, + "velocityX": 1.2019212476971914, + "velocityY": -2.3644570679392813e-20, + "timestamp": 0.7175397264656662 + }, + { + "x": 2.831968088068395, + "y": 7, + "heading": -3.1538679639272003, + "angularVelocity": 0.17816741949969286, + "velocityX": 1.068236034999556, + "velocityY": -2.3644058515684063e-20, + "timestamp": 0.7357443157300338 + }, + { + "x": 2.8489815755787715, + "y": 7, + "heading": -3.150799020167611, + "angularVelocity": 0.16858077460703824, + "velocityX": 0.9345713469996556, + "velocityY": -2.3644262680482202e-20, + "timestamp": 0.7539489049944015 + }, + { + "x": 2.8635623018508554, + "y": 7, + "heading": -3.147984125285486, + "angularVelocity": 0.15462556398536811, + "velocityX": 0.8009368440200342, + "velocityY": -2.3645061677621432e-20, + "timestamp": 0.7721534942587691 + }, + { + "x": 2.8757109340551876, + "y": 7, + "heading": -3.1454947317261395, + "angularVelocity": 0.13674538453989185, + "velocityX": 0.6673389895212279, + "velocityY": -2.3645586230834433e-20, + "timestamp": 0.7903580835231367 + }, + { + "x": 2.8854282161557525, + "y": 7, + "heading": -3.1433954232633634, + "angularVelocity": 0.11531754066459125, + "velocityX": 0.533782001859519, + "velocityY": -2.3645144712534832e-20, + "timestamp": 0.8085626727875044 + }, + { + "x": 2.892714939914606, + "y": 7, + "heading": -3.1417448863967854, + "angularVelocity": 0.09066597672756352, + "velocityX": 0.40026850664056873, + "velocityY": -2.3643698574082317e-20, + "timestamp": 0.826767262051872 + }, + { + "x": 2.8975719241751396, + "y": 7, + "heading": -3.140596702327834, + "angularVelocity": 0.06307113290374677, + "velocityX": 0.26679999147468597, + "velocityY": -2.3642052138431793e-20, + "timestamp": 0.8449718513162396 + }, + { + "x": 2.9, + "y": 7, + "heading": -3.14, + "angularVelocity": 0.03277757708062409, + "velocityX": 0.13337712758027845, + "velocityY": -2.3641921174305024e-20, + "timestamp": 0.8631764405806073 + }, + { + "x": 2.9, + "y": 7, + "heading": -3.14, + "angularVelocity": 1.1349626131054217e-19, + "velocityX": -5.87186181590248e-18, + "velocityY": 3.5234181128249e-19, + "timestamp": 0.8813810298449749 + }, + { + "x": 2.896686389644809, + "y": 6.991124257977168, + "heading": -3.14, + "angularVelocity": 7.138015320386443e-20, + "velocityX": -0.09225587401594994, + "velocityY": -0.2471139482570088, + "timestamp": 0.9172986382415722 + }, + { + "x": 2.890059169018056, + "y": 6.973372774155508, + "heading": -3.14, + "angularVelocity": 7.138015366262094e-20, + "velocityX": -0.18451174570356052, + "velocityY": -0.49422789027739433, + "timestamp": 0.9532162466381695 + }, + { + "x": 2.8801183382192987, + "y": 6.946745548801693, + "heading": -3.14, + "angularVelocity": 7.138015292840098e-20, + "velocityX": -0.2767676146193387, + "velocityY": -0.7413418248732287, + "timestamp": 0.9891338550347668 + }, + { + "x": 2.8668638973690532, + "y": 6.911242582238536, + "heading": -3.14, + "angularVelocity": 7.138015343140458e-20, + "velocityX": -0.369023480179741, + "velocityY": -0.9884557504814493, + "timestamp": 1.025051463431364 + }, + { + "x": 2.8502958466161945, + "y": 6.866863874864807, + "heading": -3.14, + "angularVelocity": 7.138015303703884e-20, + "velocityX": -0.4612793415952677, + "velocityY": -1.2355696649873245, + "timestamp": 1.0609690718279614 + }, + { + "x": 2.8304141861492957, + "y": 6.813609427185614, + "heading": -3.14, + "angularVelocity": 7.138015321902843e-20, + "velocityX": -0.5535351977606193, + "velocityY": -1.4826835654302304, + "timestamp": 1.0968866802245587 + }, + { + "x": 2.807218916214953, + "y": 6.751479239861482, + "heading": -3.14, + "angularVelocity": 7.138015311568457e-20, + "velocityX": -0.6457910470603585, + "velocityY": -1.729797447483103, + "timestamp": 1.132804288621156 + }, + { + "x": 2.7807100371494347, + "y": 6.6804733137931285, + "heading": -3.14, + "angularVelocity": 7.138015326356826e-20, + "velocityX": -0.7380468869979013, + "velocityY": -1.9769113044586646, + "timestamp": 1.1687218970177533 + }, + { + "x": 2.75088754943846, + "y": 6.60059165028159, + "heading": -3.14, + "angularVelocity": 7.138015312468868e-20, + "velocityX": -0.8303027134122785, + "velocityY": -2.2240251252114605, + "timestamp": 1.2046395054143506 + }, + { + "x": 2.7177514538453025, + "y": 6.511834251371347, + "heading": -3.14, + "angularVelocity": 7.138015316229824e-20, + "velocityX": -0.9225585185759843, + "velocityY": -2.471138889042815, + "timestamp": 1.240557113810948 + }, + { + "x": 2.681301751743851, + "y": 6.414201120742459, + "heading": -3.14, + "angularVelocity": 7.138015315767601e-20, + "velocityX": -1.0148142854885556, + "velocityY": -2.7182525504157744, + "timestamp": 1.2764747222075452 + }, + { + "x": 2.641538446339825, + "y": 6.307692266981674, + "heading": -3.14, + "angularVelocity": 7.138015319545813e-20, + "velocityX": -1.1070699631490775, + "velocityY": -2.9653659727207433, + "timestamp": 1.3123923306041425 + }, + { + "x": 2.5984615536601745, + "y": 6.192307733018326, + "heading": -3.14, + "angularVelocity": 7.138015317958231e-20, + "velocityX": -1.1993251945949435, + "velocityY": -3.212478199807885, + "timestamp": 1.3483099390007398 + }, + { + "x": 2.558698248256148, + "y": 6.08579887925754, + "heading": -3.14, + "angularVelocity": 7.13801531830952e-20, + "velocityX": -1.1070699631490775, + "velocityY": -2.9653659727207433, + "timestamp": 1.384227547397337 + }, + { + "x": 2.5222485461546964, + "y": 5.988165748628653, + "heading": -3.14, + "angularVelocity": 7.138015317499089e-20, + "velocityX": -1.0148142854885558, + "velocityY": -2.7182525504157744, + "timestamp": 1.4201451557939344 + }, + { + "x": 2.489112450561539, + "y": 5.899408349718409, + "heading": -3.14, + "angularVelocity": 7.138015312927702e-20, + "velocityX": -0.9225585185759843, + "velocityY": -2.471138889042815, + "timestamp": 1.4560627641905317 + }, + { + "x": 2.4592899628505647, + "y": 5.819526686206871, + "heading": -3.14, + "angularVelocity": 7.138015322817121e-20, + "velocityX": -0.8303027134122783, + "velocityY": -2.2240251252114605, + "timestamp": 1.491980372587129 + }, + { + "x": 2.4327810837850468, + "y": 5.748520760138518, + "heading": -3.14, + "angularVelocity": 7.138015316283442e-20, + "velocityX": -0.7380468869979014, + "velocityY": -1.9769113044586648, + "timestamp": 1.5278979809837263 + }, + { + "x": 2.409585813850704, + "y": 5.686390572814386, + "heading": -3.14, + "angularVelocity": 7.138015332876638e-20, + "velocityX": -0.6457910470603584, + "velocityY": -1.729797447483103, + "timestamp": 1.5638155893803236 + }, + { + "x": 2.3897041533838053, + "y": 5.633136125135193, + "heading": -3.14, + "angularVelocity": 7.138015307743406e-20, + "velocityX": -0.5535351977606193, + "velocityY": -1.4826835654302304, + "timestamp": 1.599733197776921 + }, + { + "x": 2.373136102630946, + "y": 5.588757417761465, + "heading": -3.14, + "angularVelocity": 7.13801531645015e-20, + "velocityX": -0.46127934159526773, + "velocityY": -1.2355696649873245, + "timestamp": 1.6356508061735182 + }, + { + "x": 2.359881661780701, + "y": 5.553254451198307, + "heading": -3.14, + "angularVelocity": 7.138015317270443e-20, + "velocityX": -0.3690234801797411, + "velocityY": -0.9884557504814494, + "timestamp": 1.6715684145701155 + }, + { + "x": 2.3499408309819434, + "y": 5.526627225844492, + "heading": -3.14, + "angularVelocity": 7.138015308841649e-20, + "velocityX": -0.2767676146193387, + "velocityY": -0.7413418248732287, + "timestamp": 1.7074860229667128 + }, + { + "x": 2.3433136103551906, + "y": 5.508875742022832, + "heading": -3.14, + "angularVelocity": 7.138015351011194e-20, + "velocityX": -0.18451174570356052, + "velocityY": -0.49422789027739433, + "timestamp": 1.7434036313633101 + }, + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 7.138015274700303e-20, + "velocityX": -0.09225587401594994, + "velocityY": -0.2471139482570088, + "timestamp": 1.7793212397599074 + }, + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 1.7845038344079976e-20, + "velocityX": 1.0123024300783969e-18, + "velocityY": 1.3576731951648499e-18, + "timestamp": 1.8152388481565047 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SW1SW2SW3S.traj b/src/main/deploy/choreo/SW1SW2SW3S.traj deleted file mode 100644 index 92419866..00000000 --- a/src/main/deploy/choreo/SW1SW2SW3S.traj +++ /dev/null @@ -1,1390 +0,0 @@ -{ - "samples": [ - { - "x": 1.5000000000000009, - "y": 7.000000000000001, - "heading": -2.1879874327352433, - "angularVelocity": 1.7903869676459323e-17, - "velocityX": -3.874772066568338e-17, - "velocityY": 5.345529425222691e-17, - "timestamp": 0 - }, - { - "x": 1.5105562350270076, - "y": 6.999193704899371, - "heading": -2.2080296213631416, - "angularVelocity": -0.5287732195601459, - "velocityX": 0.27850523140349154, - "velocityY": -0.021272489955889925, - "timestamp": 0.037903183986173754 - }, - { - "x": 1.5318236992162386, - "y": 6.997796399132959, - "heading": -2.2468914573930596, - "angularVelocity": -1.0252921243811561, - "velocityX": 0.5610996742906128, - "velocityY": -0.0368651289807619, - "timestamp": 0.07580636797234751 - }, - { - "x": 1.563959490951023, - "y": 6.996123790841906, - "heading": -2.3033249131947477, - "angularVelocity": -1.488884306457078, - "velocityX": 0.8478388450557267, - "velocityY": -0.04412843764473876, - "timestamp": 0.11370955195852125 - }, - { - "x": 1.6070655656774986, - "y": 6.994516075885353, - "heading": -2.3765471800875675, - "angularVelocity": -1.9318236409798388, - "velocityX": 1.1372679071549268, - "velocityY": -0.04241635629217263, - "timestamp": 0.15161273594469502 - }, - { - "x": 1.6611998571683553, - "y": 6.993219037939523, - "heading": -2.4662244315389974, - "angularVelocity": -2.3659556274800484, - "velocityX": 1.4282254364328189, - "velocityY": -0.034219762284504116, - "timestamp": 0.18951591993086878 - }, - { - "x": 1.7264618482511285, - "y": 6.9923314897559115, - "heading": -2.571732468091151, - "angularVelocity": -2.7836193547929966, - "velocityX": 1.7218076219288763, - "velocityY": -0.023416190680134834, - "timestamp": 0.22741910391704254 - }, - { - "x": 1.8030906567227731, - "y": 6.991874753409359, - "heading": -2.690864836138648, - "angularVelocity": -3.143070199351386, - "velocityX": 2.02169845413504, - "velocityY": -0.01205007860808777, - "timestamp": 0.26532228790321627 - }, - { - "x": 1.891489903868625, - "y": 6.992012087718063, - "heading": -2.8177289661820546, - "angularVelocity": -3.347057336650179, - "velocityX": 2.3322380298736642, - "velocityY": 0.003623292143356834, - "timestamp": 0.30322547188939003 - }, - { - "x": 1.9916450489238675, - "y": 6.993405558667383, - "heading": -2.9376325752969628, - "angularVelocity": -3.1634178584751096, - "velocityX": 2.6423939765002946, - "velocityY": 0.036763954972048825, - "timestamp": 0.3411286558755638 - }, - { - "x": 2.1022435336107876, - "y": 6.996205166878248, - "heading": -3.0352839961158606, - "angularVelocity": -2.576338200361362, - "velocityX": 2.9179206878047856, - "velocityY": 0.07386208535695371, - "timestamp": 0.37903183986173755 - }, - { - "x": 2.2229849992865587, - "y": 6.999999999999999, - "heading": -3.107963554426739, - "angularVelocity": -1.917505356208129, - "velocityX": 3.1855230346829506, - "velocityY": 0.1001191119737469, - "timestamp": 0.4169350238479113 - }, - { - "x": 2.339999999999998, - "y": 7, - "heading": -3.1400000000000006, - "angularVelocity": -0.8452177944907323, - "velocityX": 3.087207680392555, - "velocityY": 1.6969225898708918e-16, - "timestamp": 0.4548382078340851 - }, - { - "x": 2.3930972940980944, - "y": 7, - "heading": -3.148656609540402, - "angularVelocity": -0.4837617088146627, - "velocityX": 2.967263061340571, - "velocityY": -9.327833269304345e-20, - "timestamp": 0.47273257479799546 - }, - { - "x": 2.443685667923843, - "y": 7, - "heading": -3.153884756215308, - "angularVelocity": -0.29216717672051384, - "velocityX": 2.827055795143241, - "velocityY": 4.283187493730886e-19, - "timestamp": 0.49062694176190585 - }, - { - "x": 2.4916705157809598, - "y": 7, - "heading": -3.156952578702906, - "angularVelocity": -0.1714406826382175, - "velocityX": 2.6815616307574186, - "velocityY": 5.711348247375438e-19, - "timestamp": 0.5085213087258162 - }, - { - "x": 2.5370175468538037, - "y": 7, - "heading": -3.1585302824819466, - "angularVelocity": -0.08816762181346854, - "velocityX": 2.5341511753000803, - "velocityY": 6.001616976635955e-19, - "timestamp": 0.5264156756897267 - }, - { - "x": 2.579711429559374, - "y": 7, - "heading": -3.15903494293082, - "angularVelocity": -0.028202196248943594, - "velocityX": 2.385883937200842, - "velocityY": 6.17131361738519e-19, - "timestamp": 0.5443100426536371 - }, - { - "x": 2.619744428014943, - "y": 7, - "heading": -3.1587532838177634, - "angularVelocity": 0.015740099309062817, - "velocityX": 2.2371843908370495, - "velocityY": 6.327842808064475e-19, - "timestamp": 0.5622044096175476 - }, - { - "x": 2.6571123583454668, - "y": 7, - "heading": -3.1578956106587053, - "angularVelocity": 0.047929784877443427, - "velocityX": 2.0882510348585934, - "velocityY": 6.462433665541461e-19, - "timestamp": 0.580098776581458 - }, - { - "x": 2.691812877513366, - "y": 7, - "heading": -3.156623303448238, - "angularVelocity": 0.07110099022110888, - "velocityX": 1.9391867417207602, - "velocityY": 6.566869699745141e-19, - "timestamp": 0.5979931435453685 - }, - { - "x": 2.7238446670104492, - "y": 7, - "heading": -3.155064385714718, - "angularVelocity": 0.08711779168901625, - "velocityX": 1.7900487657198059, - "velocityY": 6.6414230651826e-19, - "timestamp": 0.6158875105092789 - }, - { - "x": 2.7532070082129434, - "y": 7, - "heading": -3.1533230466138633, - "angularVelocity": 0.09731213763608634, - "velocityX": 1.6408706305014444, - "velocityY": 6.69064303081725e-19, - "timestamp": 0.6337818774731894 - }, - { - "x": 2.7798995466684726, - "y": 7, - "heading": -3.151485814656567, - "angularVelocity": 0.10267096684934991, - "velocityX": 1.491672687240858, - "velocityY": 6.720225479901625e-19, - "timestamp": 0.6516762444370998 - }, - { - "x": 2.8039221545519717, - "y": 7, - "heading": -3.1496257474780043, - "angularVelocity": 0.10394707912025819, - "velocityX": 1.3424676006672658, - "velocityY": 6.735557356861779e-19, - "timestamp": 0.6695706114010103 - }, - { - "x": 2.825274847198741, - "y": 7, - "heading": -3.147805381403061, - "angularVelocity": 0.10172844217688223, - "velocityX": 1.1932633710840301, - "velocityY": 6.741208959086249e-19, - "timestamp": 0.6874649783649207 - }, - { - "x": 2.843957730858354, - "y": 7, - "heading": -3.1460788713866297, - "angularVelocity": 0.0964834363766629, - "velocityX": 1.0440650790997148, - "velocityY": 6.7408578630113935e-19, - "timestamp": 0.7053593453288312 - }, - { - "x": 2.8599709691790225, - "y": 7, - "heading": -3.144493583151788, - "angularVelocity": 0.08859146781023426, - "velocityX": 0.8948759323515318, - "velocityY": 6.737379866327979e-19, - "timestamp": 0.7232537122927416 - }, - { - "x": 2.8733147612919328, - "y": 7, - "heading": -3.1430913033651042, - "angularVelocity": 0.0783643137269309, - "velocityX": 0.74569791375259, - "velocityY": 6.73298710771389e-19, - "timestamp": 0.741148079256652 - }, - { - "x": 2.8839893272728516, - "y": 7, - "heading": -3.141909176571829, - "angularVelocity": 0.0660613921506774, - "velocityX": 0.5965321937595733, - "velocityY": 6.729364268640707e-19, - "timestamp": 0.7590424462205625 - }, - { - "x": 2.8919948984011485, - "y": 7, - "heading": -3.1409804422982917, - "angularVelocity": 0.05190092923777454, - "velocityX": 0.44737939846901414, - "velocityY": 6.727785765910106e-19, - "timestamp": 0.7769368131844729 - }, - { - "x": 2.8973317105973897, - "y": 7, - "heading": -3.1403350231628555, - "angularVelocity": 0.0360682854411675, - "velocityX": 0.29823978724731737, - "velocityY": 6.729210749119998e-19, - "timestamp": 0.7948311801483834 - }, - { - "x": 2.9000000000000017, - "y": 7, - "heading": -3.1399999999999952, - "angularVelocity": 0.018722269613629893, - "velocityX": 0.1491133722693062, - "velocityY": 6.734357502394604e-19, - "timestamp": 0.8127255471122938 - }, - { - "x": 2.900000000000001, - "y": 7, - "heading": -3.1399999999999952, - "angularVelocity": -2.240989106026273e-17, - "velocityX": 2.419701663950732e-18, - "velocityY": 6.908849296767584e-19, - "timestamp": 0.8306199140762043 - }, - { - "x": 2.8964102555077416, - "y": 6.990384612967162, - "heading": -3.1399999999999957, - "angularVelocity": -6.438647006265958e-18, - "velocityX": -0.10231695991471057, - "velocityY": -0.2740632854858319, - "timestamp": 0.8657044640398821 - }, - { - "x": 2.8892307666201873, - "y": 6.9711538391612144, - "heading": -3.1399999999999957, - "angularVelocity": -6.43864700615304e-18, - "velocityX": -0.2046339170656838, - "velocityY": -0.548126563568796, - "timestamp": 0.9007890140035599 - }, - { - "x": 2.878461533453696, - "y": 6.942307678893826, - "heading": -3.1399999999999957, - "angularVelocity": -6.438647006238391e-18, - "velocityX": -0.30695087090017215, - "velocityY": -0.8221898327683184, - "timestamp": 0.9358735639672378 - }, - { - "x": 2.8641025561504816, - "y": 6.90384613254593, - "heading": -3.139999999999996, - "angularVelocity": -6.438647005767152e-18, - "velocityX": -0.40926782068117873, - "velocityY": -1.0962530911103001, - "timestamp": 0.9709581139309156 - }, - { - "x": 2.8461538348883124, - "y": 6.855769200593692, - "heading": -3.139999999999996, - "angularVelocity": -6.43864700589687e-18, - "velocityX": -0.5115847653953329, - "velocityY": -1.370316335880356, - "timestamp": 1.0060426638945934 - }, - { - "x": 2.824615369895748, - "y": 6.7980768836493235, - "heading": -3.1399999999999966, - "angularVelocity": -6.4386470059483e-18, - "velocityX": -0.6139017035949621, - "velocityY": -1.6443795632007916, - "timestamp": 1.0411272138582712 - }, - { - "x": 2.7994871614775336, - "y": 6.730769182529107, - "heading": -3.1399999999999966, - "angularVelocity": -6.438647005954882e-18, - "velocityX": -0.7162186331085577, - "velocityY": -1.9184427672550652, - "timestamp": 1.076211763821949 - }, - { - "x": 2.770769210060313, - "y": 6.653846098375839, - "heading": -3.139999999999997, - "angularVelocity": -6.438647005960657e-18, - "velocityX": -0.8185355504617052, - "velocityY": -2.1925059387367107, - "timestamp": 1.1112963137856269 - }, - { - "x": 2.738461516284053, - "y": 6.567307632903712, - "heading": -3.139999999999997, - "angularVelocity": -6.438647005754203e-18, - "velocityX": -0.9208524495741806, - "velocityY": -2.4665690613594125, - "timestamp": 1.1463808637493047 - }, - { - "x": 2.7025640812153617, - "y": 6.471153788969718, - "heading": -3.139999999999997, - "angularVelocity": -6.438647005994911e-18, - "velocityX": -1.0231693182855384, - "velocityY": -2.7406321025505496, - "timestamp": 1.1814654137129825 - }, - { - "x": 2.6630769069874574, - "y": 6.365384572287832, - "heading": -3.1399999999999975, - "angularVelocity": -6.438647005925368e-18, - "velocityX": -1.1254861261947087, - "velocityY": -3.0146949808786836, - "timestamp": 1.2165499636766604 - }, - { - "x": 2.6200000000000006, - "y": 6.25, - "heading": -3.1399999999999975, - "angularVelocity": -6.438647005925146e-18, - "velocityX": -1.2278027516970744, - "velocityY": -3.288757370617164, - "timestamp": 1.2516345136403382 - }, - { - "x": 2.5769230930125433, - "y": 6.134615427712169, - "heading": -3.139999999999998, - "angularVelocity": -6.438647005940387e-18, - "velocityX": -1.2278027516970742, - "velocityY": -3.2887573706171636, - "timestamp": 1.286719063604016 - }, - { - "x": 2.537435918784639, - "y": 6.028846211030282, - "heading": -3.139999999999998, - "angularVelocity": -6.438647005836596e-18, - "velocityX": -1.1254861261947082, - "velocityY": -3.0146949808786836, - "timestamp": 1.3218036135676938 - }, - { - "x": 2.5015384837159482, - "y": 5.932692367096288, - "heading": -3.139999999999998, - "angularVelocity": -6.4386470060057575e-18, - "velocityX": -1.0231693182855381, - "velocityY": -2.7406321025505496, - "timestamp": 1.3568881635313716 - }, - { - "x": 2.4692307899396875, - "y": 5.846153901624162, - "heading": -3.1399999999999983, - "angularVelocity": -6.4386470058515105e-18, - "velocityX": -0.9208524495741806, - "velocityY": -2.4665690613594125, - "timestamp": 1.3919727134950495 - }, - { - "x": 2.440512838522467, - "y": 5.769230817470893, - "heading": -3.1399999999999983, - "angularVelocity": -6.438647005975929e-18, - "velocityX": -0.8185355504617052, - "velocityY": -2.1925059387367107, - "timestamp": 1.4270572634587273 - }, - { - "x": 2.4153846301042528, - "y": 5.7019231163506765, - "heading": -3.139999999999999, - "angularVelocity": -6.438647005965686e-18, - "velocityX": -0.7162186331085576, - "velocityY": -1.9184427672550652, - "timestamp": 1.4621418134224051 - }, - { - "x": 2.3938461651116887, - "y": 5.644230799406308, - "heading": -3.139999999999999, - "angularVelocity": -6.4386470057796256e-18, - "velocityX": -0.6139017035949621, - "velocityY": -1.6443795632007916, - "timestamp": 1.497226363386083 - }, - { - "x": 2.375897443849519, - "y": 5.59615386745407, - "heading": -3.1399999999999992, - "angularVelocity": -6.4386470058928764e-18, - "velocityX": -0.5115847653953328, - "velocityY": -1.370316335880356, - "timestamp": 1.5323109133497608 - }, - { - "x": 2.3615384665463046, - "y": 5.557692321106174, - "heading": -3.1399999999999992, - "angularVelocity": -6.4386470060770385e-18, - "velocityX": -0.40926782068117873, - "velocityY": -1.0962530911103003, - "timestamp": 1.5673954633134386 - }, - { - "x": 2.3507692333798134, - "y": 5.5288461608387856, - "heading": -3.1399999999999992, - "angularVelocity": -6.438647005993968e-18, - "velocityX": -0.30695087090017215, - "velocityY": -0.8221898327683184, - "timestamp": 1.6024800132771164 - }, - { - "x": 2.343589744492259, - "y": 5.509615387032838, - "heading": -3.1399999999999997, - "angularVelocity": -6.4386470056493156e-18, - "velocityX": -0.20463391706568382, - "velocityY": -0.548126563568796, - "timestamp": 1.6375645632407942 - }, - { - "x": 2.34, - "y": 5.5, - "heading": -3.1399999999999997, - "angularVelocity": -6.4386470059748625e-18, - "velocityX": -0.10231695991471057, - "velocityY": -0.2740632854858319, - "timestamp": 1.672649113204472 - }, - { - "x": 2.34, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -1.6096615234891419e-18, - "velocityX": -6.381688164787077e-19, - "velocityY": 1.1903732611956724e-19, - "timestamp": 1.70773366316815 - }, - { - "x": 2.345090910861112, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419150565163002e-20, - "velocityX": 0.20603079090412882, - "velocityY": 2.4617534447220064e-20, - "timestamp": 1.7324431293541118 - }, - { - "x": 2.355272732342947, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419155707964719e-20, - "velocityX": 0.4120615720795854, - "velocityY": 2.4617534049904126e-20, - "timestamp": 1.7571525955400737 - }, - { - "x": 2.370545464145016, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419131465999558e-20, - "velocityX": 0.6180923410942025, - "velocityY": 2.461753404990271e-20, - "timestamp": 1.7818620617260357 - }, - { - "x": 2.3909091058809784, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135838833232e-20, - "velocityX": 0.8241230944734556, - "velocityY": 2.461753404990924e-20, - "timestamp": 1.8065715279119976 - }, - { - "x": 2.416363657035711, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135956879892e-20, - "velocityX": 1.0301538270055572, - "velocityY": 2.4617534049916087e-20, - "timestamp": 1.8312809940979595 - }, - { - "x": 2.4469091168880444, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419147293142456e-20, - "velocityX": 1.2361845303516454, - "velocityY": 2.4617534049908892e-20, - "timestamp": 1.8559904602839215 - }, - { - "x": 2.482545484356221, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135953617887e-20, - "velocityX": 1.44221518991869, - "velocityY": 2.461753404989809e-20, - "timestamp": 1.8806999264698834 - }, - { - "x": 2.523272757637308, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419141237989167e-20, - "velocityX": 1.6482457765204706, - "velocityY": 2.4617534049904174e-20, - "timestamp": 1.9054093926558453 - }, - { - "x": 2.569090933125378, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135703365496e-20, - "velocityX": 1.8542762171892062, - "velocityY": 2.46175340521325e-20, - "timestamp": 1.9301188588418072 - }, - { - "x": 2.62, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419142174648979e-20, - "velocityX": 2.060306219951638, - "velocityY": 2.4617534049921363e-20, - "timestamp": 1.9548283250277692 - }, - { - "x": 2.670909066874622, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419138879259575e-20, - "velocityX": 2.060306219951638, - "velocityY": 2.4617534049891926e-20, - "timestamp": 1.979537791213731 - }, - { - "x": 2.716727242362692, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419136579202457e-20, - "velocityX": 1.8542762171892062, - "velocityY": 2.461753404988692e-20, - "timestamp": 2.004247257399693 - }, - { - "x": 2.757454515643779, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135961026862e-20, - "velocityX": 1.6482457765204708, - "velocityY": 2.461753404988646e-20, - "timestamp": 2.028956723585655 - }, - { - "x": 2.7930908831119554, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.41914544125096e-20, - "velocityX": 1.4422151899186901, - "velocityY": 2.461753404987526e-20, - "timestamp": 2.053666189771617 - }, - { - "x": 2.8236363429642886, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135953937812e-20, - "velocityX": 1.2361845303516454, - "velocityY": 2.4617534049904252e-20, - "timestamp": 2.078375655957579 - }, - { - "x": 2.8490908941190214, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135535582591e-20, - "velocityX": 1.0301538270055575, - "velocityY": 2.4617534049907063e-20, - "timestamp": 2.1030851221435407 - }, - { - "x": 2.8694545358549837, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419133272137074e-20, - "velocityX": 0.8241230944734556, - "velocityY": 2.4617534049360902e-20, - "timestamp": 2.1277945883295026 - }, - { - "x": 2.884727267657053, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419135144551174e-20, - "velocityX": 0.6180923410942025, - "velocityY": 2.4617534049906024e-20, - "timestamp": 2.1525040545154646 - }, - { - "x": 2.8949090891388876, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419134763293553e-20, - "velocityX": 0.4120615720795854, - "velocityY": 2.4617534049913036e-20, - "timestamp": 2.1772135207014265 - }, - { - "x": 2.9, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -9.419137756364966e-20, - "velocityX": 0.20603079090412882, - "velocityY": 2.461753421612352e-20, - "timestamp": 2.2019229868873884 - }, - { - "x": 2.9, - "y": 5.5, - "heading": -3.14, - "angularVelocity": -3.13968818863519e-20, - "velocityX": 4.3155351168940705e-21, - "velocityY": 1.2248960921424695e-20, - "timestamp": 2.2266324530733503 - }, - { - "x": 2.8964102554802387, - "y": 5.491025638700597, - "heading": -3.14, - "angularVelocity": -1.2978654042540716e-19, - "velocityX": -0.10543424148903582, - "velocityY": -0.2635856037225896, - "timestamp": 2.2606796892374628 - }, - { - "x": 2.889230766540635, - "y": 5.473076916351587, - "heading": -3.14, - "angularVelocity": -1.2978660186645256e-19, - "velocityX": -0.21086848004336445, - "velocityY": -0.5271712001084112, - "timestamp": 2.294726925401575 - }, - { - "x": 2.8784615333010906, - "y": 5.446153833252728, - "heading": -3.14, - "angularVelocity": -1.297865773332844e-19, - "velocityX": -0.31630271507604435, - "velocityY": -0.7907567876901109, - "timestamp": 2.3287741615656876 - }, - { - "x": 2.8641025559081537, - "y": 5.410256389770385, - "heading": -3.14, - "angularVelocity": -1.2978660189919096e-19, - "velocityX": -0.4217369458044869, - "velocityY": -1.0543423645112173, - "timestamp": 2.3628213977298 - }, - { - "x": 2.8461538345450084, - "y": 5.365384586362522, - "heading": -3.14, - "angularVelocity": -1.297866180488138e-19, - "velocityX": -0.5271711711526327, - "velocityY": -1.3179279278815819, - "timestamp": 2.3968686338939125 - }, - { - "x": 2.824615369447177, - "y": 5.311538423617943, - "heading": -3.14, - "angularVelocity": -1.2978656908936264e-19, - "velocityX": -0.6326053895832539, - "velocityY": -1.581513473958135, - "timestamp": 2.430915870058025 - }, - { - "x": 2.7994871609286895, - "y": 5.248717902321725, - "heading": -3.14, - "angularVelocity": -1.2978662840818847e-19, - "velocityX": -0.7380395987905088, - "velocityY": -1.8450989969762726, - "timestamp": 2.4649631062221373 - }, - { - "x": 2.7707692094291887, - "y": 5.176923023572972, - "heading": -3.14, - "angularVelocity": -1.2978655659771583e-19, - "velocityX": -0.843473795085051, - "velocityY": -2.108684487712628, - "timestamp": 2.4990103423862498 - }, - { - "x": 2.7384615156081376, - "y": 5.096153789020343, - "heading": -3.14, - "angularVelocity": -1.2978670299951993e-19, - "velocityX": -0.948907972010523, - "velocityY": -2.3722699300263077, - "timestamp": 2.533057578550362 - }, - { - "x": 2.7025640805646414, - "y": 5.0064102014116045, - "heading": -3.14, - "angularVelocity": -1.297866015976719e-19, - "velocityX": -1.054342116654202, - "velocityY": -2.6358552916355054, - "timestamp": 2.5671048147144746 - }, - { - "x": 2.6630769064969186, - "y": 4.907692266242297, - "heading": -3.14, - "angularVelocity": -1.297865385019268e-19, - "velocityX": -1.1597761967341322, - "velocityY": -2.8994404918353305, - "timestamp": 2.601152050878587 - }, - { - "x": 2.6199999999999997, - "y": 4.8, - "heading": -3.14, - "angularVelocity": -1.297866014682079e-19, - "velocityX": -1.2652100831116484, - "velocityY": -3.1630252077791217, - "timestamp": 2.6351992870426995 - }, - { - "x": 2.576923093503081, - "y": 4.692307733757704, - "heading": -3.14, - "angularVelocity": -1.297865958930724e-19, - "velocityX": -1.2652100831116484, - "velocityY": -3.1630252077791217, - "timestamp": 2.669246523206812 - }, - { - "x": 2.5374359194353584, - "y": 4.593589798588396, - "heading": -3.14, - "angularVelocity": -1.2978656547245638e-19, - "velocityX": -1.1597761967341322, - "velocityY": -2.8994404918353305, - "timestamp": 2.7032937593709243 - }, - { - "x": 2.501538484391862, - "y": 4.503846210979657, - "heading": -3.14, - "angularVelocity": -1.2978657690502228e-19, - "velocityX": -1.054342116654202, - "velocityY": -2.635855291635505, - "timestamp": 2.7373409955350367 - }, - { - "x": 2.469230790570811, - "y": 4.423076976427028, - "heading": -3.14, - "angularVelocity": -1.2978659288350122e-19, - "velocityX": -0.9489079720105228, - "velocityY": -2.3722699300263077, - "timestamp": 2.771388231699149 - }, - { - "x": 2.4405128390713102, - "y": 4.351282097678276, - "heading": -3.14, - "angularVelocity": -1.2978652179182088e-19, - "velocityX": -0.8434737950850509, - "velocityY": -2.108684487712628, - "timestamp": 2.8054354678632616 - }, - { - "x": 2.415384630552823, - "y": 4.2884615763820575, - "heading": -3.14, - "angularVelocity": -1.2978660602775546e-19, - "velocityX": -0.7380395987905087, - "velocityY": -1.845098996976272, - "timestamp": 2.839482704027374 - }, - { - "x": 2.3938461654549914, - "y": 4.234615413637479, - "heading": -3.14, - "angularVelocity": -1.29786602941321e-19, - "velocityX": -0.6326053895832537, - "velocityY": -1.5815134739581347, - "timestamp": 2.8735299401914864 - }, - { - "x": 2.375897444091846, - "y": 4.189743610229615, - "heading": -3.14, - "angularVelocity": -1.2978661855598955e-19, - "velocityX": -0.5271711711526327, - "velocityY": -1.3179279278815816, - "timestamp": 2.907577176355599 - }, - { - "x": 2.361538466698909, - "y": 4.153846166747273, - "heading": -3.14, - "angularVelocity": -1.2978660521209052e-19, - "velocityX": -0.42173694580448684, - "velocityY": -1.0543423645112173, - "timestamp": 2.9416244125197113 - }, - { - "x": 2.350769233459365, - "y": 4.126923083648412, - "heading": -3.14, - "angularVelocity": -1.2978660184555015e-19, - "velocityX": -0.31630271507604435, - "velocityY": -0.790756787690111, - "timestamp": 2.9756716486838237 - }, - { - "x": 2.343589744519761, - "y": 4.108974361299403, - "heading": -3.14, - "angularVelocity": -1.2978660079670228e-19, - "velocityX": -0.21086848004336442, - "velocityY": -0.5271712001084111, - "timestamp": 3.009718884847936 - }, - { - "x": 2.34, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -1.2978660200558231e-19, - "velocityX": -0.10543424148903584, - "velocityY": -0.2635856037225896, - "timestamp": 3.0437661210120486 - }, - { - "x": 2.34, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -3.2446650431779975e-20, - "velocityX": 9.479931176969037e-19, - "velocityY": 3.379108666761022e-19, - "timestamp": 3.077813357176161 - }, - { - "x": 2.34392391770176, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880321104e-21, - "velocityX": 0.18088156978903228, - "velocityY": 5.164414656858628e-21, - "timestamp": 3.0995066546543777 - }, - { - "x": 2.35177175293737, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177688e-21, - "velocityX": 0.361763131837868, - "velocityY": 5.164412389640512e-21, - "timestamp": 3.1211999521325944 - }, - { - "x": 2.363543505505338, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017764e-21, - "velocityX": 0.5426446845984692, - "velocityY": 5.164412389636511e-21, - "timestamp": 3.142893249610811 - }, - { - "x": 2.3792391751593946, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177688e-21, - "velocityX": 0.7235262260067893, - "velocityY": 5.164412389638332e-21, - "timestamp": 3.1645865470890278 - }, - { - "x": 2.398858761591705, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177688e-21, - "velocityX": 0.9044077532247774, - "velocityY": 5.1644123896339264e-21, - "timestamp": 3.1862798445672444 - }, - { - "x": 2.422402264406483, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017758e-21, - "velocityX": 1.0852892621981127, - "velocityY": 5.164412389631159e-21, - "timestamp": 3.207973142045461 - }, - { - "x": 2.449869683076017, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177634e-21, - "velocityX": 1.2661707468454422, - "velocityY": 5.164412389634155e-21, - "timestamp": 3.229666439523678 - }, - { - "x": 2.4812610168615277, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017767e-21, - "velocityX": 1.4470521974371036, - "velocityY": 5.164412389634393e-21, - "timestamp": 3.2513597370018945 - }, - { - "x": 2.5165762646549186, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017764e-21, - "velocityX": 1.6279335969486761, - "velocityY": 5.164412389641716e-21, - "timestamp": 3.273053034480111 - }, - { - "x": 2.555815424609839, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017764e-21, - "velocityX": 1.8088149113486538, - "velocityY": 5.164412389642343e-21, - "timestamp": 3.294746331958328 - }, - { - "x": 2.5989784930388242, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.47350688017764e-21, - "velocityX": 1.9896960557668744, - "velocityY": 5.164412389625726e-21, - "timestamp": 3.3164396294365446 - }, - { - "x": 2.6460654590311843, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506877552182e-21, - "velocityX": 2.170576697232967, - "velocityY": 5.1644123896641815e-21, - "timestamp": 3.3381329269147613 - }, - { - "x": 2.6899760454206136, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506878967645e-21, - "velocityX": 2.024154531302678, - "velocityY": 5.164412389654355e-21, - "timestamp": 3.359826224392978 - }, - { - "x": 2.7299627347151825, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177598e-21, - "velocityX": 1.8432739114337695, - "velocityY": 5.164412389647334e-21, - "timestamp": 3.3815195218711946 - }, - { - "x": 2.7660255155515054, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177748e-21, - "velocityX": 1.662392767744748, - "velocityY": 5.1644123896414225e-21, - "timestamp": 3.4032128193494113 - }, - { - "x": 2.798164384227858, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177586e-21, - "velocityX": 1.4815114534166427, - "velocityY": 5.164412389632274e-21, - "timestamp": 3.424906116827628 - }, - { - "x": 2.826379338896561, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177694e-21, - "velocityX": 1.3006300539157227, - "velocityY": 5.1644123896411035e-21, - "timestamp": 3.4465994143058447 - }, - { - "x": 2.8506703784493115, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177586e-21, - "velocityX": 1.1197486033251929, - "velocityY": 5.164412389640583e-21, - "timestamp": 3.4682927117840614 - }, - { - "x": 2.8710375021472916, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880177694e-21, - "velocityX": 0.938867118677168, - "velocityY": 5.164412389639504e-21, - "timestamp": 3.489986009262278 - }, - { - "x": 2.8874807094627832, - "y": 4.1, - "heading": -3.14, - "angularVelocity": -7.473506880249544e-21, - "velocityX": 0.7579856097028439, - "velocityY": 5.16441414397863e-21, - "timestamp": 3.5116793067404948 - }, - { - "x": 2.9, - "y": 4.1, - "heading": -3.14, - "angularVelocity": 2.876867495203466e-20, - "velocityX": 0.5771040824839183, - "velocityY": 1.1921469716669718e-18, - "timestamp": 3.5333726042187115 - }, - { - "x": 2.9123148891844215, - "y": 4.094450600680815, - "heading": -3.144693515931327, - "angularVelocity": -0.15297922368811448, - "velocityX": 0.40138825878138185, - "velocityY": -0.18087566170128103, - "timestamp": 3.564053345159689 - }, - { - "x": 2.91923423137959, - "y": 4.083354274189277, - "heading": -3.154053026200069, - "angularVelocity": -0.3050614157835113, - "velocityX": 0.22552721945274726, - "velocityY": -0.36167074689896445, - "timestamp": 3.594734086100667 - }, - { - "x": 2.9207529084640327, - "y": 4.066713875982502, - "heading": -3.1680469915350984, - "angularVelocity": -0.4561156251718416, - "velocityX": 0.049499361419069285, - "velocityY": -0.5423727620786116, - "timestamp": 3.6254148270416446 - }, - { - "x": 2.9168649241962763, - "y": 4.044532799937791, - "heading": -3.1866389973114897, - "angularVelocity": -0.605982945853825, - "velocityX": -0.12672393653189692, - "velocityY": -0.7229641581140979, - "timestamp": 3.6560955679826224 - }, - { - "x": 2.907563101292473, - "y": 4.016815193645298, - "heading": -3.2097864437326287, - "angularVelocity": -0.7544617799703365, - "velocityX": -0.3031811689847357, - "velocityY": -0.9034203686871307, - "timestamp": 3.6867763089236 - }, - { - "x": 2.8928386414063563, - "y": 3.983566263371193, - "heading": -3.2374384043709084, - "angularVelocity": -0.9012807315010896, - "velocityX": -0.47992517242146177, - "velocityY": -1.0837068875900944, - "timestamp": 3.717457049864578 - }, - { - "x": 2.8726804646309825, - "y": 3.944792720087984, - "heading": -3.2695320098745984, - "angularVelocity": -1.0460505359186205, - "velocityX": -0.6570303114306669, - "velocityY": -1.2637746708203776, - "timestamp": 3.7481377908055555 - }, - { - "x": 2.847074179279115, - "y": 3.900503463101137, - "heading": -3.3059862114623306, - "angularVelocity": -1.1881786576752227, - "velocityX": -0.8346045293080671, - "velocityY": -1.443552392429136, - "timestamp": 3.7788185317465333 - }, - { - "x": 2.816000393465176, - "y": 3.8507106993878955, - "heading": -3.346690735561191, - "angularVelocity": -1.326712551602523, - "velocityX": -1.0128108012031967, - "velocityY": -1.6229322430325475, - "timestamp": 3.809499272687511 - }, - { - "x": 2.779431754255908, - "y": 3.7954319510821866, - "heading": -3.3914856133189915, - "angularVelocity": -1.4600324628396322, - "velocityX": -1.1919086074100016, - "velocityY": -1.8017409818117416, - "timestamp": 3.8401800136284887 - }, - { - "x": 2.737327233217741, - "y": 3.73469413643622, - "heading": -3.440120133572727, - "angularVelocity": -1.585180760376562, - "velocityX": -1.372343683588534, - "velocityY": -1.9796723541589698, - "timestamp": 3.8708607545694664 - }, - { - "x": 2.6896194281741694, - "y": 3.6685434957014067, - "heading": -3.4921586426943634, - "angularVelocity": -1.6961294781552345, - "velocityX": -1.5549756485787085, - "velocityY": -2.1560965839146844, - "timestamp": 3.901541495510444 - }, - { - "x": 2.6361793432555767, - "y": 3.5970777414429556, - "heading": -3.546705756832886, - "angularVelocity": -1.7778942902147548, - "velocityX": -1.7418120710121818, - "velocityY": -2.329335996022185, - "timestamp": 3.932222236451422 - }, - { - "x": 2.576672175885399, - "y": 3.5206329890413772, - "heading": -3.601075431905856, - "angularVelocity": -1.7721108879855552, - "velocityX": -1.9395609605600816, - "velocityY": -2.491620151828787, - "timestamp": 3.9629029773923996 - }, - { - "x": 2.5100023278872166, - "y": 3.4449092310364833, - "heading": -3.6439610987413746, - "angularVelocity": -1.39780414423562, - "velocityX": -2.173019488885206, - "velocityY": -2.468120250112869, - "timestamp": 3.9935837183333773 - }, - { - "x": 2.4474481372182844, - "y": 3.3757853102065427, - "heading": -3.690631049746306, - "angularVelocity": -1.5211481070393027, - "velocityX": -2.038874836473845, - "velocityY": -2.2530068932467584, - "timestamp": 4.024264459274355 - }, - { - "x": 2.3898302517151278, - "y": 3.3127487578653034, - "heading": -3.7357876668192698, - "angularVelocity": -1.4718228989265274, - "velocityX": -1.8779822043411336, - "velocityY": -2.054596805941094, - "timestamp": 4.054945200215332 - }, - { - "x": 2.33731371478581, - "y": 3.255624545900845, - "heading": -3.7781142167580253, - "angularVelocity": -1.379580435172063, - "velocityX": -1.7117101907788617, - "velocityY": -1.8618915388761854, - "timestamp": 4.08562594115631 - }, - { - "x": 2.2899671288714094, - "y": 3.2043276525702034, - "heading": -3.817000216133743, - "angularVelocity": -1.2674400351192692, - "velocityX": -1.5432021672971912, - "velocityY": -1.671957448137406, - "timestamp": 4.116306682097287 - }, - { - "x": 2.2478279131933907, - "y": 3.158807852505574, - "heading": -3.8520921637880376, - "angularVelocity": -1.143777711294629, - "velocityX": -1.3734745115538192, - "velocityY": -1.4836603898256107, - "timestamp": 4.146987423038264 - }, - { - "x": 2.2109196804518123, - "y": 3.119031926923787, - "heading": -3.883159884935565, - "angularVelocity": -1.0126131310614077, - "velocityX": -1.2029772296758057, - "velocityY": -1.2964460557946098, - "timestamp": 4.1776681639792415 - }, - { - "x": 2.179258742299538, - "y": 3.084976250473309, - "heading": -3.910042236754404, - "angularVelocity": -0.8761963040773368, - "velocityX": -1.031948290074949, - "velocityY": -1.1100017602571317, - "timestamp": 4.208348904920219 - }, - { - "x": 2.152857068635237, - "y": 3.0566231569223294, - "heading": -3.9326205753539374, - "angularVelocity": -0.7359124293304734, - "velocityX": -0.8605292067454117, - "velocityY": -0.9241332732323445, - "timestamp": 4.239029645861196 - }, - { - "x": 2.1317238177985387, - "y": 3.0339589454084304, - "heading": -3.950804071446549, - "angularVelocity": -0.5926680886746454, - "velocityX": -0.688811618902993, - "velocityY": -0.7387113485133723, - "timestamp": 4.269710386802173 - }, - { - "x": 2.115866204010765, - "y": 3.0169726928739355, - "heading": -3.964520840386678, - "angularVelocity": -0.44708075878992104, - "velocityX": -0.5168588926284475, - "velocityY": -0.5536454470631014, - "timestamp": 4.300391127743151 - }, - { - "x": 2.105290024792999, - "y": 3.0056555016168804, - "heading": -3.9737122480968665, - "angularVelocity": -0.2995823252075419, - "velocityX": -0.34471720347667417, - "velocityY": -0.3688695549702323, - "timestamp": 4.331071868684128 - }, - { - "x": 2.1, - "y": 3, - "heading": -3.9783291104617295, - "angularVelocity": -0.15048079750566262, - "velocityX": -0.1724216766203777, - "velocityY": -0.18433393208332824, - "timestamp": 4.361752609625105 - }, - { - "x": 2.1, - "y": 3, - "heading": -3.9783291104617295, - "angularVelocity": -1.4227065376055964e-19, - "velocityX": -3.2613911006448295e-18, - "velocityY": -2.9118461532567482e-18, - "timestamp": 4.392433350566082 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/W2S.traj b/src/main/deploy/choreo/W2S.traj new file mode 100644 index 00000000..7d6997f6 --- /dev/null +++ b/src/main/deploy/choreo/W2S.traj @@ -0,0 +1,472 @@ +{ + "samples": [ + { + "x": 2.34, + "y": 5.5, + "heading": -3.14, + "angularVelocity": 2.8438778833914306e-22, + "velocityX": -5.403956856695926e-22, + "velocityY": -1.764496225422152e-21, + "timestamp": 0 + }, + { + "x": 2.345219454149142, + "y": 5.500680620832891, + "heading": -3.14015789834957, + "angularVelocity": -0.005897779171567437, + "velocityX": 0.19495572975659137, + "velocityY": 0.025422377009582938, + "timestamp": 0.026772509613639375 + }, + { + "x": 2.3556590020609622, + "y": 5.502036692864336, + "heading": -3.1404865067656247, + "angularVelocity": -0.01227409834927267, + "velocityX": 0.38993535019600484, + "velocityY": 0.050651659146463954, + "timestamp": 0.05354501922727875 + }, + { + "x": 2.371319433607059, + "y": 5.504061744738199, + "heading": -3.14100186046821, + "angularVelocity": -0.019249361005830575, + "velocityX": 0.5849444737193447, + "velocityY": 0.07563922482755309, + "timestamp": 0.08031752884091813 + }, + { + "x": 2.392201748509867, + "y": 5.50674744096323, + "heading": -3.1417246098705878, + "angularVelocity": -0.026995952669661743, + "velocityX": 0.7799909386219801, + "velocityY": 0.10031544535007665, + "timestamp": 0.1070900384545575 + }, + { + "x": 2.4183072518555147, + "y": 5.510082641059692, + "heading": -3.1426823480926416, + "angularVelocity": -0.03577319556048744, + "velocityX": 0.9750861507711613, + "velocityY": 0.12457554949432209, + "timestamp": 0.13386254806819686 + }, + { + "x": 2.4496377167718046, + "y": 5.514051697670015, + "heading": -3.143913819915893, + "angularVelocity": -0.04599762372011175, + "velocityX": 1.1702475923411, + "velocityY": 0.14825119750076746, + "timestamp": 0.16063505768183622 + }, + { + "x": 2.48619568337043, + "y": 5.518631029455087, + "heading": -3.145477394985239, + "angularVelocity": -0.058402260076100644, + "velocityX": 1.3655039115197647, + "velocityY": 0.17104604129973955, + "timestamp": 0.18740756729547559 + }, + { + "x": 2.527985060150862, + "y": 5.523781042535177, + "heading": -3.1474710387900213, + "angularVelocity": -0.07446607858410415, + "velocityX": 1.5609062199810606, + "velocityY": 0.19236198452856787, + "timestamp": 0.21418007690911495 + }, + { + "x": 2.5750124033881825, + "y": 5.529421473768213, + "heading": -3.1500933109792566, + "angularVelocity": -0.09794644682467382, + "velocityX": 1.7565534167690278, + "velocityY": 0.21067995919826019, + "timestamp": 0.2409525865227543 + }, + { + "x": 2.62728643823873, + "y": 5.535303378399583, + "heading": -3.1539624386682226, + "angularVelocity": -0.14451867773331506, + "velocityX": 1.9525265133873122, + "velocityY": 0.21969941242910496, + "timestamp": 0.26772509613639367 + }, + { + "x": 2.678521434738675, + "y": 5.537289220441695, + "heading": -3.171534912348283, + "angularVelocity": -0.6563625873574372, + "velocityX": 1.9137166160113463, + "velocityY": 0.07417466912029727, + "timestamp": 0.29449760575003303 + }, + { + "x": 2.724629958364521, + "y": 5.538116660747324, + "heading": -3.1901492341368907, + "angularVelocity": -0.6952774341007215, + "velocityX": 1.7222338992963062, + "velocityY": 0.030906340779074025, + "timestamp": 0.3212701153636724 + }, + { + "x": 2.7655630513066685, + "y": 5.537994392383535, + "heading": -3.209278140929457, + "angularVelocity": -0.7144980828700698, + "velocityX": 1.5289225228737546, + "velocityY": -0.004566936964587362, + "timestamp": 0.34804262497731175 + }, + { + "x": 2.8013067248822057, + "y": 5.536995150249463, + "heading": -3.228739854250013, + "angularVelocity": -0.7269289880333657, + "velocityX": 1.3350886447091723, + "velocityY": -0.03732343917295602, + "timestamp": 0.3748151345909511 + }, + { + "x": 2.8318544687583866, + "y": 5.535155887776591, + "heading": -3.248442340989224, + "angularVelocity": -0.7359222958005035, + "velocityX": 1.1410115942443573, + "velocityY": -0.06869966616553647, + "timestamp": 0.4015876442045905 + }, + { + "x": 2.857202537781273, + "y": 5.5324989630903465, + "heading": -3.268330011391853, + "angularVelocity": -0.7428392291060215, + "velocityX": 0.9467946557379203, + "velocityY": -0.09924077811861721, + "timestamp": 0.42836015381822984 + }, + { + "x": 2.8773485037250652, + "y": 5.529039358763453, + "heading": -3.288365650396495, + "angularVelocity": -0.7483661148611586, + "velocityX": 0.7524870187563222, + "velocityY": -0.1292222648089432, + "timestamp": 0.4551326634318692 + }, + { + "x": 2.8922906661715295, + "y": 5.524787814300188, + "heading": -3.308522599399848, + "angularVelocity": -0.7528972552159968, + "velocityX": 0.5581158681833853, + "velocityY": -0.15880261225486758, + "timestamp": 0.48190517304550856 + }, + { + "x": 2.9020277685918865, + "y": 5.519752404878458, + "heading": -3.3287808224995312, + "angularVelocity": -0.7566800196184369, + "velocityX": 0.3636977840656549, + "velocityY": -0.1880813377003599, + "timestamp": 0.5086776826591479 + }, + { + "x": 2.9065588448949846, + "y": 5.513939423347603, + "heading": -3.349124711283007, + "angularVelocity": -0.7598797825479614, + "velocityX": 0.16924361475585362, + "velocityY": -0.21712501423080047, + "timestamp": 0.5354501922727873 + }, + { + "x": 2.9058831293071905, + "y": 5.507353911633989, + "heading": -3.3695417632753553, + "angularVelocity": -0.7626125562000761, + "velocityX": -0.025239157536812964, + "velocityY": -0.24598036600421022, + "timestamp": 0.5622227018864268 + }, + { + "x": 2.9, + "y": 5.5, + "heading": -3.390021738749093, + "angularVelocity": -0.7649628581440181, + "velocityX": -0.21974515621028465, + "velocityY": -0.27468144526285576, + "timestamp": 0.5889952115000662 + }, + { + "x": 2.8873503491496635, + "y": 5.48418793643708, + "heading": -3.4159685355180973, + "angularVelocity": -0.7684647926624162, + "velocityX": -0.37464398416870504, + "velocityY": -0.4683049802108812, + "timestamp": 0.6227596689179652 + }, + { + "x": 2.869470623566405, + "y": 5.461838279458007, + "heading": -3.442033568339415, + "angularVelocity": -0.7719665830465854, + "velocityX": -0.5295428077508573, + "velocityY": -0.6619285096885713, + "timestamp": 0.6565241263358642 + }, + { + "x": 2.846360823424834, + "y": 5.432951029281044, + "heading": -3.4682168314242467, + "angularVelocity": -0.7754682019842498, + "velocityX": -0.6844416261615902, + "velocityY": -0.8555520327019875, + "timestamp": 0.6902885837537632 + }, + { + "x": 2.8180209489344645, + "y": 5.397526186168083, + "heading": -3.4945183177858, + "angularVelocity": -0.7789696139944609, + "velocityX": -0.8393404383671771, + "velocityY": -1.049175547958971, + "timestamp": 0.7240530411716622 + }, + { + "x": 2.7844510003513503, + "y": 5.355563750439189, + "heading": -3.5209380188430024, + "angularVelocity": -0.7824707718595512, + "velocityX": -0.9942392429892433, + "velocityY": -1.2427990537365536, + "timestamp": 0.7578174985895613 + }, + { + "x": 2.7456509779955414, + "y": 5.307063722494428, + "heading": -3.5474759238290736, + "angularVelocity": -0.7859716108455241, + "velocityX": -1.1491380381323866, + "velocityY": -1.4364225476654828, + "timestamp": 0.7915819560074603 + }, + { + "x": 2.701620882278521, + "y": 5.252026102848154, + "heading": -3.5741320188666457, + "angularVelocity": -0.7894720388262817, + "velocityX": -1.3040368210886484, + "velocityY": -1.63004602636081, + "timestamp": 0.8253464134253593 + }, + { + "x": 2.6523607137489367, + "y": 5.190450892186173, + "heading": -3.6009062854336653, + "angularVelocity": -0.792971918240461, + "velocityX": -1.4589355877956627, + "velocityY": -1.823669484744578, + "timestamp": 0.8591108708432583 + }, + { + "x": 2.5978704731749236, + "y": 5.122338091468658, + "heading": -3.6277986976154084, + "angularVelocity": -0.7964710301397292, + "velocityX": -1.6138343317528592, + "velocityY": -2.0172929146910734, + "timestamp": 0.8928753282611573 + }, + { + "x": 2.5381501617087743, + "y": 5.047687702135971, + "heading": -3.6548092166363553, + "angularVelocity": -0.7999689936266586, + "velocityX": -1.7687330415826792, + "velocityY": -2.2109163019783487, + "timestamp": 0.9266397856790564 + }, + { + "x": 2.4731997812712154, + "y": 4.9664997265890225, + "heading": -3.6819377781703686, + "angularVelocity": -0.8034650519700735, + "velocityX": -1.9236316945264456, + "velocityY": -2.4045396181580565, + "timestamp": 0.9604042430969554 + }, + { + "x": 2.403019335704405, + "y": 4.8787741696305105, + "heading": -3.709184254492685, + "angularVelocity": -0.8069573274964867, + "velocityX": -2.0785302336772222, + "velocityY": -2.598162792096527, + "timestamp": 0.9941687005148544 + }, + { + "x": 2.32760883653786, + "y": 4.784511045672329, + "heading": -3.736548266378328, + "angularVelocity": -0.8104383715384815, + "velocityX": -2.233428431358973, + "velocityY": -2.791785539198716, + "timestamp": 1.0279331579327533 + }, + { + "x": 2.249157766758394, + "y": 4.686447208447997, + "heading": -3.738325324519509, + "angularVelocity": -0.05263102910812686, + "velocityX": -2.323480836919306, + "velocityY": -2.9043510461491318, + "timestamp": 1.0616976153506523 + }, + { + "x": 2.1759367495246877, + "y": 4.594920936905864, + "heading": -3.7399833295422438, + "angularVelocity": -0.049105039723097314, + "velocityX": -2.1685826704530755, + "velocityY": -2.710728338066344, + "timestamp": 1.0954620727685513 + }, + { + "x": 2.107945796889999, + "y": 4.509932246112504, + "heading": -3.7415226557241197, + "angularVelocity": -0.045590135295933004, + "velocityX": -2.0136841470061912, + "velocityY": -2.5171051837577383, + "timestamp": 1.1292265301864504 + }, + { + "x": 2.045184912871734, + "y": 4.431481141089672, + "heading": -3.7429434277034694, + "angularVelocity": -0.042078922275138625, + "velocityX": -1.8587855045760215, + "velocityY": -2.3234818807200264, + "timestamp": 1.1629909876043494 + }, + { + "x": 1.9876540994785168, + "y": 4.359567624348151, + "heading": -3.7442457077566362, + "angularVelocity": -0.038569553689213554, + "velocityX": -1.7038868026565464, + "velocityY": -2.129858503320682, + "timestamp": 1.1967554450222484 + }, + { + "x": 1.9353533579154936, + "y": 4.294191697394372, + "heading": -3.74542953323058, + "angularVelocity": -0.0350612912060703, + "velocityX": -1.5489880650443397, + "velocityY": -1.9362350813054243, + "timestamp": 1.2305199024401474 + }, + { + "x": 1.888282688986081, + "y": 4.235353361232607, + "heading": -3.746494929013771, + "angularVelocity": -0.03155376584332283, + "velocityX": -1.3940893036373676, + "velocityY": -1.7426116295467093, + "timestamp": 1.2642843598580464 + }, + { + "x": 1.8464420932641423, + "y": 4.183052616580183, + "heading": -3.747441912878439, + "angularVelocity": -0.02804676683967347, + "velocityX": -1.2391905252343365, + "velocityY": -1.54898815654292, + "timestamp": 1.2980488172759455 + }, + { + "x": 1.8098315711800703, + "y": 4.137289463975094, + "heading": -3.7482704981506734, + "angularVelocity": -0.024540162514063414, + "velocityX": -1.0842917340843754, + "velocityY": -1.355364667605469, + "timestamp": 1.3318132746938445 + }, + { + "x": 1.7784511230686133, + "y": 4.098063903835773, + "heading": -3.748980695193332, + "angularVelocity": -0.021033865104619943, + "velocityX": -0.9293929330202054, + "velocityY": -1.1617411662752564, + "timestamp": 1.3655777321117435 + }, + { + "x": 1.7523007491975677, + "y": 4.065375936496967, + "heading": -3.7495725122955377, + "angularVelocity": -0.0175278131936343, + "velocityX": -0.7744941240247116, + "velocityY": -0.9681176550308892, + "timestamp": 1.3993421895296425 + }, + { + "x": 1.7313804497860397, + "y": 4.039225562232557, + "heading": -3.7500459562385835, + "angularVelocity": -0.014021962123839892, + "velocityX": -0.6195953085399809, + "velocityY": -0.7744941356749758, + "timestamp": 1.4331066469475415 + }, + { + "x": 1.7156902250166162, + "y": 4.019612781270777, + "heading": -3.750401032673127, + "angularVelocity": -0.010516278409242411, + "velocityX": -0.46469648764757093, + "velocityY": -0.5808706095594635, + "timestamp": 1.4668711043654405 + }, + { + "x": 1.7052300750437936, + "y": 4.006537593804749, + "heading": -3.7506377463802734, + "angularVelocity": -0.007010736296354315, + "velocityX": -0.30979766217944427, + "velocityY": -0.3872470777243053, + "timestamp": 1.5006355617833396 + }, + { + "x": 1.6999999999999975, + "y": 4.0000000000000036, + "heading": -3.7507561014580326, + "angularVelocity": -0.0035053155539891035, + "velocityX": -0.15489883278928215, + "velocityY": -0.19362354098660264, + "timestamp": 1.5344000192012386 + }, + { + "x": 1.7, + "y": 4, + "heading": -3.7507561014580326, + "angularVelocity": 1.6035161704104545e-21, + "velocityX": 7.414988771169268e-14, + "velocityY": -1.1459521982628032e-13, + "timestamp": 1.5681644766191376 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/W3S.traj b/src/main/deploy/choreo/W3S.traj new file mode 100644 index 00000000..8edb11a3 --- /dev/null +++ b/src/main/deploy/choreo/W3S.traj @@ -0,0 +1,238 @@ +{ + "samples": [ + { + "x": 1.7, + "y": 4, + "heading": 2.5324292057215536, + "angularVelocity": 9.836427314098696e-18, + "velocityX": 2.3386198211048914e-17, + "velocityY": 4.209145509360342e-19, + "timestamp": 0 + }, + { + "x": 1.726680446907769, + "y": 4, + "heading": 2.5507389446586037, + "angularVelocity": 0.30101879304795914, + "velocityX": 0.438636288254118, + "velocityY": -4.559667670299124e-19, + "timestamp": 0.06082589977675328 + }, + { + "x": 1.7800391101374584, + "y": 4, + "heading": 2.58742303958277, + "angularVelocity": 0.6030999139905887, + "velocityX": 0.8772359048625102, + "velocityY": -4.737105509825422e-19, + "timestamp": 0.12165179955350656 + }, + { + "x": 1.8600710399111293, + "y": 4, + "heading": 2.642613112610323, + "angularVelocity": 0.9073449505798018, + "velocityX": 1.3157541453144361, + "velocityY": -5.466125636924237e-19, + "timestamp": 0.18247769933025984 + }, + { + "x": 1.966768076798484, + "y": 4, + "heading": 2.7165199563471956, + "angularVelocity": 1.2150554945706835, + "velocityX": 1.7541382417532945, + "velocityY": -6.537740639145266e-19, + "timestamp": 0.24330359910701313 + }, + { + "x": 2.100119086392268, + "y": 4, + "heading": 2.809427430936263, + "angularVelocity": 1.5274328029470334, + "velocityX": 2.1923392844717964, + "velocityY": -7.370999188984064e-19, + "timestamp": 0.3041294988837664 + }, + { + "x": 2.2601117640496655, + "y": 4, + "heading": 2.921646905870359, + "angularVelocity": 1.8449291394637872, + "velocityX": 2.630338034370403, + "velocityY": -5.778823319330962e-19, + "timestamp": 0.3649553986605197 + }, + { + "x": 2.4013515665186476, + "y": 4, + "heading": 3.0051161246453697, + "angularVelocity": 1.3722644314788166, + "velocityX": 2.3220339195494386, + "velocityY": -6.643266058953634e-19, + "timestamp": 0.42578129843727297 + }, + { + "x": 2.515959575579507, + "y": 4, + "heading": 3.0690228968495292, + "angularVelocity": 1.0506506675560594, + "velocityX": 1.8841975125942059, + "velocityY": -5.253201411023923e-19, + "timestamp": 0.48660719821402626 + }, + { + "x": 2.6039369458093504, + "y": 4, + "heading": 3.1133381061427943, + "angularVelocity": 0.7285582203767191, + "velocityX": 1.4463800873074064, + "velocityY": -5.128797583419745e-19, + "timestamp": 0.5474330979907795 + }, + { + "x": 2.665283787360706, + "y": 4, + "heading": 3.138058308493628, + "angularVelocity": 0.40640915206282496, + "velocityX": 1.0085644729682035, + "velocityY": -5.254834134379204e-19, + "timestamp": 0.6082589977675328 + }, + { + "x": 2.7, + "y": 4, + "heading": 3.143185307179586, + "angularVelocity": 0.08428973029097245, + "velocityX": 0.5707472107523717, + "velocityY": -6.37300209887089e-17, + "timestamp": 0.669084897544286 + }, + { + "x": 2.7133851131067677, + "y": 3.977058292663326, + "heading": 3.131348732424157, + "angularVelocity": -0.17128774031052524, + "velocityX": 0.19369672604995564, + "velocityY": -0.33199073968173415, + "timestamp": 0.7381883531371755 + }, + { + "x": 2.7007159790989697, + "y": 3.9311730899537207, + "heading": 3.1018591119354357, + "angularVelocity": -0.42674595990012093, + "velocityX": -0.18333575215041678, + "velocityY": -0.6640073541374039, + "timestamp": 0.8072918087300649 + }, + { + "x": 2.66199484115675, + "y": 3.862341706815855, + "heading": 3.054718737972702, + "angularVelocity": -0.6821710080767611, + "velocityX": -0.5603357691638745, + "velocityY": -0.9960628241807469, + "timestamp": 0.8763952643229543 + }, + { + "x": 2.597225247659028, + "y": 3.770561041115099, + "heading": 2.989905863580779, + "angularVelocity": -0.9379107576676369, + "velocityX": -0.9372844373850924, + "velocityY": -1.3281631853901548, + "timestamp": 0.9454987199158438 + }, + { + "x": 2.506412179488274, + "y": 3.655828456407957, + "heading": 2.907352350125962, + "angularVelocity": -1.1946365452673091, + "velocityX": -1.3141610269737936, + "velocityY": -1.6603016987236956, + "timestamp": 1.014602175508733 + }, + { + "x": 2.3895616650846327, + "y": 3.5181430948085093, + "heading": 2.8069247500054657, + "angularVelocity": -1.4532934606482912, + "velocityX": -1.6909503786622855, + "velocityY": -1.992452626576132, + "timestamp": 1.0837056311016224 + }, + { + "x": 2.246679523391877, + "y": 3.357507378978086, + "heading": 2.68842324173939, + "angularVelocity": -1.7148420039290893, + "velocityX": -2.067655524077823, + "velocityY": -2.324568495970117, + "timestamp": 1.1528090866945118 + }, + { + "x": 2.090491509437375, + "y": 3.2199148203355916, + "heading": 2.5775020951989167, + "angularVelocity": -1.6051461621820693, + "velocityX": -2.2602055514869797, + "velocityY": -1.9911096696716033, + "timestamp": 1.221912542287401 + }, + { + "x": 1.960331483425684, + "y": 3.1052621757182224, + "heading": 2.4849774188469222, + "angularVelocity": -1.3389298054169396, + "velocityX": -1.8835530712188058, + "velocityY": -1.6591448811266607, + "timestamp": 1.2910159978802904 + }, + { + "x": 1.8562006631201746, + "y": 3.0135445544840174, + "heading": 2.4109267688757035, + "angularVelocity": -1.071591128618647, + "velocityX": -1.5068829686377219, + "velocityY": -1.3272508653263182, + "timestamp": 1.3601194534731798 + }, + { + "x": 1.7781008483685374, + "y": 2.944758434687115, + "heading": 2.3553834808519016, + "angularVelocity": -0.8037700509556753, + "velocityX": -1.1301868203624321, + "velocityY": -0.995407815799475, + "timestamp": 1.429222909066069 + }, + { + "x": 1.7260336446967814, + "y": 2.8989016512903865, + "heading": 2.3183561927552425, + "angularVelocity": -0.5358239726007007, + "velocityX": -0.7534674384313609, + "velocityY": -0.6635960966451635, + "timestamp": 1.4983263646589584 + }, + { + "x": 1.7000000000000002, + "y": 2.875973224639892, + "heading": 2.299843907718641, + "angularVelocity": -0.2678923199651603, + "velocityX": -0.3767343394579128, + "velocityY": -0.3317985541171716, + "timestamp": 1.5674298202518477 + }, + { + "x": 1.7, + "y": 2.8759732246398926, + "heading": 2.299843907718641, + "angularVelocity": 9.076953787892685e-18, + "velocityX": -1.1997312285823622e-16, + "velocityY": 1.5409057838224231e-16, + "timestamp": 1.636533275844737 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SW1SW2SW3S.auto b/src/main/deploy/pathplanner/autos/SW1SW2SW3S.auto new file mode 100644 index 00000000..c83648bb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SW1SW2SW3S.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.5, + "y": 7 + }, + "rotation": -124.90479933851948 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SW1S" + } + }, + { + "type": "path", + "data": { + "pathName": "W2S" + } + }, + { + "type": "path", + "data": { + "pathName": "W3S" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file