From 14a6458c584bf7a71a1d2bd001c37fe2d114ab87 Mon Sep 17 00:00:00 2001 From: Nate Laverdure <41549690+nlaverdure@users.noreply.github.com> Date: Sat, 1 Jun 2024 08:31:14 -0400 Subject: [PATCH] District Championship changes (#176) * auto position changes. Added movement to the pieces while raising arm to fix issue of piece being stuck (untested) * Dcm pfixing auto intaking (#174) * changed command structure off intake and raising arm command * changed auto to start after intake command during rase arm to insert it into preveous cycle. FIxed issue where the intake was "too fast" and reflective sensor was triggering seemingly earlier in auto * spot :/ * moved blue far piece towards the wall to avoid piece * auto corrections and stopped rollers on arm raised * Add custom movement for auto (#175) * added new constants for auto to change how intake works * added new auto (match tested ready to merge) * spot :/ (go merge) * finals, mirrored auto, changed auto intake height, added auto to pick up far piece and return but stop to avoid teamates. Improved spit out 0.3 second and reduced piece to floor speed --------- Co-authored-by: bot3O3 <75286864+bot3O3@users.noreply.github.com> --- src/main/deploy/choreo/B-driveFwd2m.1.traj | 162 +- src/main/deploy/choreo/B-driveFwd2m.traj | 162 +- .../deploy/choreo/B_PickUpFarPiece1.1.traj | 902 +-- src/main/deploy/choreo/B_PickUpFarPiece1.traj | 902 +-- .../choreo/B_PickUpFarPiece1AfterNear.1.traj | 847 +-- .../choreo/B_PickUpFarPiece1AfterNear.traj | 847 +-- .../deploy/choreo/B_PickUpNearPiece.1.traj | 514 +- src/main/deploy/choreo/B_PickUpNearPiece.traj | 514 +- .../choreo/B_ReturnFromFarPiece1.1.traj | 1051 ++-- .../deploy/choreo/B_ReturnFromFarPiece1.traj | 1051 ++-- .../choreo/B_ReturnFromFarwithStop.1.traj | 166 + .../choreo/B_ReturnFromFarwithStop.traj | 166 + .../deploy/choreo/R_PickUpFarPiece1.1.traj | 1206 ++-- src/main/deploy/choreo/R_PickUpFarPiece1.traj | 1206 ++-- .../choreo/R_ReturnFromFarwithStop.1.traj | 310 + .../choreo/R_ReturnFromFarwithStop.traj | 310 + .../pathplanner/autos/B-TwoPieceFar1.auto | 24 + src/main/deploy/pathplanner/autos/Blue5.auto | 75 + src/main/deploy/pathplanner/autos/Red5.auto | 75 + src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 46 +- src/main/java/frc/robot/intake/Intake.java | 21 +- src/main/omicron.chor | 5343 +++++++++-------- 23 files changed, 8848 insertions(+), 7055 deletions(-) create mode 100644 src/main/deploy/choreo/B_ReturnFromFarwithStop.1.traj create mode 100644 src/main/deploy/choreo/B_ReturnFromFarwithStop.traj create mode 100644 src/main/deploy/choreo/R_ReturnFromFarwithStop.1.traj create mode 100644 src/main/deploy/choreo/R_ReturnFromFarwithStop.traj create mode 100644 src/main/deploy/pathplanner/autos/Blue5.auto create mode 100644 src/main/deploy/pathplanner/autos/Red5.auto diff --git a/src/main/deploy/choreo/B-driveFwd2m.1.traj b/src/main/deploy/choreo/B-driveFwd2m.1.traj index 8baae7e..737ff9c 100644 --- a/src/main/deploy/choreo/B-driveFwd2m.1.traj +++ b/src/main/deploy/choreo/B-driveFwd2m.1.traj @@ -1,112 +1,112 @@ { "samples": [ { - "x": 0.9999999999999971, - "y": 2.0002646446228027, - "heading": 0.8049567558943416, - "angularVelocity": -8.882383616047711e-33, - "velocityX": 2.570897453430358e-16, - "velocityY": -3.401870775942321e-20, + "x": 1, + "y": 2, + "heading": 0, + "angularVelocity": -2.167255414929253e-35, + "velocityX": 0, + "velocityY": 1.0729237673159813e-36, "timestamp": 0 }, { - "x": 1.0666666719605324, - "y": 2.000255823134675, - "heading": 0.8049567558943416, - "angularVelocity": 3.396420718128266e-27, - "velocityX": 0.7268597690543107, - "velocityY": -0.00009617976470591548, - "timestamp": 0.09171875346361454 + "x": 1.066666671960537, + "y": 2, + "heading": -2.2064159489642693e-18, + "angularVelocity": -2.40562799160236e-17, + "velocityX": 0.726859772235986, + "velocityY": 1.2414751413753207e-30, + "timestamp": 0.09171875306213341 }, { - "x": 1.2000000138981668, - "y": 2.000238180158683, - "heading": 0.8049567558943416, - "angularVelocity": -1.124978050975361e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655031845, - "timestamp": 0.18343750692722907 + "x": 1.2000000138981721, + "y": 2, + "heading": -6.238939336710584e-18, + "angularVelocity": -4.3966098884338405e-17, + "velocityX": 1.453719522846644, + "velocityY": 4.076264826156204e-31, + "timestamp": 0.18343750612426682 }, { - "x": 1.4000000225084717, - "y": 2.0002117156952637, - "heading": 0.8049567558943416, - "angularVelocity": -9.05920074487619e-28, - "velocityX": 2.1805792278843166, - "velocityY": -0.0002885392836274193, - "timestamp": 0.2751562603908436 + "x": 1.4000000225084763, + "y": 2, + "heading": -1.1463424269988706e-17, + "angularVelocity": -5.696189642582716e-17, + "velocityX": 2.1805792374293405, + "velocityY": -4.260497847820802e-31, + "timestamp": 0.27515625918640024 }, { - "x": 1.6666666911827144, - "y": 2.000176429745291, - "heading": 0.8049567558943416, - "angularVelocity": 1.6037111849531954e-26, - "velocityX": 2.907438867230956, - "velocityY": -0.0003847190311701196, - "timestamp": 0.36687501385445814 + "x": 1.666666691182717, + "y": 2, + "heading": -1.660927541991535e-17, + "angularVelocity": -5.610453586198502e-17, + "velocityX": 2.9074388799576525, + "velocityY": -1.259681057530857e-30, + "timestamp": 0.36687501224853364 }, { - "x": 2.0000000000000013, - "y": 2.0001323223114014, - "heading": 0.8049567558943416, - "angularVelocity": 3.720423796149838e-26, - "velocityX": 3.634298289383412, - "velocityY": -0.00048089874997318956, - "timestamp": 0.45859376731807266 + "x": 2.0000000000000004, + "y": 2, + "heading": -1.7865129734989053e-17, + "angularVelocity": -1.3692311810090882e-17, + "velocityX": 3.6342983052917806, + "velocityY": -2.0941736191227216e-30, + "timestamp": 0.45859376531066703 }, { - "x": 2.3333333088172883, - "y": 2.0000882148775116, - "heading": 0.8049567558943416, - "angularVelocity": 1.815590327118958e-26, - "velocityX": 3.6342982893834117, - "velocityY": -0.0004808987499730965, - "timestamp": 0.5503125207816872 + "x": 2.333333308817284, + "y": 2, + "heading": -2.614419331441905e-18, + "angularVelocity": 1.6627656030926103e-16, + "velocityX": 3.6342983052917806, + "velocityY": -7.668721594358735e-31, + "timestamp": 0.5503125183728005 }, { - "x": 2.5999999774915312, - "y": 2.000052928927539, - "heading": 0.8049567558943416, - "angularVelocity": -2.3621475473176443e-26, - "velocityX": 2.9074388672309555, - "velocityY": -0.0003847190311700582, - "timestamp": 0.6420312742453018 + "x": 2.599999977491524, + "y": 2, + "heading": 1.3547937651055734e-18, + "angularVelocity": 4.327579132505987e-17, + "velocityX": 2.907438879957653, + "velocityY": 6.696205895893734e-32, + "timestamp": 0.6420312714349339 }, { - "x": 2.799999986101836, - "y": 2.0000264644641197, - "heading": 0.8049567558943416, - "angularVelocity": 5.783084016216911e-28, - "velocityX": 2.180579227884316, - "velocityY": -0.00028853928362737873, - "timestamp": 0.7337500277089164 + "x": 2.799999986101828, + "y": 2, + "heading": 1.6633017363201664e-18, + "angularVelocity": 3.3635853370126346e-18, + "velocityX": 2.1805792374293405, + "velocityY": 9.022977438111278e-31, + "timestamp": 0.7337500244970673 }, { - "x": 2.9333333280394704, - "y": 2.0000088214881275, - "heading": 0.8049567558943416, - "angularVelocity": -2.351348677796648e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655029408, - "timestamp": 0.825468781172531 + "x": 2.933333328039463, + "y": 2, + "heading": 8.002561852421958e-19, + "angularVelocity": -9.409705950343789e-18, + "velocityX": 1.453719522846644, + "velocityY": 1.4913843713845408e-30, + "timestamp": 0.8254687775592007 }, { - "x": 3.0000000000000058, + "x": 3, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -5.899958075775622e-27, - "velocityX": 0.7268597690543105, - "velocityY": -0.00009617976470590487, - "timestamp": 0.9171875346361456 + "heading": 0, + "angularVelocity": -8.725108122724779e-18, + "velocityX": 0.726859772235986, + "velocityY": 4.284468199881695e-31, + "timestamp": 0.9171875306213341 }, { - "x": 3.000000000000003, + "x": 3, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -1.1372522907103163e-26, - "velocityX": -1.0731025459966149e-17, - "velocityY": 1.4199561562482062e-21, - "timestamp": 1.0089062880997601 + "heading": 0, + "angularVelocity": 0, + "velocityX": -1.88079096131566e-37, + "velocityY": 0, + "timestamp": 1.0089062836834675 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B-driveFwd2m.traj b/src/main/deploy/choreo/B-driveFwd2m.traj index 8baae7e..737ff9c 100644 --- a/src/main/deploy/choreo/B-driveFwd2m.traj +++ b/src/main/deploy/choreo/B-driveFwd2m.traj @@ -1,112 +1,112 @@ { "samples": [ { - "x": 0.9999999999999971, - "y": 2.0002646446228027, - "heading": 0.8049567558943416, - "angularVelocity": -8.882383616047711e-33, - "velocityX": 2.570897453430358e-16, - "velocityY": -3.401870775942321e-20, + "x": 1, + "y": 2, + "heading": 0, + "angularVelocity": -2.167255414929253e-35, + "velocityX": 0, + "velocityY": 1.0729237673159813e-36, "timestamp": 0 }, { - "x": 1.0666666719605324, - "y": 2.000255823134675, - "heading": 0.8049567558943416, - "angularVelocity": 3.396420718128266e-27, - "velocityX": 0.7268597690543107, - "velocityY": -0.00009617976470591548, - "timestamp": 0.09171875346361454 + "x": 1.066666671960537, + "y": 2, + "heading": -2.2064159489642693e-18, + "angularVelocity": -2.40562799160236e-17, + "velocityX": 0.726859772235986, + "velocityY": 1.2414751413753207e-30, + "timestamp": 0.09171875306213341 }, { - "x": 1.2000000138981668, - "y": 2.000238180158683, - "heading": 0.8049567558943416, - "angularVelocity": -1.124978050975361e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655031845, - "timestamp": 0.18343750692722907 + "x": 1.2000000138981721, + "y": 2, + "heading": -6.238939336710584e-18, + "angularVelocity": -4.3966098884338405e-17, + "velocityX": 1.453719522846644, + "velocityY": 4.076264826156204e-31, + "timestamp": 0.18343750612426682 }, { - "x": 1.4000000225084717, - "y": 2.0002117156952637, - "heading": 0.8049567558943416, - "angularVelocity": -9.05920074487619e-28, - "velocityX": 2.1805792278843166, - "velocityY": -0.0002885392836274193, - "timestamp": 0.2751562603908436 + "x": 1.4000000225084763, + "y": 2, + "heading": -1.1463424269988706e-17, + "angularVelocity": -5.696189642582716e-17, + "velocityX": 2.1805792374293405, + "velocityY": -4.260497847820802e-31, + "timestamp": 0.27515625918640024 }, { - "x": 1.6666666911827144, - "y": 2.000176429745291, - "heading": 0.8049567558943416, - "angularVelocity": 1.6037111849531954e-26, - "velocityX": 2.907438867230956, - "velocityY": -0.0003847190311701196, - "timestamp": 0.36687501385445814 + "x": 1.666666691182717, + "y": 2, + "heading": -1.660927541991535e-17, + "angularVelocity": -5.610453586198502e-17, + "velocityX": 2.9074388799576525, + "velocityY": -1.259681057530857e-30, + "timestamp": 0.36687501224853364 }, { - "x": 2.0000000000000013, - "y": 2.0001323223114014, - "heading": 0.8049567558943416, - "angularVelocity": 3.720423796149838e-26, - "velocityX": 3.634298289383412, - "velocityY": -0.00048089874997318956, - "timestamp": 0.45859376731807266 + "x": 2.0000000000000004, + "y": 2, + "heading": -1.7865129734989053e-17, + "angularVelocity": -1.3692311810090882e-17, + "velocityX": 3.6342983052917806, + "velocityY": -2.0941736191227216e-30, + "timestamp": 0.45859376531066703 }, { - "x": 2.3333333088172883, - "y": 2.0000882148775116, - "heading": 0.8049567558943416, - "angularVelocity": 1.815590327118958e-26, - "velocityX": 3.6342982893834117, - "velocityY": -0.0004808987499730965, - "timestamp": 0.5503125207816872 + "x": 2.333333308817284, + "y": 2, + "heading": -2.614419331441905e-18, + "angularVelocity": 1.6627656030926103e-16, + "velocityX": 3.6342983052917806, + "velocityY": -7.668721594358735e-31, + "timestamp": 0.5503125183728005 }, { - "x": 2.5999999774915312, - "y": 2.000052928927539, - "heading": 0.8049567558943416, - "angularVelocity": -2.3621475473176443e-26, - "velocityX": 2.9074388672309555, - "velocityY": -0.0003847190311700582, - "timestamp": 0.6420312742453018 + "x": 2.599999977491524, + "y": 2, + "heading": 1.3547937651055734e-18, + "angularVelocity": 4.327579132505987e-17, + "velocityX": 2.907438879957653, + "velocityY": 6.696205895893734e-32, + "timestamp": 0.6420312714349339 }, { - "x": 2.799999986101836, - "y": 2.0000264644641197, - "heading": 0.8049567558943416, - "angularVelocity": 5.783084016216911e-28, - "velocityX": 2.180579227884316, - "velocityY": -0.00028853928362737873, - "timestamp": 0.7337500277089164 + "x": 2.799999986101828, + "y": 2, + "heading": 1.6633017363201664e-18, + "angularVelocity": 3.3635853370126346e-18, + "velocityX": 2.1805792374293405, + "velocityY": 9.022977438111278e-31, + "timestamp": 0.7337500244970673 }, { - "x": 2.9333333280394704, - "y": 2.0000088214881275, - "heading": 0.8049567558943416, - "angularVelocity": -2.351348677796648e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655029408, - "timestamp": 0.825468781172531 + "x": 2.933333328039463, + "y": 2, + "heading": 8.002561852421958e-19, + "angularVelocity": -9.409705950343789e-18, + "velocityX": 1.453719522846644, + "velocityY": 1.4913843713845408e-30, + "timestamp": 0.8254687775592007 }, { - "x": 3.0000000000000058, + "x": 3, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -5.899958075775622e-27, - "velocityX": 0.7268597690543105, - "velocityY": -0.00009617976470590487, - "timestamp": 0.9171875346361456 + "heading": 0, + "angularVelocity": -8.725108122724779e-18, + "velocityX": 0.726859772235986, + "velocityY": 4.284468199881695e-31, + "timestamp": 0.9171875306213341 }, { - "x": 3.000000000000003, + "x": 3, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -1.1372522907103163e-26, - "velocityX": -1.0731025459966149e-17, - "velocityY": 1.4199561562482062e-21, - "timestamp": 1.0089062880997601 + "heading": 0, + "angularVelocity": 0, + "velocityX": -1.88079096131566e-37, + "velocityY": 0, + "timestamp": 1.0089062836834675 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj b/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj index 58c0ba4..14191bb 100644 --- a/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj +++ b/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj @@ -1,499 +1,499 @@ { "samples": [ { - "x": 1.9, + "x": 1.8999999999999968, "y": 7.9, "heading": -1.571, - "angularVelocity": 2.300332094481187e-29, - "velocityX": -4.0491893744801466e-20, - "velocityY": -1.3926499461964718e-19, + "angularVelocity": 4.470498597716865e-29, + "velocityX": 2.7707804022206673e-16, + "velocityY": 1.8739023107890902e-17, "timestamp": 0 }, { - "x": 1.9154424529969702, - "y": 7.8954006953331355, + "x": 1.914365871743733, + "y": 7.899792228900449, "heading": -1.571, - "angularVelocity": -7.879169842980081e-20, - "velocityX": 0.34247331200662146, - "velocityY": -0.10200057610651515, - "timestamp": 0.04509096754573317 + "angularVelocity": 4.2271636316208514e-19, + "velocityX": 0.33739566619749, + "velocityY": -0.004879694723755523, + "timestamp": 0.042578708569808256 }, { - "x": 1.9463273585735072, - "y": 7.886202086121636, + "x": 1.9430976157693656, + "y": 7.899376752477764, "heading": -1.571, - "angularVelocity": -7.879169825285715e-20, - "velocityX": 0.6849466147563192, - "velocityY": -0.2040011495022858, - "timestamp": 0.09018193509146634 + "angularVelocity": 4.227163636037315e-19, + "velocityX": 0.6747913450340882, + "velocityY": -0.009757844628066633, + "timestamp": 0.08515741713961651 }, { - "x": 1.9926547162078563, - "y": 7.872404172518289, + "x": 1.986195232749876, + "y": 7.898753653015407, "heading": -1.571, - "angularVelocity": -7.879169828739293e-20, - "velocityX": 1.0274199059348594, - "velocityY": -0.30600171950962485, - "timestamp": 0.13527290263719952 + "angularVelocity": 4.227163633163533e-19, + "velocityX": 1.0121870396762844, + "velocityY": -0.014634062029792581, + "timestamp": 0.12773612570942477 }, { - "x": 2.0544245252291895, - "y": 7.8540069547195355, + "x": 2.0436587235509935, + "y": 7.897923036409323, "heading": -1.571, - "angularVelocity": -7.87916983435114e-20, - "velocityX": 1.3698931822361922, - "velocityY": -0.40800228516040743, - "timestamp": 0.18036387018293268 + "angularVelocity": 4.227163634858386e-19, + "velocityX": 1.349582754650927, + "velocityY": -0.019507792368223267, + "timestamp": 0.17031483427923302 }, { - "x": 2.13163678474307, - "y": 7.8310104329872985, + "x": 2.115488089327842, + "y": 7.896885044034559, "heading": -1.571, - "angularVelocity": -7.879169829580418e-20, - "velocityX": 1.7123664387012403, - "velocityY": -0.5100028450024455, - "timestamp": 0.22545483772866584 + "angularVelocity": 4.2271636331977046e-19, + "velocityX": 1.6869784967546948, + "velocityY": -0.024378202384055483, + "timestamp": 0.21289354284904127 }, { - "x": 2.2242914934972844, - "y": 7.8034146076882696, + "x": 2.2016833316991637, + "y": 7.895639874163583, "heading": -1.571, - "angularVelocity": -7.879169836864827e-20, - "velocityX": 2.0548396673954747, - "velocityY": -0.6120033967122365, - "timestamp": 0.27054580527439903 + "angularVelocity": 4.2271636334033824e-19, + "velocityX": 2.024374276876126, + "velocityY": -0.029243955789217388, + "timestamp": 0.25547225141884955 }, { - "x": 2.332388649613512, - "y": 7.771219479372483, + "x": 2.302244453096514, + "y": 7.894187824967971, "heading": -1.571, - "angularVelocity": -7.87916983823685e-20, - "velocityX": 2.397312854433457, - "velocityY": -0.7140039362236463, - "timestamp": 0.3156367728201322 + "angularVelocity": 4.227163633528535e-19, + "velocityX": 2.361770114104849, + "velocityY": -0.03410270636161241, + "timestamp": 0.2980509599886578 }, { - "x": 2.455928249961216, - "y": 7.73442504895667, + "x": 2.4171714575816776, + "y": 7.8925293954471245, "heading": -1.571, - "angularVelocity": -7.879169832615412e-20, - "velocityX": 2.739785972044277, - "velocityY": -0.8160044554043961, - "timestamp": 0.36072774036586536 + "angularVelocity": 4.227163633894554e-19, + "velocityX": 2.699166046728326, + "velocityY": -0.03894973747556378, + "timestamp": 0.3406296685584661 }, { - "x": 2.5949102882793076, - "y": 7.693031318274292, + "x": 2.5464643533128632, + "y": 7.890665591362613, "heading": -1.571, - "angularVelocity": -7.879169827922094e-20, - "velocityX": 3.082258950800517, - "velocityY": -0.9180049339237416, - "timestamp": 0.4058187079115985 + "angularVelocity": 4.2271636346276787e-19, + "velocityX": 3.0365621709548005, + "velocityY": -0.04377314735710882, + "timestamp": 0.3832083771282744 }, { - "x": 2.749334745784455, - "y": 7.647038292825759, + "x": 2.6901231650458177, + "y": 7.888599493997031, "heading": -1.571, - "angularVelocity": -7.879169832843695e-20, - "velocityX": 3.424731512991456, - "velocityY": -1.0200052904583357, - "timestamp": 0.4509096754573317 - }, - { - "x": 2.907, - "y": 7.6, - "heading": -1.571, - "angularVelocity": -2.3912453364100895e-19, - "velocityX": 3.496603927507973, - "velocityY": -1.0431865933693085, - "timestamp": 0.49600064300306484 - }, - { - "x": 3.0409750518324072, - "y": 7.55855968097087, - "heading": -1.5404817853182164, - "angularVelocity": 0.7344477642220753, - "velocityX": 3.2242278359283354, - "velocityY": -0.9972978425163852, - "timestamp": 0.5375532437031425 - }, - { - "x": 3.167821917464271, - "y": 7.522503707879653, - "heading": -1.488578715495742, - "angularVelocity": 1.2490931722204932, - "velocityX": 3.052681745420317, - "velocityY": -0.8677188066147962, - "timestamp": 0.5791058444032202 - }, - { - "x": 3.2926667162009906, - "y": 7.486533976140662, - "heading": -1.4329122238837848, - "angularVelocity": 1.3396632382593825, - "velocityX": 3.004500239054416, - "velocityY": -0.8656433323781126, - "timestamp": 0.6206584451032979 - }, - { - "x": 3.4110148334033306, - "y": 7.453039903093125, - "heading": -1.3612319162225908, - "angularVelocity": 1.7250498513578787, - "velocityX": 2.8481518655489513, - "velocityY": -0.8060644215578923, - "timestamp": 0.6622110458033756 - }, - { - "x": 3.5181150608793357, - "y": 7.422087979391574, - "heading": -1.2632892360186367, - "angularVelocity": 2.357077019340078, - "velocityX": 2.577461474650843, - "velocityY": -0.7448853544680833, - "timestamp": 0.7037636465034534 - }, - { - "x": 3.6131894964406737, - "y": 7.390320324704052, - "heading": -1.1401421709924013, - "angularVelocity": 2.963642779307876, - "velocityX": 2.2880501812045355, - "velocityY": -0.7645166404100056, - "timestamp": 0.7453162472035311 - }, - { - "x": 3.6966643009377673, - "y": 7.3543290738456, - "heading": -0.9935989943181865, - "angularVelocity": 3.526690849796876, - "velocityX": 2.0088948246489093, - "velocityY": -0.8661612089754375, - "timestamp": 0.7868688479036088 - }, - { - "x": 3.771161529488124, - "y": 7.3090712240596085, - "heading": -0.8292288931023285, - "angularVelocity": 3.955711518570448, - "velocityX": 1.7928415380801184, - "velocityY": -1.0891700885984243, - "timestamp": 0.8284214486036865 - }, - { - "x": 3.8550844036685596, - "y": 7.255704524470319, - "heading": -0.6866898973306557, - "angularVelocity": 3.4303267032669584, - "velocityX": 2.019678016935338, - "velocityY": -1.2843167140003124, - "timestamp": 0.8699740493037642 - }, - { - "x": 3.950562606960919, - "y": 7.198892399115019, - "heading": -0.5688079573114242, - "angularVelocity": 2.8369329003034878, - "velocityX": 2.2977672079183233, - "velocityY": -1.3672339251486203, - "timestamp": 0.9115266500038419 - }, - { - "x": 4.057836088819487, - "y": 7.143730615244361, - "heading": -0.4749573744321577, - "angularVelocity": 2.2585970865377405, - "velocityX": 2.581630994239328, - "velocityY": -1.3275170011332822, - "timestamp": 0.9530792507039196 - }, - { - "x": 4.176215433316135, - "y": 7.093067133082196, - "heading": -0.40438880402277044, - "angularVelocity": 1.6982949134456653, - "velocityX": 2.848903377940195, - "velocityY": -1.2192614014185588, - "timestamp": 0.9946318514039973 - }, - { - "x": 4.305678352995595, - "y": 7.043923680276767, - "heading": -0.36279336127293804, - "angularVelocity": 1.0010310317289781, - "velocityX": 3.1156393943645972, - "velocityY": -1.1826805537429936, - "timestamp": 1.036184452104075 - }, - { - "x": 4.439387798309326, - "y": 6.998125076293945, - "heading": -0.329998270844152, - "angularVelocity": 0.7892427880864705, - "velocityX": 3.2178357806972437, - "velocityY": -1.102183815482256, - "timestamp": 1.0777370528041528 - }, - { - "x": 4.582105276471285, - "y": 6.9549071662449835, - "heading": -0.29404604928784833, - "angularVelocity": 0.8178419640522191, - "velocityX": 3.2465404804473232, - "velocityY": -0.9831220132341564, - "timestamp": 1.1216969168568243 - }, - { - "x": 4.730015121041943, - "y": 6.913657635454265, - "heading": -0.2719754549705928, - "angularVelocity": 0.5020623878820808, - "velocityX": 3.364656551108451, - "velocityY": -0.9383452765298861, - "timestamp": 1.1656567809094958 - }, - { - "x": 4.880746443869425, - "y": 6.874004608231845, - "heading": -0.25724414225603226, - "angularVelocity": 0.33510824093785235, - "velocityX": 3.4288396034819466, - "velocityY": -0.9020279765858785, - "timestamp": 1.2096166449621673 - }, - { - "x": 5.032791573379627, - "y": 6.835936031206198, - "heading": -0.24533256299842895, - "angularVelocity": 0.2709648793119881, - "velocityX": 3.4587261081614047, - "velocityY": -0.8659848670149453, - "timestamp": 1.2535765090148387 - }, - { - "x": 5.185019069456891, - "y": 6.799899076081779, - "heading": -0.23244532732058962, - "angularVelocity": 0.2931591340322468, - "velocityX": 3.462874587029316, - "velocityY": -0.8197694852112772, - "timestamp": 1.2975363730675102 - }, - { - "x": 5.336755073872842, - "y": 6.765960464892938, - "heading": -0.21656435212756764, - "angularVelocity": 0.36126078947639223, - "velocityX": 3.451694123397726, - "velocityY": -0.772036309034291, - "timestamp": 1.3414962371201817 - }, - { - "x": 5.488275259993641, - "y": 6.732179135791566, - "heading": -0.20012870270564123, - "angularVelocity": 0.373878531614867, - "velocityX": 3.446784683847838, - "velocityY": -0.7684584524844359, - "timestamp": 1.3854561011728532 - }, - { - "x": 5.638672828674316, - "y": 6.7, - "heading": -0.17939789980934723, - "angularVelocity": 0.47158478177886626, - "velocityX": 3.4212473564629415, - "velocityY": -0.7320117221700879, - "timestamp": 1.4294159652255247 - }, - { - "x": 5.788087679940991, - "y": 6.680101103459369, - "heading": -0.13240228550987074, - "angularVelocity": 1.035498011906759, - "velocityX": 3.292196170689038, - "velocityY": -0.43845086640736536, - "timestamp": 1.474800518125784 - }, - { - "x": 5.943840213221069, - "y": 6.6639222262581566, - "heading": -0.10350726387283918, - "angularVelocity": 0.6366708448254484, - "velocityX": 3.431840203920755, - "velocityY": -0.35648422574013283, - "timestamp": 1.5201850710260432 - }, - { - "x": 6.103323466800933, - "y": 6.650235955506569, - "heading": -0.08560921386021843, - "angularVelocity": 0.394364356787972, - "velocityX": 3.514042628785103, - "velocityY": -0.30156231309943976, - "timestamp": 1.5655696239263024 - }, - { - "x": 6.264603851666927, - "y": 6.6384471876022735, - "heading": -0.07296590604456112, - "angularVelocity": 0.27858174219417636, - "velocityX": 3.5536404913019175, - "velocityY": -0.25975287076645964, - "timestamp": 1.6109541768265616 - }, - { - "x": 6.426334391670966, - "y": 6.628509153162114, - "heading": -0.061368144257997005, - "angularVelocity": 0.2555442555984282, - "velocityX": 3.563559177490868, - "velocityY": -0.21897394168447357, - "timestamp": 1.6563387297268208 - }, - { - "x": 6.5873241852765965, - "y": 6.620935768108484, - "heading": -0.04698231400096106, - "angularVelocity": 0.31697635732336293, - "velocityX": 3.5472376242071357, - "velocityY": -0.16687142584098535, - "timestamp": 1.70172328262708 - }, - { - "x": 6.747933405474403, - "y": 6.61399464660675, - "heading": -0.03144813807234412, - "angularVelocity": 0.3422789239042308, - "velocityX": 3.538852096897087, - "velocityY": -0.1529401758568234, - "timestamp": 1.7471078355273393 - }, - { - "x": 6.908430702545003, - "y": 6.607020256679557, - "heading": -0.015778635190030303, - "angularVelocity": 0.3452607083459035, - "velocityX": 3.5363859906987134, - "velocityY": -0.15367320996910241, - "timestamp": 1.7924923884275985 - }, - { - "x": 7.068823337554932, - "y": 6.6, - "heading": 9.122324332035218e-19, - "angularVelocity": 0.3476653218267468, - "velocityX": 3.5340798743224577, - "velocityY": -0.15468383471761574, - "timestamp": 1.8378769413278577 - }, - { - "x": 7.2863547293224356, - "y": 6.583697525493296, - "heading": 5.055756708499563e-13, - "angularVelocity": 8.456864118207454e-12, - "velocityX": 3.6386971266675183, - "velocityY": -0.27269520349740567, - "timestamp": 1.8976597108728264 - }, - { - "x": 7.503885953162418, - "y": 6.56739281040261, - "heading": 5.055557142423875e-13, - "angularVelocity": -3.3381871949908477e-16, - "velocityX": 3.63869431770564, - "velocityY": -0.2727326822559135, - "timestamp": 1.957442480417795 - }, - { - "x": 7.721417177009884, - "y": 6.551088095411767, - "heading": 5.055489054490857e-13, - "angularVelocity": -1.1389223621556497e-16, - "velocityX": 3.6386943178308213, - "velocityY": -0.27273268058578987, - "timestamp": 2.0172252499627636 - }, - { - "x": 7.93894840606763, - "y": 6.534783449936009, - "heading": 5.055544745362588e-13, - "angularVelocity": 9.315538919737292e-17, - "velocityX": 3.638694404984348, - "velocityY": -0.272731517791152, - "timestamp": 2.077008019507732 - }, - { - "x": 8.136688093850118, - "y": 6.522083274226791, - "heading": 3.612634459265275e-13, - "angularVelocity": -2.4135888937230423e-12, - "velocityX": 3.30763678711385, - "velocityY": -0.21243873119102055, - "timestamp": 2.1367907890527005 - }, - { - "x": 8.306183694624885, - "y": 6.511500099763441, - "heading": 2.4088629871582263e-13, - "angularVelocity": -2.0135759538568424e-12, - "velocityX": 2.835191511950123, - "velocityY": -0.17702716926470988, - "timestamp": 2.196573558597669 - }, - { - "x": 8.447435190810417, - "y": 6.5030339231865915, - "heading": 1.445467303198235e-13, - "angularVelocity": -1.611493899150014e-12, - "velocityX": 2.362745942696488, - "velocityY": -0.1416156635312972, - "timestamp": 2.2563563281426373 - }, - { - "x": 8.56044257654444, - "y": 6.49668474335296, - "heading": 7.227055213753e-14, - "angularVelocity": -1.2089800912943037e-12, - "velocityX": 1.8903002753832105, - "velocityY": -0.1062041769218409, - "timestamp": 2.3161390976876057 - }, - { - "x": 8.645205848895236, - "y": 6.492452559683135, - "heading": 2.4085120103106565e-14, - "angularVelocity": -8.060086944982917e-13, - "velocityX": 1.4178545590303353, - "velocityY": -0.07079270000432428, - "timestamp": 2.375921867232574 + "angularVelocity": 4.227163652979875e-19, + "velocityX": 3.3739588765926873, + "velocityY": -0.04852419049232661, + "timestamp": 0.42578708569808266 + }, + { + "x": 2.8454680992564305, + "y": 7.886072511387067, + "heading": -1.5709999999999997, + "angularVelocity": 4.2271698321650397e-19, + "velocityX": 3.6484181749177784, + "velocityY": -0.05934850292215207, + "timestamp": 0.46836579426789093 + }, + { + "x": 3.000000000000003, + "y": 7.87, + "heading": -1.5709999999999997, + "angularVelocity": 2.1135793766278028e-19, + "velocityX": 3.6293233386873958, + "velocityY": -0.3774776625911161, + "timestamp": 0.5109445028376992 + }, + { + "x": 3.0933267855298086, + "y": 7.854947925249578, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450956207458e-19, + "velocityX": 3.6023482406759033, + "velocityY": -0.580999492137869, + "timestamp": 0.5368517109404605 + }, + { + "x": 3.1856594234462645, + "y": 7.8346713899397304, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524509613572167e-19, + "velocityX": 3.563974842453837, + "velocityY": -0.7826599929029426, + "timestamp": 0.5627589190432217 + }, + { + "x": 3.2770421386792723, + "y": 7.8104711155350515, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450981385339e-19, + "velocityX": 3.527308495401703, + "velocityY": -0.9341135605458323, + "timestamp": 0.588666127145983 + }, + { + "x": 3.368397018870737, + "y": 7.7861659763653845, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451746814082e-19, + "velocityX": 3.5262340823876155, + "velocityY": -0.9381612666736713, + "timestamp": 0.6145733352487442 + }, + { + "x": 3.4597414869906253, + "y": 7.761821735388127, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451049308022e-19, + "velocityX": 3.5258321837525024, + "velocityY": -0.9396705689278512, + "timestamp": 0.6404805433515055 + }, + { + "x": 3.5510841613375907, + "y": 7.7374707648012215, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524500373464137e-19, + "velocityX": 3.525762945380196, + "velocityY": -0.9399303271237421, + "timestamp": 0.6663877514542668 + }, + { + "x": 3.6424273103238014, + "y": 7.7131215747012565, + "heading": -1.5709999999999997, + "angularVelocity": 2.6530371780360815e-19, + "velocityX": 3.5257812661210104, + "velocityY": -0.9398616015809562, + "timestamp": 0.692294959557028 + }, + { + "x": 3.7337711953747554, + "y": 7.688775146038041, + "heading": -1.5709999999999997, + "angularVelocity": 2.651560076415564e-19, + "velocityX": 3.5258096777019814, + "velocityY": -0.9397550120662235, + "timestamp": 0.7182021676597893 + }, + { + "x": 3.8251143320372014, + "y": 7.664425909712902, + "heading": -1.5709999999999997, + "angularVelocity": 2.6527560956628585e-19, + "velocityX": 3.525780790432169, + "velocityY": -0.9398633858408949, + "timestamp": 0.7441093757625505 + }, + { + "x": 3.9164615673273144, + "y": 7.640092054070024, + "heading": -1.5709999999999997, + "angularVelocity": 2.651878932000989e-19, + "velocityX": 3.525938994575509, + "velocityY": -0.9392697023305191, + "timestamp": 0.7700165838653118 + }, + { + "x": 4.007822648813717, + "y": 7.615810235461623, + "heading": -1.5709999999999997, + "angularVelocity": 2.653022563334735e-19, + "velocityX": 3.526473448007752, + "velocityY": -0.9372611094217806, + "timestamp": 0.7959237919680731 + }, + { + "x": 4.099836826324463, + "y": 7.594134330749512, + "heading": -1.5709999999999997, + "angularVelocity": 8.821604445268149e-19, + "velocityX": 3.551682494916745, + "velocityY": -0.8366746669941396, + "timestamp": 0.8218310000708343 + }, + { + "x": 4.314086123865472, + "y": 7.565866389645391, + "heading": -1.5166611039162066, + "angularVelocity": 0.8444409025240843, + "velocityX": 3.3294910868420495, + "velocityY": -0.4392913257079289, + "timestamp": 0.8861799627058939 + }, + { + "x": 4.512621682190808, + "y": 7.550271754810037, + "heading": -1.4174555506263589, + "angularVelocity": 1.5416806927054159, + "velocityX": 3.085295398641018, + "velocityY": -0.24234477443848973, + "timestamp": 0.9505289253409536 + }, + { + "x": 4.703674537259271, + "y": 7.533864741231446, + "heading": -1.3046929681948032, + "angularVelocity": 1.7523605325406573, + "velocityX": 2.9690121991853746, + "velocityY": -0.2549693562540318, + "timestamp": 1.0148778879760132 + }, + { + "x": 4.868121057502234, + "y": 7.5183747890499655, + "heading": -1.1331539755708713, + "angularVelocity": 2.6657615849509235, + "velocityX": 2.555542677129605, + "velocityY": -0.2407179781503281, + "timestamp": 1.0792268506110727 + }, + { + "x": 5.005308136079898, + "y": 7.491356941262662, + "heading": -0.9056990534597417, + "angularVelocity": 3.534710006145519, + "velocityX": 2.131923701018079, + "velocityY": -0.4198645429702136, + "timestamp": 1.1435758132461322 + }, + { + "x": 5.1255280916701365, + "y": 7.4382119450007, + "heading": -0.6380105243345551, + "angularVelocity": 4.159950963671538, + "velocityX": 1.8682500955315844, + "velocityY": -0.8258873816434845, + "timestamp": 1.2079247758811917 + }, + { + "x": 5.274137127844687, + "y": 7.377257884535905, + "heading": -0.4258350917264682, + "angularVelocity": 3.2972626740101054, + "velocityX": 2.309423960995877, + "velocityY": -0.9472423170300921, + "timestamp": 1.2722737385162513 + }, + { + "x": 5.448926084967563, + "y": 7.329899834255518, + "heading": -0.26459184239693584, + "angularVelocity": 2.505762994874233, + "velocityX": 2.7162668979480253, + "velocityY": -0.7359567014145671, + "timestamp": 1.3366227011513108 + }, + { + "x": 5.647781118238161, + "y": 7.297534713393957, + "heading": -0.1613424754389799, + "angularVelocity": 1.6045226329976747, + "velocityX": 3.090260124291229, + "velocityY": -0.5029625892352149, + "timestamp": 1.4009716637863703 + }, + { + "x": 5.854011535644531, + "y": 7.282280921936035, + "heading": -0.07382990470356181, + "angularVelocity": 1.359968632778447, + "velocityX": 3.2048755560510305, + "velocityY": -0.23704797767091085, + "timestamp": 1.4653206264214298 + }, + { + "x": 5.983589932246309, + "y": 7.284960854996629, + "heading": -0.027638204496835066, + "angularVelocity": 1.1636335547284058, + "velocityX": 3.264261102719684, + "velocityY": 0.06751126327279944, + "timestamp": 1.5050167146092823 + }, + { + "x": 6.11925745255698, + "y": 7.297037313900181, + "heading": -0.001860333184984073, + "angularVelocity": 0.6493806440077139, + "velocityX": 3.4176546482011396, + "velocityY": 0.3042228958778286, + "timestamp": 1.5447128027971349 + }, + { + "x": 6.262128938331292, + "y": 7.316680359638158, + "heading": -0.000009480075392796495, + "angularVelocity": 0.046625579353406145, + "velocityX": 3.599132617252429, + "velocityY": 0.49483580460253374, + "timestamp": 1.5844088909849874 + }, + { + "x": 6.405225952749555, + "y": 7.339125340175403, + "heading": -0.000007943471062431687, + "angularVelocity": 0.00003870921293486644, + "velocityX": 3.6048139993312827, + "velocityY": 0.5654204623651486, + "timestamp": 1.62410497917284 + }, + { + "x": 6.548321378993718, + "y": 7.3615804485544425, + "heading": -0.000006409115010503546, + "angularVelocity": 0.00003865257565632945, + "velocityX": 3.6047739910041234, + "velocityY": 0.5656755968668977, + "timestamp": 1.6638010673606924 + }, + { + "x": 6.691417142733098, + "y": 7.384033405453259, + "heading": -0.000004874435347527005, + "angularVelocity": 0.00003866072787080933, + "velocityX": 3.604782492980222, + "velocityY": 0.5656213980739361, + "timestamp": 1.703497155548545 + }, + { + "x": 6.83451270884544, + "y": 7.40648762220655, + "heading": -0.0000033399372412399076, + "angularVelocity": 0.000038656154204017926, + "velocityX": 3.6047775144789, + "velocityY": 0.5656531355692576, + "timestamp": 1.7431932437363975 + }, + { + "x": 6.977608912847965, + "y": 7.428937772164743, + "heading": -0.0000018048282304933975, + "angularVelocity": 0.00003867154374202805, + "velocityX": 3.6047935838250287, + "velocityY": 0.5655506873111692, + "timestamp": 1.78288933192425 + }, + { + "x": 7.12091588973999, + "y": 7.45, + "heading": 5.756100309873575e-19, + "angularVelocity": 0.000045466148249922065, + "velocityX": 3.6101032478025736, + "velocityY": 0.5305869872012366, + "timestamp": 1.8225854201121026 + }, + { + "x": 7.357842782347066, + "y": 7.451140658234923, + "heading": 1.3946076252127016e-14, + "angularVelocity": 2.1477158274944538e-13, + "velocityX": 3.6488588704771803, + "velocityY": 0.017567026152596314, + "timestamp": 1.8875171976944158 + }, + { + "x": 7.594772307901899, + "y": 7.450909424040305, + "heading": 1.3956817600616414e-14, + "angularVelocity": 1.6542514133334603e-16, + "velocityX": 3.648899419925491, + "velocityY": -0.003561186883034476, + "timestamp": 1.9524489752767291 + }, + { + "x": 7.831701831188383, + "y": 7.450675877221787, + "heading": 1.3958919096191331e-14, + "angularVelocity": 3.236467023043882e-17, + "velocityX": 3.6488993849911218, + "velocityY": -0.00359680309416508, + "timestamp": 2.0173807528590424 + }, + { + "x": 8.06023649674366, + "y": 7.450480884382308, + "heading": 1.3831887827184638e-14, + "angularVelocity": -1.9563805854348244e-15, + "velocityX": 3.519612030728179, + "velocityY": -0.0030030417576737706, + "timestamp": 2.0823125304413557 + }, + { + "x": 8.255358856071796, + "y": 7.450318830701623, + "heading": 1.0574442032523106e-14, + "angularVelocity": -5.016720496430017e-14, + "velocityX": 3.0050364643226115, + "velocityY": -0.002495753030620786, + "timestamp": 2.147244308023669 + }, + { + "x": 8.417068891314898, + "y": 7.450189715578673, + "heading": 6.865176887322045e-15, + "angularVelocity": -5.712557523159656e-14, + "velocityX": 2.4904606228915314, + "velocityY": -0.0019884735603726703, + "timestamp": 2.2121760856059822 + }, + { + "x": 8.54536659652034, + "y": 7.450093538813877, + "heading": 3.601695099473894e-15, + "angularVelocity": -5.026016396528296e-14, + "velocityX": 1.975884689785358, + "velocityY": -0.001481197163799284, + "timestamp": 2.2771078631882955 + }, + { + "x": 8.640251968711807, + "y": 7.450030300307574, + "heading": 1.2253067804043182e-15, + "angularVelocity": -3.659823290767036e-14, + "velocityX": 1.4613087108416603, + "velocityY": -0.0009739223021094422, + "timestamp": 2.3420396407706088 }, { "x": 8.701725006103516, - "y": 6.490337371826172, - "heading": -6.207507223536792e-26, - "angularVelocity": -4.028772886652685e-13, - "velocityX": 0.9454088132495556, - "velocityY": -0.035381228957146495, - "timestamp": 2.4357046367775426 + "y": 7.45, + "heading": -1.9088649305256032e-26, + "angularVelocity": -1.8870679751086276e-14, + "velocityX": 0.9467327043954562, + "velocityY": -0.00046664836083104116, + "timestamp": 2.406971418352922 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 1.0661056930411766e-25, - "angularVelocity": 3.714877122041656e-24, - "velocityX": 0.47336647327631065, - "velocityY": 3.668684939131766e-15, - "timestamp": 2.4954363573212164 + "y": 7.45, + "heading": 2.623532798918036e-26, + "angularVelocity": 9.781191839132487e-25, + "velocityX": 0.4733664733221421, + "velocityY": 5.037124173048106e-15, + "timestamp": 2.4667031388908103 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 5.321036058198176e-26, - "angularVelocity": -3.1783468612327103e-27, - "velocityX": -2.6405211555124906e-21, - "velocityY": -7.26970172585428e-20, - "timestamp": 2.55516807786489 + "y": 7.45, + "heading": 1.3100764528202313e-26, + "angularVelocity": -5.658464945099905e-28, + "velocityX": 7.815178981981403e-20, + "velocityY": 1.9973998116523263e-19, + "timestamp": 2.5264348594286985 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1.traj b/src/main/deploy/choreo/B_PickUpFarPiece1.traj index 58c0ba4..14191bb 100644 --- a/src/main/deploy/choreo/B_PickUpFarPiece1.traj +++ b/src/main/deploy/choreo/B_PickUpFarPiece1.traj @@ -1,499 +1,499 @@ { "samples": [ { - "x": 1.9, + "x": 1.8999999999999968, "y": 7.9, "heading": -1.571, - "angularVelocity": 2.300332094481187e-29, - "velocityX": -4.0491893744801466e-20, - "velocityY": -1.3926499461964718e-19, + "angularVelocity": 4.470498597716865e-29, + "velocityX": 2.7707804022206673e-16, + "velocityY": 1.8739023107890902e-17, "timestamp": 0 }, { - "x": 1.9154424529969702, - "y": 7.8954006953331355, + "x": 1.914365871743733, + "y": 7.899792228900449, "heading": -1.571, - "angularVelocity": -7.879169842980081e-20, - "velocityX": 0.34247331200662146, - "velocityY": -0.10200057610651515, - "timestamp": 0.04509096754573317 + "angularVelocity": 4.2271636316208514e-19, + "velocityX": 0.33739566619749, + "velocityY": -0.004879694723755523, + "timestamp": 0.042578708569808256 }, { - "x": 1.9463273585735072, - "y": 7.886202086121636, + "x": 1.9430976157693656, + "y": 7.899376752477764, "heading": -1.571, - "angularVelocity": -7.879169825285715e-20, - "velocityX": 0.6849466147563192, - "velocityY": -0.2040011495022858, - "timestamp": 0.09018193509146634 + "angularVelocity": 4.227163636037315e-19, + "velocityX": 0.6747913450340882, + "velocityY": -0.009757844628066633, + "timestamp": 0.08515741713961651 }, { - "x": 1.9926547162078563, - "y": 7.872404172518289, + "x": 1.986195232749876, + "y": 7.898753653015407, "heading": -1.571, - "angularVelocity": -7.879169828739293e-20, - "velocityX": 1.0274199059348594, - "velocityY": -0.30600171950962485, - "timestamp": 0.13527290263719952 + "angularVelocity": 4.227163633163533e-19, + "velocityX": 1.0121870396762844, + "velocityY": -0.014634062029792581, + "timestamp": 0.12773612570942477 }, { - "x": 2.0544245252291895, - "y": 7.8540069547195355, + "x": 2.0436587235509935, + "y": 7.897923036409323, "heading": -1.571, - "angularVelocity": -7.87916983435114e-20, - "velocityX": 1.3698931822361922, - "velocityY": -0.40800228516040743, - "timestamp": 0.18036387018293268 + "angularVelocity": 4.227163634858386e-19, + "velocityX": 1.349582754650927, + "velocityY": -0.019507792368223267, + "timestamp": 0.17031483427923302 }, { - "x": 2.13163678474307, - "y": 7.8310104329872985, + "x": 2.115488089327842, + "y": 7.896885044034559, "heading": -1.571, - "angularVelocity": -7.879169829580418e-20, - "velocityX": 1.7123664387012403, - "velocityY": -0.5100028450024455, - "timestamp": 0.22545483772866584 + "angularVelocity": 4.2271636331977046e-19, + "velocityX": 1.6869784967546948, + "velocityY": -0.024378202384055483, + "timestamp": 0.21289354284904127 }, { - "x": 2.2242914934972844, - "y": 7.8034146076882696, + "x": 2.2016833316991637, + "y": 7.895639874163583, "heading": -1.571, - "angularVelocity": -7.879169836864827e-20, - "velocityX": 2.0548396673954747, - "velocityY": -0.6120033967122365, - "timestamp": 0.27054580527439903 + "angularVelocity": 4.2271636334033824e-19, + "velocityX": 2.024374276876126, + "velocityY": -0.029243955789217388, + "timestamp": 0.25547225141884955 }, { - "x": 2.332388649613512, - "y": 7.771219479372483, + "x": 2.302244453096514, + "y": 7.894187824967971, "heading": -1.571, - "angularVelocity": -7.87916983823685e-20, - "velocityX": 2.397312854433457, - "velocityY": -0.7140039362236463, - "timestamp": 0.3156367728201322 + "angularVelocity": 4.227163633528535e-19, + "velocityX": 2.361770114104849, + "velocityY": -0.03410270636161241, + "timestamp": 0.2980509599886578 }, { - "x": 2.455928249961216, - "y": 7.73442504895667, + "x": 2.4171714575816776, + "y": 7.8925293954471245, "heading": -1.571, - "angularVelocity": -7.879169832615412e-20, - "velocityX": 2.739785972044277, - "velocityY": -0.8160044554043961, - "timestamp": 0.36072774036586536 + "angularVelocity": 4.227163633894554e-19, + "velocityX": 2.699166046728326, + "velocityY": -0.03894973747556378, + "timestamp": 0.3406296685584661 }, { - "x": 2.5949102882793076, - "y": 7.693031318274292, + "x": 2.5464643533128632, + "y": 7.890665591362613, "heading": -1.571, - "angularVelocity": -7.879169827922094e-20, - "velocityX": 3.082258950800517, - "velocityY": -0.9180049339237416, - "timestamp": 0.4058187079115985 + "angularVelocity": 4.2271636346276787e-19, + "velocityX": 3.0365621709548005, + "velocityY": -0.04377314735710882, + "timestamp": 0.3832083771282744 }, { - "x": 2.749334745784455, - "y": 7.647038292825759, + "x": 2.6901231650458177, + "y": 7.888599493997031, "heading": -1.571, - "angularVelocity": -7.879169832843695e-20, - "velocityX": 3.424731512991456, - "velocityY": -1.0200052904583357, - "timestamp": 0.4509096754573317 - }, - { - "x": 2.907, - "y": 7.6, - "heading": -1.571, - "angularVelocity": -2.3912453364100895e-19, - "velocityX": 3.496603927507973, - "velocityY": -1.0431865933693085, - "timestamp": 0.49600064300306484 - }, - { - "x": 3.0409750518324072, - "y": 7.55855968097087, - "heading": -1.5404817853182164, - "angularVelocity": 0.7344477642220753, - "velocityX": 3.2242278359283354, - "velocityY": -0.9972978425163852, - "timestamp": 0.5375532437031425 - }, - { - "x": 3.167821917464271, - "y": 7.522503707879653, - "heading": -1.488578715495742, - "angularVelocity": 1.2490931722204932, - "velocityX": 3.052681745420317, - "velocityY": -0.8677188066147962, - "timestamp": 0.5791058444032202 - }, - { - "x": 3.2926667162009906, - "y": 7.486533976140662, - "heading": -1.4329122238837848, - "angularVelocity": 1.3396632382593825, - "velocityX": 3.004500239054416, - "velocityY": -0.8656433323781126, - "timestamp": 0.6206584451032979 - }, - { - "x": 3.4110148334033306, - "y": 7.453039903093125, - "heading": -1.3612319162225908, - "angularVelocity": 1.7250498513578787, - "velocityX": 2.8481518655489513, - "velocityY": -0.8060644215578923, - "timestamp": 0.6622110458033756 - }, - { - "x": 3.5181150608793357, - "y": 7.422087979391574, - "heading": -1.2632892360186367, - "angularVelocity": 2.357077019340078, - "velocityX": 2.577461474650843, - "velocityY": -0.7448853544680833, - "timestamp": 0.7037636465034534 - }, - { - "x": 3.6131894964406737, - "y": 7.390320324704052, - "heading": -1.1401421709924013, - "angularVelocity": 2.963642779307876, - "velocityX": 2.2880501812045355, - "velocityY": -0.7645166404100056, - "timestamp": 0.7453162472035311 - }, - { - "x": 3.6966643009377673, - "y": 7.3543290738456, - "heading": -0.9935989943181865, - "angularVelocity": 3.526690849796876, - "velocityX": 2.0088948246489093, - "velocityY": -0.8661612089754375, - "timestamp": 0.7868688479036088 - }, - { - "x": 3.771161529488124, - "y": 7.3090712240596085, - "heading": -0.8292288931023285, - "angularVelocity": 3.955711518570448, - "velocityX": 1.7928415380801184, - "velocityY": -1.0891700885984243, - "timestamp": 0.8284214486036865 - }, - { - "x": 3.8550844036685596, - "y": 7.255704524470319, - "heading": -0.6866898973306557, - "angularVelocity": 3.4303267032669584, - "velocityX": 2.019678016935338, - "velocityY": -1.2843167140003124, - "timestamp": 0.8699740493037642 - }, - { - "x": 3.950562606960919, - "y": 7.198892399115019, - "heading": -0.5688079573114242, - "angularVelocity": 2.8369329003034878, - "velocityX": 2.2977672079183233, - "velocityY": -1.3672339251486203, - "timestamp": 0.9115266500038419 - }, - { - "x": 4.057836088819487, - "y": 7.143730615244361, - "heading": -0.4749573744321577, - "angularVelocity": 2.2585970865377405, - "velocityX": 2.581630994239328, - "velocityY": -1.3275170011332822, - "timestamp": 0.9530792507039196 - }, - { - "x": 4.176215433316135, - "y": 7.093067133082196, - "heading": -0.40438880402277044, - "angularVelocity": 1.6982949134456653, - "velocityX": 2.848903377940195, - "velocityY": -1.2192614014185588, - "timestamp": 0.9946318514039973 - }, - { - "x": 4.305678352995595, - "y": 7.043923680276767, - "heading": -0.36279336127293804, - "angularVelocity": 1.0010310317289781, - "velocityX": 3.1156393943645972, - "velocityY": -1.1826805537429936, - "timestamp": 1.036184452104075 - }, - { - "x": 4.439387798309326, - "y": 6.998125076293945, - "heading": -0.329998270844152, - "angularVelocity": 0.7892427880864705, - "velocityX": 3.2178357806972437, - "velocityY": -1.102183815482256, - "timestamp": 1.0777370528041528 - }, - { - "x": 4.582105276471285, - "y": 6.9549071662449835, - "heading": -0.29404604928784833, - "angularVelocity": 0.8178419640522191, - "velocityX": 3.2465404804473232, - "velocityY": -0.9831220132341564, - "timestamp": 1.1216969168568243 - }, - { - "x": 4.730015121041943, - "y": 6.913657635454265, - "heading": -0.2719754549705928, - "angularVelocity": 0.5020623878820808, - "velocityX": 3.364656551108451, - "velocityY": -0.9383452765298861, - "timestamp": 1.1656567809094958 - }, - { - "x": 4.880746443869425, - "y": 6.874004608231845, - "heading": -0.25724414225603226, - "angularVelocity": 0.33510824093785235, - "velocityX": 3.4288396034819466, - "velocityY": -0.9020279765858785, - "timestamp": 1.2096166449621673 - }, - { - "x": 5.032791573379627, - "y": 6.835936031206198, - "heading": -0.24533256299842895, - "angularVelocity": 0.2709648793119881, - "velocityX": 3.4587261081614047, - "velocityY": -0.8659848670149453, - "timestamp": 1.2535765090148387 - }, - { - "x": 5.185019069456891, - "y": 6.799899076081779, - "heading": -0.23244532732058962, - "angularVelocity": 0.2931591340322468, - "velocityX": 3.462874587029316, - "velocityY": -0.8197694852112772, - "timestamp": 1.2975363730675102 - }, - { - "x": 5.336755073872842, - "y": 6.765960464892938, - "heading": -0.21656435212756764, - "angularVelocity": 0.36126078947639223, - "velocityX": 3.451694123397726, - "velocityY": -0.772036309034291, - "timestamp": 1.3414962371201817 - }, - { - "x": 5.488275259993641, - "y": 6.732179135791566, - "heading": -0.20012870270564123, - "angularVelocity": 0.373878531614867, - "velocityX": 3.446784683847838, - "velocityY": -0.7684584524844359, - "timestamp": 1.3854561011728532 - }, - { - "x": 5.638672828674316, - "y": 6.7, - "heading": -0.17939789980934723, - "angularVelocity": 0.47158478177886626, - "velocityX": 3.4212473564629415, - "velocityY": -0.7320117221700879, - "timestamp": 1.4294159652255247 - }, - { - "x": 5.788087679940991, - "y": 6.680101103459369, - "heading": -0.13240228550987074, - "angularVelocity": 1.035498011906759, - "velocityX": 3.292196170689038, - "velocityY": -0.43845086640736536, - "timestamp": 1.474800518125784 - }, - { - "x": 5.943840213221069, - "y": 6.6639222262581566, - "heading": -0.10350726387283918, - "angularVelocity": 0.6366708448254484, - "velocityX": 3.431840203920755, - "velocityY": -0.35648422574013283, - "timestamp": 1.5201850710260432 - }, - { - "x": 6.103323466800933, - "y": 6.650235955506569, - "heading": -0.08560921386021843, - "angularVelocity": 0.394364356787972, - "velocityX": 3.514042628785103, - "velocityY": -0.30156231309943976, - "timestamp": 1.5655696239263024 - }, - { - "x": 6.264603851666927, - "y": 6.6384471876022735, - "heading": -0.07296590604456112, - "angularVelocity": 0.27858174219417636, - "velocityX": 3.5536404913019175, - "velocityY": -0.25975287076645964, - "timestamp": 1.6109541768265616 - }, - { - "x": 6.426334391670966, - "y": 6.628509153162114, - "heading": -0.061368144257997005, - "angularVelocity": 0.2555442555984282, - "velocityX": 3.563559177490868, - "velocityY": -0.21897394168447357, - "timestamp": 1.6563387297268208 - }, - { - "x": 6.5873241852765965, - "y": 6.620935768108484, - "heading": -0.04698231400096106, - "angularVelocity": 0.31697635732336293, - "velocityX": 3.5472376242071357, - "velocityY": -0.16687142584098535, - "timestamp": 1.70172328262708 - }, - { - "x": 6.747933405474403, - "y": 6.61399464660675, - "heading": -0.03144813807234412, - "angularVelocity": 0.3422789239042308, - "velocityX": 3.538852096897087, - "velocityY": -0.1529401758568234, - "timestamp": 1.7471078355273393 - }, - { - "x": 6.908430702545003, - "y": 6.607020256679557, - "heading": -0.015778635190030303, - "angularVelocity": 0.3452607083459035, - "velocityX": 3.5363859906987134, - "velocityY": -0.15367320996910241, - "timestamp": 1.7924923884275985 - }, - { - "x": 7.068823337554932, - "y": 6.6, - "heading": 9.122324332035218e-19, - "angularVelocity": 0.3476653218267468, - "velocityX": 3.5340798743224577, - "velocityY": -0.15468383471761574, - "timestamp": 1.8378769413278577 - }, - { - "x": 7.2863547293224356, - "y": 6.583697525493296, - "heading": 5.055756708499563e-13, - "angularVelocity": 8.456864118207454e-12, - "velocityX": 3.6386971266675183, - "velocityY": -0.27269520349740567, - "timestamp": 1.8976597108728264 - }, - { - "x": 7.503885953162418, - "y": 6.56739281040261, - "heading": 5.055557142423875e-13, - "angularVelocity": -3.3381871949908477e-16, - "velocityX": 3.63869431770564, - "velocityY": -0.2727326822559135, - "timestamp": 1.957442480417795 - }, - { - "x": 7.721417177009884, - "y": 6.551088095411767, - "heading": 5.055489054490857e-13, - "angularVelocity": -1.1389223621556497e-16, - "velocityX": 3.6386943178308213, - "velocityY": -0.27273268058578987, - "timestamp": 2.0172252499627636 - }, - { - "x": 7.93894840606763, - "y": 6.534783449936009, - "heading": 5.055544745362588e-13, - "angularVelocity": 9.315538919737292e-17, - "velocityX": 3.638694404984348, - "velocityY": -0.272731517791152, - "timestamp": 2.077008019507732 - }, - { - "x": 8.136688093850118, - "y": 6.522083274226791, - "heading": 3.612634459265275e-13, - "angularVelocity": -2.4135888937230423e-12, - "velocityX": 3.30763678711385, - "velocityY": -0.21243873119102055, - "timestamp": 2.1367907890527005 - }, - { - "x": 8.306183694624885, - "y": 6.511500099763441, - "heading": 2.4088629871582263e-13, - "angularVelocity": -2.0135759538568424e-12, - "velocityX": 2.835191511950123, - "velocityY": -0.17702716926470988, - "timestamp": 2.196573558597669 - }, - { - "x": 8.447435190810417, - "y": 6.5030339231865915, - "heading": 1.445467303198235e-13, - "angularVelocity": -1.611493899150014e-12, - "velocityX": 2.362745942696488, - "velocityY": -0.1416156635312972, - "timestamp": 2.2563563281426373 - }, - { - "x": 8.56044257654444, - "y": 6.49668474335296, - "heading": 7.227055213753e-14, - "angularVelocity": -1.2089800912943037e-12, - "velocityX": 1.8903002753832105, - "velocityY": -0.1062041769218409, - "timestamp": 2.3161390976876057 - }, - { - "x": 8.645205848895236, - "y": 6.492452559683135, - "heading": 2.4085120103106565e-14, - "angularVelocity": -8.060086944982917e-13, - "velocityX": 1.4178545590303353, - "velocityY": -0.07079270000432428, - "timestamp": 2.375921867232574 + "angularVelocity": 4.227163652979875e-19, + "velocityX": 3.3739588765926873, + "velocityY": -0.04852419049232661, + "timestamp": 0.42578708569808266 + }, + { + "x": 2.8454680992564305, + "y": 7.886072511387067, + "heading": -1.5709999999999997, + "angularVelocity": 4.2271698321650397e-19, + "velocityX": 3.6484181749177784, + "velocityY": -0.05934850292215207, + "timestamp": 0.46836579426789093 + }, + { + "x": 3.000000000000003, + "y": 7.87, + "heading": -1.5709999999999997, + "angularVelocity": 2.1135793766278028e-19, + "velocityX": 3.6293233386873958, + "velocityY": -0.3774776625911161, + "timestamp": 0.5109445028376992 + }, + { + "x": 3.0933267855298086, + "y": 7.854947925249578, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450956207458e-19, + "velocityX": 3.6023482406759033, + "velocityY": -0.580999492137869, + "timestamp": 0.5368517109404605 + }, + { + "x": 3.1856594234462645, + "y": 7.8346713899397304, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524509613572167e-19, + "velocityX": 3.563974842453837, + "velocityY": -0.7826599929029426, + "timestamp": 0.5627589190432217 + }, + { + "x": 3.2770421386792723, + "y": 7.8104711155350515, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450981385339e-19, + "velocityX": 3.527308495401703, + "velocityY": -0.9341135605458323, + "timestamp": 0.588666127145983 + }, + { + "x": 3.368397018870737, + "y": 7.7861659763653845, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451746814082e-19, + "velocityX": 3.5262340823876155, + "velocityY": -0.9381612666736713, + "timestamp": 0.6145733352487442 + }, + { + "x": 3.4597414869906253, + "y": 7.761821735388127, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451049308022e-19, + "velocityX": 3.5258321837525024, + "velocityY": -0.9396705689278512, + "timestamp": 0.6404805433515055 + }, + { + "x": 3.5510841613375907, + "y": 7.7374707648012215, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524500373464137e-19, + "velocityX": 3.525762945380196, + "velocityY": -0.9399303271237421, + "timestamp": 0.6663877514542668 + }, + { + "x": 3.6424273103238014, + "y": 7.7131215747012565, + "heading": -1.5709999999999997, + "angularVelocity": 2.6530371780360815e-19, + "velocityX": 3.5257812661210104, + "velocityY": -0.9398616015809562, + "timestamp": 0.692294959557028 + }, + { + "x": 3.7337711953747554, + "y": 7.688775146038041, + "heading": -1.5709999999999997, + "angularVelocity": 2.651560076415564e-19, + "velocityX": 3.5258096777019814, + "velocityY": -0.9397550120662235, + "timestamp": 0.7182021676597893 + }, + { + "x": 3.8251143320372014, + "y": 7.664425909712902, + "heading": -1.5709999999999997, + "angularVelocity": 2.6527560956628585e-19, + "velocityX": 3.525780790432169, + "velocityY": -0.9398633858408949, + "timestamp": 0.7441093757625505 + }, + { + "x": 3.9164615673273144, + "y": 7.640092054070024, + "heading": -1.5709999999999997, + "angularVelocity": 2.651878932000989e-19, + "velocityX": 3.525938994575509, + "velocityY": -0.9392697023305191, + "timestamp": 0.7700165838653118 + }, + { + "x": 4.007822648813717, + "y": 7.615810235461623, + "heading": -1.5709999999999997, + "angularVelocity": 2.653022563334735e-19, + "velocityX": 3.526473448007752, + "velocityY": -0.9372611094217806, + "timestamp": 0.7959237919680731 + }, + { + "x": 4.099836826324463, + "y": 7.594134330749512, + "heading": -1.5709999999999997, + "angularVelocity": 8.821604445268149e-19, + "velocityX": 3.551682494916745, + "velocityY": -0.8366746669941396, + "timestamp": 0.8218310000708343 + }, + { + "x": 4.314086123865472, + "y": 7.565866389645391, + "heading": -1.5166611039162066, + "angularVelocity": 0.8444409025240843, + "velocityX": 3.3294910868420495, + "velocityY": -0.4392913257079289, + "timestamp": 0.8861799627058939 + }, + { + "x": 4.512621682190808, + "y": 7.550271754810037, + "heading": -1.4174555506263589, + "angularVelocity": 1.5416806927054159, + "velocityX": 3.085295398641018, + "velocityY": -0.24234477443848973, + "timestamp": 0.9505289253409536 + }, + { + "x": 4.703674537259271, + "y": 7.533864741231446, + "heading": -1.3046929681948032, + "angularVelocity": 1.7523605325406573, + "velocityX": 2.9690121991853746, + "velocityY": -0.2549693562540318, + "timestamp": 1.0148778879760132 + }, + { + "x": 4.868121057502234, + "y": 7.5183747890499655, + "heading": -1.1331539755708713, + "angularVelocity": 2.6657615849509235, + "velocityX": 2.555542677129605, + "velocityY": -0.2407179781503281, + "timestamp": 1.0792268506110727 + }, + { + "x": 5.005308136079898, + "y": 7.491356941262662, + "heading": -0.9056990534597417, + "angularVelocity": 3.534710006145519, + "velocityX": 2.131923701018079, + "velocityY": -0.4198645429702136, + "timestamp": 1.1435758132461322 + }, + { + "x": 5.1255280916701365, + "y": 7.4382119450007, + "heading": -0.6380105243345551, + "angularVelocity": 4.159950963671538, + "velocityX": 1.8682500955315844, + "velocityY": -0.8258873816434845, + "timestamp": 1.2079247758811917 + }, + { + "x": 5.274137127844687, + "y": 7.377257884535905, + "heading": -0.4258350917264682, + "angularVelocity": 3.2972626740101054, + "velocityX": 2.309423960995877, + "velocityY": -0.9472423170300921, + "timestamp": 1.2722737385162513 + }, + { + "x": 5.448926084967563, + "y": 7.329899834255518, + "heading": -0.26459184239693584, + "angularVelocity": 2.505762994874233, + "velocityX": 2.7162668979480253, + "velocityY": -0.7359567014145671, + "timestamp": 1.3366227011513108 + }, + { + "x": 5.647781118238161, + "y": 7.297534713393957, + "heading": -0.1613424754389799, + "angularVelocity": 1.6045226329976747, + "velocityX": 3.090260124291229, + "velocityY": -0.5029625892352149, + "timestamp": 1.4009716637863703 + }, + { + "x": 5.854011535644531, + "y": 7.282280921936035, + "heading": -0.07382990470356181, + "angularVelocity": 1.359968632778447, + "velocityX": 3.2048755560510305, + "velocityY": -0.23704797767091085, + "timestamp": 1.4653206264214298 + }, + { + "x": 5.983589932246309, + "y": 7.284960854996629, + "heading": -0.027638204496835066, + "angularVelocity": 1.1636335547284058, + "velocityX": 3.264261102719684, + "velocityY": 0.06751126327279944, + "timestamp": 1.5050167146092823 + }, + { + "x": 6.11925745255698, + "y": 7.297037313900181, + "heading": -0.001860333184984073, + "angularVelocity": 0.6493806440077139, + "velocityX": 3.4176546482011396, + "velocityY": 0.3042228958778286, + "timestamp": 1.5447128027971349 + }, + { + "x": 6.262128938331292, + "y": 7.316680359638158, + "heading": -0.000009480075392796495, + "angularVelocity": 0.046625579353406145, + "velocityX": 3.599132617252429, + "velocityY": 0.49483580460253374, + "timestamp": 1.5844088909849874 + }, + { + "x": 6.405225952749555, + "y": 7.339125340175403, + "heading": -0.000007943471062431687, + "angularVelocity": 0.00003870921293486644, + "velocityX": 3.6048139993312827, + "velocityY": 0.5654204623651486, + "timestamp": 1.62410497917284 + }, + { + "x": 6.548321378993718, + "y": 7.3615804485544425, + "heading": -0.000006409115010503546, + "angularVelocity": 0.00003865257565632945, + "velocityX": 3.6047739910041234, + "velocityY": 0.5656755968668977, + "timestamp": 1.6638010673606924 + }, + { + "x": 6.691417142733098, + "y": 7.384033405453259, + "heading": -0.000004874435347527005, + "angularVelocity": 0.00003866072787080933, + "velocityX": 3.604782492980222, + "velocityY": 0.5656213980739361, + "timestamp": 1.703497155548545 + }, + { + "x": 6.83451270884544, + "y": 7.40648762220655, + "heading": -0.0000033399372412399076, + "angularVelocity": 0.000038656154204017926, + "velocityX": 3.6047775144789, + "velocityY": 0.5656531355692576, + "timestamp": 1.7431932437363975 + }, + { + "x": 6.977608912847965, + "y": 7.428937772164743, + "heading": -0.0000018048282304933975, + "angularVelocity": 0.00003867154374202805, + "velocityX": 3.6047935838250287, + "velocityY": 0.5655506873111692, + "timestamp": 1.78288933192425 + }, + { + "x": 7.12091588973999, + "y": 7.45, + "heading": 5.756100309873575e-19, + "angularVelocity": 0.000045466148249922065, + "velocityX": 3.6101032478025736, + "velocityY": 0.5305869872012366, + "timestamp": 1.8225854201121026 + }, + { + "x": 7.357842782347066, + "y": 7.451140658234923, + "heading": 1.3946076252127016e-14, + "angularVelocity": 2.1477158274944538e-13, + "velocityX": 3.6488588704771803, + "velocityY": 0.017567026152596314, + "timestamp": 1.8875171976944158 + }, + { + "x": 7.594772307901899, + "y": 7.450909424040305, + "heading": 1.3956817600616414e-14, + "angularVelocity": 1.6542514133334603e-16, + "velocityX": 3.648899419925491, + "velocityY": -0.003561186883034476, + "timestamp": 1.9524489752767291 + }, + { + "x": 7.831701831188383, + "y": 7.450675877221787, + "heading": 1.3958919096191331e-14, + "angularVelocity": 3.236467023043882e-17, + "velocityX": 3.6488993849911218, + "velocityY": -0.00359680309416508, + "timestamp": 2.0173807528590424 + }, + { + "x": 8.06023649674366, + "y": 7.450480884382308, + "heading": 1.3831887827184638e-14, + "angularVelocity": -1.9563805854348244e-15, + "velocityX": 3.519612030728179, + "velocityY": -0.0030030417576737706, + "timestamp": 2.0823125304413557 + }, + { + "x": 8.255358856071796, + "y": 7.450318830701623, + "heading": 1.0574442032523106e-14, + "angularVelocity": -5.016720496430017e-14, + "velocityX": 3.0050364643226115, + "velocityY": -0.002495753030620786, + "timestamp": 2.147244308023669 + }, + { + "x": 8.417068891314898, + "y": 7.450189715578673, + "heading": 6.865176887322045e-15, + "angularVelocity": -5.712557523159656e-14, + "velocityX": 2.4904606228915314, + "velocityY": -0.0019884735603726703, + "timestamp": 2.2121760856059822 + }, + { + "x": 8.54536659652034, + "y": 7.450093538813877, + "heading": 3.601695099473894e-15, + "angularVelocity": -5.026016396528296e-14, + "velocityX": 1.975884689785358, + "velocityY": -0.001481197163799284, + "timestamp": 2.2771078631882955 + }, + { + "x": 8.640251968711807, + "y": 7.450030300307574, + "heading": 1.2253067804043182e-15, + "angularVelocity": -3.659823290767036e-14, + "velocityX": 1.4613087108416603, + "velocityY": -0.0009739223021094422, + "timestamp": 2.3420396407706088 }, { "x": 8.701725006103516, - "y": 6.490337371826172, - "heading": -6.207507223536792e-26, - "angularVelocity": -4.028772886652685e-13, - "velocityX": 0.9454088132495556, - "velocityY": -0.035381228957146495, - "timestamp": 2.4357046367775426 + "y": 7.45, + "heading": -1.9088649305256032e-26, + "angularVelocity": -1.8870679751086276e-14, + "velocityX": 0.9467327043954562, + "velocityY": -0.00046664836083104116, + "timestamp": 2.406971418352922 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 1.0661056930411766e-25, - "angularVelocity": 3.714877122041656e-24, - "velocityX": 0.47336647327631065, - "velocityY": 3.668684939131766e-15, - "timestamp": 2.4954363573212164 + "y": 7.45, + "heading": 2.623532798918036e-26, + "angularVelocity": 9.781191839132487e-25, + "velocityX": 0.4733664733221421, + "velocityY": 5.037124173048106e-15, + "timestamp": 2.4667031388908103 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 5.321036058198176e-26, - "angularVelocity": -3.1783468612327103e-27, - "velocityX": -2.6405211555124906e-21, - "velocityY": -7.26970172585428e-20, - "timestamp": 2.55516807786489 + "y": 7.45, + "heading": 1.3100764528202313e-26, + "angularVelocity": -5.658464945099905e-28, + "velocityX": 7.815178981981403e-20, + "velocityY": 1.9973998116523263e-19, + "timestamp": 2.5264348594286985 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.1.traj b/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.1.traj index c450529..bcce6a0 100644 --- a/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.1.traj +++ b/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.1.traj @@ -1,517 +1,526 @@ { "samples": [ { - "x": 1.77, + "x": 1.9, "y": 7.9, "heading": -1.571, - "angularVelocity": -1.2871748405282096e-30, - "velocityX": -3.4109449165522936e-23, - "velocityY": 6.001012791624122e-24, + "angularVelocity": -3.4642760485591173e-29, + "velocityX": 3.5005463994906275e-27, + "velocityY": -4.952563831230141e-28, "timestamp": 0 }, { - "x": 1.807900003964869, - "y": 7.893333332635907, + "x": 1.9335666703903378, + "y": 7.893333332593777, "heading": -1.571, - "angularVelocity": -2.4513789653454947e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.06968380844403477 + "angularVelocity": -2.7619800848237704e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.06571406926224227 }, { - "x": 1.8837000104077808, - "y": 7.879999998169256, - "heading": -1.571, - "angularVelocity": -2.451378965313512e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.13936761688806953 + "x": 2.0007000097746364, + "y": 7.879999998058663, + "heading": -1.5710000000000002, + "angularVelocity": -2.761980084878875e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.13142813852448454 }, { - "x": 1.9974000168506918, - "y": 7.859999997035938, + "x": 2.1014000158256008, + "y": 7.8599999968568826, "heading": -1.5710000000000002, - "angularVelocity": -2.45137896507651e-18, - "velocityX": 1.631656032896467, - "velocityY": -0.28701073577774294, - "timestamp": 0.2090514253321043 + "angularVelocity": -2.7619800849390504e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.30434884684993946, + "timestamp": 0.1971422077867268 }, { - "x": 2.149000018337516, - "y": 7.833333330107737, + "x": 2.2356666838886423, + "y": 7.8333333299128824, "heading": -1.5710000000000002, - "angularVelocity": -2.451378965249631e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.27873523377613907 + "angularVelocity": -2.76198008487185e-18, + "velocityX": 2.0431951569949107, + "velocityY": -0.40579844230286244, + "timestamp": 0.2628562770489691 }, { - "x": 2.3385, + "x": 2.4034999999999997, "y": 7.800000000000001, "heading": -1.5710000000000002, - "angularVelocity": -2.451369395746655e-18, - "velocityX": 2.7194263042422167, - "velocityY": -0.4783511529010061, - "timestamp": 0.34841904222017384 + "angularVelocity": -2.7619800848365147e-18, + "velocityX": 2.5539936576076663, + "velocityY": -0.507247995552665, + "timestamp": 0.32857034631121135 }, { - "x": 2.527999981662484, - "y": 7.766666669892264, + "x": 2.5713333161113576, + "y": 7.766666670087118, "heading": -1.5710000000000002, - "angularVelocity": -2.4513825638540725e-18, - "velocityX": 2.719426304242217, - "velocityY": -0.47835115290100616, - "timestamp": 0.4181028506642086 + "angularVelocity": -2.7619800848960175e-18, + "velocityX": 2.553993657607666, + "velocityY": -0.5072479955526649, + "timestamp": 0.3942844155734536 }, { - "x": 2.6795999831493083, - "y": 7.740000002964063, - "heading": -1.5710000000000002, - "angularVelocity": -2.4513789650501425e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.48778665910824337 + "x": 2.705599984174399, + "y": 7.740000003143118, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084797486e-18, + "velocityX": 2.0431951569949103, + "velocityY": -0.40579844230286233, + "timestamp": 0.4599984848356959 }, { - "x": 2.7932999895922195, - "y": 7.720000001830744, - "heading": -1.5710000000000002, - "angularVelocity": -2.451378965525874e-18, - "velocityX": 1.6316560328964669, - "velocityY": -0.287010735777743, - "timestamp": 0.5574704675522781 + "x": 2.8062999902253636, + "y": 7.7200000019413375, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084858984e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.3043488468499394, + "timestamp": 0.5257125540979382 }, { - "x": 2.869099996035131, - "y": 7.706666667364093, + "x": 2.873433329609662, + "y": 7.706666667406224, "heading": -1.5710000000000004, - "angularVelocity": -2.45137896490822e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.6271542759963129 + "angularVelocity": -2.7619800847561526e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.5914266233601804 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -2.4513789653928325e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.6968380844403477 + "angularVelocity": -2.76198008510439e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.6571406926224226 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -8.171263216835412e-19, - "velocityX": 3.118534113064725e-17, - "velocityY": -4.765080041362501e-18, - "timestamp": 0.7665218928843824 - }, - { - "x": 2.9235569599541296, - "y": 7.698949367220036, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653510822e-18, - "velocityX": 0.361868027527245, - "velocityY": -0.022962573612202903, - "timestamp": 0.8122760316716063 + "angularVelocity": -9.20660028293447e-19, + "velocityX": 3.665984348782275e-17, + "velocityY": -5.747314644128388e-18, + "timestamp": 0.7228547618846648 }, { - "x": 2.9566708794154706, - "y": 7.6968481016884684, + "x": 2.923556957543874, + "y": 7.698949367372981, "heading": -1.5710000000000004, - "angularVelocity": -1.1603542652771816e-18, - "velocityX": 0.7237360452866696, - "velocityY": -0.04592514660458234, - "timestamp": 0.8580301704588302 + "angularVelocity": -1.4410194825190118e-18, + "velocityX": 0.3618680011852347, + "velocityY": -0.02296257194065333, + "timestamp": 0.7686088973419505 }, { - "x": 3.006341757820839, - "y": 7.693696203441033, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653006352e-18, - "velocityX": 1.085604050737173, - "velocityY": -0.0688877188158911, - "timestamp": 0.9037843092460541 + "x": 2.9566708721846076, + "y": 7.696848102147308, + "heading": -1.5710000000000006, + "angularVelocity": -1.4410194825318246e-18, + "velocityX": 0.7237359926005489, + "velocityY": -0.045925143261349936, + "timestamp": 0.8143630327992362 }, { - "x": 3.0725695944386313, - "y": 7.689493672524154, + "x": 3.0063417433588766, + "y": 7.6936962043587265, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653002277e-18, - "velocityX": 1.447472040197785, - "velocityY": -0.0918502900125508, - "timestamp": 0.949538448033278 + "angularVelocity": -1.4410194826287186e-18, + "velocityX": 1.085603971703869, + "velocityY": -0.06888771380078085, + "timestamp": 0.8601171682565218 }, { - "x": 3.1553543882799935, - "y": 7.684240509000582, + "x": 3.0725695703348617, + "y": 7.689493674053676, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653590159e-18, - "velocityX": 1.8093400080460489, - "velocityY": -0.11481285983778473, - "timestamp": 0.9952925868205019 + "angularVelocity": -1.441019482377035e-18, + "velocityX": 1.447471934812594, + "velocityY": -0.09185028332526422, + "timestamp": 0.9058713037138075 }, { - "x": 3.254696137934192, - "y": 7.677936712959833, + "x": 3.1553543521233594, + "y": 7.6842405112949255, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542649957643e-18, - "velocityX": 2.1712079450614197, - "velocityY": -0.13777542770649714, - "timestamp": 1.0410467256077258 + "angularVelocity": -1.4410194823401156e-18, + "velocityX": 1.8093398763013369, + "velocityY": -0.11481285147783742, + "timestamp": 0.9516254391710932 }, { - "x": 3.3705948412254285, - "y": 7.670582284539977, + "x": 3.2546960873130226, + "y": 7.6779367161720335, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651840332e-18, - "velocityX": 2.53307583452264, - "velocityY": -0.16073799255762966, - "timestamp": 1.0868008643949496 + "angularVelocity": -1.4410194827029632e-18, + "velocityX": 2.171207786943753, + "velocityY": -0.13777541767303667, + "timestamp": 0.9973795746283789 }, { - "x": 3.503050494358926, - "y": 7.662177223981812, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653102151e-18, - "velocityX": 2.8949436410454266, - "velocityY": -0.1837005521458494, - "timestamp": 1.1325550031821736 + "x": 3.370594773726837, + "y": 7.6705822888231445, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194824370812e-18, + "velocityX": 2.5330756500054417, + "velocityY": -0.16073798084896954, + "timestamp": 1.0431337100856646 }, { - "x": 3.652063089043379, - "y": 7.652721531811468, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651181787e-18, - "velocityX": 3.2568112663538877, - "velocityY": -0.2066631002349949, - "timestamp": 1.1783091419693976 + "x": 3.5030504075671254, + "y": 7.6621772294892425, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194825282925e-18, + "velocityX": 2.8949434300653256, + "velocityY": -0.1837005387579684, + "timestamp": 1.0888878455429503 }, { - "x": 3.817632592880325, - "y": 7.642215210084812, + "x": 3.6520629805328038, + "y": 7.652721538697079, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542640713407e-18, - "velocityX": 3.6186781835632806, - "velocityY": -0.2296256033912485, - "timestamp": 1.2240632807566216 + "angularVelocity": -1.4410194824897673e-18, + "velocityX": 3.2568110286971184, + "velocityY": -0.20666308515432835, + "timestamp": 1.134641981000236 }, { - "x": 3.9666453345551678, - "y": 7.632759508587096, + "x": 3.817632460165024, + "y": 7.642215218506348, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542647952754e-18, - "velocityX": 3.256814478955544, - "velocityY": -0.20666330409274528, - "timestamp": 1.2698174195438456 + "angularVelocity": -1.4410194819422767e-18, + "velocityX": 3.61867791791012, + "velocityY": -0.22962558653405252, + "timestamp": 1.1803961164575216 }, { - "x": 4.099101141061488, - "y": 7.624354438296557, + "x": 3.9666452436917647, + "y": 7.632759514352893, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542651048875e-18, - "velocityX": 2.894946993141186, - "velocityY": -0.1837007648552898, - "timestamp": 1.3155715583310696 + "angularVelocity": -1.4410194824624615e-18, + "velocityX": 3.25681563069441, + "velocityY": -0.2066633771770923, + "timestamp": 1.2261502519148073 + }, + { + "x": 4.099101094429873, + "y": 7.624354441255597, + "heading": -1.571000000000001, + "angularVelocity": -1.4410194824570077e-18, + "velocityX": 2.894948170558449, + "velocityY": -0.18370083956907635, + "timestamp": 1.271904387372093 }, { "x": 4.215, "y": 7.617, - "heading": -1.5710000000000008, - "angularVelocity": -3.5267965497472325e-18, - "velocityX": 2.533079236328726, - "velocityY": -0.16073820842147152, - "timestamp": 1.3613256971182937 - }, - { - "x": 4.372102638958384, - "y": 7.604137670056067, - "heading": -1.4067929344278918, - "angularVelocity": 2.5012754729123667, - "velocityX": 2.3930576689167724, - "velocityY": -0.19592476305046946, - "timestamp": 1.426975029768865 - }, - { - "x": 4.504023469666986, - "y": 7.584807496549773, - "heading": -1.1481197216785133, - "angularVelocity": 3.940226081589852, - "velocityX": 2.0094771017860147, - "velocityY": -0.2944458492712955, - "timestamp": 1.4926243624194364 - }, - { - "x": 4.60700440416476, - "y": 7.557706508847077, - "heading": -0.8272570767570808, - "angularVelocity": 4.887523329879827, - "velocityX": 1.5686516578342056, - "velocityY": -0.41281436701235225, - "timestamp": 1.5582736950700078 - }, - { - "x": 4.700954879611763, - "y": 7.512168160289293, - "heading": -0.46366508061347683, - "angularVelocity": 5.538395920632406, - "velocityX": 1.431095666224292, - "velocityY": -0.6936604946152823, - "timestamp": 1.6239230277205792 - }, - { - "x": 4.823042014881433, - "y": 7.4730785715901, - "heading": -0.16917269346197744, - "angularVelocity": 4.485839768083199, - "velocityX": 1.8596858542233212, - "velocityY": -0.5954300999091777, - "timestamp": 1.6895723603711506 - }, - { - "x": 4.964956593821971, - "y": 7.435238066935198, - "heading": 2.8484788827976314e-16, - "angularVelocity": 2.57691413806861, - "velocityX": 2.161706345068334, - "velocityY": -0.576403493029435, - "timestamp": 1.755221693021722 + "heading": -1.571000000000001, + "angularVelocity": -4.672933317458188e-18, + "velocityX": 2.5330804398600892, + "velocityY": -0.1607382847923455, + "timestamp": 1.3176585228293787 + }, + { + "x": 4.372100003975642, + "y": 7.604134525546081, + "heading": -1.4068015764148147, + "angularVelocity": 2.5011316235769736, + "velocityX": 2.3930058488366557, + "velocityY": -0.19597170488397217, + "timestamp": 1.383308175985225 + }, + { + "x": 4.50402015326741, + "y": 7.584792726357379, + "heading": -1.1481306958042976, + "angularVelocity": 3.940171321174791, + "velocityX": 2.009456911811971, + "velocityY": -0.2946214985011475, + "timestamp": 1.4489578291410714 + }, + { + "x": 4.607002128532945, + "y": 7.557661434831198, + "heading": -0.8272795511530444, + "angularVelocity": 4.887324292310532, + "velocityX": 1.5686598529527007, + "velocityY": -0.41327395077246554, + "timestamp": 1.5146074822969178 + }, + { + "x": 4.700950132368006, + "y": 7.512144695800665, + "heading": -0.46367956909020736, + "angularVelocity": 5.5384905263762105, + "velocityX": 1.4310510310194355, + "velocityY": -0.6933279437461265, + "timestamp": 1.5802571354527641 + }, + { + "x": 4.8230397238523155, + "y": 7.473064704270448, + "heading": -0.16917524125362357, + "angularVelocity": 4.485999752933481, + "velocityX": 1.8597141891098792, + "velocityY": -0.5952810053284208, + "timestamp": 1.6459067886086105 + }, + { + "x": 4.964955510700854, + "y": 7.435231531409891, + "heading": 3.1934304094208187e-16, + "angularVelocity": 2.5769403663425607, + "velocityX": 2.1617141908013076, + "velocityY": -0.5762889983690314, + "timestamp": 1.7115564417644569 }, { "x": 5.1133599281311035, "y": 7.399226188659668, - "heading": 2.560323167481079e-16, - "angularVelocity": -9.137154494425779e-18, - "velocityX": 2.260545969274894, - "velocityY": -0.5485490380710709, - "timestamp": 1.8208710256722933 - }, - { - "x": 5.309797730179564, - "y": 7.378773507546932, - "heading": 2.2755149667394e-16, - "angularVelocity": -2.487788098670228e-18, - "velocityX": 2.760397768454213, - "velocityY": -0.2874066738371958, - "timestamp": 1.8920338864357054 - }, - { - "x": 5.5370497739551165, - "y": 7.384033087440106, - "heading": 1.9907067659976473e-16, - "angularVelocity": -2.4877880987734015e-18, - "velocityX": 3.193407928482695, - "velocityY": 0.07390905644553293, - "timestamp": 1.9631967471991174 - }, - { - "x": 5.734541367744454, - "y": 7.416217562641249, - "heading": 1.7058985652559142e-16, - "angularVelocity": -2.4877880987466103e-18, - "velocityX": 2.7752059384736096, - "velocityY": 0.4522650559006595, - "timestamp": 2.0343596079625295 - }, - { - "x": 5.8919109073823455, - "y": 7.447474939732356, - "heading": 1.421090364514119e-16, - "angularVelocity": -2.4877880988332663e-18, - "velocityX": 2.211399850283212, - "velocityY": 0.43923721946865085, - "timestamp": 2.1055224687259413 - }, - { - "x": 6.009566431740217, - "y": 7.472949876850092, - "heading": 1.1362821637723588e-16, - "angularVelocity": -2.487788098784733e-18, - "velocityX": 1.6533276360131506, - "velocityY": 0.3579807900424195, - "timestamp": 2.176685329489353 - }, - { - "x": 6.087823373238344, - "y": 7.4907833719278125, - "heading": 8.51473963030607e-17, - "angularVelocity": -2.4877880987719023e-18, - "velocityX": 1.0996879644611663, - "velocityY": 0.2506011546812913, - "timestamp": 2.247848190252765 + "heading": 2.8711384441848255e-16, + "angularVelocity": -9.522901504497603e-18, + "velocityX": 2.260551431666984, + "velocityY": -0.5484468084647249, + "timestamp": 1.7772060949203032 + }, + { + "x": 5.309798294852915, + "y": 7.378778747778154, + "heading": 2.551760796876138e-16, + "angularVelocity": -2.748952599816597e-18, + "velocityX": 2.7604163011269454, + "velocityY": -0.2873341398001768, + "timestamp": 1.8483686824768482 + }, + { + "x": 5.537052553943782, + "y": 7.384041108580089, + "heading": 2.232383149567335e-16, + "angularVelocity": -2.7489525999792118e-18, + "velocityX": 3.193451318926863, + "velocityY": 0.07394841843766191, + "timestamp": 1.919531270033393 + }, + { + "x": 5.7345430393183845, + "y": 7.4162242310813244, + "heading": 1.9130055022585706e-16, + "angularVelocity": -2.7489525999248097e-18, + "velocityX": 2.775201017216226, + "velocityY": 0.45224778365094176, + "timestamp": 1.990693857589938 + }, + { + "x": 5.89191179894478, + "y": 7.447479383073381, + "heading": 1.593627854949789e-16, + "angularVelocity": -2.748952599950355e-18, + "velocityX": 2.2113973792956694, + "velocityY": 0.4392076379638292, + "timestamp": 2.061856445146483 + }, + { + "x": 6.009566832246889, + "y": 7.472952249257499, + "heading": 1.2742502076410038e-16, + "angularVelocity": -2.748952599954475e-18, + "velocityX": 1.653327082979625, + "velocityY": 0.35795306296166557, + "timestamp": 2.133019032703028 + }, + { + "x": 6.087823494374736, + "y": 7.490784200402746, + "heading": 9.54872560332223e-17, + "angularVelocity": -2.748952599948443e-18, + "velocityX": 1.0996882605721798, + "velocityY": 0.25058042094223126, + "timestamp": 2.2041816202595728 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 5.666657622888386e-17, - "angularVelocity": -2.4877880987955836e-18, - "velocityX": 0.5488836890134096, - "velocityY": 0.12951458068604244, - "timestamp": 2.3190110510161768 + "heading": 6.354949130235964e-17, + "angularVelocity": -2.748952599731403e-18, + "velocityX": 0.5488840940352727, + "velocityY": 0.12950343591616847, + "timestamp": 2.2753442078161177 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 2.830378172433852e-17, - "angularVelocity": -8.292626564371985e-19, - "velocityX": 1.450767762103865e-19, - "velocityY": -4.7804854612827675e-19, - "timestamp": 2.3901739117795886 - }, - { - "x": 6.15611975515317, - "y": 7.501785785181937, - "heading": 2.830378172433841e-17, - "angularVelocity": -1.7359237081093472e-30, - "velocityX": 0.4808977303168891, - "velocityY": 0.02937381122626305, - "timestamp": 2.450969059136598 - }, - { - "x": 6.2145922519504335, - "y": 7.505357339791126, - "heading": 2.8303781724338447e-17, - "angularVelocity": 6.560557249735871e-31, - "velocityX": 0.9617954613037508, - "velocityY": 0.0587473633087091, - "timestamp": 2.5117642064936074 - }, - { - "x": 6.302300997223708, - "y": 7.5107146417709965, - "heading": 2.8303781724344634e-17, - "angularVelocity": 1.0166114443831184e-28, - "velocityX": 1.44269319322838, - "velocityY": 0.08812055258968164, - "timestamp": 2.572559353850617 - }, - { - "x": 6.419245991058483, - "y": 7.51785765803668, - "heading": 2.830378172433783e-17, - "angularVelocity": -1.1196675906591495e-28, - "velocityX": 1.9235909265592357, - "velocityY": 0.11749319766817592, - "timestamp": 2.6333545012076263 - }, - { - "x": 6.565427233597177, - "y": 7.526786333446685, - "heading": 2.830378172467021e-17, - "angularVelocity": 5.467389383569676e-27, - "velocityX": 2.4044886622326676, - "velocityY": 0.14686493574187343, - "timestamp": 2.6941496485646357 - }, - { - "x": 6.740844725124364, - "y": 7.537500557717893, - "heading": 2.8303781724055295e-17, - "angularVelocity": -8.483831515101901e-30, - "velocityX": 2.8853864025869926, - "velocityY": 0.1762348598037055, - "timestamp": 2.754944795921645 + "heading": 3.1742141835585016e-17, + "angularVelocity": -9.163175252374902e-19, + "velocityX": -2.2913448895975365e-19, + "velocityY": -1.0265149362709145e-18, + "timestamp": 2.3465067953726626 + }, + { + "x": 6.156119663954512, + "y": 7.501787279405357, + "heading": 3.174214182765833e-17, + "angularVelocity": 1.1020322934166523e-25, + "velocityX": 0.48089622932964227, + "velocityY": 0.029398389176605127, + "timestamp": 2.4073019428421385 + }, + { + "x": 6.214591998656713, + "y": 7.50536149033989, + "heading": 3.1742141831543016e-17, + "angularVelocity": 6.390315645162633e-26, + "velocityX": 0.9617927932744761, + "velocityY": 0.058791056248813034, + "timestamp": 2.4680970903116144 + }, + { + "x": 6.302300539353782, + "y": 7.51072214579269, + "heading": 3.1742141831543016e-17, + "angularVelocity": 5.527535991984969e-30, + "velocityX": 1.4426898255507186, + "velocityY": 0.08817571263384313, + "timestamp": 2.5288922377810903 + }, + { + "x": 6.4192453287368645, + "y": 7.517868515281858, + "heading": 3.1742141831542363e-17, + "angularVelocity": -5.5005027620127605e-30, + "velocityX": 1.9235875600400183, + "velocityY": 0.11754835355494872, + "timestamp": 2.589687385250566 + }, + { + "x": 6.565426437911824, + "y": 7.526799381429512, + "heading": 3.174214183154203e-17, + "angularVelocity": -1.9865124104755493e-32, + "velocityX": 2.404486464127003, + "velocityY": 0.14690097021540635, + "timestamp": 2.650482532720042 + }, + { + "x": 6.740844008917749, + "y": 7.537512309825002, + "heading": 3.1742141831543275e-17, + "angularVelocity": 2.560924529534356e-29, + "velocityX": 2.8853877045696796, + "velocityY": 0.17621354403110642, + "timestamp": 2.711277680189518 }, { "x": 6.945498466491699, "y": 7.55, - "heading": 2.8303781724055554e-17, - "angularVelocity": 4.2167202635793064e-30, - "velocityX": 3.3662841569498863, - "velocityY": 0.20559934181433973, - "timestamp": 2.8157399432786545 - }, - { - "x": 7.229321610470641, - "y": 7.550003362381949, - "heading": 1.1593994241588832e-17, - "angularVelocity": -2.1482520451536383e-16, - "velocityX": 3.6489012810873196, - "velocityY": 0.00004322762277017461, - "timestamp": 2.8935231229579816 - }, - { - "x": 7.5131447544694705, - "y": 7.5500027545013815, - "heading": 7.195372591415266e-18, - "angularVelocity": -5.654977937784957e-17, - "velocityX": 3.6489012813430093, - "velocityY": -0.000007815064517695191, - "timestamp": 2.9713063026373088 - }, - { - "x": 7.796967898468301, - "y": 7.55000214698679, - "heading": -2.2105881487706106e-17, - "angularVelocity": -3.7670424634117877e-16, - "velocityX": 3.6489012813430186, - "velocityY": -0.00000781035943334551, - "timestamp": 3.049089482316636 - }, - { - "x": 8.080791042466867, - "y": 7.5500015394568125, - "heading": -3.9909490658933576e-17, - "angularVelocity": -2.28887649548727e-16, - "velocityX": 3.6489012813396258, - "velocityY": -0.000007810557249203658, - "timestamp": 3.126872661995963 - }, - { - "x": 8.320527341192129, - "y": 7.550001026304512, - "heading": -3.990273647201336e-17, - "angularVelocity": 8.683351527189107e-20, - "velocityX": 3.082109778921481, - "velocityY": -0.000006597214238392456, - "timestamp": 3.20465584167529 - }, - { - "x": 8.51231639819258, - "y": 7.550000615782698, - "heading": -3.9899086872036086e-17, - "angularVelocity": 4.6920169533433444e-20, - "velocityX": 2.465688054809929, - "velocityY": -0.0000052777710475807675, - "timestamp": 3.2824390213546173 - }, - { - "x": 8.656158196885205, - "y": 7.550000307891346, - "heading": -3.989681664095531e-17, - "angularVelocity": 2.9186658222831174e-20, - "velocityX": 1.8492661175029514, - "velocityY": -0.00000395832817870783, - "timestamp": 3.3602222010339444 - }, - { - "x": 8.752052731742296, - "y": 7.550000102630447, - "heading": -3.9895416797891945e-17, - "angularVelocity": 1.7996732314772988e-20, - "velocityX": 1.2328441091304374, - "velocityY": -0.0000026388854100440123, - "timestamp": 3.4380053807132716 - }, - { - "x": 8.8, + "heading": 3.174214183411669e-17, + "angularVelocity": 4.2334783572302846e-26, + "velocityX": 3.3662959313768197, + "velocityY": 0.2054060347705879, + "timestamp": 2.772072827658994 + }, + { + "x": 7.221347124992817, + "y": 7.550083175294458, + "heading": 2.582386400207731e-18, + "angularVelocity": -3.857226029687031e-16, + "velocityX": 3.6489010624124374, + "velocityY": 0.0011002352593078705, + "timestamp": 2.8476705634657145 + }, + { + "x": 7.4971957957486515, + "y": 7.550070265650631, + "heading": -3.139160687691863e-17, + "angularVelocity": -4.494049049826975e-16, + "velocityX": 3.6489012245167154, + "velocityY": -0.00017076759891002745, + "timestamp": 2.923268299272435 + }, + { + "x": 7.7730444665050475, + "y": 7.550057368010946, + "heading": -4.4250107790457757e-17, + "angularVelocity": -1.700910850877103e-16, + "velocityX": 3.648901224524143, + "velocityY": -0.00017060880920745955, + "timestamp": 2.9988660350791556 + }, + { + "x": 8.04889313726144, + "y": 7.550044470349665, + "heading": -3.3139122030001965e-17, + "angularVelocity": 1.4697511296982546e-16, + "velocityX": 3.648901224524074, + "velocityY": -0.00017060909487947776, + "timestamp": 3.074463770885876 + }, + { + "x": 8.320637928532271, + "y": 7.550031764514427, + "heading": -3.3149587273190146e-17, + "angularVelocity": -1.3843328871118217e-19, + "velocityX": 3.594615478505821, + "velocityY": -0.00016807163736953773, + "timestamp": 3.1500615066925968 + }, + { + "x": 8.54709194269371, + "y": 7.5500211763353215, + "heading": -3.3161210294625444e-17, + "angularVelocity": -1.5374827446451776e-19, + "velocityX": 2.99551318230494, + "velocityY": -0.0001400594738974264, + "timestamp": 3.2256592424993173 + }, + { + "x": 8.728255161631012, + "y": 7.5500127057980455, + "heading": -3.317341059346824e-17, + "angularVelocity": -1.613844477074772e-19, + "velocityX": 2.39641064648388, + "velocityY": -0.00011204749964645467, + "timestamp": 3.301256978306038 + }, + { + "x": 8.864127579305954, + "y": 7.550006352897831, + "heading": -3.318651341446678e-17, + "angularVelocity": -1.7332292901257806e-19, + "velocityX": 1.797308030789794, + "velocityY": -0.00008403558846668993, + "timestamp": 3.3768547141127585 + }, + { + "x": 8.954709192699438, + "y": 7.5500021176322925, + "heading": -3.320067915378452e-17, + "angularVelocity": -1.8738311623596625e-19, + "velocityX": 1.19820537515927, + "velocityY": -0.00005602370882446872, + "timestamp": 3.452452449919479 + }, + { + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": -3.908565443774477e-21, - "velocityX": 0.6164220652250822, - "velocityY": -0.000001319442690505465, - "timestamp": 3.5157885603925987 + "heading": -3.3215694928340293e-17, + "angularVelocity": -1.9862730534835854e-19, + "velocityX": 0.5991026955669089, + "velocityY": -0.00002801184810473847, + "timestamp": 3.5280501857261997 }, { - "x": 8.8, + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": 2.0117689927326026e-32, - "velocityX": 8.595312932705472e-20, - "velocityY": 5.963533551498734e-19, - "timestamp": 3.593571740071926 + "heading": -3.321569492834034e-17, + "angularVelocity": -5.16984725650092e-31, + "velocityX": 2.95751067102717e-19, + "velocityY": 2.6561323414610804e-18, + "timestamp": 3.6036479215329202 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.traj b/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.traj index c450529..bcce6a0 100644 --- a/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.traj +++ b/src/main/deploy/choreo/B_PickUpFarPiece1AfterNear.traj @@ -1,517 +1,526 @@ { "samples": [ { - "x": 1.77, + "x": 1.9, "y": 7.9, "heading": -1.571, - "angularVelocity": -1.2871748405282096e-30, - "velocityX": -3.4109449165522936e-23, - "velocityY": 6.001012791624122e-24, + "angularVelocity": -3.4642760485591173e-29, + "velocityX": 3.5005463994906275e-27, + "velocityY": -4.952563831230141e-28, "timestamp": 0 }, { - "x": 1.807900003964869, - "y": 7.893333332635907, + "x": 1.9335666703903378, + "y": 7.893333332593777, "heading": -1.571, - "angularVelocity": -2.4513789653454947e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.06968380844403477 + "angularVelocity": -2.7619800848237704e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.06571406926224227 }, { - "x": 1.8837000104077808, - "y": 7.879999998169256, - "heading": -1.571, - "angularVelocity": -2.451378965313512e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.13936761688806953 + "x": 2.0007000097746364, + "y": 7.879999998058663, + "heading": -1.5710000000000002, + "angularVelocity": -2.761980084878875e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.13142813852448454 }, { - "x": 1.9974000168506918, - "y": 7.859999997035938, + "x": 2.1014000158256008, + "y": 7.8599999968568826, "heading": -1.5710000000000002, - "angularVelocity": -2.45137896507651e-18, - "velocityX": 1.631656032896467, - "velocityY": -0.28701073577774294, - "timestamp": 0.2090514253321043 + "angularVelocity": -2.7619800849390504e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.30434884684993946, + "timestamp": 0.1971422077867268 }, { - "x": 2.149000018337516, - "y": 7.833333330107737, + "x": 2.2356666838886423, + "y": 7.8333333299128824, "heading": -1.5710000000000002, - "angularVelocity": -2.451378965249631e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.27873523377613907 + "angularVelocity": -2.76198008487185e-18, + "velocityX": 2.0431951569949107, + "velocityY": -0.40579844230286244, + "timestamp": 0.2628562770489691 }, { - "x": 2.3385, + "x": 2.4034999999999997, "y": 7.800000000000001, "heading": -1.5710000000000002, - "angularVelocity": -2.451369395746655e-18, - "velocityX": 2.7194263042422167, - "velocityY": -0.4783511529010061, - "timestamp": 0.34841904222017384 + "angularVelocity": -2.7619800848365147e-18, + "velocityX": 2.5539936576076663, + "velocityY": -0.507247995552665, + "timestamp": 0.32857034631121135 }, { - "x": 2.527999981662484, - "y": 7.766666669892264, + "x": 2.5713333161113576, + "y": 7.766666670087118, "heading": -1.5710000000000002, - "angularVelocity": -2.4513825638540725e-18, - "velocityX": 2.719426304242217, - "velocityY": -0.47835115290100616, - "timestamp": 0.4181028506642086 + "angularVelocity": -2.7619800848960175e-18, + "velocityX": 2.553993657607666, + "velocityY": -0.5072479955526649, + "timestamp": 0.3942844155734536 }, { - "x": 2.6795999831493083, - "y": 7.740000002964063, - "heading": -1.5710000000000002, - "angularVelocity": -2.4513789650501425e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.48778665910824337 + "x": 2.705599984174399, + "y": 7.740000003143118, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084797486e-18, + "velocityX": 2.0431951569949103, + "velocityY": -0.40579844230286233, + "timestamp": 0.4599984848356959 }, { - "x": 2.7932999895922195, - "y": 7.720000001830744, - "heading": -1.5710000000000002, - "angularVelocity": -2.451378965525874e-18, - "velocityX": 1.6316560328964669, - "velocityY": -0.287010735777743, - "timestamp": 0.5574704675522781 + "x": 2.8062999902253636, + "y": 7.7200000019413375, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084858984e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.3043488468499394, + "timestamp": 0.5257125540979382 }, { - "x": 2.869099996035131, - "y": 7.706666667364093, + "x": 2.873433329609662, + "y": 7.706666667406224, "heading": -1.5710000000000004, - "angularVelocity": -2.45137896490822e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.6271542759963129 + "angularVelocity": -2.7619800847561526e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.5914266233601804 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -2.4513789653928325e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.6968380844403477 + "angularVelocity": -2.76198008510439e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.6571406926224226 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -8.171263216835412e-19, - "velocityX": 3.118534113064725e-17, - "velocityY": -4.765080041362501e-18, - "timestamp": 0.7665218928843824 - }, - { - "x": 2.9235569599541296, - "y": 7.698949367220036, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653510822e-18, - "velocityX": 0.361868027527245, - "velocityY": -0.022962573612202903, - "timestamp": 0.8122760316716063 + "angularVelocity": -9.20660028293447e-19, + "velocityX": 3.665984348782275e-17, + "velocityY": -5.747314644128388e-18, + "timestamp": 0.7228547618846648 }, { - "x": 2.9566708794154706, - "y": 7.6968481016884684, + "x": 2.923556957543874, + "y": 7.698949367372981, "heading": -1.5710000000000004, - "angularVelocity": -1.1603542652771816e-18, - "velocityX": 0.7237360452866696, - "velocityY": -0.04592514660458234, - "timestamp": 0.8580301704588302 + "angularVelocity": -1.4410194825190118e-18, + "velocityX": 0.3618680011852347, + "velocityY": -0.02296257194065333, + "timestamp": 0.7686088973419505 }, { - "x": 3.006341757820839, - "y": 7.693696203441033, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653006352e-18, - "velocityX": 1.085604050737173, - "velocityY": -0.0688877188158911, - "timestamp": 0.9037843092460541 + "x": 2.9566708721846076, + "y": 7.696848102147308, + "heading": -1.5710000000000006, + "angularVelocity": -1.4410194825318246e-18, + "velocityX": 0.7237359926005489, + "velocityY": -0.045925143261349936, + "timestamp": 0.8143630327992362 }, { - "x": 3.0725695944386313, - "y": 7.689493672524154, + "x": 3.0063417433588766, + "y": 7.6936962043587265, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653002277e-18, - "velocityX": 1.447472040197785, - "velocityY": -0.0918502900125508, - "timestamp": 0.949538448033278 + "angularVelocity": -1.4410194826287186e-18, + "velocityX": 1.085603971703869, + "velocityY": -0.06888771380078085, + "timestamp": 0.8601171682565218 }, { - "x": 3.1553543882799935, - "y": 7.684240509000582, + "x": 3.0725695703348617, + "y": 7.689493674053676, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653590159e-18, - "velocityX": 1.8093400080460489, - "velocityY": -0.11481285983778473, - "timestamp": 0.9952925868205019 + "angularVelocity": -1.441019482377035e-18, + "velocityX": 1.447471934812594, + "velocityY": -0.09185028332526422, + "timestamp": 0.9058713037138075 }, { - "x": 3.254696137934192, - "y": 7.677936712959833, + "x": 3.1553543521233594, + "y": 7.6842405112949255, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542649957643e-18, - "velocityX": 2.1712079450614197, - "velocityY": -0.13777542770649714, - "timestamp": 1.0410467256077258 + "angularVelocity": -1.4410194823401156e-18, + "velocityX": 1.8093398763013369, + "velocityY": -0.11481285147783742, + "timestamp": 0.9516254391710932 }, { - "x": 3.3705948412254285, - "y": 7.670582284539977, + "x": 3.2546960873130226, + "y": 7.6779367161720335, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651840332e-18, - "velocityX": 2.53307583452264, - "velocityY": -0.16073799255762966, - "timestamp": 1.0868008643949496 + "angularVelocity": -1.4410194827029632e-18, + "velocityX": 2.171207786943753, + "velocityY": -0.13777541767303667, + "timestamp": 0.9973795746283789 }, { - "x": 3.503050494358926, - "y": 7.662177223981812, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653102151e-18, - "velocityX": 2.8949436410454266, - "velocityY": -0.1837005521458494, - "timestamp": 1.1325550031821736 + "x": 3.370594773726837, + "y": 7.6705822888231445, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194824370812e-18, + "velocityX": 2.5330756500054417, + "velocityY": -0.16073798084896954, + "timestamp": 1.0431337100856646 }, { - "x": 3.652063089043379, - "y": 7.652721531811468, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651181787e-18, - "velocityX": 3.2568112663538877, - "velocityY": -0.2066631002349949, - "timestamp": 1.1783091419693976 + "x": 3.5030504075671254, + "y": 7.6621772294892425, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194825282925e-18, + "velocityX": 2.8949434300653256, + "velocityY": -0.1837005387579684, + "timestamp": 1.0888878455429503 }, { - "x": 3.817632592880325, - "y": 7.642215210084812, + "x": 3.6520629805328038, + "y": 7.652721538697079, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542640713407e-18, - "velocityX": 3.6186781835632806, - "velocityY": -0.2296256033912485, - "timestamp": 1.2240632807566216 + "angularVelocity": -1.4410194824897673e-18, + "velocityX": 3.2568110286971184, + "velocityY": -0.20666308515432835, + "timestamp": 1.134641981000236 }, { - "x": 3.9666453345551678, - "y": 7.632759508587096, + "x": 3.817632460165024, + "y": 7.642215218506348, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542647952754e-18, - "velocityX": 3.256814478955544, - "velocityY": -0.20666330409274528, - "timestamp": 1.2698174195438456 + "angularVelocity": -1.4410194819422767e-18, + "velocityX": 3.61867791791012, + "velocityY": -0.22962558653405252, + "timestamp": 1.1803961164575216 }, { - "x": 4.099101141061488, - "y": 7.624354438296557, + "x": 3.9666452436917647, + "y": 7.632759514352893, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542651048875e-18, - "velocityX": 2.894946993141186, - "velocityY": -0.1837007648552898, - "timestamp": 1.3155715583310696 + "angularVelocity": -1.4410194824624615e-18, + "velocityX": 3.25681563069441, + "velocityY": -0.2066633771770923, + "timestamp": 1.2261502519148073 + }, + { + "x": 4.099101094429873, + "y": 7.624354441255597, + "heading": -1.571000000000001, + "angularVelocity": -1.4410194824570077e-18, + "velocityX": 2.894948170558449, + "velocityY": -0.18370083956907635, + "timestamp": 1.271904387372093 }, { "x": 4.215, "y": 7.617, - "heading": -1.5710000000000008, - "angularVelocity": -3.5267965497472325e-18, - "velocityX": 2.533079236328726, - "velocityY": -0.16073820842147152, - "timestamp": 1.3613256971182937 - }, - { - "x": 4.372102638958384, - "y": 7.604137670056067, - "heading": -1.4067929344278918, - "angularVelocity": 2.5012754729123667, - "velocityX": 2.3930576689167724, - "velocityY": -0.19592476305046946, - "timestamp": 1.426975029768865 - }, - { - "x": 4.504023469666986, - "y": 7.584807496549773, - "heading": -1.1481197216785133, - "angularVelocity": 3.940226081589852, - "velocityX": 2.0094771017860147, - "velocityY": -0.2944458492712955, - "timestamp": 1.4926243624194364 - }, - { - "x": 4.60700440416476, - "y": 7.557706508847077, - "heading": -0.8272570767570808, - "angularVelocity": 4.887523329879827, - "velocityX": 1.5686516578342056, - "velocityY": -0.41281436701235225, - "timestamp": 1.5582736950700078 - }, - { - "x": 4.700954879611763, - "y": 7.512168160289293, - "heading": -0.46366508061347683, - "angularVelocity": 5.538395920632406, - "velocityX": 1.431095666224292, - "velocityY": -0.6936604946152823, - "timestamp": 1.6239230277205792 - }, - { - "x": 4.823042014881433, - "y": 7.4730785715901, - "heading": -0.16917269346197744, - "angularVelocity": 4.485839768083199, - "velocityX": 1.8596858542233212, - "velocityY": -0.5954300999091777, - "timestamp": 1.6895723603711506 - }, - { - "x": 4.964956593821971, - "y": 7.435238066935198, - "heading": 2.8484788827976314e-16, - "angularVelocity": 2.57691413806861, - "velocityX": 2.161706345068334, - "velocityY": -0.576403493029435, - "timestamp": 1.755221693021722 + "heading": -1.571000000000001, + "angularVelocity": -4.672933317458188e-18, + "velocityX": 2.5330804398600892, + "velocityY": -0.1607382847923455, + "timestamp": 1.3176585228293787 + }, + { + "x": 4.372100003975642, + "y": 7.604134525546081, + "heading": -1.4068015764148147, + "angularVelocity": 2.5011316235769736, + "velocityX": 2.3930058488366557, + "velocityY": -0.19597170488397217, + "timestamp": 1.383308175985225 + }, + { + "x": 4.50402015326741, + "y": 7.584792726357379, + "heading": -1.1481306958042976, + "angularVelocity": 3.940171321174791, + "velocityX": 2.009456911811971, + "velocityY": -0.2946214985011475, + "timestamp": 1.4489578291410714 + }, + { + "x": 4.607002128532945, + "y": 7.557661434831198, + "heading": -0.8272795511530444, + "angularVelocity": 4.887324292310532, + "velocityX": 1.5686598529527007, + "velocityY": -0.41327395077246554, + "timestamp": 1.5146074822969178 + }, + { + "x": 4.700950132368006, + "y": 7.512144695800665, + "heading": -0.46367956909020736, + "angularVelocity": 5.5384905263762105, + "velocityX": 1.4310510310194355, + "velocityY": -0.6933279437461265, + "timestamp": 1.5802571354527641 + }, + { + "x": 4.8230397238523155, + "y": 7.473064704270448, + "heading": -0.16917524125362357, + "angularVelocity": 4.485999752933481, + "velocityX": 1.8597141891098792, + "velocityY": -0.5952810053284208, + "timestamp": 1.6459067886086105 + }, + { + "x": 4.964955510700854, + "y": 7.435231531409891, + "heading": 3.1934304094208187e-16, + "angularVelocity": 2.5769403663425607, + "velocityX": 2.1617141908013076, + "velocityY": -0.5762889983690314, + "timestamp": 1.7115564417644569 }, { "x": 5.1133599281311035, "y": 7.399226188659668, - "heading": 2.560323167481079e-16, - "angularVelocity": -9.137154494425779e-18, - "velocityX": 2.260545969274894, - "velocityY": -0.5485490380710709, - "timestamp": 1.8208710256722933 - }, - { - "x": 5.309797730179564, - "y": 7.378773507546932, - "heading": 2.2755149667394e-16, - "angularVelocity": -2.487788098670228e-18, - "velocityX": 2.760397768454213, - "velocityY": -0.2874066738371958, - "timestamp": 1.8920338864357054 - }, - { - "x": 5.5370497739551165, - "y": 7.384033087440106, - "heading": 1.9907067659976473e-16, - "angularVelocity": -2.4877880987734015e-18, - "velocityX": 3.193407928482695, - "velocityY": 0.07390905644553293, - "timestamp": 1.9631967471991174 - }, - { - "x": 5.734541367744454, - "y": 7.416217562641249, - "heading": 1.7058985652559142e-16, - "angularVelocity": -2.4877880987466103e-18, - "velocityX": 2.7752059384736096, - "velocityY": 0.4522650559006595, - "timestamp": 2.0343596079625295 - }, - { - "x": 5.8919109073823455, - "y": 7.447474939732356, - "heading": 1.421090364514119e-16, - "angularVelocity": -2.4877880988332663e-18, - "velocityX": 2.211399850283212, - "velocityY": 0.43923721946865085, - "timestamp": 2.1055224687259413 - }, - { - "x": 6.009566431740217, - "y": 7.472949876850092, - "heading": 1.1362821637723588e-16, - "angularVelocity": -2.487788098784733e-18, - "velocityX": 1.6533276360131506, - "velocityY": 0.3579807900424195, - "timestamp": 2.176685329489353 - }, - { - "x": 6.087823373238344, - "y": 7.4907833719278125, - "heading": 8.51473963030607e-17, - "angularVelocity": -2.4877880987719023e-18, - "velocityX": 1.0996879644611663, - "velocityY": 0.2506011546812913, - "timestamp": 2.247848190252765 + "heading": 2.8711384441848255e-16, + "angularVelocity": -9.522901504497603e-18, + "velocityX": 2.260551431666984, + "velocityY": -0.5484468084647249, + "timestamp": 1.7772060949203032 + }, + { + "x": 5.309798294852915, + "y": 7.378778747778154, + "heading": 2.551760796876138e-16, + "angularVelocity": -2.748952599816597e-18, + "velocityX": 2.7604163011269454, + "velocityY": -0.2873341398001768, + "timestamp": 1.8483686824768482 + }, + { + "x": 5.537052553943782, + "y": 7.384041108580089, + "heading": 2.232383149567335e-16, + "angularVelocity": -2.7489525999792118e-18, + "velocityX": 3.193451318926863, + "velocityY": 0.07394841843766191, + "timestamp": 1.919531270033393 + }, + { + "x": 5.7345430393183845, + "y": 7.4162242310813244, + "heading": 1.9130055022585706e-16, + "angularVelocity": -2.7489525999248097e-18, + "velocityX": 2.775201017216226, + "velocityY": 0.45224778365094176, + "timestamp": 1.990693857589938 + }, + { + "x": 5.89191179894478, + "y": 7.447479383073381, + "heading": 1.593627854949789e-16, + "angularVelocity": -2.748952599950355e-18, + "velocityX": 2.2113973792956694, + "velocityY": 0.4392076379638292, + "timestamp": 2.061856445146483 + }, + { + "x": 6.009566832246889, + "y": 7.472952249257499, + "heading": 1.2742502076410038e-16, + "angularVelocity": -2.748952599954475e-18, + "velocityX": 1.653327082979625, + "velocityY": 0.35795306296166557, + "timestamp": 2.133019032703028 + }, + { + "x": 6.087823494374736, + "y": 7.490784200402746, + "heading": 9.54872560332223e-17, + "angularVelocity": -2.748952599948443e-18, + "velocityX": 1.0996882605721798, + "velocityY": 0.25058042094223126, + "timestamp": 2.2041816202595728 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 5.666657622888386e-17, - "angularVelocity": -2.4877880987955836e-18, - "velocityX": 0.5488836890134096, - "velocityY": 0.12951458068604244, - "timestamp": 2.3190110510161768 + "heading": 6.354949130235964e-17, + "angularVelocity": -2.748952599731403e-18, + "velocityX": 0.5488840940352727, + "velocityY": 0.12950343591616847, + "timestamp": 2.2753442078161177 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 2.830378172433852e-17, - "angularVelocity": -8.292626564371985e-19, - "velocityX": 1.450767762103865e-19, - "velocityY": -4.7804854612827675e-19, - "timestamp": 2.3901739117795886 - }, - { - "x": 6.15611975515317, - "y": 7.501785785181937, - "heading": 2.830378172433841e-17, - "angularVelocity": -1.7359237081093472e-30, - "velocityX": 0.4808977303168891, - "velocityY": 0.02937381122626305, - "timestamp": 2.450969059136598 - }, - { - "x": 6.2145922519504335, - "y": 7.505357339791126, - "heading": 2.8303781724338447e-17, - "angularVelocity": 6.560557249735871e-31, - "velocityX": 0.9617954613037508, - "velocityY": 0.0587473633087091, - "timestamp": 2.5117642064936074 - }, - { - "x": 6.302300997223708, - "y": 7.5107146417709965, - "heading": 2.8303781724344634e-17, - "angularVelocity": 1.0166114443831184e-28, - "velocityX": 1.44269319322838, - "velocityY": 0.08812055258968164, - "timestamp": 2.572559353850617 - }, - { - "x": 6.419245991058483, - "y": 7.51785765803668, - "heading": 2.830378172433783e-17, - "angularVelocity": -1.1196675906591495e-28, - "velocityX": 1.9235909265592357, - "velocityY": 0.11749319766817592, - "timestamp": 2.6333545012076263 - }, - { - "x": 6.565427233597177, - "y": 7.526786333446685, - "heading": 2.830378172467021e-17, - "angularVelocity": 5.467389383569676e-27, - "velocityX": 2.4044886622326676, - "velocityY": 0.14686493574187343, - "timestamp": 2.6941496485646357 - }, - { - "x": 6.740844725124364, - "y": 7.537500557717893, - "heading": 2.8303781724055295e-17, - "angularVelocity": -8.483831515101901e-30, - "velocityX": 2.8853864025869926, - "velocityY": 0.1762348598037055, - "timestamp": 2.754944795921645 + "heading": 3.1742141835585016e-17, + "angularVelocity": -9.163175252374902e-19, + "velocityX": -2.2913448895975365e-19, + "velocityY": -1.0265149362709145e-18, + "timestamp": 2.3465067953726626 + }, + { + "x": 6.156119663954512, + "y": 7.501787279405357, + "heading": 3.174214182765833e-17, + "angularVelocity": 1.1020322934166523e-25, + "velocityX": 0.48089622932964227, + "velocityY": 0.029398389176605127, + "timestamp": 2.4073019428421385 + }, + { + "x": 6.214591998656713, + "y": 7.50536149033989, + "heading": 3.1742141831543016e-17, + "angularVelocity": 6.390315645162633e-26, + "velocityX": 0.9617927932744761, + "velocityY": 0.058791056248813034, + "timestamp": 2.4680970903116144 + }, + { + "x": 6.302300539353782, + "y": 7.51072214579269, + "heading": 3.1742141831543016e-17, + "angularVelocity": 5.527535991984969e-30, + "velocityX": 1.4426898255507186, + "velocityY": 0.08817571263384313, + "timestamp": 2.5288922377810903 + }, + { + "x": 6.4192453287368645, + "y": 7.517868515281858, + "heading": 3.1742141831542363e-17, + "angularVelocity": -5.5005027620127605e-30, + "velocityX": 1.9235875600400183, + "velocityY": 0.11754835355494872, + "timestamp": 2.589687385250566 + }, + { + "x": 6.565426437911824, + "y": 7.526799381429512, + "heading": 3.174214183154203e-17, + "angularVelocity": -1.9865124104755493e-32, + "velocityX": 2.404486464127003, + "velocityY": 0.14690097021540635, + "timestamp": 2.650482532720042 + }, + { + "x": 6.740844008917749, + "y": 7.537512309825002, + "heading": 3.1742141831543275e-17, + "angularVelocity": 2.560924529534356e-29, + "velocityX": 2.8853877045696796, + "velocityY": 0.17621354403110642, + "timestamp": 2.711277680189518 }, { "x": 6.945498466491699, "y": 7.55, - "heading": 2.8303781724055554e-17, - "angularVelocity": 4.2167202635793064e-30, - "velocityX": 3.3662841569498863, - "velocityY": 0.20559934181433973, - "timestamp": 2.8157399432786545 - }, - { - "x": 7.229321610470641, - "y": 7.550003362381949, - "heading": 1.1593994241588832e-17, - "angularVelocity": -2.1482520451536383e-16, - "velocityX": 3.6489012810873196, - "velocityY": 0.00004322762277017461, - "timestamp": 2.8935231229579816 - }, - { - "x": 7.5131447544694705, - "y": 7.5500027545013815, - "heading": 7.195372591415266e-18, - "angularVelocity": -5.654977937784957e-17, - "velocityX": 3.6489012813430093, - "velocityY": -0.000007815064517695191, - "timestamp": 2.9713063026373088 - }, - { - "x": 7.796967898468301, - "y": 7.55000214698679, - "heading": -2.2105881487706106e-17, - "angularVelocity": -3.7670424634117877e-16, - "velocityX": 3.6489012813430186, - "velocityY": -0.00000781035943334551, - "timestamp": 3.049089482316636 - }, - { - "x": 8.080791042466867, - "y": 7.5500015394568125, - "heading": -3.9909490658933576e-17, - "angularVelocity": -2.28887649548727e-16, - "velocityX": 3.6489012813396258, - "velocityY": -0.000007810557249203658, - "timestamp": 3.126872661995963 - }, - { - "x": 8.320527341192129, - "y": 7.550001026304512, - "heading": -3.990273647201336e-17, - "angularVelocity": 8.683351527189107e-20, - "velocityX": 3.082109778921481, - "velocityY": -0.000006597214238392456, - "timestamp": 3.20465584167529 - }, - { - "x": 8.51231639819258, - "y": 7.550000615782698, - "heading": -3.9899086872036086e-17, - "angularVelocity": 4.6920169533433444e-20, - "velocityX": 2.465688054809929, - "velocityY": -0.0000052777710475807675, - "timestamp": 3.2824390213546173 - }, - { - "x": 8.656158196885205, - "y": 7.550000307891346, - "heading": -3.989681664095531e-17, - "angularVelocity": 2.9186658222831174e-20, - "velocityX": 1.8492661175029514, - "velocityY": -0.00000395832817870783, - "timestamp": 3.3602222010339444 - }, - { - "x": 8.752052731742296, - "y": 7.550000102630447, - "heading": -3.9895416797891945e-17, - "angularVelocity": 1.7996732314772988e-20, - "velocityX": 1.2328441091304374, - "velocityY": -0.0000026388854100440123, - "timestamp": 3.4380053807132716 - }, - { - "x": 8.8, + "heading": 3.174214183411669e-17, + "angularVelocity": 4.2334783572302846e-26, + "velocityX": 3.3662959313768197, + "velocityY": 0.2054060347705879, + "timestamp": 2.772072827658994 + }, + { + "x": 7.221347124992817, + "y": 7.550083175294458, + "heading": 2.582386400207731e-18, + "angularVelocity": -3.857226029687031e-16, + "velocityX": 3.6489010624124374, + "velocityY": 0.0011002352593078705, + "timestamp": 2.8476705634657145 + }, + { + "x": 7.4971957957486515, + "y": 7.550070265650631, + "heading": -3.139160687691863e-17, + "angularVelocity": -4.494049049826975e-16, + "velocityX": 3.6489012245167154, + "velocityY": -0.00017076759891002745, + "timestamp": 2.923268299272435 + }, + { + "x": 7.7730444665050475, + "y": 7.550057368010946, + "heading": -4.4250107790457757e-17, + "angularVelocity": -1.700910850877103e-16, + "velocityX": 3.648901224524143, + "velocityY": -0.00017060880920745955, + "timestamp": 2.9988660350791556 + }, + { + "x": 8.04889313726144, + "y": 7.550044470349665, + "heading": -3.3139122030001965e-17, + "angularVelocity": 1.4697511296982546e-16, + "velocityX": 3.648901224524074, + "velocityY": -0.00017060909487947776, + "timestamp": 3.074463770885876 + }, + { + "x": 8.320637928532271, + "y": 7.550031764514427, + "heading": -3.3149587273190146e-17, + "angularVelocity": -1.3843328871118217e-19, + "velocityX": 3.594615478505821, + "velocityY": -0.00016807163736953773, + "timestamp": 3.1500615066925968 + }, + { + "x": 8.54709194269371, + "y": 7.5500211763353215, + "heading": -3.3161210294625444e-17, + "angularVelocity": -1.5374827446451776e-19, + "velocityX": 2.99551318230494, + "velocityY": -0.0001400594738974264, + "timestamp": 3.2256592424993173 + }, + { + "x": 8.728255161631012, + "y": 7.5500127057980455, + "heading": -3.317341059346824e-17, + "angularVelocity": -1.613844477074772e-19, + "velocityX": 2.39641064648388, + "velocityY": -0.00011204749964645467, + "timestamp": 3.301256978306038 + }, + { + "x": 8.864127579305954, + "y": 7.550006352897831, + "heading": -3.318651341446678e-17, + "angularVelocity": -1.7332292901257806e-19, + "velocityX": 1.797308030789794, + "velocityY": -0.00008403558846668993, + "timestamp": 3.3768547141127585 + }, + { + "x": 8.954709192699438, + "y": 7.5500021176322925, + "heading": -3.320067915378452e-17, + "angularVelocity": -1.8738311623596625e-19, + "velocityX": 1.19820537515927, + "velocityY": -0.00005602370882446872, + "timestamp": 3.452452449919479 + }, + { + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": -3.908565443774477e-21, - "velocityX": 0.6164220652250822, - "velocityY": -0.000001319442690505465, - "timestamp": 3.5157885603925987 + "heading": -3.3215694928340293e-17, + "angularVelocity": -1.9862730534835854e-19, + "velocityX": 0.5991026955669089, + "velocityY": -0.00002801184810473847, + "timestamp": 3.5280501857261997 }, { - "x": 8.8, + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": 2.0117689927326026e-32, - "velocityX": 8.595312932705472e-20, - "velocityY": 5.963533551498734e-19, - "timestamp": 3.593571740071926 + "heading": -3.321569492834034e-17, + "angularVelocity": -5.16984725650092e-31, + "velocityX": 2.95751067102717e-19, + "velocityY": 2.6561323414610804e-18, + "timestamp": 3.6036479215329202 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpNearPiece.1.traj b/src/main/deploy/choreo/B_PickUpNearPiece.1.traj index c413152..5ca66d0 100644 --- a/src/main/deploy/choreo/B_PickUpNearPiece.1.traj +++ b/src/main/deploy/choreo/B_PickUpNearPiece.1.traj @@ -4,460 +4,460 @@ "x": 1.77, "y": 7.9, "heading": -1.571, - "angularVelocity": -5.001059619972217e-29, - "velocityX": 1.788147943312785e-31, - "velocityY": 2.1020984301178844e-18, + "angularVelocity": 2.18585903371176e-28, + "velocityX": 1.6675991903204138e-31, + "velocityY": 2.1032812804683124e-18, "timestamp": 0 }, { "x": 1.77, - "y": 7.896666665318824, + "y": 7.8966666653183735, "heading": -1.571, - "angularVelocity": 7.683916356601382e-22, - "velocityX": 9.998046435594066e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.02050894424315847 + "angularVelocity": 8.067708733019786e-22, + "velocityX": 9.994992923130725e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.020508944244543796 }, { "x": 1.77, - "y": 7.889999996400785, + "y": 7.8899999963994345, "heading": -1.571, - "angularVelocity": 7.683917142451062e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.04101788848631694 + "angularVelocity": 8.067710793880657e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.04101788848908759 }, { "x": 1.77, - "y": 7.8799999939859795, + "y": 7.879999993983276, "heading": -1.571, - "angularVelocity": 7.683923566300495e-22, - "velocityX": 2.990166401978325e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.061526832729475414 + "angularVelocity": 8.067707982153918e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.06152683273363139 }, { "x": 1.77, - "y": 7.8666666595980566, + "y": 7.866666659593829, "heading": -1.571, - "angularVelocity": 7.683920333199294e-22, - "velocityX": 3.9853467158164286e-17, - "velocityY": -0.6501229039311227, - "timestamp": 0.08203577697263388 + "angularVelocity": 8.067715580788947e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.08203577697817518 }, { "x": 1.77, "y": 7.85, "heading": -1.571, - "angularVelocity": 7.6839203283387345e-22, - "velocityX": 4.9805250105364423e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.10254472121579235 + "angularVelocity": 8.067712787171335e-22, + "velocityX": 4.980219657624676e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.10254472122271899 }, { "x": 1.77, - "y": 7.833333340401944, + "y": 7.833333340406171, "heading": -1.571, - "angularVelocity": 7.683920326683815e-22, - "velocityX": 4.980525010536431e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.12305366545895081 + "angularVelocity": 8.067712790353108e-22, + "velocityX": 4.980219657624708e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.12305366546726279 }, { "x": 1.77, - "y": 7.820000006014021, + "y": 7.820000006016724, "heading": -1.571, - "angularVelocity": 7.683920326387786e-22, - "velocityX": 3.985346715816429e-17, - "velocityY": -0.6501229039311228, - "timestamp": 0.1435626097021093 + "angularVelocity": 8.067712793311768e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.1435626097118066 }, { "x": 1.77, - "y": 7.810000003599215, + "y": 7.810000003600566, "heading": -1.571, - "angularVelocity": 7.683920326387407e-22, - "velocityX": 2.9901664019783255e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.16407155394526776 + "angularVelocity": 8.067712794323227e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.1640715539563504 }, { "x": 1.77, - "y": 7.803333334681176, + "y": 7.803333334681627, "heading": -1.571, - "angularVelocity": 7.683923930888569e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.18458049818842623 + "angularVelocity": 8.067712783004652e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.1845804982008942 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 7.68391635572018e-22, - "velocityX": 9.998046435593288e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.2050894424315847 + "angularVelocity": 8.067716774548183e-22, + "velocityX": 9.994992923129957e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.205089442445438 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 3.841958700142675e-22, - "velocityX": 1.516102151151558e-20, - "velocityY": 1.2612627111587971e-18, - "timestamp": 0.22559838667474316 + "angularVelocity": 4.033854514049993e-22, + "velocityX": 1.437728787092847e-20, + "velocityY": 1.2619724294779088e-18, + "timestamp": 0.2255983866899818 }, { - "x": 1.8468750067060988, + "x": 1.8468750067060982, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583235197619974e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997518092183e-19, - "timestamp": 0.32408933900510295 + "angularVelocity": 4.780221469151799e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036155566e-19, + "timestamp": 0.3240893390203413 }, { - "x": 2.0006250165417403, + "x": 2.0006250165417385, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.5832292867617415e-25, - "velocityX": 1.5610571955881865, - "velocityY": 1.0102997518139742e-19, - "timestamp": 0.4225802913354627 + "angularVelocity": 4.779931148188201e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036211387e-19, + "timestamp": 0.42258029135070085 }, { - "x": 2.2312500211616264, + "x": 2.2312500211616237, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583091752273785e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518132804e-19, - "timestamp": 0.5210712436658225 + "angularVelocity": 4.779411037477553e-25, + "velocityX": 2.341585690493883, + "velocityY": 1.0108689036215326e-19, + "timestamp": 0.5210712436810604 }, { - "x": 2.538749978838374, + "x": 2.5387499788383763, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583544986591083e-25, - "velocityX": 3.1221137617324173, - "velocityY": 1.0102997518134437e-19, - "timestamp": 0.6195621959961822 + "angularVelocity": 4.781363462107374e-25, + "velocityX": 3.122113761732479, + "velocityY": 1.0108689036222194e-19, + "timestamp": 0.6195621960114199 }, { - "x": 2.76937498345826, + "x": 2.7693749834582615, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583037423957827e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518136517e-19, - "timestamp": 0.718053148326542 + "angularVelocity": 4.779378758545355e-25, + "velocityX": 2.3415856904938837, + "velocityY": 1.0108689036239423e-19, + "timestamp": 0.7180531483417795 }, { - "x": 2.9231249932939014, + "x": 2.923124993293902, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583270603340756e-25, - "velocityX": 1.5610571955881862, - "velocityY": 1.0102997518134589e-19, - "timestamp": 0.8165441006569018 + "angularVelocity": 4.779884871772051e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036199126e-19, + "timestamp": 0.816544100672139 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583237967720886e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997530405439e-19, - "timestamp": 0.9150350529872615 + "angularVelocity": 4.779655463247018e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036198308e-19, + "timestamp": 0.9150350530024985 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 1.5277643827009332e-25, - "velocityX": 8.205072252280436e-23, - "velocityY": 2.539441495294102e-20, - "timestamp": 1.0135260053176214 + "angularVelocity": 1.5936410800223104e-25, + "velocityX": -1.507822169205296e-22, + "velocityY": 2.5440102474480186e-20, + "timestamp": 1.013526005332858 }, { "x": 3, "y": 7.668749997164577, "heading": -1.571, - "angularVelocity": 6.966538206440015e-26, - "velocityX": 6.122589528955434e-17, + "angularVelocity": 7.336223698049225e-26, + "velocityX": 6.122564906084829e-17, "velocityY": -0.9999999859073642, - "timestamp": 1.1447760100027031 + "timestamp": 1.1447760100179396 }, { "x": 3, "y": 7.537499994329149, "heading": -1.571, - "angularVelocity": 6.965517753645813e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.337555266120999e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.2760260146877849 + "timestamp": 1.2760260147030211 }, { "x": 3, - "y": 7.406249991493721, + "y": 7.406249991493722, "heading": -1.571, - "angularVelocity": 6.965810499047246e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336209273675485e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.4072760193728666 + "timestamp": 1.4072760193881027 }, { "x": 3, - "y": 7.274999988658292, + "y": 7.274999988658293, "heading": -1.571, - "angularVelocity": 6.965243755612967e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335860790281625e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.5385260240579484 + "timestamp": 1.5385260240731842 }, { "x": 3, - "y": 7.143749985822865, + "y": 7.143749985822866, "heading": -1.571, - "angularVelocity": 6.966418165061469e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335353332117909e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.66977602874303 + "timestamp": 1.6697760287582657 }, { "x": 3, - "y": 7.012499982987436, + "y": 7.012499982987438, "heading": -1.571, - "angularVelocity": 6.965888956818575e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336320963838537e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.8010260334281118 + "timestamp": 1.8010260334433472 }, { "x": 3, - "y": 6.881249980152008, + "y": 6.881249980152011, "heading": -1.571, - "angularVelocity": 6.966018121357878e-26, - "velocityX": 6.122589528955833e-17, + "angularVelocity": 7.337274968697998e-26, + "velocityX": 6.122564906085217e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.9322760381131936 + "timestamp": 1.9322760381284287 }, { "x": 3, "y": 6.75, "heading": -1.571, - "angularVelocity": 3.506162881778858e-26, - "velocityX": 6.033433304097125e-17, - "velocityY": -0.9999998130813552, - "timestamp": 2.063526042798275 + "angularVelocity": 3.6968138862699375e-26, + "velocityX": 6.033492244151313e-17, + "velocityY": -0.999999813081372, + "timestamp": 2.0635260428135105 }, { - "x": 2.947790541091234, - "y": 6.708274167558872, + "x": 2.9477905302655625, + "y": 6.708274157634863, "heading": -1.571, - "angularVelocity": 1.8367126795602432e-25, - "velocityX": -0.5284778249133775, - "velocityY": -0.4223598105033264, - "timestamp": 2.162318189691711 + "angularVelocity": 1.9356698765722176e-25, + "velocityX": -0.5284779345875366, + "velocityY": -0.4223599110317691, + "timestamp": 2.1623181896893984 }, { - "x": 2.843375074671145, - "y": 6.723617786210881, + "x": 2.8433750381449614, + "y": 6.723617752718413, "heading": -1.571, - "angularVelocity": 1.8422931226784615e-25, - "velocityX": -1.05692071387738, - "velocityY": 0.15531212889381546, - "timestamp": 2.2611103365851473 + "angularVelocity": 1.9452475397041562e-25, + "velocityX": -1.056920974212429, + "velocityY": 0.15531189035527532, + "timestamp": 2.2611103365652863 }, { - "x": 2.6867593540916537, - "y": 6.796036114551366, + "x": 2.686759270236978, + "y": 6.7960360376723505, "heading": -1.571, - "angularVelocity": 1.8420943963618568e-25, - "velocityX": -1.5853053659055247, - "velocityY": 0.7330372971709972, - "timestamp": 2.3599024834785833 + "angularVelocity": 1.94415608308657e-25, + "velocityX": -1.5853058452585174, + "velocityY": 0.733036858131207, + "timestamp": 2.359902483441174 }, { - "x": 2.4779548883976252, - "y": 6.925539668717246, + "x": 2.4779547220704496, + "y": 6.92553951628491, "heading": -1.571, - "angularVelocity": 1.8414864642036264e-25, - "velocityX": -2.1135735203655353, - "velocityY": 1.3108689125419186, - "timestamp": 2.4586946303720194 + "angularVelocity": 1.943670176910069e-25, + "velocityX": -2.1135743555492152, + "velocityY": 1.3108681480042503, + "timestamp": 2.458694630317062 }, { - "x": 2.2169961917679095, - "y": 7.112159959748603, + "x": 2.2169958673173573, + "y": 7.1121596626476205, "heading": -1.571, - "angularVelocity": 1.8422871659159952e-25, - "velocityX": -2.641492313262553, - "velocityY": 1.8890194909182378, - "timestamp": 2.5574867772654555 + "angularVelocity": 1.9434969946845302e-25, + "velocityX": -2.641493914298006, + "velocityY": 1.8890180268798127, + "timestamp": 2.55748677719295 }, { - "x": 2.026837658650624, - "y": 7.31963208391883, + "x": 2.0268375106829706, + "y": 7.319631948507334, "heading": -1.571, - "angularVelocity": 1.8414775640545203e-25, - "velocityX": -1.9248345045320685, - "velocityY": 2.100087210312572, - "timestamp": 2.6562789241588916 + "angularVelocity": 1.9443911276635488e-25, + "velocityX": -1.9248327184678096, + "velocityY": 2.100088847348966, + "timestamp": 2.656278924068838 }, { - "x": 1.8889712227429223, - "y": 7.470113664922766, + "x": 1.8889711653847767, + "y": 7.470113612444433, "heading": -1.571, - "angularVelocity": 1.842116721306413e-25, - "velocityX": -1.3955201930818828, - "velocityY": 1.523213997629358, - "timestamp": 2.7550710710523276 + "angularVelocity": 1.9431166040683193e-25, + "velocityX": -1.3955192761565818, + "velocityY": 1.5232148373711183, + "timestamp": 2.7550710709447257 }, { - "x": 1.8033624960430958, - "y": 7.563573145350679, + "x": 1.8033624835208033, + "y": 7.563573133893712, "heading": -1.571, - "angularVelocity": 1.8415246383265505e-25, - "velocityX": -0.8665539659966102, - "velocityY": 0.9460213525749656, - "timestamp": 2.8538632179457637 + "angularVelocity": 1.94352961414745e-25, + "velocityX": -0.8665535123102769, + "velocityY": 0.9460217679720164, + "timestamp": 2.8538632178206136 }, { "x": 1.77, "y": 7.6, "heading": -1.571, - "angularVelocity": 9.209160199279817e-26, - "velocityX": -0.3377039278140482, - "velocityY": 0.36872216866198404, - "timestamp": 2.9526553648391998 + "angularVelocity": 9.71650033036472e-26, + "velocityX": -0.3377038011201076, + "velocityY": 0.3687222846978969, + "timestamp": 2.9526553646965015 }, { - "x": 1.769999806106138, - "y": 7.642307692307283, + "x": 1.7799998137324806, + "y": 7.642307736334161, "heading": -1.571, - "angularVelocity": 4.521417535191126e-26, - "velocityX": -0.0000022914728636606335, - "velocityY": 0.4999999887447983, - "timestamp": 3.0372707513584936 + "angularVelocity": 4.913051031934083e-26, + "velocityX": 0.11501058214794466, + "velocityY": 0.4865928021587884, + "timestamp": 3.0396022659791786 }, { - "x": 1.7699998225003861, - "y": 7.68461538461501, + "x": 1.7899998294834822, + "y": 7.684615424918922, "heading": -1.571, - "angularVelocity": 4.5222328288936885e-26, - "velocityX": 1.9375020074514916e-7, - "velocityY": 0.4999999887500343, - "timestamp": 3.1218861378777873 + "angularVelocity": 4.909750963875263e-26, + "velocityX": 0.11501290561799446, + "velocityY": 0.4865922529799326, + "timestamp": 3.1265491672618557 }, { - "x": 1.769999838665324, - "y": 7.726923076922736, + "x": 1.799999845000905, + "y": 7.726923113558894, "heading": -1.571, - "angularVelocity": 4.5223430345422915e-26, - "velocityX": 1.9104016727085167e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.206501524397081 + "angularVelocity": 4.91154883871533e-26, + "velocityX": 0.11501290293154022, + "velocityY": 0.48659225361491326, + "timestamp": 3.213496068544533 }, { - "x": 1.7699998547535982, - "y": 7.769230769230463, + "x": 1.8099998604642755, + "y": 7.769230802211641, "heading": -1.571, - "angularVelocity": 4.522360821860641e-26, - "velocityX": 1.901341494252509e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.291116910916375 + "angularVelocity": 4.911173445042056e-26, + "velocityX": 0.11501290230986914, + "velocityY": 0.48659225376185367, + "timestamp": 3.30044296982721 }, { - "x": 1.7699998708857232, - "y": 7.8115384615381895, + "x": 1.8199998759636424, + "y": 7.81153849085588, "heading": -1.571, - "angularVelocity": 4.522227599449808e-26, - "velocityX": 1.9065238145885414e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.3757322974356687 + "angularVelocity": 4.910186740067351e-26, + "velocityX": 0.11501290272387527, + "velocityY": 0.4865922536639975, + "timestamp": 3.387389871109887 }, { - "x": 1.7699998870253404, - "y": 7.853846153845916, + "x": 1.8299998914682623, + "y": 7.853846179498878, "heading": -1.571, - "angularVelocity": 4.521986926265768e-26, - "velocityX": 1.907409270385452e-7, - "velocityY": 0.4999999887500347, - "timestamp": 3.4603476839549625 + "angularVelocity": 4.9095323376375024e-26, + "velocityX": 0.11501290278429054, + "velocityY": 0.48659225364971753, + "timestamp": 3.474336772392564 }, { - "x": 1.7699999031640838, - "y": 7.896153846153643, + "x": 1.8399999069719994, + "y": 7.8961538681420835, "heading": -1.571, - "angularVelocity": 4.521897586059284e-26, - "velocityX": 1.9073060076657246e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.5449630704742563 + "angularVelocity": 4.9098565172353323e-26, + "velocityX": 0.11501290277413707, + "velocityY": 0.48659225365211745, + "timestamp": 3.5612836736752413 }, { - "x": 1.769999919301922, - "y": 7.93846153846137, + "x": 1.8499999224750308, + "y": 7.938461556785456, "heading": -1.571, - "angularVelocity": 4.522736988509059e-26, - "velocityX": 1.907199008902848e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.62957845699355 + "angularVelocity": 4.910188971582487e-26, + "velocityX": 0.11501290276601811, + "velocityY": 0.48659225365403647, + "timestamp": 3.6482305749579185 }, { - "x": 1.7699999354395248, - "y": 7.980769230769097, + "x": 1.8599999379779208, + "y": 7.980769245428863, "heading": -1.571, - "angularVelocity": 4.522707345878583e-26, - "velocityX": 1.9071711976583354e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.714193843512844 + "angularVelocity": 4.9116368716604623e-26, + "velocityX": 0.11501290276439573, + "velocityY": 0.48659225365441994, + "timestamp": 3.7351774762405956 }, { - "x": 1.769999951576638, - "y": 8.023076923076822, + "x": 1.8699999534805036, + "y": 8.023076934072343, "heading": -1.571, - "angularVelocity": 4.522884645346994e-26, - "velocityX": 1.907113334822706e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.7988092300321377 + "angularVelocity": 4.9088992585035167e-26, + "velocityX": 0.11501290276085954, + "velocityY": 0.48659225365525577, + "timestamp": 3.8221243775232727 }, { - "x": 1.769999967709957, - "y": 8.065384615384549, + "x": 1.879999968980049, + "y": 8.06538462271654, "heading": -1.571, - "angularVelocity": 4.522407576023322e-26, - "velocityX": 1.9066649420464386e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.8834246165514315 + "angularVelocity": 4.9115516494556264e-26, + "velocityX": 0.11501290272592707, + "velocityY": 0.48659225366351255, + "timestamp": 3.90907127880595 }, { - "x": 1.769999983845188, - "y": 8.107692307692274, + "x": 1.88999998448058, + "y": 8.107692311360502, "heading": -1.571, - "angularVelocity": 4.522843638364402e-26, - "velocityX": 1.9068908979017438e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.9680400030707252 + "angularVelocity": 4.911175288807037e-26, + "velocityX": 0.11501290273726322, + "velocityY": 0.48659225366083314, + "timestamp": 3.996018180088627 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 4.5197757674515026e-26, - "velocityX": 1.90920500829837e-7, - "velocityY": 0.49999998875003127, - "timestamp": 4.052655389590019 + "angularVelocity": 4.9135183566524024e-26, + "velocityX": 0.11501290295450972, + "velocityY": 0.48659225360948094, + "timestamp": 4.082965081371304 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 1.5086235456844618e-26, - "velocityX": -8.006405502592549e-21, - "velocityY": 4.1987950762241055e-20, - "timestamp": 4.137270776109312 + "angularVelocity": 1.6371251773517092e-26, + "velocityX": -6.721181362730927e-21, + "velocityY": 6.32096054231833e-20, + "timestamp": 4.169911982653981 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_PickUpNearPiece.traj b/src/main/deploy/choreo/B_PickUpNearPiece.traj index c413152..5ca66d0 100644 --- a/src/main/deploy/choreo/B_PickUpNearPiece.traj +++ b/src/main/deploy/choreo/B_PickUpNearPiece.traj @@ -4,460 +4,460 @@ "x": 1.77, "y": 7.9, "heading": -1.571, - "angularVelocity": -5.001059619972217e-29, - "velocityX": 1.788147943312785e-31, - "velocityY": 2.1020984301178844e-18, + "angularVelocity": 2.18585903371176e-28, + "velocityX": 1.6675991903204138e-31, + "velocityY": 2.1032812804683124e-18, "timestamp": 0 }, { "x": 1.77, - "y": 7.896666665318824, + "y": 7.8966666653183735, "heading": -1.571, - "angularVelocity": 7.683916356601382e-22, - "velocityX": 9.998046435594066e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.02050894424315847 + "angularVelocity": 8.067708733019786e-22, + "velocityX": 9.994992923130725e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.020508944244543796 }, { "x": 1.77, - "y": 7.889999996400785, + "y": 7.8899999963994345, "heading": -1.571, - "angularVelocity": 7.683917142451062e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.04101788848631694 + "angularVelocity": 8.067710793880657e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.04101788848908759 }, { "x": 1.77, - "y": 7.8799999939859795, + "y": 7.879999993983276, "heading": -1.571, - "angularVelocity": 7.683923566300495e-22, - "velocityX": 2.990166401978325e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.061526832729475414 + "angularVelocity": 8.067707982153918e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.06152683273363139 }, { "x": 1.77, - "y": 7.8666666595980566, + "y": 7.866666659593829, "heading": -1.571, - "angularVelocity": 7.683920333199294e-22, - "velocityX": 3.9853467158164286e-17, - "velocityY": -0.6501229039311227, - "timestamp": 0.08203577697263388 + "angularVelocity": 8.067715580788947e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.08203577697817518 }, { "x": 1.77, "y": 7.85, "heading": -1.571, - "angularVelocity": 7.6839203283387345e-22, - "velocityX": 4.9805250105364423e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.10254472121579235 + "angularVelocity": 8.067712787171335e-22, + "velocityX": 4.980219657624676e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.10254472122271899 }, { "x": 1.77, - "y": 7.833333340401944, + "y": 7.833333340406171, "heading": -1.571, - "angularVelocity": 7.683920326683815e-22, - "velocityX": 4.980525010536431e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.12305366545895081 + "angularVelocity": 8.067712790353108e-22, + "velocityX": 4.980219657624708e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.12305366546726279 }, { "x": 1.77, - "y": 7.820000006014021, + "y": 7.820000006016724, "heading": -1.571, - "angularVelocity": 7.683920326387786e-22, - "velocityX": 3.985346715816429e-17, - "velocityY": -0.6501229039311228, - "timestamp": 0.1435626097021093 + "angularVelocity": 8.067712793311768e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.1435626097118066 }, { "x": 1.77, - "y": 7.810000003599215, + "y": 7.810000003600566, "heading": -1.571, - "angularVelocity": 7.683920326387407e-22, - "velocityX": 2.9901664019783255e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.16407155394526776 + "angularVelocity": 8.067712794323227e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.1640715539563504 }, { "x": 1.77, - "y": 7.803333334681176, + "y": 7.803333334681627, "heading": -1.571, - "angularVelocity": 7.683923930888569e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.18458049818842623 + "angularVelocity": 8.067712783004652e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.1845804982008942 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 7.68391635572018e-22, - "velocityX": 9.998046435593288e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.2050894424315847 + "angularVelocity": 8.067716774548183e-22, + "velocityX": 9.994992923129957e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.205089442445438 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 3.841958700142675e-22, - "velocityX": 1.516102151151558e-20, - "velocityY": 1.2612627111587971e-18, - "timestamp": 0.22559838667474316 + "angularVelocity": 4.033854514049993e-22, + "velocityX": 1.437728787092847e-20, + "velocityY": 1.2619724294779088e-18, + "timestamp": 0.2255983866899818 }, { - "x": 1.8468750067060988, + "x": 1.8468750067060982, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583235197619974e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997518092183e-19, - "timestamp": 0.32408933900510295 + "angularVelocity": 4.780221469151799e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036155566e-19, + "timestamp": 0.3240893390203413 }, { - "x": 2.0006250165417403, + "x": 2.0006250165417385, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.5832292867617415e-25, - "velocityX": 1.5610571955881865, - "velocityY": 1.0102997518139742e-19, - "timestamp": 0.4225802913354627 + "angularVelocity": 4.779931148188201e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036211387e-19, + "timestamp": 0.42258029135070085 }, { - "x": 2.2312500211616264, + "x": 2.2312500211616237, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583091752273785e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518132804e-19, - "timestamp": 0.5210712436658225 + "angularVelocity": 4.779411037477553e-25, + "velocityX": 2.341585690493883, + "velocityY": 1.0108689036215326e-19, + "timestamp": 0.5210712436810604 }, { - "x": 2.538749978838374, + "x": 2.5387499788383763, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583544986591083e-25, - "velocityX": 3.1221137617324173, - "velocityY": 1.0102997518134437e-19, - "timestamp": 0.6195621959961822 + "angularVelocity": 4.781363462107374e-25, + "velocityX": 3.122113761732479, + "velocityY": 1.0108689036222194e-19, + "timestamp": 0.6195621960114199 }, { - "x": 2.76937498345826, + "x": 2.7693749834582615, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583037423957827e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518136517e-19, - "timestamp": 0.718053148326542 + "angularVelocity": 4.779378758545355e-25, + "velocityX": 2.3415856904938837, + "velocityY": 1.0108689036239423e-19, + "timestamp": 0.7180531483417795 }, { - "x": 2.9231249932939014, + "x": 2.923124993293902, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583270603340756e-25, - "velocityX": 1.5610571955881862, - "velocityY": 1.0102997518134589e-19, - "timestamp": 0.8165441006569018 + "angularVelocity": 4.779884871772051e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036199126e-19, + "timestamp": 0.816544100672139 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583237967720886e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997530405439e-19, - "timestamp": 0.9150350529872615 + "angularVelocity": 4.779655463247018e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036198308e-19, + "timestamp": 0.9150350530024985 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 1.5277643827009332e-25, - "velocityX": 8.205072252280436e-23, - "velocityY": 2.539441495294102e-20, - "timestamp": 1.0135260053176214 + "angularVelocity": 1.5936410800223104e-25, + "velocityX": -1.507822169205296e-22, + "velocityY": 2.5440102474480186e-20, + "timestamp": 1.013526005332858 }, { "x": 3, "y": 7.668749997164577, "heading": -1.571, - "angularVelocity": 6.966538206440015e-26, - "velocityX": 6.122589528955434e-17, + "angularVelocity": 7.336223698049225e-26, + "velocityX": 6.122564906084829e-17, "velocityY": -0.9999999859073642, - "timestamp": 1.1447760100027031 + "timestamp": 1.1447760100179396 }, { "x": 3, "y": 7.537499994329149, "heading": -1.571, - "angularVelocity": 6.965517753645813e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.337555266120999e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.2760260146877849 + "timestamp": 1.2760260147030211 }, { "x": 3, - "y": 7.406249991493721, + "y": 7.406249991493722, "heading": -1.571, - "angularVelocity": 6.965810499047246e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336209273675485e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.4072760193728666 + "timestamp": 1.4072760193881027 }, { "x": 3, - "y": 7.274999988658292, + "y": 7.274999988658293, "heading": -1.571, - "angularVelocity": 6.965243755612967e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335860790281625e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.5385260240579484 + "timestamp": 1.5385260240731842 }, { "x": 3, - "y": 7.143749985822865, + "y": 7.143749985822866, "heading": -1.571, - "angularVelocity": 6.966418165061469e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335353332117909e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.66977602874303 + "timestamp": 1.6697760287582657 }, { "x": 3, - "y": 7.012499982987436, + "y": 7.012499982987438, "heading": -1.571, - "angularVelocity": 6.965888956818575e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336320963838537e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.8010260334281118 + "timestamp": 1.8010260334433472 }, { "x": 3, - "y": 6.881249980152008, + "y": 6.881249980152011, "heading": -1.571, - "angularVelocity": 6.966018121357878e-26, - "velocityX": 6.122589528955833e-17, + "angularVelocity": 7.337274968697998e-26, + "velocityX": 6.122564906085217e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.9322760381131936 + "timestamp": 1.9322760381284287 }, { "x": 3, "y": 6.75, "heading": -1.571, - "angularVelocity": 3.506162881778858e-26, - "velocityX": 6.033433304097125e-17, - "velocityY": -0.9999998130813552, - "timestamp": 2.063526042798275 + "angularVelocity": 3.6968138862699375e-26, + "velocityX": 6.033492244151313e-17, + "velocityY": -0.999999813081372, + "timestamp": 2.0635260428135105 }, { - "x": 2.947790541091234, - "y": 6.708274167558872, + "x": 2.9477905302655625, + "y": 6.708274157634863, "heading": -1.571, - "angularVelocity": 1.8367126795602432e-25, - "velocityX": -0.5284778249133775, - "velocityY": -0.4223598105033264, - "timestamp": 2.162318189691711 + "angularVelocity": 1.9356698765722176e-25, + "velocityX": -0.5284779345875366, + "velocityY": -0.4223599110317691, + "timestamp": 2.1623181896893984 }, { - "x": 2.843375074671145, - "y": 6.723617786210881, + "x": 2.8433750381449614, + "y": 6.723617752718413, "heading": -1.571, - "angularVelocity": 1.8422931226784615e-25, - "velocityX": -1.05692071387738, - "velocityY": 0.15531212889381546, - "timestamp": 2.2611103365851473 + "angularVelocity": 1.9452475397041562e-25, + "velocityX": -1.056920974212429, + "velocityY": 0.15531189035527532, + "timestamp": 2.2611103365652863 }, { - "x": 2.6867593540916537, - "y": 6.796036114551366, + "x": 2.686759270236978, + "y": 6.7960360376723505, "heading": -1.571, - "angularVelocity": 1.8420943963618568e-25, - "velocityX": -1.5853053659055247, - "velocityY": 0.7330372971709972, - "timestamp": 2.3599024834785833 + "angularVelocity": 1.94415608308657e-25, + "velocityX": -1.5853058452585174, + "velocityY": 0.733036858131207, + "timestamp": 2.359902483441174 }, { - "x": 2.4779548883976252, - "y": 6.925539668717246, + "x": 2.4779547220704496, + "y": 6.92553951628491, "heading": -1.571, - "angularVelocity": 1.8414864642036264e-25, - "velocityX": -2.1135735203655353, - "velocityY": 1.3108689125419186, - "timestamp": 2.4586946303720194 + "angularVelocity": 1.943670176910069e-25, + "velocityX": -2.1135743555492152, + "velocityY": 1.3108681480042503, + "timestamp": 2.458694630317062 }, { - "x": 2.2169961917679095, - "y": 7.112159959748603, + "x": 2.2169958673173573, + "y": 7.1121596626476205, "heading": -1.571, - "angularVelocity": 1.8422871659159952e-25, - "velocityX": -2.641492313262553, - "velocityY": 1.8890194909182378, - "timestamp": 2.5574867772654555 + "angularVelocity": 1.9434969946845302e-25, + "velocityX": -2.641493914298006, + "velocityY": 1.8890180268798127, + "timestamp": 2.55748677719295 }, { - "x": 2.026837658650624, - "y": 7.31963208391883, + "x": 2.0268375106829706, + "y": 7.319631948507334, "heading": -1.571, - "angularVelocity": 1.8414775640545203e-25, - "velocityX": -1.9248345045320685, - "velocityY": 2.100087210312572, - "timestamp": 2.6562789241588916 + "angularVelocity": 1.9443911276635488e-25, + "velocityX": -1.9248327184678096, + "velocityY": 2.100088847348966, + "timestamp": 2.656278924068838 }, { - "x": 1.8889712227429223, - "y": 7.470113664922766, + "x": 1.8889711653847767, + "y": 7.470113612444433, "heading": -1.571, - "angularVelocity": 1.842116721306413e-25, - "velocityX": -1.3955201930818828, - "velocityY": 1.523213997629358, - "timestamp": 2.7550710710523276 + "angularVelocity": 1.9431166040683193e-25, + "velocityX": -1.3955192761565818, + "velocityY": 1.5232148373711183, + "timestamp": 2.7550710709447257 }, { - "x": 1.8033624960430958, - "y": 7.563573145350679, + "x": 1.8033624835208033, + "y": 7.563573133893712, "heading": -1.571, - "angularVelocity": 1.8415246383265505e-25, - "velocityX": -0.8665539659966102, - "velocityY": 0.9460213525749656, - "timestamp": 2.8538632179457637 + "angularVelocity": 1.94352961414745e-25, + "velocityX": -0.8665535123102769, + "velocityY": 0.9460217679720164, + "timestamp": 2.8538632178206136 }, { "x": 1.77, "y": 7.6, "heading": -1.571, - "angularVelocity": 9.209160199279817e-26, - "velocityX": -0.3377039278140482, - "velocityY": 0.36872216866198404, - "timestamp": 2.9526553648391998 + "angularVelocity": 9.71650033036472e-26, + "velocityX": -0.3377038011201076, + "velocityY": 0.3687222846978969, + "timestamp": 2.9526553646965015 }, { - "x": 1.769999806106138, - "y": 7.642307692307283, + "x": 1.7799998137324806, + "y": 7.642307736334161, "heading": -1.571, - "angularVelocity": 4.521417535191126e-26, - "velocityX": -0.0000022914728636606335, - "velocityY": 0.4999999887447983, - "timestamp": 3.0372707513584936 + "angularVelocity": 4.913051031934083e-26, + "velocityX": 0.11501058214794466, + "velocityY": 0.4865928021587884, + "timestamp": 3.0396022659791786 }, { - "x": 1.7699998225003861, - "y": 7.68461538461501, + "x": 1.7899998294834822, + "y": 7.684615424918922, "heading": -1.571, - "angularVelocity": 4.5222328288936885e-26, - "velocityX": 1.9375020074514916e-7, - "velocityY": 0.4999999887500343, - "timestamp": 3.1218861378777873 + "angularVelocity": 4.909750963875263e-26, + "velocityX": 0.11501290561799446, + "velocityY": 0.4865922529799326, + "timestamp": 3.1265491672618557 }, { - "x": 1.769999838665324, - "y": 7.726923076922736, + "x": 1.799999845000905, + "y": 7.726923113558894, "heading": -1.571, - "angularVelocity": 4.5223430345422915e-26, - "velocityX": 1.9104016727085167e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.206501524397081 + "angularVelocity": 4.91154883871533e-26, + "velocityX": 0.11501290293154022, + "velocityY": 0.48659225361491326, + "timestamp": 3.213496068544533 }, { - "x": 1.7699998547535982, - "y": 7.769230769230463, + "x": 1.8099998604642755, + "y": 7.769230802211641, "heading": -1.571, - "angularVelocity": 4.522360821860641e-26, - "velocityX": 1.901341494252509e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.291116910916375 + "angularVelocity": 4.911173445042056e-26, + "velocityX": 0.11501290230986914, + "velocityY": 0.48659225376185367, + "timestamp": 3.30044296982721 }, { - "x": 1.7699998708857232, - "y": 7.8115384615381895, + "x": 1.8199998759636424, + "y": 7.81153849085588, "heading": -1.571, - "angularVelocity": 4.522227599449808e-26, - "velocityX": 1.9065238145885414e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.3757322974356687 + "angularVelocity": 4.910186740067351e-26, + "velocityX": 0.11501290272387527, + "velocityY": 0.4865922536639975, + "timestamp": 3.387389871109887 }, { - "x": 1.7699998870253404, - "y": 7.853846153845916, + "x": 1.8299998914682623, + "y": 7.853846179498878, "heading": -1.571, - "angularVelocity": 4.521986926265768e-26, - "velocityX": 1.907409270385452e-7, - "velocityY": 0.4999999887500347, - "timestamp": 3.4603476839549625 + "angularVelocity": 4.9095323376375024e-26, + "velocityX": 0.11501290278429054, + "velocityY": 0.48659225364971753, + "timestamp": 3.474336772392564 }, { - "x": 1.7699999031640838, - "y": 7.896153846153643, + "x": 1.8399999069719994, + "y": 7.8961538681420835, "heading": -1.571, - "angularVelocity": 4.521897586059284e-26, - "velocityX": 1.9073060076657246e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.5449630704742563 + "angularVelocity": 4.9098565172353323e-26, + "velocityX": 0.11501290277413707, + "velocityY": 0.48659225365211745, + "timestamp": 3.5612836736752413 }, { - "x": 1.769999919301922, - "y": 7.93846153846137, + "x": 1.8499999224750308, + "y": 7.938461556785456, "heading": -1.571, - "angularVelocity": 4.522736988509059e-26, - "velocityX": 1.907199008902848e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.62957845699355 + "angularVelocity": 4.910188971582487e-26, + "velocityX": 0.11501290276601811, + "velocityY": 0.48659225365403647, + "timestamp": 3.6482305749579185 }, { - "x": 1.7699999354395248, - "y": 7.980769230769097, + "x": 1.8599999379779208, + "y": 7.980769245428863, "heading": -1.571, - "angularVelocity": 4.522707345878583e-26, - "velocityX": 1.9071711976583354e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.714193843512844 + "angularVelocity": 4.9116368716604623e-26, + "velocityX": 0.11501290276439573, + "velocityY": 0.48659225365441994, + "timestamp": 3.7351774762405956 }, { - "x": 1.769999951576638, - "y": 8.023076923076822, + "x": 1.8699999534805036, + "y": 8.023076934072343, "heading": -1.571, - "angularVelocity": 4.522884645346994e-26, - "velocityX": 1.907113334822706e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.7988092300321377 + "angularVelocity": 4.9088992585035167e-26, + "velocityX": 0.11501290276085954, + "velocityY": 0.48659225365525577, + "timestamp": 3.8221243775232727 }, { - "x": 1.769999967709957, - "y": 8.065384615384549, + "x": 1.879999968980049, + "y": 8.06538462271654, "heading": -1.571, - "angularVelocity": 4.522407576023322e-26, - "velocityX": 1.9066649420464386e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.8834246165514315 + "angularVelocity": 4.9115516494556264e-26, + "velocityX": 0.11501290272592707, + "velocityY": 0.48659225366351255, + "timestamp": 3.90907127880595 }, { - "x": 1.769999983845188, - "y": 8.107692307692274, + "x": 1.88999998448058, + "y": 8.107692311360502, "heading": -1.571, - "angularVelocity": 4.522843638364402e-26, - "velocityX": 1.9068908979017438e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.9680400030707252 + "angularVelocity": 4.911175288807037e-26, + "velocityX": 0.11501290273726322, + "velocityY": 0.48659225366083314, + "timestamp": 3.996018180088627 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 4.5197757674515026e-26, - "velocityX": 1.90920500829837e-7, - "velocityY": 0.49999998875003127, - "timestamp": 4.052655389590019 + "angularVelocity": 4.9135183566524024e-26, + "velocityX": 0.11501290295450972, + "velocityY": 0.48659225360948094, + "timestamp": 4.082965081371304 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 1.5086235456844618e-26, - "velocityX": -8.006405502592549e-21, - "velocityY": 4.1987950762241055e-20, - "timestamp": 4.137270776109312 + "angularVelocity": 1.6371251773517092e-26, + "velocityX": -6.721181362730927e-21, + "velocityY": 6.32096054231833e-20, + "timestamp": 4.169911982653981 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_ReturnFromFarPiece1.1.traj b/src/main/deploy/choreo/B_ReturnFromFarPiece1.1.traj index b1fed0f..75714ee 100644 --- a/src/main/deploy/choreo/B_ReturnFromFarPiece1.1.traj +++ b/src/main/deploy/choreo/B_ReturnFromFarPiece1.1.traj @@ -1,589 +1,598 @@ { "samples": [ { - "x": 8.3, - "y": 7.6, - "heading": 2.757501519692808e-19, - "angularVelocity": 9.600967361428564e-29, - "velocityX": -2.4319155594097383e-17, - "velocityY": -4.0122443313037055e-19, + "x": 8.73, + "y": 7.45, + "heading": 3.0153335539807125e-17, + "angularVelocity": -9.150386789447993e-29, + "velocityX": 8.944879250579314e-18, + "velocityY": -6.136035981354337e-20, "timestamp": 0 }, { - "x": 8.2519779299027, - "y": 7.59802796735826, - "heading": 5.5317266150126195e-19, - "angularVelocity": 2.1474447549717986e-20, - "velocityX": -0.6166429049270267, - "velocityY": -0.025322522214253233, - "timestamp": 0.07787662796993104 - }, - { - "x": 8.155933791545712, - "y": 7.594083902150697, - "heading": 8.305951710241554e-19, - "angularVelocity": 2.147444743302548e-20, - "velocityX": -1.2332857862576319, - "velocityY": -0.050645043453678214, - "timestamp": 0.15575325593986208 - }, - { - "x": 8.011867587991711, - "y": 7.5881678045038425, - "heading": 1.1080176805595397e-18, - "angularVelocity": 2.147444759342935e-20, - "velocityX": -1.8499286282609135, - "velocityY": -0.07596756306832102, - "timestamp": 0.2336298839097931 - }, - { - "x": 7.819779325366047, - "y": 7.580279674670781, - "heading": 1.385440190094531e-18, - "angularVelocity": 2.147444758838943e-20, - "velocityX": -2.466571391609719, - "velocityY": -0.10129007943317028, - "timestamp": 0.31150651187972417 - }, - { - "x": 7.579669022044645, - "y": 7.570419513410852, - "heading": 1.6628626996296796e-18, - "angularVelocity": 2.1474447590405832e-20, - "velocityX": -3.0832139189964716, - "velocityY": -0.1266125860474561, - "timestamp": 0.3893831398496552 - }, - { - "x": 7.295744194818641, - "y": 7.558760102681403, - "heading": 1.9402852103929644e-18, - "angularVelocity": 2.1474463360667863e-20, - "velocityX": -3.6458284677609587, - "velocityY": -0.14971643012009433, - "timestamp": 0.4672597678195863 - }, - { - "x": 7.011819367581371, - "y": 7.547100692219137, - "heading": 2.2177077235682638e-18, - "angularVelocity": 2.1474494332923423e-20, - "velocityX": -3.6458284679056314, - "velocityY": -0.14971642668926177, - "timestamp": 0.5451363957895173 - }, - { - "x": 6.727894540344498, - "y": 7.5354412817471905, - "heading": 2.4951302352091332e-18, - "angularVelocity": 2.1474474629589685e-20, - "velocityX": -3.645828467900525, - "velocityY": -0.14971642681353683, - "timestamp": 0.6230130237594483 - }, - { - "x": 6.443969713075486, - "y": 7.5237818720578895, - "heading": 2.7725527581105576e-18, - "angularVelocity": 2.147461922435792e-20, - "velocityX": -3.645828468313219, - "velocityY": -0.1497164167637409, - "timestamp": 0.7008896517293793 - }, - { - "x": 6.160044885669281, - "y": 7.512122465709375, - "heading": 3.0499752697600583e-18, - "angularVelocity": 2.1474474740432105e-20, - "velocityX": -3.645828470074894, - "velocityY": -0.14971637386530257, - "timestamp": 0.7787662796993103 - }, - { - "x": 5.8761200592911305, - "y": 7.500463034323216, - "heading": 3.3273979967340457e-18, - "angularVelocity": 2.1477215638632408e-20, - "velocityX": -3.6458284568738266, - "velocityY": -0.1497166953692409, - "timestamp": 0.8566429076692413 - }, - { - "x": 5.592195230920835, - "y": 7.488803651443019, - "heading": 3.604820428609217e-18, - "angularVelocity": 2.1473426326984874e-20, - "velocityX": -3.6458284824546, - "velocityY": -0.14971607251278177, - "timestamp": 0.9345195356391723 - }, - { - "x": 5.308268547058106, + "x": 8.678452834172791, + "y": 7.450409597940094, + "heading": 6.050280891646632e-17, + "angularVelocity": 2.4319148836270395e-18, + "velocityX": -0.6391337938457895, + "velocityY": 0.005078608711127938, + "timestamp": 0.08065160428623162 + }, + { + "x": 8.57535850438037, + "y": 7.451228793805321, + "heading": 9.085228229308535e-17, + "angularVelocity": 2.431914883129373e-18, + "velocityX": -1.2782675646046895, + "velocityY": 0.010157217236737312, + "timestamp": 0.16130320857246325 + }, + { + "x": 8.420717013726057, + "y": 7.452457587570761, + "heading": 1.2120175566971728e-16, + "angularVelocity": 2.4319148832885226e-18, + "velocityX": -1.9174012968854468, + "velocityY": 0.015235825453377785, + "timestamp": 0.24195481285869486 + }, + { + "x": 8.2145283684165, + "y": 7.454095979186639, + "heading": 1.5155122904634846e-16, + "angularVelocity": 2.431914883280121e-18, + "velocityX": -2.55653495220995, + "velocityY": 0.020314433052855834, + "timestamp": 0.3226064171449265 + }, + { + "x": 7.95679258707161, + "y": 7.456143968504021, + "heading": 1.8190070242340144e-16, + "angularVelocity": 2.431914888508902e-18, + "velocityX": -3.195668376665999, + "velocityY": 0.025393038805691565, + "timestamp": 0.40325802143115813 + }, + { + "x": 7.6625121380166075, + "y": 7.4584823447093935, + "heading": 2.122501739665664e-16, + "angularVelocity": 2.431892150115273e-18, + "velocityX": -3.648786055273063, + "velocityY": 0.028993548560726043, + "timestamp": 0.48390962571738977 + }, + { + "x": 7.3682316889613775, + "y": 7.460820720895092, + "heading": 2.4259964550994947e-16, + "angularVelocity": 2.4318921528197456e-18, + "velocityX": -3.6487860552758766, + "velocityY": 0.02899354831677369, + "timestamp": 0.5645612300036214 + }, + { + "x": 7.073951239906084, + "y": 7.463159097072805, + "heading": 2.7294911705334123e-16, + "angularVelocity": 2.431892152926584e-18, + "velocityX": -3.6487860552766636, + "velocityY": 0.028993548217774053, + "timestamp": 0.645212834289853 + }, + { + "x": 6.779670790850629, + "y": 7.46549747323022, + "heading": 3.0329858859672815e-16, + "angularVelocity": 2.4318921529031445e-18, + "velocityX": -3.6487860552786633, + "velocityY": 0.028993547966090615, + "timestamp": 0.7258644385760846 + }, + { + "x": 6.485390341794982, + "y": 7.467835849363504, + "heading": 3.3364806014011606e-16, + "angularVelocity": 2.4318921529142702e-18, + "velocityX": -3.6487860552810405, + "velocityY": 0.028993547666888664, + "timestamp": 0.8065160428623163 + }, + { + "x": 6.191109892739618, + "y": 7.470174225532392, + "heading": 3.6399753168349943e-16, + "angularVelocity": 2.4318921528614766e-18, + "velocityX": -3.6487860552775326, + "velocityY": 0.028993548108348557, + "timestamp": 0.8871676471485479 + }, + { + "x": 5.896829443688916, + "y": 7.472512602287961, + "heading": 3.9434700322679583e-16, + "angularVelocity": 2.431892151781526e-18, + "velocityX": -3.648786055219731, + "velocityY": 0.028993555382614684, + "timestamp": 0.9678192514347795 + }, + { + "x": 5.602548994628648, + "y": 7.474850977839731, + "heading": 4.246964747702855e-16, + "angularVelocity": 2.4318921541771888e-18, + "velocityX": -3.6487860553383333, + "velocityY": 0.028993540456692966, + "timestamp": 1.0484708557210112 + }, + { + "x": 5.3082685470581055, "y": 7.477189540863037, - "heading": 3.881406761933183e-18, - "angularVelocity": 1.0737232772850433e-20, - "velocityX": -3.64585230850462, - "velocityY": -0.14913473891635948, - "timestamp": 1.0123961636091034 - }, - { - "x": 5.028627693595278, - "y": 7.497782181981604, - "heading": 4.1587848720220655e-18, - "angularVelocity": 2.118506785259316e-20, - "velocityX": -3.639047714019591, - "velocityY": 0.2679780248850872, - "timestamp": 1.0892406777344608 - }, - { - "x": 4.748989879107207, - "y": 7.518416049826399, - "heading": 4.436163015430491e-18, - "angularVelocity": 2.118550144945551e-20, - "velocityX": -3.6390081669576992, - "velocityY": 0.2685145202575587, - "timestamp": 1.166085191859818 - }, - { - "x": 4.469352066877837, - "y": 7.53904994828081, - "heading": 4.713541138262534e-18, - "angularVelocity": 2.1185233682992707e-20, - "velocityX": -3.6390081375645655, - "velocityY": 0.26851491858936644, - "timestamp": 1.2429297059851754 - }, - { - "x": 4.189714253377734, - "y": 7.5596838295128, - "heading": 4.9909192511776896e-18, - "angularVelocity": 2.118509579668935e-20, - "velocityX": -3.6390081541009813, - "velocityY": 0.268514694469029, - "timestamp": 1.3197742201105327 - }, - { - "x": 3.9100764400605628, - "y": 7.580317713223909, - "heading": 5.2682973643801506e-18, - "angularVelocity": 2.1185099535494865e-20, - "velocityX": -3.6390081517204464, - "velocityY": 0.2685147267305082, - "timestamp": 1.39661873423589 - }, - { - "x": 3.6304386268060798, - "y": 7.600951597744441, - "heading": 5.545591457915407e-18, - "angularVelocity": 2.0091727183515773e-20, - "velocityX": -3.6390081509046595, - "velocityY": 0.2685147372637596, - "timestamp": 1.4734632483612473 - }, - { - "x": 3.397087952925323, - "y": 7.618170051677312, - "heading": 5.822969568480078e-18, - "angularVelocity": 2.1185065209273208e-20, - "velocityX": -3.0366601511734386, - "velocityY": 0.22406874620592004, - "timestamp": 1.5503077624866046 - }, - { - "x": 3.210407395644333, - "y": 7.6319448161653884, - "heading": 6.100347679045377e-18, - "angularVelocity": 2.1185065217376727e-20, - "velocityX": -2.4293283574733384, - "velocityY": 0.17925501442568673, - "timestamp": 1.627152276611962 - }, - { - "x": 3.0703969716895227, - "y": 7.642275889973785, - "heading": 6.37772578961099e-18, - "angularVelocity": 2.1185065221445832e-20, - "velocityX": -1.821996346107541, - "velocityY": 0.13444126657556482, - "timestamp": 1.7039967907373192 - }, - { - "x": 2.9770566866367267, - "y": 7.6491632726910055, - "heading": 6.655103900175346e-18, - "angularVelocity": 2.1185065205170544e-20, - "velocityX": -1.2146642621818082, - "velocityY": 0.08962751337051074, - "timestamp": 1.7808413048626766 - }, - { - "x": 2.9303865432739244, + "heading": 4.54947878770186e-16, + "angularVelocity": 1.2159517589791973e-18, + "velocityX": -3.6487860368672314, + "velocityY": 0.028995864918033788, + "timestamp": 1.1291224600072427 + }, + { + "x": 5.0286303652572135, + "y": 7.497818426819656, + "heading": 4.852792666366976e-16, + "angularVelocity": 2.317046686913831e-18, + "velocityX": -3.6390129500795747, + "velocityY": 0.2684496897326407, + "timestamp": 1.2059669740655883 + }, + { + "x": 4.74899219578683, + "y": 7.518447479923772, + "heading": 5.156106618040484e-16, + "angularVelocity": 2.317141694856443e-18, + "velocityX": -3.639012789619103, + "velocityY": 0.26845186487162814, + "timestamp": 1.2828114881239339 + }, + { + "x": 4.469354026278326, + "y": 7.53907653251117, + "heading": 5.459420533388648e-16, + "angularVelocity": 2.3170944316093188e-18, + "velocityX": -3.639012790115152, + "velocityY": 0.26845185814741257, + "timestamp": 1.3596560021822794 + }, + { + "x": 4.18971585681519, + "y": 7.559705585713528, + "heading": 5.762734448613439e-16, + "angularVelocity": 2.3170942710563877e-18, + "velocityX": -3.6390127895247923, + "velocityY": 0.26845186615006567, + "timestamp": 1.436500516240625 + }, + { + "x": 3.9100776873548275, + "y": 7.580334638953499, + "heading": 6.066048363838091e-16, + "angularVelocity": 2.317094270874412e-18, + "velocityX": -3.639012789488683, + "velocityY": 0.268451866639544, + "timestamp": 1.5133450302989706 + }, + { + "x": 3.6304395178927162, + "y": 7.600963692130584, + "heading": 6.369359381798683e-16, + "angularVelocity": 2.31332397689679e-18, + "velocityX": -3.639012789511441, + "velocityY": 0.2684518658211763, + "timestamp": 1.5901895443573162 + }, + { + "x": 3.397088546983748, + "y": 7.618178114609468, + "heading": 6.672673315632139e-16, + "angularVelocity": 2.3171184870498213e-18, + "velocityX": -3.036664019135996, + "velocityY": 0.22401628391863487, + "timestamp": 1.6670340584156618 + }, + { + "x": 3.2104077520795884, + "y": 7.631949653927409, + "heading": 6.975987249465594e-16, + "angularVelocity": 2.3171184870564727e-18, + "velocityX": -2.42933145185119, + "velocityY": 0.1792130445054763, + "timestamp": 1.7438785724740073 + }, + { + "x": 3.0703971499072202, + "y": 7.642278308855744, + "heading": 7.279301183299096e-16, + "angularVelocity": 2.317118487112907e-18, + "velocityX": -1.8219986668932868, + "velocityY": 0.13440978910339627, + "timestamp": 1.820723086532353 + }, + { + "x": 2.977056746042644, + "y": 7.649164078985233, + "heading": 7.582615117132512e-16, + "angularVelocity": 2.3171184869995114e-18, + "velocityX": -1.2146658093732723, + "velocityY": 0.08960652837573517, + "timestamp": 1.8975676005906985 + }, + { + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 6.9324820107247815e-18, - "angularVelocity": 2.1185065010859018e-20, - "velocityX": -0.6073321419752766, - "velocityY": 0.04481375748833556, - "timestamp": 1.8576858189880339 + "heading": 7.885929050965928e-16, + "angularVelocity": 2.317118487006186e-18, + "velocityX": -0.6073329155713472, + "velocityY": 0.04480326498624666, + "timestamp": 1.974412114649044 }, { - "x": 2.9303865432739253, + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 7.210735620677998e-18, - "angularVelocity": 3.2578193238109785e-20, - "velocityX": -2.4863039981147584e-16, - "velocityY": 1.807761247430653e-17, - "timestamp": 1.9345303331133912 - }, - { - "x": 2.9024626512592158, - "y": 7.657656184782597, - "heading": -0.014631272955918305, - "angularVelocity": -0.24325141375371975, - "velocityX": -0.4642471116866648, - "velocityY": 0.08394553709310819, - "timestamp": 1.9946791015424687 - }, - { - "x": 2.846452095529944, - "y": 7.66640274559091, - "heading": -0.0450509493286765, - "angularVelocity": -0.505740635548114, - "velocityX": -0.9312003752049336, - "velocityY": 0.14541545964696787, - "timestamp": 2.0548278699715463 - }, - { - "x": 2.7622341154612973, - "y": 7.676698098927908, - "heading": -0.09302514522042853, - "angularVelocity": -0.7975923222487746, - "velocityX": -1.4001613377662077, - "velocityY": 0.17116482358466473, - "timestamp": 2.114976638400624 - }, - { - "x": 2.6499960299160383, - "y": 7.684708411225515, - "heading": -0.16124738434413563, - "angularVelocity": -1.1342250374444285, - "velocityX": -1.8660080410058681, - "velocityY": 0.13317500103185942, - "timestamp": 2.1751254068297015 - }, - { - "x": 2.5118120173105134, - "y": 7.6824383358290635, - "heading": -0.25311412970645, - "angularVelocity": -1.5273254592175989, - "velocityX": -2.2973706064898027, - "velocityY": -0.03774101208951341, - "timestamp": 2.235274175258779 - }, - { - "x": 2.360541446978503, - "y": 7.655914177380236, - "heading": -0.37558688187152506, - "angularVelocity": -2.0361639209534976, - "velocityX": -2.514940443217486, - "velocityY": -0.44097591923436846, - "timestamp": 2.2954229436878566 + "heading": 8.190372202752585e-16, + "angularVelocity": 3.786602635702565e-18, + "velocityX": -1.873158733653468e-17, + "velocityY": 7.970286224256345e-18, + "timestamp": 2.0512566287073897 + }, + { + "x": 2.9024623028632064, + "y": 7.657657459817362, + "heading": -0.014619904665161576, + "angularVelocity": -0.24306191404368274, + "velocityX": -0.4642519549801812, + "velocityY": 0.08396656348239065, + "timestamp": 2.1114055200823554 + }, + { + "x": 2.846450920088731, + "y": 7.666406270741266, + "heading": -0.04501686057067315, + "angularVelocity": -0.5053618647116935, + "velocityX": -0.9312122217731417, + "velocityY": 0.14545257150892452, + "timestamp": 2.171554411457321 + }, + { + "x": 2.762231494912032, + "y": 7.676704300161181, + "heading": -0.09295722077356841, + "angularVelocity": -0.7970281597403814, + "velocityX": -1.4001825013145273, + "velocityY": 0.1712089646958622, + "timestamp": 2.231703302832287 + }, + { + "x": 2.6499913633055336, + "y": 7.684716598970154, + "heading": -0.16113573353687305, + "angularVelocity": -1.1334957503776308, + "velocityX": -1.8660382434448235, + "velocityY": 0.13320775538526416, + "timestamp": 2.2918521942072525 + }, + { + "x": 2.511805828604735, + "y": 7.6824456933879235, + "heading": -0.252952190903312, + "angularVelocity": -1.5264862787614344, + "velocityX": -2.297391216063361, + "velocityY": -0.03775473712510196, + "timestamp": 2.352001085582218 + }, + { + "x": 2.3605380306783226, + "y": 7.655917745838296, + "heading": -0.37536830181828656, + "angularVelocity": -2.035218074959957, + "velocityX": -2.514889210233606, + "velocityY": -0.4410380132236904, + "timestamp": 2.412149976957184 }, { "x": 2.2177579402923584, "y": 7.6022491455078125, - "heading": -0.511351339604183, - "angularVelocity": -2.257144431689229, - "velocityX": -2.373839239194111, - "velocityY": -0.892204998938606, - "timestamp": 2.355571712116934 - }, - { - "x": 2.1016275380662224, - "y": 7.534017578559174, - "heading": -0.6392495296972435, - "angularVelocity": -2.3152962059520767, - "velocityX": -2.1022680576965818, - "velocityY": -1.2351721941287113, - "timestamp": 2.4108122423628147 - }, - { - "x": 2.008734470369056, - "y": 7.459090350747871, - "heading": -0.766874318638819, - "angularVelocity": -2.310346920522132, - "velocityX": -1.6816107174150479, - "velocityY": -1.3563814010798148, - "timestamp": 2.466052772608695 - }, - { - "x": 1.9376872862851824, - "y": 7.394462340595956, - "heading": -0.8899091991736856, - "angularVelocity": -2.2272574138450025, - "velocityX": -1.2861423264337029, - "velocityY": -1.169938265694507, - "timestamp": 2.5212933028545756 - }, - { - "x": 1.8830375504540935, - "y": 7.347535921403511, - "heading": -1.0069040331207888, - "angularVelocity": -2.117916562827061, - "velocityX": -0.9893050553249492, - "velocityY": -0.8494925552590561, - "timestamp": 2.576533833100456 - }, - { - "x": 1.8413050810141178, - "y": 7.320984027474417, - "heading": -1.1176685546197516, - "angularVelocity": -2.005131395479613, - "velocityX": -0.7554682993484771, - "velocityY": -0.48065964991469606, - "timestamp": 2.6317743633463366 - }, - { - "x": 1.810393214225769, - "y": 7.315992832183838, - "heading": -1.222253260337211, - "angularVelocity": -1.8932603516283095, - "velocityX": -0.5595867137904185, - "velocityY": -0.09035386279518245, - "timestamp": 2.687014893592217 - }, - { - "x": 1.7990257860880356, - "y": 7.317843004345945, - "heading": -1.2653301184454402, - "angularVelocity": -1.8463104956152538, - "velocityX": -0.48721756415307704, - "velocityY": 0.07929994042288478, - "timestamp": 2.7103462120300073 - }, - { - "x": 1.7893194289617778, - "y": 7.323659017290917, - "heading": -1.3071398480430554, - "angularVelocity": -1.7920002981871601, - "velocityX": -0.41602265864852256, - "velocityY": 0.2492792235671207, - "timestamp": 2.7336775304677974 - }, - { - "x": 1.7812419817978389, - "y": 7.333448844934152, - "heading": -1.347483652309462, - "angularVelocity": -1.729169501242827, - "velocityX": -0.3462062028548906, - "velocityY": 0.4196002754553388, - "timestamp": 2.7570088489055875 - }, - { - "x": 1.7747551329306968, - "y": 7.347220693006434, - "heading": -1.3861276889127772, - "angularVelocity": -1.6563160245898274, - "velocityX": -0.2780318173807648, - "velocityY": 0.590273031890653, - "timestamp": 2.7803401673433776 - }, - { - "x": 1.7698124234870385, - "y": 7.364982670787121, - "heading": -1.4227914173931293, - "angularVelocity": -1.571438347049096, - "velocityX": -0.21184869842837598, - "velocityY": 0.7612933588835104, - "timestamp": 2.8036714857811678 - }, - { - "x": 1.7663562566686506, - "y": 7.386742092358209, - "heading": -1.4571298020017938, - "angularVelocity": -1.471772146106038, - "velocityX": -0.14813422685937813, - "velocityY": 0.9326271736037851, - "timestamp": 2.827002804218958 - }, - { - "x": 1.764313200072897, - "y": 7.41250397367922, - "heading": -1.48870457067619, - "angularVelocity": -1.353321234656823, - "velocityX": -0.08756713004468872, - "velocityY": 1.1041759765827466, - "timestamp": 2.850334122656748 - }, - { - "x": 1.7635861805743018, - "y": 7.442267647668979, - "heading": -1.516934428841365, - "angularVelocity": -1.2099555471094359, - "velocityX": -0.031160669318076883, - "velocityY": 1.2756961878994322, - "timestamp": 2.873665441094538 - }, - { - "x": 1.7640407156271012, - "y": 7.4760185199259155, - "heading": -1.541001115950982, - "angularVelocity": -1.0315185219298435, - "velocityX": 0.01948175599311238, - "velocityY": 1.4465908708471107, - "timestamp": 2.8969967595323283 - }, - { - "x": 1.7654797273438503, - "y": 7.51370577350444, - "heading": -1.559655084780015, - "angularVelocity": -0.7995248480611737, - "velocityX": 0.06167725671372724, - "velocityY": 1.6153074966170409, - "timestamp": 2.9203280779701184 - }, - { - "x": 1.7676028546878815, - "y": 7.555176328390615, + "heading": -0.5110801667427121, + "angularVelocity": -2.2562654410128142, + "velocityX": -2.3737775896131, + "velocityY": -0.8922625023279553, + "timestamp": 2.4722988683321496 + }, + { + "x": 2.101645188237642, + "y": 7.5340268603699245, + "heading": -0.6389171271632187, + "angularVelocity": -2.3145402090323093, + "velocityX": -2.1022686437775406, + "velocityY": -1.235192244730253, + "timestamp": 2.527530987404247 + }, + { + "x": 2.0087612536344777, + "y": 7.459105901882302, + "heading": -0.7664975716198472, + "angularVelocity": -2.3098958830696934, + "velocityX": -1.6817014476981458, + "velocityY": -1.3564744526591341, + "timestamp": 2.582763106476344 + }, + { + "x": 1.9377174081938675, + "y": 7.394479840665335, + "heading": -0.8895097341539958, + "angularVelocity": -2.2271852791638636, + "velocityX": -1.2862777426237315, + "velocityY": -1.1700811466678287, + "timestamp": 2.6379952255484413 + }, + { + "x": 1.883064537205771, + "y": 7.3475517575068015, + "heading": -1.0065004560625568, + "angularVelocity": -2.1181646453779086, + "velocityX": -0.9895124776303853, + "velocityY": -0.8496520493271641, + "timestamp": 2.6932273446205386 + }, + { + "x": 1.8413220448871523, + "y": 7.320994180762134, + "heading": -1.1172766871950899, + "angularVelocity": -2.0056487600316086, + "velocityX": -0.7557648161887509, + "velocityY": -0.4808357381638806, + "timestamp": 2.748459463692636 + }, + { + "x": 1.8103932142257688, + "y": 7.31599283218384, + "heading": -1.2218872603273923, + "angularVelocity": -1.8940170119713384, + "velocityX": -0.5599790697967124, + "velocityY": -0.0905514519812198, + "timestamp": 2.803691582764733 + }, + { + "x": 1.799014560094557, + "y": 7.3178395552871685, + "heading": -1.2649896877020275, + "angularVelocity": -1.8471473915609153, + "velocityX": -0.4876303395876735, + "velocityY": 0.07914101294769894, + "timestamp": 2.8270261729402635 + }, + { + "x": 1.7892970764427014, + "y": 7.323653437979558, + "heading": -1.3068267846648367, + "angularVelocity": -1.7929218661843884, + "velocityX": -0.41644115362513584, + "velocityY": 0.24915298055619764, + "timestamp": 2.850360763115794 + }, + { + "x": 1.7812089941276659, + "y": 7.333442300970777, + "heading": -1.3471998191924708, + "angularVelocity": -1.730179712838083, + "velocityX": -0.3466134290074906, + "velocityY": 0.41950010338730764, + "timestamp": 2.8736953532913243 + }, + { + "x": 1.7747124589766448, + "y": 7.347214179987926, + "heading": -1.3858749923031177, + "angularVelocity": -1.657418142820578, + "velocityX": -0.27840793869984504, + "velocityY": 0.5901915959731657, + "timestamp": 2.8970299434668547 + }, + { + "x": 1.7697615522281, + "y": 7.3649769955487, + "heading": -1.4225717760886871, + "angularVelocity": -1.5726345957298005, + "velocityX": -0.21217028931756562, + "velocityY": 0.7612225210337143, + "timestamp": 2.920364533642385 + }, + { + "x": 1.7662993223458214, + "y": 7.38673785288221, + "heading": -1.4569451030122402, + "angularVelocity": -1.4730632364525922, + "velocityX": -0.14837328859330837, + "velocityY": 0.9325579395116675, + "timestamp": 2.9436991238179155 + }, + { + "x": 1.7642531161176147, + "y": 7.412501539771869, + "heading": -1.488556602137018, + "angularVelocity": -1.3547055632310927, + "velocityX": -0.08768982925275201, + "velocityY": 1.1040985376551697, + "timestamp": 2.967033713993446 + }, + { + "x": 1.7635268049301072, + "y": 7.442267147857092, + "heading": -1.5168247606019365, + "angularVelocity": -1.211427252587835, + "velocityX": -0.031125945728338194, + "velocityY": 1.275600208156052, + "timestamp": 2.9903683041689764 + }, + { + "x": 1.7639870250382692, + "y": 7.476019844911383, + "heading": -1.5409308010829128, + "angularVelocity": -1.0330603752274243, + "velocityX": 0.019722656574941596, + "velocityY": 1.446466246044897, + "timestamp": 3.0137028943445068 + }, + { + "x": 1.7654378590732076, + "y": 7.513708586961909, + "heading": -1.5596235507230065, + "angularVelocity": -0.801074692149455, + "velocityX": 0.06217525245863937, + "velocityY": 1.6151448029256037, + "timestamp": 3.037037484520037 + }, + { + "x": 1.7675794351945344, + "y": 7.555179767773511, "heading": -1.5707963267948966, - "angularVelocity": -0.47752303602545026, - "velocityX": 0.09099902989648029, - "velocityY": 1.7774629837892288, - "timestamp": 2.9436593964079085 + "angularVelocity": -0.4788074694467324, + "velocityX": 0.09177689025107998, + "velocityY": 1.7772405900199395, + "timestamp": 3.0603720746955676 }, { "x": 1.7700000000000002, - "y": 7.6, + "y": 7.599999999999997, "heading": -1.5707963267948966, - "angularVelocity": 2.1680750549204353e-19, - "velocityX": 0.10274367128149403, - "velocityY": 1.9211803965942749, - "timestamp": 2.9669907148456987 + "angularVelocity": 2.3078506585471477e-17, + "velocityX": 0.1037329041360114, + "velocityY": 1.9207636341194518, + "timestamp": 3.083706664871098 }, { - "x": 1.7728698177937292, - "y": 7.6642827500403135, + "x": 1.7729209385382234, + "y": 7.664272399608001, "heading": -1.5707963267948966, - "angularVelocity": 2.7602298655314276e-30, - "velocityX": 0.09630721046077882, - "velocityY": 2.1572422997179523, - "timestamp": 2.9967892915047587 + "angularVelocity": 2.836096341921572e-28, + "velocityX": 0.09802067351807021, + "velocityY": 2.1568491824425395, + "timestamp": 3.113505873889124 }, { - "x": 1.775470402283232, - "y": 7.735597258297608, + "x": 1.7756102473631166, + "y": 7.735578195245327, "heading": -1.5707963267948966, - "angularVelocity": -1.997147332419783e-30, - "velocityX": 0.08727210427766764, - "velocityY": 2.3932186115208283, - "timestamp": 3.0265878681638188 + "angularVelocity": -1.3962527377821026e-29, + "velocityX": 0.09024765802608749, + "velocityY": 2.3928754482762344, + "timestamp": 3.1433050829071503 }, { - "x": 1.7747633273574823, - "y": 7.8129986766433595, + "x": 1.7748330223177948, + "y": 7.812982865560118, "heading": -1.5707963267948966, - "angularVelocity": -2.066382572868049e-30, - "velocityX": -0.02372847984821113, - "velocityY": 2.5974870958213288, - "timestamp": 3.056386444822879 + "angularVelocity": 2.544623352948458e-29, + "velocityX": -0.02608206965374826, + "velocityY": 2.597541104798205, + "timestamp": 3.1731042919251764 }, { - "x": 1.7739937081633357, - "y": 7.8833634639968295, + "x": 1.7740290427893892, + "y": 7.883350378923669, "heading": -1.5707963267948966, - "angularVelocity": 4.723938447739201e-30, - "velocityX": -0.025827381051023144, - "velocityY": 2.3613472602583356, - "timestamp": 3.086185021481939 + "angularVelocity": -2.464825660281466e-29, + "velocityX": -0.02697989493346029, + "velocityY": 2.36138863018112, + "timestamp": 3.2029035009432025 }, { - "x": 1.7732502174839604, - "y": 7.946691377441134, + "x": 1.7732661288748879, + "y": 7.946680790945236, "heading": -1.5707963267948966, - "angularVelocity": -6.603517224826132e-31, - "velocityX": -0.024950543372751627, - "velocityY": 2.1251992727327242, - "timestamp": 3.115983598140999 + "angularVelocity": -6.565156523146849e-31, + "velocityX": -0.02560181762067708, + "velocityY": 2.1252380216958553, + "timestamp": 3.2327027099612287 }, { - "x": 1.7725615197118283, - "y": 8.002982577289577, + "x": 1.7725664082140153, + "y": 8.002974261088676, "heading": -1.5707963267948966, - "angularVelocity": -9.857872645178015e-31, - "velocityX": -0.023111767384506342, - "velocityY": 1.8890566651051475, - "timestamp": 3.145782174800059 + "angularVelocity": 1.0402852363586799e-33, + "velocityX": -0.02348118235190311, + "velocityY": 1.8890927644888424, + "timestamp": 3.262501918979255 }, { - "x": 1.7719417844356995, - "y": 8.052237185917901, + "x": 1.7719408549042543, + "y": 8.052230894249453, "heading": -1.5707963267948966, - "angularVelocity": 9.859209524828823e-31, - "velocityX": -0.020797479128623474, - "velocityY": 1.652918164242209, - "timestamp": 3.175580751459119 + "angularVelocity": -6.130235273525354e-33, + "velocityX": -0.02099227900324947, + "velocityY": 1.6529510273573067, + "timestamp": 3.292301127997281 }, { - "x": 1.771399461982678, - "y": 8.094455289890794, + "x": 1.7713960246129865, + "y": 8.094450761255887, "heading": -1.5707963267948966, - "angularVelocity": -6.553352029785504e-31, - "velocityX": -0.018199609304387406, - "velocityY": 1.4167825683732993, - "timestamp": 3.205379328118179 + "angularVelocity": 3.3057640560004466e-30, + "velocityX": -0.018283380976903497, + "velocityY": 1.4168116670780435, + "timestamp": 3.322100337015307 }, { - "x": 1.7709401646159184, - "y": 8.129636952308697, + "x": 1.7709362757556117, + "y": 8.129633912577273, "heading": -1.5707963267948966, - "angularVelocity": 6.554127986694498e-31, - "velocityX": -0.015413399506111826, - "velocityY": 1.1806490900700566, - "timestamp": 3.235177904777239 + "angularVelocity": -1.3860232852704577e-29, + "velocityX": -0.015428223518035878, + "velocityY": 1.1806740004443153, + "timestamp": 3.351899546033333 }, { - "x": 1.7705678907115057, - "y": 8.157782220857532, + "x": 1.770564715580057, + "y": 8.157780385843287, "heading": -1.5707963267948966, - "angularVelocity": 1.301401764569401e-30, - "velocityX": -0.012493009604860767, - "velocityY": 0.9445172120420281, - "timestamp": 3.264976481436299 + "angularVelocity": -1.6405726594716626e-28, + "velocityX": -0.01246879322679122, + "velocityY": 0.9445375965848751, + "timestamp": 3.3816987550513593 }, { - "x": 1.770285633389739, - "y": 8.178891132721446, + "x": 1.7702836712860994, + "y": 8.178890210135783, "heading": -1.5707963267948966, - "angularVelocity": -3.940207593218983e-30, - "velocityX": -0.00947217462752943, - "velocityY": 0.7083865818636769, - "timestamp": 3.294775058095359 + "angularVelocity": 2.6411722797252613e-28, + "velocityX": -0.009431266910373286, + "velocityY": 0.708402168652532, + "timestamp": 3.4114979640693854 }, { - "x": 1.7700957172852196, - "y": 8.192963717658412, + "x": 1.7700949509971242, + "y": 8.19296340858137, "heading": -1.5707963267948966, - "angularVelocity": 2.638767171459303e-30, - "velocityX": -0.006373328051609348, - "velocityY": 0.472256950322705, - "timestamp": 3.3245736347544192 + "angularVelocity": -9.675024300747488e-29, + "velocityX": -0.006333063702032236, + "velocityY": 0.47226751680135254, + "timestamp": 3.4412971730874116 }, { "x": 1.77, - "y": 8.2, + "y": 8.200000000000001, "heading": -1.5707963267948966, - "angularVelocity": 2.638857621585667e-30, - "velocityX": -0.0032121428588589426, - "velocityY": 0.23612813531646912, - "timestamp": 3.3543722114134793 + "angularVelocity": 2.089025035794717e-29, + "velocityX": -0.003186359646984827, + "velocityY": 0.23613349650910906, + "timestamp": 3.4710963821054377 }, { "x": 1.77, "y": 8.2, "heading": -1.5707963267948966, - "angularVelocity": -8.796691590043117e-31, - "velocityX": 4.572133438774496e-18, - "velocityY": 3.60345450735681e-18, - "timestamp": 3.3841707880725393 + "angularVelocity": -4.325450385226991e-30, + "velocityX": -1.494896119886391e-17, + "velocityY": 1.7968192567227342e-16, + "timestamp": 3.500895591123464 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_ReturnFromFarPiece1.traj b/src/main/deploy/choreo/B_ReturnFromFarPiece1.traj index b1fed0f..75714ee 100644 --- a/src/main/deploy/choreo/B_ReturnFromFarPiece1.traj +++ b/src/main/deploy/choreo/B_ReturnFromFarPiece1.traj @@ -1,589 +1,598 @@ { "samples": [ { - "x": 8.3, - "y": 7.6, - "heading": 2.757501519692808e-19, - "angularVelocity": 9.600967361428564e-29, - "velocityX": -2.4319155594097383e-17, - "velocityY": -4.0122443313037055e-19, + "x": 8.73, + "y": 7.45, + "heading": 3.0153335539807125e-17, + "angularVelocity": -9.150386789447993e-29, + "velocityX": 8.944879250579314e-18, + "velocityY": -6.136035981354337e-20, "timestamp": 0 }, { - "x": 8.2519779299027, - "y": 7.59802796735826, - "heading": 5.5317266150126195e-19, - "angularVelocity": 2.1474447549717986e-20, - "velocityX": -0.6166429049270267, - "velocityY": -0.025322522214253233, - "timestamp": 0.07787662796993104 - }, - { - "x": 8.155933791545712, - "y": 7.594083902150697, - "heading": 8.305951710241554e-19, - "angularVelocity": 2.147444743302548e-20, - "velocityX": -1.2332857862576319, - "velocityY": -0.050645043453678214, - "timestamp": 0.15575325593986208 - }, - { - "x": 8.011867587991711, - "y": 7.5881678045038425, - "heading": 1.1080176805595397e-18, - "angularVelocity": 2.147444759342935e-20, - "velocityX": -1.8499286282609135, - "velocityY": -0.07596756306832102, - "timestamp": 0.2336298839097931 - }, - { - "x": 7.819779325366047, - "y": 7.580279674670781, - "heading": 1.385440190094531e-18, - "angularVelocity": 2.147444758838943e-20, - "velocityX": -2.466571391609719, - "velocityY": -0.10129007943317028, - "timestamp": 0.31150651187972417 - }, - { - "x": 7.579669022044645, - "y": 7.570419513410852, - "heading": 1.6628626996296796e-18, - "angularVelocity": 2.1474447590405832e-20, - "velocityX": -3.0832139189964716, - "velocityY": -0.1266125860474561, - "timestamp": 0.3893831398496552 - }, - { - "x": 7.295744194818641, - "y": 7.558760102681403, - "heading": 1.9402852103929644e-18, - "angularVelocity": 2.1474463360667863e-20, - "velocityX": -3.6458284677609587, - "velocityY": -0.14971643012009433, - "timestamp": 0.4672597678195863 - }, - { - "x": 7.011819367581371, - "y": 7.547100692219137, - "heading": 2.2177077235682638e-18, - "angularVelocity": 2.1474494332923423e-20, - "velocityX": -3.6458284679056314, - "velocityY": -0.14971642668926177, - "timestamp": 0.5451363957895173 - }, - { - "x": 6.727894540344498, - "y": 7.5354412817471905, - "heading": 2.4951302352091332e-18, - "angularVelocity": 2.1474474629589685e-20, - "velocityX": -3.645828467900525, - "velocityY": -0.14971642681353683, - "timestamp": 0.6230130237594483 - }, - { - "x": 6.443969713075486, - "y": 7.5237818720578895, - "heading": 2.7725527581105576e-18, - "angularVelocity": 2.147461922435792e-20, - "velocityX": -3.645828468313219, - "velocityY": -0.1497164167637409, - "timestamp": 0.7008896517293793 - }, - { - "x": 6.160044885669281, - "y": 7.512122465709375, - "heading": 3.0499752697600583e-18, - "angularVelocity": 2.1474474740432105e-20, - "velocityX": -3.645828470074894, - "velocityY": -0.14971637386530257, - "timestamp": 0.7787662796993103 - }, - { - "x": 5.8761200592911305, - "y": 7.500463034323216, - "heading": 3.3273979967340457e-18, - "angularVelocity": 2.1477215638632408e-20, - "velocityX": -3.6458284568738266, - "velocityY": -0.1497166953692409, - "timestamp": 0.8566429076692413 - }, - { - "x": 5.592195230920835, - "y": 7.488803651443019, - "heading": 3.604820428609217e-18, - "angularVelocity": 2.1473426326984874e-20, - "velocityX": -3.6458284824546, - "velocityY": -0.14971607251278177, - "timestamp": 0.9345195356391723 - }, - { - "x": 5.308268547058106, + "x": 8.678452834172791, + "y": 7.450409597940094, + "heading": 6.050280891646632e-17, + "angularVelocity": 2.4319148836270395e-18, + "velocityX": -0.6391337938457895, + "velocityY": 0.005078608711127938, + "timestamp": 0.08065160428623162 + }, + { + "x": 8.57535850438037, + "y": 7.451228793805321, + "heading": 9.085228229308535e-17, + "angularVelocity": 2.431914883129373e-18, + "velocityX": -1.2782675646046895, + "velocityY": 0.010157217236737312, + "timestamp": 0.16130320857246325 + }, + { + "x": 8.420717013726057, + "y": 7.452457587570761, + "heading": 1.2120175566971728e-16, + "angularVelocity": 2.4319148832885226e-18, + "velocityX": -1.9174012968854468, + "velocityY": 0.015235825453377785, + "timestamp": 0.24195481285869486 + }, + { + "x": 8.2145283684165, + "y": 7.454095979186639, + "heading": 1.5155122904634846e-16, + "angularVelocity": 2.431914883280121e-18, + "velocityX": -2.55653495220995, + "velocityY": 0.020314433052855834, + "timestamp": 0.3226064171449265 + }, + { + "x": 7.95679258707161, + "y": 7.456143968504021, + "heading": 1.8190070242340144e-16, + "angularVelocity": 2.431914888508902e-18, + "velocityX": -3.195668376665999, + "velocityY": 0.025393038805691565, + "timestamp": 0.40325802143115813 + }, + { + "x": 7.6625121380166075, + "y": 7.4584823447093935, + "heading": 2.122501739665664e-16, + "angularVelocity": 2.431892150115273e-18, + "velocityX": -3.648786055273063, + "velocityY": 0.028993548560726043, + "timestamp": 0.48390962571738977 + }, + { + "x": 7.3682316889613775, + "y": 7.460820720895092, + "heading": 2.4259964550994947e-16, + "angularVelocity": 2.4318921528197456e-18, + "velocityX": -3.6487860552758766, + "velocityY": 0.02899354831677369, + "timestamp": 0.5645612300036214 + }, + { + "x": 7.073951239906084, + "y": 7.463159097072805, + "heading": 2.7294911705334123e-16, + "angularVelocity": 2.431892152926584e-18, + "velocityX": -3.6487860552766636, + "velocityY": 0.028993548217774053, + "timestamp": 0.645212834289853 + }, + { + "x": 6.779670790850629, + "y": 7.46549747323022, + "heading": 3.0329858859672815e-16, + "angularVelocity": 2.4318921529031445e-18, + "velocityX": -3.6487860552786633, + "velocityY": 0.028993547966090615, + "timestamp": 0.7258644385760846 + }, + { + "x": 6.485390341794982, + "y": 7.467835849363504, + "heading": 3.3364806014011606e-16, + "angularVelocity": 2.4318921529142702e-18, + "velocityX": -3.6487860552810405, + "velocityY": 0.028993547666888664, + "timestamp": 0.8065160428623163 + }, + { + "x": 6.191109892739618, + "y": 7.470174225532392, + "heading": 3.6399753168349943e-16, + "angularVelocity": 2.4318921528614766e-18, + "velocityX": -3.6487860552775326, + "velocityY": 0.028993548108348557, + "timestamp": 0.8871676471485479 + }, + { + "x": 5.896829443688916, + "y": 7.472512602287961, + "heading": 3.9434700322679583e-16, + "angularVelocity": 2.431892151781526e-18, + "velocityX": -3.648786055219731, + "velocityY": 0.028993555382614684, + "timestamp": 0.9678192514347795 + }, + { + "x": 5.602548994628648, + "y": 7.474850977839731, + "heading": 4.246964747702855e-16, + "angularVelocity": 2.4318921541771888e-18, + "velocityX": -3.6487860553383333, + "velocityY": 0.028993540456692966, + "timestamp": 1.0484708557210112 + }, + { + "x": 5.3082685470581055, "y": 7.477189540863037, - "heading": 3.881406761933183e-18, - "angularVelocity": 1.0737232772850433e-20, - "velocityX": -3.64585230850462, - "velocityY": -0.14913473891635948, - "timestamp": 1.0123961636091034 - }, - { - "x": 5.028627693595278, - "y": 7.497782181981604, - "heading": 4.1587848720220655e-18, - "angularVelocity": 2.118506785259316e-20, - "velocityX": -3.639047714019591, - "velocityY": 0.2679780248850872, - "timestamp": 1.0892406777344608 - }, - { - "x": 4.748989879107207, - "y": 7.518416049826399, - "heading": 4.436163015430491e-18, - "angularVelocity": 2.118550144945551e-20, - "velocityX": -3.6390081669576992, - "velocityY": 0.2685145202575587, - "timestamp": 1.166085191859818 - }, - { - "x": 4.469352066877837, - "y": 7.53904994828081, - "heading": 4.713541138262534e-18, - "angularVelocity": 2.1185233682992707e-20, - "velocityX": -3.6390081375645655, - "velocityY": 0.26851491858936644, - "timestamp": 1.2429297059851754 - }, - { - "x": 4.189714253377734, - "y": 7.5596838295128, - "heading": 4.9909192511776896e-18, - "angularVelocity": 2.118509579668935e-20, - "velocityX": -3.6390081541009813, - "velocityY": 0.268514694469029, - "timestamp": 1.3197742201105327 - }, - { - "x": 3.9100764400605628, - "y": 7.580317713223909, - "heading": 5.2682973643801506e-18, - "angularVelocity": 2.1185099535494865e-20, - "velocityX": -3.6390081517204464, - "velocityY": 0.2685147267305082, - "timestamp": 1.39661873423589 - }, - { - "x": 3.6304386268060798, - "y": 7.600951597744441, - "heading": 5.545591457915407e-18, - "angularVelocity": 2.0091727183515773e-20, - "velocityX": -3.6390081509046595, - "velocityY": 0.2685147372637596, - "timestamp": 1.4734632483612473 - }, - { - "x": 3.397087952925323, - "y": 7.618170051677312, - "heading": 5.822969568480078e-18, - "angularVelocity": 2.1185065209273208e-20, - "velocityX": -3.0366601511734386, - "velocityY": 0.22406874620592004, - "timestamp": 1.5503077624866046 - }, - { - "x": 3.210407395644333, - "y": 7.6319448161653884, - "heading": 6.100347679045377e-18, - "angularVelocity": 2.1185065217376727e-20, - "velocityX": -2.4293283574733384, - "velocityY": 0.17925501442568673, - "timestamp": 1.627152276611962 - }, - { - "x": 3.0703969716895227, - "y": 7.642275889973785, - "heading": 6.37772578961099e-18, - "angularVelocity": 2.1185065221445832e-20, - "velocityX": -1.821996346107541, - "velocityY": 0.13444126657556482, - "timestamp": 1.7039967907373192 - }, - { - "x": 2.9770566866367267, - "y": 7.6491632726910055, - "heading": 6.655103900175346e-18, - "angularVelocity": 2.1185065205170544e-20, - "velocityX": -1.2146642621818082, - "velocityY": 0.08962751337051074, - "timestamp": 1.7808413048626766 - }, - { - "x": 2.9303865432739244, + "heading": 4.54947878770186e-16, + "angularVelocity": 1.2159517589791973e-18, + "velocityX": -3.6487860368672314, + "velocityY": 0.028995864918033788, + "timestamp": 1.1291224600072427 + }, + { + "x": 5.0286303652572135, + "y": 7.497818426819656, + "heading": 4.852792666366976e-16, + "angularVelocity": 2.317046686913831e-18, + "velocityX": -3.6390129500795747, + "velocityY": 0.2684496897326407, + "timestamp": 1.2059669740655883 + }, + { + "x": 4.74899219578683, + "y": 7.518447479923772, + "heading": 5.156106618040484e-16, + "angularVelocity": 2.317141694856443e-18, + "velocityX": -3.639012789619103, + "velocityY": 0.26845186487162814, + "timestamp": 1.2828114881239339 + }, + { + "x": 4.469354026278326, + "y": 7.53907653251117, + "heading": 5.459420533388648e-16, + "angularVelocity": 2.3170944316093188e-18, + "velocityX": -3.639012790115152, + "velocityY": 0.26845185814741257, + "timestamp": 1.3596560021822794 + }, + { + "x": 4.18971585681519, + "y": 7.559705585713528, + "heading": 5.762734448613439e-16, + "angularVelocity": 2.3170942710563877e-18, + "velocityX": -3.6390127895247923, + "velocityY": 0.26845186615006567, + "timestamp": 1.436500516240625 + }, + { + "x": 3.9100776873548275, + "y": 7.580334638953499, + "heading": 6.066048363838091e-16, + "angularVelocity": 2.317094270874412e-18, + "velocityX": -3.639012789488683, + "velocityY": 0.268451866639544, + "timestamp": 1.5133450302989706 + }, + { + "x": 3.6304395178927162, + "y": 7.600963692130584, + "heading": 6.369359381798683e-16, + "angularVelocity": 2.31332397689679e-18, + "velocityX": -3.639012789511441, + "velocityY": 0.2684518658211763, + "timestamp": 1.5901895443573162 + }, + { + "x": 3.397088546983748, + "y": 7.618178114609468, + "heading": 6.672673315632139e-16, + "angularVelocity": 2.3171184870498213e-18, + "velocityX": -3.036664019135996, + "velocityY": 0.22401628391863487, + "timestamp": 1.6670340584156618 + }, + { + "x": 3.2104077520795884, + "y": 7.631949653927409, + "heading": 6.975987249465594e-16, + "angularVelocity": 2.3171184870564727e-18, + "velocityX": -2.42933145185119, + "velocityY": 0.1792130445054763, + "timestamp": 1.7438785724740073 + }, + { + "x": 3.0703971499072202, + "y": 7.642278308855744, + "heading": 7.279301183299096e-16, + "angularVelocity": 2.317118487112907e-18, + "velocityX": -1.8219986668932868, + "velocityY": 0.13440978910339627, + "timestamp": 1.820723086532353 + }, + { + "x": 2.977056746042644, + "y": 7.649164078985233, + "heading": 7.582615117132512e-16, + "angularVelocity": 2.3171184869995114e-18, + "velocityX": -1.2146658093732723, + "velocityY": 0.08960652837573517, + "timestamp": 1.8975676005906985 + }, + { + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 6.9324820107247815e-18, - "angularVelocity": 2.1185065010859018e-20, - "velocityX": -0.6073321419752766, - "velocityY": 0.04481375748833556, - "timestamp": 1.8576858189880339 + "heading": 7.885929050965928e-16, + "angularVelocity": 2.317118487006186e-18, + "velocityX": -0.6073329155713472, + "velocityY": 0.04480326498624666, + "timestamp": 1.974412114649044 }, { - "x": 2.9303865432739253, + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 7.210735620677998e-18, - "angularVelocity": 3.2578193238109785e-20, - "velocityX": -2.4863039981147584e-16, - "velocityY": 1.807761247430653e-17, - "timestamp": 1.9345303331133912 - }, - { - "x": 2.9024626512592158, - "y": 7.657656184782597, - "heading": -0.014631272955918305, - "angularVelocity": -0.24325141375371975, - "velocityX": -0.4642471116866648, - "velocityY": 0.08394553709310819, - "timestamp": 1.9946791015424687 - }, - { - "x": 2.846452095529944, - "y": 7.66640274559091, - "heading": -0.0450509493286765, - "angularVelocity": -0.505740635548114, - "velocityX": -0.9312003752049336, - "velocityY": 0.14541545964696787, - "timestamp": 2.0548278699715463 - }, - { - "x": 2.7622341154612973, - "y": 7.676698098927908, - "heading": -0.09302514522042853, - "angularVelocity": -0.7975923222487746, - "velocityX": -1.4001613377662077, - "velocityY": 0.17116482358466473, - "timestamp": 2.114976638400624 - }, - { - "x": 2.6499960299160383, - "y": 7.684708411225515, - "heading": -0.16124738434413563, - "angularVelocity": -1.1342250374444285, - "velocityX": -1.8660080410058681, - "velocityY": 0.13317500103185942, - "timestamp": 2.1751254068297015 - }, - { - "x": 2.5118120173105134, - "y": 7.6824383358290635, - "heading": -0.25311412970645, - "angularVelocity": -1.5273254592175989, - "velocityX": -2.2973706064898027, - "velocityY": -0.03774101208951341, - "timestamp": 2.235274175258779 - }, - { - "x": 2.360541446978503, - "y": 7.655914177380236, - "heading": -0.37558688187152506, - "angularVelocity": -2.0361639209534976, - "velocityX": -2.514940443217486, - "velocityY": -0.44097591923436846, - "timestamp": 2.2954229436878566 + "heading": 8.190372202752585e-16, + "angularVelocity": 3.786602635702565e-18, + "velocityX": -1.873158733653468e-17, + "velocityY": 7.970286224256345e-18, + "timestamp": 2.0512566287073897 + }, + { + "x": 2.9024623028632064, + "y": 7.657657459817362, + "heading": -0.014619904665161576, + "angularVelocity": -0.24306191404368274, + "velocityX": -0.4642519549801812, + "velocityY": 0.08396656348239065, + "timestamp": 2.1114055200823554 + }, + { + "x": 2.846450920088731, + "y": 7.666406270741266, + "heading": -0.04501686057067315, + "angularVelocity": -0.5053618647116935, + "velocityX": -0.9312122217731417, + "velocityY": 0.14545257150892452, + "timestamp": 2.171554411457321 + }, + { + "x": 2.762231494912032, + "y": 7.676704300161181, + "heading": -0.09295722077356841, + "angularVelocity": -0.7970281597403814, + "velocityX": -1.4001825013145273, + "velocityY": 0.1712089646958622, + "timestamp": 2.231703302832287 + }, + { + "x": 2.6499913633055336, + "y": 7.684716598970154, + "heading": -0.16113573353687305, + "angularVelocity": -1.1334957503776308, + "velocityX": -1.8660382434448235, + "velocityY": 0.13320775538526416, + "timestamp": 2.2918521942072525 + }, + { + "x": 2.511805828604735, + "y": 7.6824456933879235, + "heading": -0.252952190903312, + "angularVelocity": -1.5264862787614344, + "velocityX": -2.297391216063361, + "velocityY": -0.03775473712510196, + "timestamp": 2.352001085582218 + }, + { + "x": 2.3605380306783226, + "y": 7.655917745838296, + "heading": -0.37536830181828656, + "angularVelocity": -2.035218074959957, + "velocityX": -2.514889210233606, + "velocityY": -0.4410380132236904, + "timestamp": 2.412149976957184 }, { "x": 2.2177579402923584, "y": 7.6022491455078125, - "heading": -0.511351339604183, - "angularVelocity": -2.257144431689229, - "velocityX": -2.373839239194111, - "velocityY": -0.892204998938606, - "timestamp": 2.355571712116934 - }, - { - "x": 2.1016275380662224, - "y": 7.534017578559174, - "heading": -0.6392495296972435, - "angularVelocity": -2.3152962059520767, - "velocityX": -2.1022680576965818, - "velocityY": -1.2351721941287113, - "timestamp": 2.4108122423628147 - }, - { - "x": 2.008734470369056, - "y": 7.459090350747871, - "heading": -0.766874318638819, - "angularVelocity": -2.310346920522132, - "velocityX": -1.6816107174150479, - "velocityY": -1.3563814010798148, - "timestamp": 2.466052772608695 - }, - { - "x": 1.9376872862851824, - "y": 7.394462340595956, - "heading": -0.8899091991736856, - "angularVelocity": -2.2272574138450025, - "velocityX": -1.2861423264337029, - "velocityY": -1.169938265694507, - "timestamp": 2.5212933028545756 - }, - { - "x": 1.8830375504540935, - "y": 7.347535921403511, - "heading": -1.0069040331207888, - "angularVelocity": -2.117916562827061, - "velocityX": -0.9893050553249492, - "velocityY": -0.8494925552590561, - "timestamp": 2.576533833100456 - }, - { - "x": 1.8413050810141178, - "y": 7.320984027474417, - "heading": -1.1176685546197516, - "angularVelocity": -2.005131395479613, - "velocityX": -0.7554682993484771, - "velocityY": -0.48065964991469606, - "timestamp": 2.6317743633463366 - }, - { - "x": 1.810393214225769, - "y": 7.315992832183838, - "heading": -1.222253260337211, - "angularVelocity": -1.8932603516283095, - "velocityX": -0.5595867137904185, - "velocityY": -0.09035386279518245, - "timestamp": 2.687014893592217 - }, - { - "x": 1.7990257860880356, - "y": 7.317843004345945, - "heading": -1.2653301184454402, - "angularVelocity": -1.8463104956152538, - "velocityX": -0.48721756415307704, - "velocityY": 0.07929994042288478, - "timestamp": 2.7103462120300073 - }, - { - "x": 1.7893194289617778, - "y": 7.323659017290917, - "heading": -1.3071398480430554, - "angularVelocity": -1.7920002981871601, - "velocityX": -0.41602265864852256, - "velocityY": 0.2492792235671207, - "timestamp": 2.7336775304677974 - }, - { - "x": 1.7812419817978389, - "y": 7.333448844934152, - "heading": -1.347483652309462, - "angularVelocity": -1.729169501242827, - "velocityX": -0.3462062028548906, - "velocityY": 0.4196002754553388, - "timestamp": 2.7570088489055875 - }, - { - "x": 1.7747551329306968, - "y": 7.347220693006434, - "heading": -1.3861276889127772, - "angularVelocity": -1.6563160245898274, - "velocityX": -0.2780318173807648, - "velocityY": 0.590273031890653, - "timestamp": 2.7803401673433776 - }, - { - "x": 1.7698124234870385, - "y": 7.364982670787121, - "heading": -1.4227914173931293, - "angularVelocity": -1.571438347049096, - "velocityX": -0.21184869842837598, - "velocityY": 0.7612933588835104, - "timestamp": 2.8036714857811678 - }, - { - "x": 1.7663562566686506, - "y": 7.386742092358209, - "heading": -1.4571298020017938, - "angularVelocity": -1.471772146106038, - "velocityX": -0.14813422685937813, - "velocityY": 0.9326271736037851, - "timestamp": 2.827002804218958 - }, - { - "x": 1.764313200072897, - "y": 7.41250397367922, - "heading": -1.48870457067619, - "angularVelocity": -1.353321234656823, - "velocityX": -0.08756713004468872, - "velocityY": 1.1041759765827466, - "timestamp": 2.850334122656748 - }, - { - "x": 1.7635861805743018, - "y": 7.442267647668979, - "heading": -1.516934428841365, - "angularVelocity": -1.2099555471094359, - "velocityX": -0.031160669318076883, - "velocityY": 1.2756961878994322, - "timestamp": 2.873665441094538 - }, - { - "x": 1.7640407156271012, - "y": 7.4760185199259155, - "heading": -1.541001115950982, - "angularVelocity": -1.0315185219298435, - "velocityX": 0.01948175599311238, - "velocityY": 1.4465908708471107, - "timestamp": 2.8969967595323283 - }, - { - "x": 1.7654797273438503, - "y": 7.51370577350444, - "heading": -1.559655084780015, - "angularVelocity": -0.7995248480611737, - "velocityX": 0.06167725671372724, - "velocityY": 1.6153074966170409, - "timestamp": 2.9203280779701184 - }, - { - "x": 1.7676028546878815, - "y": 7.555176328390615, + "heading": -0.5110801667427121, + "angularVelocity": -2.2562654410128142, + "velocityX": -2.3737775896131, + "velocityY": -0.8922625023279553, + "timestamp": 2.4722988683321496 + }, + { + "x": 2.101645188237642, + "y": 7.5340268603699245, + "heading": -0.6389171271632187, + "angularVelocity": -2.3145402090323093, + "velocityX": -2.1022686437775406, + "velocityY": -1.235192244730253, + "timestamp": 2.527530987404247 + }, + { + "x": 2.0087612536344777, + "y": 7.459105901882302, + "heading": -0.7664975716198472, + "angularVelocity": -2.3098958830696934, + "velocityX": -1.6817014476981458, + "velocityY": -1.3564744526591341, + "timestamp": 2.582763106476344 + }, + { + "x": 1.9377174081938675, + "y": 7.394479840665335, + "heading": -0.8895097341539958, + "angularVelocity": -2.2271852791638636, + "velocityX": -1.2862777426237315, + "velocityY": -1.1700811466678287, + "timestamp": 2.6379952255484413 + }, + { + "x": 1.883064537205771, + "y": 7.3475517575068015, + "heading": -1.0065004560625568, + "angularVelocity": -2.1181646453779086, + "velocityX": -0.9895124776303853, + "velocityY": -0.8496520493271641, + "timestamp": 2.6932273446205386 + }, + { + "x": 1.8413220448871523, + "y": 7.320994180762134, + "heading": -1.1172766871950899, + "angularVelocity": -2.0056487600316086, + "velocityX": -0.7557648161887509, + "velocityY": -0.4808357381638806, + "timestamp": 2.748459463692636 + }, + { + "x": 1.8103932142257688, + "y": 7.31599283218384, + "heading": -1.2218872603273923, + "angularVelocity": -1.8940170119713384, + "velocityX": -0.5599790697967124, + "velocityY": -0.0905514519812198, + "timestamp": 2.803691582764733 + }, + { + "x": 1.799014560094557, + "y": 7.3178395552871685, + "heading": -1.2649896877020275, + "angularVelocity": -1.8471473915609153, + "velocityX": -0.4876303395876735, + "velocityY": 0.07914101294769894, + "timestamp": 2.8270261729402635 + }, + { + "x": 1.7892970764427014, + "y": 7.323653437979558, + "heading": -1.3068267846648367, + "angularVelocity": -1.7929218661843884, + "velocityX": -0.41644115362513584, + "velocityY": 0.24915298055619764, + "timestamp": 2.850360763115794 + }, + { + "x": 1.7812089941276659, + "y": 7.333442300970777, + "heading": -1.3471998191924708, + "angularVelocity": -1.730179712838083, + "velocityX": -0.3466134290074906, + "velocityY": 0.41950010338730764, + "timestamp": 2.8736953532913243 + }, + { + "x": 1.7747124589766448, + "y": 7.347214179987926, + "heading": -1.3858749923031177, + "angularVelocity": -1.657418142820578, + "velocityX": -0.27840793869984504, + "velocityY": 0.5901915959731657, + "timestamp": 2.8970299434668547 + }, + { + "x": 1.7697615522281, + "y": 7.3649769955487, + "heading": -1.4225717760886871, + "angularVelocity": -1.5726345957298005, + "velocityX": -0.21217028931756562, + "velocityY": 0.7612225210337143, + "timestamp": 2.920364533642385 + }, + { + "x": 1.7662993223458214, + "y": 7.38673785288221, + "heading": -1.4569451030122402, + "angularVelocity": -1.4730632364525922, + "velocityX": -0.14837328859330837, + "velocityY": 0.9325579395116675, + "timestamp": 2.9436991238179155 + }, + { + "x": 1.7642531161176147, + "y": 7.412501539771869, + "heading": -1.488556602137018, + "angularVelocity": -1.3547055632310927, + "velocityX": -0.08768982925275201, + "velocityY": 1.1040985376551697, + "timestamp": 2.967033713993446 + }, + { + "x": 1.7635268049301072, + "y": 7.442267147857092, + "heading": -1.5168247606019365, + "angularVelocity": -1.211427252587835, + "velocityX": -0.031125945728338194, + "velocityY": 1.275600208156052, + "timestamp": 2.9903683041689764 + }, + { + "x": 1.7639870250382692, + "y": 7.476019844911383, + "heading": -1.5409308010829128, + "angularVelocity": -1.0330603752274243, + "velocityX": 0.019722656574941596, + "velocityY": 1.446466246044897, + "timestamp": 3.0137028943445068 + }, + { + "x": 1.7654378590732076, + "y": 7.513708586961909, + "heading": -1.5596235507230065, + "angularVelocity": -0.801074692149455, + "velocityX": 0.06217525245863937, + "velocityY": 1.6151448029256037, + "timestamp": 3.037037484520037 + }, + { + "x": 1.7675794351945344, + "y": 7.555179767773511, "heading": -1.5707963267948966, - "angularVelocity": -0.47752303602545026, - "velocityX": 0.09099902989648029, - "velocityY": 1.7774629837892288, - "timestamp": 2.9436593964079085 + "angularVelocity": -0.4788074694467324, + "velocityX": 0.09177689025107998, + "velocityY": 1.7772405900199395, + "timestamp": 3.0603720746955676 }, { "x": 1.7700000000000002, - "y": 7.6, + "y": 7.599999999999997, "heading": -1.5707963267948966, - "angularVelocity": 2.1680750549204353e-19, - "velocityX": 0.10274367128149403, - "velocityY": 1.9211803965942749, - "timestamp": 2.9669907148456987 + "angularVelocity": 2.3078506585471477e-17, + "velocityX": 0.1037329041360114, + "velocityY": 1.9207636341194518, + "timestamp": 3.083706664871098 }, { - "x": 1.7728698177937292, - "y": 7.6642827500403135, + "x": 1.7729209385382234, + "y": 7.664272399608001, "heading": -1.5707963267948966, - "angularVelocity": 2.7602298655314276e-30, - "velocityX": 0.09630721046077882, - "velocityY": 2.1572422997179523, - "timestamp": 2.9967892915047587 + "angularVelocity": 2.836096341921572e-28, + "velocityX": 0.09802067351807021, + "velocityY": 2.1568491824425395, + "timestamp": 3.113505873889124 }, { - "x": 1.775470402283232, - "y": 7.735597258297608, + "x": 1.7756102473631166, + "y": 7.735578195245327, "heading": -1.5707963267948966, - "angularVelocity": -1.997147332419783e-30, - "velocityX": 0.08727210427766764, - "velocityY": 2.3932186115208283, - "timestamp": 3.0265878681638188 + "angularVelocity": -1.3962527377821026e-29, + "velocityX": 0.09024765802608749, + "velocityY": 2.3928754482762344, + "timestamp": 3.1433050829071503 }, { - "x": 1.7747633273574823, - "y": 7.8129986766433595, + "x": 1.7748330223177948, + "y": 7.812982865560118, "heading": -1.5707963267948966, - "angularVelocity": -2.066382572868049e-30, - "velocityX": -0.02372847984821113, - "velocityY": 2.5974870958213288, - "timestamp": 3.056386444822879 + "angularVelocity": 2.544623352948458e-29, + "velocityX": -0.02608206965374826, + "velocityY": 2.597541104798205, + "timestamp": 3.1731042919251764 }, { - "x": 1.7739937081633357, - "y": 7.8833634639968295, + "x": 1.7740290427893892, + "y": 7.883350378923669, "heading": -1.5707963267948966, - "angularVelocity": 4.723938447739201e-30, - "velocityX": -0.025827381051023144, - "velocityY": 2.3613472602583356, - "timestamp": 3.086185021481939 + "angularVelocity": -2.464825660281466e-29, + "velocityX": -0.02697989493346029, + "velocityY": 2.36138863018112, + "timestamp": 3.2029035009432025 }, { - "x": 1.7732502174839604, - "y": 7.946691377441134, + "x": 1.7732661288748879, + "y": 7.946680790945236, "heading": -1.5707963267948966, - "angularVelocity": -6.603517224826132e-31, - "velocityX": -0.024950543372751627, - "velocityY": 2.1251992727327242, - "timestamp": 3.115983598140999 + "angularVelocity": -6.565156523146849e-31, + "velocityX": -0.02560181762067708, + "velocityY": 2.1252380216958553, + "timestamp": 3.2327027099612287 }, { - "x": 1.7725615197118283, - "y": 8.002982577289577, + "x": 1.7725664082140153, + "y": 8.002974261088676, "heading": -1.5707963267948966, - "angularVelocity": -9.857872645178015e-31, - "velocityX": -0.023111767384506342, - "velocityY": 1.8890566651051475, - "timestamp": 3.145782174800059 + "angularVelocity": 1.0402852363586799e-33, + "velocityX": -0.02348118235190311, + "velocityY": 1.8890927644888424, + "timestamp": 3.262501918979255 }, { - "x": 1.7719417844356995, - "y": 8.052237185917901, + "x": 1.7719408549042543, + "y": 8.052230894249453, "heading": -1.5707963267948966, - "angularVelocity": 9.859209524828823e-31, - "velocityX": -0.020797479128623474, - "velocityY": 1.652918164242209, - "timestamp": 3.175580751459119 + "angularVelocity": -6.130235273525354e-33, + "velocityX": -0.02099227900324947, + "velocityY": 1.6529510273573067, + "timestamp": 3.292301127997281 }, { - "x": 1.771399461982678, - "y": 8.094455289890794, + "x": 1.7713960246129865, + "y": 8.094450761255887, "heading": -1.5707963267948966, - "angularVelocity": -6.553352029785504e-31, - "velocityX": -0.018199609304387406, - "velocityY": 1.4167825683732993, - "timestamp": 3.205379328118179 + "angularVelocity": 3.3057640560004466e-30, + "velocityX": -0.018283380976903497, + "velocityY": 1.4168116670780435, + "timestamp": 3.322100337015307 }, { - "x": 1.7709401646159184, - "y": 8.129636952308697, + "x": 1.7709362757556117, + "y": 8.129633912577273, "heading": -1.5707963267948966, - "angularVelocity": 6.554127986694498e-31, - "velocityX": -0.015413399506111826, - "velocityY": 1.1806490900700566, - "timestamp": 3.235177904777239 + "angularVelocity": -1.3860232852704577e-29, + "velocityX": -0.015428223518035878, + "velocityY": 1.1806740004443153, + "timestamp": 3.351899546033333 }, { - "x": 1.7705678907115057, - "y": 8.157782220857532, + "x": 1.770564715580057, + "y": 8.157780385843287, "heading": -1.5707963267948966, - "angularVelocity": 1.301401764569401e-30, - "velocityX": -0.012493009604860767, - "velocityY": 0.9445172120420281, - "timestamp": 3.264976481436299 + "angularVelocity": -1.6405726594716626e-28, + "velocityX": -0.01246879322679122, + "velocityY": 0.9445375965848751, + "timestamp": 3.3816987550513593 }, { - "x": 1.770285633389739, - "y": 8.178891132721446, + "x": 1.7702836712860994, + "y": 8.178890210135783, "heading": -1.5707963267948966, - "angularVelocity": -3.940207593218983e-30, - "velocityX": -0.00947217462752943, - "velocityY": 0.7083865818636769, - "timestamp": 3.294775058095359 + "angularVelocity": 2.6411722797252613e-28, + "velocityX": -0.009431266910373286, + "velocityY": 0.708402168652532, + "timestamp": 3.4114979640693854 }, { - "x": 1.7700957172852196, - "y": 8.192963717658412, + "x": 1.7700949509971242, + "y": 8.19296340858137, "heading": -1.5707963267948966, - "angularVelocity": 2.638767171459303e-30, - "velocityX": -0.006373328051609348, - "velocityY": 0.472256950322705, - "timestamp": 3.3245736347544192 + "angularVelocity": -9.675024300747488e-29, + "velocityX": -0.006333063702032236, + "velocityY": 0.47226751680135254, + "timestamp": 3.4412971730874116 }, { "x": 1.77, - "y": 8.2, + "y": 8.200000000000001, "heading": -1.5707963267948966, - "angularVelocity": 2.638857621585667e-30, - "velocityX": -0.0032121428588589426, - "velocityY": 0.23612813531646912, - "timestamp": 3.3543722114134793 + "angularVelocity": 2.089025035794717e-29, + "velocityX": -0.003186359646984827, + "velocityY": 0.23613349650910906, + "timestamp": 3.4710963821054377 }, { "x": 1.77, "y": 8.2, "heading": -1.5707963267948966, - "angularVelocity": -8.796691590043117e-31, - "velocityX": 4.572133438774496e-18, - "velocityY": 3.60345450735681e-18, - "timestamp": 3.3841707880725393 + "angularVelocity": -4.325450385226991e-30, + "velocityX": -1.494896119886391e-17, + "velocityY": 1.7968192567227342e-16, + "timestamp": 3.500895591123464 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/B_ReturnFromFarwithStop.1.traj b/src/main/deploy/choreo/B_ReturnFromFarwithStop.1.traj new file mode 100644 index 0000000..83d722a --- /dev/null +++ b/src/main/deploy/choreo/B_ReturnFromFarwithStop.1.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 8.73, + "y": 7.45, + "heading": 0, + "angularVelocity": 0, + "velocityX": -2.693667833342153e-29, + "velocityY": -4.53306417990796e-35, + "timestamp": 0 + }, + { + "x": 8.657925937477573, + "y": 7.455789387308592, + "heading": -1.2188341073833624e-16, + "angularVelocity": -1.2760069620061655e-15, + "velocityX": -0.7545490196731954, + "velocityY": 0.06060955030593678, + "timestamp": 0.09551939058067259 + }, + { + "x": 8.513777815345431, + "y": 7.46736816169181, + "heading": -4.062778690542764e-16, + "angularVelocity": -2.9773478323983444e-15, + "velocityX": -1.5090980088529606, + "velocityY": 0.1212190981624727, + "timestamp": 0.19103878116134518 + }, + { + "x": 8.297555639429, + "y": 7.484736322681725, + "heading": -9.34438717110679e-16, + "angularVelocity": -5.529357236530814e-15, + "velocityX": -2.26364693704592, + "velocityY": 0.18182864112021124, + "timestamp": 0.28655817174201775 + }, + { + "x": 8.009259427204483, + "y": 7.50789386887455, + "heading": -1.9501377833476253e-15, + "angularVelocity": -1.0633432982967515e-14, + "velocityX": -3.018195682279077, + "velocityY": 0.24243816938160717, + "timestamp": 0.38207756232269036 + }, + { + "x": 7.66183760867627, + "y": 7.535800710760828, + "heading": -1.9431705716357535e-15, + "angularVelocity": 7.29402864644545e-17, + "velocityX": -3.637186297108895, + "velocityY": 0.2921589189025347, + "timestamp": 0.47759695290336296 + }, + { + "x": 7.31441579014799, + "y": 7.563707552647111, + "heading": -1.943030848678189e-15, + "angularVelocity": 1.4627706079539333e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.5731163434840355 + }, + { + "x": 6.9669939716197105, + "y": 7.591614394533395, + "heading": -1.9365713004742454e-15, + "angularVelocity": 6.762551733917979e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.2921589189025843, + "timestamp": 0.6686357340647081 + }, + { + "x": 6.619572153091432, + "y": 7.619521236419678, + "heading": -1.937339896809014e-15, + "angularVelocity": -8.046495378072972e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.7641551246453807 + }, + { + "x": 6.272150334563152, + "y": 7.647428078305961, + "heading": -1.9502459990087004e-15, + "angularVelocity": -1.351149972924591e-16, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.8596745152260533 + }, + { + "x": 5.924728516034873, + "y": 7.675334920192245, + "heading": -1.9529774200702442e-15, + "angularVelocity": -2.8595461564986775e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.9551939058067259 + }, + { + "x": 5.577306697506593, + "y": 7.7032417620785285, + "heading": -1.9466253347863878e-15, + "angularVelocity": 6.650047959108812e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 1.0507132963873984 + }, + { + "x": 5.22988487897838, + "y": 7.731148603964805, + "heading": -1.9387248800321926e-15, + "angularVelocity": 8.271048197596407e-17, + "velocityX": -3.6371862971088955, + "velocityY": 0.29215891890252993, + "timestamp": 1.146232686968071 + }, + { + "x": 4.941588666753863, + "y": 7.754306150157631, + "heading": -1.9386609172268905e-15, + "angularVelocity": 6.696315831572461e-19, + "velocityX": -3.0181956822790776, + "velocityY": 0.24243816938160498, + "timestamp": 1.2417520775487436 + }, + { + "x": 4.72536649083743, + "y": 7.771674311147546, + "heading": -1.9386284069178885e-15, + "angularVelocity": 3.4035295166666236e-19, + "velocityX": -2.26364693704592, + "velocityY": 0.1818286411202101, + "timestamp": 1.3372714681294162 + }, + { + "x": 4.581218368705289, + "y": 7.783253085530764, + "heading": -1.9386109286969595e-15, + "angularVelocity": 1.8298085390218347e-19, + "velocityX": -1.5090980088529609, + "velocityY": 0.12121909816247209, + "timestamp": 1.4327908587100888 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 7.85782206050662e-20, + "velocityX": -0.7545490196731954, + "velocityY": 0.060609550305936524, + "timestamp": 1.5283102492907614 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 0, + "velocityX": 1.333662779175558e-32, + "velocityY": 0, + "timestamp": 1.623829639871434 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/B_ReturnFromFarwithStop.traj b/src/main/deploy/choreo/B_ReturnFromFarwithStop.traj new file mode 100644 index 0000000..83d722a --- /dev/null +++ b/src/main/deploy/choreo/B_ReturnFromFarwithStop.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 8.73, + "y": 7.45, + "heading": 0, + "angularVelocity": 0, + "velocityX": -2.693667833342153e-29, + "velocityY": -4.53306417990796e-35, + "timestamp": 0 + }, + { + "x": 8.657925937477573, + "y": 7.455789387308592, + "heading": -1.2188341073833624e-16, + "angularVelocity": -1.2760069620061655e-15, + "velocityX": -0.7545490196731954, + "velocityY": 0.06060955030593678, + "timestamp": 0.09551939058067259 + }, + { + "x": 8.513777815345431, + "y": 7.46736816169181, + "heading": -4.062778690542764e-16, + "angularVelocity": -2.9773478323983444e-15, + "velocityX": -1.5090980088529606, + "velocityY": 0.1212190981624727, + "timestamp": 0.19103878116134518 + }, + { + "x": 8.297555639429, + "y": 7.484736322681725, + "heading": -9.34438717110679e-16, + "angularVelocity": -5.529357236530814e-15, + "velocityX": -2.26364693704592, + "velocityY": 0.18182864112021124, + "timestamp": 0.28655817174201775 + }, + { + "x": 8.009259427204483, + "y": 7.50789386887455, + "heading": -1.9501377833476253e-15, + "angularVelocity": -1.0633432982967515e-14, + "velocityX": -3.018195682279077, + "velocityY": 0.24243816938160717, + "timestamp": 0.38207756232269036 + }, + { + "x": 7.66183760867627, + "y": 7.535800710760828, + "heading": -1.9431705716357535e-15, + "angularVelocity": 7.29402864644545e-17, + "velocityX": -3.637186297108895, + "velocityY": 0.2921589189025347, + "timestamp": 0.47759695290336296 + }, + { + "x": 7.31441579014799, + "y": 7.563707552647111, + "heading": -1.943030848678189e-15, + "angularVelocity": 1.4627706079539333e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.5731163434840355 + }, + { + "x": 6.9669939716197105, + "y": 7.591614394533395, + "heading": -1.9365713004742454e-15, + "angularVelocity": 6.762551733917979e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.2921589189025843, + "timestamp": 0.6686357340647081 + }, + { + "x": 6.619572153091432, + "y": 7.619521236419678, + "heading": -1.937339896809014e-15, + "angularVelocity": -8.046495378072972e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.7641551246453807 + }, + { + "x": 6.272150334563152, + "y": 7.647428078305961, + "heading": -1.9502459990087004e-15, + "angularVelocity": -1.351149972924591e-16, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.8596745152260533 + }, + { + "x": 5.924728516034873, + "y": 7.675334920192245, + "heading": -1.9529774200702442e-15, + "angularVelocity": -2.8595461564986775e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.9551939058067259 + }, + { + "x": 5.577306697506593, + "y": 7.7032417620785285, + "heading": -1.9466253347863878e-15, + "angularVelocity": 6.650047959108812e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 1.0507132963873984 + }, + { + "x": 5.22988487897838, + "y": 7.731148603964805, + "heading": -1.9387248800321926e-15, + "angularVelocity": 8.271048197596407e-17, + "velocityX": -3.6371862971088955, + "velocityY": 0.29215891890252993, + "timestamp": 1.146232686968071 + }, + { + "x": 4.941588666753863, + "y": 7.754306150157631, + "heading": -1.9386609172268905e-15, + "angularVelocity": 6.696315831572461e-19, + "velocityX": -3.0181956822790776, + "velocityY": 0.24243816938160498, + "timestamp": 1.2417520775487436 + }, + { + "x": 4.72536649083743, + "y": 7.771674311147546, + "heading": -1.9386284069178885e-15, + "angularVelocity": 3.4035295166666236e-19, + "velocityX": -2.26364693704592, + "velocityY": 0.1818286411202101, + "timestamp": 1.3372714681294162 + }, + { + "x": 4.581218368705289, + "y": 7.783253085530764, + "heading": -1.9386109286969595e-15, + "angularVelocity": 1.8298085390218347e-19, + "velocityX": -1.5090980088529609, + "velocityY": 0.12121909816247209, + "timestamp": 1.4327908587100888 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 7.85782206050662e-20, + "velocityX": -0.7545490196731954, + "velocityY": 0.060609550305936524, + "timestamp": 1.5283102492907614 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 0, + "velocityX": 1.333662779175558e-32, + "velocityY": 0, + "timestamp": 1.623829639871434 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/R_PickUpFarPiece1.1.traj b/src/main/deploy/choreo/R_PickUpFarPiece1.1.traj index ca53ede..b12a07e 100644 --- a/src/main/deploy/choreo/R_PickUpFarPiece1.1.traj +++ b/src/main/deploy/choreo/R_PickUpFarPiece1.1.traj @@ -4,739 +4,757 @@ "x": 14.65, "y": 7.9, "heading": -1.571, - "angularVelocity": -4.2261388604687027e-29, - "velocityX": -1.358742071286637e-27, - "velocityY": -4.102470365728161e-28, + "angularVelocity": 5.121565466053403e-29, + "velocityX": -7.318157607438789e-28, + "velocityY": -7.303102655433029e-29, "timestamp": 0 }, { - "x": 14.61609999625719, - "y": 7.893333332597285, + "x": 14.612130307338697, + "y": 7.8982505033556425, "heading": -1.571, - "angularVelocity": 5.482485458181477e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.06602728549153251 + "angularVelocity": 2.4796809944092742e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.06916416332818838 }, { - "x": 14.548299990175126, - "y": 7.879999998067872, + "x": 14.53639092351289, + "y": 7.894751510136077, "heading": -1.571, - "angularVelocity": 5.482485455324769e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.13205457098306503 + "angularVelocity": 2.4796809946674783e-18, + "velocityX": -1.0950668696217758, + "velocityY": -0.05058968476151164, + "timestamp": 0.13832832665637676 }, { - "x": 14.446599984093062, - "y": 7.859999996871792, - "heading": -1.571, - "angularVelocity": 5.4824854577856795e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.19808185647459753 - }, - { - "x": 14.31099998268951, - "y": 7.833333329929108, - "heading": -1.571, - "angularVelocity": 5.482485457103134e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.26410914196613006 - }, - { - "x": 14.141499999999999, - "y": 7.800000000000001, - "heading": -1.571, - "angularVelocity": 5.482485456843769e-19, - "velocityX": -2.567120265927734, - "velocityY": -0.5048417435452771, - "timestamp": 0.3301364274576626 - }, - { - "x": 13.97200001731049, - "y": 7.766666670070894, - "heading": -1.571, - "angularVelocity": 5.482485456705774e-19, - "velocityX": -2.5671202659277337, - "velocityY": -0.5048417435452771, - "timestamp": 0.3961637129491951 - }, - { - "x": 13.836400015906937, - "y": 7.740000003128209, - "heading": -1.571, - "angularVelocity": 5.48248545665747e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.46219099844072764 - }, - { - "x": 13.734700009824873, - "y": 7.720000001932129, - "heading": -1.571, - "angularVelocity": 5.482485455640794e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.5282182839322601 - }, - { - "x": 13.666900003742809, - "y": 7.7066666674027156, - "heading": -1.571, - "angularVelocity": 5.482485458874598e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.5942455694237926 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 5.482485458591381e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.6602728549153251 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 1.8274951520571554e-19, - "velocityX": 7.923170915113678e-19, - "velocityY": -2.3856267514882045e-18, - "timestamp": 0.7263001404068575 - }, - { - "x": 13.61789791542167, - "y": 7.699041687293577, - "heading": -1.571, - "angularVelocity": 3.6283789505269005e-19, - "velocityX": -0.3456036812103074, - "velocityY": -0.021930508822978256, - "timestamp": 0.7699978363499157 - }, - { - "x": 13.587693746684298, - "y": 7.697125061907338, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378947932204e-19, - "velocityX": -0.6912073528254145, - "velocityY": -0.04386101703708676, - "timestamp": 0.8136955322929739 - }, - { - "x": 13.542387494311997, - "y": 7.694250123874538, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378952839068e-19, - "velocityX": -1.036811012446521, - "velocityY": -0.06579152449010808, - "timestamp": 0.857393228236032 - }, - { - "x": 13.48197915897862, - "y": 7.69041687323794, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378951779324e-19, - "velocityX": -1.3824146566467697, - "velocityY": -0.08772203096458873, - "timestamp": 0.9010909241790902 - }, - { - "x": 13.406468741582641, - "y": 7.685625310054556, + "x": 14.422781851017241, + "y": 7.889503020456549, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789513575714e-19, - "velocityX": -1.7280182802858743, - "velocityY": -0.10965253613434846, - "timestamp": 0.9447886201221484 + "angularVelocity": 2.479680994653865e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.20749248998456515 }, { - "x": 13.315856243381926, - "y": 7.679875434404205, + "x": 14.271303094841082, + "y": 7.882505034547556, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789489243236e-19, - "velocityX": -2.073621875139378, - "velocityY": -0.13158303947749891, - "timestamp": 0.9884863160652065 + "angularVelocity": 2.479680994579454e-18, + "velocityX": -2.190133573327318, + "velocityY": -0.10117936185805763, + "timestamp": 0.2766566533127535 }, { - "x": 13.210141666263272, - "y": 7.673167246406615, + "x": 14.081954669952392, + "y": 7.873757553100586, "heading": -1.5709999999999997, - "angularVelocity": 3.628378951941211e-19, - "velocityX": -2.419225426814483, - "velocityY": -0.15351354008073578, - "timestamp": 1.0321840120082648 + "angularVelocity": 2.479680994607856e-18, + "velocityX": -2.7376666726989773, + "velocityY": -0.12647418874225766, + "timestamp": 0.3458208166409419 }, { - "x": 13.089325013371338, - "y": 7.6655007462613325, + "x": 13.892606245063702, + "y": 7.865010071653616, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789509770827e-19, - "velocityX": -2.7648289065256026, - "velocityY": -0.17544403611745055, - "timestamp": 1.075881707951323 + "angularVelocity": 2.4796809946116294e-18, + "velocityX": -2.737666672698978, + "velocityY": -0.12647418874225766, + "timestamp": 0.41498497996913025 }, { - "x": 12.953406290995442, - "y": 7.656875934367449, + "x": 13.741127488887543, + "y": 7.858012085744623, "heading": -1.5709999999999997, - "angularVelocity": 3.628378950782348e-19, - "velocityX": -3.110432242308825, - "velocityY": -0.19737452302112604, - "timestamp": 1.1195794038943812 + "angularVelocity": 2.479680994536425e-18, + "velocityX": -2.1901335733273184, + "velocityY": -0.10117936185805763, + "timestamp": 0.4841491432973186 }, { - "x": 12.802385518003497, - "y": 7.647292811922241, + "x": 13.627518416391894, + "y": 7.852763596065095, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789514392664e-19, - "velocityX": -3.4560351463092576, - "velocityY": -0.21930498252574065, - "timestamp": 1.1632770998374393 - }, + "angularVelocity": 2.47968099468633e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.553313306625507 + }, + { + "x": 13.551779032566087, + "y": 7.849264602845529, + "heading": -1.5709999999999995, + "angularVelocity": 2.479680994669315e-18, + "velocityX": -1.095066869621776, + "velocityY": -0.05058968476151165, + "timestamp": 0.6224774699536955 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 2.4796809942925844e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.6916416332818839 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 8.26560334321477e-19, + "velocityX": 2.2636957131725456e-19, + "velocityY": 2.8360545446294056e-18, + "timestamp": 0.7608057966100723 + }, + { + "x": 13.499332557357375, + "y": 7.8449192409498565, + "heading": -1.5709999999999995, + "angularVelocity": 1.549669581859894e-18, + "velocityX": -0.33723861810938877, + "velocityY": -0.060056189169625046, + "timestamp": 0.8040297388416239 + }, + { + "x": 13.47017899267198, + "y": 7.839727510520138, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695813124185e-18, + "velocityX": -0.6744772267466429, + "velocityY": -0.12011237665243168, + "timestamp": 0.8472536810731754 + }, + { + "x": 13.42644864636038, + "y": 7.831939915003153, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695815123512e-18, + "velocityX": -1.0117158235437291, + "velocityY": -0.1801685620267154, + "timestamp": 0.890477623304727 + }, + { + "x": 13.368141519080574, + "y": 7.821556454516082, + "heading": -1.5709999999999993, + "angularVelocity": 1.549669581692892e-18, + "velocityX": -1.348954405117744, + "velocityY": -0.2402247446900413, + "timestamp": 0.9337015655362786 + }, + { + "x": 13.295257611709896, + "y": 7.808577129215161, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814706194e-18, + "velocityX": -1.6861929663943322, + "velocityY": -0.3002809237387572, + "timestamp": 0.9769255077678302 + }, + { + "x": 13.207796925476618, + "y": 7.793001939319122, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814452525e-18, + "velocityX": -2.0234314992545284, + "velocityY": -0.3603370977270199, + "timestamp": 1.0201494499993817 + }, + { + "x": 13.105759462223137, + "y": 7.774830885156067, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815528492e-18, + "velocityX": -2.3606699894901486, + "velocityY": -0.42039326412460515, + "timestamp": 1.0633733922309334 + }, + { + "x": 12.989145225020126, + "y": 7.754063967272826, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815122657e-18, + "velocityX": -2.6979084086848464, + "velocityY": -0.48044941787106793, + "timestamp": 1.106597334462485 + }, + { + "x": 12.857954220008912, + "y": 7.730701186763059, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814976101e-18, + "velocityX": -3.035146685797867, + "velocityY": -0.5405055463153154, + "timestamp": 1.1498212766940368 + }, + { + "x": 12.71218646561342, + "y": 7.70474254690774, + "heading": -1.570999999999999, + "angularVelocity": 1.5496695815191512e-18, + "velocityX": -3.372384536667451, + "velocityY": -0.6005615988532015, + "timestamp": 1.1930452189255885 + }, + { + "x": 12.556909538173402, + "y": 7.677090492761939, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692345146303e-18, + "velocityX": -3.592382356245949, + "velocityY": -0.6397392907307838, + "timestamp": 1.2362691611571401 + }, + { + "x": 12.401632610733195, + "y": 7.649438438616105, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692344266034e-18, + "velocityX": -3.592382356250238, + "velocityY": -0.6397392907315478, + "timestamp": 1.2794931033886918 + }, + { + "x": 12.24635568329299, + "y": 7.621786384470271, + "heading": -1.570999999999999, + "angularVelocity": 1.549669234478791e-18, + "velocityX": -3.5923823562502384, + "velocityY": -0.6397392907315479, + "timestamp": 1.3227170456202435 + }, + { + "x": 12.091078758239746, + "y": 7.594134330749512, + "heading": -1.570999999999999, + "angularVelocity": 3.234878008246914e-18, + "velocityX": -3.592382301027084, + "velocityY": -0.6397392808972879, + "timestamp": 1.3659409878517952 + }, + { + "x": 12.00283728057898, + "y": 7.582107515529106, + "heading": -1.5788342162362206, + "angularVelocity": -0.3119507041975876, + "velocityX": -3.513687938342297, + "velocityY": -0.47889582877488296, + "timestamp": 1.3910546232141834 + }, + { + "x": 11.916970328359971, + "y": 7.573753042701851, + "heading": -1.5952402865312374, + "angularVelocity": -0.6532734133600175, + "velocityX": -3.4191366952627433, + "velocityY": -0.3326668045754269, + "timestamp": 1.4161682585765716 + }, + { + "x": 11.833816357634106, + "y": 7.56871710218962, + "heading": -1.6206538515119442, + "angularVelocity": -1.011942899307139, + "velocityX": -3.311108468604305, + "velocityY": -0.20052614603931174, + "timestamp": 1.4412818939389598 + }, + { + "x": 11.753731808376116, + "y": 7.566574800515445, + "heading": -1.6553839157644212, + "angularVelocity": -1.3829166407559692, + "velocityX": -3.188887156413404, + "velocityY": -0.08530432345956382, + "timestamp": 1.466395529301348 + }, + { + "x": 11.677147361800145, + "y": 7.566714891670578, + "heading": -1.6997475676814937, + "angularVelocity": -1.766516526856978, + "velocityX": -3.0495165463240133, + "velocityY": 0.0055782905616100845, + "timestamp": 1.4915091646637362 + }, + { + "x": 11.604396744023525, + "y": 7.568557677988233, + "heading": -1.7537219634693322, + "angularVelocity": -2.1492067957909815, + "velocityX": -2.8968572939289086, + "velocityY": 0.07337791964526899, + "timestamp": 1.5166228000261244 + }, + { + "x": 11.535756559334365, + "y": 7.571432948149072, + "heading": -1.8171150418628605, + "angularVelocity": -2.5242493760370213, + "velocityX": -2.7331839336949435, + "velocityY": 0.1144904001078914, + "timestamp": 1.5417364353885126 + }, + { + "x": 11.471380887760333, + "y": 7.574513868964298, + "heading": -1.889488559627284, + "angularVelocity": -2.881841546237513, + "velocityX": -2.5633752598977027, + "velocityY": 0.12267920477200389, + "timestamp": 1.5668500707509008 + }, + { + "x": 11.411255276796373, + "y": 7.576880729956244, + "heading": -1.9701854617324397, + "angularVelocity": -3.213270438176076, + "velocityX": -2.3941420704876353, + "velocityY": 0.09424605230448276, + "timestamp": 1.591963706113289 + }, + { + "x": 11.35519068850538, + "y": 7.57762868801966, + "heading": -2.058445803348398, + "angularVelocity": -3.5144390822869056, + "velocityX": -2.2324361838505236, + "velocityY": 0.029782946698429406, + "timestamp": 1.6170773414756772 + }, + { + "x": 11.302817578230115, + "y": 7.575911070751928, + "heading": -2.1534608466432936, + "angularVelocity": -3.783404589727689, + "velocityX": -2.0854451981786317, + "velocityY": -0.06839381248247857, + "timestamp": 1.6421909768380654 + }, + { + "x": 11.253523245308234, + "y": 7.570911967645187, + "heading": -2.254232970772999, + "angularVelocity": -4.012645826678452, + "velocityX": -1.9628513439257314, + "velocityY": -0.19905931716328515, + "timestamp": 1.6673046122004536 + }, + { + "x": 11.206151241323777, + "y": 7.561834135279753, + "heading": -2.3588178409636855, + "angularVelocity": -4.164465585385836, + "velocityX": -1.8863061162162815, + "velocityY": -0.36147026244552627, + "timestamp": 1.6924182475628418 + }, + { + "x": 11.158562568853249, + "y": 7.548296021124272, + "heading": -2.4631530925128593, + "angularVelocity": -4.154526019174503, + "velocityX": -1.8949336399871903, + "velocityY": -0.5390742503060728, + "timestamp": 1.71753188292523 + }, + { + "x": 11.108338354901361, + "y": 7.5311193171207975, + "heading": -2.5624670465584027, + "angularVelocity": -3.9545829431864727, + "velocityX": -1.9998782823423, + "velocityY": -0.6839592817038007, + "timestamp": 1.7426455182876182 + }, + { + "x": 11.054321656083031, + "y": 7.51174890982464, + "heading": -2.654640214483575, + "angularVelocity": -3.6702439370135087, + "velocityX": -2.150891260419857, + "velocityY": -0.7713103665237963, + "timestamp": 1.7677591536500064 + }, + { + "x": 10.996033122413428, + "y": 7.491461551309535, + "heading": -2.738672595307348, + "angularVelocity": -3.346085885665274, + "velocityX": -2.3209914784730197, + "velocityY": -0.8078224527158445, + "timestamp": 1.7928727890123946 + }, + { + "x": 10.933381820015002, + "y": 7.471624968268411, + "heading": -2.8141850565003126, + "angularVelocity": -3.006831153809885, + "velocityX": -2.4947125931527254, + "velocityY": -0.7898730213622603, + "timestamp": 1.8179864243747828 + }, + { + "x": 10.866838632540613, + "y": 7.45402686067354, + "heading": -2.88213732233945, + "angularVelocity": -2.7057916888040063, + "velocityX": -2.6496835887816075, + "velocityY": -0.7007391538860639, + "timestamp": 1.843100059737171 + }, + { + "x": 10.797385329302331, + "y": 7.4401695505140575, + "heading": -2.944658830810706, + "angularVelocity": -2.4895443279744143, + "velocityX": -2.765561506172815, + "velocityY": -0.5517843179424147, + "timestamp": 1.8682136950995591 + }, + { + "x": 10.724987539198043, + "y": 7.429906192310285, + "heading": -3.000767776942746, + "angularVelocity": -2.234202468993167, + "velocityX": -2.88280804668823, + "velocityY": -0.40867672305021235, + "timestamp": 1.8933273304619473 + }, + { + "x": 10.649597795782494, + "y": 7.422945580592199, + "heading": -3.0495257230757873, + "angularVelocity": -1.9414929551011741, + "velocityX": -3.0019446538772403, + "velocityY": -0.277164640548885, + "timestamp": 1.9184409658243355 + }, + { + "x": 10.571198566507455, + "y": 7.418912040075103, + "heading": -3.0901902225982423, + "angularVelocity": -1.6192199550436448, + "velocityX": -3.121779389711535, + "velocityY": -0.16061157450510036, + "timestamp": 1.9435546011867237 + }, { - "x": 12.643257011655002, - "y": 7.637195207926121, - "heading": -1.5709999999999997, - "angularVelocity": 3.6283776976177666e-19, - "velocityX": -3.641576584629373, - "velocityY": -0.23107863648642077, - "timestamp": 1.2069747957804975 - }, - { - "x": 12.484128505306387, - "y": 7.627097603929993, - "heading": -1.5709999999999997, - "angularVelocity": 3.628377701929186e-19, - "velocityX": -3.641576584632129, - "velocityY": -0.23107863648659568, - "timestamp": 1.2506724917235557 - }, - { - "x": 12.325, - "y": 7.617, - "heading": -1.5709999999999997, - "angularVelocity": 7.36418634228573e-19, - "velocityX": -3.6415765607812913, - "velocityY": -0.23107863497312509, - "timestamp": 1.2943701876666138 - }, - { - "x": 12.229388489581337, - "y": 7.6109112417716736, - "heading": -1.5863134997517452, - "angularVelocity": -0.5552849027741624, - "velocityX": -3.466982017671504, - "velocityY": -0.22078529243098394, - "timestamp": 1.3219479224028547 - }, - { - "x": 12.138013718648649, - "y": 7.607167866658879, - "heading": -1.6148419963350573, - "angularVelocity": -1.0344757049904476, - "velocityX": -3.313353029413532, - "velocityY": -0.13573903544277124, - "timestamp": 1.3495256571390957 - }, - { - "x": 12.050416525228757, - "y": 7.6063106068200685, - "heading": -1.6545433874529916, - "angularVelocity": -1.4396175573391428, - "velocityX": -3.1763737760802657, - "velocityY": -0.03108521591825106, - "timestamp": 1.3771033918753366 - }, - { - "x": 11.966360780867383, - "y": 7.607509934453374, - "heading": -1.7036222302241029, - "angularVelocity": -1.7796546105222553, - "velocityX": -3.0479568088277835, - "velocityY": 0.04348898286151133, - "timestamp": 1.4046811266115775 - }, - { - "x": 11.885408562298633, - "y": 7.609633528647565, - "heading": -1.7598323716256097, - "angularVelocity": -2.038243602642159, - "velocityX": -2.9354194368390085, - "velocityY": 0.07700393866649435, - "timestamp": 1.4322588613478184 - }, - { - "x": 11.807516118182633, - "y": 7.611964238202936, - "heading": -1.8223777210675893, - "angularVelocity": -2.2679654453193687, - "velocityX": -2.8244685381514696, - "velocityY": 0.08451417702220176, - "timestamp": 1.4598365960840594 - }, - { - "x": 11.73334817309539, - "y": 7.614170969229463, - "heading": -1.8925164168284103, - "angularVelocity": -2.5433088116786786, - "velocityX": -2.6894139709660934, - "velocityY": 0.08001857468104064, - "timestamp": 1.4874143308203003 - }, - { - "x": 11.663695028307295, - "y": 7.615711030101031, - "heading": -1.9717573083827227, - "angularVelocity": -2.873364774597518, - "velocityX": -2.5257021816429535, - "velocityY": 0.05584435727941134, - "timestamp": 1.5149920655565412 - }, - { - "x": 11.598996797656051, - "y": 7.61568305023367, - "heading": -2.0607742017770874, - "angularVelocity": -3.2278537104564915, - "velocityX": -2.3460313644334034, - "velocityY": -0.0010145817859481632, - "timestamp": 1.5425698002927821 - }, - { - "x": 11.539114067010884, - "y": 7.613037677777285, - "heading": -2.1591584444474705, - "angularVelocity": -3.5675244399638015, - "velocityX": -2.171415862031463, - "velocityY": -0.09592421138591448, - "timestamp": 1.570147535029023 - }, - { - "x": 11.483508926014887, - "y": 7.606776074097583, - "heading": -2.266083388472652, - "angularVelocity": -3.8772199764711472, - "velocityX": -2.0163056004352273, - "velocityY": -0.22705286491397708, - "timestamp": 1.597725269765264 - }, - { - "x": 11.431087870109415, - "y": 7.595775934313344, - "heading": -2.379919574340892, - "angularVelocity": -4.127829459416678, - "velocityX": -1.900847056759369, - "velocityY": -0.3988775687867351, - "timestamp": 1.6253030045015049 - }, - { - "x": 11.377404486650294, - "y": 7.579105655561354, - "heading": -2.4911780043079133, - "angularVelocity": -4.034357101158641, - "velocityX": -1.9466204883235783, - "velocityY": -0.6044832511236621, - "timestamp": 1.6528807392377458 - }, - { - "x": 11.319370240403567, - "y": 7.55896629351811, - "heading": -2.5937282135555715, - "angularVelocity": -3.718587122128468, - "velocityX": -2.1043877171847964, - "velocityY": -0.7302761534208634, - "timestamp": 1.6804584739739867 - }, - { - "x": 11.256272025403964, - "y": 7.536866509262458, - "heading": -2.6862489609900964, - "angularVelocity": -3.3549074396216714, - "velocityX": -2.288012978697651, - "velocityY": -0.8013632906045023, - "timestamp": 1.7080362087102277 - }, - { - "x": 11.187885977180416, - "y": 7.514090907716094, - "heading": -2.7680211777317423, - "angularVelocity": -2.965153502408188, - "velocityX": -2.4797558203240193, - "velocityY": -0.8258691935430503, - "timestamp": 1.7356139434464686 - }, - { - "x": 11.114289943184776, - "y": 7.491935954019794, - "heading": -2.838743341274155, - "angularVelocity": -2.5644660164736934, - "velocityX": -2.66867582488287, - "velocityY": -0.8033637972151884, - "timestamp": 1.7631916781827095 - }, - { - "x": 11.03615204188367, - "y": 7.47266971293565, - "heading": -2.8998457718181054, - "angularVelocity": -2.215643566389652, - "velocityX": -2.8333690946131496, - "velocityY": -0.6986157952569366, - "timestamp": 1.7907694129189504 - }, - { - "x": 10.95383190394823, - "y": 7.456821193599831, - "heading": -2.9513961589840805, - "angularVelocity": -1.8692756188648758, - "velocityX": -2.985021747535358, - "velocityY": -0.5746853208720242, - "timestamp": 1.8183471476551913 - }, - { - "x": 10.867220309314249, - "y": 7.443764913233828, - "heading": -2.991973638967047, - "angularVelocity": -1.4713855351450116, - "velocityX": -3.1406348440997536, - "velocityY": -0.47343556281455274, - "timestamp": 1.8459248823914323 - }, - { - "x": 10.77690563773812, - "y": 7.432954012684261, - "heading": -3.0224568962493583, - "angularVelocity": -1.1053575492644694, - "velocityX": -3.2749126220813625, - "velocityY": -0.39201553909209025, - "timestamp": 1.8735026171276732 - }, - { - "x": 10.68603576950712, - "y": 7.424800287046038, - "heading": -3.0521033841408234, - "angularVelocity": -1.0750153402739042, - "velocityX": -3.295044683694906, - "velocityY": -0.2956633572774275, - "timestamp": 1.901080351863914 - }, - { - "x": 10.596200893845815, - "y": 7.419761050330992, - "heading": -3.0855587154803814, - "angularVelocity": -1.2131283319507546, - "velocityX": -3.257514676985064, - "velocityY": -0.18272844971648752, - "timestamp": 1.928658086600155 + "x": 10.489548200758582, + "y": 7.417268311990692, + "heading": -3.1214676257157588, + "angularVelocity": -1.2454351059179014, + "velocityX": -3.251236412835276, + "velocityY": -0.06545161864044534, + "timestamp": 1.968668236549112 }, { - "x": 10.506861440016637, - "y": 7.417879567543994, - "heading": -3.1208586855613984, - "angularVelocity": -1.2800170289051036, - "velocityX": -3.2395501183704285, - "velocityY": -0.0682247039140415, - "timestamp": 1.956235821336396 - }, - { - "x": 10.412835700689799, - "y": 7.416682265603839, - "heading": -3.141685307179586, - "angularVelocity": -0.7551969665883731, - "velocityX": -3.4094801558619205, - "velocityY": -0.043415528926000375, - "timestamp": 1.9838135560726369 + "x": 10.404255697585379, + "y": 7.417275691573601, + "heading": -3.141685307179586, + "angularVelocity": -0.8050479817886684, + "velocityX": -3.3962627051968566, + "velocityY": 0.00029384765695126695, + "timestamp": 1.9937818719115001 }, { "x": 10.317412376403809, "y": 7.418716907501221, "heading": -3.141685307179586, - "angularVelocity": 5.937778880078681e-19, - "velocityX": -3.4601581746520087, - "velocityY": 0.07377842730165238, - "timestamp": 2.011391290808878 + "angularVelocity": 7.47770773326549e-18, + "velocityX": -3.4580147369515903, + "velocityY": 0.05738778583033072, + "timestamp": 2.0188955072738883 }, { - "x": 10.251727837257341, - "y": 7.420700742643172, + "x": 10.251732343249431, + "y": 7.420561186854179, "heading": -3.141685307179586, - "angularVelocity": -1.9442515737792743e-28, - "velocityX": -3.3048160733761773, - "velocityY": 0.09981359920071599, - "timestamp": 2.03126669013159 + "angularVelocity": 3.1949765996869766e-27, + "velocityX": -3.304533757090645, + "velocityY": 0.09279050400334278, + "timestamp": 2.0387712410331353 }, { - "x": 10.189167330602293, - "y": 7.422886644704941, + "x": 10.189171391771723, + "y": 7.422674521924776, "heading": -3.141685307179586, - "angularVelocity": -2.386183922112865e-28, - "velocityX": -3.1476352066827475, - "velocityY": 0.10998028398210202, - "timestamp": 2.051142089454302 + "angularVelocity": -2.371251182895612e-27, + "velocityX": -3.1476046235828683, + "velocityY": 0.10632739883689808, + "timestamp": 2.0586469747923823 }, { - "x": 10.129736436099503, - "y": 7.425149795146946, + "x": 10.129739097787168, + "y": 7.424900096756528, "heading": -3.141685307179586, - "angularVelocity": 2.7299904854133836e-28, - "velocityX": -2.9901736079776406, - "velocityY": 0.11386691684825133, - "timestamp": 2.0710174887770143 + "angularVelocity": -5.101900895484119e-28, + "velocityX": -2.99019370577468, + "velocityY": 0.11197447393435742, + "timestamp": 2.0785227085516293 }, { - "x": 10.073436091249755, - "y": 7.427423630307786, + "x": 10.073437312690587, + "y": 7.427157718473777, "heading": -3.141685307179586, - "angularVelocity": 1.8386974301791735e-28, - "velocityX": -2.8326648403694086, - "velocityY": 0.1144045019634283, - "timestamp": 2.0908928880997264 + "angularVelocity": -5.203506078458522e-28, + "velocityX": -2.8326896394648293, + "velocityY": 0.11358683631985049, + "timestamp": 2.0983984423108764 }, { - "x": 10.020266165929796, - "y": 7.4296668280040885, + "x": 10.020266158378618, + "y": 7.429398776074464, "heading": -3.141685307179586, - "angularVelocity": 5.650458450043071e-29, - "velocityX": -2.6751626197114717, - "velocityY": 0.11286302528469754, - "timestamp": 2.1107682874224385 + "angularVelocity": 2.645370534116333e-28, + "velocityX": -2.6751794402172218, + "velocityY": 0.11275345241754915, + "timestamp": 2.1182741760701234 }, { - "x": 9.970226259362496, - "y": 7.431851251828943, + "x": 9.970225293830397, + "y": 7.431590675011163, "heading": -3.141685307179586, - "angularVelocity": -4.102009655023591e-29, - "velocityX": -2.5176805635356243, - "velocityY": 0.10990590877633212, - "timestamp": 2.1306436867451506 + "angularVelocity": 5.191709905827532e-29, + "velocityX": -2.5176863986186695, + "velocityY": 0.11028015182983106, + "timestamp": 2.1381499098293704 }, { - "x": 9.923315923027515, - "y": 7.43395651278906, + "x": 9.92331426564673, + "y": 7.433710048067309, "heading": -3.141685307179586, - "angularVelocity": -3.5765307146759774e-29, - "velocityX": -2.3602210739674327, - "velocityY": 0.10592295158126644, - "timestamp": 2.1505190860678627 + "angularVelocity": 1.0273003820442257e-28, + "velocityX": -2.3602161687157612, + "velocityY": 0.1066311856365788, + "timestamp": 2.1580256435886174 }, { - "x": 9.879534728461786, - "y": 7.4359671584530975, + "x": 9.87953261796101, + "y": 7.435739325604849, "heading": -3.141685307179586, - "angularVelocity": -1.1942964906968253e-29, - "velocityX": -2.2027831418560893, - "velocityY": 0.1011625291844662, - "timestamp": 2.170394485390575 + "angularVelocity": -2.0170615068521438e-28, + "velocityX": -2.202768874651166, + "velocityY": 0.10209824513253805, + "timestamp": 2.1779013773478644 }, { - "x": 9.838882286352929, - "y": 7.437871074521549, + "x": 9.838879927126666, + "y": 7.437664817248215, "heading": -3.141685307179586, - "angularVelocity": -5.641467124729511e-31, - "velocityX": -2.0453647973957065, - "velocityY": 0.09579259453048798, - "timestamp": 2.190269884713287 + "angularVelocity": -2.3492440709815457e-28, + "velocityX": -2.045342895349926, + "velocityY": 0.0968765061300322, + "timestamp": 2.1977771111071114 }, { - "x": 9.801358249188596, - "y": 7.439658508993957, + "x": 9.801355810492355, + "y": 7.439475555427537, "heading": -3.141685307179586, - "angularVelocity": 3.5641593543820497e-29, - "velocityX": -1.8879639374819575, - "velocityY": 0.08993200304483205, - "timestamp": 2.210145284035999 + "angularVelocity": 2.8629022838202107e-28, + "velocityX": -1.8879361682359468, + "velocityY": 0.09110296008466975, + "timestamp": 2.2176528448663584 }, { - "x": 9.766962308401824, - "y": 7.441321442796545, + "x": 9.766959925956705, + "y": 7.441162556870241, "heading": -3.141685307179586, - "angularVelocity": 1.9252934621446341e-29, - "velocityX": -1.7305786026380017, - "velocityY": 0.08366794425548009, - "timestamp": 2.230020683358711 + "angularVelocity": -1.5840875871720601e-28, + "velocityX": -1.7305466531340934, + "velocityY": 0.08487744216837698, + "timestamp": 2.2375285786256054 }, { - "x": 9.735694189965349, - "y": 7.442853165687241, + "x": 9.735691968423001, + "y": 7.442718328937222, "heading": -3.141685307179586, - "angularVelocity": -5.183683118127715e-29, - "velocityX": -1.5732070550525459, - "velocityY": 0.07706627000674852, - "timestamp": 2.249896082681423 + "angularVelocity": 9.330544204917246e-29, + "velocityX": -1.5731724882437799, + "velocityY": 0.0782749500383743, + "timestamp": 2.2574043123848524 }, { - "x": 9.707553649885064, - "y": 7.444247980075085, + "x": 9.707551665488573, + "y": 7.444136527128733, "heading": -3.141685307179586, - "angularVelocity": 1.3886630469011578e-29, - "velocityX": -1.4158477836532484, - "velocityY": 0.07017793027434112, - "timestamp": 2.2697714820041353 + "angularVelocity": -6.58238928802494e-29, + "velocityX": -1.4158120286419644, + "velocityY": 0.07135324958012863, + "timestamp": 2.2772800461440994 }, { - "x": 9.682540470094276, - "y": 7.445500987918728, + "x": 9.682538773252615, + "y": 7.445411710024687, "heading": -3.141685307179586, - "angularVelocity": -4.3165247768849716e-30, - "velocityX": -1.258499483942791, - "velocityY": 0.06304315316127049, - "timestamp": 2.2896468813268473 + "angularVelocity": 2.6630221770954893e-29, + "velocityX": -1.2584638403259507, + "velocityY": 0.06415777708639624, + "timestamp": 2.2971557799033464 }, { - "x": 9.660654454881797, - "y": 7.446607933488268, + "x": 9.660653072546591, + "y": 7.446539159318976, "heading": -3.141685307179586, - "angularVelocity": 5.491476754830618e-28, - "velocityX": -1.10116103113811, - "velocityY": 0.05569425557531322, - "timestamp": 2.3095222806495594 + "angularVelocity": -4.1528582058952054e-29, + "velocityX": -1.1011266789504681, + "velocityY": 0.056724914307379, + "timestamp": 2.3170315136625934 }, { - "x": 9.641895427846347, - "y": 7.4475650848138315, + "x": 9.64189436565749, + "y": 7.447514744684453, "heading": -3.141685307179586, - "angularVelocity": -5.338468664029318e-28, - "velocityX": -0.9438314536912185, - "velocityY": 0.04815758969279047, - "timestamp": 2.3293976799722715 + "angularVelocity": 4.9624701627257074e-29, + "velocityX": -0.9437994650322394, + "velocityY": 0.04908424399792036, + "timestamp": 2.3369072474218404 }, { - "x": 9.626263229320996, - "y": 7.44836914261392, + "x": 9.626262473522596, + "y": 7.448334820329731, "heading": -3.141685307179586, - "angularVelocity": 1.3824274242904182e-29, - "velocityX": -0.7865099096393957, - "velocityY": 0.04045492556066521, - "timestamp": 2.3492730792949836 + "angularVelocity": 7.099030390231211e-29, + "velocityX": -0.7864812602262983, + "velocityY": 0.041260144415945804, + "timestamp": 2.3567829811810874 }, { - "x": 9.613757714199988, - "y": 7.449017169178514, + "x": 9.6137572333429, + "y": 7.4489961444732815, "heading": -3.141685307179586, - "angularVelocity": -3.217502623717656e-29, - "velocityX": -0.6291956663591478, - "velocityY": 0.032604455089033264, - "timestamp": 2.3691484786176957 + "angularVelocity": -2.7212817911682713e-28, + "velocityX": -0.6291712462629009, + "velocityY": 0.03327294235078537, + "timestamp": 2.3766587149403344 }, { - "x": 9.60437875010338, - "y": 7.4495065320260005, + "x": 9.604378496555762, + "y": 7.449495815722204, "heading": -3.141685307179586, - "angularVelocity": -3.058650941614844e-29, - "velocityX": -0.47188808357125706, - "velocityY": 0.02462153537345558, - "timestamp": 2.389023877940408 + "angularVelocity": 3.8071659238390546e-28, + "velocityX": -0.47186870687348514, + "velocityY": 0.025139763642160577, + "timestamp": 2.3965344486995814 }, { - "x": 9.59812621582273, - "y": 7.44983485868733, + "x": 9.598126127110815, + "y": 7.449831222141457, "heading": -3.141685307179586, - "angularVelocity": 1.512533584247321e-28, - "velocityX": -0.31458659919880055, - "velocityY": 0.016519248544322844, - "timestamp": 2.40889927726312 + "angularVelocity": -1.0181149112849948e-28, + "velocityX": -0.31457301253283737, + "velocityY": 0.016875171669945208, + "timestamp": 2.4164101824588284 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.7605650970541834e-28, - "velocityX": -0.15729071763386918, - "velocityY": 0.00830882992530101, - "timestamp": 2.428774676585832 + "angularVelocity": -2.710350913224058e-28, + "velocityX": -0.15728360767367736, + "velocityY": 0.008491654224556767, + "timestamp": 2.4362859162180754 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.1524356647417298e-29, - "velocityX": 5.1753202830495987e-20, - "velocityY": -1.8305849582326549e-19, - "timestamp": 2.448650075908544 + "angularVelocity": -2.3460450184878346e-27, + "velocityX": 2.2121450356253165e-19, + "velocityY": -1.3122030803241336e-17, + "timestamp": 2.4561616499773224 + }, + { + "x": 9.518209324474356, + "y": 7.45, + "heading": -3.141685307179586, + "angularVelocity": 1.919174990104461e-28, + "velocityX": -0.7801003830985155, + "velocityY": -5.128926779471519e-16, + "timestamp": 2.5545985655197003 }, { - "x": 9.515249994687553, + "x": 9.364627976334331, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.7318379706310427e-39, - "velocityX": -0.7949898942995212, - "velocityY": -6.391097195939509e-16, - "timestamp": 2.5489658231706116 + "angularVelocity": 1.1076124666691254e-36, + "velocityX": -1.5602007366220767, + "velocityY": -1.0126641040061392e-15, + "timestamp": 2.653035481062078 }, { - "x": 9.355749987014017, + "x": 9.134255961402465, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.552617178039786e-38, - "velocityX": -1.5899797591783358, - "velocityY": -1.2780363487427944e-15, - "timestamp": 2.649281570432679 + "angularVelocity": -2.2303140690094456e-37, + "velocityX": -2.3403010309957435, + "velocityY": -1.5124370881453142e-15, + "timestamp": 2.751472396604456 }, { - "x": 9.116499982882113, + "x": 8.827093297146337, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.6158839795897883e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9169629139981945e-15, - "timestamp": 2.7495973176947466 + "angularVelocity": 1.3462495199303324e-32, + "velocityX": -3.120401147919862, + "velocityY": -2.0122147466163613e-15, + "timestamp": 2.849909312146834 }, { - "x": 8.7975, + "x": 8.467906702853663, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.1000527102694011e-38, - "velocityX": -3.179959194728896, - "velocityY": -2.5558892876024355e-15, - "timestamp": 2.849913064956814 + "angularVelocity": 2.849468284749269e-32, + "velocityX": -3.648901352847056, + "velocityY": -2.3373733239441694e-15, + "timestamp": 2.948346227689212 }, { - "x": 8.478500017117888, + "x": 8.160744038597535, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": 5.118648427773132e-37, - "velocityX": -3.179959194728896, - "velocityY": -2.555705369200667e-15, - "timestamp": 2.9502288122188816 + "angularVelocity": -4.1951574405831105e-32, + "velocityX": -3.120401147919862, + "velocityY": -1.9990570963133027e-15, + "timestamp": 3.0467831432315897 }, { - "x": 8.239250012985984, + "x": 7.930372023665668, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.6789246386101953e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9167794084287804e-15, - "timestamp": 3.050544559480949 + "angularVelocity": 1.3042757896301787e-36, + "velocityX": -2.3403010309957435, + "velocityY": -1.4992965357471283e-15, + "timestamp": 3.1452200587739676 }, { - "x": 8.079750005312448, + "x": 7.776790675525645, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -5.8112581235806796e-39, - "velocityX": -1.5899797591783358, - "velocityY": -1.2778530495291014e-15, - "timestamp": 3.1508603067430165 + "angularVelocity": -2.9690915854327103e-38, + "velocityX": -1.5602007366220767, + "velocityY": -9.995321001705852e-16, + "timestamp": 3.2436569743163455 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.3509031450352823e-38, - "velocityX": -0.7949898942995212, - "velocityY": -6.389265579367251e-16, - "timestamp": 3.251176054005084 + "angularVelocity": -1.2572582518763259e-39, + "velocityX": -0.7801003830985155, + "velocityY": -4.997663730275874e-16, + "timestamp": 3.3420938898587234 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -6.25650180816182e-40, - "velocityX": -1.8255808163460085e-27, - "velocityY": -6.990702072612187e-30, - "timestamp": 3.3514918012671515 + "angularVelocity": 7.618384863796292e-41, + "velocityX": 3.506318460129851e-26, + "velocityY": -3.8726720926841052e-28, + "timestamp": 3.4405308054011012 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/R_PickUpFarPiece1.traj b/src/main/deploy/choreo/R_PickUpFarPiece1.traj index ca53ede..b12a07e 100644 --- a/src/main/deploy/choreo/R_PickUpFarPiece1.traj +++ b/src/main/deploy/choreo/R_PickUpFarPiece1.traj @@ -4,739 +4,757 @@ "x": 14.65, "y": 7.9, "heading": -1.571, - "angularVelocity": -4.2261388604687027e-29, - "velocityX": -1.358742071286637e-27, - "velocityY": -4.102470365728161e-28, + "angularVelocity": 5.121565466053403e-29, + "velocityX": -7.318157607438789e-28, + "velocityY": -7.303102655433029e-29, "timestamp": 0 }, { - "x": 14.61609999625719, - "y": 7.893333332597285, + "x": 14.612130307338697, + "y": 7.8982505033556425, "heading": -1.571, - "angularVelocity": 5.482485458181477e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.06602728549153251 + "angularVelocity": 2.4796809944092742e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.06916416332818838 }, { - "x": 14.548299990175126, - "y": 7.879999998067872, + "x": 14.53639092351289, + "y": 7.894751510136077, "heading": -1.571, - "angularVelocity": 5.482485455324769e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.13205457098306503 + "angularVelocity": 2.4796809946674783e-18, + "velocityX": -1.0950668696217758, + "velocityY": -0.05058968476151164, + "timestamp": 0.13832832665637676 }, { - "x": 14.446599984093062, - "y": 7.859999996871792, - "heading": -1.571, - "angularVelocity": 5.4824854577856795e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.19808185647459753 - }, - { - "x": 14.31099998268951, - "y": 7.833333329929108, - "heading": -1.571, - "angularVelocity": 5.482485457103134e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.26410914196613006 - }, - { - "x": 14.141499999999999, - "y": 7.800000000000001, - "heading": -1.571, - "angularVelocity": 5.482485456843769e-19, - "velocityX": -2.567120265927734, - "velocityY": -0.5048417435452771, - "timestamp": 0.3301364274576626 - }, - { - "x": 13.97200001731049, - "y": 7.766666670070894, - "heading": -1.571, - "angularVelocity": 5.482485456705774e-19, - "velocityX": -2.5671202659277337, - "velocityY": -0.5048417435452771, - "timestamp": 0.3961637129491951 - }, - { - "x": 13.836400015906937, - "y": 7.740000003128209, - "heading": -1.571, - "angularVelocity": 5.48248545665747e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.46219099844072764 - }, - { - "x": 13.734700009824873, - "y": 7.720000001932129, - "heading": -1.571, - "angularVelocity": 5.482485455640794e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.5282182839322601 - }, - { - "x": 13.666900003742809, - "y": 7.7066666674027156, - "heading": -1.571, - "angularVelocity": 5.482485458874598e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.5942455694237926 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 5.482485458591381e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.6602728549153251 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 1.8274951520571554e-19, - "velocityX": 7.923170915113678e-19, - "velocityY": -2.3856267514882045e-18, - "timestamp": 0.7263001404068575 - }, - { - "x": 13.61789791542167, - "y": 7.699041687293577, - "heading": -1.571, - "angularVelocity": 3.6283789505269005e-19, - "velocityX": -0.3456036812103074, - "velocityY": -0.021930508822978256, - "timestamp": 0.7699978363499157 - }, - { - "x": 13.587693746684298, - "y": 7.697125061907338, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378947932204e-19, - "velocityX": -0.6912073528254145, - "velocityY": -0.04386101703708676, - "timestamp": 0.8136955322929739 - }, - { - "x": 13.542387494311997, - "y": 7.694250123874538, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378952839068e-19, - "velocityX": -1.036811012446521, - "velocityY": -0.06579152449010808, - "timestamp": 0.857393228236032 - }, - { - "x": 13.48197915897862, - "y": 7.69041687323794, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378951779324e-19, - "velocityX": -1.3824146566467697, - "velocityY": -0.08772203096458873, - "timestamp": 0.9010909241790902 - }, - { - "x": 13.406468741582641, - "y": 7.685625310054556, + "x": 14.422781851017241, + "y": 7.889503020456549, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789513575714e-19, - "velocityX": -1.7280182802858743, - "velocityY": -0.10965253613434846, - "timestamp": 0.9447886201221484 + "angularVelocity": 2.479680994653865e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.20749248998456515 }, { - "x": 13.315856243381926, - "y": 7.679875434404205, + "x": 14.271303094841082, + "y": 7.882505034547556, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789489243236e-19, - "velocityX": -2.073621875139378, - "velocityY": -0.13158303947749891, - "timestamp": 0.9884863160652065 + "angularVelocity": 2.479680994579454e-18, + "velocityX": -2.190133573327318, + "velocityY": -0.10117936185805763, + "timestamp": 0.2766566533127535 }, { - "x": 13.210141666263272, - "y": 7.673167246406615, + "x": 14.081954669952392, + "y": 7.873757553100586, "heading": -1.5709999999999997, - "angularVelocity": 3.628378951941211e-19, - "velocityX": -2.419225426814483, - "velocityY": -0.15351354008073578, - "timestamp": 1.0321840120082648 + "angularVelocity": 2.479680994607856e-18, + "velocityX": -2.7376666726989773, + "velocityY": -0.12647418874225766, + "timestamp": 0.3458208166409419 }, { - "x": 13.089325013371338, - "y": 7.6655007462613325, + "x": 13.892606245063702, + "y": 7.865010071653616, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789509770827e-19, - "velocityX": -2.7648289065256026, - "velocityY": -0.17544403611745055, - "timestamp": 1.075881707951323 + "angularVelocity": 2.4796809946116294e-18, + "velocityX": -2.737666672698978, + "velocityY": -0.12647418874225766, + "timestamp": 0.41498497996913025 }, { - "x": 12.953406290995442, - "y": 7.656875934367449, + "x": 13.741127488887543, + "y": 7.858012085744623, "heading": -1.5709999999999997, - "angularVelocity": 3.628378950782348e-19, - "velocityX": -3.110432242308825, - "velocityY": -0.19737452302112604, - "timestamp": 1.1195794038943812 + "angularVelocity": 2.479680994536425e-18, + "velocityX": -2.1901335733273184, + "velocityY": -0.10117936185805763, + "timestamp": 0.4841491432973186 }, { - "x": 12.802385518003497, - "y": 7.647292811922241, + "x": 13.627518416391894, + "y": 7.852763596065095, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789514392664e-19, - "velocityX": -3.4560351463092576, - "velocityY": -0.21930498252574065, - "timestamp": 1.1632770998374393 - }, + "angularVelocity": 2.47968099468633e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.553313306625507 + }, + { + "x": 13.551779032566087, + "y": 7.849264602845529, + "heading": -1.5709999999999995, + "angularVelocity": 2.479680994669315e-18, + "velocityX": -1.095066869621776, + "velocityY": -0.05058968476151165, + "timestamp": 0.6224774699536955 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 2.4796809942925844e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.6916416332818839 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 8.26560334321477e-19, + "velocityX": 2.2636957131725456e-19, + "velocityY": 2.8360545446294056e-18, + "timestamp": 0.7608057966100723 + }, + { + "x": 13.499332557357375, + "y": 7.8449192409498565, + "heading": -1.5709999999999995, + "angularVelocity": 1.549669581859894e-18, + "velocityX": -0.33723861810938877, + "velocityY": -0.060056189169625046, + "timestamp": 0.8040297388416239 + }, + { + "x": 13.47017899267198, + "y": 7.839727510520138, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695813124185e-18, + "velocityX": -0.6744772267466429, + "velocityY": -0.12011237665243168, + "timestamp": 0.8472536810731754 + }, + { + "x": 13.42644864636038, + "y": 7.831939915003153, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695815123512e-18, + "velocityX": -1.0117158235437291, + "velocityY": -0.1801685620267154, + "timestamp": 0.890477623304727 + }, + { + "x": 13.368141519080574, + "y": 7.821556454516082, + "heading": -1.5709999999999993, + "angularVelocity": 1.549669581692892e-18, + "velocityX": -1.348954405117744, + "velocityY": -0.2402247446900413, + "timestamp": 0.9337015655362786 + }, + { + "x": 13.295257611709896, + "y": 7.808577129215161, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814706194e-18, + "velocityX": -1.6861929663943322, + "velocityY": -0.3002809237387572, + "timestamp": 0.9769255077678302 + }, + { + "x": 13.207796925476618, + "y": 7.793001939319122, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814452525e-18, + "velocityX": -2.0234314992545284, + "velocityY": -0.3603370977270199, + "timestamp": 1.0201494499993817 + }, + { + "x": 13.105759462223137, + "y": 7.774830885156067, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815528492e-18, + "velocityX": -2.3606699894901486, + "velocityY": -0.42039326412460515, + "timestamp": 1.0633733922309334 + }, + { + "x": 12.989145225020126, + "y": 7.754063967272826, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815122657e-18, + "velocityX": -2.6979084086848464, + "velocityY": -0.48044941787106793, + "timestamp": 1.106597334462485 + }, + { + "x": 12.857954220008912, + "y": 7.730701186763059, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814976101e-18, + "velocityX": -3.035146685797867, + "velocityY": -0.5405055463153154, + "timestamp": 1.1498212766940368 + }, + { + "x": 12.71218646561342, + "y": 7.70474254690774, + "heading": -1.570999999999999, + "angularVelocity": 1.5496695815191512e-18, + "velocityX": -3.372384536667451, + "velocityY": -0.6005615988532015, + "timestamp": 1.1930452189255885 + }, + { + "x": 12.556909538173402, + "y": 7.677090492761939, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692345146303e-18, + "velocityX": -3.592382356245949, + "velocityY": -0.6397392907307838, + "timestamp": 1.2362691611571401 + }, + { + "x": 12.401632610733195, + "y": 7.649438438616105, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692344266034e-18, + "velocityX": -3.592382356250238, + "velocityY": -0.6397392907315478, + "timestamp": 1.2794931033886918 + }, + { + "x": 12.24635568329299, + "y": 7.621786384470271, + "heading": -1.570999999999999, + "angularVelocity": 1.549669234478791e-18, + "velocityX": -3.5923823562502384, + "velocityY": -0.6397392907315479, + "timestamp": 1.3227170456202435 + }, + { + "x": 12.091078758239746, + "y": 7.594134330749512, + "heading": -1.570999999999999, + "angularVelocity": 3.234878008246914e-18, + "velocityX": -3.592382301027084, + "velocityY": -0.6397392808972879, + "timestamp": 1.3659409878517952 + }, + { + "x": 12.00283728057898, + "y": 7.582107515529106, + "heading": -1.5788342162362206, + "angularVelocity": -0.3119507041975876, + "velocityX": -3.513687938342297, + "velocityY": -0.47889582877488296, + "timestamp": 1.3910546232141834 + }, + { + "x": 11.916970328359971, + "y": 7.573753042701851, + "heading": -1.5952402865312374, + "angularVelocity": -0.6532734133600175, + "velocityX": -3.4191366952627433, + "velocityY": -0.3326668045754269, + "timestamp": 1.4161682585765716 + }, + { + "x": 11.833816357634106, + "y": 7.56871710218962, + "heading": -1.6206538515119442, + "angularVelocity": -1.011942899307139, + "velocityX": -3.311108468604305, + "velocityY": -0.20052614603931174, + "timestamp": 1.4412818939389598 + }, + { + "x": 11.753731808376116, + "y": 7.566574800515445, + "heading": -1.6553839157644212, + "angularVelocity": -1.3829166407559692, + "velocityX": -3.188887156413404, + "velocityY": -0.08530432345956382, + "timestamp": 1.466395529301348 + }, + { + "x": 11.677147361800145, + "y": 7.566714891670578, + "heading": -1.6997475676814937, + "angularVelocity": -1.766516526856978, + "velocityX": -3.0495165463240133, + "velocityY": 0.0055782905616100845, + "timestamp": 1.4915091646637362 + }, + { + "x": 11.604396744023525, + "y": 7.568557677988233, + "heading": -1.7537219634693322, + "angularVelocity": -2.1492067957909815, + "velocityX": -2.8968572939289086, + "velocityY": 0.07337791964526899, + "timestamp": 1.5166228000261244 + }, + { + "x": 11.535756559334365, + "y": 7.571432948149072, + "heading": -1.8171150418628605, + "angularVelocity": -2.5242493760370213, + "velocityX": -2.7331839336949435, + "velocityY": 0.1144904001078914, + "timestamp": 1.5417364353885126 + }, + { + "x": 11.471380887760333, + "y": 7.574513868964298, + "heading": -1.889488559627284, + "angularVelocity": -2.881841546237513, + "velocityX": -2.5633752598977027, + "velocityY": 0.12267920477200389, + "timestamp": 1.5668500707509008 + }, + { + "x": 11.411255276796373, + "y": 7.576880729956244, + "heading": -1.9701854617324397, + "angularVelocity": -3.213270438176076, + "velocityX": -2.3941420704876353, + "velocityY": 0.09424605230448276, + "timestamp": 1.591963706113289 + }, + { + "x": 11.35519068850538, + "y": 7.57762868801966, + "heading": -2.058445803348398, + "angularVelocity": -3.5144390822869056, + "velocityX": -2.2324361838505236, + "velocityY": 0.029782946698429406, + "timestamp": 1.6170773414756772 + }, + { + "x": 11.302817578230115, + "y": 7.575911070751928, + "heading": -2.1534608466432936, + "angularVelocity": -3.783404589727689, + "velocityX": -2.0854451981786317, + "velocityY": -0.06839381248247857, + "timestamp": 1.6421909768380654 + }, + { + "x": 11.253523245308234, + "y": 7.570911967645187, + "heading": -2.254232970772999, + "angularVelocity": -4.012645826678452, + "velocityX": -1.9628513439257314, + "velocityY": -0.19905931716328515, + "timestamp": 1.6673046122004536 + }, + { + "x": 11.206151241323777, + "y": 7.561834135279753, + "heading": -2.3588178409636855, + "angularVelocity": -4.164465585385836, + "velocityX": -1.8863061162162815, + "velocityY": -0.36147026244552627, + "timestamp": 1.6924182475628418 + }, + { + "x": 11.158562568853249, + "y": 7.548296021124272, + "heading": -2.4631530925128593, + "angularVelocity": -4.154526019174503, + "velocityX": -1.8949336399871903, + "velocityY": -0.5390742503060728, + "timestamp": 1.71753188292523 + }, + { + "x": 11.108338354901361, + "y": 7.5311193171207975, + "heading": -2.5624670465584027, + "angularVelocity": -3.9545829431864727, + "velocityX": -1.9998782823423, + "velocityY": -0.6839592817038007, + "timestamp": 1.7426455182876182 + }, + { + "x": 11.054321656083031, + "y": 7.51174890982464, + "heading": -2.654640214483575, + "angularVelocity": -3.6702439370135087, + "velocityX": -2.150891260419857, + "velocityY": -0.7713103665237963, + "timestamp": 1.7677591536500064 + }, + { + "x": 10.996033122413428, + "y": 7.491461551309535, + "heading": -2.738672595307348, + "angularVelocity": -3.346085885665274, + "velocityX": -2.3209914784730197, + "velocityY": -0.8078224527158445, + "timestamp": 1.7928727890123946 + }, + { + "x": 10.933381820015002, + "y": 7.471624968268411, + "heading": -2.8141850565003126, + "angularVelocity": -3.006831153809885, + "velocityX": -2.4947125931527254, + "velocityY": -0.7898730213622603, + "timestamp": 1.8179864243747828 + }, + { + "x": 10.866838632540613, + "y": 7.45402686067354, + "heading": -2.88213732233945, + "angularVelocity": -2.7057916888040063, + "velocityX": -2.6496835887816075, + "velocityY": -0.7007391538860639, + "timestamp": 1.843100059737171 + }, + { + "x": 10.797385329302331, + "y": 7.4401695505140575, + "heading": -2.944658830810706, + "angularVelocity": -2.4895443279744143, + "velocityX": -2.765561506172815, + "velocityY": -0.5517843179424147, + "timestamp": 1.8682136950995591 + }, + { + "x": 10.724987539198043, + "y": 7.429906192310285, + "heading": -3.000767776942746, + "angularVelocity": -2.234202468993167, + "velocityX": -2.88280804668823, + "velocityY": -0.40867672305021235, + "timestamp": 1.8933273304619473 + }, + { + "x": 10.649597795782494, + "y": 7.422945580592199, + "heading": -3.0495257230757873, + "angularVelocity": -1.9414929551011741, + "velocityX": -3.0019446538772403, + "velocityY": -0.277164640548885, + "timestamp": 1.9184409658243355 + }, + { + "x": 10.571198566507455, + "y": 7.418912040075103, + "heading": -3.0901902225982423, + "angularVelocity": -1.6192199550436448, + "velocityX": -3.121779389711535, + "velocityY": -0.16061157450510036, + "timestamp": 1.9435546011867237 + }, { - "x": 12.643257011655002, - "y": 7.637195207926121, - "heading": -1.5709999999999997, - "angularVelocity": 3.6283776976177666e-19, - "velocityX": -3.641576584629373, - "velocityY": -0.23107863648642077, - "timestamp": 1.2069747957804975 - }, - { - "x": 12.484128505306387, - "y": 7.627097603929993, - "heading": -1.5709999999999997, - "angularVelocity": 3.628377701929186e-19, - "velocityX": -3.641576584632129, - "velocityY": -0.23107863648659568, - "timestamp": 1.2506724917235557 - }, - { - "x": 12.325, - "y": 7.617, - "heading": -1.5709999999999997, - "angularVelocity": 7.36418634228573e-19, - "velocityX": -3.6415765607812913, - "velocityY": -0.23107863497312509, - "timestamp": 1.2943701876666138 - }, - { - "x": 12.229388489581337, - "y": 7.6109112417716736, - "heading": -1.5863134997517452, - "angularVelocity": -0.5552849027741624, - "velocityX": -3.466982017671504, - "velocityY": -0.22078529243098394, - "timestamp": 1.3219479224028547 - }, - { - "x": 12.138013718648649, - "y": 7.607167866658879, - "heading": -1.6148419963350573, - "angularVelocity": -1.0344757049904476, - "velocityX": -3.313353029413532, - "velocityY": -0.13573903544277124, - "timestamp": 1.3495256571390957 - }, - { - "x": 12.050416525228757, - "y": 7.6063106068200685, - "heading": -1.6545433874529916, - "angularVelocity": -1.4396175573391428, - "velocityX": -3.1763737760802657, - "velocityY": -0.03108521591825106, - "timestamp": 1.3771033918753366 - }, - { - "x": 11.966360780867383, - "y": 7.607509934453374, - "heading": -1.7036222302241029, - "angularVelocity": -1.7796546105222553, - "velocityX": -3.0479568088277835, - "velocityY": 0.04348898286151133, - "timestamp": 1.4046811266115775 - }, - { - "x": 11.885408562298633, - "y": 7.609633528647565, - "heading": -1.7598323716256097, - "angularVelocity": -2.038243602642159, - "velocityX": -2.9354194368390085, - "velocityY": 0.07700393866649435, - "timestamp": 1.4322588613478184 - }, - { - "x": 11.807516118182633, - "y": 7.611964238202936, - "heading": -1.8223777210675893, - "angularVelocity": -2.2679654453193687, - "velocityX": -2.8244685381514696, - "velocityY": 0.08451417702220176, - "timestamp": 1.4598365960840594 - }, - { - "x": 11.73334817309539, - "y": 7.614170969229463, - "heading": -1.8925164168284103, - "angularVelocity": -2.5433088116786786, - "velocityX": -2.6894139709660934, - "velocityY": 0.08001857468104064, - "timestamp": 1.4874143308203003 - }, - { - "x": 11.663695028307295, - "y": 7.615711030101031, - "heading": -1.9717573083827227, - "angularVelocity": -2.873364774597518, - "velocityX": -2.5257021816429535, - "velocityY": 0.05584435727941134, - "timestamp": 1.5149920655565412 - }, - { - "x": 11.598996797656051, - "y": 7.61568305023367, - "heading": -2.0607742017770874, - "angularVelocity": -3.2278537104564915, - "velocityX": -2.3460313644334034, - "velocityY": -0.0010145817859481632, - "timestamp": 1.5425698002927821 - }, - { - "x": 11.539114067010884, - "y": 7.613037677777285, - "heading": -2.1591584444474705, - "angularVelocity": -3.5675244399638015, - "velocityX": -2.171415862031463, - "velocityY": -0.09592421138591448, - "timestamp": 1.570147535029023 - }, - { - "x": 11.483508926014887, - "y": 7.606776074097583, - "heading": -2.266083388472652, - "angularVelocity": -3.8772199764711472, - "velocityX": -2.0163056004352273, - "velocityY": -0.22705286491397708, - "timestamp": 1.597725269765264 - }, - { - "x": 11.431087870109415, - "y": 7.595775934313344, - "heading": -2.379919574340892, - "angularVelocity": -4.127829459416678, - "velocityX": -1.900847056759369, - "velocityY": -0.3988775687867351, - "timestamp": 1.6253030045015049 - }, - { - "x": 11.377404486650294, - "y": 7.579105655561354, - "heading": -2.4911780043079133, - "angularVelocity": -4.034357101158641, - "velocityX": -1.9466204883235783, - "velocityY": -0.6044832511236621, - "timestamp": 1.6528807392377458 - }, - { - "x": 11.319370240403567, - "y": 7.55896629351811, - "heading": -2.5937282135555715, - "angularVelocity": -3.718587122128468, - "velocityX": -2.1043877171847964, - "velocityY": -0.7302761534208634, - "timestamp": 1.6804584739739867 - }, - { - "x": 11.256272025403964, - "y": 7.536866509262458, - "heading": -2.6862489609900964, - "angularVelocity": -3.3549074396216714, - "velocityX": -2.288012978697651, - "velocityY": -0.8013632906045023, - "timestamp": 1.7080362087102277 - }, - { - "x": 11.187885977180416, - "y": 7.514090907716094, - "heading": -2.7680211777317423, - "angularVelocity": -2.965153502408188, - "velocityX": -2.4797558203240193, - "velocityY": -0.8258691935430503, - "timestamp": 1.7356139434464686 - }, - { - "x": 11.114289943184776, - "y": 7.491935954019794, - "heading": -2.838743341274155, - "angularVelocity": -2.5644660164736934, - "velocityX": -2.66867582488287, - "velocityY": -0.8033637972151884, - "timestamp": 1.7631916781827095 - }, - { - "x": 11.03615204188367, - "y": 7.47266971293565, - "heading": -2.8998457718181054, - "angularVelocity": -2.215643566389652, - "velocityX": -2.8333690946131496, - "velocityY": -0.6986157952569366, - "timestamp": 1.7907694129189504 - }, - { - "x": 10.95383190394823, - "y": 7.456821193599831, - "heading": -2.9513961589840805, - "angularVelocity": -1.8692756188648758, - "velocityX": -2.985021747535358, - "velocityY": -0.5746853208720242, - "timestamp": 1.8183471476551913 - }, - { - "x": 10.867220309314249, - "y": 7.443764913233828, - "heading": -2.991973638967047, - "angularVelocity": -1.4713855351450116, - "velocityX": -3.1406348440997536, - "velocityY": -0.47343556281455274, - "timestamp": 1.8459248823914323 - }, - { - "x": 10.77690563773812, - "y": 7.432954012684261, - "heading": -3.0224568962493583, - "angularVelocity": -1.1053575492644694, - "velocityX": -3.2749126220813625, - "velocityY": -0.39201553909209025, - "timestamp": 1.8735026171276732 - }, - { - "x": 10.68603576950712, - "y": 7.424800287046038, - "heading": -3.0521033841408234, - "angularVelocity": -1.0750153402739042, - "velocityX": -3.295044683694906, - "velocityY": -0.2956633572774275, - "timestamp": 1.901080351863914 - }, - { - "x": 10.596200893845815, - "y": 7.419761050330992, - "heading": -3.0855587154803814, - "angularVelocity": -1.2131283319507546, - "velocityX": -3.257514676985064, - "velocityY": -0.18272844971648752, - "timestamp": 1.928658086600155 + "x": 10.489548200758582, + "y": 7.417268311990692, + "heading": -3.1214676257157588, + "angularVelocity": -1.2454351059179014, + "velocityX": -3.251236412835276, + "velocityY": -0.06545161864044534, + "timestamp": 1.968668236549112 }, { - "x": 10.506861440016637, - "y": 7.417879567543994, - "heading": -3.1208586855613984, - "angularVelocity": -1.2800170289051036, - "velocityX": -3.2395501183704285, - "velocityY": -0.0682247039140415, - "timestamp": 1.956235821336396 - }, - { - "x": 10.412835700689799, - "y": 7.416682265603839, - "heading": -3.141685307179586, - "angularVelocity": -0.7551969665883731, - "velocityX": -3.4094801558619205, - "velocityY": -0.043415528926000375, - "timestamp": 1.9838135560726369 + "x": 10.404255697585379, + "y": 7.417275691573601, + "heading": -3.141685307179586, + "angularVelocity": -0.8050479817886684, + "velocityX": -3.3962627051968566, + "velocityY": 0.00029384765695126695, + "timestamp": 1.9937818719115001 }, { "x": 10.317412376403809, "y": 7.418716907501221, "heading": -3.141685307179586, - "angularVelocity": 5.937778880078681e-19, - "velocityX": -3.4601581746520087, - "velocityY": 0.07377842730165238, - "timestamp": 2.011391290808878 + "angularVelocity": 7.47770773326549e-18, + "velocityX": -3.4580147369515903, + "velocityY": 0.05738778583033072, + "timestamp": 2.0188955072738883 }, { - "x": 10.251727837257341, - "y": 7.420700742643172, + "x": 10.251732343249431, + "y": 7.420561186854179, "heading": -3.141685307179586, - "angularVelocity": -1.9442515737792743e-28, - "velocityX": -3.3048160733761773, - "velocityY": 0.09981359920071599, - "timestamp": 2.03126669013159 + "angularVelocity": 3.1949765996869766e-27, + "velocityX": -3.304533757090645, + "velocityY": 0.09279050400334278, + "timestamp": 2.0387712410331353 }, { - "x": 10.189167330602293, - "y": 7.422886644704941, + "x": 10.189171391771723, + "y": 7.422674521924776, "heading": -3.141685307179586, - "angularVelocity": -2.386183922112865e-28, - "velocityX": -3.1476352066827475, - "velocityY": 0.10998028398210202, - "timestamp": 2.051142089454302 + "angularVelocity": -2.371251182895612e-27, + "velocityX": -3.1476046235828683, + "velocityY": 0.10632739883689808, + "timestamp": 2.0586469747923823 }, { - "x": 10.129736436099503, - "y": 7.425149795146946, + "x": 10.129739097787168, + "y": 7.424900096756528, "heading": -3.141685307179586, - "angularVelocity": 2.7299904854133836e-28, - "velocityX": -2.9901736079776406, - "velocityY": 0.11386691684825133, - "timestamp": 2.0710174887770143 + "angularVelocity": -5.101900895484119e-28, + "velocityX": -2.99019370577468, + "velocityY": 0.11197447393435742, + "timestamp": 2.0785227085516293 }, { - "x": 10.073436091249755, - "y": 7.427423630307786, + "x": 10.073437312690587, + "y": 7.427157718473777, "heading": -3.141685307179586, - "angularVelocity": 1.8386974301791735e-28, - "velocityX": -2.8326648403694086, - "velocityY": 0.1144045019634283, - "timestamp": 2.0908928880997264 + "angularVelocity": -5.203506078458522e-28, + "velocityX": -2.8326896394648293, + "velocityY": 0.11358683631985049, + "timestamp": 2.0983984423108764 }, { - "x": 10.020266165929796, - "y": 7.4296668280040885, + "x": 10.020266158378618, + "y": 7.429398776074464, "heading": -3.141685307179586, - "angularVelocity": 5.650458450043071e-29, - "velocityX": -2.6751626197114717, - "velocityY": 0.11286302528469754, - "timestamp": 2.1107682874224385 + "angularVelocity": 2.645370534116333e-28, + "velocityX": -2.6751794402172218, + "velocityY": 0.11275345241754915, + "timestamp": 2.1182741760701234 }, { - "x": 9.970226259362496, - "y": 7.431851251828943, + "x": 9.970225293830397, + "y": 7.431590675011163, "heading": -3.141685307179586, - "angularVelocity": -4.102009655023591e-29, - "velocityX": -2.5176805635356243, - "velocityY": 0.10990590877633212, - "timestamp": 2.1306436867451506 + "angularVelocity": 5.191709905827532e-29, + "velocityX": -2.5176863986186695, + "velocityY": 0.11028015182983106, + "timestamp": 2.1381499098293704 }, { - "x": 9.923315923027515, - "y": 7.43395651278906, + "x": 9.92331426564673, + "y": 7.433710048067309, "heading": -3.141685307179586, - "angularVelocity": -3.5765307146759774e-29, - "velocityX": -2.3602210739674327, - "velocityY": 0.10592295158126644, - "timestamp": 2.1505190860678627 + "angularVelocity": 1.0273003820442257e-28, + "velocityX": -2.3602161687157612, + "velocityY": 0.1066311856365788, + "timestamp": 2.1580256435886174 }, { - "x": 9.879534728461786, - "y": 7.4359671584530975, + "x": 9.87953261796101, + "y": 7.435739325604849, "heading": -3.141685307179586, - "angularVelocity": -1.1942964906968253e-29, - "velocityX": -2.2027831418560893, - "velocityY": 0.1011625291844662, - "timestamp": 2.170394485390575 + "angularVelocity": -2.0170615068521438e-28, + "velocityX": -2.202768874651166, + "velocityY": 0.10209824513253805, + "timestamp": 2.1779013773478644 }, { - "x": 9.838882286352929, - "y": 7.437871074521549, + "x": 9.838879927126666, + "y": 7.437664817248215, "heading": -3.141685307179586, - "angularVelocity": -5.641467124729511e-31, - "velocityX": -2.0453647973957065, - "velocityY": 0.09579259453048798, - "timestamp": 2.190269884713287 + "angularVelocity": -2.3492440709815457e-28, + "velocityX": -2.045342895349926, + "velocityY": 0.0968765061300322, + "timestamp": 2.1977771111071114 }, { - "x": 9.801358249188596, - "y": 7.439658508993957, + "x": 9.801355810492355, + "y": 7.439475555427537, "heading": -3.141685307179586, - "angularVelocity": 3.5641593543820497e-29, - "velocityX": -1.8879639374819575, - "velocityY": 0.08993200304483205, - "timestamp": 2.210145284035999 + "angularVelocity": 2.8629022838202107e-28, + "velocityX": -1.8879361682359468, + "velocityY": 0.09110296008466975, + "timestamp": 2.2176528448663584 }, { - "x": 9.766962308401824, - "y": 7.441321442796545, + "x": 9.766959925956705, + "y": 7.441162556870241, "heading": -3.141685307179586, - "angularVelocity": 1.9252934621446341e-29, - "velocityX": -1.7305786026380017, - "velocityY": 0.08366794425548009, - "timestamp": 2.230020683358711 + "angularVelocity": -1.5840875871720601e-28, + "velocityX": -1.7305466531340934, + "velocityY": 0.08487744216837698, + "timestamp": 2.2375285786256054 }, { - "x": 9.735694189965349, - "y": 7.442853165687241, + "x": 9.735691968423001, + "y": 7.442718328937222, "heading": -3.141685307179586, - "angularVelocity": -5.183683118127715e-29, - "velocityX": -1.5732070550525459, - "velocityY": 0.07706627000674852, - "timestamp": 2.249896082681423 + "angularVelocity": 9.330544204917246e-29, + "velocityX": -1.5731724882437799, + "velocityY": 0.0782749500383743, + "timestamp": 2.2574043123848524 }, { - "x": 9.707553649885064, - "y": 7.444247980075085, + "x": 9.707551665488573, + "y": 7.444136527128733, "heading": -3.141685307179586, - "angularVelocity": 1.3886630469011578e-29, - "velocityX": -1.4158477836532484, - "velocityY": 0.07017793027434112, - "timestamp": 2.2697714820041353 + "angularVelocity": -6.58238928802494e-29, + "velocityX": -1.4158120286419644, + "velocityY": 0.07135324958012863, + "timestamp": 2.2772800461440994 }, { - "x": 9.682540470094276, - "y": 7.445500987918728, + "x": 9.682538773252615, + "y": 7.445411710024687, "heading": -3.141685307179586, - "angularVelocity": -4.3165247768849716e-30, - "velocityX": -1.258499483942791, - "velocityY": 0.06304315316127049, - "timestamp": 2.2896468813268473 + "angularVelocity": 2.6630221770954893e-29, + "velocityX": -1.2584638403259507, + "velocityY": 0.06415777708639624, + "timestamp": 2.2971557799033464 }, { - "x": 9.660654454881797, - "y": 7.446607933488268, + "x": 9.660653072546591, + "y": 7.446539159318976, "heading": -3.141685307179586, - "angularVelocity": 5.491476754830618e-28, - "velocityX": -1.10116103113811, - "velocityY": 0.05569425557531322, - "timestamp": 2.3095222806495594 + "angularVelocity": -4.1528582058952054e-29, + "velocityX": -1.1011266789504681, + "velocityY": 0.056724914307379, + "timestamp": 2.3170315136625934 }, { - "x": 9.641895427846347, - "y": 7.4475650848138315, + "x": 9.64189436565749, + "y": 7.447514744684453, "heading": -3.141685307179586, - "angularVelocity": -5.338468664029318e-28, - "velocityX": -0.9438314536912185, - "velocityY": 0.04815758969279047, - "timestamp": 2.3293976799722715 + "angularVelocity": 4.9624701627257074e-29, + "velocityX": -0.9437994650322394, + "velocityY": 0.04908424399792036, + "timestamp": 2.3369072474218404 }, { - "x": 9.626263229320996, - "y": 7.44836914261392, + "x": 9.626262473522596, + "y": 7.448334820329731, "heading": -3.141685307179586, - "angularVelocity": 1.3824274242904182e-29, - "velocityX": -0.7865099096393957, - "velocityY": 0.04045492556066521, - "timestamp": 2.3492730792949836 + "angularVelocity": 7.099030390231211e-29, + "velocityX": -0.7864812602262983, + "velocityY": 0.041260144415945804, + "timestamp": 2.3567829811810874 }, { - "x": 9.613757714199988, - "y": 7.449017169178514, + "x": 9.6137572333429, + "y": 7.4489961444732815, "heading": -3.141685307179586, - "angularVelocity": -3.217502623717656e-29, - "velocityX": -0.6291956663591478, - "velocityY": 0.032604455089033264, - "timestamp": 2.3691484786176957 + "angularVelocity": -2.7212817911682713e-28, + "velocityX": -0.6291712462629009, + "velocityY": 0.03327294235078537, + "timestamp": 2.3766587149403344 }, { - "x": 9.60437875010338, - "y": 7.4495065320260005, + "x": 9.604378496555762, + "y": 7.449495815722204, "heading": -3.141685307179586, - "angularVelocity": -3.058650941614844e-29, - "velocityX": -0.47188808357125706, - "velocityY": 0.02462153537345558, - "timestamp": 2.389023877940408 + "angularVelocity": 3.8071659238390546e-28, + "velocityX": -0.47186870687348514, + "velocityY": 0.025139763642160577, + "timestamp": 2.3965344486995814 }, { - "x": 9.59812621582273, - "y": 7.44983485868733, + "x": 9.598126127110815, + "y": 7.449831222141457, "heading": -3.141685307179586, - "angularVelocity": 1.512533584247321e-28, - "velocityX": -0.31458659919880055, - "velocityY": 0.016519248544322844, - "timestamp": 2.40889927726312 + "angularVelocity": -1.0181149112849948e-28, + "velocityX": -0.31457301253283737, + "velocityY": 0.016875171669945208, + "timestamp": 2.4164101824588284 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.7605650970541834e-28, - "velocityX": -0.15729071763386918, - "velocityY": 0.00830882992530101, - "timestamp": 2.428774676585832 + "angularVelocity": -2.710350913224058e-28, + "velocityX": -0.15728360767367736, + "velocityY": 0.008491654224556767, + "timestamp": 2.4362859162180754 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.1524356647417298e-29, - "velocityX": 5.1753202830495987e-20, - "velocityY": -1.8305849582326549e-19, - "timestamp": 2.448650075908544 + "angularVelocity": -2.3460450184878346e-27, + "velocityX": 2.2121450356253165e-19, + "velocityY": -1.3122030803241336e-17, + "timestamp": 2.4561616499773224 + }, + { + "x": 9.518209324474356, + "y": 7.45, + "heading": -3.141685307179586, + "angularVelocity": 1.919174990104461e-28, + "velocityX": -0.7801003830985155, + "velocityY": -5.128926779471519e-16, + "timestamp": 2.5545985655197003 }, { - "x": 9.515249994687553, + "x": 9.364627976334331, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.7318379706310427e-39, - "velocityX": -0.7949898942995212, - "velocityY": -6.391097195939509e-16, - "timestamp": 2.5489658231706116 + "angularVelocity": 1.1076124666691254e-36, + "velocityX": -1.5602007366220767, + "velocityY": -1.0126641040061392e-15, + "timestamp": 2.653035481062078 }, { - "x": 9.355749987014017, + "x": 9.134255961402465, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.552617178039786e-38, - "velocityX": -1.5899797591783358, - "velocityY": -1.2780363487427944e-15, - "timestamp": 2.649281570432679 + "angularVelocity": -2.2303140690094456e-37, + "velocityX": -2.3403010309957435, + "velocityY": -1.5124370881453142e-15, + "timestamp": 2.751472396604456 }, { - "x": 9.116499982882113, + "x": 8.827093297146337, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.6158839795897883e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9169629139981945e-15, - "timestamp": 2.7495973176947466 + "angularVelocity": 1.3462495199303324e-32, + "velocityX": -3.120401147919862, + "velocityY": -2.0122147466163613e-15, + "timestamp": 2.849909312146834 }, { - "x": 8.7975, + "x": 8.467906702853663, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.1000527102694011e-38, - "velocityX": -3.179959194728896, - "velocityY": -2.5558892876024355e-15, - "timestamp": 2.849913064956814 + "angularVelocity": 2.849468284749269e-32, + "velocityX": -3.648901352847056, + "velocityY": -2.3373733239441694e-15, + "timestamp": 2.948346227689212 }, { - "x": 8.478500017117888, + "x": 8.160744038597535, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": 5.118648427773132e-37, - "velocityX": -3.179959194728896, - "velocityY": -2.555705369200667e-15, - "timestamp": 2.9502288122188816 + "angularVelocity": -4.1951574405831105e-32, + "velocityX": -3.120401147919862, + "velocityY": -1.9990570963133027e-15, + "timestamp": 3.0467831432315897 }, { - "x": 8.239250012985984, + "x": 7.930372023665668, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.6789246386101953e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9167794084287804e-15, - "timestamp": 3.050544559480949 + "angularVelocity": 1.3042757896301787e-36, + "velocityX": -2.3403010309957435, + "velocityY": -1.4992965357471283e-15, + "timestamp": 3.1452200587739676 }, { - "x": 8.079750005312448, + "x": 7.776790675525645, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -5.8112581235806796e-39, - "velocityX": -1.5899797591783358, - "velocityY": -1.2778530495291014e-15, - "timestamp": 3.1508603067430165 + "angularVelocity": -2.9690915854327103e-38, + "velocityX": -1.5602007366220767, + "velocityY": -9.995321001705852e-16, + "timestamp": 3.2436569743163455 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.3509031450352823e-38, - "velocityX": -0.7949898942995212, - "velocityY": -6.389265579367251e-16, - "timestamp": 3.251176054005084 + "angularVelocity": -1.2572582518763259e-39, + "velocityX": -0.7801003830985155, + "velocityY": -4.997663730275874e-16, + "timestamp": 3.3420938898587234 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -6.25650180816182e-40, - "velocityX": -1.8255808163460085e-27, - "velocityY": -6.990702072612187e-30, - "timestamp": 3.3514918012671515 + "angularVelocity": 7.618384863796292e-41, + "velocityX": 3.506318460129851e-26, + "velocityY": -3.8726720926841052e-28, + "timestamp": 3.4405308054011012 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/R_ReturnFromFarwithStop.1.traj b/src/main/deploy/choreo/R_ReturnFromFarwithStop.1.traj new file mode 100644 index 0000000..c8d7159 --- /dev/null +++ b/src/main/deploy/choreo/R_ReturnFromFarwithStop.1.traj @@ -0,0 +1,310 @@ +{ + "samples": [ + { + "x": 7.7, + "y": 7.45, + "heading": 3.1415, + "angularVelocity": 0, + "velocityX": 1.3345047844501093e-28, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 7.720983776232949, + "y": 7.452031789914187, + "heading": 3.1415, + "angularVelocity": -2.8559927040215486e-16, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589325, + "timestamp": 0.051577320630114186 + }, + { + "x": 7.762951328065995, + "y": 7.456095369681284, + "heading": 3.1415, + "angularVelocity": -6.035032687716479e-16, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368897, + "timestamp": 0.10315464126022837 + }, + { + "x": 7.825902654655366, + "y": 7.462190739219591, + "heading": 3.1415, + "angularVelocity": -9.133792065608597e-16, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741078, + "timestamp": 0.15473196189034255 + }, + { + "x": 7.909837754819842, + "y": 7.470317898414736, + "heading": 3.1415, + "angularVelocity": -1.285554414608073e-15, + "velocityX": 1.6273644915795704, + "velocityY": 0.15757234179354235, + "timestamp": 0.20630928252045674 + }, + { + "x": 8.01475662678774, + "y": 7.48047684709517, + "heading": 3.1414999999999997, + "angularVelocity": -1.8501098820899896e-15, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355356, + "timestamp": 0.25788660315057094 + }, + { + "x": 8.140659267606683, + "y": 7.492667584975027, + "heading": 3.1414999999999997, + "angularVelocity": -2.568015817611512e-15, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908125, + "timestamp": 0.3094639237806851 + }, + { + "x": 8.2875456713738, + "y": 7.506890111482749, + "heading": 3.1414999999999997, + "angularVelocity": -3.6664166549374125e-15, + "velocityX": 2.8478874429965697, + "velocityY": 0.27575155773604454, + "timestamp": 0.36104124441079927 + }, + { + "x": 8.455415820402175, + "y": 7.523144424905771, + "heading": 3.1414999999999993, + "angularVelocity": -5.781579183621917e-15, + "velocityX": 3.254727988477392, + "velocityY": 0.315144587275328, + "timestamp": 0.41261856504091343 + }, + { + "x": 8.642740284809822, + "y": 7.541282435587867, + "heading": 3.1414999999999993, + "angularVelocity": 2.8772388403772396e-16, + "velocityX": 3.6319153868252347, + "velocityY": 0.3516664002804812, + "timestamp": 0.4641958856710276 + }, + { + "x": 8.830064749214753, + "y": 7.5594204462697006, + "heading": 3.1414999999999993, + "angularVelocity": 4.478421854687767e-16, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 0.5157732063011418 + }, + { + "x": 9.017389213619715, + "y": 7.577558456951537, + "heading": 3.1414999999999993, + "angularVelocity": -2.6797870831169e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 0.567350526931256 + }, + { + "x": 9.20471367802468, + "y": 7.595696467633373, + "heading": 3.1414999999999993, + "angularVelocity": -1.5978294903773436e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6189278475613702 + }, + { + "x": 9.392038142429643, + "y": 7.613834478315209, + "heading": 3.1414999999999993, + "angularVelocity": 3.6921602148337384e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6705051681914844 + }, + { + "x": 9.579362606834605, + "y": 7.631972488997046, + "heading": 3.1414999999999993, + "angularVelocity": 2.453996709110038e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7220824888215986 + }, + { + "x": 9.766687071239568, + "y": 7.650110499678882, + "heading": 3.1414999999999993, + "angularVelocity": 2.90499526121897e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7736598094517129 + }, + { + "x": 9.954011535644533, + "y": 7.668248510360718, + "heading": 3.1414999999999993, + "angularVelocity": 4.2648077713252974e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8252371300818271 + }, + { + "x": 10.141336000049495, + "y": 7.686386521042555, + "heading": 3.1414999999999993, + "angularVelocity": 1.9645772050389573e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8768144507119413 + }, + { + "x": 10.328660464454458, + "y": 7.704524531724391, + "heading": 3.1414999999999993, + "angularVelocity": 8.516171089710268e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9283917713420555 + }, + { + "x": 10.515984928859421, + "y": 7.722662542406227, + "heading": 3.1414999999999993, + "angularVelocity": -3.343515828741549e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9799690919721697 + }, + { + "x": 10.703309393264385, + "y": 7.740800553088063, + "heading": 3.1414999999999993, + "angularVelocity": -4.869114069203101e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.031546412602284 + }, + { + "x": 10.890633857669348, + "y": 7.758938563769899, + "heading": 3.1414999999999993, + "angularVelocity": -1.948527081472739e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 1.0831237332323982 + }, + { + "x": 11.077958322074311, + "y": 7.777076574451735, + "heading": 3.1414999999999993, + "angularVelocity": -1.203013192737632e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.1347010538625124 + }, + { + "x": 11.265282786479242, + "y": 7.795214585133569, + "heading": 3.1414999999999993, + "angularVelocity": 1.3156045718444795e-17, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 1.1862783744926266 + }, + { + "x": 11.452607250886889, + "y": 7.813352595815664, + "heading": 3.1414999999999993, + "angularVelocity": -3.1679641053244916e-16, + "velocityX": 3.6319153868252356, + "velocityY": 0.3516664002804744, + "timestamp": 1.2378556951227409 + }, + { + "x": 11.620477399915263, + "y": 7.829606909238687, + "heading": 3.1414999999999993, + "angularVelocity": -3.349318744028512e-17, + "velocityX": 3.254727988477392, + "velocityY": 0.3151445872753285, + "timestamp": 1.289433015752855 + }, + { + "x": 11.76736380368238, + "y": 7.843829435746409, + "heading": 3.1414999999999993, + "angularVelocity": -3.0900120798738865e-17, + "velocityX": 2.8478874429965697, + "velocityY": 0.2757515577360448, + "timestamp": 1.3410103363829693 + }, + { + "x": 11.893266444501323, + "y": 7.856020173626265, + "heading": 3.1414999999999993, + "angularVelocity": -1.4649487274207734e-17, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908127, + "timestamp": 1.3925876570130835 + }, + { + "x": 11.998185316469222, + "y": 7.8661791223067, + "heading": 3.1414999999999993, + "angularVelocity": -3.815569156988782e-18, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355373, + "timestamp": 1.4441649776431977 + }, + { + "x": 12.082120416633698, + "y": 7.874306281501845, + "heading": 3.1414999999999993, + "angularVelocity": -1.6271982814457388e-18, + "velocityX": 1.6273644915795704, + "velocityY": 0.1575723417935425, + "timestamp": 1.495742298273312 + }, + { + "x": 12.145071743223069, + "y": 7.880401651040152, + "heading": 3.1414999999999993, + "angularVelocity": -1.1137179997081401e-18, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741088, + "timestamp": 1.5473196189034262 + }, + { + "x": 12.187039295056113, + "y": 7.8844652308072485, + "heading": 3.1414999999999993, + "angularVelocity": -6.858817655923581e-19, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368902, + "timestamp": 1.5988969395335404 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": -3.1997477749711506e-19, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589327, + "timestamp": 1.6504742601636546 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": 0, + "velocityX": -1.1615407482795014e-30, + "velocityY": 0, + "timestamp": 1.7020515807937688 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/R_ReturnFromFarwithStop.traj b/src/main/deploy/choreo/R_ReturnFromFarwithStop.traj new file mode 100644 index 0000000..c8d7159 --- /dev/null +++ b/src/main/deploy/choreo/R_ReturnFromFarwithStop.traj @@ -0,0 +1,310 @@ +{ + "samples": [ + { + "x": 7.7, + "y": 7.45, + "heading": 3.1415, + "angularVelocity": 0, + "velocityX": 1.3345047844501093e-28, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 7.720983776232949, + "y": 7.452031789914187, + "heading": 3.1415, + "angularVelocity": -2.8559927040215486e-16, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589325, + "timestamp": 0.051577320630114186 + }, + { + "x": 7.762951328065995, + "y": 7.456095369681284, + "heading": 3.1415, + "angularVelocity": -6.035032687716479e-16, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368897, + "timestamp": 0.10315464126022837 + }, + { + "x": 7.825902654655366, + "y": 7.462190739219591, + "heading": 3.1415, + "angularVelocity": -9.133792065608597e-16, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741078, + "timestamp": 0.15473196189034255 + }, + { + "x": 7.909837754819842, + "y": 7.470317898414736, + "heading": 3.1415, + "angularVelocity": -1.285554414608073e-15, + "velocityX": 1.6273644915795704, + "velocityY": 0.15757234179354235, + "timestamp": 0.20630928252045674 + }, + { + "x": 8.01475662678774, + "y": 7.48047684709517, + "heading": 3.1414999999999997, + "angularVelocity": -1.8501098820899896e-15, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355356, + "timestamp": 0.25788660315057094 + }, + { + "x": 8.140659267606683, + "y": 7.492667584975027, + "heading": 3.1414999999999997, + "angularVelocity": -2.568015817611512e-15, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908125, + "timestamp": 0.3094639237806851 + }, + { + "x": 8.2875456713738, + "y": 7.506890111482749, + "heading": 3.1414999999999997, + "angularVelocity": -3.6664166549374125e-15, + "velocityX": 2.8478874429965697, + "velocityY": 0.27575155773604454, + "timestamp": 0.36104124441079927 + }, + { + "x": 8.455415820402175, + "y": 7.523144424905771, + "heading": 3.1414999999999993, + "angularVelocity": -5.781579183621917e-15, + "velocityX": 3.254727988477392, + "velocityY": 0.315144587275328, + "timestamp": 0.41261856504091343 + }, + { + "x": 8.642740284809822, + "y": 7.541282435587867, + "heading": 3.1414999999999993, + "angularVelocity": 2.8772388403772396e-16, + "velocityX": 3.6319153868252347, + "velocityY": 0.3516664002804812, + "timestamp": 0.4641958856710276 + }, + { + "x": 8.830064749214753, + "y": 7.5594204462697006, + "heading": 3.1414999999999993, + "angularVelocity": 4.478421854687767e-16, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 0.5157732063011418 + }, + { + "x": 9.017389213619715, + "y": 7.577558456951537, + "heading": 3.1414999999999993, + "angularVelocity": -2.6797870831169e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 0.567350526931256 + }, + { + "x": 9.20471367802468, + "y": 7.595696467633373, + "heading": 3.1414999999999993, + "angularVelocity": -1.5978294903773436e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6189278475613702 + }, + { + "x": 9.392038142429643, + "y": 7.613834478315209, + "heading": 3.1414999999999993, + "angularVelocity": 3.6921602148337384e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6705051681914844 + }, + { + "x": 9.579362606834605, + "y": 7.631972488997046, + "heading": 3.1414999999999993, + "angularVelocity": 2.453996709110038e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7220824888215986 + }, + { + "x": 9.766687071239568, + "y": 7.650110499678882, + "heading": 3.1414999999999993, + "angularVelocity": 2.90499526121897e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7736598094517129 + }, + { + "x": 9.954011535644533, + "y": 7.668248510360718, + "heading": 3.1414999999999993, + "angularVelocity": 4.2648077713252974e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8252371300818271 + }, + { + "x": 10.141336000049495, + "y": 7.686386521042555, + "heading": 3.1414999999999993, + "angularVelocity": 1.9645772050389573e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8768144507119413 + }, + { + "x": 10.328660464454458, + "y": 7.704524531724391, + "heading": 3.1414999999999993, + "angularVelocity": 8.516171089710268e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9283917713420555 + }, + { + "x": 10.515984928859421, + "y": 7.722662542406227, + "heading": 3.1414999999999993, + "angularVelocity": -3.343515828741549e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9799690919721697 + }, + { + "x": 10.703309393264385, + "y": 7.740800553088063, + "heading": 3.1414999999999993, + "angularVelocity": -4.869114069203101e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.031546412602284 + }, + { + "x": 10.890633857669348, + "y": 7.758938563769899, + "heading": 3.1414999999999993, + "angularVelocity": -1.948527081472739e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 1.0831237332323982 + }, + { + "x": 11.077958322074311, + "y": 7.777076574451735, + "heading": 3.1414999999999993, + "angularVelocity": -1.203013192737632e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.1347010538625124 + }, + { + "x": 11.265282786479242, + "y": 7.795214585133569, + "heading": 3.1414999999999993, + "angularVelocity": 1.3156045718444795e-17, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 1.1862783744926266 + }, + { + "x": 11.452607250886889, + "y": 7.813352595815664, + "heading": 3.1414999999999993, + "angularVelocity": -3.1679641053244916e-16, + "velocityX": 3.6319153868252356, + "velocityY": 0.3516664002804744, + "timestamp": 1.2378556951227409 + }, + { + "x": 11.620477399915263, + "y": 7.829606909238687, + "heading": 3.1414999999999993, + "angularVelocity": -3.349318744028512e-17, + "velocityX": 3.254727988477392, + "velocityY": 0.3151445872753285, + "timestamp": 1.289433015752855 + }, + { + "x": 11.76736380368238, + "y": 7.843829435746409, + "heading": 3.1414999999999993, + "angularVelocity": -3.0900120798738865e-17, + "velocityX": 2.8478874429965697, + "velocityY": 0.2757515577360448, + "timestamp": 1.3410103363829693 + }, + { + "x": 11.893266444501323, + "y": 7.856020173626265, + "heading": 3.1414999999999993, + "angularVelocity": -1.4649487274207734e-17, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908127, + "timestamp": 1.3925876570130835 + }, + { + "x": 11.998185316469222, + "y": 7.8661791223067, + "heading": 3.1414999999999993, + "angularVelocity": -3.815569156988782e-18, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355373, + "timestamp": 1.4441649776431977 + }, + { + "x": 12.082120416633698, + "y": 7.874306281501845, + "heading": 3.1414999999999993, + "angularVelocity": -1.6271982814457388e-18, + "velocityX": 1.6273644915795704, + "velocityY": 0.1575723417935425, + "timestamp": 1.495742298273312 + }, + { + "x": 12.145071743223069, + "y": 7.880401651040152, + "heading": 3.1414999999999993, + "angularVelocity": -1.1137179997081401e-18, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741088, + "timestamp": 1.5473196189034262 + }, + { + "x": 12.187039295056113, + "y": 7.8844652308072485, + "heading": 3.1414999999999993, + "angularVelocity": -6.858817655923581e-19, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368902, + "timestamp": 1.5988969395335404 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": -3.1997477749711506e-19, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589327, + "timestamp": 1.6504742601636546 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": 0, + "velocityX": -1.1615407482795014e-30, + "velocityY": 0, + "timestamp": 1.7020515807937688 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto b/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto index c42d52d..924c501 100644 --- a/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto +++ b/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto @@ -60,6 +60,30 @@ } ] } + }, + { + "type": "path", + "data": { + "pathName": "B_ReturnFromFarPiece1" + } + }, + { + "type": "named", + "data": { + "name": "raiseArmAndWait" + } + }, + { + "type": "named", + "data": { + "name": "outtakeAndWait" + } + }, + { + "type": "named", + "data": { + "name": "resetArmAndIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Blue5.auto b/src/main/deploy/pathplanner/autos/Blue5.auto new file mode 100644 index 0000000..d553a64 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue5.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.449083685874939, + "y": 7.3158488273620605 + }, + "rotation": -90.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B_OnePiece" + } + }, + { + "type": "named", + "data": { + "name": "raiseArmAndWait" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "outtakeAndWait" + } + }, + { + "type": "named", + "data": { + "name": "resetArmAndIntake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B_PickUpFarPiece1" + } + }, + { + "type": "named", + "data": { + "name": "intakePiece" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "B_ReturnFromFarwithStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red5.auto b/src/main/deploy/pathplanner/autos/Red5.auto new file mode 100644 index 0000000..fd8eba3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red5.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 15.091, + "y": 7.3158488273620605 + }, + "rotation": -90.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R_OnePiece " + } + }, + { + "type": "named", + "data": { + "name": "raiseArmAndWait" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "outtakeAndWait" + } + }, + { + "type": "named", + "data": { + "name": "resetArmAndIntake" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R_PickUpFarPiece1" + } + }, + { + "type": "named", + "data": { + "name": "intakePiece" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "R_ReturnFromFarwithStop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b4cc4be..7717341 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -213,6 +213,9 @@ public static enum IntakeState { 0.26; // originally 0.29 // also with 0.23 public static final double kPositionTolerance = 0.01; + public static final double kAutoFirstRepositionDistance = 0.15; + public static final double kAutoSecondRepositionDistance = 0.28; + public static final double kRollerDiameter = 0.0508; // 2 inches public static final double kGearRatio = 10.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ed16be..85318a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; @@ -203,8 +202,8 @@ private void updateSelectedAutonomous() { case 5: m_autonomous = m_swerve.redAllianceSupplier().getAsBoolean() - ? null - : new Autonomous("TestFlip"); + ? new Autonomous("Red5") + : new Autonomous("Blue5"); break; default: @@ -243,7 +242,7 @@ public double getPDHCurrent(int CANBusPort) { // spotless:off private void createNamedCommands() { NamedCommands.registerCommand("raiseArmAndWait", - m_arm.createDeployCommand() + m_arm.createDeployCommand().alongWith(m_intake.createStopIntakeCommand()) .andThen(new WaitCommand(1.8))); NamedCommands.registerCommand("resetArmAndIntake", @@ -255,8 +254,12 @@ private void createNamedCommands() { .withTimeout(0.7)); NamedCommands.registerCommand("intakePiece", - createAutoIntakeCommandSequence() - ); + m_arm.createStowCommand() + .andThen(m_intake.createIntakeCommand()).until(m_intake.hasNote) + .andThen(m_arm.createCarryCommand() + .alongWith(m_intake.createAdvanceAfterIntakingCommand(IntakeConstants.kAutoFirstRepositionDistance, IntakeConstants.kAutoSecondRepositionDistance))) + .andThen(m_intake.createStopIntakeCommand()) + ); NamedCommands.registerCommand("stopIntake", m_intake.createStopIntakeCommand()); @@ -284,16 +287,18 @@ private void configureDriverButtonBindings() { new JoystickButton(m_driver,OIConstants.kZorroAIn) .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver))); - Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn); // Reverse intake to outake or reject intaking Note D_Button.and(armDeployed.negate()) - .whileTrue(m_arm.createStowCommand().andThen(new WaitCommand(0.2).andThen(m_intake.createOuttakeToFloorCommand()))); - // Shoot Note into Amp + .whileTrue(m_arm.createStowCommand() + .andThen(new WaitCommand(0.3) + .andThen(m_intake.createOuttakeToFloorCommand()))); + + // Shoot Note into Amp D_Button.and(armDeployed) - .whileTrue(m_intake.createOuttakeToAmpCommand()); + .whileTrue(m_intake.createOuttakeToAmpCommand()); } // spotless:on @@ -303,7 +308,6 @@ private void configureOperatorButtonBindings() { JoystickButton leftBumper = new JoystickButton(m_operator, Button.kLeftBumper.value); JoystickButton leftJoystickDown = new JoystickButton(m_operator, Button.kLeftStick.value); - Trigger hasNote = new Trigger(m_intake.eitherSensorSupplier()); Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); // CLIMBER @@ -329,18 +333,21 @@ private void configureOperatorButtonBindings() { // Control position of Note in intake Trigger leftStick = new Trigger(() -> Math.abs(m_operator.getLeftY()) > 0.2); //While arm is down - leftStick.and(armDeployed.negate()).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); + leftStick.and(armDeployed.negate()) + .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); + //While arm is up - leftStick.and(armDeployed).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); + leftStick.and(armDeployed) + .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); // Intake Note from floor - rightBumper.and(hasNote.negate()) + rightBumper.and(m_intake.hasNote.negate()) .whileTrue(m_arm.createStowCommand() .andThen(m_intake.createIntakeCommand())); - hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) + m_intake.hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) .onTrue(m_arm.createCarryCommand() - .andThen(m_intake.createAdvanceAfterIntakingCommand().withInterruptBehavior(InterruptionBehavior.kCancelIncoming))); + .andThen(m_intake.createAdvanceAfterIntakingCommand(IntakeConstants.kFirstRepositionDistance, IntakeConstants.kSecondRepositionDistance).withInterruptBehavior(InterruptionBehavior.kCancelIncoming))); // Reverse intake to outake or reject intaking Note leftBumper.and(armDeployed.negate()) @@ -388,11 +395,4 @@ private void configureOperatorButtonBindings() { } // spotless:on - public Command createAutoIntakeCommandSequence() { - return new SequentialCommandGroup( - m_arm.createStowCommand(), - m_intake.createIntakeCommand().until(m_intake.eitherSensorSupplier()), - m_arm.createCarryCommand(), - m_intake.createAdvanceAfterIntakingCommand()); - } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 4b547a3..5d104e7 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakeConstants.IntakeState; import java.util.function.BooleanSupplier; @@ -61,6 +62,7 @@ public Intake() { m_velocityController.setD(IntakeConstants.kVelocityD); m_positionController.setTolerance(IntakeConstants.kPositionTolerance); + m_positionController.setGoal(17); m_relativeEncoder = m_motor.getEncoder(); m_relativeEncoder.setPosition(0.0); @@ -88,23 +90,30 @@ public Command createStopIntakeCommand() { } private void configurePositionController(double targetPosition) { + SmartDashboard.putNumber("ResetBeam", 1); m_positionController.reset(m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + m_positionController.calculate(m_relativeEncoder.getPosition()); } // spotless:off private void advanceAfterIntaking(double targetPosition) { m_secondSensorTriggered.rising().ifHigh( () -> { + SmartDashboard.putNumber("RealativeEncoderReset", 0); m_positionController.reset( m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); + SmartDashboard.putNumber("RealativeEncoderReset", 1); m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + SmartDashboard.putNumber("RealativeEncoderReset", 2); }); setState(IntakeState.PROCESSING); m_motor.set(m_positionController.calculate(m_relativeEncoder.getPosition())); } // spotless:on + public Trigger hasNote = new Trigger(this.eitherSensorSupplier()); + public Command createIntakeCommand() { return this.run( () -> { @@ -125,16 +134,16 @@ public Command createOuttakeToFloorCommand() { return this.run( () -> { setState(IntakeState.OUTTAKING); - m_motor.set(-1.0); + m_motor.set(-0.7); }); } - public Command createAdvanceAfterIntakingCommand() { + public Command createAdvanceAfterIntakingCommand(double firstDistance, double secondDistance) { return new FunctionalCommand( // initialize - () -> this.configurePositionController(IntakeConstants.kFirstRepositionDistance), + () -> this.configurePositionController(firstDistance), // execute - () -> this.advanceAfterIntaking(IntakeConstants.kSecondRepositionDistance), + () -> this.advanceAfterIntaking(secondDistance), // end interrupted -> {}, // isFinished @@ -160,7 +169,7 @@ private void setVelocity(double targetVelocity) { } public Command createSetVelocityCommand(double targetVelocity) { - return this.startEnd(() -> this.setVelocity(targetVelocity), () -> {}); + return this.runOnce(() -> this.setVelocity(targetVelocity)); } public Command createJoystickControlCommand(XboxController m_controller, double factor) { @@ -182,6 +191,8 @@ public void periodic() { SmartDashboard.putNumber( "IntakeSensorRetroReflective", !m_noteSensorRetroReflective.get() ? 1d : 0d); SmartDashboard.putNumber("IntakeSensorBeamBreak", !m_noteSensorBeamBreak.get() ? 1d : 0d); + SmartDashboard.putNumber("RealativeEncoderReset", -1); + SmartDashboard.putNumber("ResetBeam", -1); if (m_state != null) SmartDashboard.putString("Intake State", m_state.name()); } diff --git a/src/main/omicron.chor b/src/main/omicron.chor index 35b76dd..e5b25d4 100644 --- a/src/main/omicron.chor +++ b/src/main/omicron.chor @@ -17,8 +17,8 @@ "waypoints": [ { "x": 1, - "y": 2.0002646446228027, - "heading": 0.8049567558943416, + "y": 2, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -26,8 +26,8 @@ }, { "x": 3, - "y": 2, - "heading": 0.8049567558943416, + "y": 2.1, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -36,112 +36,112 @@ ], "trajectory": [ { - "x": 0.9999999999999971, - "y": 2.0002646446228027, - "heading": 0.8049567558943416, - "angularVelocity": -8.882383616047711e-33, - "velocityX": 2.570897453430358e-16, - "velocityY": -3.401870775942321e-20, + "x": 1, + "y": 2, + "heading": 0, + "angularVelocity": -2.167255414929253e-35, + "velocityX": 0, + "velocityY": 1.0729237673159813e-36, "timestamp": 0 }, { - "x": 1.0666666719605324, - "y": 2.000255823134675, - "heading": 0.8049567558943416, - "angularVelocity": 3.396420718128266e-27, - "velocityX": 0.7268597690543107, - "velocityY": -0.00009617976470591548, - "timestamp": 0.09171875346361454 - }, - { - "x": 1.2000000138981668, - "y": 2.000238180158683, - "heading": 0.8049567558943416, - "angularVelocity": -1.124978050975361e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655031845, - "timestamp": 0.18343750692722907 - }, - { - "x": 1.4000000225084717, - "y": 2.0002117156952637, - "heading": 0.8049567558943416, - "angularVelocity": -9.05920074487619e-28, - "velocityX": 2.1805792278843166, - "velocityY": -0.0002885392836274193, - "timestamp": 0.2751562603908436 - }, - { - "x": 1.6666666911827144, - "y": 2.000176429745291, - "heading": 0.8049567558943416, - "angularVelocity": 1.6037111849531954e-26, - "velocityX": 2.907438867230956, - "velocityY": -0.0003847190311701196, - "timestamp": 0.36687501385445814 - }, - { - "x": 2.0000000000000013, - "y": 2.0001323223114014, - "heading": 0.8049567558943416, - "angularVelocity": 3.720423796149838e-26, - "velocityX": 3.634298289383412, - "velocityY": -0.00048089874997318956, - "timestamp": 0.45859376731807266 - }, - { - "x": 2.3333333088172883, - "y": 2.0000882148775116, - "heading": 0.8049567558943416, - "angularVelocity": 1.815590327118958e-26, - "velocityX": 3.6342982893834117, - "velocityY": -0.0004808987499730965, - "timestamp": 0.5503125207816872 - }, - { - "x": 2.5999999774915312, - "y": 2.000052928927539, - "heading": 0.8049567558943416, - "angularVelocity": -2.3621475473176443e-26, - "velocityX": 2.9074388672309555, - "velocityY": -0.0003847190311700582, - "timestamp": 0.6420312742453018 - }, - { - "x": 2.799999986101836, - "y": 2.0000264644641197, - "heading": 0.8049567558943416, - "angularVelocity": 5.783084016216911e-28, - "velocityX": 2.180579227884316, - "velocityY": -0.00028853928362737873, - "timestamp": 0.7337500277089164 - }, - { - "x": 2.9333333280394704, - "y": 2.0000088214881275, - "heading": 0.8049567558943416, - "angularVelocity": -2.351348677796648e-27, - "velocityX": 1.453719516483294, - "velocityY": -0.00019235952655029408, - "timestamp": 0.825468781172531 - }, - { - "x": 3.0000000000000058, + "x": 1.066666671960537, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -5.899958075775622e-27, - "velocityX": 0.7268597690543105, - "velocityY": -0.00009617976470590487, - "timestamp": 0.9171875346361456 + "heading": -2.2064159489642693e-18, + "angularVelocity": -2.40562799160236e-17, + "velocityX": 0.726859772235986, + "velocityY": 1.2414751413753207e-30, + "timestamp": 0.09171875306213341 }, { - "x": 3.000000000000003, + "x": 1.2000000138981721, + "y": 2, + "heading": -6.238939336710584e-18, + "angularVelocity": -4.3966098884338405e-17, + "velocityX": 1.453719522846644, + "velocityY": 4.076264826156204e-31, + "timestamp": 0.18343750612426682 + }, + { + "x": 1.4000000225084763, "y": 2, - "heading": 0.8049567558943416, - "angularVelocity": -1.1372522907103163e-26, - "velocityX": -1.0731025459966149e-17, - "velocityY": 1.4199561562482062e-21, - "timestamp": 1.0089062880997601 + "heading": -1.1463424269988706e-17, + "angularVelocity": -5.696189642582716e-17, + "velocityX": 2.1805792374293405, + "velocityY": -4.260497847820802e-31, + "timestamp": 0.27515625918640024 + }, + { + "x": 1.666666691182717, + "y": 2, + "heading": -1.660927541991535e-17, + "angularVelocity": -5.610453586198502e-17, + "velocityX": 2.9074388799576525, + "velocityY": -1.259681057530857e-30, + "timestamp": 0.36687501224853364 + }, + { + "x": 2.0000000000000004, + "y": 2, + "heading": -1.7865129734989053e-17, + "angularVelocity": -1.3692311810090882e-17, + "velocityX": 3.6342983052917806, + "velocityY": -2.0941736191227216e-30, + "timestamp": 0.45859376531066703 + }, + { + "x": 2.333333308817284, + "y": 2, + "heading": -2.614419331441905e-18, + "angularVelocity": 1.6627656030926103e-16, + "velocityX": 3.6342983052917806, + "velocityY": -7.668721594358735e-31, + "timestamp": 0.5503125183728005 + }, + { + "x": 2.599999977491524, + "y": 2, + "heading": 1.3547937651055734e-18, + "angularVelocity": 4.327579132505987e-17, + "velocityX": 2.907438879957653, + "velocityY": 6.696205895893734e-32, + "timestamp": 0.6420312714349339 + }, + { + "x": 2.799999986101828, + "y": 2, + "heading": 1.6633017363201664e-18, + "angularVelocity": 3.3635853370126346e-18, + "velocityX": 2.1805792374293405, + "velocityY": 9.022977438111278e-31, + "timestamp": 0.7337500244970673 + }, + { + "x": 2.933333328039463, + "y": 2, + "heading": 8.002561852421958e-19, + "angularVelocity": -9.409705950343789e-18, + "velocityX": 1.453719522846644, + "velocityY": 1.4913843713845408e-30, + "timestamp": 0.8254687775592007 + }, + { + "x": 3, + "y": 2, + "heading": 0, + "angularVelocity": -8.725108122724779e-18, + "velocityX": 0.726859772235986, + "velocityY": 4.284468199881695e-31, + "timestamp": 0.9171875306213341 + }, + { + "x": 3, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": -1.88079096131566e-37, + "velocityY": 0, + "timestamp": 1.0089062836834675 } ], "constraints": [ @@ -156,13 +156,6 @@ "last" ], "type": "StopPoint" - }, - { - "scope": [ - 0, - 1 - ], - "type": "ZeroAngularVelocity" } ], "usesControlIntervalGuessing": true, @@ -824,7 +817,7 @@ "controlIntervalCount": 14 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, "isInitialGuess": false, @@ -838,460 +831,460 @@ "x": 1.77, "y": 7.9, "heading": -1.571, - "angularVelocity": -5.001059619972217e-29, - "velocityX": 1.788147943312785e-31, - "velocityY": 2.1020984301178844e-18, + "angularVelocity": 2.18585903371176e-28, + "velocityX": 1.6675991903204138e-31, + "velocityY": 2.1032812804683124e-18, "timestamp": 0 }, { "x": 1.77, - "y": 7.896666665318824, + "y": 7.8966666653183735, "heading": -1.571, - "angularVelocity": 7.683916356601382e-22, - "velocityX": 9.998046435594066e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.02050894424315847 + "angularVelocity": 8.067708733019786e-22, + "velocityX": 9.994992923130725e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.020508944244543796 }, { "x": 1.77, - "y": 7.889999996400785, + "y": 7.8899999963994345, "heading": -1.571, - "angularVelocity": 7.683917142451062e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.04101788848631694 + "angularVelocity": 8.067710793880657e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.04101788848908759 }, { "x": 1.77, - "y": 7.8799999939859795, + "y": 7.879999993983276, "heading": -1.571, - "angularVelocity": 7.683923566300495e-22, - "velocityX": 2.990166401978325e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.061526832729475414 + "angularVelocity": 8.067707982153918e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.06152683273363139 }, { "x": 1.77, - "y": 7.8666666595980566, + "y": 7.866666659593829, "heading": -1.571, - "angularVelocity": 7.683920333199294e-22, - "velocityX": 3.9853467158164286e-17, - "velocityY": -0.6501229039311227, - "timestamp": 0.08203577697263388 + "angularVelocity": 8.067715580788947e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.08203577697817518 }, { "x": 1.77, "y": 7.85, "heading": -1.571, - "angularVelocity": 7.6839203283387345e-22, - "velocityX": 4.9805250105364423e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.10254472121579235 + "angularVelocity": 8.067712787171335e-22, + "velocityX": 4.980219657624676e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.10254472122271899 }, { "x": 1.77, - "y": 7.833333340401944, + "y": 7.833333340406171, "heading": -1.571, - "angularVelocity": 7.683920326683815e-22, - "velocityX": 4.980525010536431e-17, - "velocityY": -0.8126532209776981, - "timestamp": 0.12305366545895081 + "angularVelocity": 8.067712790353108e-22, + "velocityX": 4.980219657624708e-17, + "velocityY": -0.8126532207166868, + "timestamp": 0.12305366546726279 }, { "x": 1.77, - "y": 7.820000006014021, + "y": 7.820000006016724, "heading": -1.571, - "angularVelocity": 7.683920326387786e-22, - "velocityX": 3.985346715816429e-17, - "velocityY": -0.6501229039311228, - "timestamp": 0.1435626097021093 + "angularVelocity": 8.067712793311768e-22, + "velocityX": 3.985041364688782e-17, + "velocityY": -0.6501229039614865, + "timestamp": 0.1435626097118066 }, { "x": 1.77, - "y": 7.810000003599215, + "y": 7.810000003600566, "heading": -1.571, - "angularVelocity": 7.683920326387407e-22, - "velocityX": 2.9901664019783255e-17, - "velocityY": -0.48759225712664783, - "timestamp": 0.16407155394526776 + "angularVelocity": 8.067712794323227e-22, + "velocityX": 2.989861050867049e-17, + "velocityY": -0.4875922571596851, + "timestamp": 0.1640715539563504 }, { "x": 1.77, - "y": 7.803333334681176, + "y": 7.803333334681627, "heading": -1.571, - "angularVelocity": 7.683923930888569e-22, - "velocityX": 1.9949856332479876e-17, - "velocityY": -0.32506153603017973, - "timestamp": 0.18458049818842623 + "angularVelocity": 8.067712783004652e-22, + "velocityX": 1.9946802820688036e-17, + "velocityY": -0.3250615360521264, + "timestamp": 0.1845804982008942 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 7.68391635572018e-22, - "velocityX": 9.998046435593288e-18, - "velocityY": -0.16253077884730605, - "timestamp": 0.2050894424315847 + "angularVelocity": 8.067716774548183e-22, + "velocityX": 9.994992923129957e-18, + "velocityY": -0.16253077885828557, + "timestamp": 0.205089442445438 }, { "x": 1.77, "y": 7.8, "heading": -1.571, - "angularVelocity": 3.841958700142675e-22, - "velocityX": 1.516102151151558e-20, - "velocityY": 1.2612627111587971e-18, - "timestamp": 0.22559838667474316 + "angularVelocity": 4.033854514049993e-22, + "velocityX": 1.437728787092847e-20, + "velocityY": 1.2619724294779088e-18, + "timestamp": 0.2255983866899818 }, { - "x": 1.8468750067060988, + "x": 1.8468750067060982, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583235197619974e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997518092183e-19, - "timestamp": 0.32408933900510295 + "angularVelocity": 4.780221469151799e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036155566e-19, + "timestamp": 0.3240893390203413 }, { - "x": 2.0006250165417403, + "x": 2.0006250165417385, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.5832292867617415e-25, - "velocityX": 1.5610571955881865, - "velocityY": 1.0102997518139742e-19, - "timestamp": 0.4225802913354627 + "angularVelocity": 4.779931148188201e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036211387e-19, + "timestamp": 0.42258029135070085 }, { - "x": 2.2312500211616264, + "x": 2.2312500211616237, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583091752273785e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518132804e-19, - "timestamp": 0.5210712436658225 + "angularVelocity": 4.779411037477553e-25, + "velocityX": 2.341585690493883, + "velocityY": 1.0108689036215326e-19, + "timestamp": 0.5210712436810604 }, { - "x": 2.538749978838374, + "x": 2.5387499788383763, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583544986591083e-25, - "velocityX": 3.1221137617324173, - "velocityY": 1.0102997518134437e-19, - "timestamp": 0.6195621959961822 + "angularVelocity": 4.781363462107374e-25, + "velocityX": 3.122113761732479, + "velocityY": 1.0108689036222194e-19, + "timestamp": 0.6195621960114199 }, { - "x": 2.76937498345826, + "x": 2.7693749834582615, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583037423957827e-25, - "velocityX": 2.3415856904938894, - "velocityY": 1.0102997518136517e-19, - "timestamp": 0.718053148326542 + "angularVelocity": 4.779378758545355e-25, + "velocityX": 2.3415856904938837, + "velocityY": 1.0108689036239423e-19, + "timestamp": 0.7180531483417795 }, { - "x": 2.9231249932939014, + "x": 2.923124993293902, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583270603340756e-25, - "velocityX": 1.5610571955881862, - "velocityY": 1.0102997518134589e-19, - "timestamp": 0.8165441006569018 + "angularVelocity": 4.779884871772051e-25, + "velocityX": 1.5610571955881822, + "velocityY": 1.0108689036199126e-19, + "timestamp": 0.816544100672139 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 4.583237967720886e-25, - "velocityX": 0.7805286159508688, - "velocityY": 1.0102997530405439e-19, - "timestamp": 0.9150350529872615 + "angularVelocity": 4.779655463247018e-25, + "velocityX": 0.7805286159508666, + "velocityY": 1.0108689036198308e-19, + "timestamp": 0.9150350530024985 }, { "x": 3, "y": 7.8, "heading": -1.571, - "angularVelocity": 1.5277643827009332e-25, - "velocityX": 8.205072252280436e-23, - "velocityY": 2.539441495294102e-20, - "timestamp": 1.0135260053176214 + "angularVelocity": 1.5936410800223104e-25, + "velocityX": -1.507822169205296e-22, + "velocityY": 2.5440102474480186e-20, + "timestamp": 1.013526005332858 }, { "x": 3, "y": 7.668749997164577, "heading": -1.571, - "angularVelocity": 6.966538206440015e-26, - "velocityX": 6.122589528955434e-17, + "angularVelocity": 7.336223698049225e-26, + "velocityX": 6.122564906084829e-17, "velocityY": -0.9999999859073642, - "timestamp": 1.1447760100027031 + "timestamp": 1.1447760100179396 }, { "x": 3, "y": 7.537499994329149, "heading": -1.571, - "angularVelocity": 6.965517753645813e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.337555266120999e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.2760260146877849 + "timestamp": 1.2760260147030211 }, { "x": 3, - "y": 7.406249991493721, + "y": 7.406249991493722, "heading": -1.571, - "angularVelocity": 6.965810499047246e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336209273675485e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.4072760193728666 + "timestamp": 1.4072760193881027 }, { "x": 3, - "y": 7.274999988658292, + "y": 7.274999988658293, "heading": -1.571, - "angularVelocity": 6.965243755612967e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335860790281625e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.5385260240579484 + "timestamp": 1.5385260240731842 }, { "x": 3, - "y": 7.143749985822865, + "y": 7.143749985822866, "heading": -1.571, - "angularVelocity": 6.966418165061469e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.335353332117909e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.66977602874303 + "timestamp": 1.6697760287582657 }, { "x": 3, - "y": 7.012499982987436, + "y": 7.012499982987438, "heading": -1.571, - "angularVelocity": 6.965888956818575e-26, - "velocityX": 6.122589528955664e-17, + "angularVelocity": 7.336320963838537e-26, + "velocityX": 6.122564906085055e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.8010260334281118 + "timestamp": 1.8010260334433472 }, { "x": 3, - "y": 6.881249980152008, + "y": 6.881249980152011, "heading": -1.571, - "angularVelocity": 6.966018121357878e-26, - "velocityX": 6.122589528955833e-17, + "angularVelocity": 7.337274968697998e-26, + "velocityX": 6.122564906085217e-17, "velocityY": -0.9999999859073992, - "timestamp": 1.9322760381131936 + "timestamp": 1.9322760381284287 }, { "x": 3, "y": 6.75, "heading": -1.571, - "angularVelocity": 3.506162881778858e-26, - "velocityX": 6.033433304097125e-17, - "velocityY": -0.9999998130813552, - "timestamp": 2.063526042798275 + "angularVelocity": 3.6968138862699375e-26, + "velocityX": 6.033492244151313e-17, + "velocityY": -0.999999813081372, + "timestamp": 2.0635260428135105 }, { - "x": 2.947790541091234, - "y": 6.708274167558872, + "x": 2.9477905302655625, + "y": 6.708274157634863, "heading": -1.571, - "angularVelocity": 1.8367126795602432e-25, - "velocityX": -0.5284778249133775, - "velocityY": -0.4223598105033264, - "timestamp": 2.162318189691711 + "angularVelocity": 1.9356698765722176e-25, + "velocityX": -0.5284779345875366, + "velocityY": -0.4223599110317691, + "timestamp": 2.1623181896893984 }, { - "x": 2.843375074671145, - "y": 6.723617786210881, + "x": 2.8433750381449614, + "y": 6.723617752718413, "heading": -1.571, - "angularVelocity": 1.8422931226784615e-25, - "velocityX": -1.05692071387738, - "velocityY": 0.15531212889381546, - "timestamp": 2.2611103365851473 + "angularVelocity": 1.9452475397041562e-25, + "velocityX": -1.056920974212429, + "velocityY": 0.15531189035527532, + "timestamp": 2.2611103365652863 }, { - "x": 2.6867593540916537, - "y": 6.796036114551366, + "x": 2.686759270236978, + "y": 6.7960360376723505, "heading": -1.571, - "angularVelocity": 1.8420943963618568e-25, - "velocityX": -1.5853053659055247, - "velocityY": 0.7330372971709972, - "timestamp": 2.3599024834785833 + "angularVelocity": 1.94415608308657e-25, + "velocityX": -1.5853058452585174, + "velocityY": 0.733036858131207, + "timestamp": 2.359902483441174 }, { - "x": 2.4779548883976252, - "y": 6.925539668717246, + "x": 2.4779547220704496, + "y": 6.92553951628491, "heading": -1.571, - "angularVelocity": 1.8414864642036264e-25, - "velocityX": -2.1135735203655353, - "velocityY": 1.3108689125419186, - "timestamp": 2.4586946303720194 + "angularVelocity": 1.943670176910069e-25, + "velocityX": -2.1135743555492152, + "velocityY": 1.3108681480042503, + "timestamp": 2.458694630317062 }, { - "x": 2.2169961917679095, - "y": 7.112159959748603, + "x": 2.2169958673173573, + "y": 7.1121596626476205, "heading": -1.571, - "angularVelocity": 1.8422871659159952e-25, - "velocityX": -2.641492313262553, - "velocityY": 1.8890194909182378, - "timestamp": 2.5574867772654555 + "angularVelocity": 1.9434969946845302e-25, + "velocityX": -2.641493914298006, + "velocityY": 1.8890180268798127, + "timestamp": 2.55748677719295 }, { - "x": 2.026837658650624, - "y": 7.31963208391883, + "x": 2.0268375106829706, + "y": 7.319631948507334, "heading": -1.571, - "angularVelocity": 1.8414775640545203e-25, - "velocityX": -1.9248345045320685, - "velocityY": 2.100087210312572, - "timestamp": 2.6562789241588916 + "angularVelocity": 1.9443911276635488e-25, + "velocityX": -1.9248327184678096, + "velocityY": 2.100088847348966, + "timestamp": 2.656278924068838 }, { - "x": 1.8889712227429223, - "y": 7.470113664922766, + "x": 1.8889711653847767, + "y": 7.470113612444433, "heading": -1.571, - "angularVelocity": 1.842116721306413e-25, - "velocityX": -1.3955201930818828, - "velocityY": 1.523213997629358, - "timestamp": 2.7550710710523276 + "angularVelocity": 1.9431166040683193e-25, + "velocityX": -1.3955192761565818, + "velocityY": 1.5232148373711183, + "timestamp": 2.7550710709447257 }, { - "x": 1.8033624960430958, - "y": 7.563573145350679, + "x": 1.8033624835208033, + "y": 7.563573133893712, "heading": -1.571, - "angularVelocity": 1.8415246383265505e-25, - "velocityX": -0.8665539659966102, - "velocityY": 0.9460213525749656, - "timestamp": 2.8538632179457637 + "angularVelocity": 1.94352961414745e-25, + "velocityX": -0.8665535123102769, + "velocityY": 0.9460217679720164, + "timestamp": 2.8538632178206136 }, { "x": 1.77, "y": 7.6, "heading": -1.571, - "angularVelocity": 9.209160199279817e-26, - "velocityX": -0.3377039278140482, - "velocityY": 0.36872216866198404, - "timestamp": 2.9526553648391998 + "angularVelocity": 9.71650033036472e-26, + "velocityX": -0.3377038011201076, + "velocityY": 0.3687222846978969, + "timestamp": 2.9526553646965015 }, { - "x": 1.769999806106138, - "y": 7.642307692307283, + "x": 1.7799998137324806, + "y": 7.642307736334161, "heading": -1.571, - "angularVelocity": 4.521417535191126e-26, - "velocityX": -0.0000022914728636606335, - "velocityY": 0.4999999887447983, - "timestamp": 3.0372707513584936 + "angularVelocity": 4.913051031934083e-26, + "velocityX": 0.11501058214794466, + "velocityY": 0.4865928021587884, + "timestamp": 3.0396022659791786 }, { - "x": 1.7699998225003861, - "y": 7.68461538461501, + "x": 1.7899998294834822, + "y": 7.684615424918922, "heading": -1.571, - "angularVelocity": 4.5222328288936885e-26, - "velocityX": 1.9375020074514916e-7, - "velocityY": 0.4999999887500343, - "timestamp": 3.1218861378777873 + "angularVelocity": 4.909750963875263e-26, + "velocityX": 0.11501290561799446, + "velocityY": 0.4865922529799326, + "timestamp": 3.1265491672618557 }, { - "x": 1.769999838665324, - "y": 7.726923076922736, + "x": 1.799999845000905, + "y": 7.726923113558894, "heading": -1.571, - "angularVelocity": 4.5223430345422915e-26, - "velocityX": 1.9104016727085167e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.206501524397081 + "angularVelocity": 4.91154883871533e-26, + "velocityX": 0.11501290293154022, + "velocityY": 0.48659225361491326, + "timestamp": 3.213496068544533 }, { - "x": 1.7699998547535982, - "y": 7.769230769230463, + "x": 1.8099998604642755, + "y": 7.769230802211641, "heading": -1.571, - "angularVelocity": 4.522360821860641e-26, - "velocityX": 1.901341494252509e-7, - "velocityY": 0.499999988750035, - "timestamp": 3.291116910916375 + "angularVelocity": 4.911173445042056e-26, + "velocityX": 0.11501290230986914, + "velocityY": 0.48659225376185367, + "timestamp": 3.30044296982721 }, { - "x": 1.7699998708857232, - "y": 7.8115384615381895, + "x": 1.8199998759636424, + "y": 7.81153849085588, "heading": -1.571, - "angularVelocity": 4.522227599449808e-26, - "velocityX": 1.9065238145885414e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.3757322974356687 + "angularVelocity": 4.910186740067351e-26, + "velocityX": 0.11501290272387527, + "velocityY": 0.4865922536639975, + "timestamp": 3.387389871109887 }, { - "x": 1.7699998870253404, - "y": 7.853846153845916, + "x": 1.8299998914682623, + "y": 7.853846179498878, "heading": -1.571, - "angularVelocity": 4.521986926265768e-26, - "velocityX": 1.907409270385452e-7, - "velocityY": 0.4999999887500347, - "timestamp": 3.4603476839549625 + "angularVelocity": 4.9095323376375024e-26, + "velocityX": 0.11501290278429054, + "velocityY": 0.48659225364971753, + "timestamp": 3.474336772392564 }, { - "x": 1.7699999031640838, - "y": 7.896153846153643, + "x": 1.8399999069719994, + "y": 7.8961538681420835, "heading": -1.571, - "angularVelocity": 4.521897586059284e-26, - "velocityX": 1.9073060076657246e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.5449630704742563 + "angularVelocity": 4.9098565172353323e-26, + "velocityX": 0.11501290277413707, + "velocityY": 0.48659225365211745, + "timestamp": 3.5612836736752413 }, { - "x": 1.769999919301922, - "y": 7.93846153846137, + "x": 1.8499999224750308, + "y": 7.938461556785456, "heading": -1.571, - "angularVelocity": 4.522736988509059e-26, - "velocityX": 1.907199008902848e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.62957845699355 + "angularVelocity": 4.910188971582487e-26, + "velocityX": 0.11501290276601811, + "velocityY": 0.48659225365403647, + "timestamp": 3.6482305749579185 }, { - "x": 1.7699999354395248, - "y": 7.980769230769097, + "x": 1.8599999379779208, + "y": 7.980769245428863, "heading": -1.571, - "angularVelocity": 4.522707345878583e-26, - "velocityX": 1.9071711976583354e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.714193843512844 + "angularVelocity": 4.9116368716604623e-26, + "velocityX": 0.11501290276439573, + "velocityY": 0.48659225365441994, + "timestamp": 3.7351774762405956 }, { - "x": 1.769999951576638, - "y": 8.023076923076822, + "x": 1.8699999534805036, + "y": 8.023076934072343, "heading": -1.571, - "angularVelocity": 4.522884645346994e-26, - "velocityX": 1.907113334822706e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.7988092300321377 + "angularVelocity": 4.9088992585035167e-26, + "velocityX": 0.11501290276085954, + "velocityY": 0.48659225365525577, + "timestamp": 3.8221243775232727 }, { - "x": 1.769999967709957, - "y": 8.065384615384549, + "x": 1.879999968980049, + "y": 8.06538462271654, "heading": -1.571, - "angularVelocity": 4.522407576023322e-26, - "velocityX": 1.9066649420464386e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.8834246165514315 + "angularVelocity": 4.9115516494556264e-26, + "velocityX": 0.11501290272592707, + "velocityY": 0.48659225366351255, + "timestamp": 3.90907127880595 }, { - "x": 1.769999983845188, - "y": 8.107692307692274, + "x": 1.88999998448058, + "y": 8.107692311360502, "heading": -1.571, - "angularVelocity": 4.522843638364402e-26, - "velocityX": 1.9068908979017438e-7, - "velocityY": 0.49999998875003476, - "timestamp": 3.9680400030707252 + "angularVelocity": 4.911175288807037e-26, + "velocityX": 0.11501290273726322, + "velocityY": 0.48659225366083314, + "timestamp": 3.996018180088627 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 4.5197757674515026e-26, - "velocityX": 1.90920500829837e-7, - "velocityY": 0.49999998875003127, - "timestamp": 4.052655389590019 + "angularVelocity": 4.9135183566524024e-26, + "velocityX": 0.11501290295450972, + "velocityY": 0.48659225360948094, + "timestamp": 4.082965081371304 }, { - "x": 1.77, + "x": 1.9, "y": 8.15, "heading": -1.571, - "angularVelocity": 1.5086235456844618e-26, - "velocityX": -8.006405502592549e-21, - "velocityY": 4.1987950762241055e-20, - "timestamp": 4.137270776109312 + "angularVelocity": 1.6371251773517092e-26, + "velocityX": -6.721181362730927e-21, + "velocityY": 6.32096054231833e-20, + "timestamp": 4.169911982653981 } ], "constraints": [ @@ -1910,7 +1903,7 @@ "B_PickUpFarPiece1AfterNear": { "waypoints": [ { - "x": 1.77, + "x": 1.9, "y": 7.9, "heading": -1.571, "isInitialGuess": false, @@ -1961,10 +1954,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "x": 8.8, + "x": 9, "y": 7.55, "heading": 0, "isInitialGuess": false, @@ -1975,517 +1968,526 @@ ], "trajectory": [ { - "x": 1.77, + "x": 1.9, "y": 7.9, "heading": -1.571, - "angularVelocity": -1.2871748405282096e-30, - "velocityX": -3.4109449165522936e-23, - "velocityY": 6.001012791624122e-24, + "angularVelocity": -3.4642760485591173e-29, + "velocityX": 3.5005463994906275e-27, + "velocityY": -4.952563831230141e-28, "timestamp": 0 }, { - "x": 1.807900003964869, - "y": 7.893333332635907, + "x": 1.9335666703903378, + "y": 7.893333332593777, "heading": -1.571, - "angularVelocity": -2.4513789653454947e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.06968380844403477 + "angularVelocity": -2.7619800848237704e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.06571406926224227 }, { - "x": 1.8837000104077808, - "y": 7.879999998169256, - "heading": -1.571, - "angularVelocity": -2.451378965313512e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.13936761688806953 + "x": 2.0007000097746364, + "y": 7.879999998058663, + "heading": -1.5710000000000002, + "angularVelocity": -2.761980084878875e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.13142813852448454 }, { - "x": 1.9974000168506918, - "y": 7.859999997035938, + "x": 2.1014000158256008, + "y": 7.8599999968568826, "heading": -1.5710000000000002, - "angularVelocity": -2.45137896507651e-18, - "velocityX": 1.631656032896467, - "velocityY": -0.28701073577774294, - "timestamp": 0.2090514253321043 + "angularVelocity": -2.7619800849390504e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.30434884684993946, + "timestamp": 0.1971422077867268 }, { - "x": 2.149000018337516, - "y": 7.833333330107737, + "x": 2.2356666838886423, + "y": 7.8333333299128824, "heading": -1.5710000000000002, - "angularVelocity": -2.451378965249631e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.27873523377613907 + "angularVelocity": -2.76198008487185e-18, + "velocityX": 2.0431951569949107, + "velocityY": -0.40579844230286244, + "timestamp": 0.2628562770489691 }, { - "x": 2.3385, + "x": 2.4034999999999997, "y": 7.800000000000001, "heading": -1.5710000000000002, - "angularVelocity": -2.451369395746655e-18, - "velocityX": 2.7194263042422167, - "velocityY": -0.4783511529010061, - "timestamp": 0.34841904222017384 + "angularVelocity": -2.7619800848365147e-18, + "velocityX": 2.5539936576076663, + "velocityY": -0.507247995552665, + "timestamp": 0.32857034631121135 }, { - "x": 2.527999981662484, - "y": 7.766666669892264, + "x": 2.5713333161113576, + "y": 7.766666670087118, "heading": -1.5710000000000002, - "angularVelocity": -2.4513825638540725e-18, - "velocityX": 2.719426304242217, - "velocityY": -0.47835115290100616, - "timestamp": 0.4181028506642086 + "angularVelocity": -2.7619800848960175e-18, + "velocityX": 2.553993657607666, + "velocityY": -0.5072479955526649, + "timestamp": 0.3942844155734536 }, { - "x": 2.6795999831493083, - "y": 7.740000002964063, - "heading": -1.5710000000000002, - "angularVelocity": -2.4513789650501425e-18, - "velocityX": 2.1755412752530408, - "velocityY": -0.38268096310519667, - "timestamp": 0.48778665910824337 + "x": 2.705599984174399, + "y": 7.740000003143118, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084797486e-18, + "velocityX": 2.0431951569949103, + "velocityY": -0.40579844230286233, + "timestamp": 0.4599984848356959 }, { - "x": 2.7932999895922195, - "y": 7.720000001830744, - "heading": -1.5710000000000002, - "angularVelocity": -2.451378965525874e-18, - "velocityX": 1.6316560328964669, - "velocityY": -0.287010735777743, - "timestamp": 0.5574704675522781 + "x": 2.8062999902253636, + "y": 7.7200000019413375, + "heading": -1.5710000000000004, + "angularVelocity": -2.761980084858984e-18, + "velocityX": 1.532396443889444, + "velocityY": -0.3043488468499394, + "timestamp": 0.5257125540979382 }, { - "x": 2.869099996035131, - "y": 7.706666667364093, + "x": 2.873433329609662, + "y": 7.706666667406224, "heading": -1.5710000000000004, - "angularVelocity": -2.45137896490822e-18, - "velocityX": 1.0877707194173973, - "velocityY": -0.19134049593973587, - "timestamp": 0.6271542759963129 + "angularVelocity": -2.7619800847561526e-18, + "velocityX": 1.0215976599530407, + "velocityY": -0.20289923732930318, + "timestamp": 0.5914266233601804 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -2.4513789653928325e-18, - "velocityX": 0.543885370377075, - "velocityY": -0.09567024984645128, - "timestamp": 0.6968380844403477 + "angularVelocity": -2.76198008510439e-18, + "velocityX": 0.5107988406011633, + "velocityY": -0.10144962077480908, + "timestamp": 0.6571406926224226 }, { "x": 2.907, "y": 7.7, "heading": -1.5710000000000004, - "angularVelocity": -8.171263216835412e-19, - "velocityX": 3.118534113064725e-17, - "velocityY": -4.765080041362501e-18, - "timestamp": 0.7665218928843824 - }, - { - "x": 2.9235569599541296, - "y": 7.698949367220036, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653510822e-18, - "velocityX": 0.361868027527245, - "velocityY": -0.022962573612202903, - "timestamp": 0.8122760316716063 + "angularVelocity": -9.20660028293447e-19, + "velocityX": 3.665984348782275e-17, + "velocityY": -5.747314644128388e-18, + "timestamp": 0.7228547618846648 }, { - "x": 2.9566708794154706, - "y": 7.6968481016884684, + "x": 2.923556957543874, + "y": 7.698949367372981, "heading": -1.5710000000000004, - "angularVelocity": -1.1603542652771816e-18, - "velocityX": 0.7237360452866696, - "velocityY": -0.04592514660458234, - "timestamp": 0.8580301704588302 + "angularVelocity": -1.4410194825190118e-18, + "velocityX": 0.3618680011852347, + "velocityY": -0.02296257194065333, + "timestamp": 0.7686088973419505 }, { - "x": 3.006341757820839, - "y": 7.693696203441033, - "heading": -1.5710000000000004, - "angularVelocity": -1.1603542653006352e-18, - "velocityX": 1.085604050737173, - "velocityY": -0.0688877188158911, - "timestamp": 0.9037843092460541 + "x": 2.9566708721846076, + "y": 7.696848102147308, + "heading": -1.5710000000000006, + "angularVelocity": -1.4410194825318246e-18, + "velocityX": 0.7237359926005489, + "velocityY": -0.045925143261349936, + "timestamp": 0.8143630327992362 }, { - "x": 3.0725695944386313, - "y": 7.689493672524154, + "x": 3.0063417433588766, + "y": 7.6936962043587265, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653002277e-18, - "velocityX": 1.447472040197785, - "velocityY": -0.0918502900125508, - "timestamp": 0.949538448033278 + "angularVelocity": -1.4410194826287186e-18, + "velocityX": 1.085603971703869, + "velocityY": -0.06888771380078085, + "timestamp": 0.8601171682565218 }, { - "x": 3.1553543882799935, - "y": 7.684240509000582, + "x": 3.0725695703348617, + "y": 7.689493674053676, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653590159e-18, - "velocityX": 1.8093400080460489, - "velocityY": -0.11481285983778473, - "timestamp": 0.9952925868205019 + "angularVelocity": -1.441019482377035e-18, + "velocityX": 1.447471934812594, + "velocityY": -0.09185028332526422, + "timestamp": 0.9058713037138075 }, { - "x": 3.254696137934192, - "y": 7.677936712959833, + "x": 3.1553543521233594, + "y": 7.6842405112949255, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542649957643e-18, - "velocityX": 2.1712079450614197, - "velocityY": -0.13777542770649714, - "timestamp": 1.0410467256077258 + "angularVelocity": -1.4410194823401156e-18, + "velocityX": 1.8093398763013369, + "velocityY": -0.11481285147783742, + "timestamp": 0.9516254391710932 }, { - "x": 3.3705948412254285, - "y": 7.670582284539977, + "x": 3.2546960873130226, + "y": 7.6779367161720335, "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651840332e-18, - "velocityX": 2.53307583452264, - "velocityY": -0.16073799255762966, - "timestamp": 1.0868008643949496 + "angularVelocity": -1.4410194827029632e-18, + "velocityX": 2.171207786943753, + "velocityY": -0.13777541767303667, + "timestamp": 0.9973795746283789 }, { - "x": 3.503050494358926, - "y": 7.662177223981812, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542653102151e-18, - "velocityX": 2.8949436410454266, - "velocityY": -0.1837005521458494, - "timestamp": 1.1325550031821736 + "x": 3.370594773726837, + "y": 7.6705822888231445, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194824370812e-18, + "velocityX": 2.5330756500054417, + "velocityY": -0.16073798084896954, + "timestamp": 1.0431337100856646 }, { - "x": 3.652063089043379, - "y": 7.652721531811468, - "heading": -1.5710000000000006, - "angularVelocity": -1.1603542651181787e-18, - "velocityX": 3.2568112663538877, - "velocityY": -0.2066631002349949, - "timestamp": 1.1783091419693976 + "x": 3.5030504075671254, + "y": 7.6621772294892425, + "heading": -1.5710000000000008, + "angularVelocity": -1.4410194825282925e-18, + "velocityX": 2.8949434300653256, + "velocityY": -0.1837005387579684, + "timestamp": 1.0888878455429503 }, { - "x": 3.817632592880325, - "y": 7.642215210084812, + "x": 3.6520629805328038, + "y": 7.652721538697079, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542640713407e-18, - "velocityX": 3.6186781835632806, - "velocityY": -0.2296256033912485, - "timestamp": 1.2240632807566216 + "angularVelocity": -1.4410194824897673e-18, + "velocityX": 3.2568110286971184, + "velocityY": -0.20666308515432835, + "timestamp": 1.134641981000236 }, { - "x": 3.9666453345551678, - "y": 7.632759508587096, + "x": 3.817632460165024, + "y": 7.642215218506348, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542647952754e-18, - "velocityX": 3.256814478955544, - "velocityY": -0.20666330409274528, - "timestamp": 1.2698174195438456 + "angularVelocity": -1.4410194819422767e-18, + "velocityX": 3.61867791791012, + "velocityY": -0.22962558653405252, + "timestamp": 1.1803961164575216 }, { - "x": 4.099101141061488, - "y": 7.624354438296557, + "x": 3.9666452436917647, + "y": 7.632759514352893, "heading": -1.5710000000000008, - "angularVelocity": -1.1603542651048875e-18, - "velocityX": 2.894946993141186, - "velocityY": -0.1837007648552898, - "timestamp": 1.3155715583310696 + "angularVelocity": -1.4410194824624615e-18, + "velocityX": 3.25681563069441, + "velocityY": -0.2066633771770923, + "timestamp": 1.2261502519148073 + }, + { + "x": 4.099101094429873, + "y": 7.624354441255597, + "heading": -1.571000000000001, + "angularVelocity": -1.4410194824570077e-18, + "velocityX": 2.894948170558449, + "velocityY": -0.18370083956907635, + "timestamp": 1.271904387372093 }, { "x": 4.215, "y": 7.617, - "heading": -1.5710000000000008, - "angularVelocity": -3.5267965497472325e-18, - "velocityX": 2.533079236328726, - "velocityY": -0.16073820842147152, - "timestamp": 1.3613256971182937 - }, - { - "x": 4.372102638958384, - "y": 7.604137670056067, - "heading": -1.4067929344278918, - "angularVelocity": 2.5012754729123667, - "velocityX": 2.3930576689167724, - "velocityY": -0.19592476305046946, - "timestamp": 1.426975029768865 - }, - { - "x": 4.504023469666986, - "y": 7.584807496549773, - "heading": -1.1481197216785133, - "angularVelocity": 3.940226081589852, - "velocityX": 2.0094771017860147, - "velocityY": -0.2944458492712955, - "timestamp": 1.4926243624194364 - }, - { - "x": 4.60700440416476, - "y": 7.557706508847077, - "heading": -0.8272570767570808, - "angularVelocity": 4.887523329879827, - "velocityX": 1.5686516578342056, - "velocityY": -0.41281436701235225, - "timestamp": 1.5582736950700078 - }, - { - "x": 4.700954879611763, - "y": 7.512168160289293, - "heading": -0.46366508061347683, - "angularVelocity": 5.538395920632406, - "velocityX": 1.431095666224292, - "velocityY": -0.6936604946152823, - "timestamp": 1.6239230277205792 - }, - { - "x": 4.823042014881433, - "y": 7.4730785715901, - "heading": -0.16917269346197744, - "angularVelocity": 4.485839768083199, - "velocityX": 1.8596858542233212, - "velocityY": -0.5954300999091777, - "timestamp": 1.6895723603711506 - }, - { - "x": 4.964956593821971, - "y": 7.435238066935198, - "heading": 2.8484788827976314e-16, - "angularVelocity": 2.57691413806861, - "velocityX": 2.161706345068334, - "velocityY": -0.576403493029435, - "timestamp": 1.755221693021722 + "heading": -1.571000000000001, + "angularVelocity": -4.672933317458188e-18, + "velocityX": 2.5330804398600892, + "velocityY": -0.1607382847923455, + "timestamp": 1.3176585228293787 + }, + { + "x": 4.372100003975642, + "y": 7.604134525546081, + "heading": -1.4068015764148147, + "angularVelocity": 2.5011316235769736, + "velocityX": 2.3930058488366557, + "velocityY": -0.19597170488397217, + "timestamp": 1.383308175985225 + }, + { + "x": 4.50402015326741, + "y": 7.584792726357379, + "heading": -1.1481306958042976, + "angularVelocity": 3.940171321174791, + "velocityX": 2.009456911811971, + "velocityY": -0.2946214985011475, + "timestamp": 1.4489578291410714 + }, + { + "x": 4.607002128532945, + "y": 7.557661434831198, + "heading": -0.8272795511530444, + "angularVelocity": 4.887324292310532, + "velocityX": 1.5686598529527007, + "velocityY": -0.41327395077246554, + "timestamp": 1.5146074822969178 + }, + { + "x": 4.700950132368006, + "y": 7.512144695800665, + "heading": -0.46367956909020736, + "angularVelocity": 5.5384905263762105, + "velocityX": 1.4310510310194355, + "velocityY": -0.6933279437461265, + "timestamp": 1.5802571354527641 + }, + { + "x": 4.8230397238523155, + "y": 7.473064704270448, + "heading": -0.16917524125362357, + "angularVelocity": 4.485999752933481, + "velocityX": 1.8597141891098792, + "velocityY": -0.5952810053284208, + "timestamp": 1.6459067886086105 + }, + { + "x": 4.964955510700854, + "y": 7.435231531409891, + "heading": 3.1934304094208187e-16, + "angularVelocity": 2.5769403663425607, + "velocityX": 2.1617141908013076, + "velocityY": -0.5762889983690314, + "timestamp": 1.7115564417644569 }, { "x": 5.1133599281311035, "y": 7.399226188659668, - "heading": 2.560323167481079e-16, - "angularVelocity": -9.137154494425779e-18, - "velocityX": 2.260545969274894, - "velocityY": -0.5485490380710709, - "timestamp": 1.8208710256722933 - }, - { - "x": 5.309797730179564, - "y": 7.378773507546932, - "heading": 2.2755149667394e-16, - "angularVelocity": -2.487788098670228e-18, - "velocityX": 2.760397768454213, - "velocityY": -0.2874066738371958, - "timestamp": 1.8920338864357054 - }, - { - "x": 5.5370497739551165, - "y": 7.384033087440106, - "heading": 1.9907067659976473e-16, - "angularVelocity": -2.4877880987734015e-18, - "velocityX": 3.193407928482695, - "velocityY": 0.07390905644553293, - "timestamp": 1.9631967471991174 - }, - { - "x": 5.734541367744454, - "y": 7.416217562641249, - "heading": 1.7058985652559142e-16, - "angularVelocity": -2.4877880987466103e-18, - "velocityX": 2.7752059384736096, - "velocityY": 0.4522650559006595, - "timestamp": 2.0343596079625295 - }, - { - "x": 5.8919109073823455, - "y": 7.447474939732356, - "heading": 1.421090364514119e-16, - "angularVelocity": -2.4877880988332663e-18, - "velocityX": 2.211399850283212, - "velocityY": 0.43923721946865085, - "timestamp": 2.1055224687259413 - }, - { - "x": 6.009566431740217, - "y": 7.472949876850092, - "heading": 1.1362821637723588e-16, - "angularVelocity": -2.487788098784733e-18, - "velocityX": 1.6533276360131506, - "velocityY": 0.3579807900424195, - "timestamp": 2.176685329489353 - }, - { - "x": 6.087823373238344, - "y": 7.4907833719278125, - "heading": 8.51473963030607e-17, - "angularVelocity": -2.4877880987719023e-18, - "velocityX": 1.0996879644611663, - "velocityY": 0.2506011546812913, - "timestamp": 2.247848190252765 + "heading": 2.8711384441848255e-16, + "angularVelocity": -9.522901504497603e-18, + "velocityX": 2.260551431666984, + "velocityY": -0.5484468084647249, + "timestamp": 1.7772060949203032 + }, + { + "x": 5.309798294852915, + "y": 7.378778747778154, + "heading": 2.551760796876138e-16, + "angularVelocity": -2.748952599816597e-18, + "velocityX": 2.7604163011269454, + "velocityY": -0.2873341398001768, + "timestamp": 1.8483686824768482 + }, + { + "x": 5.537052553943782, + "y": 7.384041108580089, + "heading": 2.232383149567335e-16, + "angularVelocity": -2.7489525999792118e-18, + "velocityX": 3.193451318926863, + "velocityY": 0.07394841843766191, + "timestamp": 1.919531270033393 + }, + { + "x": 5.7345430393183845, + "y": 7.4162242310813244, + "heading": 1.9130055022585706e-16, + "angularVelocity": -2.7489525999248097e-18, + "velocityX": 2.775201017216226, + "velocityY": 0.45224778365094176, + "timestamp": 1.990693857589938 + }, + { + "x": 5.89191179894478, + "y": 7.447479383073381, + "heading": 1.593627854949789e-16, + "angularVelocity": -2.748952599950355e-18, + "velocityX": 2.2113973792956694, + "velocityY": 0.4392076379638292, + "timestamp": 2.061856445146483 + }, + { + "x": 6.009566832246889, + "y": 7.472952249257499, + "heading": 1.2742502076410038e-16, + "angularVelocity": -2.748952599954475e-18, + "velocityX": 1.653327082979625, + "velocityY": 0.35795306296166557, + "timestamp": 2.133019032703028 + }, + { + "x": 6.087823494374736, + "y": 7.490784200402746, + "heading": 9.54872560332223e-17, + "angularVelocity": -2.748952599948443e-18, + "velocityX": 1.0996882605721798, + "velocityY": 0.25058042094223126, + "timestamp": 2.2041816202595728 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 5.666657622888386e-17, - "angularVelocity": -2.4877880987955836e-18, - "velocityX": 0.5488836890134096, - "velocityY": 0.12951458068604244, - "timestamp": 2.3190110510161768 + "heading": 6.354949130235964e-17, + "angularVelocity": -2.748952599731403e-18, + "velocityX": 0.5488840940352727, + "velocityY": 0.12950343591616847, + "timestamp": 2.2753442078161177 }, { "x": 6.126883506774902, "y": 7.5, - "heading": 2.830378172433852e-17, - "angularVelocity": -8.292626564371985e-19, - "velocityX": 1.450767762103865e-19, - "velocityY": -4.7804854612827675e-19, - "timestamp": 2.3901739117795886 - }, - { - "x": 6.15611975515317, - "y": 7.501785785181937, - "heading": 2.830378172433841e-17, - "angularVelocity": -1.7359237081093472e-30, - "velocityX": 0.4808977303168891, - "velocityY": 0.02937381122626305, - "timestamp": 2.450969059136598 - }, - { - "x": 6.2145922519504335, - "y": 7.505357339791126, - "heading": 2.8303781724338447e-17, - "angularVelocity": 6.560557249735871e-31, - "velocityX": 0.9617954613037508, - "velocityY": 0.0587473633087091, - "timestamp": 2.5117642064936074 - }, - { - "x": 6.302300997223708, - "y": 7.5107146417709965, - "heading": 2.8303781724344634e-17, - "angularVelocity": 1.0166114443831184e-28, - "velocityX": 1.44269319322838, - "velocityY": 0.08812055258968164, - "timestamp": 2.572559353850617 - }, - { - "x": 6.419245991058483, - "y": 7.51785765803668, - "heading": 2.830378172433783e-17, - "angularVelocity": -1.1196675906591495e-28, - "velocityX": 1.9235909265592357, - "velocityY": 0.11749319766817592, - "timestamp": 2.6333545012076263 - }, - { - "x": 6.565427233597177, - "y": 7.526786333446685, - "heading": 2.830378172467021e-17, - "angularVelocity": 5.467389383569676e-27, - "velocityX": 2.4044886622326676, - "velocityY": 0.14686493574187343, - "timestamp": 2.6941496485646357 - }, - { - "x": 6.740844725124364, - "y": 7.537500557717893, - "heading": 2.8303781724055295e-17, - "angularVelocity": -8.483831515101901e-30, - "velocityX": 2.8853864025869926, - "velocityY": 0.1762348598037055, - "timestamp": 2.754944795921645 + "heading": 3.1742141835585016e-17, + "angularVelocity": -9.163175252374902e-19, + "velocityX": -2.2913448895975365e-19, + "velocityY": -1.0265149362709145e-18, + "timestamp": 2.3465067953726626 + }, + { + "x": 6.156119663954512, + "y": 7.501787279405357, + "heading": 3.174214182765833e-17, + "angularVelocity": 1.1020322934166523e-25, + "velocityX": 0.48089622932964227, + "velocityY": 0.029398389176605127, + "timestamp": 2.4073019428421385 + }, + { + "x": 6.214591998656713, + "y": 7.50536149033989, + "heading": 3.1742141831543016e-17, + "angularVelocity": 6.390315645162633e-26, + "velocityX": 0.9617927932744761, + "velocityY": 0.058791056248813034, + "timestamp": 2.4680970903116144 + }, + { + "x": 6.302300539353782, + "y": 7.51072214579269, + "heading": 3.1742141831543016e-17, + "angularVelocity": 5.527535991984969e-30, + "velocityX": 1.4426898255507186, + "velocityY": 0.08817571263384313, + "timestamp": 2.5288922377810903 + }, + { + "x": 6.4192453287368645, + "y": 7.517868515281858, + "heading": 3.1742141831542363e-17, + "angularVelocity": -5.5005027620127605e-30, + "velocityX": 1.9235875600400183, + "velocityY": 0.11754835355494872, + "timestamp": 2.589687385250566 + }, + { + "x": 6.565426437911824, + "y": 7.526799381429512, + "heading": 3.174214183154203e-17, + "angularVelocity": -1.9865124104755493e-32, + "velocityX": 2.404486464127003, + "velocityY": 0.14690097021540635, + "timestamp": 2.650482532720042 + }, + { + "x": 6.740844008917749, + "y": 7.537512309825002, + "heading": 3.1742141831543275e-17, + "angularVelocity": 2.560924529534356e-29, + "velocityX": 2.8853877045696796, + "velocityY": 0.17621354403110642, + "timestamp": 2.711277680189518 }, { "x": 6.945498466491699, "y": 7.55, - "heading": 2.8303781724055554e-17, - "angularVelocity": 4.2167202635793064e-30, - "velocityX": 3.3662841569498863, - "velocityY": 0.20559934181433973, - "timestamp": 2.8157399432786545 - }, - { - "x": 7.229321610470641, - "y": 7.550003362381949, - "heading": 1.1593994241588832e-17, - "angularVelocity": -2.1482520451536383e-16, - "velocityX": 3.6489012810873196, - "velocityY": 0.00004322762277017461, - "timestamp": 2.8935231229579816 - }, - { - "x": 7.5131447544694705, - "y": 7.5500027545013815, - "heading": 7.195372591415266e-18, - "angularVelocity": -5.654977937784957e-17, - "velocityX": 3.6489012813430093, - "velocityY": -0.000007815064517695191, - "timestamp": 2.9713063026373088 - }, - { - "x": 7.796967898468301, - "y": 7.55000214698679, - "heading": -2.2105881487706106e-17, - "angularVelocity": -3.7670424634117877e-16, - "velocityX": 3.6489012813430186, - "velocityY": -0.00000781035943334551, - "timestamp": 3.049089482316636 - }, - { - "x": 8.080791042466867, - "y": 7.5500015394568125, - "heading": -3.9909490658933576e-17, - "angularVelocity": -2.28887649548727e-16, - "velocityX": 3.6489012813396258, - "velocityY": -0.000007810557249203658, - "timestamp": 3.126872661995963 - }, - { - "x": 8.320527341192129, - "y": 7.550001026304512, - "heading": -3.990273647201336e-17, - "angularVelocity": 8.683351527189107e-20, - "velocityX": 3.082109778921481, - "velocityY": -0.000006597214238392456, - "timestamp": 3.20465584167529 - }, - { - "x": 8.51231639819258, - "y": 7.550000615782698, - "heading": -3.9899086872036086e-17, - "angularVelocity": 4.6920169533433444e-20, - "velocityX": 2.465688054809929, - "velocityY": -0.0000052777710475807675, - "timestamp": 3.2824390213546173 - }, - { - "x": 8.656158196885205, - "y": 7.550000307891346, - "heading": -3.989681664095531e-17, - "angularVelocity": 2.9186658222831174e-20, - "velocityX": 1.8492661175029514, - "velocityY": -0.00000395832817870783, - "timestamp": 3.3602222010339444 - }, - { - "x": 8.752052731742296, - "y": 7.550000102630447, - "heading": -3.9895416797891945e-17, - "angularVelocity": 1.7996732314772988e-20, - "velocityX": 1.2328441091304374, - "velocityY": -0.0000026388854100440123, - "timestamp": 3.4380053807132716 - }, - { - "x": 8.8, + "heading": 3.174214183411669e-17, + "angularVelocity": 4.2334783572302846e-26, + "velocityX": 3.3662959313768197, + "velocityY": 0.2054060347705879, + "timestamp": 2.772072827658994 + }, + { + "x": 7.221347124992817, + "y": 7.550083175294458, + "heading": 2.582386400207731e-18, + "angularVelocity": -3.857226029687031e-16, + "velocityX": 3.6489010624124374, + "velocityY": 0.0011002352593078705, + "timestamp": 2.8476705634657145 + }, + { + "x": 7.4971957957486515, + "y": 7.550070265650631, + "heading": -3.139160687691863e-17, + "angularVelocity": -4.494049049826975e-16, + "velocityX": 3.6489012245167154, + "velocityY": -0.00017076759891002745, + "timestamp": 2.923268299272435 + }, + { + "x": 7.7730444665050475, + "y": 7.550057368010946, + "heading": -4.4250107790457757e-17, + "angularVelocity": -1.700910850877103e-16, + "velocityX": 3.648901224524143, + "velocityY": -0.00017060880920745955, + "timestamp": 2.9988660350791556 + }, + { + "x": 8.04889313726144, + "y": 7.550044470349665, + "heading": -3.3139122030001965e-17, + "angularVelocity": 1.4697511296982546e-16, + "velocityX": 3.648901224524074, + "velocityY": -0.00017060909487947776, + "timestamp": 3.074463770885876 + }, + { + "x": 8.320637928532271, + "y": 7.550031764514427, + "heading": -3.3149587273190146e-17, + "angularVelocity": -1.3843328871118217e-19, + "velocityX": 3.594615478505821, + "velocityY": -0.00016807163736953773, + "timestamp": 3.1500615066925968 + }, + { + "x": 8.54709194269371, + "y": 7.5500211763353215, + "heading": -3.3161210294625444e-17, + "angularVelocity": -1.5374827446451776e-19, + "velocityX": 2.99551318230494, + "velocityY": -0.0001400594738974264, + "timestamp": 3.2256592424993173 + }, + { + "x": 8.728255161631012, + "y": 7.5500127057980455, + "heading": -3.317341059346824e-17, + "angularVelocity": -1.613844477074772e-19, + "velocityX": 2.39641064648388, + "velocityY": -0.00011204749964645467, + "timestamp": 3.301256978306038 + }, + { + "x": 8.864127579305954, + "y": 7.550006352897831, + "heading": -3.318651341446678e-17, + "angularVelocity": -1.7332292901257806e-19, + "velocityX": 1.797308030789794, + "velocityY": -0.00008403558846668993, + "timestamp": 3.3768547141127585 + }, + { + "x": 8.954709192699438, + "y": 7.5500021176322925, + "heading": -3.320067915378452e-17, + "angularVelocity": -1.8738311623596625e-19, + "velocityX": 1.19820537515927, + "velocityY": -0.00005602370882446872, + "timestamp": 3.452452449919479 + }, + { + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": -3.908565443774477e-21, - "velocityX": 0.6164220652250822, - "velocityY": -0.000001319442690505465, - "timestamp": 3.5157885603925987 + "heading": -3.3215694928340293e-17, + "angularVelocity": -1.9862730534835854e-19, + "velocityX": 0.5991026955669089, + "velocityY": -0.00002801184810473847, + "timestamp": 3.5280501857261997 }, { - "x": 8.8, + "x": 9, "y": 7.55, - "heading": -3.989572081854024e-17, - "angularVelocity": 2.0117689927326026e-32, - "velocityX": 8.595312932705472e-20, - "velocityY": 5.963533551498734e-19, - "timestamp": 3.593571740071926 + "heading": -3.321569492834034e-17, + "angularVelocity": -5.16984725650092e-31, + "velocityX": 2.95751067102717e-19, + "velocityY": 2.6561323414610804e-18, + "timestamp": 3.6036479215329202 } ], "constraints": [ @@ -3404,7 +3406,7 @@ "waypoints": [ { "x": 8.8, - "y": 7.55, + "y": 7.6, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -5237,29 +5239,29 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { - "x": 2.907, - "y": 7.6, + "x": 3, + "y": 7.87, "heading": -0.8280080399812538, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 12 }, { - "x": 4.439387798309326, - "y": 6.998125076293945, + "x": 4.099836826324463, + "y": 7.594134330749512, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 8 + "controlIntervalCount": 10 }, { - "x": 5.638672828674316, - "y": 6.7, + "x": 5.854011535644531, + "y": 7.282280921936035, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -5267,17 +5269,17 @@ "controlIntervalCount": 9 }, { - "x": 7.068823337554932, - "y": 6.6, + "x": 7.12091588973999, + "y": 7.45, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 9 }, { "x": 8.701725006103516, - "y": 6.490337371826172, + "y": 7.45, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -5286,7 +5288,7 @@ }, { "x": 8.73, - "y": 6.490337371826172, + "y": 7.45, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -5296,499 +5298,499 @@ ], "trajectory": [ { - "x": 1.9, + "x": 1.8999999999999968, "y": 7.9, "heading": -1.571, - "angularVelocity": 2.300332094481187e-29, - "velocityX": -4.0491893744801466e-20, - "velocityY": -1.3926499461964718e-19, + "angularVelocity": 4.470498597716865e-29, + "velocityX": 2.7707804022206673e-16, + "velocityY": 1.8739023107890902e-17, "timestamp": 0 }, { - "x": 1.9154424529969702, - "y": 7.8954006953331355, + "x": 1.914365871743733, + "y": 7.899792228900449, "heading": -1.571, - "angularVelocity": -7.879169842980081e-20, - "velocityX": 0.34247331200662146, - "velocityY": -0.10200057610651515, - "timestamp": 0.04509096754573317 + "angularVelocity": 4.2271636316208514e-19, + "velocityX": 0.33739566619749, + "velocityY": -0.004879694723755523, + "timestamp": 0.042578708569808256 }, { - "x": 1.9463273585735072, - "y": 7.886202086121636, + "x": 1.9430976157693656, + "y": 7.899376752477764, "heading": -1.571, - "angularVelocity": -7.879169825285715e-20, - "velocityX": 0.6849466147563192, - "velocityY": -0.2040011495022858, - "timestamp": 0.09018193509146634 + "angularVelocity": 4.227163636037315e-19, + "velocityX": 0.6747913450340882, + "velocityY": -0.009757844628066633, + "timestamp": 0.08515741713961651 }, { - "x": 1.9926547162078563, - "y": 7.872404172518289, + "x": 1.986195232749876, + "y": 7.898753653015407, "heading": -1.571, - "angularVelocity": -7.879169828739293e-20, - "velocityX": 1.0274199059348594, - "velocityY": -0.30600171950962485, - "timestamp": 0.13527290263719952 + "angularVelocity": 4.227163633163533e-19, + "velocityX": 1.0121870396762844, + "velocityY": -0.014634062029792581, + "timestamp": 0.12773612570942477 }, { - "x": 2.0544245252291895, - "y": 7.8540069547195355, + "x": 2.0436587235509935, + "y": 7.897923036409323, "heading": -1.571, - "angularVelocity": -7.87916983435114e-20, - "velocityX": 1.3698931822361922, - "velocityY": -0.40800228516040743, - "timestamp": 0.18036387018293268 + "angularVelocity": 4.227163634858386e-19, + "velocityX": 1.349582754650927, + "velocityY": -0.019507792368223267, + "timestamp": 0.17031483427923302 }, { - "x": 2.13163678474307, - "y": 7.8310104329872985, + "x": 2.115488089327842, + "y": 7.896885044034559, "heading": -1.571, - "angularVelocity": -7.879169829580418e-20, - "velocityX": 1.7123664387012403, - "velocityY": -0.5100028450024455, - "timestamp": 0.22545483772866584 + "angularVelocity": 4.2271636331977046e-19, + "velocityX": 1.6869784967546948, + "velocityY": -0.024378202384055483, + "timestamp": 0.21289354284904127 }, { - "x": 2.2242914934972844, - "y": 7.8034146076882696, + "x": 2.2016833316991637, + "y": 7.895639874163583, "heading": -1.571, - "angularVelocity": -7.879169836864827e-20, - "velocityX": 2.0548396673954747, - "velocityY": -0.6120033967122365, - "timestamp": 0.27054580527439903 + "angularVelocity": 4.2271636334033824e-19, + "velocityX": 2.024374276876126, + "velocityY": -0.029243955789217388, + "timestamp": 0.25547225141884955 }, { - "x": 2.332388649613512, - "y": 7.771219479372483, + "x": 2.302244453096514, + "y": 7.894187824967971, "heading": -1.571, - "angularVelocity": -7.87916983823685e-20, - "velocityX": 2.397312854433457, - "velocityY": -0.7140039362236463, - "timestamp": 0.3156367728201322 + "angularVelocity": 4.227163633528535e-19, + "velocityX": 2.361770114104849, + "velocityY": -0.03410270636161241, + "timestamp": 0.2980509599886578 }, { - "x": 2.455928249961216, - "y": 7.73442504895667, + "x": 2.4171714575816776, + "y": 7.8925293954471245, "heading": -1.571, - "angularVelocity": -7.879169832615412e-20, - "velocityX": 2.739785972044277, - "velocityY": -0.8160044554043961, - "timestamp": 0.36072774036586536 + "angularVelocity": 4.227163633894554e-19, + "velocityX": 2.699166046728326, + "velocityY": -0.03894973747556378, + "timestamp": 0.3406296685584661 }, { - "x": 2.5949102882793076, - "y": 7.693031318274292, + "x": 2.5464643533128632, + "y": 7.890665591362613, "heading": -1.571, - "angularVelocity": -7.879169827922094e-20, - "velocityX": 3.082258950800517, - "velocityY": -0.9180049339237416, - "timestamp": 0.4058187079115985 + "angularVelocity": 4.2271636346276787e-19, + "velocityX": 3.0365621709548005, + "velocityY": -0.04377314735710882, + "timestamp": 0.3832083771282744 }, { - "x": 2.749334745784455, - "y": 7.647038292825759, + "x": 2.6901231650458177, + "y": 7.888599493997031, "heading": -1.571, - "angularVelocity": -7.879169832843695e-20, - "velocityX": 3.424731512991456, - "velocityY": -1.0200052904583357, - "timestamp": 0.4509096754573317 + "angularVelocity": 4.227163652979875e-19, + "velocityX": 3.3739588765926873, + "velocityY": -0.04852419049232661, + "timestamp": 0.42578708569808266 }, { - "x": 2.907, - "y": 7.6, - "heading": -1.571, - "angularVelocity": -2.3912453364100895e-19, - "velocityX": 3.496603927507973, - "velocityY": -1.0431865933693085, - "timestamp": 0.49600064300306484 - }, - { - "x": 3.0409750518324072, - "y": 7.55855968097087, - "heading": -1.5404817853182164, - "angularVelocity": 0.7344477642220753, - "velocityX": 3.2242278359283354, - "velocityY": -0.9972978425163852, - "timestamp": 0.5375532437031425 - }, - { - "x": 3.167821917464271, - "y": 7.522503707879653, - "heading": -1.488578715495742, - "angularVelocity": 1.2490931722204932, - "velocityX": 3.052681745420317, - "velocityY": -0.8677188066147962, - "timestamp": 0.5791058444032202 - }, - { - "x": 3.2926667162009906, - "y": 7.486533976140662, - "heading": -1.4329122238837848, - "angularVelocity": 1.3396632382593825, - "velocityX": 3.004500239054416, - "velocityY": -0.8656433323781126, - "timestamp": 0.6206584451032979 - }, - { - "x": 3.4110148334033306, - "y": 7.453039903093125, - "heading": -1.3612319162225908, - "angularVelocity": 1.7250498513578787, - "velocityX": 2.8481518655489513, - "velocityY": -0.8060644215578923, - "timestamp": 0.6622110458033756 - }, - { - "x": 3.5181150608793357, - "y": 7.422087979391574, - "heading": -1.2632892360186367, - "angularVelocity": 2.357077019340078, - "velocityX": 2.577461474650843, - "velocityY": -0.7448853544680833, - "timestamp": 0.7037636465034534 - }, - { - "x": 3.6131894964406737, - "y": 7.390320324704052, - "heading": -1.1401421709924013, - "angularVelocity": 2.963642779307876, - "velocityX": 2.2880501812045355, - "velocityY": -0.7645166404100056, - "timestamp": 0.7453162472035311 - }, - { - "x": 3.6966643009377673, - "y": 7.3543290738456, - "heading": -0.9935989943181865, - "angularVelocity": 3.526690849796876, - "velocityX": 2.0088948246489093, - "velocityY": -0.8661612089754375, - "timestamp": 0.7868688479036088 - }, - { - "x": 3.771161529488124, - "y": 7.3090712240596085, - "heading": -0.8292288931023285, - "angularVelocity": 3.955711518570448, - "velocityX": 1.7928415380801184, - "velocityY": -1.0891700885984243, - "timestamp": 0.8284214486036865 - }, - { - "x": 3.8550844036685596, - "y": 7.255704524470319, - "heading": -0.6866898973306557, - "angularVelocity": 3.4303267032669584, - "velocityX": 2.019678016935338, - "velocityY": -1.2843167140003124, - "timestamp": 0.8699740493037642 - }, - { - "x": 3.950562606960919, - "y": 7.198892399115019, - "heading": -0.5688079573114242, - "angularVelocity": 2.8369329003034878, - "velocityX": 2.2977672079183233, - "velocityY": -1.3672339251486203, - "timestamp": 0.9115266500038419 - }, - { - "x": 4.057836088819487, - "y": 7.143730615244361, - "heading": -0.4749573744321577, - "angularVelocity": 2.2585970865377405, - "velocityX": 2.581630994239328, - "velocityY": -1.3275170011332822, - "timestamp": 0.9530792507039196 - }, - { - "x": 4.176215433316135, - "y": 7.093067133082196, - "heading": -0.40438880402277044, - "angularVelocity": 1.6982949134456653, - "velocityX": 2.848903377940195, - "velocityY": -1.2192614014185588, - "timestamp": 0.9946318514039973 - }, - { - "x": 4.305678352995595, - "y": 7.043923680276767, - "heading": -0.36279336127293804, - "angularVelocity": 1.0010310317289781, - "velocityX": 3.1156393943645972, - "velocityY": -1.1826805537429936, - "timestamp": 1.036184452104075 - }, - { - "x": 4.439387798309326, - "y": 6.998125076293945, - "heading": -0.329998270844152, - "angularVelocity": 0.7892427880864705, - "velocityX": 3.2178357806972437, - "velocityY": -1.102183815482256, - "timestamp": 1.0777370528041528 - }, - { - "x": 4.582105276471285, - "y": 6.9549071662449835, - "heading": -0.29404604928784833, - "angularVelocity": 0.8178419640522191, - "velocityX": 3.2465404804473232, - "velocityY": -0.9831220132341564, - "timestamp": 1.1216969168568243 - }, - { - "x": 4.730015121041943, - "y": 6.913657635454265, - "heading": -0.2719754549705928, - "angularVelocity": 0.5020623878820808, - "velocityX": 3.364656551108451, - "velocityY": -0.9383452765298861, - "timestamp": 1.1656567809094958 - }, - { - "x": 4.880746443869425, - "y": 6.874004608231845, - "heading": -0.25724414225603226, - "angularVelocity": 0.33510824093785235, - "velocityX": 3.4288396034819466, - "velocityY": -0.9020279765858785, - "timestamp": 1.2096166449621673 - }, - { - "x": 5.032791573379627, - "y": 6.835936031206198, - "heading": -0.24533256299842895, - "angularVelocity": 0.2709648793119881, - "velocityX": 3.4587261081614047, - "velocityY": -0.8659848670149453, - "timestamp": 1.2535765090148387 - }, - { - "x": 5.185019069456891, - "y": 6.799899076081779, - "heading": -0.23244532732058962, - "angularVelocity": 0.2931591340322468, - "velocityX": 3.462874587029316, - "velocityY": -0.8197694852112772, - "timestamp": 1.2975363730675102 - }, - { - "x": 5.336755073872842, - "y": 6.765960464892938, - "heading": -0.21656435212756764, - "angularVelocity": 0.36126078947639223, - "velocityX": 3.451694123397726, - "velocityY": -0.772036309034291, - "timestamp": 1.3414962371201817 - }, - { - "x": 5.488275259993641, - "y": 6.732179135791566, - "heading": -0.20012870270564123, - "angularVelocity": 0.373878531614867, - "velocityX": 3.446784683847838, - "velocityY": -0.7684584524844359, - "timestamp": 1.3854561011728532 + "x": 2.8454680992564305, + "y": 7.886072511387067, + "heading": -1.5709999999999997, + "angularVelocity": 4.2271698321650397e-19, + "velocityX": 3.6484181749177784, + "velocityY": -0.05934850292215207, + "timestamp": 0.46836579426789093 }, { - "x": 5.638672828674316, - "y": 6.7, - "heading": -0.17939789980934723, - "angularVelocity": 0.47158478177886626, - "velocityX": 3.4212473564629415, - "velocityY": -0.7320117221700879, - "timestamp": 1.4294159652255247 - }, - { - "x": 5.788087679940991, - "y": 6.680101103459369, - "heading": -0.13240228550987074, - "angularVelocity": 1.035498011906759, - "velocityX": 3.292196170689038, - "velocityY": -0.43845086640736536, - "timestamp": 1.474800518125784 - }, - { - "x": 5.943840213221069, - "y": 6.6639222262581566, - "heading": -0.10350726387283918, - "angularVelocity": 0.6366708448254484, - "velocityX": 3.431840203920755, - "velocityY": -0.35648422574013283, - "timestamp": 1.5201850710260432 - }, - { - "x": 6.103323466800933, - "y": 6.650235955506569, - "heading": -0.08560921386021843, - "angularVelocity": 0.394364356787972, - "velocityX": 3.514042628785103, - "velocityY": -0.30156231309943976, - "timestamp": 1.5655696239263024 - }, - { - "x": 6.264603851666927, - "y": 6.6384471876022735, - "heading": -0.07296590604456112, - "angularVelocity": 0.27858174219417636, - "velocityX": 3.5536404913019175, - "velocityY": -0.25975287076645964, - "timestamp": 1.6109541768265616 - }, - { - "x": 6.426334391670966, - "y": 6.628509153162114, - "heading": -0.061368144257997005, - "angularVelocity": 0.2555442555984282, - "velocityX": 3.563559177490868, - "velocityY": -0.21897394168447357, - "timestamp": 1.6563387297268208 - }, - { - "x": 6.5873241852765965, - "y": 6.620935768108484, - "heading": -0.04698231400096106, - "angularVelocity": 0.31697635732336293, - "velocityX": 3.5472376242071357, - "velocityY": -0.16687142584098535, - "timestamp": 1.70172328262708 - }, - { - "x": 6.747933405474403, - "y": 6.61399464660675, - "heading": -0.03144813807234412, - "angularVelocity": 0.3422789239042308, - "velocityX": 3.538852096897087, - "velocityY": -0.1529401758568234, - "timestamp": 1.7471078355273393 - }, - { - "x": 6.908430702545003, - "y": 6.607020256679557, - "heading": -0.015778635190030303, - "angularVelocity": 0.3452607083459035, - "velocityX": 3.5363859906987134, - "velocityY": -0.15367320996910241, - "timestamp": 1.7924923884275985 - }, - { - "x": 7.068823337554932, - "y": 6.6, - "heading": 9.122324332035218e-19, - "angularVelocity": 0.3476653218267468, - "velocityX": 3.5340798743224577, - "velocityY": -0.15468383471761574, - "timestamp": 1.8378769413278577 - }, - { - "x": 7.2863547293224356, - "y": 6.583697525493296, - "heading": 5.055756708499563e-13, - "angularVelocity": 8.456864118207454e-12, - "velocityX": 3.6386971266675183, - "velocityY": -0.27269520349740567, - "timestamp": 1.8976597108728264 - }, - { - "x": 7.503885953162418, - "y": 6.56739281040261, - "heading": 5.055557142423875e-13, - "angularVelocity": -3.3381871949908477e-16, - "velocityX": 3.63869431770564, - "velocityY": -0.2727326822559135, - "timestamp": 1.957442480417795 - }, - { - "x": 7.721417177009884, - "y": 6.551088095411767, - "heading": 5.055489054490857e-13, - "angularVelocity": -1.1389223621556497e-16, - "velocityX": 3.6386943178308213, - "velocityY": -0.27273268058578987, - "timestamp": 2.0172252499627636 - }, - { - "x": 7.93894840606763, - "y": 6.534783449936009, - "heading": 5.055544745362588e-13, - "angularVelocity": 9.315538919737292e-17, - "velocityX": 3.638694404984348, - "velocityY": -0.272731517791152, - "timestamp": 2.077008019507732 - }, - { - "x": 8.136688093850118, - "y": 6.522083274226791, - "heading": 3.612634459265275e-13, - "angularVelocity": -2.4135888937230423e-12, - "velocityX": 3.30763678711385, - "velocityY": -0.21243873119102055, - "timestamp": 2.1367907890527005 - }, - { - "x": 8.306183694624885, - "y": 6.511500099763441, - "heading": 2.4088629871582263e-13, - "angularVelocity": -2.0135759538568424e-12, - "velocityX": 2.835191511950123, - "velocityY": -0.17702716926470988, - "timestamp": 2.196573558597669 - }, - { - "x": 8.447435190810417, - "y": 6.5030339231865915, - "heading": 1.445467303198235e-13, - "angularVelocity": -1.611493899150014e-12, - "velocityX": 2.362745942696488, - "velocityY": -0.1416156635312972, - "timestamp": 2.2563563281426373 - }, - { - "x": 8.56044257654444, - "y": 6.49668474335296, - "heading": 7.227055213753e-14, - "angularVelocity": -1.2089800912943037e-12, - "velocityX": 1.8903002753832105, - "velocityY": -0.1062041769218409, - "timestamp": 2.3161390976876057 - }, - { - "x": 8.645205848895236, - "y": 6.492452559683135, - "heading": 2.4085120103106565e-14, - "angularVelocity": -8.060086944982917e-13, - "velocityX": 1.4178545590303353, - "velocityY": -0.07079270000432428, - "timestamp": 2.375921867232574 + "x": 3.000000000000003, + "y": 7.87, + "heading": -1.5709999999999997, + "angularVelocity": 2.1135793766278028e-19, + "velocityX": 3.6293233386873958, + "velocityY": -0.3774776625911161, + "timestamp": 0.5109445028376992 + }, + { + "x": 3.0933267855298086, + "y": 7.854947925249578, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450956207458e-19, + "velocityX": 3.6023482406759033, + "velocityY": -0.580999492137869, + "timestamp": 0.5368517109404605 + }, + { + "x": 3.1856594234462645, + "y": 7.8346713899397304, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524509613572167e-19, + "velocityX": 3.563974842453837, + "velocityY": -0.7826599929029426, + "timestamp": 0.5627589190432217 + }, + { + "x": 3.2770421386792723, + "y": 7.8104711155350515, + "heading": -1.5709999999999997, + "angularVelocity": 2.652450981385339e-19, + "velocityX": 3.527308495401703, + "velocityY": -0.9341135605458323, + "timestamp": 0.588666127145983 + }, + { + "x": 3.368397018870737, + "y": 7.7861659763653845, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451746814082e-19, + "velocityX": 3.5262340823876155, + "velocityY": -0.9381612666736713, + "timestamp": 0.6145733352487442 + }, + { + "x": 3.4597414869906253, + "y": 7.761821735388127, + "heading": -1.5709999999999997, + "angularVelocity": 2.652451049308022e-19, + "velocityX": 3.5258321837525024, + "velocityY": -0.9396705689278512, + "timestamp": 0.6404805433515055 + }, + { + "x": 3.5510841613375907, + "y": 7.7374707648012215, + "heading": -1.5709999999999997, + "angularVelocity": 2.6524500373464137e-19, + "velocityX": 3.525762945380196, + "velocityY": -0.9399303271237421, + "timestamp": 0.6663877514542668 + }, + { + "x": 3.6424273103238014, + "y": 7.7131215747012565, + "heading": -1.5709999999999997, + "angularVelocity": 2.6530371780360815e-19, + "velocityX": 3.5257812661210104, + "velocityY": -0.9398616015809562, + "timestamp": 0.692294959557028 + }, + { + "x": 3.7337711953747554, + "y": 7.688775146038041, + "heading": -1.5709999999999997, + "angularVelocity": 2.651560076415564e-19, + "velocityX": 3.5258096777019814, + "velocityY": -0.9397550120662235, + "timestamp": 0.7182021676597893 + }, + { + "x": 3.8251143320372014, + "y": 7.664425909712902, + "heading": -1.5709999999999997, + "angularVelocity": 2.6527560956628585e-19, + "velocityX": 3.525780790432169, + "velocityY": -0.9398633858408949, + "timestamp": 0.7441093757625505 + }, + { + "x": 3.9164615673273144, + "y": 7.640092054070024, + "heading": -1.5709999999999997, + "angularVelocity": 2.651878932000989e-19, + "velocityX": 3.525938994575509, + "velocityY": -0.9392697023305191, + "timestamp": 0.7700165838653118 + }, + { + "x": 4.007822648813717, + "y": 7.615810235461623, + "heading": -1.5709999999999997, + "angularVelocity": 2.653022563334735e-19, + "velocityX": 3.526473448007752, + "velocityY": -0.9372611094217806, + "timestamp": 0.7959237919680731 + }, + { + "x": 4.099836826324463, + "y": 7.594134330749512, + "heading": -1.5709999999999997, + "angularVelocity": 8.821604445268149e-19, + "velocityX": 3.551682494916745, + "velocityY": -0.8366746669941396, + "timestamp": 0.8218310000708343 + }, + { + "x": 4.314086123865472, + "y": 7.565866389645391, + "heading": -1.5166611039162066, + "angularVelocity": 0.8444409025240843, + "velocityX": 3.3294910868420495, + "velocityY": -0.4392913257079289, + "timestamp": 0.8861799627058939 + }, + { + "x": 4.512621682190808, + "y": 7.550271754810037, + "heading": -1.4174555506263589, + "angularVelocity": 1.5416806927054159, + "velocityX": 3.085295398641018, + "velocityY": -0.24234477443848973, + "timestamp": 0.9505289253409536 + }, + { + "x": 4.703674537259271, + "y": 7.533864741231446, + "heading": -1.3046929681948032, + "angularVelocity": 1.7523605325406573, + "velocityX": 2.9690121991853746, + "velocityY": -0.2549693562540318, + "timestamp": 1.0148778879760132 + }, + { + "x": 4.868121057502234, + "y": 7.5183747890499655, + "heading": -1.1331539755708713, + "angularVelocity": 2.6657615849509235, + "velocityX": 2.555542677129605, + "velocityY": -0.2407179781503281, + "timestamp": 1.0792268506110727 + }, + { + "x": 5.005308136079898, + "y": 7.491356941262662, + "heading": -0.9056990534597417, + "angularVelocity": 3.534710006145519, + "velocityX": 2.131923701018079, + "velocityY": -0.4198645429702136, + "timestamp": 1.1435758132461322 + }, + { + "x": 5.1255280916701365, + "y": 7.4382119450007, + "heading": -0.6380105243345551, + "angularVelocity": 4.159950963671538, + "velocityX": 1.8682500955315844, + "velocityY": -0.8258873816434845, + "timestamp": 1.2079247758811917 + }, + { + "x": 5.274137127844687, + "y": 7.377257884535905, + "heading": -0.4258350917264682, + "angularVelocity": 3.2972626740101054, + "velocityX": 2.309423960995877, + "velocityY": -0.9472423170300921, + "timestamp": 1.2722737385162513 + }, + { + "x": 5.448926084967563, + "y": 7.329899834255518, + "heading": -0.26459184239693584, + "angularVelocity": 2.505762994874233, + "velocityX": 2.7162668979480253, + "velocityY": -0.7359567014145671, + "timestamp": 1.3366227011513108 + }, + { + "x": 5.647781118238161, + "y": 7.297534713393957, + "heading": -0.1613424754389799, + "angularVelocity": 1.6045226329976747, + "velocityX": 3.090260124291229, + "velocityY": -0.5029625892352149, + "timestamp": 1.4009716637863703 + }, + { + "x": 5.854011535644531, + "y": 7.282280921936035, + "heading": -0.07382990470356181, + "angularVelocity": 1.359968632778447, + "velocityX": 3.2048755560510305, + "velocityY": -0.23704797767091085, + "timestamp": 1.4653206264214298 + }, + { + "x": 5.983589932246309, + "y": 7.284960854996629, + "heading": -0.027638204496835066, + "angularVelocity": 1.1636335547284058, + "velocityX": 3.264261102719684, + "velocityY": 0.06751126327279944, + "timestamp": 1.5050167146092823 + }, + { + "x": 6.11925745255698, + "y": 7.297037313900181, + "heading": -0.001860333184984073, + "angularVelocity": 0.6493806440077139, + "velocityX": 3.4176546482011396, + "velocityY": 0.3042228958778286, + "timestamp": 1.5447128027971349 + }, + { + "x": 6.262128938331292, + "y": 7.316680359638158, + "heading": -0.000009480075392796495, + "angularVelocity": 0.046625579353406145, + "velocityX": 3.599132617252429, + "velocityY": 0.49483580460253374, + "timestamp": 1.5844088909849874 + }, + { + "x": 6.405225952749555, + "y": 7.339125340175403, + "heading": -0.000007943471062431687, + "angularVelocity": 0.00003870921293486644, + "velocityX": 3.6048139993312827, + "velocityY": 0.5654204623651486, + "timestamp": 1.62410497917284 + }, + { + "x": 6.548321378993718, + "y": 7.3615804485544425, + "heading": -0.000006409115010503546, + "angularVelocity": 0.00003865257565632945, + "velocityX": 3.6047739910041234, + "velocityY": 0.5656755968668977, + "timestamp": 1.6638010673606924 + }, + { + "x": 6.691417142733098, + "y": 7.384033405453259, + "heading": -0.000004874435347527005, + "angularVelocity": 0.00003866072787080933, + "velocityX": 3.604782492980222, + "velocityY": 0.5656213980739361, + "timestamp": 1.703497155548545 + }, + { + "x": 6.83451270884544, + "y": 7.40648762220655, + "heading": -0.0000033399372412399076, + "angularVelocity": 0.000038656154204017926, + "velocityX": 3.6047775144789, + "velocityY": 0.5656531355692576, + "timestamp": 1.7431932437363975 + }, + { + "x": 6.977608912847965, + "y": 7.428937772164743, + "heading": -0.0000018048282304933975, + "angularVelocity": 0.00003867154374202805, + "velocityX": 3.6047935838250287, + "velocityY": 0.5655506873111692, + "timestamp": 1.78288933192425 + }, + { + "x": 7.12091588973999, + "y": 7.45, + "heading": 5.756100309873575e-19, + "angularVelocity": 0.000045466148249922065, + "velocityX": 3.6101032478025736, + "velocityY": 0.5305869872012366, + "timestamp": 1.8225854201121026 + }, + { + "x": 7.357842782347066, + "y": 7.451140658234923, + "heading": 1.3946076252127016e-14, + "angularVelocity": 2.1477158274944538e-13, + "velocityX": 3.6488588704771803, + "velocityY": 0.017567026152596314, + "timestamp": 1.8875171976944158 + }, + { + "x": 7.594772307901899, + "y": 7.450909424040305, + "heading": 1.3956817600616414e-14, + "angularVelocity": 1.6542514133334603e-16, + "velocityX": 3.648899419925491, + "velocityY": -0.003561186883034476, + "timestamp": 1.9524489752767291 + }, + { + "x": 7.831701831188383, + "y": 7.450675877221787, + "heading": 1.3958919096191331e-14, + "angularVelocity": 3.236467023043882e-17, + "velocityX": 3.6488993849911218, + "velocityY": -0.00359680309416508, + "timestamp": 2.0173807528590424 + }, + { + "x": 8.06023649674366, + "y": 7.450480884382308, + "heading": 1.3831887827184638e-14, + "angularVelocity": -1.9563805854348244e-15, + "velocityX": 3.519612030728179, + "velocityY": -0.0030030417576737706, + "timestamp": 2.0823125304413557 + }, + { + "x": 8.255358856071796, + "y": 7.450318830701623, + "heading": 1.0574442032523106e-14, + "angularVelocity": -5.016720496430017e-14, + "velocityX": 3.0050364643226115, + "velocityY": -0.002495753030620786, + "timestamp": 2.147244308023669 + }, + { + "x": 8.417068891314898, + "y": 7.450189715578673, + "heading": 6.865176887322045e-15, + "angularVelocity": -5.712557523159656e-14, + "velocityX": 2.4904606228915314, + "velocityY": -0.0019884735603726703, + "timestamp": 2.2121760856059822 + }, + { + "x": 8.54536659652034, + "y": 7.450093538813877, + "heading": 3.601695099473894e-15, + "angularVelocity": -5.026016396528296e-14, + "velocityX": 1.975884689785358, + "velocityY": -0.001481197163799284, + "timestamp": 2.2771078631882955 + }, + { + "x": 8.640251968711807, + "y": 7.450030300307574, + "heading": 1.2253067804043182e-15, + "angularVelocity": -3.659823290767036e-14, + "velocityX": 1.4613087108416603, + "velocityY": -0.0009739223021094422, + "timestamp": 2.3420396407706088 }, { "x": 8.701725006103516, - "y": 6.490337371826172, - "heading": -6.207507223536792e-26, - "angularVelocity": -4.028772886652685e-13, - "velocityX": 0.9454088132495556, - "velocityY": -0.035381228957146495, - "timestamp": 2.4357046367775426 + "y": 7.45, + "heading": -1.9088649305256032e-26, + "angularVelocity": -1.8870679751086276e-14, + "velocityX": 0.9467327043954562, + "velocityY": -0.00046664836083104116, + "timestamp": 2.406971418352922 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 1.0661056930411766e-25, - "angularVelocity": 3.714877122041656e-24, - "velocityX": 0.47336647327631065, - "velocityY": 3.668684939131766e-15, - "timestamp": 2.4954363573212164 + "y": 7.45, + "heading": 2.623532798918036e-26, + "angularVelocity": 9.781191839132487e-25, + "velocityX": 0.4733664733221421, + "velocityY": 5.037124173048106e-15, + "timestamp": 2.4667031388908103 }, { "x": 8.73, - "y": 6.490337371826172, - "heading": 5.321036058198176e-26, - "angularVelocity": -3.1783468612327103e-27, - "velocityX": -2.6405211555124906e-21, - "velocityY": -7.26970172585428e-20, - "timestamp": 2.55516807786489 + "y": 7.45, + "heading": 1.3100764528202313e-26, + "angularVelocity": -5.658464945099905e-28, + "velocityX": 7.815178981981403e-20, + "velocityY": 1.9973998116523263e-19, + "timestamp": 2.5264348594286985 } ], "constraints": [ @@ -5807,7 +5809,7 @@ { "scope": [ 0, - 1 + 2 ], "type": "ZeroAngularVelocity" } @@ -5829,17 +5831,17 @@ "controlIntervalCount": 11 }, { - "x": 13.633, - "y": 7.7, + "x": 13.513909339904785, + "y": 7.847515106201172, "heading": -0.9002702999287823, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { - "x": 12.325, - "y": 7.617, + "x": 12.091078758239746, + "y": 7.594134330749512, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -5849,7 +5851,7 @@ { "x": 10.317412376403809, "y": 7.418716907501221, - "heading": -3.141685307179586, + "heading": 3.1415, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -5862,10 +5864,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": 0, "isInitialGuess": false, @@ -5879,739 +5881,757 @@ "x": 14.65, "y": 7.9, "heading": -1.571, - "angularVelocity": -4.2261388604687027e-29, - "velocityX": -1.358742071286637e-27, - "velocityY": -4.102470365728161e-28, + "angularVelocity": 5.121565466053403e-29, + "velocityX": -7.318157607438789e-28, + "velocityY": -7.303102655433029e-29, "timestamp": 0 }, { - "x": 14.61609999625719, - "y": 7.893333332597285, - "heading": -1.571, - "angularVelocity": 5.482485458181477e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.06602728549153251 - }, - { - "x": 14.548299990175126, - "y": 7.879999998067872, - "heading": -1.571, - "angularVelocity": 5.482485455324769e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.13205457098306503 - }, - { - "x": 14.446599984093062, - "y": 7.859999996871792, - "heading": -1.571, - "angularVelocity": 5.4824854577856795e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.19808185647459753 - }, - { - "x": 14.31099998268951, - "y": 7.833333329929108, - "heading": -1.571, - "angularVelocity": 5.482485457103134e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.26410914196613006 - }, - { - "x": 14.141499999999999, - "y": 7.800000000000001, - "heading": -1.571, - "angularVelocity": 5.482485456843769e-19, - "velocityX": -2.567120265927734, - "velocityY": -0.5048417435452771, - "timestamp": 0.3301364274576626 - }, - { - "x": 13.97200001731049, - "y": 7.766666670070894, - "heading": -1.571, - "angularVelocity": 5.482485456705774e-19, - "velocityX": -2.5671202659277337, - "velocityY": -0.5048417435452771, - "timestamp": 0.3961637129491951 - }, - { - "x": 13.836400015906937, - "y": 7.740000003128209, + "x": 14.612130307338697, + "y": 7.8982505033556425, "heading": -1.571, - "angularVelocity": 5.48248545665747e-19, - "velocityX": -2.053696443736754, - "velocityY": -0.4038734402628818, - "timestamp": 0.46219099844072764 + "angularVelocity": 2.4796809944092742e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.06916416332818838 }, { - "x": 13.734700009824873, - "y": 7.720000001932129, + "x": 14.53639092351289, + "y": 7.894751510136077, "heading": -1.571, - "angularVelocity": 5.482485455640794e-19, - "velocityX": -1.5402724089741158, - "velocityY": -0.3029050951768173, - "timestamp": 0.5282182839322601 + "angularVelocity": 2.4796809946674783e-18, + "velocityX": -1.0950668696217758, + "velocityY": -0.05058968476151164, + "timestamp": 0.13832832665637676 }, { - "x": 13.666900003742809, - "y": 7.7066666674027156, - "heading": -1.571, - "angularVelocity": 5.482485458874598e-19, - "velocityX": -1.0268483033542253, - "velocityY": -0.20193673615618984, - "timestamp": 0.5942455694237926 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 5.482485458591381e-19, - "velocityX": -0.513424162305703, - "velocityY": -0.10096837016827985, - "timestamp": 0.6602728549153251 - }, - { - "x": 13.633, - "y": 7.7, - "heading": -1.571, - "angularVelocity": 1.8274951520571554e-19, - "velocityX": 7.923170915113678e-19, - "velocityY": -2.3856267514882045e-18, - "timestamp": 0.7263001404068575 - }, - { - "x": 13.61789791542167, - "y": 7.699041687293577, - "heading": -1.571, - "angularVelocity": 3.6283789505269005e-19, - "velocityX": -0.3456036812103074, - "velocityY": -0.021930508822978256, - "timestamp": 0.7699978363499157 - }, - { - "x": 13.587693746684298, - "y": 7.697125061907338, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378947932204e-19, - "velocityX": -0.6912073528254145, - "velocityY": -0.04386101703708676, - "timestamp": 0.8136955322929739 - }, - { - "x": 13.542387494311997, - "y": 7.694250123874538, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378952839068e-19, - "velocityX": -1.036811012446521, - "velocityY": -0.06579152449010808, - "timestamp": 0.857393228236032 - }, - { - "x": 13.48197915897862, - "y": 7.69041687323794, - "heading": -1.5709999999999997, - "angularVelocity": 3.628378951779324e-19, - "velocityX": -1.3824146566467697, - "velocityY": -0.08772203096458873, - "timestamp": 0.9010909241790902 - }, - { - "x": 13.406468741582641, - "y": 7.685625310054556, - "heading": -1.5709999999999997, - "angularVelocity": 3.6283789513575714e-19, - "velocityX": -1.7280182802858743, - "velocityY": -0.10965253613434846, - "timestamp": 0.9447886201221484 - }, - { - "x": 13.315856243381926, - "y": 7.679875434404205, + "x": 14.422781851017241, + "y": 7.889503020456549, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789489243236e-19, - "velocityX": -2.073621875139378, - "velocityY": -0.13158303947749891, - "timestamp": 0.9884863160652065 + "angularVelocity": 2.479680994653865e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.20749248998456515 }, { - "x": 13.210141666263272, - "y": 7.673167246406615, + "x": 14.271303094841082, + "y": 7.882505034547556, "heading": -1.5709999999999997, - "angularVelocity": 3.628378951941211e-19, - "velocityX": -2.419225426814483, - "velocityY": -0.15351354008073578, - "timestamp": 1.0321840120082648 + "angularVelocity": 2.479680994579454e-18, + "velocityX": -2.190133573327318, + "velocityY": -0.10117936185805763, + "timestamp": 0.2766566533127535 }, { - "x": 13.089325013371338, - "y": 7.6655007462613325, + "x": 14.081954669952392, + "y": 7.873757553100586, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789509770827e-19, - "velocityX": -2.7648289065256026, - "velocityY": -0.17544403611745055, - "timestamp": 1.075881707951323 + "angularVelocity": 2.479680994607856e-18, + "velocityX": -2.7376666726989773, + "velocityY": -0.12647418874225766, + "timestamp": 0.3458208166409419 }, { - "x": 12.953406290995442, - "y": 7.656875934367449, + "x": 13.892606245063702, + "y": 7.865010071653616, "heading": -1.5709999999999997, - "angularVelocity": 3.628378950782348e-19, - "velocityX": -3.110432242308825, - "velocityY": -0.19737452302112604, - "timestamp": 1.1195794038943812 + "angularVelocity": 2.4796809946116294e-18, + "velocityX": -2.737666672698978, + "velocityY": -0.12647418874225766, + "timestamp": 0.41498497996913025 }, { - "x": 12.802385518003497, - "y": 7.647292811922241, + "x": 13.741127488887543, + "y": 7.858012085744623, "heading": -1.5709999999999997, - "angularVelocity": 3.6283789514392664e-19, - "velocityX": -3.4560351463092576, - "velocityY": -0.21930498252574065, - "timestamp": 1.1632770998374393 + "angularVelocity": 2.479680994536425e-18, + "velocityX": -2.1901335733273184, + "velocityY": -0.10117936185805763, + "timestamp": 0.4841491432973186 }, { - "x": 12.643257011655002, - "y": 7.637195207926121, + "x": 13.627518416391894, + "y": 7.852763596065095, "heading": -1.5709999999999997, - "angularVelocity": 3.6283776976177666e-19, - "velocityX": -3.641576584629373, - "velocityY": -0.23107863648642077, - "timestamp": 1.2069747957804975 - }, - { - "x": 12.484128505306387, - "y": 7.627097603929993, - "heading": -1.5709999999999997, - "angularVelocity": 3.628377701929186e-19, - "velocityX": -3.641576584632129, - "velocityY": -0.23107863648659568, - "timestamp": 1.2506724917235557 - }, - { - "x": 12.325, - "y": 7.617, - "heading": -1.5709999999999997, - "angularVelocity": 7.36418634228573e-19, - "velocityX": -3.6415765607812913, - "velocityY": -0.23107863497312509, - "timestamp": 1.2943701876666138 - }, - { - "x": 12.229388489581337, - "y": 7.6109112417716736, - "heading": -1.5863134997517452, - "angularVelocity": -0.5552849027741624, - "velocityX": -3.466982017671504, - "velocityY": -0.22078529243098394, - "timestamp": 1.3219479224028547 - }, - { - "x": 12.138013718648649, - "y": 7.607167866658879, - "heading": -1.6148419963350573, - "angularVelocity": -1.0344757049904476, - "velocityX": -3.313353029413532, - "velocityY": -0.13573903544277124, - "timestamp": 1.3495256571390957 - }, - { - "x": 12.050416525228757, - "y": 7.6063106068200685, - "heading": -1.6545433874529916, - "angularVelocity": -1.4396175573391428, - "velocityX": -3.1763737760802657, - "velocityY": -0.03108521591825106, - "timestamp": 1.3771033918753366 - }, - { - "x": 11.966360780867383, - "y": 7.607509934453374, - "heading": -1.7036222302241029, - "angularVelocity": -1.7796546105222553, - "velocityX": -3.0479568088277835, - "velocityY": 0.04348898286151133, - "timestamp": 1.4046811266115775 - }, - { - "x": 11.885408562298633, - "y": 7.609633528647565, - "heading": -1.7598323716256097, - "angularVelocity": -2.038243602642159, - "velocityX": -2.9354194368390085, - "velocityY": 0.07700393866649435, - "timestamp": 1.4322588613478184 - }, - { - "x": 11.807516118182633, - "y": 7.611964238202936, - "heading": -1.8223777210675893, - "angularVelocity": -2.2679654453193687, - "velocityX": -2.8244685381514696, - "velocityY": 0.08451417702220176, - "timestamp": 1.4598365960840594 - }, - { - "x": 11.73334817309539, - "y": 7.614170969229463, - "heading": -1.8925164168284103, - "angularVelocity": -2.5433088116786786, - "velocityX": -2.6894139709660934, - "velocityY": 0.08001857468104064, - "timestamp": 1.4874143308203003 - }, - { - "x": 11.663695028307295, - "y": 7.615711030101031, - "heading": -1.9717573083827227, - "angularVelocity": -2.873364774597518, - "velocityX": -2.5257021816429535, - "velocityY": 0.05584435727941134, - "timestamp": 1.5149920655565412 - }, - { - "x": 11.598996797656051, - "y": 7.61568305023367, - "heading": -2.0607742017770874, - "angularVelocity": -3.2278537104564915, - "velocityX": -2.3460313644334034, - "velocityY": -0.0010145817859481632, - "timestamp": 1.5425698002927821 - }, - { - "x": 11.539114067010884, - "y": 7.613037677777285, - "heading": -2.1591584444474705, - "angularVelocity": -3.5675244399638015, - "velocityX": -2.171415862031463, - "velocityY": -0.09592421138591448, - "timestamp": 1.570147535029023 - }, - { - "x": 11.483508926014887, - "y": 7.606776074097583, - "heading": -2.266083388472652, - "angularVelocity": -3.8772199764711472, - "velocityX": -2.0163056004352273, - "velocityY": -0.22705286491397708, - "timestamp": 1.597725269765264 - }, - { - "x": 11.431087870109415, - "y": 7.595775934313344, - "heading": -2.379919574340892, - "angularVelocity": -4.127829459416678, - "velocityX": -1.900847056759369, - "velocityY": -0.3988775687867351, - "timestamp": 1.6253030045015049 - }, - { - "x": 11.377404486650294, - "y": 7.579105655561354, - "heading": -2.4911780043079133, - "angularVelocity": -4.034357101158641, - "velocityX": -1.9466204883235783, - "velocityY": -0.6044832511236621, - "timestamp": 1.6528807392377458 - }, - { - "x": 11.319370240403567, - "y": 7.55896629351811, - "heading": -2.5937282135555715, - "angularVelocity": -3.718587122128468, - "velocityX": -2.1043877171847964, - "velocityY": -0.7302761534208634, - "timestamp": 1.6804584739739867 - }, - { - "x": 11.256272025403964, - "y": 7.536866509262458, - "heading": -2.6862489609900964, - "angularVelocity": -3.3549074396216714, - "velocityX": -2.288012978697651, - "velocityY": -0.8013632906045023, - "timestamp": 1.7080362087102277 - }, - { - "x": 11.187885977180416, - "y": 7.514090907716094, - "heading": -2.7680211777317423, - "angularVelocity": -2.965153502408188, - "velocityX": -2.4797558203240193, - "velocityY": -0.8258691935430503, - "timestamp": 1.7356139434464686 - }, - { - "x": 11.114289943184776, - "y": 7.491935954019794, - "heading": -2.838743341274155, - "angularVelocity": -2.5644660164736934, - "velocityX": -2.66867582488287, - "velocityY": -0.8033637972151884, - "timestamp": 1.7631916781827095 - }, - { - "x": 11.03615204188367, - "y": 7.47266971293565, - "heading": -2.8998457718181054, - "angularVelocity": -2.215643566389652, - "velocityX": -2.8333690946131496, - "velocityY": -0.6986157952569366, - "timestamp": 1.7907694129189504 - }, - { - "x": 10.95383190394823, - "y": 7.456821193599831, - "heading": -2.9513961589840805, - "angularVelocity": -1.8692756188648758, - "velocityX": -2.985021747535358, - "velocityY": -0.5746853208720242, - "timestamp": 1.8183471476551913 - }, - { - "x": 10.867220309314249, - "y": 7.443764913233828, - "heading": -2.991973638967047, - "angularVelocity": -1.4713855351450116, - "velocityX": -3.1406348440997536, - "velocityY": -0.47343556281455274, - "timestamp": 1.8459248823914323 - }, - { - "x": 10.77690563773812, - "y": 7.432954012684261, - "heading": -3.0224568962493583, - "angularVelocity": -1.1053575492644694, - "velocityX": -3.2749126220813625, - "velocityY": -0.39201553909209025, - "timestamp": 1.8735026171276732 - }, - { - "x": 10.68603576950712, - "y": 7.424800287046038, - "heading": -3.0521033841408234, - "angularVelocity": -1.0750153402739042, - "velocityX": -3.295044683694906, - "velocityY": -0.2956633572774275, - "timestamp": 1.901080351863914 - }, - { - "x": 10.596200893845815, - "y": 7.419761050330992, - "heading": -3.0855587154803814, - "angularVelocity": -1.2131283319507546, - "velocityX": -3.257514676985064, - "velocityY": -0.18272844971648752, - "timestamp": 1.928658086600155 - }, - { - "x": 10.506861440016637, - "y": 7.417879567543994, - "heading": -3.1208586855613984, - "angularVelocity": -1.2800170289051036, - "velocityX": -3.2395501183704285, - "velocityY": -0.0682247039140415, - "timestamp": 1.956235821336396 - }, - { - "x": 10.412835700689799, - "y": 7.416682265603839, + "angularVelocity": 2.47968099468633e-18, + "velocityX": -1.6426002575432905, + "velocityY": -0.07588452497608139, + "timestamp": 0.553313306625507 + }, + { + "x": 13.551779032566087, + "y": 7.849264602845529, + "heading": -1.5709999999999995, + "angularVelocity": 2.479680994669315e-18, + "velocityX": -1.095066869621776, + "velocityY": -0.05058968476151165, + "timestamp": 0.6224774699536955 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 2.4796809942925844e-18, + "velocityX": -0.5475334456315131, + "velocityY": -0.025294842880644937, + "timestamp": 0.6916416332818839 + }, + { + "x": 13.513909339904783, + "y": 7.847515106201172, + "heading": -1.5709999999999995, + "angularVelocity": 8.26560334321477e-19, + "velocityX": 2.2636957131725456e-19, + "velocityY": 2.8360545446294056e-18, + "timestamp": 0.7608057966100723 + }, + { + "x": 13.499332557357375, + "y": 7.8449192409498565, + "heading": -1.5709999999999995, + "angularVelocity": 1.549669581859894e-18, + "velocityX": -0.33723861810938877, + "velocityY": -0.060056189169625046, + "timestamp": 0.8040297388416239 + }, + { + "x": 13.47017899267198, + "y": 7.839727510520138, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695813124185e-18, + "velocityX": -0.6744772267466429, + "velocityY": -0.12011237665243168, + "timestamp": 0.8472536810731754 + }, + { + "x": 13.42644864636038, + "y": 7.831939915003153, + "heading": -1.5709999999999995, + "angularVelocity": 1.5496695815123512e-18, + "velocityX": -1.0117158235437291, + "velocityY": -0.1801685620267154, + "timestamp": 0.890477623304727 + }, + { + "x": 13.368141519080574, + "y": 7.821556454516082, + "heading": -1.5709999999999993, + "angularVelocity": 1.549669581692892e-18, + "velocityX": -1.348954405117744, + "velocityY": -0.2402247446900413, + "timestamp": 0.9337015655362786 + }, + { + "x": 13.295257611709896, + "y": 7.808577129215161, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814706194e-18, + "velocityX": -1.6861929663943322, + "velocityY": -0.3002809237387572, + "timestamp": 0.9769255077678302 + }, + { + "x": 13.207796925476618, + "y": 7.793001939319122, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814452525e-18, + "velocityX": -2.0234314992545284, + "velocityY": -0.3603370977270199, + "timestamp": 1.0201494499993817 + }, + { + "x": 13.105759462223137, + "y": 7.774830885156067, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815528492e-18, + "velocityX": -2.3606699894901486, + "velocityY": -0.42039326412460515, + "timestamp": 1.0633733922309334 + }, + { + "x": 12.989145225020126, + "y": 7.754063967272826, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695815122657e-18, + "velocityX": -2.6979084086848464, + "velocityY": -0.48044941787106793, + "timestamp": 1.106597334462485 + }, + { + "x": 12.857954220008912, + "y": 7.730701186763059, + "heading": -1.5709999999999993, + "angularVelocity": 1.5496695814976101e-18, + "velocityX": -3.035146685797867, + "velocityY": -0.5405055463153154, + "timestamp": 1.1498212766940368 + }, + { + "x": 12.71218646561342, + "y": 7.70474254690774, + "heading": -1.570999999999999, + "angularVelocity": 1.5496695815191512e-18, + "velocityX": -3.372384536667451, + "velocityY": -0.6005615988532015, + "timestamp": 1.1930452189255885 + }, + { + "x": 12.556909538173402, + "y": 7.677090492761939, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692345146303e-18, + "velocityX": -3.592382356245949, + "velocityY": -0.6397392907307838, + "timestamp": 1.2362691611571401 + }, + { + "x": 12.401632610733195, + "y": 7.649438438616105, + "heading": -1.570999999999999, + "angularVelocity": 1.5496692344266034e-18, + "velocityX": -3.592382356250238, + "velocityY": -0.6397392907315478, + "timestamp": 1.2794931033886918 + }, + { + "x": 12.24635568329299, + "y": 7.621786384470271, + "heading": -1.570999999999999, + "angularVelocity": 1.549669234478791e-18, + "velocityX": -3.5923823562502384, + "velocityY": -0.6397392907315479, + "timestamp": 1.3227170456202435 + }, + { + "x": 12.091078758239746, + "y": 7.594134330749512, + "heading": -1.570999999999999, + "angularVelocity": 3.234878008246914e-18, + "velocityX": -3.592382301027084, + "velocityY": -0.6397392808972879, + "timestamp": 1.3659409878517952 + }, + { + "x": 12.00283728057898, + "y": 7.582107515529106, + "heading": -1.5788342162362206, + "angularVelocity": -0.3119507041975876, + "velocityX": -3.513687938342297, + "velocityY": -0.47889582877488296, + "timestamp": 1.3910546232141834 + }, + { + "x": 11.916970328359971, + "y": 7.573753042701851, + "heading": -1.5952402865312374, + "angularVelocity": -0.6532734133600175, + "velocityX": -3.4191366952627433, + "velocityY": -0.3326668045754269, + "timestamp": 1.4161682585765716 + }, + { + "x": 11.833816357634106, + "y": 7.56871710218962, + "heading": -1.6206538515119442, + "angularVelocity": -1.011942899307139, + "velocityX": -3.311108468604305, + "velocityY": -0.20052614603931174, + "timestamp": 1.4412818939389598 + }, + { + "x": 11.753731808376116, + "y": 7.566574800515445, + "heading": -1.6553839157644212, + "angularVelocity": -1.3829166407559692, + "velocityX": -3.188887156413404, + "velocityY": -0.08530432345956382, + "timestamp": 1.466395529301348 + }, + { + "x": 11.677147361800145, + "y": 7.566714891670578, + "heading": -1.6997475676814937, + "angularVelocity": -1.766516526856978, + "velocityX": -3.0495165463240133, + "velocityY": 0.0055782905616100845, + "timestamp": 1.4915091646637362 + }, + { + "x": 11.604396744023525, + "y": 7.568557677988233, + "heading": -1.7537219634693322, + "angularVelocity": -2.1492067957909815, + "velocityX": -2.8968572939289086, + "velocityY": 0.07337791964526899, + "timestamp": 1.5166228000261244 + }, + { + "x": 11.535756559334365, + "y": 7.571432948149072, + "heading": -1.8171150418628605, + "angularVelocity": -2.5242493760370213, + "velocityX": -2.7331839336949435, + "velocityY": 0.1144904001078914, + "timestamp": 1.5417364353885126 + }, + { + "x": 11.471380887760333, + "y": 7.574513868964298, + "heading": -1.889488559627284, + "angularVelocity": -2.881841546237513, + "velocityX": -2.5633752598977027, + "velocityY": 0.12267920477200389, + "timestamp": 1.5668500707509008 + }, + { + "x": 11.411255276796373, + "y": 7.576880729956244, + "heading": -1.9701854617324397, + "angularVelocity": -3.213270438176076, + "velocityX": -2.3941420704876353, + "velocityY": 0.09424605230448276, + "timestamp": 1.591963706113289 + }, + { + "x": 11.35519068850538, + "y": 7.57762868801966, + "heading": -2.058445803348398, + "angularVelocity": -3.5144390822869056, + "velocityX": -2.2324361838505236, + "velocityY": 0.029782946698429406, + "timestamp": 1.6170773414756772 + }, + { + "x": 11.302817578230115, + "y": 7.575911070751928, + "heading": -2.1534608466432936, + "angularVelocity": -3.783404589727689, + "velocityX": -2.0854451981786317, + "velocityY": -0.06839381248247857, + "timestamp": 1.6421909768380654 + }, + { + "x": 11.253523245308234, + "y": 7.570911967645187, + "heading": -2.254232970772999, + "angularVelocity": -4.012645826678452, + "velocityX": -1.9628513439257314, + "velocityY": -0.19905931716328515, + "timestamp": 1.6673046122004536 + }, + { + "x": 11.206151241323777, + "y": 7.561834135279753, + "heading": -2.3588178409636855, + "angularVelocity": -4.164465585385836, + "velocityX": -1.8863061162162815, + "velocityY": -0.36147026244552627, + "timestamp": 1.6924182475628418 + }, + { + "x": 11.158562568853249, + "y": 7.548296021124272, + "heading": -2.4631530925128593, + "angularVelocity": -4.154526019174503, + "velocityX": -1.8949336399871903, + "velocityY": -0.5390742503060728, + "timestamp": 1.71753188292523 + }, + { + "x": 11.108338354901361, + "y": 7.5311193171207975, + "heading": -2.5624670465584027, + "angularVelocity": -3.9545829431864727, + "velocityX": -1.9998782823423, + "velocityY": -0.6839592817038007, + "timestamp": 1.7426455182876182 + }, + { + "x": 11.054321656083031, + "y": 7.51174890982464, + "heading": -2.654640214483575, + "angularVelocity": -3.6702439370135087, + "velocityX": -2.150891260419857, + "velocityY": -0.7713103665237963, + "timestamp": 1.7677591536500064 + }, + { + "x": 10.996033122413428, + "y": 7.491461551309535, + "heading": -2.738672595307348, + "angularVelocity": -3.346085885665274, + "velocityX": -2.3209914784730197, + "velocityY": -0.8078224527158445, + "timestamp": 1.7928727890123946 + }, + { + "x": 10.933381820015002, + "y": 7.471624968268411, + "heading": -2.8141850565003126, + "angularVelocity": -3.006831153809885, + "velocityX": -2.4947125931527254, + "velocityY": -0.7898730213622603, + "timestamp": 1.8179864243747828 + }, + { + "x": 10.866838632540613, + "y": 7.45402686067354, + "heading": -2.88213732233945, + "angularVelocity": -2.7057916888040063, + "velocityX": -2.6496835887816075, + "velocityY": -0.7007391538860639, + "timestamp": 1.843100059737171 + }, + { + "x": 10.797385329302331, + "y": 7.4401695505140575, + "heading": -2.944658830810706, + "angularVelocity": -2.4895443279744143, + "velocityX": -2.765561506172815, + "velocityY": -0.5517843179424147, + "timestamp": 1.8682136950995591 + }, + { + "x": 10.724987539198043, + "y": 7.429906192310285, + "heading": -3.000767776942746, + "angularVelocity": -2.234202468993167, + "velocityX": -2.88280804668823, + "velocityY": -0.40867672305021235, + "timestamp": 1.8933273304619473 + }, + { + "x": 10.649597795782494, + "y": 7.422945580592199, + "heading": -3.0495257230757873, + "angularVelocity": -1.9414929551011741, + "velocityX": -3.0019446538772403, + "velocityY": -0.277164640548885, + "timestamp": 1.9184409658243355 + }, + { + "x": 10.571198566507455, + "y": 7.418912040075103, + "heading": -3.0901902225982423, + "angularVelocity": -1.6192199550436448, + "velocityX": -3.121779389711535, + "velocityY": -0.16061157450510036, + "timestamp": 1.9435546011867237 + }, + { + "x": 10.489548200758582, + "y": 7.417268311990692, + "heading": -3.1214676257157588, + "angularVelocity": -1.2454351059179014, + "velocityX": -3.251236412835276, + "velocityY": -0.06545161864044534, + "timestamp": 1.968668236549112 + }, + { + "x": 10.404255697585379, + "y": 7.417275691573601, "heading": -3.141685307179586, - "angularVelocity": -0.7551969665883731, - "velocityX": -3.4094801558619205, - "velocityY": -0.043415528926000375, - "timestamp": 1.9838135560726369 + "angularVelocity": -0.8050479817886684, + "velocityX": -3.3962627051968566, + "velocityY": 0.00029384765695126695, + "timestamp": 1.9937818719115001 }, { "x": 10.317412376403809, "y": 7.418716907501221, "heading": -3.141685307179586, - "angularVelocity": 5.937778880078681e-19, - "velocityX": -3.4601581746520087, - "velocityY": 0.07377842730165238, - "timestamp": 2.011391290808878 + "angularVelocity": 7.47770773326549e-18, + "velocityX": -3.4580147369515903, + "velocityY": 0.05738778583033072, + "timestamp": 2.0188955072738883 }, { - "x": 10.251727837257341, - "y": 7.420700742643172, + "x": 10.251732343249431, + "y": 7.420561186854179, "heading": -3.141685307179586, - "angularVelocity": -1.9442515737792743e-28, - "velocityX": -3.3048160733761773, - "velocityY": 0.09981359920071599, - "timestamp": 2.03126669013159 + "angularVelocity": 3.1949765996869766e-27, + "velocityX": -3.304533757090645, + "velocityY": 0.09279050400334278, + "timestamp": 2.0387712410331353 }, { - "x": 10.189167330602293, - "y": 7.422886644704941, + "x": 10.189171391771723, + "y": 7.422674521924776, "heading": -3.141685307179586, - "angularVelocity": -2.386183922112865e-28, - "velocityX": -3.1476352066827475, - "velocityY": 0.10998028398210202, - "timestamp": 2.051142089454302 + "angularVelocity": -2.371251182895612e-27, + "velocityX": -3.1476046235828683, + "velocityY": 0.10632739883689808, + "timestamp": 2.0586469747923823 }, { - "x": 10.129736436099503, - "y": 7.425149795146946, + "x": 10.129739097787168, + "y": 7.424900096756528, "heading": -3.141685307179586, - "angularVelocity": 2.7299904854133836e-28, - "velocityX": -2.9901736079776406, - "velocityY": 0.11386691684825133, - "timestamp": 2.0710174887770143 + "angularVelocity": -5.101900895484119e-28, + "velocityX": -2.99019370577468, + "velocityY": 0.11197447393435742, + "timestamp": 2.0785227085516293 }, { - "x": 10.073436091249755, - "y": 7.427423630307786, + "x": 10.073437312690587, + "y": 7.427157718473777, "heading": -3.141685307179586, - "angularVelocity": 1.8386974301791735e-28, - "velocityX": -2.8326648403694086, - "velocityY": 0.1144045019634283, - "timestamp": 2.0908928880997264 + "angularVelocity": -5.203506078458522e-28, + "velocityX": -2.8326896394648293, + "velocityY": 0.11358683631985049, + "timestamp": 2.0983984423108764 }, { - "x": 10.020266165929796, - "y": 7.4296668280040885, + "x": 10.020266158378618, + "y": 7.429398776074464, "heading": -3.141685307179586, - "angularVelocity": 5.650458450043071e-29, - "velocityX": -2.6751626197114717, - "velocityY": 0.11286302528469754, - "timestamp": 2.1107682874224385 + "angularVelocity": 2.645370534116333e-28, + "velocityX": -2.6751794402172218, + "velocityY": 0.11275345241754915, + "timestamp": 2.1182741760701234 }, { - "x": 9.970226259362496, - "y": 7.431851251828943, + "x": 9.970225293830397, + "y": 7.431590675011163, "heading": -3.141685307179586, - "angularVelocity": -4.102009655023591e-29, - "velocityX": -2.5176805635356243, - "velocityY": 0.10990590877633212, - "timestamp": 2.1306436867451506 + "angularVelocity": 5.191709905827532e-29, + "velocityX": -2.5176863986186695, + "velocityY": 0.11028015182983106, + "timestamp": 2.1381499098293704 }, { - "x": 9.923315923027515, - "y": 7.43395651278906, + "x": 9.92331426564673, + "y": 7.433710048067309, "heading": -3.141685307179586, - "angularVelocity": -3.5765307146759774e-29, - "velocityX": -2.3602210739674327, - "velocityY": 0.10592295158126644, - "timestamp": 2.1505190860678627 + "angularVelocity": 1.0273003820442257e-28, + "velocityX": -2.3602161687157612, + "velocityY": 0.1066311856365788, + "timestamp": 2.1580256435886174 }, { - "x": 9.879534728461786, - "y": 7.4359671584530975, + "x": 9.87953261796101, + "y": 7.435739325604849, "heading": -3.141685307179586, - "angularVelocity": -1.1942964906968253e-29, - "velocityX": -2.2027831418560893, - "velocityY": 0.1011625291844662, - "timestamp": 2.170394485390575 + "angularVelocity": -2.0170615068521438e-28, + "velocityX": -2.202768874651166, + "velocityY": 0.10209824513253805, + "timestamp": 2.1779013773478644 }, { - "x": 9.838882286352929, - "y": 7.437871074521549, + "x": 9.838879927126666, + "y": 7.437664817248215, "heading": -3.141685307179586, - "angularVelocity": -5.641467124729511e-31, - "velocityX": -2.0453647973957065, - "velocityY": 0.09579259453048798, - "timestamp": 2.190269884713287 + "angularVelocity": -2.3492440709815457e-28, + "velocityX": -2.045342895349926, + "velocityY": 0.0968765061300322, + "timestamp": 2.1977771111071114 }, { - "x": 9.801358249188596, - "y": 7.439658508993957, + "x": 9.801355810492355, + "y": 7.439475555427537, "heading": -3.141685307179586, - "angularVelocity": 3.5641593543820497e-29, - "velocityX": -1.8879639374819575, - "velocityY": 0.08993200304483205, - "timestamp": 2.210145284035999 + "angularVelocity": 2.8629022838202107e-28, + "velocityX": -1.8879361682359468, + "velocityY": 0.09110296008466975, + "timestamp": 2.2176528448663584 }, { - "x": 9.766962308401824, - "y": 7.441321442796545, + "x": 9.766959925956705, + "y": 7.441162556870241, "heading": -3.141685307179586, - "angularVelocity": 1.9252934621446341e-29, - "velocityX": -1.7305786026380017, - "velocityY": 0.08366794425548009, - "timestamp": 2.230020683358711 + "angularVelocity": -1.5840875871720601e-28, + "velocityX": -1.7305466531340934, + "velocityY": 0.08487744216837698, + "timestamp": 2.2375285786256054 }, { - "x": 9.735694189965349, - "y": 7.442853165687241, + "x": 9.735691968423001, + "y": 7.442718328937222, "heading": -3.141685307179586, - "angularVelocity": -5.183683118127715e-29, - "velocityX": -1.5732070550525459, - "velocityY": 0.07706627000674852, - "timestamp": 2.249896082681423 + "angularVelocity": 9.330544204917246e-29, + "velocityX": -1.5731724882437799, + "velocityY": 0.0782749500383743, + "timestamp": 2.2574043123848524 }, { - "x": 9.707553649885064, - "y": 7.444247980075085, + "x": 9.707551665488573, + "y": 7.444136527128733, "heading": -3.141685307179586, - "angularVelocity": 1.3886630469011578e-29, - "velocityX": -1.4158477836532484, - "velocityY": 0.07017793027434112, - "timestamp": 2.2697714820041353 + "angularVelocity": -6.58238928802494e-29, + "velocityX": -1.4158120286419644, + "velocityY": 0.07135324958012863, + "timestamp": 2.2772800461440994 }, { - "x": 9.682540470094276, - "y": 7.445500987918728, + "x": 9.682538773252615, + "y": 7.445411710024687, "heading": -3.141685307179586, - "angularVelocity": -4.3165247768849716e-30, - "velocityX": -1.258499483942791, - "velocityY": 0.06304315316127049, - "timestamp": 2.2896468813268473 + "angularVelocity": 2.6630221770954893e-29, + "velocityX": -1.2584638403259507, + "velocityY": 0.06415777708639624, + "timestamp": 2.2971557799033464 }, { - "x": 9.660654454881797, - "y": 7.446607933488268, + "x": 9.660653072546591, + "y": 7.446539159318976, "heading": -3.141685307179586, - "angularVelocity": 5.491476754830618e-28, - "velocityX": -1.10116103113811, - "velocityY": 0.05569425557531322, - "timestamp": 2.3095222806495594 + "angularVelocity": -4.1528582058952054e-29, + "velocityX": -1.1011266789504681, + "velocityY": 0.056724914307379, + "timestamp": 2.3170315136625934 }, { - "x": 9.641895427846347, - "y": 7.4475650848138315, + "x": 9.64189436565749, + "y": 7.447514744684453, "heading": -3.141685307179586, - "angularVelocity": -5.338468664029318e-28, - "velocityX": -0.9438314536912185, - "velocityY": 0.04815758969279047, - "timestamp": 2.3293976799722715 + "angularVelocity": 4.9624701627257074e-29, + "velocityX": -0.9437994650322394, + "velocityY": 0.04908424399792036, + "timestamp": 2.3369072474218404 }, { - "x": 9.626263229320996, - "y": 7.44836914261392, + "x": 9.626262473522596, + "y": 7.448334820329731, "heading": -3.141685307179586, - "angularVelocity": 1.3824274242904182e-29, - "velocityX": -0.7865099096393957, - "velocityY": 0.04045492556066521, - "timestamp": 2.3492730792949836 + "angularVelocity": 7.099030390231211e-29, + "velocityX": -0.7864812602262983, + "velocityY": 0.041260144415945804, + "timestamp": 2.3567829811810874 }, { - "x": 9.613757714199988, - "y": 7.449017169178514, + "x": 9.6137572333429, + "y": 7.4489961444732815, "heading": -3.141685307179586, - "angularVelocity": -3.217502623717656e-29, - "velocityX": -0.6291956663591478, - "velocityY": 0.032604455089033264, - "timestamp": 2.3691484786176957 + "angularVelocity": -2.7212817911682713e-28, + "velocityX": -0.6291712462629009, + "velocityY": 0.03327294235078537, + "timestamp": 2.3766587149403344 }, { - "x": 9.60437875010338, - "y": 7.4495065320260005, + "x": 9.604378496555762, + "y": 7.449495815722204, "heading": -3.141685307179586, - "angularVelocity": -3.058650941614844e-29, - "velocityX": -0.47188808357125706, - "velocityY": 0.02462153537345558, - "timestamp": 2.389023877940408 + "angularVelocity": 3.8071659238390546e-28, + "velocityX": -0.47186870687348514, + "velocityY": 0.025139763642160577, + "timestamp": 2.3965344486995814 }, { - "x": 9.59812621582273, - "y": 7.44983485868733, + "x": 9.598126127110815, + "y": 7.449831222141457, "heading": -3.141685307179586, - "angularVelocity": 1.512533584247321e-28, - "velocityX": -0.31458659919880055, - "velocityY": 0.016519248544322844, - "timestamp": 2.40889927726312 + "angularVelocity": -1.0181149112849948e-28, + "velocityX": -0.31457301253283737, + "velocityY": 0.016875171669945208, + "timestamp": 2.4164101824588284 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.7605650970541834e-28, - "velocityX": -0.15729071763386918, - "velocityY": 0.00830882992530101, - "timestamp": 2.428774676585832 + "angularVelocity": -2.710350913224058e-28, + "velocityX": -0.15728360767367736, + "velocityY": 0.008491654224556767, + "timestamp": 2.4362859162180754 }, { "x": 9.595, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.1524356647417298e-29, - "velocityX": 5.1753202830495987e-20, - "velocityY": -1.8305849582326549e-19, - "timestamp": 2.448650075908544 + "angularVelocity": -2.3460450184878346e-27, + "velocityX": 2.2121450356253165e-19, + "velocityY": -1.3122030803241336e-17, + "timestamp": 2.4561616499773224 }, { - "x": 9.515249994687553, + "x": 9.518209324474356, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -2.7318379706310427e-39, - "velocityX": -0.7949898942995212, - "velocityY": -6.391097195939509e-16, - "timestamp": 2.5489658231706116 + "angularVelocity": 1.919174990104461e-28, + "velocityX": -0.7801003830985155, + "velocityY": -5.128926779471519e-16, + "timestamp": 2.5545985655197003 }, { - "x": 9.355749987014017, + "x": 9.364627976334331, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.552617178039786e-38, - "velocityX": -1.5899797591783358, - "velocityY": -1.2780363487427944e-15, - "timestamp": 2.649281570432679 + "angularVelocity": 1.1076124666691254e-36, + "velocityX": -1.5602007366220767, + "velocityY": -1.0126641040061392e-15, + "timestamp": 2.653035481062078 }, { - "x": 9.116499982882113, + "x": 9.134255961402465, + "y": 7.45, + "heading": -3.141685307179586, + "angularVelocity": -2.2303140690094456e-37, + "velocityX": -2.3403010309957435, + "velocityY": -1.5124370881453142e-15, + "timestamp": 2.751472396604456 + }, + { + "x": 8.827093297146337, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.6158839795897883e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9169629139981945e-15, - "timestamp": 2.7495973176947466 + "angularVelocity": 1.3462495199303324e-32, + "velocityX": -3.120401147919862, + "velocityY": -2.0122147466163613e-15, + "timestamp": 2.849909312146834 }, { - "x": 8.7975, + "x": 8.467906702853663, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.1000527102694011e-38, - "velocityX": -3.179959194728896, - "velocityY": -2.5558892876024355e-15, - "timestamp": 2.849913064956814 + "angularVelocity": 2.849468284749269e-32, + "velocityX": -3.648901352847056, + "velocityY": -2.3373733239441694e-15, + "timestamp": 2.948346227689212 }, { - "x": 8.478500017117888, + "x": 8.160744038597535, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": 5.118648427773132e-37, - "velocityX": -3.179959194728896, - "velocityY": -2.555705369200667e-15, - "timestamp": 2.9502288122188816 + "angularVelocity": -4.1951574405831105e-32, + "velocityX": -3.120401147919862, + "velocityY": -1.9990570963133027e-15, + "timestamp": 3.0467831432315897 }, { - "x": 8.239250012985984, + "x": 7.930372023665668, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -3.6789246386101953e-38, - "velocityX": -2.3849695652157354, - "velocityY": -1.9167794084287804e-15, - "timestamp": 3.050544559480949 + "angularVelocity": 1.3042757896301787e-36, + "velocityX": -2.3403010309957435, + "velocityY": -1.4992965357471283e-15, + "timestamp": 3.1452200587739676 }, { - "x": 8.079750005312448, + "x": 7.776790675525645, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -5.8112581235806796e-39, - "velocityX": -1.5899797591783358, - "velocityY": -1.2778530495291014e-15, - "timestamp": 3.1508603067430165 + "angularVelocity": -2.9690915854327103e-38, + "velocityX": -1.5602007366220767, + "velocityY": -9.995321001705852e-16, + "timestamp": 3.2436569743163455 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -1.3509031450352823e-38, - "velocityX": -0.7949898942995212, - "velocityY": -6.389265579367251e-16, - "timestamp": 3.251176054005084 + "angularVelocity": -1.2572582518763259e-39, + "velocityX": -0.7801003830985155, + "velocityY": -4.997663730275874e-16, + "timestamp": 3.3420938898587234 }, { - "x": 8, + "x": 7.7, "y": 7.45, "heading": -3.141685307179586, - "angularVelocity": -6.25650180816182e-40, - "velocityX": -1.8255808163460085e-27, - "velocityY": -6.990702072612187e-30, - "timestamp": 3.3514918012671515 + "angularVelocity": 7.618384863796292e-41, + "velocityX": 3.506318460129851e-26, + "velocityY": -3.8726720926841052e-28, + "timestamp": 3.4405308054011012 } ], "constraints": [ @@ -6670,13 +6690,13 @@ "B_ReturnFromFarPiece1": { "waypoints": [ { - "x": 8.3, - "y": 7.6, + "x": 8.73, + "y": 7.45, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { "x": 5.3082685470581055, @@ -6735,589 +6755,598 @@ ], "trajectory": [ { - "x": 8.3, - "y": 7.6, - "heading": 2.757501519692808e-19, - "angularVelocity": 9.600967361428564e-29, - "velocityX": -2.4319155594097383e-17, - "velocityY": -4.0122443313037055e-19, + "x": 8.73, + "y": 7.45, + "heading": 3.0153335539807125e-17, + "angularVelocity": -9.150386789447993e-29, + "velocityX": 8.944879250579314e-18, + "velocityY": -6.136035981354337e-20, "timestamp": 0 }, { - "x": 8.2519779299027, - "y": 7.59802796735826, - "heading": 5.5317266150126195e-19, - "angularVelocity": 2.1474447549717986e-20, - "velocityX": -0.6166429049270267, - "velocityY": -0.025322522214253233, - "timestamp": 0.07787662796993104 - }, - { - "x": 8.155933791545712, - "y": 7.594083902150697, - "heading": 8.305951710241554e-19, - "angularVelocity": 2.147444743302548e-20, - "velocityX": -1.2332857862576319, - "velocityY": -0.050645043453678214, - "timestamp": 0.15575325593986208 - }, - { - "x": 8.011867587991711, - "y": 7.5881678045038425, - "heading": 1.1080176805595397e-18, - "angularVelocity": 2.147444759342935e-20, - "velocityX": -1.8499286282609135, - "velocityY": -0.07596756306832102, - "timestamp": 0.2336298839097931 - }, - { - "x": 7.819779325366047, - "y": 7.580279674670781, - "heading": 1.385440190094531e-18, - "angularVelocity": 2.147444758838943e-20, - "velocityX": -2.466571391609719, - "velocityY": -0.10129007943317028, - "timestamp": 0.31150651187972417 - }, - { - "x": 7.579669022044645, - "y": 7.570419513410852, - "heading": 1.6628626996296796e-18, - "angularVelocity": 2.1474447590405832e-20, - "velocityX": -3.0832139189964716, - "velocityY": -0.1266125860474561, - "timestamp": 0.3893831398496552 - }, - { - "x": 7.295744194818641, - "y": 7.558760102681403, - "heading": 1.9402852103929644e-18, - "angularVelocity": 2.1474463360667863e-20, - "velocityX": -3.6458284677609587, - "velocityY": -0.14971643012009433, - "timestamp": 0.4672597678195863 - }, - { - "x": 7.011819367581371, - "y": 7.547100692219137, - "heading": 2.2177077235682638e-18, - "angularVelocity": 2.1474494332923423e-20, - "velocityX": -3.6458284679056314, - "velocityY": -0.14971642668926177, - "timestamp": 0.5451363957895173 - }, - { - "x": 6.727894540344498, - "y": 7.5354412817471905, - "heading": 2.4951302352091332e-18, - "angularVelocity": 2.1474474629589685e-20, - "velocityX": -3.645828467900525, - "velocityY": -0.14971642681353683, - "timestamp": 0.6230130237594483 - }, - { - "x": 6.443969713075486, - "y": 7.5237818720578895, - "heading": 2.7725527581105576e-18, - "angularVelocity": 2.147461922435792e-20, - "velocityX": -3.645828468313219, - "velocityY": -0.1497164167637409, - "timestamp": 0.7008896517293793 - }, - { - "x": 6.160044885669281, - "y": 7.512122465709375, - "heading": 3.0499752697600583e-18, - "angularVelocity": 2.1474474740432105e-20, - "velocityX": -3.645828470074894, - "velocityY": -0.14971637386530257, - "timestamp": 0.7787662796993103 - }, - { - "x": 5.8761200592911305, - "y": 7.500463034323216, - "heading": 3.3273979967340457e-18, - "angularVelocity": 2.1477215638632408e-20, - "velocityX": -3.6458284568738266, - "velocityY": -0.1497166953692409, - "timestamp": 0.8566429076692413 - }, - { - "x": 5.592195230920835, - "y": 7.488803651443019, - "heading": 3.604820428609217e-18, - "angularVelocity": 2.1473426326984874e-20, - "velocityX": -3.6458284824546, - "velocityY": -0.14971607251278177, - "timestamp": 0.9345195356391723 - }, - { - "x": 5.308268547058106, + "x": 8.678452834172791, + "y": 7.450409597940094, + "heading": 6.050280891646632e-17, + "angularVelocity": 2.4319148836270395e-18, + "velocityX": -0.6391337938457895, + "velocityY": 0.005078608711127938, + "timestamp": 0.08065160428623162 + }, + { + "x": 8.57535850438037, + "y": 7.451228793805321, + "heading": 9.085228229308535e-17, + "angularVelocity": 2.431914883129373e-18, + "velocityX": -1.2782675646046895, + "velocityY": 0.010157217236737312, + "timestamp": 0.16130320857246325 + }, + { + "x": 8.420717013726057, + "y": 7.452457587570761, + "heading": 1.2120175566971728e-16, + "angularVelocity": 2.4319148832885226e-18, + "velocityX": -1.9174012968854468, + "velocityY": 0.015235825453377785, + "timestamp": 0.24195481285869486 + }, + { + "x": 8.2145283684165, + "y": 7.454095979186639, + "heading": 1.5155122904634846e-16, + "angularVelocity": 2.431914883280121e-18, + "velocityX": -2.55653495220995, + "velocityY": 0.020314433052855834, + "timestamp": 0.3226064171449265 + }, + { + "x": 7.95679258707161, + "y": 7.456143968504021, + "heading": 1.8190070242340144e-16, + "angularVelocity": 2.431914888508902e-18, + "velocityX": -3.195668376665999, + "velocityY": 0.025393038805691565, + "timestamp": 0.40325802143115813 + }, + { + "x": 7.6625121380166075, + "y": 7.4584823447093935, + "heading": 2.122501739665664e-16, + "angularVelocity": 2.431892150115273e-18, + "velocityX": -3.648786055273063, + "velocityY": 0.028993548560726043, + "timestamp": 0.48390962571738977 + }, + { + "x": 7.3682316889613775, + "y": 7.460820720895092, + "heading": 2.4259964550994947e-16, + "angularVelocity": 2.4318921528197456e-18, + "velocityX": -3.6487860552758766, + "velocityY": 0.02899354831677369, + "timestamp": 0.5645612300036214 + }, + { + "x": 7.073951239906084, + "y": 7.463159097072805, + "heading": 2.7294911705334123e-16, + "angularVelocity": 2.431892152926584e-18, + "velocityX": -3.6487860552766636, + "velocityY": 0.028993548217774053, + "timestamp": 0.645212834289853 + }, + { + "x": 6.779670790850629, + "y": 7.46549747323022, + "heading": 3.0329858859672815e-16, + "angularVelocity": 2.4318921529031445e-18, + "velocityX": -3.6487860552786633, + "velocityY": 0.028993547966090615, + "timestamp": 0.7258644385760846 + }, + { + "x": 6.485390341794982, + "y": 7.467835849363504, + "heading": 3.3364806014011606e-16, + "angularVelocity": 2.4318921529142702e-18, + "velocityX": -3.6487860552810405, + "velocityY": 0.028993547666888664, + "timestamp": 0.8065160428623163 + }, + { + "x": 6.191109892739618, + "y": 7.470174225532392, + "heading": 3.6399753168349943e-16, + "angularVelocity": 2.4318921528614766e-18, + "velocityX": -3.6487860552775326, + "velocityY": 0.028993548108348557, + "timestamp": 0.8871676471485479 + }, + { + "x": 5.896829443688916, + "y": 7.472512602287961, + "heading": 3.9434700322679583e-16, + "angularVelocity": 2.431892151781526e-18, + "velocityX": -3.648786055219731, + "velocityY": 0.028993555382614684, + "timestamp": 0.9678192514347795 + }, + { + "x": 5.602548994628648, + "y": 7.474850977839731, + "heading": 4.246964747702855e-16, + "angularVelocity": 2.4318921541771888e-18, + "velocityX": -3.6487860553383333, + "velocityY": 0.028993540456692966, + "timestamp": 1.0484708557210112 + }, + { + "x": 5.3082685470581055, "y": 7.477189540863037, - "heading": 3.881406761933183e-18, - "angularVelocity": 1.0737232772850433e-20, - "velocityX": -3.64585230850462, - "velocityY": -0.14913473891635948, - "timestamp": 1.0123961636091034 - }, - { - "x": 5.028627693595278, - "y": 7.497782181981604, - "heading": 4.1587848720220655e-18, - "angularVelocity": 2.118506785259316e-20, - "velocityX": -3.639047714019591, - "velocityY": 0.2679780248850872, - "timestamp": 1.0892406777344608 - }, - { - "x": 4.748989879107207, - "y": 7.518416049826399, - "heading": 4.436163015430491e-18, - "angularVelocity": 2.118550144945551e-20, - "velocityX": -3.6390081669576992, - "velocityY": 0.2685145202575587, - "timestamp": 1.166085191859818 - }, - { - "x": 4.469352066877837, - "y": 7.53904994828081, - "heading": 4.713541138262534e-18, - "angularVelocity": 2.1185233682992707e-20, - "velocityX": -3.6390081375645655, - "velocityY": 0.26851491858936644, - "timestamp": 1.2429297059851754 - }, - { - "x": 4.189714253377734, - "y": 7.5596838295128, - "heading": 4.9909192511776896e-18, - "angularVelocity": 2.118509579668935e-20, - "velocityX": -3.6390081541009813, - "velocityY": 0.268514694469029, - "timestamp": 1.3197742201105327 - }, - { - "x": 3.9100764400605628, - "y": 7.580317713223909, - "heading": 5.2682973643801506e-18, - "angularVelocity": 2.1185099535494865e-20, - "velocityX": -3.6390081517204464, - "velocityY": 0.2685147267305082, - "timestamp": 1.39661873423589 - }, - { - "x": 3.6304386268060798, - "y": 7.600951597744441, - "heading": 5.545591457915407e-18, - "angularVelocity": 2.0091727183515773e-20, - "velocityX": -3.6390081509046595, - "velocityY": 0.2685147372637596, - "timestamp": 1.4734632483612473 - }, - { - "x": 3.397087952925323, - "y": 7.618170051677312, - "heading": 5.822969568480078e-18, - "angularVelocity": 2.1185065209273208e-20, - "velocityX": -3.0366601511734386, - "velocityY": 0.22406874620592004, - "timestamp": 1.5503077624866046 - }, - { - "x": 3.210407395644333, - "y": 7.6319448161653884, - "heading": 6.100347679045377e-18, - "angularVelocity": 2.1185065217376727e-20, - "velocityX": -2.4293283574733384, - "velocityY": 0.17925501442568673, - "timestamp": 1.627152276611962 - }, - { - "x": 3.0703969716895227, - "y": 7.642275889973785, - "heading": 6.37772578961099e-18, - "angularVelocity": 2.1185065221445832e-20, - "velocityX": -1.821996346107541, - "velocityY": 0.13444126657556482, - "timestamp": 1.7039967907373192 - }, - { - "x": 2.9770566866367267, - "y": 7.6491632726910055, - "heading": 6.655103900175346e-18, - "angularVelocity": 2.1185065205170544e-20, - "velocityX": -1.2146642621818082, - "velocityY": 0.08962751337051074, - "timestamp": 1.7808413048626766 - }, - { - "x": 2.9303865432739244, + "heading": 4.54947878770186e-16, + "angularVelocity": 1.2159517589791973e-18, + "velocityX": -3.6487860368672314, + "velocityY": 0.028995864918033788, + "timestamp": 1.1291224600072427 + }, + { + "x": 5.0286303652572135, + "y": 7.497818426819656, + "heading": 4.852792666366976e-16, + "angularVelocity": 2.317046686913831e-18, + "velocityX": -3.6390129500795747, + "velocityY": 0.2684496897326407, + "timestamp": 1.2059669740655883 + }, + { + "x": 4.74899219578683, + "y": 7.518447479923772, + "heading": 5.156106618040484e-16, + "angularVelocity": 2.317141694856443e-18, + "velocityX": -3.639012789619103, + "velocityY": 0.26845186487162814, + "timestamp": 1.2828114881239339 + }, + { + "x": 4.469354026278326, + "y": 7.53907653251117, + "heading": 5.459420533388648e-16, + "angularVelocity": 2.3170944316093188e-18, + "velocityX": -3.639012790115152, + "velocityY": 0.26845185814741257, + "timestamp": 1.3596560021822794 + }, + { + "x": 4.18971585681519, + "y": 7.559705585713528, + "heading": 5.762734448613439e-16, + "angularVelocity": 2.3170942710563877e-18, + "velocityX": -3.6390127895247923, + "velocityY": 0.26845186615006567, + "timestamp": 1.436500516240625 + }, + { + "x": 3.9100776873548275, + "y": 7.580334638953499, + "heading": 6.066048363838091e-16, + "angularVelocity": 2.317094270874412e-18, + "velocityX": -3.639012789488683, + "velocityY": 0.268451866639544, + "timestamp": 1.5133450302989706 + }, + { + "x": 3.6304395178927162, + "y": 7.600963692130584, + "heading": 6.369359381798683e-16, + "angularVelocity": 2.31332397689679e-18, + "velocityX": -3.639012789511441, + "velocityY": 0.2684518658211763, + "timestamp": 1.5901895443573162 + }, + { + "x": 3.397088546983748, + "y": 7.618178114609468, + "heading": 6.672673315632139e-16, + "angularVelocity": 2.3171184870498213e-18, + "velocityX": -3.036664019135996, + "velocityY": 0.22401628391863487, + "timestamp": 1.6670340584156618 + }, + { + "x": 3.2104077520795884, + "y": 7.631949653927409, + "heading": 6.975987249465594e-16, + "angularVelocity": 2.3171184870564727e-18, + "velocityX": -2.42933145185119, + "velocityY": 0.1792130445054763, + "timestamp": 1.7438785724740073 + }, + { + "x": 3.0703971499072202, + "y": 7.642278308855744, + "heading": 7.279301183299096e-16, + "angularVelocity": 2.317118487112907e-18, + "velocityX": -1.8219986668932868, + "velocityY": 0.13440978910339627, + "timestamp": 1.820723086532353 + }, + { + "x": 2.977056746042644, + "y": 7.649164078985233, + "heading": 7.582615117132512e-16, + "angularVelocity": 2.3171184869995114e-18, + "velocityX": -1.2146658093732723, + "velocityY": 0.08960652837573517, + "timestamp": 1.8975676005906985 + }, + { + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 6.9324820107247815e-18, - "angularVelocity": 2.1185065010859018e-20, - "velocityX": -0.6073321419752766, - "velocityY": 0.04481375748833556, - "timestamp": 1.8576858189880339 + "heading": 7.885929050965928e-16, + "angularVelocity": 2.317118487006186e-18, + "velocityX": -0.6073329155713472, + "velocityY": 0.04480326498624666, + "timestamp": 1.974412114649044 }, { - "x": 2.9303865432739253, + "x": 2.930386543273926, "y": 7.652606964111328, - "heading": 7.210735620677998e-18, - "angularVelocity": 3.2578193238109785e-20, - "velocityX": -2.4863039981147584e-16, - "velocityY": 1.807761247430653e-17, - "timestamp": 1.9345303331133912 - }, - { - "x": 2.9024626512592158, - "y": 7.657656184782597, - "heading": -0.014631272955918305, - "angularVelocity": -0.24325141375371975, - "velocityX": -0.4642471116866648, - "velocityY": 0.08394553709310819, - "timestamp": 1.9946791015424687 - }, - { - "x": 2.846452095529944, - "y": 7.66640274559091, - "heading": -0.0450509493286765, - "angularVelocity": -0.505740635548114, - "velocityX": -0.9312003752049336, - "velocityY": 0.14541545964696787, - "timestamp": 2.0548278699715463 - }, - { - "x": 2.7622341154612973, - "y": 7.676698098927908, - "heading": -0.09302514522042853, - "angularVelocity": -0.7975923222487746, - "velocityX": -1.4001613377662077, - "velocityY": 0.17116482358466473, - "timestamp": 2.114976638400624 - }, - { - "x": 2.6499960299160383, - "y": 7.684708411225515, - "heading": -0.16124738434413563, - "angularVelocity": -1.1342250374444285, - "velocityX": -1.8660080410058681, - "velocityY": 0.13317500103185942, - "timestamp": 2.1751254068297015 - }, - { - "x": 2.5118120173105134, - "y": 7.6824383358290635, - "heading": -0.25311412970645, - "angularVelocity": -1.5273254592175989, - "velocityX": -2.2973706064898027, - "velocityY": -0.03774101208951341, - "timestamp": 2.235274175258779 - }, - { - "x": 2.360541446978503, - "y": 7.655914177380236, - "heading": -0.37558688187152506, - "angularVelocity": -2.0361639209534976, - "velocityX": -2.514940443217486, - "velocityY": -0.44097591923436846, - "timestamp": 2.2954229436878566 + "heading": 8.190372202752585e-16, + "angularVelocity": 3.786602635702565e-18, + "velocityX": -1.873158733653468e-17, + "velocityY": 7.970286224256345e-18, + "timestamp": 2.0512566287073897 + }, + { + "x": 2.9024623028632064, + "y": 7.657657459817362, + "heading": -0.014619904665161576, + "angularVelocity": -0.24306191404368274, + "velocityX": -0.4642519549801812, + "velocityY": 0.08396656348239065, + "timestamp": 2.1114055200823554 + }, + { + "x": 2.846450920088731, + "y": 7.666406270741266, + "heading": -0.04501686057067315, + "angularVelocity": -0.5053618647116935, + "velocityX": -0.9312122217731417, + "velocityY": 0.14545257150892452, + "timestamp": 2.171554411457321 + }, + { + "x": 2.762231494912032, + "y": 7.676704300161181, + "heading": -0.09295722077356841, + "angularVelocity": -0.7970281597403814, + "velocityX": -1.4001825013145273, + "velocityY": 0.1712089646958622, + "timestamp": 2.231703302832287 + }, + { + "x": 2.6499913633055336, + "y": 7.684716598970154, + "heading": -0.16113573353687305, + "angularVelocity": -1.1334957503776308, + "velocityX": -1.8660382434448235, + "velocityY": 0.13320775538526416, + "timestamp": 2.2918521942072525 + }, + { + "x": 2.511805828604735, + "y": 7.6824456933879235, + "heading": -0.252952190903312, + "angularVelocity": -1.5264862787614344, + "velocityX": -2.297391216063361, + "velocityY": -0.03775473712510196, + "timestamp": 2.352001085582218 + }, + { + "x": 2.3605380306783226, + "y": 7.655917745838296, + "heading": -0.37536830181828656, + "angularVelocity": -2.035218074959957, + "velocityX": -2.514889210233606, + "velocityY": -0.4410380132236904, + "timestamp": 2.412149976957184 }, { "x": 2.2177579402923584, "y": 7.6022491455078125, - "heading": -0.511351339604183, - "angularVelocity": -2.257144431689229, - "velocityX": -2.373839239194111, - "velocityY": -0.892204998938606, - "timestamp": 2.355571712116934 - }, - { - "x": 2.1016275380662224, - "y": 7.534017578559174, - "heading": -0.6392495296972435, - "angularVelocity": -2.3152962059520767, - "velocityX": -2.1022680576965818, - "velocityY": -1.2351721941287113, - "timestamp": 2.4108122423628147 - }, - { - "x": 2.008734470369056, - "y": 7.459090350747871, - "heading": -0.766874318638819, - "angularVelocity": -2.310346920522132, - "velocityX": -1.6816107174150479, - "velocityY": -1.3563814010798148, - "timestamp": 2.466052772608695 - }, - { - "x": 1.9376872862851824, - "y": 7.394462340595956, - "heading": -0.8899091991736856, - "angularVelocity": -2.2272574138450025, - "velocityX": -1.2861423264337029, - "velocityY": -1.169938265694507, - "timestamp": 2.5212933028545756 - }, - { - "x": 1.8830375504540935, - "y": 7.347535921403511, - "heading": -1.0069040331207888, - "angularVelocity": -2.117916562827061, - "velocityX": -0.9893050553249492, - "velocityY": -0.8494925552590561, - "timestamp": 2.576533833100456 - }, - { - "x": 1.8413050810141178, - "y": 7.320984027474417, - "heading": -1.1176685546197516, - "angularVelocity": -2.005131395479613, - "velocityX": -0.7554682993484771, - "velocityY": -0.48065964991469606, - "timestamp": 2.6317743633463366 - }, - { - "x": 1.810393214225769, - "y": 7.315992832183838, - "heading": -1.222253260337211, - "angularVelocity": -1.8932603516283095, - "velocityX": -0.5595867137904185, - "velocityY": -0.09035386279518245, - "timestamp": 2.687014893592217 - }, - { - "x": 1.7990257860880356, - "y": 7.317843004345945, - "heading": -1.2653301184454402, - "angularVelocity": -1.8463104956152538, - "velocityX": -0.48721756415307704, - "velocityY": 0.07929994042288478, - "timestamp": 2.7103462120300073 - }, - { - "x": 1.7893194289617778, - "y": 7.323659017290917, - "heading": -1.3071398480430554, - "angularVelocity": -1.7920002981871601, - "velocityX": -0.41602265864852256, - "velocityY": 0.2492792235671207, - "timestamp": 2.7336775304677974 - }, - { - "x": 1.7812419817978389, - "y": 7.333448844934152, - "heading": -1.347483652309462, - "angularVelocity": -1.729169501242827, - "velocityX": -0.3462062028548906, - "velocityY": 0.4196002754553388, - "timestamp": 2.7570088489055875 - }, - { - "x": 1.7747551329306968, - "y": 7.347220693006434, - "heading": -1.3861276889127772, - "angularVelocity": -1.6563160245898274, - "velocityX": -0.2780318173807648, - "velocityY": 0.590273031890653, - "timestamp": 2.7803401673433776 - }, - { - "x": 1.7698124234870385, - "y": 7.364982670787121, - "heading": -1.4227914173931293, - "angularVelocity": -1.571438347049096, - "velocityX": -0.21184869842837598, - "velocityY": 0.7612933588835104, - "timestamp": 2.8036714857811678 - }, - { - "x": 1.7663562566686506, - "y": 7.386742092358209, - "heading": -1.4571298020017938, - "angularVelocity": -1.471772146106038, - "velocityX": -0.14813422685937813, - "velocityY": 0.9326271736037851, - "timestamp": 2.827002804218958 - }, - { - "x": 1.764313200072897, - "y": 7.41250397367922, - "heading": -1.48870457067619, - "angularVelocity": -1.353321234656823, - "velocityX": -0.08756713004468872, - "velocityY": 1.1041759765827466, - "timestamp": 2.850334122656748 - }, - { - "x": 1.7635861805743018, - "y": 7.442267647668979, - "heading": -1.516934428841365, - "angularVelocity": -1.2099555471094359, - "velocityX": -0.031160669318076883, - "velocityY": 1.2756961878994322, - "timestamp": 2.873665441094538 - }, - { - "x": 1.7640407156271012, - "y": 7.4760185199259155, - "heading": -1.541001115950982, - "angularVelocity": -1.0315185219298435, - "velocityX": 0.01948175599311238, - "velocityY": 1.4465908708471107, - "timestamp": 2.8969967595323283 - }, - { - "x": 1.7654797273438503, - "y": 7.51370577350444, - "heading": -1.559655084780015, - "angularVelocity": -0.7995248480611737, - "velocityX": 0.06167725671372724, - "velocityY": 1.6153074966170409, - "timestamp": 2.9203280779701184 - }, - { - "x": 1.7676028546878815, - "y": 7.555176328390615, + "heading": -0.5110801667427121, + "angularVelocity": -2.2562654410128142, + "velocityX": -2.3737775896131, + "velocityY": -0.8922625023279553, + "timestamp": 2.4722988683321496 + }, + { + "x": 2.101645188237642, + "y": 7.5340268603699245, + "heading": -0.6389171271632187, + "angularVelocity": -2.3145402090323093, + "velocityX": -2.1022686437775406, + "velocityY": -1.235192244730253, + "timestamp": 2.527530987404247 + }, + { + "x": 2.0087612536344777, + "y": 7.459105901882302, + "heading": -0.7664975716198472, + "angularVelocity": -2.3098958830696934, + "velocityX": -1.6817014476981458, + "velocityY": -1.3564744526591341, + "timestamp": 2.582763106476344 + }, + { + "x": 1.9377174081938675, + "y": 7.394479840665335, + "heading": -0.8895097341539958, + "angularVelocity": -2.2271852791638636, + "velocityX": -1.2862777426237315, + "velocityY": -1.1700811466678287, + "timestamp": 2.6379952255484413 + }, + { + "x": 1.883064537205771, + "y": 7.3475517575068015, + "heading": -1.0065004560625568, + "angularVelocity": -2.1181646453779086, + "velocityX": -0.9895124776303853, + "velocityY": -0.8496520493271641, + "timestamp": 2.6932273446205386 + }, + { + "x": 1.8413220448871523, + "y": 7.320994180762134, + "heading": -1.1172766871950899, + "angularVelocity": -2.0056487600316086, + "velocityX": -0.7557648161887509, + "velocityY": -0.4808357381638806, + "timestamp": 2.748459463692636 + }, + { + "x": 1.8103932142257688, + "y": 7.31599283218384, + "heading": -1.2218872603273923, + "angularVelocity": -1.8940170119713384, + "velocityX": -0.5599790697967124, + "velocityY": -0.0905514519812198, + "timestamp": 2.803691582764733 + }, + { + "x": 1.799014560094557, + "y": 7.3178395552871685, + "heading": -1.2649896877020275, + "angularVelocity": -1.8471473915609153, + "velocityX": -0.4876303395876735, + "velocityY": 0.07914101294769894, + "timestamp": 2.8270261729402635 + }, + { + "x": 1.7892970764427014, + "y": 7.323653437979558, + "heading": -1.3068267846648367, + "angularVelocity": -1.7929218661843884, + "velocityX": -0.41644115362513584, + "velocityY": 0.24915298055619764, + "timestamp": 2.850360763115794 + }, + { + "x": 1.7812089941276659, + "y": 7.333442300970777, + "heading": -1.3471998191924708, + "angularVelocity": -1.730179712838083, + "velocityX": -0.3466134290074906, + "velocityY": 0.41950010338730764, + "timestamp": 2.8736953532913243 + }, + { + "x": 1.7747124589766448, + "y": 7.347214179987926, + "heading": -1.3858749923031177, + "angularVelocity": -1.657418142820578, + "velocityX": -0.27840793869984504, + "velocityY": 0.5901915959731657, + "timestamp": 2.8970299434668547 + }, + { + "x": 1.7697615522281, + "y": 7.3649769955487, + "heading": -1.4225717760886871, + "angularVelocity": -1.5726345957298005, + "velocityX": -0.21217028931756562, + "velocityY": 0.7612225210337143, + "timestamp": 2.920364533642385 + }, + { + "x": 1.7662993223458214, + "y": 7.38673785288221, + "heading": -1.4569451030122402, + "angularVelocity": -1.4730632364525922, + "velocityX": -0.14837328859330837, + "velocityY": 0.9325579395116675, + "timestamp": 2.9436991238179155 + }, + { + "x": 1.7642531161176147, + "y": 7.412501539771869, + "heading": -1.488556602137018, + "angularVelocity": -1.3547055632310927, + "velocityX": -0.08768982925275201, + "velocityY": 1.1040985376551697, + "timestamp": 2.967033713993446 + }, + { + "x": 1.7635268049301072, + "y": 7.442267147857092, + "heading": -1.5168247606019365, + "angularVelocity": -1.211427252587835, + "velocityX": -0.031125945728338194, + "velocityY": 1.275600208156052, + "timestamp": 2.9903683041689764 + }, + { + "x": 1.7639870250382692, + "y": 7.476019844911383, + "heading": -1.5409308010829128, + "angularVelocity": -1.0330603752274243, + "velocityX": 0.019722656574941596, + "velocityY": 1.446466246044897, + "timestamp": 3.0137028943445068 + }, + { + "x": 1.7654378590732076, + "y": 7.513708586961909, + "heading": -1.5596235507230065, + "angularVelocity": -0.801074692149455, + "velocityX": 0.06217525245863937, + "velocityY": 1.6151448029256037, + "timestamp": 3.037037484520037 + }, + { + "x": 1.7675794351945344, + "y": 7.555179767773511, "heading": -1.5707963267948966, - "angularVelocity": -0.47752303602545026, - "velocityX": 0.09099902989648029, - "velocityY": 1.7774629837892288, - "timestamp": 2.9436593964079085 + "angularVelocity": -0.4788074694467324, + "velocityX": 0.09177689025107998, + "velocityY": 1.7772405900199395, + "timestamp": 3.0603720746955676 }, { "x": 1.7700000000000002, - "y": 7.6, + "y": 7.599999999999997, "heading": -1.5707963267948966, - "angularVelocity": 2.1680750549204353e-19, - "velocityX": 0.10274367128149403, - "velocityY": 1.9211803965942749, - "timestamp": 2.9669907148456987 + "angularVelocity": 2.3078506585471477e-17, + "velocityX": 0.1037329041360114, + "velocityY": 1.9207636341194518, + "timestamp": 3.083706664871098 }, { - "x": 1.7728698177937292, - "y": 7.6642827500403135, + "x": 1.7729209385382234, + "y": 7.664272399608001, "heading": -1.5707963267948966, - "angularVelocity": 2.7602298655314276e-30, - "velocityX": 0.09630721046077882, - "velocityY": 2.1572422997179523, - "timestamp": 2.9967892915047587 + "angularVelocity": 2.836096341921572e-28, + "velocityX": 0.09802067351807021, + "velocityY": 2.1568491824425395, + "timestamp": 3.113505873889124 }, { - "x": 1.775470402283232, - "y": 7.735597258297608, + "x": 1.7756102473631166, + "y": 7.735578195245327, "heading": -1.5707963267948966, - "angularVelocity": -1.997147332419783e-30, - "velocityX": 0.08727210427766764, - "velocityY": 2.3932186115208283, - "timestamp": 3.0265878681638188 + "angularVelocity": -1.3962527377821026e-29, + "velocityX": 0.09024765802608749, + "velocityY": 2.3928754482762344, + "timestamp": 3.1433050829071503 }, { - "x": 1.7747633273574823, - "y": 7.8129986766433595, + "x": 1.7748330223177948, + "y": 7.812982865560118, "heading": -1.5707963267948966, - "angularVelocity": -2.066382572868049e-30, - "velocityX": -0.02372847984821113, - "velocityY": 2.5974870958213288, - "timestamp": 3.056386444822879 + "angularVelocity": 2.544623352948458e-29, + "velocityX": -0.02608206965374826, + "velocityY": 2.597541104798205, + "timestamp": 3.1731042919251764 }, { - "x": 1.7739937081633357, - "y": 7.8833634639968295, + "x": 1.7740290427893892, + "y": 7.883350378923669, "heading": -1.5707963267948966, - "angularVelocity": 4.723938447739201e-30, - "velocityX": -0.025827381051023144, - "velocityY": 2.3613472602583356, - "timestamp": 3.086185021481939 + "angularVelocity": -2.464825660281466e-29, + "velocityX": -0.02697989493346029, + "velocityY": 2.36138863018112, + "timestamp": 3.2029035009432025 }, { - "x": 1.7732502174839604, - "y": 7.946691377441134, + "x": 1.7732661288748879, + "y": 7.946680790945236, "heading": -1.5707963267948966, - "angularVelocity": -6.603517224826132e-31, - "velocityX": -0.024950543372751627, - "velocityY": 2.1251992727327242, - "timestamp": 3.115983598140999 + "angularVelocity": -6.565156523146849e-31, + "velocityX": -0.02560181762067708, + "velocityY": 2.1252380216958553, + "timestamp": 3.2327027099612287 }, { - "x": 1.7725615197118283, - "y": 8.002982577289577, + "x": 1.7725664082140153, + "y": 8.002974261088676, "heading": -1.5707963267948966, - "angularVelocity": -9.857872645178015e-31, - "velocityX": -0.023111767384506342, - "velocityY": 1.8890566651051475, - "timestamp": 3.145782174800059 + "angularVelocity": 1.0402852363586799e-33, + "velocityX": -0.02348118235190311, + "velocityY": 1.8890927644888424, + "timestamp": 3.262501918979255 }, { - "x": 1.7719417844356995, - "y": 8.052237185917901, + "x": 1.7719408549042543, + "y": 8.052230894249453, "heading": -1.5707963267948966, - "angularVelocity": 9.859209524828823e-31, - "velocityX": -0.020797479128623474, - "velocityY": 1.652918164242209, - "timestamp": 3.175580751459119 + "angularVelocity": -6.130235273525354e-33, + "velocityX": -0.02099227900324947, + "velocityY": 1.6529510273573067, + "timestamp": 3.292301127997281 }, { - "x": 1.771399461982678, - "y": 8.094455289890794, + "x": 1.7713960246129865, + "y": 8.094450761255887, "heading": -1.5707963267948966, - "angularVelocity": -6.553352029785504e-31, - "velocityX": -0.018199609304387406, - "velocityY": 1.4167825683732993, - "timestamp": 3.205379328118179 + "angularVelocity": 3.3057640560004466e-30, + "velocityX": -0.018283380976903497, + "velocityY": 1.4168116670780435, + "timestamp": 3.322100337015307 }, { - "x": 1.7709401646159184, - "y": 8.129636952308697, + "x": 1.7709362757556117, + "y": 8.129633912577273, "heading": -1.5707963267948966, - "angularVelocity": 6.554127986694498e-31, - "velocityX": -0.015413399506111826, - "velocityY": 1.1806490900700566, - "timestamp": 3.235177904777239 + "angularVelocity": -1.3860232852704577e-29, + "velocityX": -0.015428223518035878, + "velocityY": 1.1806740004443153, + "timestamp": 3.351899546033333 }, { - "x": 1.7705678907115057, - "y": 8.157782220857532, + "x": 1.770564715580057, + "y": 8.157780385843287, "heading": -1.5707963267948966, - "angularVelocity": 1.301401764569401e-30, - "velocityX": -0.012493009604860767, - "velocityY": 0.9445172120420281, - "timestamp": 3.264976481436299 + "angularVelocity": -1.6405726594716626e-28, + "velocityX": -0.01246879322679122, + "velocityY": 0.9445375965848751, + "timestamp": 3.3816987550513593 }, { - "x": 1.770285633389739, - "y": 8.178891132721446, + "x": 1.7702836712860994, + "y": 8.178890210135783, "heading": -1.5707963267948966, - "angularVelocity": -3.940207593218983e-30, - "velocityX": -0.00947217462752943, - "velocityY": 0.7083865818636769, - "timestamp": 3.294775058095359 + "angularVelocity": 2.6411722797252613e-28, + "velocityX": -0.009431266910373286, + "velocityY": 0.708402168652532, + "timestamp": 3.4114979640693854 }, { - "x": 1.7700957172852196, - "y": 8.192963717658412, + "x": 1.7700949509971242, + "y": 8.19296340858137, "heading": -1.5707963267948966, - "angularVelocity": 2.638767171459303e-30, - "velocityX": -0.006373328051609348, - "velocityY": 0.472256950322705, - "timestamp": 3.3245736347544192 + "angularVelocity": -9.675024300747488e-29, + "velocityX": -0.006333063702032236, + "velocityY": 0.47226751680135254, + "timestamp": 3.4412971730874116 }, { "x": 1.77, - "y": 8.2, + "y": 8.200000000000001, "heading": -1.5707963267948966, - "angularVelocity": 2.638857621585667e-30, - "velocityX": -0.0032121428588589426, - "velocityY": 0.23612813531646912, - "timestamp": 3.3543722114134793 + "angularVelocity": 2.089025035794717e-29, + "velocityX": -0.003186359646984827, + "velocityY": 0.23613349650910906, + "timestamp": 3.4710963821054377 }, { "x": 1.77, "y": 8.2, "heading": -1.5707963267948966, - "angularVelocity": -8.796691590043117e-31, - "velocityX": 4.572133438774496e-18, - "velocityY": 3.60345450735681e-18, - "timestamp": 3.3841707880725393 + "angularVelocity": -4.325450385226991e-30, + "velocityX": -1.494896119886391e-17, + "velocityY": 1.7968192567227342e-16, + "timestamp": 3.500895591123464 } ], "constraints": [ @@ -10350,6 +10379,558 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "B_ReturnFromFarwithStop": { + "waypoints": [ + { + "x": 8.73, + "y": 7.45, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 8.73, + "y": 7.45, + "heading": 0, + "angularVelocity": 0, + "velocityX": -2.693667833342153e-29, + "velocityY": -4.53306417990796e-35, + "timestamp": 0 + }, + { + "x": 8.657925937477573, + "y": 7.455789387308592, + "heading": -1.2188341073833624e-16, + "angularVelocity": -1.2760069620061655e-15, + "velocityX": -0.7545490196731954, + "velocityY": 0.06060955030593678, + "timestamp": 0.09551939058067259 + }, + { + "x": 8.513777815345431, + "y": 7.46736816169181, + "heading": -4.062778690542764e-16, + "angularVelocity": -2.9773478323983444e-15, + "velocityX": -1.5090980088529606, + "velocityY": 0.1212190981624727, + "timestamp": 0.19103878116134518 + }, + { + "x": 8.297555639429, + "y": 7.484736322681725, + "heading": -9.34438717110679e-16, + "angularVelocity": -5.529357236530814e-15, + "velocityX": -2.26364693704592, + "velocityY": 0.18182864112021124, + "timestamp": 0.28655817174201775 + }, + { + "x": 8.009259427204483, + "y": 7.50789386887455, + "heading": -1.9501377833476253e-15, + "angularVelocity": -1.0633432982967515e-14, + "velocityX": -3.018195682279077, + "velocityY": 0.24243816938160717, + "timestamp": 0.38207756232269036 + }, + { + "x": 7.66183760867627, + "y": 7.535800710760828, + "heading": -1.9431705716357535e-15, + "angularVelocity": 7.29402864644545e-17, + "velocityX": -3.637186297108895, + "velocityY": 0.2921589189025347, + "timestamp": 0.47759695290336296 + }, + { + "x": 7.31441579014799, + "y": 7.563707552647111, + "heading": -1.943030848678189e-15, + "angularVelocity": 1.4627706079539333e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.5731163434840355 + }, + { + "x": 6.9669939716197105, + "y": 7.591614394533395, + "heading": -1.9365713004742454e-15, + "angularVelocity": 6.762551733917979e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.2921589189025843, + "timestamp": 0.6686357340647081 + }, + { + "x": 6.619572153091432, + "y": 7.619521236419678, + "heading": -1.937339896809014e-15, + "angularVelocity": -8.046495378072972e-18, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.7641551246453807 + }, + { + "x": 6.272150334563152, + "y": 7.647428078305961, + "heading": -1.9502459990087004e-15, + "angularVelocity": -1.351149972924591e-16, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.8596745152260533 + }, + { + "x": 5.924728516034873, + "y": 7.675334920192245, + "heading": -1.9529774200702442e-15, + "angularVelocity": -2.8595461564986775e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 0.9551939058067259 + }, + { + "x": 5.577306697506593, + "y": 7.7032417620785285, + "heading": -1.9466253347863878e-15, + "angularVelocity": 6.650047959108812e-17, + "velocityX": -3.6371862971095723, + "velocityY": 0.29215891890258433, + "timestamp": 1.0507132963873984 + }, + { + "x": 5.22988487897838, + "y": 7.731148603964805, + "heading": -1.9387248800321926e-15, + "angularVelocity": 8.271048197596407e-17, + "velocityX": -3.6371862971088955, + "velocityY": 0.29215891890252993, + "timestamp": 1.146232686968071 + }, + { + "x": 4.941588666753863, + "y": 7.754306150157631, + "heading": -1.9386609172268905e-15, + "angularVelocity": 6.696315831572461e-19, + "velocityX": -3.0181956822790776, + "velocityY": 0.24243816938160498, + "timestamp": 1.2417520775487436 + }, + { + "x": 4.72536649083743, + "y": 7.771674311147546, + "heading": -1.9386284069178885e-15, + "angularVelocity": 3.4035295166666236e-19, + "velocityX": -2.26364693704592, + "velocityY": 0.1818286411202101, + "timestamp": 1.3372714681294162 + }, + { + "x": 4.581218368705289, + "y": 7.783253085530764, + "heading": -1.9386109286969595e-15, + "angularVelocity": 1.8298085390218347e-19, + "velocityX": -1.5090980088529609, + "velocityY": 0.12121909816247209, + "timestamp": 1.4327908587100888 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 7.85782206050662e-20, + "velocityX": -0.7545490196731954, + "velocityY": 0.060609550305936524, + "timestamp": 1.5283102492907614 + }, + { + "x": 4.509144306182861, + "y": 7.7890424728393555, + "heading": -1.938603422952659e-15, + "angularVelocity": 0, + "velocityX": 1.333662779175558e-32, + "velocityY": 0, + "timestamp": 1.623829639871434 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "R_ReturnFromFarwithStop": { + "waypoints": [ + { + "x": 7.7, + "y": 7.45, + "heading": 3.1415, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 33 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 7.7, + "y": 7.45, + "heading": 3.1415, + "angularVelocity": 0, + "velocityX": 1.3345047844501093e-28, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 7.720983776232949, + "y": 7.452031789914187, + "heading": 3.1415, + "angularVelocity": -2.8559927040215486e-16, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589325, + "timestamp": 0.051577320630114186 + }, + { + "x": 7.762951328065995, + "y": 7.456095369681284, + "heading": 3.1415, + "angularVelocity": -6.035032687716479e-16, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368897, + "timestamp": 0.10315464126022837 + }, + { + "x": 7.825902654655366, + "y": 7.462190739219591, + "heading": 3.1415, + "angularVelocity": -9.133792065608597e-16, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741078, + "timestamp": 0.15473196189034255 + }, + { + "x": 7.909837754819842, + "y": 7.470317898414736, + "heading": 3.1415, + "angularVelocity": -1.285554414608073e-15, + "velocityX": 1.6273644915795704, + "velocityY": 0.15757234179354235, + "timestamp": 0.20630928252045674 + }, + { + "x": 8.01475662678774, + "y": 7.48047684709517, + "heading": 3.1414999999999997, + "angularVelocity": -1.8501098820899896e-15, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355356, + "timestamp": 0.25788660315057094 + }, + { + "x": 8.140659267606683, + "y": 7.492667584975027, + "heading": 3.1414999999999997, + "angularVelocity": -2.568015817611512e-15, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908125, + "timestamp": 0.3094639237806851 + }, + { + "x": 8.2875456713738, + "y": 7.506890111482749, + "heading": 3.1414999999999997, + "angularVelocity": -3.6664166549374125e-15, + "velocityX": 2.8478874429965697, + "velocityY": 0.27575155773604454, + "timestamp": 0.36104124441079927 + }, + { + "x": 8.455415820402175, + "y": 7.523144424905771, + "heading": 3.1414999999999993, + "angularVelocity": -5.781579183621917e-15, + "velocityX": 3.254727988477392, + "velocityY": 0.315144587275328, + "timestamp": 0.41261856504091343 + }, + { + "x": 8.642740284809822, + "y": 7.541282435587867, + "heading": 3.1414999999999993, + "angularVelocity": 2.8772388403772396e-16, + "velocityX": 3.6319153868252347, + "velocityY": 0.3516664002804812, + "timestamp": 0.4641958856710276 + }, + { + "x": 8.830064749214753, + "y": 7.5594204462697006, + "heading": 3.1414999999999993, + "angularVelocity": 4.478421854687767e-16, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 0.5157732063011418 + }, + { + "x": 9.017389213619715, + "y": 7.577558456951537, + "heading": 3.1414999999999993, + "angularVelocity": -2.6797870831169e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 0.567350526931256 + }, + { + "x": 9.20471367802468, + "y": 7.595696467633373, + "heading": 3.1414999999999993, + "angularVelocity": -1.5978294903773436e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6189278475613702 + }, + { + "x": 9.392038142429643, + "y": 7.613834478315209, + "heading": 3.1414999999999993, + "angularVelocity": 3.6921602148337384e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.6705051681914844 + }, + { + "x": 9.579362606834605, + "y": 7.631972488997046, + "heading": 3.1414999999999993, + "angularVelocity": 2.453996709110038e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7220824888215986 + }, + { + "x": 9.766687071239568, + "y": 7.650110499678882, + "heading": 3.1414999999999993, + "angularVelocity": 2.90499526121897e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.7736598094517129 + }, + { + "x": 9.954011535644533, + "y": 7.668248510360718, + "heading": 3.1414999999999993, + "angularVelocity": 4.2648077713252974e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8252371300818271 + }, + { + "x": 10.141336000049495, + "y": 7.686386521042555, + "heading": 3.1414999999999993, + "angularVelocity": 1.9645772050389573e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.8768144507119413 + }, + { + "x": 10.328660464454458, + "y": 7.704524531724391, + "heading": 3.1414999999999993, + "angularVelocity": 8.516171089710268e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9283917713420555 + }, + { + "x": 10.515984928859421, + "y": 7.722662542406227, + "heading": 3.1414999999999993, + "angularVelocity": -3.343515828741549e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 0.9799690919721697 + }, + { + "x": 10.703309393264385, + "y": 7.740800553088063, + "heading": 3.1414999999999993, + "angularVelocity": -4.869114069203101e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.031546412602284 + }, + { + "x": 10.890633857669348, + "y": 7.758938563769899, + "heading": 3.1414999999999993, + "angularVelocity": -1.948527081472739e-16, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736323, + "timestamp": 1.0831237332323982 + }, + { + "x": 11.077958322074311, + "y": 7.777076574451735, + "heading": 3.1414999999999993, + "angularVelocity": -1.203013192737632e-17, + "velocityX": 3.6319153867545726, + "velocityY": 0.3516664002736322, + "timestamp": 1.1347010538625124 + }, + { + "x": 11.265282786479242, + "y": 7.795214585133569, + "heading": 3.1414999999999993, + "angularVelocity": 1.3156045718444795e-17, + "velocityX": 3.6319153867539664, + "velocityY": 0.3516664002735736, + "timestamp": 1.1862783744926266 + }, + { + "x": 11.452607250886889, + "y": 7.813352595815664, + "heading": 3.1414999999999993, + "angularVelocity": -3.1679641053244916e-16, + "velocityX": 3.6319153868252356, + "velocityY": 0.3516664002804744, + "timestamp": 1.2378556951227409 + }, + { + "x": 11.620477399915263, + "y": 7.829606909238687, + "heading": 3.1414999999999993, + "angularVelocity": -3.349318744028512e-17, + "velocityX": 3.254727988477392, + "velocityY": 0.3151445872753285, + "timestamp": 1.289433015752855 + }, + { + "x": 11.76736380368238, + "y": 7.843829435746409, + "heading": 3.1414999999999993, + "angularVelocity": -3.0900120798738865e-17, + "velocityX": 2.8478874429965697, + "velocityY": 0.2757515577360448, + "timestamp": 1.3410103363829693 + }, + { + "x": 11.893266444501323, + "y": 7.856020173626265, + "heading": 3.1414999999999993, + "angularVelocity": -1.4649487274207734e-17, + "velocityX": 2.441046554573701, + "velocityY": 0.2363584949908127, + "timestamp": 1.3925876570130835 + }, + { + "x": 11.998185316469222, + "y": 7.8661791223067, + "heading": 3.1414999999999993, + "angularVelocity": -3.815569156988782e-18, + "velocityX": 2.034205551698658, + "velocityY": 0.19696542116355373, + "timestamp": 1.4441649776431977 + }, + { + "x": 12.082120416633698, + "y": 7.874306281501845, + "heading": 3.1414999999999993, + "angularVelocity": -1.6271982814457388e-18, + "velocityX": 1.6273644915795704, + "velocityY": 0.1575723417935425, + "timestamp": 1.495742298273312 + }, + { + "x": 12.145071743223069, + "y": 7.880401651040152, + "heading": 3.1414999999999993, + "angularVelocity": -1.1137179997081401e-18, + "velocityX": 1.220523397109212, + "velocityY": 0.11817925909741088, + "timestamp": 1.5473196189034262 + }, + { + "x": 12.187039295056113, + "y": 7.8844652308072485, + "heading": 3.1414999999999993, + "angularVelocity": -6.858817655923581e-19, + "velocityX": 0.8136822797361822, + "velocityY": 0.07878617418368902, + "timestamp": 1.5988969395335404 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": -3.1997477749711506e-19, + "velocityX": 0.40684114600326593, + "velocityY": 0.03939308768589327, + "timestamp": 1.6504742601636546 + }, + { + "x": 12.208023071289062, + "y": 7.8864970207214355, + "heading": 3.1414999999999993, + "angularVelocity": 0, + "velocityX": -1.1615407482795014e-30, + "velocityY": 0, + "timestamp": 1.7020515807937688 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } }, "splitTrajectoriesAtStopPoints": false,