diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 149080f58..307635fba 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -11,15 +11,32 @@ control: recovery: brake_amount : 0.5 brake_speed : 2.0 + + # Pure Pursuit controller parameters pure_pursuit: - lookahead: 2.0 - lookahead_scale: 3.0 - crosstrack_gain: 0.5 - desired_speed: trajectory + lookahead: 2.0 # Base lookahead distance (meters) + lookahead_scale: 3.0 # Velocity-dependent lookahead scaling factor + crosstrack_gain: 0.5 # Gain for crosstrack error correction (default: 1) + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Stanley controller parameters (fine tune this) + stanley: + control_gain: 1.5 + softening_gain: 0.2 + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # MPC controller parameters + mpc: + dt: 0.2 # Time step for the MPC controller (seconds) + horizon: 30 # Prediction horizon for the MPC controller (number of time steps) + switch_gear: False # Whether to switch gears during runtime + desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value + + # Shared longitudinal control parameters longitudinal_control: - pid_p: 1.0 - pid_i: 0.1 - pid_d: 0.0 + pid_p: 1.0 # Proportional gain for speed PID controller + pid_i: 0.1 # Integral gain for speed PID controller + pid_d: 1.0 # Derivative gain for speed PID controller #configure the simulator, if using simulator: @@ -31,4 +48,4 @@ simulator: #orientation_noise: 0.04 #2.3 degrees noise #velocity_noise: # constant: 0.04 #4cm/s noise - # linear: 0.02 #2% noise \ No newline at end of file + # linear: 0.02 #2% noise diff --git a/GEMstack/knowledge/routes/backward_15m.csv b/GEMstack/knowledge/routes/backward_15m.csv new file mode 100644 index 000000000..27912b180 --- /dev/null +++ b/GEMstack/knowledge/routes/backward_15m.csv @@ -0,0 +1,16 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0,0 +-3.0,0,0 +-4,0,0 +-5,0,0 +-6,0,0 +-7,0,0 +-8,0,0 +-9,0,0 +-10,0,0 +-11,0,0 +-12,0,0 +-13,0,0 +-14,0,0 +-15,0,0 diff --git a/GEMstack/knowledge/routes/backward_curve.csv b/GEMstack/knowledge/routes/backward_curve.csv new file mode 100644 index 000000000..9b9aacc13 --- /dev/null +++ b/GEMstack/knowledge/routes/backward_curve.csv @@ -0,0 +1,20 @@ +0.0,0,0 +-1.0,0,0 +-2.0,0.2,0 +-3.0,0.4,0 +-4.0,0.6,0 +-5.0,0.8,0 +-6.0,1.0,0 +-7.0,1.2,0 +-8.0,1.4,0 +-9.0,1.6,0 +-10.0,1.8,0 +-11.0,2.0,0 +-12.0,2.2,0 +-13.0,2.4,0 +-14.0,2.6,0 +-15.0,2.8,0 +-16.0,3.0,0 +-17.0,3.2,0 +-18.0,3.4,0 +-19.0,3.6,0 diff --git a/GEMstack/knowledge/routes/forward_curve.csv b/GEMstack/knowledge/routes/forward_curve.csv new file mode 100644 index 000000000..9b776d8ec --- /dev/null +++ b/GEMstack/knowledge/routes/forward_curve.csv @@ -0,0 +1,33 @@ +0.0,0,0 +1.0,0,0 +2.0,0,0 +3.0,0,0 +4.0,0.4,0 +5.0,0.8,0 +6.0,1.2,0 +7.0,1.6,0 +8.0,2.0,0 +9.0,2.4,0 +10.0,2.8,0 +11.0,3.2,0 +12.0,3.6,0 +13.0,4.0,0 +14.0,4.4,0 +15.0,4.8,0 +16.0,5.2,0 +17.0,5.6,0 +18.0,6.0,0 +19.0,6.4,0 +20.0,6.8,0 +21.0,7.2,0 +22.0,7.6,0 +23.0,8.0,0 +24.0,8.4,0 +25.0,8.8,0 +26.0,9.2,0 +27.0,9.6,0 +28.0,10.0,0 +29.0,10.4,0 +30.0,10.8,0 +31.0,11.2,0 +32.0,11.6,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/offset.csv b/GEMstack/knowledge/routes/offset.csv new file mode 100644 index 000000000..947697fbb --- /dev/null +++ b/GEMstack/knowledge/routes/offset.csv @@ -0,0 +1,31 @@ +0.0,0.5,0 +1.0,0.7,0 +2.0,0.9,0 +3.0,1.1,0 +4.0,1.3,0 +5.0,1.5,0 +6.0,1.7,0 +7.0,1.9,0 +8.0,2.1,0 +9.0,2.3,0 +10.0,2.5,0 +11.0,2.7,0 +12.0,2.9,0 +13.0,3.1,0 +14.0,3.3,0 +15.0,3.5,0 +16.0,3.7,0 +17.0,3.9,0 +18.0,4.1,0 +19.0,4.3,0 +20.0,4.5,0 +21.0,4.7,0 +22.0,4.9,0 +23.0,5.1,0 +24.0,5.3,0 +25.0,5.5,0 +26.0,5.7,0 +27.0,5.9,0 +28.0,6.1,0 +29.0,6.3,0 +30.0,6.5,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/parallel_parking.csv b/GEMstack/knowledge/routes/parallel_parking.csv new file mode 100644 index 000000000..a2196cb60 --- /dev/null +++ b/GEMstack/knowledge/routes/parallel_parking.csv @@ -0,0 +1,266 @@ +0.0,0.0,0.0 +0.1,5.5511151231257715e-18,0.0 +0.2,1.1102230246251555e-17,0.0 +0.30000000000000004,1.6653345369377338e-17,0.0 +0.4,2.220446049250312e-17,0.0 +0.5,2.77555756156289e-17,0.0 +0.6,3.330669073875468e-17,0.0 +0.7,3.885780586188047e-17,0.0 +0.7999999999999999,4.4408920985006246e-17,0.0 +0.8999999999999999,4.996003610813203e-17,0.0 +1.0,5.5511151231257815e-17,0.0 +1.0999999999999999,6.106226635438359e-17,0.0 +1.2,6.661338147750938e-17,0.0 +1.3,7.216449660063518e-17,0.0 +1.4000000000000001,7.771561172376095e-17,0.0 +1.5000000000000002,8.326672684688674e-17,0.0 +1.6000000000000003,8.881784197001253e-17,0.0 +1.7000000000000004,9.436895709313832e-17,0.0 +1.8000000000000005,9.99200722162641e-17,0.0 +1.9000000000000001,1.0547118733938988e-16,0.0 +2.0000000000000004,1.1102230246251568e-16,0.0 +2.1000000000000005,1.1657341758564147e-16,0.0 +2.2,1.2212453270876723e-16,0.0 +2.3000000000000003,1.2767564783189302e-16,0.0 +2.400000000000001,1.3322676295501883e-16,0.0 +2.500000000000001,1.3877787807814462e-16,0.0 +2.600000000000001,1.443289932012704e-16,0.0 +2.7000000000000006,1.4988010832439617e-16,0.0 +2.8000000000000007,1.5543122344752196e-16,0.0 +2.9000000000000012,1.6098233857064777e-16,0.0 +3.0000000000000013,1.6653345369377356e-16,0.0 +3.1000000000000014,1.7208456881689934e-16,0.0 +3.200000000000001,1.776356839400251e-16,0.0 +3.300000000000001,1.831867990631509e-16,0.0 +3.4000000000000017,1.887379141862767e-16,0.0 +3.5000000000000018,1.942890293094025e-16,0.0 +3.6000000000000014,1.9984014443252826e-16,0.0 +3.7000000000000024,2.053912595556541e-16,0.0 +3.8000000000000016,2.1094237467877983e-16,0.0 +3.900000000000002,2.1649348980190564e-16,0.0 +4.000000000000002,2.220446049250314e-16,0.0 +4.100000000000001,2.2759572004815717e-16,0.0 +4.200000000000001,2.3314683517128293e-16,0.0 +4.300000000000001,2.386979502944087e-16,0.0 +4.4,2.4424906541753446e-16,0.0 +4.5,2.498001805406602e-16,0.0 +4.6,2.55351295663786e-16,0.0 +4.699999999999999,2.6090241078691175e-16,0.0 +4.799999999999999,2.664535259100375e-16,0.0 +4.899999999999999,2.7200464103316327e-16,0.0 +4.999999999999998,2.7755575615628904e-16,0.0 +5.099999999999998,2.831068712794148e-16,0.0 +5.1999999999999975,2.8865798640254056e-16,0.0 +5.299999999999997,2.9420910152566633e-16,0.0 +5.399999999999997,2.997602166487921e-16,0.0 +5.4999999999999964,3.0531133177191785e-16,0.0 +5.599999999999996,3.108624468950436e-16,0.0 +5.699999999999996,3.164135620181694e-16,0.0 +5.799999999999995,3.2196467714129514e-16,0.0 +5.899999999999995,3.275157922644209e-16,0.0 +5.999999999999995,3.3306690738754667e-16,0.0 +6.099999999999994,3.3861802251067243e-16,0.0 +6.199999999999994,3.441691376337982e-16,0.0 +6.299999999999994,3.4972025275692396e-16,0.0 +6.399999999999993,3.552713678800497e-16,0.0 +6.499999999999993,3.608224830031755e-16,0.0 +6.5999999999999925,3.6637359812630124e-16,0.0 +6.699999999999992,3.71924713249427e-16,0.0 +6.799999999999992,3.7747582837255277e-16,0.0 +6.8999999999999915,3.8302694349567853e-16,0.0 +6.999999999999991,3.885780586188043e-16,0.0 +7.099999999999991,3.9412917374193006e-16,0.0 +7.19999999999999,3.996802888650558e-16,0.0 +7.29999999999999,4.052314039881816e-16,0.0 +7.39999999999999,4.1078251911130735e-16,0.0 +7.4999999999999885,4.1633363423443306e-16,0.0 +7.599999999999989,4.2188474935755887e-16,0.0 +7.6999999999999895,4.274358644806847e-16,0.0 +7.799999999999988,4.329869796038104e-16,0.0 +7.899999999999988,4.3853809472693616e-16,0.0 +7.999999999999987,4.440892098500619e-16,0.0 +8.099999999999987,4.496403249731877e-16,0.0 +8.199999999999987,4.551914400963135e-16,0.0 +8.299999999999986,4.607425552194392e-16,0.0 +8.399999999999986,4.66293670342565e-16,0.0 +8.499999999999986,4.718447854656907e-16,0.0 +8.599999999999985,4.773959005888165e-16,0.0 +8.699999999999985,4.829470157119423e-16,0.0 +8.799999999999985,4.88498130835068e-16,0.0 +8.899999999999984,4.940492459581938e-16,0.0 +8.999999999999984,4.996003610813196e-16,0.0 +9.099999999999984,5.051514762044453e-16,0.0 +9.199999999999983,5.107025913275711e-16,0.0 +9.299999999999983,5.162537064506968e-16,0.0 +9.399999999999983,5.218048215738226e-16,0.0 +9.499999999999982,5.273559366969484e-16,0.0 +9.599999999999982,5.329070518200741e-16,0.0 +9.699999999999982,5.384581669431999e-16,0.0 +9.799999999999981,5.440092820663257e-16,0.0 +9.89999999999998,5.495603971894514e-16,0.0 +9.99999999999998,5.551115123125772e-16,0.0 +10.09999999999998,5.606626274357029e-16,0.0 +10.19999999999998,5.662137425588287e-16,0.0 +10.29999999999998,5.717648576819545e-16,0.0 +10.399999999999979,5.773159728050802e-16,0.0 +10.499999999999979,5.82867087928206e-16,0.0 +10.599999999999978,5.884182030513318e-16,0.0 +10.699999999999978,5.939693181744575e-16,0.0 +10.799999999999978,5.995204332975833e-16,0.0 +10.899999999999977,6.050715484207091e-16,0.0 +10.999999999999977,6.106226635438348e-16,0.0 +11.099999999999977,6.161737786669606e-16,0.0 +11.199999999999976,6.217248937900863e-16,0.0 +11.299999999999976,6.272760089132121e-16,0.0 +11.399999999999975,6.328271240363379e-16,0.0 +11.499999999999975,6.383782391594636e-16,0.0 +11.599999999999975,6.439293542825894e-16,0.0 +11.699999999999974,6.494804694057152e-16,0.0 +11.799999999999974,6.550315845288409e-16,0.0 +11.899999999999974,6.605826996519667e-16,0.0 +11.999999999999973,6.661338147750924e-16,0.0 +12.099999999999973,6.716849298982182e-16,0.0 +12.199999999999973,6.77236045021344e-16,0.0 +12.299999999999972,6.827871601444697e-16,0.0 +12.399999999999972,6.883382752675955e-16,0.0 +12.499999999999972,6.938893903907213e-16,0.0 +12.599999999999971,6.99440505513847e-16,0.0 +12.69999999999997,7.049916206369728e-16,0.0 +12.79999999999997,7.105427357600985e-16,0.0 +12.89999999999997,7.160938508832243e-16,0.0 +12.99999999999997,7.216449660063501e-16,0.0 +13.09999999999997,7.271960811294758e-16,0.0 +13.199999999999969,7.327471962526016e-16,0.0 +13.299999999999969,7.382983113757274e-16,0.0 +13.399999999999968,7.438494264988531e-16,0.0 +13.499999999999968,7.494005416219789e-16,0.0 +13.599999999999968,7.549516567451047e-16,0.0 +13.699999999999967,7.605027718682304e-16,0.0 +13.799999999999967,7.660538869913562e-16,0.0 +13.899999999999967,7.716050021144819e-16,0.0 +13.999999999999966,7.771561172376077e-16,0.0 +14.099999999999966,7.827072323607335e-16,0.0 +14.199999999999966,7.882583474838592e-16,0.0 +14.299999999999965,7.93809462606985e-16,0.0 +14.399999999999965,7.993605777301108e-16,0.0 +14.499999999999964,8.049116928532365e-16,0.0 +14.599999999999964,8.104628079763623e-16,0.0 +14.699999999999964,8.16013923099488e-16,0.0 +14.799999999999963,8.215650382226138e-16,0.0 +14.899999999999965,8.271161533457397e-16,0.0 +14.999999999999961,8.326672684688652e-16,0.0 +15.099999999999962,8.382183835919911e-16,0.0 +15.199999999999962,8.437694987151169e-16,0.0 +15.299999999999962,8.493206138382426e-16,0.0 +15.399999999999963,8.548717289613685e-16,0.0 +15.499999999999963,8.604228440844942e-16,0.0 +15.599999999999959,8.659739592076198e-16,0.0 +15.69999999999996,8.715250743307457e-16,0.0 +15.79999999999996,8.770761894538714e-16,0.0 +15.89999999999996,8.826273045769972e-16,0.0 +15.999999999999961,8.881784197001231e-16,0.0 +16.09999999999996,8.937295348232487e-16,0.0 +16.19999999999996,8.992806499463746e-16,0.0 +16.29999999999996,9.048317650695005e-16,0.0 +16.399999999999963,9.103828801926263e-16,0.0 +16.499999999999964,9.159339953157522e-16,0.0 +16.599999999999966,9.21485110438878e-16,0.0 +16.699999999999967,9.270362255620039e-16,0.0 +16.79999999999997,9.325873406851298e-16,0.0 +16.89999999999997,9.381384558082556e-16,0.0 +16.99999999999997,9.436895709313815e-16,0.0 +17.099999999999973,9.492406860545073e-16,0.0 +17.199999999999974,9.547918011776332e-16,0.0 +17.299999999999976,9.60342916300759e-16,0.0 +17.399999999999977,9.65894031423885e-16,0.0 +17.49999999999998,9.714451465470108e-16,0.0 +17.59999999999998,9.769962616701367e-16,0.0 +17.69999999999998,9.825473767932625e-16,0.0 +17.799999999999983,9.880984919163884e-16,0.0 +17.899999999999984,9.936496070395142e-16,0.0 +17.999999999999986,9.9920072216264e-16,0.0 +18.099999999999987,1.004751837285766e-15,0.0 +18.19999999999999,1.0103029524088918e-15,0.0 +18.29999999999999,1.0158540675320177e-15,0.0 +18.39999999999999,1.0214051826551435e-15,0.0 +18.499999999999993,1.0269562977782694e-15,0.0 +18.599999999999994,1.0325074129013953e-15,0.0 +18.699999999999996,1.0380585280245211e-15,0.0 +18.799999999999997,1.043609643147647e-15,0.0 +18.9,1.0491607582707729e-15,0.0 +19.0,1.0547118733938987e-15,0.0 +19.1,1.0602629885170246e-15,0.0 +19.200000000000003,1.0658141036401504e-15,0.0 +19.300000000000004,1.0713652187632763e-15,0.0 +19.400000000000006,1.0769163338864022e-15,0.0 +19.500000000000007,1.082467449009528e-15,0.0 +19.60000000000001,1.0880185641326539e-15,0.0 +19.70000000000001,1.0935696792557797e-15,0.0 +19.80000000000001,1.0991207943789056e-15,0.0 +19.900000000000013,1.1046719095020315e-15,0.0 +20.000000000000014,1.1102230246251573e-15,0.0 +20.100000000000016,1.1157741397482832e-15,0.0 +20.200000000000017,1.121325254871409e-15,0.0 +20.30000000000002,0.0,0.0 +20.399987538147936,0.0013671557150166803,0.0 +20.499900316364652,0.0054676006480075985,0.0 +20.599663630616394,0.012298268927097164,0.0 +20.52616230766001,0.004930129155760037,0.0 +20.426921530655086,-0.007343626548595887,0.0 +20.32805343623484,-0.022326176576537883,0.0 +20.229631947330805,-0.04000631858795244,0.0 +20.13173065295093,-0.060370833273464486,0.0 +20.034422753157635,-0.08340449423841373,0.0 +19.93778100433665,-0.10909007938751655,0.0 +19.84187766479763,-0.1374083838016975,0.0 +19.746784440747163,-0.1683382340974619,0.0 +19.652572432674567,-0.20185650425807727,0.0 +19.559312082190594,-0.23793813292471971,0.0 +19.46707311935877,-0.2765561421346672,0.0 +19.375924510558733,-0.3176816574925169,0.0 +19.285934406920614,-0.36128392975935425,0.0 +19.197170093368932,-0.4073303578437292,0.0 +19.109697938314167,-0.4557865131772457,0.0 +19.023583344029596,-0.5066161654565449,0.0 +18.93889069775051,-0.5597813097324302,0.0 +18.855683323532343,-0.615242194825882,0.0 +18.774023434903786,-0.6729573530497173,0.0 +18.693972088350165,-0.7328836312136658,0.0 +18.615589137661985,-0.7949762228896875,0.0 +18.538933189182693,-0.8591887019134016,0.0 +18.464061557989158,-0.9254730570965805,0.0 +18.391030225037614,-0.9937797281247577,0.0 +18.31989379530713,-1.0640576426130972,0.0 +18.25070545697186,-1.1362542542928378,0.0 +18.183516941632668,-1.2103155822997387,0.0 +18.11681825596512,-1.2848189821117841,0.0 +18.048140850868222,-1.3575017925196606,0.0 +17.97750187000663,-1.4282796936079698,0.0 +17.90495412961572,-1.4970997652717235,0.0 +17.830551873096038,-1.5639105512602882,0.0 +17.754350730455897,-1.6286620976508384,0.0 +17.676407676717258,-1.6913059901985248,0.0 +17.596780989315917,-1.7517953905354469,0.0 +17.51553020452793,-1.8100850711913459,0.0 +17.432716072954754,-1.8661314494098449,0.0 +17.348400514100494,-1.9198926197349477,0.0 +17.262646570075166,-1.9713283853434362,0.0 +17.175518358458564,-2.0204002880997307,0.0 +17.08708102436004,-2.067071637310749,0.0 +16.997400691709995,-2.111307537159263,0.0 +16.906544413819503,-2.1530749127952387,0.0 +16.814580123245072,-2.1923425350656536,0.0 +16.72157658099596,-2.2290810438643,0.0 +16.6276033251221,-2.263262970084114,0.0 +16.532730618721022,-2.2948627561556223,0.0 +16.43702939740265,-2.323856775156145,0.0 +16.340571216251295,-2.350223348475467,0.0 +16.243428196324448,-2.3739427620247726,0.0 +16.14567297072841,-2.3949972809767224,0.0 +16.047378630311055,-2.4133711630256496,0.0 +15.948618669012365,-2.4290506701579635,0.0 +15.84946692891356,-2.4420240789239593,0.0 +15.74999754502592,-2.452281689203354,0.0 +15.824349913856949,-2.4469683510106868,0.0 +15.92423128589668,-2.442163310002374,0.0 +16.02420669425922,-2.440090962933454,0.0 diff --git a/GEMstack/knowledge/routes/reverse_15m.csv b/GEMstack/knowledge/routes/reverse_15m.csv new file mode 100644 index 000000000..9568aef2e --- /dev/null +++ b/GEMstack/knowledge/routes/reverse_15m.csv @@ -0,0 +1,39 @@ +0.0,0,0 +-0.5,0,0 +-1.0,0,0 +-1.5,0,0 +-2.0,0,0 +-2.5,0.5,0 +-3.0,1.0,0 +-3.5,1.0,0 +-4.0,1.0,0 +-4.5,1.0,0 +-5.0,1.0,0 +-5.5,1.0,0 +-6.0,1.0,0 +-6.5,1.0,0 +-7.0,1.0,0 +-7.5,1.0,0 +-8.0,1.0,0 +-8.5,0.5,0 +-9.0,0,0 +-9.5,-0.5,0 +-10.0,-1.0,0 +-10.5,-1.0,0 +-11.0,-1.0,0 +-11.5,-1.0,0 +-12.0,-1.0,0 +-12.5,-1.0,0 +-13.0,-1.0,0 +-13.5,-1.0,0 +-14.0,-1.0,0 +-14.5,-1.0,0 +-15.0,-1.0,0 +-15.5,-1.0,0 +-16.0,-1.0,0 +-16.5,-1.0,0 +-17.0,-1.0,0 +-17.5,-1.0,0 +-18.0,-1.0,0 +-18.5,-1.0,0 +-19.0,-1.0,0 \ No newline at end of file diff --git a/GEMstack/onboard/planning/mpc.py b/GEMstack/onboard/planning/mpc.py new file mode 100644 index 000000000..d811567ec --- /dev/null +++ b/GEMstack/onboard/planning/mpc.py @@ -0,0 +1,361 @@ +from ...utils import settings +from ...state.vehicle import VehicleState,VehicleGearEnum +from ...state.trajectory import Trajectory, Path +from ...knowledge.vehicle.geometry import front2steer +from ..component import Component +import numpy as np +import casadi +import math +import time + +########################### +# Bo-Hao Wu's code # +########################### + +class MPCController(object): + """Model Predictive Controller for trajectory tracking.""" + def __init__(self, T=None, dt=None, desired_speed=None): + self.T = T if T is not None else settings.get('control.mpc.horizon', 30) + self.dt = dt if dt is not None else settings.get('control.mpc.dt', 0.2) + self.L = settings.get('vehicle.geometry.wheelbase') + self.v_bounds = [-settings.get('vehicle.limits.max_reverse_speed'), settings.get('vehicle.limits.max_speed')] + self.delta_bounds = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')] + self.delta_rate_bounds = [-0.4, 0.4] # Predefined front wheel rate limit to simplify computation + self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')] + self.a_bounds = [-settings.get('vehicle.limits.max_deceleration'), settings.get('vehicle.limits.max_acceleration')] + self.switch_gear = settings.get('control.mpc.switch_gear', False) + + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.pure_pursuit.desired_speed',"path") + self.desired_speed = self.desired_speed_source if isinstance(self.desired_speed_source,(int,float)) else None + + self.prev_x = None # Previous state trajectory + self.prev_u = None # Previous control inputs + self.path = None + self.path_arg = None + self.iter = 0 + + def set_path(self, path : Path): + if path == self.path_arg: + return + self.path_arg = path + self.iter = 0 + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + self.current_path_parameter = 0.0 + # self.prev_u = None + # self.prev_x = None + if not isinstance(path,Trajectory): + if self.desired_speed_source in ['path', 'trajectory']: + print("rase") + raise ValueError("MPC: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + self.path = path.arc_length_parameterize(self.desired_speed) + self.current_traj_parameter = 0.0 + else: + self.path = path + self.current_traj_parameter = self.path.domain()[0] + + def clip_reverse_path_with_times(self, points, times): + def angle_between(p1, p2): + delta = p2 - p1 + return np.arctan2(delta[1], delta[0]) + + points = np.array(points) + times = np.array(times) + + if len(points) < 3: + return points, times + + ref_angle = angle_between(points[0], points[1]) + clipped_points = [points[0]] + clipped_times = [times[0]] + + for i in range(1, len(points)): + angle = angle_between(points[i-1], points[i]) + angle_diff = np.abs((angle - ref_angle + np.pi) % (2 * np.pi) - np.pi) + if angle_diff > np.pi / 2: + break + clipped_points.append(points[i]) + clipped_times.append(times[i]) + + return np.array(clipped_points), np.array(clipped_times) + + def compute(self, state: VehicleState, component: Component = None): + """Compute the control commands using MPC.""" + start_time = time.time() + if self.iter < 10 and self.prev_x is not None: + self.iter += 1 + # return float(self.prev_u[self.iter, 0]), float(self.prev_x[self.iter + 1, 4]) + self.iter = 0 + + if self.path is not None: + if self.path.frame != state.pose.frame: + print("Transforming trajectory from",self.path.frame.name,"to",state.pose.frame.name) + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + x0 = np.array([state.pose.x, state.pose.y, state.pose.yaw % (2 * np.pi), state.v, state.front_wheel_angle]) + # print(x0) + + if self.switch_gear: + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+1.0], True) + else: + closest_dist,closest_time = self.path.closest_point_local((x0[0], x0[1]),[self.current_traj_parameter-0.0,self.current_traj_parameter+5.0], True) + self.current_traj_parameter = closest_time + + times = self.path.times + points = self.path.points + j = math.floor(self.current_path_parameter) + while j < len(times) - 1 and times[j+1] < closest_time: + j += 1 + self.current_path_parameter = j + + if self.switch_gear: + # Slice path from j + sliced_points = points[j:] + sliced_times = times[j:] + + # Clip reversed part + new_points, new_times = self.clip_reverse_path_with_times(sliced_points, sliced_times) + else: + new_points = points + new_times = times + + # Interpolate trajectory points to match MPC time horizon + traj_points = [] + j = 0 + for i in range(self.T + 1): + t_query = closest_time + i * self.dt + if t_query <= new_times[0]: + traj_points.append(new_points[0]) + elif t_query >= new_times[-1]: + traj_points.append(new_points[-1]) + else: + while j < len(new_times) - 2 and new_times[j+1] < t_query: + j += 1 + alpha = (t_query - new_times[j]) / (new_times[j+1] - new_times[j]) + pt = (1 - alpha) * np.array(new_points[j]) + alpha * np.array(new_points[j+1]) + traj_points.append(pt) + + # print("trajectory points: ", traj_points) + # traj_str = ", ".join( + # [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + # ) + # print(f"traj_points = [{traj_str}]") + + # Apply gradually decreasing offset correction + cur_offset = np.array([state.pose.x, state.pose.y]) - np.array(traj_points[0][0:2]) + overcorrection_guard = 5 + for i in range(len(traj_points)): + s = i / (len(traj_points) + overcorrection_guard) # Normalized [0, 1] and add some offset to prevent overcorrection + decay_ratio = (1 - s) ** 1 # linear decay on each iteration to perform smooth offset correction over time + + # Get direction of trajectory at this point + if i < len(traj_points) - 1: + tangent = np.array(traj_points[i+1]) - np.array(traj_points[i]) + else: + tangent = np.array(traj_points[i]) - np.array(traj_points[i-1]) + + tangent_norm = np.linalg.norm(tangent) + if tangent_norm == 0: + continue # Skip degenerate points + + tangent /= tangent_norm + normal = np.array([-tangent[1], tangent[0]]) # Rotate tangent by 90° to get normal + + # Project initial offset onto this normal + offset_proj = np.dot(cur_offset, normal) * normal + + # Apply decaying lateral offset + traj_points[i] = np.array(traj_points[i]) + decay_ratio * offset_proj + + # print("trajectory points after correction: ", traj_points) + # traj_corrected_str = ", ".join( + # [f"np.array([{round(p[0], 8)}, {round(p[1], 8)}])" for p in traj_points] + # ) + # print(f"traj_corrected = [{traj_corrected_str}]") + + # Compute target angles + target_angles = [] + prev_angle = x0[2] + + for i in range(1, len(traj_points)): + dx = traj_points[i][0] - traj_points[i-1][0] + dy = traj_points[i][1] - traj_points[i-1][1] + raw_angle = np.arctan2(dy, dx) % (2 * np.pi) + + # Convert raw_angle to be close to prev_angle (modulo 2π), within ±0.5π + # Compute smallest angular difference + delta = ((raw_angle - prev_angle + np.pi) % (2 * np.pi)) - np.pi + + # Clip delta to ±0.5π + if delta > 0.5 * np.pi: + delta -= np.pi + elif delta < -0.5 * np.pi: + delta += np.pi + + corrected_angle = prev_angle + delta + assert abs(corrected_angle - prev_angle) < 0.5 * np.pi + target_angles.append(corrected_angle) + prev_angle = corrected_angle + + # print(target_angles) + + # delta_desired = [] + # for i in range(1, len(target_angles)): + # ds = np.linalg.norm(np.array(traj_points[i]) - np.array(traj_points[i-1])) + # dtheta = target_angles[i] - target_angles[i-1] + # dtheta = (dtheta + np.pi) % (2 * np.pi) - np.pi # Normalize to [-pi, pi] + + # curvature = dtheta / ds if ds > 1e-6 else 0.0 + # delta_ref = np.arctan(self.L * curvature) + # delta_desired.append(delta_ref) + + # Optimization setup + opti = casadi.Opti() + x = opti.variable(self.T+1, 5) # [x, y, theta, v, delta] + u = opti.variable(self.T, 2) # [a, delta_dot] + + def model(x, u): + """Dynamic model of the vehicle using kinematic bicycle model""" + px, py, theta, v, delta = x[0], x[1], x[2], x[3], x[4] + a, delta_dot = u[0], u[1] + dx = v * casadi.cos(theta) + dy = v * casadi.sin(theta) + dtheta = v * casadi.tan(delta) / self.L + dv = a + ddelta = delta_dot + return casadi.vertcat(dx, dy, dtheta, dv, ddelta) + + obj = 0 + for t in range(self.T): + # Vehicle dynamics + x_next = x[t,:] + self.dt * model(x[t,:], u[t,:]).T + opti.subject_to(x[t+1,:] == x_next) + target = traj_points[t+1] + # Cost function + weight = 10 if t < 3 else 1 # Larger weight for the first three points + obj += weight * casadi.sumsqr(x[t + 1, 0:2] - casadi.reshape(target[0:2], 1, 2)) + + # Control effort penalty + obj += 0.1 * casadi.sumsqr(u[t, :]) + + # Heading angle error + theta_error = x[t + 1, 2] - target_angles[t] + obj += 1 * weight * casadi.sumsqr(theta_error) + + # Front wheel angle error + # if t != self.T - 1: + # delta_error = x[t + 1, 4] - delta_desired[t] + # obj += 0.1 * weight * casadi.sumsqr(delta_error) + + # Initial condition + opti.subject_to(x[0, :] == casadi.reshape(x0, 1, 5)) + + # Constraints + for t in range(self.T): + opti.subject_to(opti.bounded(self.a_bounds[0], u[t,0], self.a_bounds[1])) # a + opti.subject_to(opti.bounded(self.delta_rate_bounds[0], u[t,1], self.delta_rate_bounds[1])) # delta_dot + opti.subject_to(opti.bounded(self.delta_bounds[0], x[t+1,4], self.delta_bounds[1])) # delta as state + opti.subject_to(opti.bounded(self.v_bounds[0], x[t+1,3], self.v_bounds[1])) # v + + # Initial guess + if self.prev_x is not None and self.prev_u is not None: + if len(self.prev_x) == self.T+1 and len(self.prev_u) == self.T: + opti.set_initial(x, np.vstack((self.prev_x[1:], self.prev_x[-1]))) + opti.set_initial(u, np.vstack((self.prev_u[1:], self.prev_u[-1]))) + + # Solver settings + opti.minimize(obj) + p_opts = {"expand": True, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.sb': 'yes'} + s_opts = {"max_iter": 150} + + opti.solver("ipopt", p_opts, s_opts) + + try: + # Solve the optimization problem + sol = opti.solve() + acc = float(sol.value(u[0,0])) + # delta = float(sol.value(u[0,1])) + delta = float(sol.value(x[1,4])) + self.prev_x = sol.value(x) + self.prev_u = sol.value(u) + ###Jugal's Code start here### + ###adding heading error and position error(similar to cross track error) for evaluation metric### + # NEW: Position and Heading Error + vehicle_x = state.pose.x + vehicle_y = state.pose.y + vehicle_yaw = state.pose.yaw + + path_x = self.path.points[self.current_path_parameter][0] + path_y = self.path.points[self.current_path_parameter][1] + path_yaw = target_angles[0] + + position_error = np.linalg.norm([vehicle_x - path_x, vehicle_y - path_y]) + heading_error = ((vehicle_yaw - path_yaw + np.pi) % (2 * np.pi)) - np.pi + + total_time_elapse = time.time() - start_time + ###Jugal's code end here### + if component is not None: + component.debug("mpc/accel", acc) + component.debug("mpc/delta", delta) + component.debug("mpc/position_error", position_error) + component.debug("mpc/heading_error", heading_error) + component.debug("mpc/closest_time", closest_time) + component.debug("mpc/state_x", state.pose.x) + component.debug("mpc/state_y", state.pose.y) + component.debug("mpc/state_yaw", state.pose.yaw) + component.debug("mpc/target_x", self.path.points[self.current_path_parameter][0]) + component.debug("mpc/target_y", self.path.points[self.current_path_parameter][1]) + component.debug("mpc/target_theta", target_angles[0]) + component.debug("mpc/time_elapsed", total_time_elapse) + + # xy_array = [f"np.array([{round(self.prev_x[t,0],8)}, {round(self.prev_x[t,1],8)}])" for t in range(self.prev_x.shape[0])] + # print("mpc = [", ", ".join(xy_array), "]") + + print(self.current_path_parameter) + print(acc, delta) + # print(self.prev_u[0]) + # print(self.prev_x[0]) + # print(self.prev_x[1]) + # print(self.prev_u) + # print(x0[4]) + # print(delta) + return acc, delta + except RuntimeError: + # Handle optimization failure + print("MPC optimization failed.") + return 0.0, 0.0 + + +class MPCTrajectoryTracker(Component): + def __init__(self, vehicle_interface=None, **args): + self.mpc = MPCController(**args) + self.vehicle_interface = vehicle_interface + self.counter = 0 + + def rate(self): + return 5.0 + + def state_inputs(self): + return ['vehicle', 'trajectory'] + + def state_outputs(self): + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + self.mpc.set_path(trajectory) + accel, delta = self.mpc.compute(vehicle, self) + + # Clip acceleration and steering angle to vehicle limits + accel = np.clip(accel, self.mpc.a_bounds[0], self.mpc.a_bounds[1]) + delta = np.clip(delta, self.mpc.delta_bounds[0], self.mpc.delta_bounds[1]) + + # Convert delta to steering angle + steering_angle = np.clip(front2steer(delta), + self.mpc.steering_angle_range[0], self.mpc.steering_angle_range[1]) + self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel, steering_angle, vehicle)) + + def healthy(self): + return self.mpc.path is not None diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py new file mode 100644 index 000000000..2bc8579ab --- /dev/null +++ b/GEMstack/onboard/planning/stanley.py @@ -0,0 +1,476 @@ +import numpy as np +from math import sin, cos, atan2 + +from ...mathutils.control import PID +from ...utils import settings +from ...mathutils import transforms +from ...knowledge.vehicle.geometry import front2steer +from ...knowledge.vehicle.dynamics import acceleration_to_pedal_positions +from ...state.vehicle import VehicleState, ObjectFrameEnum +from ...state.trajectory import Path, Trajectory +from ..interface.gem import GEMVehicleCommand +from ..component import Component + +##################################### +# 1. Angle normalization +##################################### +def normalise_angle(angle): + """Wraps an angle to the [-pi, pi] range.""" + while angle > np.pi: + angle -= 2.0 * np.pi + while angle < -np.pi: + angle += 2.0 * np.pi + return angle + +##################################### +# 2. Stanley-based controller with longitudinal PID +##################################### +class Stanley(object): + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None + ): + """ + Initializes the Stanley controller. + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param desired_speed: Desired speed (float) or 'path'/'trajectory'. + """ + self.k = control_gain if control_gain is not None else settings.get('control.stanley.control_gain') + self.k_soft = softening_gain if softening_gain is not None else settings.get('control.stanley.softening_gain') + + self.max_steer = settings.get('vehicle.geometry.max_wheel_angle') + self.wheelbase = settings.get('vehicle.geometry.wheelbase') + + self.speed_limit = settings.get('vehicle.limits.max_speed') + self.reverse_speed_limit = settings.get('vehicle.limits.max_reverse_speed') + self.max_accel = settings.get('vehicle.limits.max_acceleration') + self.max_decel = settings.get('vehicle.limits.max_deceleration') + + # PID controller for longitudinal speed control + p = settings.get('control.longitudinal_control.pid_p') + d = settings.get('control.longitudinal_control.pid_d') + i = settings.get('control.longitudinal_control.pid_i') + self.pid_speed = PID(p, d, i, windup_limit=20) + + # Determine desired speed source + if desired_speed is not None: + self.desired_speed_source = desired_speed + else: + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + if isinstance(self.desired_speed_source, (int, float)): + self.desired_speed = self.desired_speed_source + else: + self.desired_speed = None + + # Initialize state variables + self.path_arg = None + self.path = None + self.trajectory = None + self.current_path_parameter = 0.0 + self.current_traj_parameter = 0.0 + self.t_last = None + self.reverse = None + self.sharp_turn = False + + def set_path(self, path: Path): + # Sets the path for the controller. + if path == self.path_arg: + return + self.path_arg = path + + # Ensure path is 2D + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + + # Parameterize path by arc length + if not isinstance(path, Trajectory): + self.path = path.arc_length_parameterize() + self.trajectory = None + self.current_traj_parameter = 0.0 + if self.desired_speed_source in ['path', 'trajectory']: + raise ValueError("Stanley: Provided path has no timing. Either provide a Trajectory or set a constant desired_speed.") + else: + self.path = path.arc_length_parameterize() + self.trajectory = path + self.current_traj_parameter = self.trajectory.domain()[0] + + self.current_path_parameter = 0.0 + + def _find_front_axle_position(self, x, y, yaw): + """Compute front-axle world position from the center/rear and yaw.""" + fx = x + self.wheelbase * cos(yaw) + fy = y + self.wheelbase * sin(yaw) + return fx, fy + + def _find_rear_axle_position(self, x, y, yaw): + """Compute rear-axle world position from the vehicle's reference point and yaw.""" + rx = x - self.wheelbase * cos(yaw) + ry = y - self.wheelbase * sin(yaw) + return rx, ry + + def initialize_state_and_direction(self, state: VehicleState): + # Initializes the controller's state and determines initial driving direction (forward/reverse). + if self.path is None: + raise ValueError("Stanley: Path must be set before initializing state and direction.") + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + path_domain_start, path_domain_end = self.path.domain() + search_end = min(path_domain_end, path_domain_start + 5.0) + search_domain_start = [path_domain_start, search_end] + _, closest_param_at_start = self.path.closest_point_local((fx, fy), search_domain_start) + tangent_at_start = self.path.eval_tangent(closest_param_at_start) + initial_reverse = False + if np.linalg.norm(tangent_at_start) > 1e-6: + path_yaw_at_start = atan2(tangent_at_start[1], tangent_at_start[0]) + heading_diff = normalise_angle(path_yaw_at_start - curr_yaw) + initial_reverse = abs(heading_diff) > (np.pi / 2.0) + self.pid_speed.reset() + self.current_path_parameter = closest_param_at_start + if self.trajectory: + self.current_traj_parameter = self.trajectory.domain()[0] + return initial_reverse + + def is_target_behind_vehicle(self, vehicle_pose, target_point_coords): + """Checks if a target point is behind the vehicle.""" + curr_x = vehicle_pose.x + curr_y = vehicle_pose.y + curr_yaw = vehicle_pose.yaw + if curr_yaw is None: + return False + target_x = target_point_coords[0] + target_y = target_point_coords[1] + vec_x = target_x - curr_x + vec_y = target_y - curr_y + if abs(vec_x) < 1e-6 and abs(vec_y) < 1e-6: + return False + heading_x = cos(curr_yaw) + heading_y = sin(curr_yaw) + dot_product = vec_x * heading_x + vec_y * heading_y + return (dot_product < 0) + + def _check_sharp_turn_ahead(self, lookahead_s=3.0, threshold_angle=np.pi/2.0, num_steps=4): + """Checks for sharp turns within a lookahead distance on the path.""" + if not self.path: + return False + + path = self.path + current_s = self.current_path_parameter + domain_start, domain_end = path.domain() + + if current_s >= domain_end - 1e-3: + return False + + step_s = lookahead_s / num_steps + s_prev = current_s + + try: + tangent_prev = path.eval_tangent(s_prev) + if np.linalg.norm(tangent_prev) < 1e-6: + s_prev_adjusted = min(s_prev + step_s / 2, domain_end) + if s_prev_adjusted <= s_prev: + return False + tangent_prev = path.eval_tangent(s_prev_adjusted) + if np.linalg.norm(tangent_prev) < 1e-6: + return False + s_prev = s_prev_adjusted + + angle_prev = atan2(tangent_prev[1], tangent_prev[0]) + + except Exception as e: + return False + + for i in range(num_steps): + s_next = s_prev + step_s + s_next = min(s_next, domain_end) + + if s_next <= s_prev + 1e-6: + break + + try: + tangent_next = path.eval_tangent(s_next) + if np.linalg.norm(tangent_next) < 1e-6: + s_prev = s_next + continue + + angle_next = atan2(tangent_next[1], tangent_next[0]) + except Exception as e: + break + + angle_change = abs(normalise_angle(angle_next - angle_prev)) + + if angle_change > threshold_angle: + return True + + angle_prev = angle_next + s_prev = s_next + return False + + def compute(self, state: VehicleState, component: Component = None): + # Computes the control commands (acceleration and steering angle). + t = state.pose.t + if self.t_last is None: + self.t_last = t + dt = 0 + else: + dt = t - self.t_last + + curr_x = state.pose.x + curr_y = state.pose.y + curr_yaw = state.pose.yaw if state.pose.yaw is not None else 0.0 + speed = state.v + + # Handle no path case + if self.path is None: + if component: + component.debug_event("No path provided to Stanley controller. Doing nothing.") + self.t_last = t + self.pid_speed.reset() + return (0.0, 0.0) + + # Transform path and trajectory to vehicle frame if necessary + if self.path.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming path from {self.path.frame.name} to {state.pose.frame.name}") + self.path = self.path.to_frame(state.pose.frame, current_pose=state.pose) + + if self.trajectory and self.trajectory.frame != state.pose.frame: + if component: + component.debug_event(f"Transforming trajectory from {self.trajectory.frame.name} to {state.pose.frame.name}") + self.trajectory = self.trajectory.to_frame(state.pose.frame, current_pose=state.pose) + + # Initialize reverse direction if not set + if self.reverse is None: + self.reverse = self.initialize_state_and_direction(state) + + # Find front or rear axle position based on direction + if self.reverse: + fx, fy = self._find_rear_axle_position(curr_x, curr_y, curr_yaw) + else: + fx, fy = self._find_front_axle_position(curr_x, curr_y, curr_yaw) + + # Find the closest point on the path + search_start = self.current_path_parameter + search_end = self.current_path_parameter + 5.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # Get target point and tangent on the path + target_x, target_y = self.path.eval(self.current_path_parameter) + tangent = self.path.eval_tangent(self.current_path_parameter) + path_yaw = atan2(tangent[1], tangent[0]) + + dx = fx - target_x + dy = fy - target_y + + # Check for sharp turn ahead and potentially switch to reverse + if not self.reverse: + try: + is_sharp_turn_ahead = self._check_sharp_turn_ahead( + lookahead_s=2.0, + threshold_angle=np.pi/2.0 + ) + except Exception as e: + is_sharp_turn_ahead = False + if is_sharp_turn_ahead and not self.sharp_turn: + self.sharp_turn = True + use_reverse = self.is_target_behind_vehicle(state.pose, (target_x, target_y)) + if use_reverse and self.sharp_turn: + self.reverse = True + self.sharp_turn = False + + # Calculate cross-track error based on direction + if self.reverse: + cross_track_error = dx * (-tangent[1]) + dy * tangent[0] + else: + left_vec = np.array([sin(curr_yaw), -cos(curr_yaw)]) + cross_track_error = np.sign(np.dot(np.array([dx, dy]), left_vec)) * closest_dist + + # Calculate yaw error + yaw_error = normalise_angle(path_yaw - curr_yaw) + + desired_speed = abs(self.desired_speed) + feedforward_accel = 0.0 + + # Stanley control logic for reverse direction + if self.reverse: + heading_term = -yaw_error + cross_term_input = self.k * (-cross_track_error) / (self.k_soft + abs(speed)) + cross_term = atan2(cross_term_input, 1.0) + desired_steering_angle = heading_term + cross_term + + # Determine desired speed from trajectory in reverse + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + current_trajectory_param_for_eval = self.current_path_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + current_trajectory_param_for_eval = self.trajectory.parameter_to_time(self.current_path_parameter) + + if self.trajectory is not None: + deriv = self.trajectory.eval_derivative(current_trajectory_param_for_eval) + path_speed_magnitude = np.linalg.norm(deriv) + else: + path_speed_magnitude = 0.0 + + desired_speed = min(path_speed_magnitude, self.reverse_speed_limit) + + desired_speed = desired_speed * -1.0 + + # Handle reaching the start in reverse + is_at_start_backward = self.current_path_parameter <= self.path.domain()[0] + 1e-3 + + if is_at_start_backward: + desired_speed = 0.0 + feedforward_accel = -2.0 * -1.0 + + # Calculate feedforward acceleration for reverse + else: + difference_dt = 0.1 + current_speed_abs = abs(speed) + if current_speed_abs < 0.1: estimated_path_step = 0.1 * difference_dt * -1.0 + else: estimated_path_step = current_speed_abs * difference_dt * -1.0 + + future_parameter = self.current_path_parameter + estimated_path_step + + future_parameter = np.clip(future_parameter, self.path.domain()[0], self.path.domain()[-1]) + + future_trajectory_param_for_eval = future_parameter + if hasattr(self.trajectory, 'parameter_to_time'): + future_trajectory_param_for_eval = self.trajectory.parameter_to_time(future_parameter) + + if self.trajectory is not None: + future_deriv = self.trajectory.eval_derivative(future_trajectory_param_for_eval) + next_path_speed_magnitude = min(np.linalg.norm(future_deriv), self.speed_limit) + else: + next_path_speed_magnitude = 0.0 + + next_desired_speed = next_path_speed_magnitude * -1.0 + + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + + # Reduce speed based on cross-track error in reverse + desired_speed_magnitude_slowed = abs(desired_speed) * np.exp(-abs(cross_track_error) * 0.6) + desired_speed = desired_speed_magnitude_slowed * -1.0 if desired_speed_magnitude_slowed != 0 else 0.0 + + # Stanley control logic for forward direction + else: + cross_term = atan2(self.k * cross_track_error, self.k_soft + speed) + desired_steering_angle = yaw_error + cross_term + + # Determine desired speed from trajectory in forward + if self.trajectory and self.desired_speed_source in ['path', 'trajectory']: + if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]: + if component: + component.debug_event("Stanley: Past the end of trajectory, stopping.") + desired_speed = 0.0 + feedforward_accel = -2.0 + else: + if self.desired_speed_source == 'path': + current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter) + else: + self.current_traj_parameter += dt + current_trajectory_time = self.current_traj_parameter + + deriv = self.trajectory.eval_derivative(current_trajectory_time) + desired_speed = min(np.linalg.norm(deriv), self.speed_limit) + + # Calculate feedforward acceleration for forward + difference_dt = 0.1 + future_t = current_trajectory_time + difference_dt + if future_t > self.trajectory.domain()[1]: + future_t = self.trajectory.domain()[1] + future_deriv = self.trajectory.eval_derivative(future_t) + next_desired_speed = min(np.linalg.norm(future_deriv), self.speed_limit) + feedforward_accel = (next_desired_speed - desired_speed) / difference_dt + feedforward_accel = np.clip(feedforward_accel, -self.max_decel, self.max_accel) + # Use constant desired speed if no trajectory or source is not path/trajectory + else: + if desired_speed is None: + desired_speed = 4.0 + + # Reduce speed based on cross-track error in forward + desired_speed *= np.exp(-abs(cross_track_error) * 0.6) + + # Apply speed limits + if self.reverse and abs(desired_speed) > self.reverse_speed_limit: + desired_speed = self.reverse_speed_limit * -1.0 if desired_speed != 0 else 0.0 + elif not self.reverse and desired_speed > self.speed_limit: + desired_speed = self.speed_limit + + # Longitudinal control using PID + speed_error = desired_speed - speed + output_accel = self.pid_speed.advance(e=speed_error, t=t, feedforward_term=feedforward_accel) + # Clamp acceleration to limits + if output_accel > self.max_accel: + output_accel = self.max_accel + elif output_accel < -self.max_decel: + output_accel = -self.max_decel + + # Stop acceleration if at target speed and decelerating + if desired_speed == 0 and abs(speed) < 0.01 and output_accel < 0: + output_accel = 0.0 + + # Debugging information + if component is not None: + component.debug('curr pt',(curr_x,curr_y)) + component.debug("desired_x",target_x) + component.debug("desired_y",target_y) + component.debug("Stanley: path param", self.current_path_parameter) + component.debug("Stanley: crosstrack dist", closest_dist) + component.debug("crosstrack error", cross_track_error) + component.debug("Stanley: yaw_error", yaw_error) + component.debug('steering_angle', desired_steering_angle) + component.debug("Stanley: desired_speed (m/s)", desired_speed) + component.debug("Stanley: feedforward_accel (m/s^2)", feedforward_accel) + component.debug("Stanley: output_accel (m/s^2)", output_accel) + + self.t_last = t + return (output_accel, desired_steering_angle) + +##################################### +# 3. Tracker component +##################################### +class StanleyTrajectoryTracker(Component): + + def __init__(self, vehicle_interface=None, **kwargs): + # Initializes the Stanley trajectory tracker component. + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + + def rate(self): + return 50.0 # Component update rate + + def state_inputs(self): + return ["vehicle", "trajectory"] # Required state inputs + + def state_outputs(self): + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + # Updates the controller with current state and trajectory. + self.stanley.set_path(trajectory) + + # Compute control commands using Stanley controller + accel, f_delta = self.stanley.compute(vehicle, self) + + # Convert front wheel angle to steering angle and clip + steering_angle = front2steer(f_delta) + steering_angle = np.clip( + steering_angle, + settings.get('vehicle.geometry.min_steering_angle', -0.5), + settings.get('vehicle.geometry.max_steering_angle', 0.5) + ) + + # Create and send vehicle command + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + self.vehicle_interface.send_command(cmd) + + def healthy(self): + """Optional: check if the controller has a valid path.""" + return self.stanley.path is not None \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 368cb5b9d..17abf89ee 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,12 +16,23 @@ drive: planning: route_planning: type: StaticRoutePlanner + # args: [!relative_path '../GEMstack/knowledge/routes/parallel_parking.csv'] args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/reverse_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/offset.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/backward_15m.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/forward_curve.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/backward_curve.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/yamada.csv'] + # args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_oval.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker trajectory_tracking: - type: pure_pursuit.PurePursuitTrajectoryTracker + # type: pure_pursuit.PurePursuitTrajectoryTracker + type: stanley.StanleyTrajectoryTracker + # type: mpc.MPCTrajectoryTracker args: {desired_speed: 2.5} #approximately 5mph print: False log: @@ -66,7 +77,8 @@ variants: vehicle_interface: type: gem_simulator.GEMDoubleIntegratorSimulationInterface args: - scene: !relative_path '../scenes/xyhead_demo.yaml' + scene: !relative_path '../scenes/xyhead_demo.yaml' + drive: perception: state_estimation : OmniscientStateEstimator @@ -90,7 +102,7 @@ variants: type: obstacle_detection.GazeboObstacleDetector args: tracked_obstacle_prefixes: ['cone'] - visualization: !include "mpl_visualization.yaml" + # visualization: !include "mpl_visualization.yaml" log_ros: log: ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file diff --git a/logs/logplot_mpc.py b/logs/logplot_mpc.py new file mode 100644 index 000000000..3c6fbb79f --- /dev/null +++ b/logs/logplot_mpc.py @@ -0,0 +1,90 @@ +###Jugal's Code### +###Following file gives the plots for MPC logs. Go inside logs folder where the logs are saved and run- python3 logplot_mpc.py "filename" to get the plots. +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +import sys +import os + +def find_mpc_csv(directory): + for filename in os.listdir(directory): + if filename.startswith("MPCTrajectoryTracker_debug") and filename.endswith(".csv"): + return os.path.join(directory, filename) + raise FileNotFoundError("No MPCTrajectoryTracker_debug.csv found in the directory.") + +def plot_mpc_debug(csv_path): + df = pd.read_csv(csv_path) + + time = df['mpc/accel time'] - df['mpc/accel time'].iloc[0] + accel = df['mpc/accel'] + delta = df['mpc/delta'] + state_x = df['mpc/state_x'] + state_y = df['mpc/state_y'] + target_x = df['mpc/target_x'] + target_y = df['mpc/target_y'] + yaw = df['mpc/state_yaw'] + target_theta = df['mpc/target_theta'] + + position_error = df['mpc/position_error'] + heading_error = df['mpc/heading_error'] + + jerk_time = time[1:] + jerk = np.diff(accel) / np.diff(time) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.5, wspace=0.3) + + axs[0,0].plot(jerk_time, jerk, color='blue') + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].set_xlabel("Time (s)") + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].grid(True) + + axs[0,1].plot(time, position_error, label="Position Error", color='green') + axs[0,1].set_title("Position Error Over Time") + axs[0,1].set_xlabel("Time (s)") + axs[0,1].set_ylabel("Error (m)") + axs[0,1].legend() + axs[0,1].grid(True) + + axs[1,0].plot(time, heading_error, label="Heading Error", color='red') + axs[1,0].set_title("Heading Error Over Time") + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Error (rad)") + axs[1,0].legend() + axs[1,0].grid(True) + + axs[1,1].plot(state_x, state_y, label='Actual Trajectory', color='blue') + axs[1,1].plot(target_x, target_y, label='Target Trajectory', color='orange') + axs[1,1].set_title("Trajectory Comparison") + axs[1,1].set_xlabel("X (m)") + axs[1,1].set_ylabel("Y (m)") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + + print("Max (abs) position error:", np.max(np.abs(position_error))) + print("Avg position error:", np.mean(np.abs(position_error))) + print("Max (abs) heading error:", np.max(np.abs(heading_error))) + print("Avg heading error:", np.mean(np.abs(heading_error))) + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + +if __name__ == '__main__': + if len(sys.argv) != 2: + print("Usage: python3 logplot_mpc.py ") + sys.exit(1) + + log_dir = sys.argv[1] + if not os.path.isdir(log_dir): + print(f"Error: '{log_dir}' is not a valid directory.") + sys.exit(1) + + try: + csv_file = find_mpc_csv(log_dir) + print(f"Found MPC CSV: {csv_file}") + plot_mpc_debug(csv_file) + except Exception as e: + print(f"Error: {e}") + sys.exit(1) diff --git a/logs/logplot_pp.py b/logs/logplot_pp.py new file mode 100644 index 000000000..3c67f6790 --- /dev/null +++ b/logs/logplot_pp.py @@ -0,0 +1,121 @@ +###Jugal's Code### +####Following code can be used to visualize performance of Pure Pursuit after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_pp.py "logfilename".### + + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 11], data[:, 14] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + #tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/logs/logplot_s.py b/logs/logplot_s.py new file mode 100644 index 000000000..a7d671e7b --- /dev/null +++ b/logs/logplot_s.py @@ -0,0 +1,120 @@ +###Jugal's Code### +####Following code can be used to visualize performance of Stanley after using it.### +###How to use?- After running the simulation or on actual vehicle you get logs in log folder### +### You can run the command- python3 logplot_s.py "logfilename".### + +import json +import sys +import os +import matplotlib.pyplot as plt +import numpy as np + +# Safety thresholds (not used in the current plots) +ACC_SAFE = 0.8 # |acceleration| <= 0.8 m/s² is safe +ACC_UNSAFE = 2.0 # |acceleration| >= 2.0 m/s² is unsafe +HR_SAFE = 0.05 # |heading_rate| <= 0.05 rad/s is safe +HR_UNSAFE = 0.2 # |heading_rate| >= 0.2 rad/s is unsafe + +def parse_behavior_log(filename): + times = [] + accelerations = [] + heading_rates = [] + + with open(filename, 'r') as f: + first_time = None # Store the first timestamp for shifting + for line in f: + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + if "vehicle" in entry: + t = entry.get("time") + vehicle_data = entry["vehicle"].get("data", {}) + acceleration = vehicle_data.get("acceleration") + heading_rate = vehicle_data.get("heading_rate") + if t is not None and acceleration is not None and heading_rate is not None: + if first_time is None: + first_time = t # Set the first timestamp + times.append(t - first_time) # Shift time to start from 0 + accelerations.append(acceleration) + heading_rates.append(heading_rate) + + return np.array(times), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + first_time = data[0, 18] # Normalize time to start from 0 + cte_time = data[:, 18] - first_time + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + x_desired, y_desired = data[:, 8], data[:, 11] + return cte_time, cte, x_actual, y_actual, x_desired, y_desired + +def compute_derivative(times, values): + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired): + global_min_time = min(time_jerk[0], time_heading_acc[0], cte_time[0]) + global_max_time = max(time_jerk[-1], time_heading_acc[-1], cte_time[-1]) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + fig.subplots_adjust(hspace=0.4, wspace=0.3) + + axs[0,0].plot(time_jerk, jerk, marker='', linestyle='-', color='blue') + axs[0,0].set_ylabel("Jerk (m/s³)") + axs[0,0].set_title("Vehicle Jerk Over Time") + axs[0,0].grid(True) + axs[0,0].set_xlim(global_min_time, global_max_time) + + axs[0,1].plot(time_heading_acc, heading_acc, marker='', linestyle='-', color='orange') + axs[0,1].set_ylabel("Heading Acceleration (rad/s²)") + axs[0,1].set_title("Vehicle Heading Acceleration Over Time") + axs[0,1].grid(True) + axs[0,1].set_xlim(global_min_time, global_max_time) + + axs[1,0].plot(cte_time, cte, marker='', linestyle='-', color='green') + axs[1,0].set_xlabel("Time (s)") + axs[1,0].set_ylabel("Cross Track Error") + axs[1,0].set_title("Vehicle Cross Track Error Over Time") + axs[1,0].grid(True) + axs[1,0].set_xlim(global_min_time, global_max_time) + + axs[1,1].plot(x_actual, y_actual, marker='', linestyle='-', color='blue', label='Actual') + axs[1,1].plot(x_desired, y_desired, marker='', linestyle='-', color='orange', label='Desired') + axs[1,1].set_xlabel("X Position (m)") + axs[1,1].set_ylabel("Y Position (m)") + axs[1,1].set_title("Vehicle Position") + axs[1,1].legend() + axs[1,1].grid(True) + + plt.show() + +if __name__=='__main__': + if len(sys.argv) != 2: + print("Usage: python test_comfort_metrics.py ") + sys.exit(1) + + log_dir = sys.argv[1] + behavior_file = os.path.join(log_dir, "behavior.json") + #tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + tracker_file = os.path.join(log_dir, "StanleyTrajectoryTracker_debug.csv") + + times, accelerations, heading_rates = parse_behavior_log(behavior_file) + time_jerk, jerk = compute_derivative(times, accelerations) + time_heading_acc, heading_acc = compute_derivative(times, heading_rates) + + cte_time, cte, x_actual, y_actual, x_desired, y_desired = parse_tracker_csv(tracker_file) + + plot_metrics(time_jerk, jerk, time_heading_acc, heading_acc, cte_time, cte, x_actual, y_actual, x_desired, y_desired) + + print("Max (abs) jerk:", np.max(np.abs(jerk))) + print("Avg jerk:", np.mean(np.abs(jerk))) + print("Max (abs) heading acceleration:", np.max(np.abs(heading_acc))) + print("Avg heading acceleration:", np.mean(np.abs(heading_acc))) + print("Max (abs) cross track error:", np.max(np.abs(cte))) + print("Avg cross track error:", np.mean(np.abs(cte))) + diff --git a/requirements.txt b/requirements.txt index f8e808d90..8f3a1caf5 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,8 +7,9 @@ shapely klampt==0.9.2 pyyaml dacite +casadi # Perception ultralytics lap==0.5.12 -open3d \ No newline at end of file +open3d