diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 307635fba..469c439a6 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -18,11 +18,40 @@ control: 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", "racing" or a constant value + longitudinal_control: + pid_p: 1.0 + pid_i: 0.7 + pid_d: 0.1 + racing: # These tuning parameters will be used if the speed source is 'racing' + pure_pursuit: + lookahead: 2.0 + lookahead_scale: 2.0 + crosstrack_gain: 0.3 + stanley: + control_gain: 0.12 + softening_gain: 0.2 + longitudinal_control: + pid_p: 1.0 + pid_i: 0.7 + pid_d: 0.1 + launch_control: + stage_duration: 0.5 +planning: + longitudinal_plan: + mode: 'real' # 'real' or 'sim' + yielder: 'expert' # 'expert', 'analytic', or 'simulation' + planner: 'dt' # 'milestone', 'dt', or 'dx' + desired_speed: 1.0 + acceleration: 0.5 + max_deceleration: 6.0 + deceleration: 2.0 + min_deceleration: 0.5 + yield_deceleration: 0.5 desired_speed: trajectory # Speed reference source: can be "trajectory", "path", or a constant value # MPC controller parameters @@ -37,13 +66,12 @@ control: 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: dt: 0.01 real_time_multiplier: 1.0 # make the simulator run faster than real time by making this > 1 gnss_emulator: - dt: 0.1 #10Hz + dt: 0.05 #10Hz #position_noise: 0.1 #10cm noise #orientation_noise: 0.04 #2.3 degrees noise #velocity_noise: diff --git a/GEMstack/knowledge/routes/circle_10m_rad.csv b/GEMstack/knowledge/routes/circle_10m_rad.csv new file mode 100644 index 000000000..6c1c629e6 --- /dev/null +++ b/GEMstack/knowledge/routes/circle_10m_rad.csv @@ -0,0 +1,100 @@ +10.0,0.0 +9.980267284282716,0.6279051952931337 +9.921147013144777,1.2533323356430426 +9.822872507286887,1.8738131458572465 +9.685831611286313,2.4868988716485485 +9.510565162951536,3.0901699437494745 +9.297764858882513,3.68124552684678 +9.048270524660193,4.257792915650727 +8.763066800438635,4.817536741017153 +8.443279255020151,5.3582679497899655 +8.090169943749476,5.87785252292473 +7.705132427757894,6.3742398974868975 +7.289686274214117,6.845471059286887 +6.845471059286887,7.289686274214117 +6.374239897486897,7.705132427757894 +5.87785252292473,8.090169943749476 +5.3582679497899655,8.443279255020151 +4.817536741017151,8.763066800438637 +4.257792915650727,9.048270524660195 +3.681245526846779,9.297764858882514 +3.090169943749475,9.510565162951536 +2.486898871648548,9.685831611286313 +1.8738131458572451,9.822872507286887 +1.2533323356430426,9.921147013144777 +0.6279051952931329,9.980267284282716 +-1.6081226496766366e-15,9.999999999999998 +-0.627905195293134,9.980267284282716 +-1.2533323356430437,9.921147013144777 +-1.8738131458572485,9.822872507286887 +-2.486898871648549,9.685831611286313 +-3.0901699437494763,9.510565162951536 +-3.68124552684678,9.297764858882514 +-4.257792915650728,9.048270524660195 +-4.817536741017154,8.763066800438635 +-5.358267949789967,8.44327925502015 +-5.877852522924729,8.090169943749476 +-6.374239897486897,7.705132427757894 +-6.845471059286887,7.289686274214116 +-7.289686274214117,6.845471059286886 +-7.705132427757895,6.374239897486894 +-8.090169943749475,5.877852522924732 +-8.443279255020151,5.3582679497899655 +-8.763066800438637,4.817536741017151 +-9.048270524660195,4.257792915650725 +-9.297764858882514,3.6812455268467774 +-9.510565162951538,3.090169943749471 +-9.685831611286313,2.4868988716485485 +-9.822872507286887,1.8738131458572458 +-9.921147013144777,1.2533323356430408 +-9.980267284282716,0.6279051952931314 +-10.0,-3.216245299353273e-15 +-9.980267284282716,-0.6279051952931334 +-9.921147013144777,-1.253332335643043 +-9.822872507286887,-1.873813145857248 +-9.685831611286313,-2.4868988716485503 +-9.510565162951535,-3.090169943749478 +-9.297764858882513,-3.6812455268467836 +-9.048270524660193,-4.257792915650727 +-8.763066800438635,-4.817536741017154 +-8.44327925502015,-5.358267949789967 +-8.090169943749473,-5.877852522924733 +-7.705132427757891,-6.3742398974869 +-7.289686274214117,-6.845471059286887 +-6.845471059286886,-7.289686274214117 +-6.374239897486895,-7.705132427757895 +-5.877852522924733,-8.090169943749475 +-5.358267949789962,-8.443279255020153 +-4.817536741017153,-8.763066800438635 +-4.257792915650722,-9.048270524660197 +-3.6812455268467783,-9.297764858882514 +-3.0901699437494763,-9.510565162951535 +-2.4868988716485445,-9.685831611286313 +-1.8738131458572465,-9.822872507286887 +-1.2533323356430373,-9.921147013144777 +-0.6279051952931319,-9.980267284282716 +-1.8369701987210296e-15,-10.0 +0.6279051952931372,-9.980267284282716 +1.2533323356430424,-9.921147013144777 +1.8738131458572516,-9.822872507286887 +2.48689887164855,-9.685831611286313 +3.0901699437494727,-9.510565162951538 +3.681245526846783,-9.297764858882513 +4.257792915650727,-9.048270524660195 +4.817536741017157,-8.763066800438633 +5.358267949789966,-8.44327925502015 +5.877852522924735,-8.090169943749471 +6.374239897486898,-7.705132427757891 +6.845471059286886,-7.289686274214117 +7.289686274214119,-6.845471059286883 +7.705132427757894,-6.374239897486895 +8.09016994374948,-5.877852522924725 +8.443279255020153,-5.358267949789962 +8.763066800438635,-4.817536741017153 +9.048270524660197,-4.257792915650722 +9.297764858882513,-3.6812455268467787 +9.510565162951538,-3.0901699437494683 +9.685831611286313,-2.4868988716485454 +9.822872507286887,-1.8738131458572471 +9.921147013144777,-1.253332335643038 +9.980267284282716,-0.6279051952931326 diff --git a/GEMstack/knowledge/routes/circle_20m_rad.csv b/GEMstack/knowledge/routes/circle_20m_rad.csv new file mode 100644 index 000000000..6b28cb31a --- /dev/null +++ b/GEMstack/knowledge/routes/circle_20m_rad.csv @@ -0,0 +1,100 @@ +20.0,0.0 +19.960534568565432,1.2558103905862674 +19.842294026289558,2.506664671286085 +19.645745014573773,3.7476262917144925 +19.37166322257262,4.973797743297096 +19.02113032590307,6.180339887498948 +18.595529717765025,7.362491053693559 +18.09654104932039,8.515585831301454 +17.526133600877273,9.635073482034306 +16.886558510040302,10.716535899579933 +16.18033988749895,11.755705045849464 +15.410264855515782,12.748479794973795 +14.579372548428232,13.690942118573775 +13.690942118573773,14.579372548428232 +12.748479794973793,15.410264855515784 +11.75570504584946,16.18033988749895 +10.716535899579931,16.886558510040302 +9.635073482034302,17.526133600877273 +8.515585831301454,18.096541049320393 +7.3624910536935575,18.59552971776503 +6.180339887498949,19.02113032590307 +4.973797743297094,19.37166322257262 +3.7476262917144902,19.645745014573773 +2.506664671286085,19.842294026289558 +1.255810390586266,19.960534568565432 +-3.2162452993532727e-15,20.0 +-1.255810390586268,19.960534568565432 +-2.5066646712860874,19.842294026289554 +-3.7476262917144965,19.645745014573773 +-4.973797743297097,19.37166322257262 +-6.180339887498951,19.02113032590307 +-7.362491053693559,18.595529717765025 +-8.515585831301454,18.09654104932039 +-9.635073482034308,17.52613360087727 +-10.716535899579938,16.8865585100403 +-11.75570504584946,16.18033988749895 +-12.748479794973795,15.410264855515784 +-13.690942118573775,14.579372548428228 +-14.579372548428234,13.69094211857377 +-15.410264855515788,12.74847979497379 +-16.180339887498945,11.755705045849465 +-16.886558510040302,10.716535899579933 +-17.526133600877273,9.635073482034304 +-18.096541049320393,8.51558583130145 +-18.59552971776503,7.362491053693555 +-19.021130325903073,6.180339887498942 +-19.37166322257262,4.973797743297096 +-19.645745014573773,3.7476262917144916 +-19.842294026289558,2.5066646712860816 +-19.960534568565432,1.2558103905862628 +-20.0,-6.432490598706546e-15 +-19.960534568565432,-1.255810390586267 +-19.842294026289558,-2.5066646712860856 +-19.645745014573773,-3.7476262917144956 +-19.37166322257262,-4.973797743297101 +-19.02113032590307,-6.180339887498954 +-18.595529717765025,-7.362491053693566 +-18.09654104932039,-8.515585831301454 +-17.52613360087727,-9.635073482034308 +-16.8865585100403,-10.716535899579934 +-16.180339887498945,-11.755705045849467 +-15.41026485551578,-12.7484797949738 +-14.579372548428232,-13.690942118573775 +-13.690942118573773,-14.579372548428232 +-12.74847979497379,-15.410264855515788 +-11.755705045849465,-16.180339887498945 +-10.716535899579927,-16.886558510040306 +-9.635073482034306,-17.526133600877273 +-8.515585831301443,-18.096541049320397 +-7.362491053693557,-18.59552971776503 +-6.180339887498951,-19.02113032590307 +-4.973797743297089,-19.371663222572625 +-3.7476262917144925,-19.645745014573773 +-2.5066646712860745,-19.842294026289558 +-1.255810390586264,-19.960534568565432 +-3.673940397442059e-15,-20.0 +1.2558103905862743,-19.960534568565432 +2.5066646712860847,-19.842294026289558 +3.7476262917145027,-19.645745014573773 +4.973797743297099,-19.37166322257262 +6.180339887498945,-19.021130325903073 +7.3624910536935655,-18.595529717765025 +8.515585831301452,-18.096541049320393 +9.635073482034315,-17.526133600877266 +10.716535899579934,-16.8865585100403 +11.755705045849474,-16.18033988749894 +12.7484797949738,-15.41026485551578 +13.690942118573773,-14.579372548428232 +14.579372548428237,-13.690942118573766 +15.410264855515788,-12.748479794973793 +16.180339887498956,-11.755705045849451 +16.886558510040306,-10.716535899579927 +17.526133600877273,-9.635073482034306 +18.096541049320393,-8.515585831301443 +18.59552971776503,-7.3624910536935575 +19.021130325903076,-6.180339887498936 +19.371663222572625,-4.97379774329709 +19.645745014573773,-3.747626291714494 +19.842294026289558,-2.506664671286076 +19.960534568565432,-1.2558103905862652 diff --git a/GEMstack/knowledge/routes/circle_5m_rad.csv b/GEMstack/knowledge/routes/circle_5m_rad.csv new file mode 100644 index 000000000..061b3b441 --- /dev/null +++ b/GEMstack/knowledge/routes/circle_5m_rad.csv @@ -0,0 +1,100 @@ +5.0,0.0 +4.990133642141358,0.31395259764656686 +4.9605735065723895,0.6266661678215213 +4.911436253643443,0.9369065729286231 +4.842915805643155,1.243449435824274 +4.755282581475767,1.545084971874737 +4.648882429441256,1.8406227634233898 +4.524135262330097,2.1288964578253635 +4.381533400219318,2.4087683705085765 +4.221639627510076,2.679133974894983 +4.045084971874737,2.938926261462366 +3.8525662138789456,3.1871199487434487 +3.644843137107058,3.4227355296434436 +3.422735529643443,3.644843137107058 +3.1871199487434483,3.852566213878946 +2.938926261462365,4.045084971874737 +2.6791339748949827,4.221639627510076 +2.4087683705085756,4.381533400219318 +2.1288964578253635,4.524135262330098 +1.8406227634233894,4.648882429441257 +1.5450849718747373,4.755282581475767 +1.2434494358242736,4.842915805643155 +0.9369065729286226,4.911436253643443 +0.6266661678215213,4.9605735065723895 +0.3139525976465665,4.990133642141358 +-8.040613248383182e-16,5.0 +-0.313952597646567,4.990133642141358 +-0.6266661678215218,4.960573506572389 +-0.9369065729286241,4.911436253643443 +-1.2434494358242743,4.842915805643155 +-1.5450849718747377,4.755282581475767 +-1.8406227634233898,4.648882429441256 +-2.1288964578253635,4.524135262330097 +-2.408768370508577,4.381533400219317 +-2.6791339748949845,4.221639627510075 +-2.938926261462365,4.045084971874737 +-3.1871199487434487,3.852566213878946 +-3.4227355296434436,3.644843137107057 +-3.6448431371070584,3.4227355296434423 +-3.852566213878947,3.1871199487434474 +-4.045084971874736,2.9389262614623664 +-4.221639627510076,2.679133974894983 +-4.381533400219318,2.408768370508576 +-4.524135262330098,2.1288964578253626 +-4.648882429441257,1.8406227634233887 +-4.755282581475768,1.5450849718747355 +-4.842915805643155,1.243449435824274 +-4.911436253643443,0.9369065729286229 +-4.9605735065723895,0.6266661678215204 +-4.990133642141358,0.3139525976465657 +-5.0,-1.6081226496766366e-15 +-4.990133642141358,-0.31395259764656674 +-4.9605735065723895,-0.6266661678215214 +-4.911436253643443,-0.9369065729286239 +-4.842915805643155,-1.2434494358242751 +-4.755282581475767,-1.5450849718747386 +-4.648882429441256,-1.8406227634233916 +-4.524135262330097,-2.1288964578253635 +-4.381533400219317,-2.408768370508577 +-4.221639627510075,-2.6791339748949836 +-4.045084971874736,-2.938926261462367 +-3.852566213878945,-3.18711994874345 +-3.644843137107058,-3.4227355296434436 +-3.422735529643443,-3.644843137107058 +-3.1871199487434474,-3.852566213878947 +-2.9389262614623664,-4.045084971874736 +-2.679133974894982,-4.2216396275100765 +-2.4087683705085765,-4.381533400219318 +-2.128896457825361,-4.524135262330099 +-1.8406227634233892,-4.648882429441257 +-1.5450849718747377,-4.755282581475767 +-1.2434494358242723,-4.842915805643156 +-0.9369065729286231,-4.911436253643443 +-0.6266661678215186,-4.9605735065723895 +-0.313952597646566,-4.990133642141358 +-9.184850993605148e-16,-5.0 +0.3139525976465686,-4.990133642141358 +0.6266661678215212,-4.9605735065723895 +0.9369065729286257,-4.911436253643443 +1.2434494358242747,-4.842915805643155 +1.5450849718747361,-4.755282581475768 +1.8406227634233914,-4.648882429441256 +2.128896457825363,-4.524135262330098 +2.4087683705085787,-4.3815334002193165 +2.6791339748949836,-4.221639627510075 +2.9389262614623686,-4.045084971874735 +3.18711994874345,-3.852566213878945 +3.422735529643443,-3.644843137107058 +3.6448431371070593,-3.4227355296434414 +3.852566213878947,-3.1871199487434483 +4.045084971874739,-2.938926261462363 +4.2216396275100765,-2.679133974894982 +4.381533400219318,-2.4087683705085765 +4.524135262330098,-2.128896457825361 +4.648882429441257,-1.8406227634233894 +4.755282581475769,-1.545084971874734 +4.842915805643156,-1.2434494358242725 +4.911436253643443,-0.9369065729286234 +4.9605735065723895,-0.626666167821519 +4.990133642141358,-0.3139525976465663 diff --git a/GEMstack/knowledge/routes/figure8.csv b/GEMstack/knowledge/routes/figure8.csv new file mode 100644 index 000000000..a19aedcd8 --- /dev/null +++ b/GEMstack/knowledge/routes/figure8.csv @@ -0,0 +1,200 @@ +-7.0710678118654755,7.071067811865475 +-7.2906556139397285,6.844431415617231 +-7.502308944661675,6.611639328565266 +-7.705152494913562,6.373587931399371 +-7.898325044373529,6.13117354038889 +-8.080982947051604,5.88528888686235 +-8.25230356767972,5.6368196318881 +-8.411488655184105,5.386640929976428 +-8.55776763969854,5.135614055407286 +-8.690400839861038,4.884583104517498 +-8.808682567473495,4.63437178695796 +-8.911944116992771,4.385780318556234 +-8.999556627760255,4.139582427994652 +-9.070933807364344,3.8965224890402577 +-9.12553450506384,3.65731278954247 +-9.162865124778294,3.422630947849536 +-9.182481867771463,3.1931174866877035 +-9.18399279581362,2.9693735739001195 +-9.167059706305327,2.7519589387584693 +-9.131399811576024,2.541389971841754 +-9.076787215333287,2.338138015726586 +-9.003054180029041,2.142627852954777 +-8.910092179725005,1.9552363969398612 +-8.797852733877264,1.7762915906480434 +-8.666348018316427,1.6060715170438167 +-8.515651250571414,1.4448037244299101 +-8.345896847568358,1.2926647689384532 +-8.157280354627865,1.1497799755490092 +-7.950058145580334,1.0162234181227694 +-7.72454689471666,0.8920181180543993 +-7.48112282218691,0.7771364602573145 +-7.220220715348916,0.6715008243179756 +-6.942332729448554,0.5749844277838516 +-6.648006971880344,0.48741237769124446 +-6.337845875127566,0.40856292559692964 +-6.012504364311742,0.3381689205545477 +-5.672687826088908,0.27591945367658344 +-5.319149886411543,0.22146168714843872 +-4.952690005426821,0.1744028598158298 +-4.5741508985015,0.13431246075351044 +-4.184415793048013,0.10072456154483156 +-3.784405531472644,0.07314029736083127 +-3.375075531172583,0.05103048632677254 +-2.9574126130714227,0.03383837610581075 +-2.5324317107003727,0.0209825061160398 +-2.101172472303035,0.01185967333046567 +-1.6646957688628319,0.005847989191503644 +-1.224080121322797,0.0023100148039367774 +-0.7804180605858775,0.0005959612544599313 +-0.3348124341485519,4.69416431974623e-05 +0.2232546531379846,-3.4775920885465883e-06 +1.1155779279562612,-0.00043453652732550196 +2.005122325691386,-0.0025320070504719627 +2.8896746304541985,-0.007620297045278557 +3.7670385349273756,-0.01700890073662653 +4.6350420699393595,-0.03198585311115387 +5.49154494132252,-0.05381127418390186 +6.334445746125748,-0.08371103016248593 +7.161689040726918,-0.12287053808868764 +7.97127223397421,-0.17242873995852737 +8.761252279174599,-0.23347227163807283 +9.529752139540019,-0.3070298511062006 +10.274967002594085,-0.3940669096704861 +10.995170220031003,-0.4954804888217348 +11.688718950599176,-0.6120944243204658 +12.354059484751527,-0.7446548379490805 +12.989732231057182,-0.8938259561212369 +13.5943763457005,-1.0601862732199647 +14.166733987797977,-1.2442250761436462 +14.705654184735884,-1.446339345079779 +15.21009629326522,-1.6668310440060212 +15.67913304368062,-1.9059048128428548 +16.111953156048813,-2.163666071558234 +16.507863519134226,-2.4401195448584856 +16.866290924387744,-2.7351682143983016 +17.18678334911195,-3.048612703712158 +17.469010784686596,-3.3801510993177653 +17.71276560752314,-3.7293792096751686 +17.91796249221132,-4.095791261910843 +18.084637868115514,-4.478781034440925 +18.21294892246776,-4.877643421859495 +18.303172154780448,-5.291576426702616 +18.355701489157553,-5.719683570964906 +18.37104595281231,-6.160976718538519 +18.34982693079418,-6.6143792980723 +18.292775008582552,-7.078729914117767 +18.2007264158116,-7.552786332844942 +18.07461908594467,-8.035229827081785 +17.91548834821038,-8.52466986396156 +17.72446226954103,-9.019649117059142 +17.502756665610935,-9.518648783565657 +17.251669801352808,-10.02009418579633 +16.972576802528867,-10.522360635153587 +16.666923801045655,-11.02377953558188 +16.33622183772296,-11.522644702555922 +15.982040547153561,-12.017218872744364 +15.60600165011863,-12.50574037869042 +15.209772279749702,-12.98642996215177 +14.795058168249529,-13.457497699148421 +14.363596721498746,-13.917150009280299 +13.917150009280297,-14.363596721498748 +13.457497699148425,-14.795058168249525 +12.986429962151774,-15.209772279749696 +12.505740378690426,-15.606001650118628 +12.017218872744364,-15.982040547153561 +11.522644702555919,-16.33622183772296 +11.023779535581888,-16.66692380104565 +10.522360635153591,-16.972576802528863 +10.020094185796335,-17.251669801352808 +9.518648783565663,-17.502756665610935 +9.01964911705914,-17.724462269541032 +8.524669863961567,-17.91548834821038 +8.035229827081789,-18.074619085944665 +7.552786332844948,-18.200726415811594 +7.0787299141177735,-18.292775008582552 +6.614379298072299,-18.34982693079418 +6.160976718538518,-18.37104595281231 +5.71968357096491,-18.355701489157553 +5.291576426702621,-18.303172154780448 +4.877643421859499,-18.21294892246776 +4.478781034440929,-18.084637868115514 +4.0957912619108425,-17.917962492211316 +3.7293792096751748,-17.71276560752314 +3.380151099317769,-17.469010784686596 +3.048612703712161,-17.186783349111952 +2.7351682143983043,-16.86629092438774 +2.4401195448584865,-16.507863519134226 +2.163666071558236,-16.111953156048816 +1.9059048128428593,-15.679133043680622 +1.6668310440060221,-15.210096293265215 +1.4463393450797826,-14.705654184735888 +1.2442250761436453,-14.166733987797969 +1.0601862732199674,-13.594376345700503 +0.8938259561212387,-12.989732231057184 +0.7446548379490805,-12.35405948475152 +0.6120944243204676,-11.688718950599178 +0.4954804888217357,-10.995170220030998 +0.39406690967048785,-10.274967002594092 +0.3070298511062015,-9.529752139540019 +0.23347227163807283,-8.76125227917459 +0.17242873995852914,-7.9712722339742115 +0.12287053808868809,-7.161689040726909 +0.08371103016248771,-6.334445746125757 +0.05381127418390319,-5.491544941322518 +0.031985853111154317,-4.635042069939345 +0.017008900736627197,-3.767038534927379 +0.007620297045278779,-2.8896746304541896 +0.002532007050472407,-2.0051223256913895 +0.000434536527325613,-1.1155779279562583 +3.477592088574344e-06,-0.22325465313799434 +-4.694164319751781e-05,0.3348124341485501 +-0.0005959612544601534,0.7804180605858819 +-0.0023100148039368884,1.2240801213227952 +-0.005847989191504088,1.664695768862833 +-0.01185967333046567,2.10117247230303 +-0.0209825061160398,2.5324317107003713 +-0.033838376105811196,2.957412613071427 +-0.05103048632677276,3.3750755311725817 +-0.07314029736083216,3.784405531472645 +-0.10072456154483156,4.184415793048008 +-0.13431246075351133,4.574150898501502 +-0.17440285981583115,4.952690005426826 +-0.22146168714843872,5.319149886411541 +-0.27591945367658477,5.672687826088911 +-0.33816892055454817,6.012504364311738 +-0.40856292559693097,6.337845875127567 +-0.4874123776912467,6.648006971880346 +-0.5749844277838512,6.9423327294485535 +-0.6715008243179788,7.220220715348918 +-0.7771364602573145,7.481122822186904 +-0.8920181180544007,7.72454689471666 +-1.016223418122772,7.950058145580337 +-1.1497799755490097,8.157280354627865 +-1.2926647689384554,8.345896847568358 +-1.4448037244299101,8.515651250571414 +-1.6060715170438193,8.666348018316429 +-1.7762915906480434,8.797852733877262 +-1.955236396939862,8.910092179725007 +-2.14262785295478,9.003054180029043 +-2.3381380157265865,9.076787215333283 +-2.541389971841756,9.131399811576022 +-2.751958938758469,9.167059706305324 +-2.96937357390012,9.18399279581362 +-3.1931174866877075,9.182481867771461 +-3.4226309478495374,9.162865124778294 +-3.6573127895424724,9.125534505063838 +-3.896522489040256,9.070933807364344 +-4.139582427994653,8.999556627760255 +-4.385780318556237,8.911944116992768 +-4.634371786957962,8.808682567473493 +-4.884583104517502,8.690400839861034 +-5.135614055407285,8.557767639698541 +-5.38664092997643,8.411488655184103 +-5.636819631888104,8.252303567679718 +-5.88528888686235,8.080982947051606 +-6.131173540388894,7.898325044373527 +-6.373587931399368,7.705152494913563 +-6.611639328565269,7.502308944661674 +-6.844431415617234,7.290655613939725 +-7.071067811865475,7.0710678118654755 diff --git a/GEMstack/knowledge/routes/forward_50m.csv b/GEMstack/knowledge/routes/forward_50m.csv new file mode 100644 index 000000000..6ecc0a99a --- /dev/null +++ b/GEMstack/knowledge/routes/forward_50m.csv @@ -0,0 +1,51 @@ +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 +16,0,0 +17,0,0 +18,0,0 +19,0,0 +20,0,0 +21,0,0 +22,0,0 +23,0,0 +24,0,0 +25,0,0 +26,0,0 +27,0,0 +28,0,0 +29,0,0 +30,0,0 +31,0,0 +32,0,0 +33,0,0 +34,0,0 +35,0,0 +36,0,0 +37,0,0 +38,0,0 +39,0,0 +40,0,0 +41,0,0 +42,0,0 +43,0,0 +44,0,0 +45,0,0 +46,0,0 +47,0,0 +48,0,0 +49,0,0 +50,0,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/forward_65m.csv b/GEMstack/knowledge/routes/forward_65m.csv new file mode 100644 index 000000000..ee345b68a --- /dev/null +++ b/GEMstack/knowledge/routes/forward_65m.csv @@ -0,0 +1,66 @@ +0,0,0 +1,0,0 +2,0,0 +3,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 +16,0,0 +17,0,0 +18,0,0 +19,0,0 +20,0,0 +21,0,0 +22,0,0 +23,0,0 +24,0,0 +25,0,0 +26,0,0 +27,0,0 +28,0,0 +29,0,0 +30,0,0 +31,0,0 +32,0,0 +33,0,0 +34,0,0 +35,0,0 +36,0,0 +37,0,0 +38,0,0 +39,0,0 +40,0,0 +41,0,0 +42,0,0 +43,0,0 +44,0,0 +45,0,0 +46,0,0 +47,0,0 +48,0,0 +49,0,0 +50,0,0 +51,0,0 +52,0,0 +53,0,0 +54,0,0 +55,0,0 +56,0,0 +57,0,0 +58,0,0 +59,0,0 +60,0,0 +61,0,0 +62,0,0 +63,0,0 +64,0,0 +65,0,0 diff --git a/GEMstack/knowledge/routes/race_track_7.csv b/GEMstack/knowledge/routes/race_track_7.csv new file mode 100644 index 000000000..6e1ba685a --- /dev/null +++ b/GEMstack/knowledge/routes/race_track_7.csv @@ -0,0 +1,1000 @@ +0.0,0.0 +0.1507537688442211,0.0 +0.3015075376884422,0.0 +0.4522613065326633,0.0 +0.6030150753768844,0.0 +0.7537688442211055,0.0 +0.9045226130653266,0.0 +1.0552763819095476,0.0 +1.2060301507537687,0.0 +1.3567839195979898,0.0 +1.507537688442211,0.0 +1.658291457286432,0.0 +1.809045226130653,0.0 +1.9597989949748742,0.0 +2.1105527638190953,0.0 +2.2613065326633164,0.0 +2.4120603015075375,0.0 +2.5628140703517586,0.0 +2.7135678391959797,0.0 +2.8643216080402008,0.0 +3.015075376884422,0.0 +3.165829145728643,0.0 +3.316582914572864,0.0 +3.467336683417085,0.0 +3.618090452261306,0.0 +3.7688442211055273,0.0 +3.9195979899497484,0.0 +4.0703517587939695,0.0 +4.221105527638191,0.0 +4.371859296482412,0.0 +4.522613065326633,0.0 +4.673366834170854,0.0 +4.824120603015075,0.0 +4.974874371859296,0.0 +5.125628140703517,0.0 +5.276381909547738,0.0 +5.427135678391959,0.0 +5.57788944723618,0.0 +5.7286432160804015,0.0 +5.879396984924623,0.0 +6.030150753768844,0.0 +6.180904522613065,0.0 +6.331658291457286,0.0 +6.482412060301507,0.0 +6.633165829145728,0.0 +6.783919597989949,0.0 +6.93467336683417,0.0 +7.085427135678391,0.0 +7.236180904522612,0.0 +7.3869346733668335,0.0 +7.537688442211055,0.0 +7.688442211055276,0.0 +7.839195979899497,0.0 +7.989949748743718,0.0 +8.140703517587939,0.0 +8.29145728643216,0.0 +8.442211055276381,0.0 +8.592964824120603,0.0 +8.743718592964823,0.0 +8.894472361809044,0.0 +9.045226130653266,0.0 +9.195979899497488,0.0 +9.346733668341708,0.0 +9.497487437185928,0.0 +9.64824120603015,0.0 +9.798994974874372,0.0 +9.949748743718592,0.0 +10.100502512562812,0.0 +10.251256281407034,0.0 +10.402010050251256,0.0 +10.552763819095476,0.0 +10.703517587939697,0.0 +10.854271356783919,0.0 +11.00502512562814,0.0 +11.15577889447236,0.0 +11.306532663316581,0.0 +11.457286432160803,0.0 +11.608040201005025,0.0 +11.758793969849245,0.0 +11.909547738693465,0.0 +12.060301507537687,0.0 +12.21105527638191,0.0 +12.36180904522613,0.0 +12.51256281407035,0.0 +12.663316582914572,0.0 +12.814070351758794,0.0 +12.964824120603014,0.0 +13.115577889447234,0.0 +13.266331658291456,0.0 +13.417085427135678,0.0 +13.567839195979898,0.0 +13.718592964824118,0.0 +13.86934673366834,0.0 +14.020100502512562,0.0 +14.170854271356783,0.0 +14.321608040201003,0.0 +14.472361809045225,0.0 +14.623115577889447,0.0 +14.773869346733667,0.0 +14.924623115577887,0.0 +15.07537688442211,0.0 +15.226130653266331,0.0 +15.376884422110551,0.0 +15.527638190954772,0.0 +15.678391959798994,0.0 +15.829145728643216,0.0 +15.979899497487436,0.0 +16.130653266331656,0.0 +16.281407035175878,0.0 +16.4321608040201,0.0 +16.58291457286432,0.0 +16.73366834170854,0.0 +16.884422110552762,0.0 +17.035175879396984,0.0 +17.185929648241206,0.0 +17.336683417085425,0.0 +17.487437185929647,0.0 +17.63819095477387,0.0 +17.788944723618087,0.0 +17.93969849246231,0.0 +18.09045226130653,0.0 +18.241206030150753,0.0 +18.391959798994975,0.0 +18.542713567839193,0.0 +18.693467336683415,0.0 +18.844221105527637,0.0 +18.994974874371856,0.0 +19.145728643216078,0.0 +19.2964824120603,0.0 +19.44723618090452,0.0 +19.597989949748744,0.0 +19.748743718592962,0.0 +19.899497487437184,0.0 +20.050251256281406,0.0 +20.201005025125625,0.0 +20.351758793969847,0.0 +20.50251256281407,0.0 +20.65326633165829,0.0 +20.804020100502512,0.0 +20.95477386934673,0.0 +21.105527638190953,0.0 +21.256281407035175,0.0 +21.407035175879393,0.0 +21.557788944723615,0.0 +21.708542713567837,0.0 +21.85929648241206,0.0 +22.01005025125628,0.0 +22.1608040201005,0.0 +22.31155778894472,0.0 +22.462311557788944,0.0 +22.613065326633162,0.0 +22.763819095477384,0.0 +22.914572864321606,0.0 +23.065326633165828,0.0 +23.21608040201005,0.0 +23.36683417085427,0.0 +23.51758793969849,0.0 +23.668341708542712,0.0 +23.81909547738693,0.0 +23.969849246231153,0.0 +24.120603015075375,0.0 +24.271356783919597,0.0 +24.42211055276382,0.0 +24.572864321608037,0.0 +24.72361809045226,0.0 +24.87437185929648,0.0 +25.0251256281407,0.0 +25.17587939698492,0.0 +25.326633165829143,0.0 +25.477386934673365,0.0 +25.628140703517587,0.0 +25.778894472361806,0.0 +25.929648241206028,0.0 +26.08040201005025,0.0 +26.23115577889447,0.0 +26.38190954773869,0.0 +26.532663316582912,0.0 +26.683417085427134,0.0 +26.834170854271356,0.0 +26.984924623115575,0.0 +27.135678391959797,0.0 +27.28643216080402,0.0 +27.437185929648237,0.0 +27.58793969849246,0.0 +27.73869346733668,0.0 +27.889447236180903,0.0 +28.040201005025125,0.0 +28.190954773869343,0.0 +28.341708542713565,0.0 +28.492462311557787,0.0 +28.643216080402006,0.0 +28.793969849246228,0.0 +28.94472361809045,0.0 +29.09547738693467,0.0 +29.246231155778894,0.0 +29.396984924623112,0.0 +29.547738693467334,0.0 +29.698492462311556,0.0 +29.849246231155774,0.0 +30.0,0.0 +30.0,0.0 +30.11050369409546,0.0008722733763990576 +30.220979848353675,0.0034888761167835014 +30.331400929800907,0.007849156108894917 +30.441739419188714,0.013952026679546314 +30.551967817852336,0.02179596686544727 +30.662058654563914,0.031379021792258044 +30.771984492378927,0.04269880316178565 +30.881717935474022,0.05575248984719572 +30.99123163597466,0.07053682859609989 +31.100498300770756,0.08704813484133389 +31.209490698318696,0.10528229361922659 +31.318181665428025,0.12523476059513694 +31.426544114031064,0.14690056319599787 +31.534551037933838,0.17027430184958448 +31.64217551954658,0.19535015133020117 +31.749390736592158,0.22212186221045016 +31.856169968790752,0.250582762418718 +31.9624866045191,0.28072575890200113 +32.06831414744269,0.31254333939363654 +32.17362622311918,0.3460275742855199 +32.278396585571485,0.3811701186043308 +32.38259912382879,0.4179622140912711 +32.486207868434,0.45639469138481203 +32.58919699791583,0.49645797230588595 +32.691540845224075,0.538142072244975 +32.79321390412639,0.5814366026504789 +32.89419083556494,0.6263307736177648 +32.994446473971465,0.6728133965782348 +33.093955833539056,0.7208728870877552 +33.19269411444911,0.770497267713738 +33.29063670905202,0.8216741710201712 +33.38775920799987,0.8743908426498441 +33.48403740632978,0.9286341445029977 +33.57944730949629,0.9843905580116159 +33.67396513935128,1.0416461875085377 +33.767567340070016,1.1003867636905458 +33.86023058402175,1.1605976471745771 +33.951931777583425,1.222263832146166 +34.04264806689512,1.2853699500992066 +34.1323568435557,1.349900273666103 +34.2210357502573,1.4158387205373648 +34.30866268635725,1.4831688574696535 +34.39521581338601,1.5518739043812904 +34.48067356048975,1.6219367385342043 +34.565014629806306,1.6933398988012822 +34.64821800177302,1.7660655900180355 +34.730262940365265,1.8400956874175476 +34.81112899826429,1.9154117411475244 +34.89079602195314,1.9919949808684008 +34.96924415673933,2.0698263204312948 +35.04645385170301,2.148886362634678 +35.12240586456954,2.229155404058564 +35.19708126650498,2.310613439975014 +35.270461446833636,2.393240169333736 +35.34252811767618,2.4770149998215327 +35.413263318507376,2.561917052994329 +35.482649420632235,2.6479251694805335 +35.550669131579454,2.7350179142543833 +35.617305499411046,2.8231735819780086 +35.682541916947145,2.912370202410856 +35.74636212590484,3.002585545885137 +35.80875022095008,3.0937971288459187 +35.86969065366164,3.1859822194545067 +35.92916823640609,3.2791178432536903 +35.98716814612288,3.373180788893458 +36.043675928018565,3.4681476139157574 +36.09867749916922,3.5639946505968445 +36.15215915203022,3.6606980118457733 +36.20410755785246,3.7582335971575653 +36.25450977000412,3.856577098619559 +36.30335322719726,3.9557040069694485 +36.35062575661837,4.055589617703511 +36.39631557696206,4.156209037233477 +36.44041130136722,4.257537189090548 +36.48290194025489,4.359548820174968 +36.52377690406704,4.462218507049628 +36.563026005905776,4.565520662276123 +36.60063946407206,4.669429540791672 +36.63660790450356,4.773919246325333 +36.670922363110876,4.87896373785189 +36.70357428801151,4.984536836081834 +36.73455554166124,5.090612229985778 +36.76385840288215,5.197163483351724 +36.79147556878691,5.304164041373518 +36.817400156598765,5.4115872372688685 +36.84162570536695,5.519406298925267 +36.86414617757683,5.627594355572153 +36.88495596065459,5.736124444477683 +36.904049868366016,5.844969517668405 +36.92142314210902,5.954102448670174 +36.937071452099545,6.063496039268646 +36.950990898450684,6.1731230262876435 +36.963178012144596,6.282956088383706 +36.97362975589705,6.392967852855145 +36.98234352491437,6.503130902463889 +36.98931714754265,6.613417782268443 +36.99454888580893,6.72380100646623 +36.99803743585434,6.834253065243631 +36.9997819282591,6.944746431632008 +36.9997819282591,7.055253568367993 +36.99803743585434,7.165746934756368 +36.99454888580893,7.276198993533769 +36.98931714754265,7.386582217731556 +36.98234352491437,7.496869097536112 +36.97362975589705,7.607032147144856 +36.963178012144596,7.717043911616293 +36.950990898450684,7.826876973712356 +36.937071452099545,7.936503960731353 +36.92142314210902,8.045897551329826 +36.904049868366016,8.155030482331595 +36.88495596065459,8.263875555522315 +36.86414617757683,8.372405644427847 +36.84162570536695,8.480593701074733 +36.817400156598765,8.58841276273113 +36.79147556878691,8.695835958626482 +36.76385840288215,8.802836516648277 +36.73455554166124,8.909387770014222 +36.70357428801151,9.015463163918165 +36.670922363110876,9.121036262148108 +36.63660790450356,9.226080753674667 +36.60063946407206,9.330570459208328 +36.563026005905776,9.434479337723877 +36.52377690406704,9.537781492950371 +36.48290194025489,9.640451179825032 +36.44041130136722,9.742462810909451 +36.39631557696206,9.843790962766523 +36.35062575661837,9.944410382296489 +36.30335322719726,10.04429599303055 +36.25450977000412,10.143422901380442 +36.20410755785246,10.241766402842433 +36.15215915203022,10.339301988154228 +36.09867749916922,10.436005349403155 +36.043675928018565,10.531852386084243 +35.98716814612288,10.626819211106543 +35.92916823640609,10.72088215674631 +35.86969065366164,10.814017780545493 +35.80875022095008,10.90620287115408 +35.74636212590484,10.997414454114864 +35.682541916947145,11.087629797589145 +35.617305499411046,11.176826418021992 +35.550669131579454,11.264982085745618 +35.482649420632235,11.352074830519467 +35.413263318507376,11.438082947005672 +35.34252811767618,11.522985000178469 +35.270461446833636,11.606759830666263 +35.19708126650498,11.689386560024987 +35.12240586456954,11.770844595941435 +35.04645385170301,11.851113637365323 +34.96924415673933,11.930173679568703 +34.89079602195314,12.008005019131598 +34.81112899826429,12.084588258852476 +34.730262940365265,12.159904312582452 +34.64821800177302,12.233934409981963 +34.565014629806306,12.306660101198718 +34.48067356048975,12.378063261465794 +34.39521581338601,12.448126095618711 +34.30866268635725,12.516831142530346 +34.2210357502573,12.584161279462634 +34.1323568435557,12.650099726333897 +34.04264806689512,12.714630049900794 +33.951931777583425,12.777736167853835 +33.86023058402175,12.839402352825422 +33.767567340070016,12.899613236309454 +33.67396513935128,12.958353812491461 +33.57944730949629,13.015609441988385 +33.48403740632978,13.071365855497003 +33.38775920799987,13.125609157350155 +33.29063670905202,13.17832582897983 +33.19269411444911,13.229502732286262 +33.093955833539056,13.279127112912246 +32.994446473971465,13.327186603421765 +32.89419083556494,13.373669226382235 +32.79321390412639,13.418563397349521 +32.691540845224075,13.461857927755023 +32.58919699791583,13.503542027694113 +32.486207868434,13.543605308615188 +32.38259912382879,13.582037785908728 +32.27839658557148,13.61882988139567 +32.173626223119186,13.653972425714478 +32.06831414744269,13.687456660606363 +31.962486604519103,13.719274241097999 +31.856169968790752,13.749417237581282 +31.749390736592158,13.77787813778955 +31.642175519546583,13.804649848669799 +31.534551037933838,13.829725698150416 +31.426544114031067,13.853099436804001 +31.318181665428025,13.874765239404862 +31.209490698318696,13.894717706380774 +31.100498300770756,13.912951865158666 +30.99123163597466,13.9294631714039 +30.881717935474025,13.944247510152804 +30.771984492378927,13.957301196838214 +30.662058654563914,13.968620978207742 +30.551967817852336,13.978204033134553 +30.441739419188714,13.986047973320453 +30.331400929800907,13.992150843891105 +30.220979848353675,13.996511123883216 +30.110503694095456,13.999127726623602 +30.0,14.0 +30.0,14.0 +29.748743718592966,14.0 +29.49748743718593,14.0 +29.246231155778894,14.0 +28.99497487437186,14.0 +28.743718592964825,14.0 +28.492462311557787,14.0 +28.241206030150753,14.0 +27.98994974874372,14.0 +27.738693467336685,14.0 +27.48743718592965,14.0 +27.236180904522612,14.0 +26.984924623115578,14.0 +26.733668341708544,14.0 +26.482412060301506,14.0 +26.231155778894472,14.0 +25.979899497487438,14.0 +25.728643216080403,14.0 +25.47738693467337,14.0 +25.22613065326633,14.0 +24.974874371859297,14.0 +24.723618090452263,14.0 +24.472361809045225,14.0 +24.22110552763819,14.0 +23.969849246231156,14.0 +23.718592964824122,14.0 +23.467336683417088,14.0 +23.21608040201005,14.0 +22.964824120603016,14.0 +22.71356783919598,14.0 +22.462311557788944,14.0 +22.21105527638191,14.0 +21.959798994974875,14.0 +21.70854271356784,14.0 +21.457286432160807,14.0 +21.20603015075377,14.0 +20.954773869346734,14.0 +20.7035175879397,14.0 +20.452261306532662,14.0 +20.201005025125628,14.0 +19.949748743718594,14.0 +19.69849246231156,14.0 +19.447236180904525,14.0 +19.195979899497488,14.0 +18.944723618090453,14.0 +18.69346733668342,14.0 +18.44221105527638,14.0 +18.190954773869347,14.0 +17.939698492462313,14.0 +17.68844221105528,14.0 +17.437185929648244,14.0 +17.185929648241206,14.0 +16.934673366834172,14.0 +16.683417085427138,14.0 +16.4321608040201,14.0 +16.180904522613066,14.0 +15.929648241206031,14.0 +15.678391959798997,14.0 +15.427135678391961,14.0 +15.175879396984925,14.0 +14.92462311557789,14.0 +14.673366834170857,14.0 +14.42211055276382,14.0 +14.170854271356784,14.0 +13.91959798994975,14.0 +13.668341708542716,14.0 +13.417085427135682,14.0 +13.165829145728644,14.0 +12.91457286432161,14.0 +12.663316582914575,14.0 +12.412060301507537,14.0 +12.160804020100503,14.0 +11.909547738693469,14.0 +11.658291457286435,14.0 +11.4070351758794,14.0 +11.155778894472363,14.0 +10.904522613065328,14.0 +10.653266331658294,14.0 +10.402010050251256,14.0 +10.150753768844222,14.0 +9.899497487437188,14.0 +9.648241206030153,14.0 +9.39698492462312,14.0 +9.145728643216081,14.0 +8.894472361809047,14.0 +8.643216080402013,14.0 +8.391959798994975,14.0 +8.14070351758794,14.0 +7.8894472361809065,14.0 +7.638190954773872,14.0 +7.386934673366838,14.0 +7.1356783919598,14.0 +6.884422110552766,14.0 +6.633165829145732,14.0 +6.381909547738694,14.0 +6.1306532663316595,14.0 +5.879396984924625,14.0 +5.628140703517591,14.0 +5.376884422110557,14.0 +5.125628140703519,14.0 +4.874371859296485,14.0 +4.62311557788945,14.0 +4.371859296482413,14.0 +4.120603015075378,14.0 +3.869346733668344,14.0 +3.6180904522613098,14.0 +3.3668341708542755,14.0 +3.1155778894472377,14.0 +2.8643216080402034,14.0 +2.613065326633169,14.0 +2.3618090452261313,14.0 +2.110552763819097,14.0 +1.8592964824120628,14.0 +1.6080402010050285,14.0 +1.3567839195979943,14.0 +1.1055276381909565,14.0 +0.8542713567839222,14.0 +0.6030150753768879,14.0 +0.3517587939698501,14.0 +0.10050251256281584,14.0 +-0.15075376884421843,14.0 +-0.4020100502512527,14.0 +-0.653266331658287,14.0 +-0.9045226130653248,14.0 +-1.155778894472359,14.0 +-1.4070351758793933,14.0 +-1.6582914572864311,14.0 +-1.9095477386934654,14.0 +-2.1608040201004997,14.0 +-2.4120603015075375,14.0 +-2.663316582914568,14.0 +-2.914572864321606,14.0 +-3.1658291457286367,14.0 +-3.4170854271356745,14.0 +-3.6683417085427124,14.0 +-3.919597989949743,14.0 +-4.170854271356781,14.0 +-4.422110552763819,14.0 +-4.673366834170849,14.0 +-4.924623115577887,14.0 +-5.175879396984925,14.0 +-5.427135678391956,14.0 +-5.678391959798994,14.0 +-5.929648241206024,14.0 +-6.180904522613062,14.0 +-6.4321608040201,14.0 +-6.683417085427131,14.0 +-6.9346733668341685,14.0 +-7.185929648241199,14.0 +-7.437185929648237,14.0 +-7.688442211055275,14.0 +-7.9396984924623055,14.0 +-8.190954773869343,14.0 +-8.442211055276381,14.0 +-8.693467336683412,14.0 +-8.94472361809045,14.0 +-9.195979899497488,14.0 +-9.447236180904518,14.0 +-9.698492462311556,14.0 +-9.949748743718587,14.0 +-10.201005025125625,14.0 +-10.452261306532662,14.0 +-10.703517587939693,14.0 +-10.954773869346731,14.0 +-11.206030150753762,14.0 +-11.4572864321608,14.0 +-11.708542713567837,14.0 +-11.959798994974868,14.0 +-12.211055276381906,14.0 +-12.462311557788944,14.0 +-12.713567839195974,14.0 +-12.964824120603012,14.0 +-13.21608040201005,14.0 +-13.46733668341708,14.0 +-13.718592964824118,14.0 +-13.96984924623115,14.0 +-14.221105527638187,14.0 +-14.472361809045225,14.0 +-14.723618090452256,14.0 +-14.974874371859293,14.0 +-15.226130653266324,14.0 +-15.477386934673362,14.0 +-15.7286432160804,14.0 +-15.97989949748743,14.0 +-16.23115577889447,14.0 +-16.482412060301506,14.0 +-16.733668341708537,14.0 +-16.984924623115575,14.0 +-17.236180904522612,14.0 +-17.487437185929643,14.0 +-17.73869346733668,14.0 +-17.98994974874371,14.0 +-18.24120603015075,14.0 +-18.492462311557787,14.0 +-18.743718592964818,14.0 +-18.994974874371856,14.0 +-19.246231155778887,14.0 +-19.497487437185924,14.0 +-19.748743718592962,14.0 +-20.0,14.0 +-20.0,14.0 +-20.11050369409546,13.999127726623602 +-20.220979848353675,13.996511123883216 +-20.331400929800907,13.992150843891105 +-20.441739419188714,13.986047973320453 +-20.551967817852336,13.978204033134553 +-20.662058654563914,13.968620978207742 +-20.771984492378927,13.957301196838214 +-20.881717935474022,13.944247510152804 +-20.99123163597466,13.9294631714039 +-21.100498300770756,13.912951865158666 +-21.209490698318696,13.894717706380774 +-21.318181665428025,13.874765239404862 +-21.426544114031064,13.853099436804001 +-21.534551037933838,13.829725698150416 +-21.64217551954658,13.804649848669799 +-21.749390736592158,13.77787813778955 +-21.856169968790752,13.749417237581282 +-21.9624866045191,13.719274241097999 +-22.068314147442692,13.687456660606363 +-22.173626223119182,13.65397242571448 +-22.278396585571482,13.61882988139567 +-22.38259912382879,13.582037785908728 +-22.486207868434,13.543605308615188 +-22.589196997915828,13.503542027694113 +-22.691540845224075,13.461857927755025 +-22.793213904126386,13.418563397349521 +-22.894190835564938,13.373669226382235 +-22.99444647397147,13.327186603421765 +-23.093955833539056,13.279127112912246 +-23.19269411444911,13.229502732286262 +-23.29063670905202,13.17832582897983 +-23.38775920799987,13.125609157350155 +-23.484037406329783,13.071365855497003 +-23.579447309496288,13.015609441988385 +-23.673965139351278,12.958353812491463 +-23.76756734007002,12.899613236309454 +-23.860230584021753,12.839402352825424 +-23.951931777583425,12.777736167853835 +-24.04264806689512,12.714630049900794 +-24.132356843555698,12.650099726333897 +-24.2210357502573,12.584161279462634 +-24.308662686357255,12.516831142530346 +-24.39521581338601,12.44812609561871 +-24.48067356048975,12.378063261465796 +-24.565014629806306,12.306660101198718 +-24.648218001773017,12.233934409981964 +-24.73026294036526,12.159904312582452 +-24.81112899826429,12.084588258852476 +-24.890796021953143,12.008005019131598 +-24.96924415673933,11.930173679568705 +-25.046453851703014,11.851113637365323 +-25.122405864569537,11.770844595941437 +-25.19708126650498,11.689386560024985 +-25.27046144683364,11.606759830666263 +-25.34252811767618,11.522985000178467 +-25.413263318507372,11.438082947005672 +-25.482649420632235,11.352074830519467 +-25.55066913157945,11.264982085745617 +-25.617305499411046,11.176826418021992 +-25.682541916947145,11.087629797589145 +-25.746362125904838,10.997414454114864 +-25.80875022095008,10.90620287115408 +-25.869690653661642,10.814017780545493 +-25.929168236406092,10.72088215674631 +-25.987168146122883,10.626819211106541 +-26.043675928018565,10.531852386084243 +-26.098677499169217,10.436005349403155 +-26.152159152030222,10.339301988154226 +-26.20410755785246,10.241766402842435 +-26.25450977000412,10.143422901380442 +-26.303353227197263,10.044295993030552 +-26.35062575661837,9.944410382296489 +-26.396315576962056,9.843790962766523 +-26.44041130136722,9.742462810909451 +-26.482901940254887,9.640451179825032 +-26.52377690406704,9.537781492950373 +-26.563026005905776,9.434479337723877 +-26.60063946407206,9.330570459208328 +-26.636607904503567,9.226080753674667 +-26.670922363110876,9.12103626214811 +-26.703574288011506,9.015463163918167 +-26.73455554166124,8.909387770014222 +-26.763858402882153,8.802836516648277 +-26.791475568786904,8.695835958626482 +-26.817400156598765,8.588412762731132 +-26.84162570536695,8.480593701074733 +-26.864146177576828,8.372405644427847 +-26.884955960654587,8.263875555522317 +-26.90404986836602,8.155030482331595 +-26.92142314210902,8.045897551329826 +-26.937071452099545,7.936503960731354 +-26.950990898450687,7.8268769737123565 +-26.9631780121446,7.717043911616294 +-26.973629755897047,7.607032147144855 +-26.98234352491437,7.496869097536111 +-26.98931714754265,7.386582217731557 +-26.99454888580893,7.27619899353377 +-26.998037435854346,7.165746934756369 +-26.999781928259093,7.055253568367992 +-26.999781928259093,6.944746431632007 +-26.998037435854346,6.834253065243632 +-26.99454888580893,6.723801006466231 +-26.98931714754265,6.613417782268444 +-26.98234352491437,6.503130902463888 +-26.973629755897047,6.392967852855144 +-26.9631780121446,6.282956088383707 +-26.950990898450687,6.173123026287644 +-26.937071452099545,6.063496039268647 +-26.92142314210902,5.954102448670174 +-26.90404986836602,5.844969517668405 +-26.884955960654587,5.736124444477685 +-26.864146177576828,5.627594355572154 +-26.84162570536695,5.519406298925267 +-26.817400156598765,5.41158723726887 +-26.791475568786904,5.304164041373517 +-26.763858402882153,5.197163483351723 +-26.73455554166124,5.090612229985778 +-26.703574288011506,4.984536836081835 +-26.670922363110876,4.878963737851892 +-26.636607904503567,4.773919246325333 +-26.60063946407206,4.669429540791672 +-26.563026005905776,4.565520662276123 +-26.523776904067045,4.462218507049629 +-26.48290194025489,4.359548820174968 +-26.44041130136722,4.257537189090549 +-26.396315576962056,4.156209037233477 +-26.35062575661837,4.055589617703512 +-26.303353227197263,3.9557040069694493 +-26.25450977000412,3.856577098619558 +-26.20410755785246,3.758233597157566 +-26.152159152030222,3.6606980118457724 +-26.098677499169217,3.5639946505968454 +-26.043675928018565,3.4681476139157583 +-25.987168146122883,3.3731807888934573 +-25.929168236406092,3.2791178432536907 +-25.869690653661642,3.1859822194545067 +-25.80875022095008,3.0937971288459196 +-25.746362125904838,3.002585545885137 +-25.682541916947145,2.9123702024108553 +-25.617305499411046,2.8231735819780086 +-25.55066913157945,2.7350179142543825 +-25.482649420632235,2.6479251694805344 +-25.413263318507372,2.561917052994329 +-25.342528117676178,2.477014999821531 +-25.27046144683364,2.393240169333737 +-25.19708126650498,2.310613439975013 +-25.122405864569537,2.229155404058565 +-25.046453851703014,2.148886362634678 +-24.969244156739332,2.0698263204312966 +-24.890796021953147,1.9919949808684017 +-24.811128998264287,1.9154117411475244 +-24.73026294036526,1.8400956874175485 +-24.648218001773017,1.7660655900180364 +-24.565014629806306,1.6933398988012822 +-24.48067356048975,1.6219367385342052 +-24.39521581338601,1.5518739043812895 +-24.308662686357255,1.4831688574696535 +-24.2210357502573,1.4158387205373648 +-24.1323568435557,1.349900273666103 +-24.04264806689512,1.2853699500992066 +-23.951931777583425,1.222263832146166 +-23.860230584021753,1.160597647174578 +-23.76756734007002,1.1003867636905458 +-23.673965139351278,1.0416461875085385 +-23.579447309496288,0.9843905580116159 +-23.48403740632978,0.9286341445029969 +-23.38775920799987,0.874390842649845 +-23.29063670905202,0.8216741710201712 +-23.19269411444911,0.770497267713738 +-23.093955833539056,0.7208728870877552 +-22.994446473971465,0.6728133965782348 +-22.894190835564938,0.6263307736177648 +-22.793213904126386,0.5814366026504789 +-22.691540845224075,0.5381420722449759 +-22.589196997915828,0.49645797230588595 +-22.486207868434,0.45639469138481203 +-22.38259912382879,0.417962214091272 +-22.278396585571482,0.3811701186043308 +-22.173626223119182,0.3460275742855208 +-22.068314147442692,0.31254333939363654 +-21.962486604519103,0.280725758902002 +-21.856169968790752,0.250582762418718 +-21.749390736592158,0.22212186221044927 +-21.642175519546583,0.19535015133020117 +-21.534551037933838,0.17027430184958448 +-21.426544114031067,0.14690056319599876 +-21.318181665428025,0.12523476059513694 +-21.209490698318696,0.10528229361922659 +-21.100498300770756,0.08704813484133389 +-20.99123163597466,0.07053682859609989 +-20.881717935474025,0.05575248984719572 +-20.771984492378927,0.04269880316178565 +-20.662058654563914,0.031379021792258044 +-20.551967817852336,0.02179596686544727 +-20.441739419188714,0.013952026679546314 +-20.331400929800907,0.007849156108894917 +-20.220979848353675,0.0034888761167835014 +-20.110503694095456,0.0008722733763990576 +-20.0,0.0 +-20.0,0.0 +-19.899497487437184,0.0 +-19.798994974874372,0.0 +-19.698492462311556,0.0 +-19.597989949748744,0.0 +-19.497487437185928,0.0 +-19.396984924623116,0.0 +-19.2964824120603,0.0 +-19.195979899497488,0.0 +-19.09547738693467,0.0 +-18.99497487437186,0.0 +-18.894472361809044,0.0 +-18.79396984924623,0.0 +-18.693467336683415,0.0 +-18.592964824120603,0.0 +-18.492462311557787,0.0 +-18.391959798994975,0.0 +-18.29145728643216,0.0 +-18.190954773869347,0.0 +-18.09045226130653,0.0 +-17.98994974874372,0.0 +-17.889447236180903,0.0 +-17.78894472361809,0.0 +-17.688442211055275,0.0 +-17.587939698492463,0.0 +-17.487437185929647,0.0 +-17.386934673366834,0.0 +-17.28643216080402,0.0 +-17.185929648241206,0.0 +-17.08542713567839,0.0 +-16.984924623115578,0.0 +-16.884422110552762,0.0 +-16.78391959798995,0.0 +-16.683417085427134,0.0 +-16.582914572864322,0.0 +-16.482412060301506,0.0 +-16.381909547738694,0.0 +-16.281407035175878,0.0 +-16.180904522613066,0.0 +-16.08040201005025,0.0 +-15.979899497487438,0.0 +-15.879396984924622,0.0 +-15.77889447236181,0.0 +-15.678391959798994,0.0 +-15.577889447236181,0.0 +-15.477386934673365,0.0 +-15.376884422110553,0.0 +-15.276381909547737,0.0 +-15.175879396984925,0.0 +-15.07537688442211,0.0 +-14.974874371859297,0.0 +-14.874371859296481,0.0 +-14.773869346733669,0.0 +-14.673366834170853,0.0 +-14.57286432160804,0.0 +-14.472361809045225,0.0 +-14.371859296482413,0.0 +-14.271356783919597,0.0 +-14.170854271356784,0.0 +-14.070351758793969,0.0 +-13.969849246231156,0.0 +-13.86934673366834,0.0 +-13.768844221105528,0.0 +-13.668341708542712,0.0 +-13.5678391959799,0.0 +-13.467336683417084,0.0 +-13.366834170854272,0.0 +-13.266331658291456,0.0 +-13.165829145728644,0.0 +-13.065326633165828,0.0 +-12.964824120603016,0.0 +-12.8643216080402,0.0 +-12.763819095477388,0.0 +-12.663316582914572,0.0 +-12.56281407035176,0.0 +-12.462311557788944,0.0 +-12.361809045226131,0.0 +-12.261306532663315,0.0 +-12.160804020100503,0.0 +-12.060301507537687,0.0 +-11.959798994974873,0.0 +-11.85929648241206,0.0 +-11.758793969849245,0.0 +-11.658291457286431,0.0 +-11.557788944723617,0.0 +-11.457286432160803,0.0 +-11.356783919597989,0.0 +-11.256281407035175,0.0 +-11.15577889447236,0.0 +-11.055276381909547,0.0 +-10.954773869346733,0.0 +-10.854271356783919,0.0 +-10.753768844221105,0.0 +-10.65326633165829,0.0 +-10.552763819095476,0.0 +-10.452261306532662,0.0 +-10.351758793969848,0.0 +-10.251256281407034,0.0 +-10.15075376884422,0.0 +-10.050251256281406,0.0 +-9.949748743718592,0.0 +-9.849246231155778,0.0 +-9.748743718592964,0.0 +-9.64824120603015,0.0 +-9.547738693467336,0.0 +-9.447236180904522,0.0 +-9.346733668341708,0.0 +-9.246231155778894,0.0 +-9.14572864321608,0.0 +-9.045226130653266,0.0 +-8.944723618090451,0.0 +-8.844221105527637,0.0 +-8.743718592964823,0.0 +-8.64321608040201,0.0 +-8.542713567839195,0.0 +-8.442211055276381,0.0 +-8.341708542713567,0.0 +-8.241206030150753,0.0 +-8.140703517587939,0.0 +-8.040201005025125,0.0 +-7.939698492462311,0.0 +-7.839195979899497,0.0 +-7.738693467336683,0.0 +-7.638190954773869,0.0 +-7.537688442211055,0.0 +-7.4371859296482405,0.0 +-7.3366834170854265,0.0 +-7.236180904522612,0.0 +-7.135678391959798,0.0 +-7.035175879396984,0.0 +-6.93467336683417,0.0 +-6.834170854271356,0.0 +-6.733668341708542,0.0 +-6.633165829145728,0.0 +-6.532663316582914,0.0 +-6.4321608040201,0.0 +-6.331658291457286,0.0 +-6.231155778894472,0.0 +-6.130653266331658,0.0 +-6.030150753768844,0.0 +-5.92964824120603,0.0 +-5.829145728643216,0.0 +-5.7286432160804015,0.0 +-5.628140703517587,0.0 +-5.527638190954773,0.0 +-5.427135678391959,0.0 +-5.326633165829145,0.0 +-5.226130653266331,0.0 +-5.125628140703517,0.0 +-5.025125628140703,0.0 +-4.924623115577889,0.0 +-4.824120603015075,0.0 +-4.723618090452261,0.0 +-4.623115577889447,0.0 +-4.522613065326633,0.0 +-4.422110552763819,0.0 +-4.321608040201005,0.0 +-4.221105527638191,0.0 +-4.1206030150753765,0.0 +-4.0201005025125625,0.0 +-3.9195979899497466,0.0 +-3.8190954773869343,0.0 +-3.7185929648241185,0.0 +-3.618090452261306,0.0 +-3.5175879396984904,0.0 +-3.417085427135678,0.0 +-3.3165829145728623,0.0 +-3.21608040201005,0.0 +-3.115577889447234,0.0 +-3.015075376884422,0.0 +-2.914572864321606,0.0 +-2.8140703517587937,0.0 +-2.713567839195978,0.0 +-2.6130653266331656,0.0 +-2.5125628140703498,0.0 +-2.4120603015075375,0.0 +-2.3115577889447216,0.0 +-2.2110552763819094,0.0 +-2.1105527638190935,0.0 +-2.0100502512562812,0.0 +-1.9095477386934654,0.0 +-1.809045226130653,0.0 +-1.7085427135678373,0.0 +-1.608040201005025,0.0 +-1.5075376884422091,0.0 +-1.4070351758793969,0.0 +-1.306532663316581,0.0 +-1.2060301507537687,0.0 +-1.105527638190953,0.0 +-1.0050251256281406,0.0 +-0.9045226130653248,0.0 +-0.8040201005025125,0.0 +-0.7035175879396967,0.0 +-0.6030150753768844,0.0 +-0.5025125628140685,0.0 +-0.40201005025125625,0.0 +-0.3015075376884404,0.0 +-0.20100502512562812,0.0 +-0.10050251256281229,0.0 +0.0,0.0 diff --git a/GEMstack/knowledge/routes/racing_circle.csv b/GEMstack/knowledge/routes/racing_circle.csv new file mode 100644 index 000000000..caa59e86e --- /dev/null +++ b/GEMstack/knowledge/routes/racing_circle.csv @@ -0,0 +1,71 @@ +0,0,0 +1,0,0 +2,0,0 +3,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 +16,0,0 +17,0,0 +18,0,0 +19,0,0 +20,0,0 +21,0,0 +22,0,0 +23,0,0 +24,0,0 +25,0,0 +26,0,0 +27,0,0 +28,0,0 +29,0,0 +30,0,0 +31,0,0 +32,0,0 +33,0,0 +34,0,0 +35,0,0 +36,0,0 +37,0,0 +38,0,0 +39,0,0 +40,0,0 +41,0,0 +42,0,0 +43,0,0 +44,0,0 +45,0,0 +46,0,0 +47,0,0 +48,0,0 +49,0,0 +50,0,0 +51,0,0 +52,0,0 +53,0,0 +54,0,0 +55,0,0 +56,0,0 +57,0,0 +58,0,0 +59,0,0 +60,0,0 +61,0,0 +62,0,0 +63,0,0 +64,0,0 +65,0,0 +66,0,0 +67,0,0 +68,0,0 +69,0,0 +70,0,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/slalom.csv b/GEMstack/knowledge/routes/slalom.csv new file mode 100644 index 000000000..caa59e86e --- /dev/null +++ b/GEMstack/knowledge/routes/slalom.csv @@ -0,0 +1,71 @@ +0,0,0 +1,0,0 +2,0,0 +3,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 +16,0,0 +17,0,0 +18,0,0 +19,0,0 +20,0,0 +21,0,0 +22,0,0 +23,0,0 +24,0,0 +25,0,0 +26,0,0 +27,0,0 +28,0,0 +29,0,0 +30,0,0 +31,0,0 +32,0,0 +33,0,0 +34,0,0 +35,0,0 +36,0,0 +37,0,0 +38,0,0 +39,0,0 +40,0,0 +41,0,0 +42,0,0 +43,0,0 +44,0,0 +45,0,0 +46,0,0 +47,0,0 +48,0,0 +49,0,0 +50,0,0 +51,0,0 +52,0,0 +53,0,0 +54,0,0 +55,0,0 +56,0,0 +57,0,0 +58,0,0 +59,0,0 +60,0,0 +61,0,0 +62,0,0 +63,0,0 +64,0,0 +65,0,0 +66,0,0 +67,0,0 +68,0,0 +69,0,0 +70,0,0 \ No newline at end of file diff --git a/GEMstack/knowledge/routes/turn_right.csv b/GEMstack/knowledge/routes/turn_right.csv new file mode 100644 index 000000000..7ec523657 --- /dev/null +++ b/GEMstack/knowledge/routes/turn_right.csv @@ -0,0 +1,21 @@ +0,0,0 +1,0,0 +2,0,0 +3,0,0 +4,0,0 +5,0,0 +6,0,0 +7,0,0 +8,0,0 +9,0,0 +10,0,0 +10,-1,0 +10,-2,0 +10,-3,0 +10,-4,0 +10,-5,0 +10,-6,0 +10,-7,0 +10,-8,0 +10,-9,0 +10,-10,0 diff --git a/GEMstack/knowledge/routes/turn_right2.csv b/GEMstack/knowledge/routes/turn_right2.csv new file mode 100644 index 000000000..f8ae9b5df --- /dev/null +++ b/GEMstack/knowledge/routes/turn_right2.csv @@ -0,0 +1,28 @@ +0,0,0 +1,0,0 +2,0,0 +3,0,0 +4,0,0 +5,0,0 +6,0,0 +7,0,0 +8,0,0 +9,0,0 +11,-.1,0 +12,-.5,0 +13,-1,0 +14,-2,0 +14.5,-2.8,0 +14.75,-3.5,0 +14.9,-4,0 +15,-5,0 +15,-6,0 +15,-7,0 +15,-8,0 +15,-9,0 +15,-10,0 +15,-11,0 +15,-12,0 +15,-13,0 +15,-14,0 +15,-16,0 diff --git a/GEMstack/knowledge/routes/turn_right_wider.csv b/GEMstack/knowledge/routes/turn_right_wider.csv new file mode 100644 index 000000000..0041e35f7 --- /dev/null +++ b/GEMstack/knowledge/routes/turn_right_wider.csv @@ -0,0 +1,34 @@ +0,0,0 +1,0,0 +2,0,0 +3,0,0 +4,0,0 +5,0,0 +6,0,0 +7,0,0 +8,0,0 +9,0,0 +10,0,0 +11,-.06,0 +12,-.2,0 +13,-.5,0 +14,-.85,0 +15,-1.4,0 +16,-2,0 +17,-2.85,0 +18,-4,0 +19,-5.7,0 +19.5,-6.9,0 +19.75,-7.8,0 +19.95,-9,0 +20,-10,0 +20,-11,0 +20,-12,0 +20,-13,0 +20,-14,0 +20,-15,0 +20,-16,0 +20,-17,0 +20,-18,0 +20,-19,0 +20,-20,0 \ No newline at end of file diff --git a/GEMstack/knowledge/vehicle/gem_e2_fast_limits.yaml b/GEMstack/knowledge/vehicle/gem_e2_fast_limits.yaml index 59150d0c3..ccd374398 100644 --- a/GEMstack/knowledge/vehicle/gem_e2_fast_limits.yaml +++ b/GEMstack/knowledge/vehicle/gem_e2_fast_limits.yaml @@ -5,4 +5,6 @@ max_acceleration: 3.0 #m/s^2 max_deceleration: 5.0 #m/s^2 max_accelerator_pedal: 1.0 #0-1 max_brake_pedal: 1.0 #0-1 - +max_longitudinal_acceleration: 2.07 #m/s^2 e4 +min_longitudinal_acceleration: -2.61 #m/s^2 deceleration 2.61 e4 +max_lateral_acceleration: 3.3 #m/s^2 e4 diff --git a/GEMstack/knowledge/vehicle/gem_e4.yaml b/GEMstack/knowledge/vehicle/gem_e4.yaml index 78785ab03..23208868f 100644 --- a/GEMstack/knowledge/vehicle/gem_e4.yaml +++ b/GEMstack/knowledge/vehicle/gem_e4.yaml @@ -7,7 +7,7 @@ max_command_rate : 10.0 #for hardware, max rate of commands to send to #using !include directives helps maintain reuse of common settings geometry: !include gem_e4_geometry.yaml dynamics: !include gem_e4_dynamics.yaml -limits: !include gem_e2_slow_limits.yaml +limits: !include gem_e2_fast_limits.yaml control_defaults: !include gem_e2_control_defaults.yaml calibration: !include ../calibration/gem_e4.yaml sensors: !include gem_e4_sensor_environment.yaml diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index 86cf2f683..9855249f8 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -3,7 +3,7 @@ from ...mathutils.dubins import SecondOrderDubinsCar from ...mathutils.dynamics import simulate from ...mathutils import transforms -from ...state import VehicleState,ObjectPose,ObjectFrameEnum,Roadgraph,AgentState,AgentEnum,AgentActivityEnum,Obstacle,Sign,AllState,VehicleGearEnum +from ...state import VehicleState,ObjectPose,ObjectFrameEnum,Roadgraph,AgentState,AgentEnum,AgentActivityEnum,Obstacle,Sign,AllState from ...knowledge.vehicle.geometry import front2steer,steer2front,heading_rate from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration, acceleration_to_pedal_positions from ...utils.loops import TimedLooper @@ -19,7 +19,7 @@ 'bicyclist' : (1.8,0.5,1.6), 'car' : (4.0,2.5,1.4), 'medium_truck': (6.0,2.5,3.0), - 'large_truck': (10.0,2.5,3.5) + 'large_truck': (10.0,2.5,3.5), } AGENT_TYPE_TO_ENUM = { @@ -27,7 +27,7 @@ 'bicyclist' : AgentEnum.BICYCLIST, 'car' : AgentEnum.CAR, 'medium_truck': AgentEnum.MEDIUM_TRUCK, - 'large_truck': AgentEnum.LARGE_TRUCK + 'large_truck': AgentEnum.LARGE_TRUCK, } AGENT_NOMINAL_VELOCITY = { @@ -35,7 +35,7 @@ 'bicyclist' : 5.0, 'car' : 20.0, 'medium_truck': 15.0, - 'large_truck': 10.0 + 'large_truck': 10.0, } AGENT_NOMINAL_ACCELERATION = { @@ -43,7 +43,7 @@ 'bicyclist' : 2.0, 'car' : 5.0, 'medium_truck': 3.0, - 'large_truck': 2.0 + 'large_truck': 2.0, } class AgentSimulation: @@ -207,7 +207,7 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]): reading.brake_pedal_position = brake_pedal_position reading.accelerator_pedal_position = 0 reading.speed = v - if v > 0: + if v >= 0: reading.gear = 1 else: reading.gear = -1 diff --git a/GEMstack/onboard/planning/launch_control.py b/GEMstack/onboard/planning/launch_control.py new file mode 100644 index 000000000..173505bd8 --- /dev/null +++ b/GEMstack/onboard/planning/launch_control.py @@ -0,0 +1,42 @@ +import rospy + +# To use launch control, enable as an arg for stanley or pure_pursuit controller in the launch file. + +class LaunchControl: + def __init__(self, stage_duration, stop_threshold): + self.enable_launch_control = True + self.stage_duration = stage_duration + self.stop_threshold = stop_threshold + self._launch_start_time = None + + def reset(self): + self.enable_launch_control = True + self._launch_start_time = None + + + def apply_launch_control(self, cmd, vehicle_velocity): + + if self._launch_start_time == None: + self._launch_start_time = rospy.get_time() + elapsed = rospy.get_time() - self._launch_start_time + if self.enable_launch_control: + print("launch control active") + + if elapsed < self.stage_duration: + cmd.accelerator_pedal_position = 0.0 + cmd.brake_pedal_position = 1.0 + elif elapsed < 2 * self.stage_duration: + cmd.accelerator_pedal_position = 1.0 + cmd.brake_pedal_position = 1.0 + elif elapsed < 3 * self.stage_duration: + cmd.accelerator_pedal_position = 1.0 + cmd.brake_pedal_position = 0.0 + else: + self.enable_launch_control = False + + # if we stop, re-enable launch control + if vehicle_velocity < self.stop_threshold and elapsed > 3 * self.stage_duration: + self.reset() + self.enable_launch_control = False + + return cmd \ No newline at end of file diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index e4cb0f48b..45e56ccb0 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/GEMstack/onboard/planning/pure_pursuit.py @@ -7,11 +7,13 @@ from ...knowledge.vehicle.geometry import front2steer from ..interface.gem import GEMVehicleCommand from ..component import Component +from .launch_control import LaunchControl import numpy as np +import rospy class PurePursuit(object): """Implements a pure pursuit controller on a second-order Dubins vehicle.""" - def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = None, desired_speed = None): + def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = None, desired_speed = None, launch_control = None): self.look_ahead = lookahead if lookahead is not None else settings.get('control.pure_pursuit.lookahead',4.0) self.look_ahead_scale = lookahead_scale if lookahead_scale is not None else settings.get('control.pure_pursuit.lookahead_scale',3.0) self.crosstrack_gain = crosstrack_gain if crosstrack_gain is not None else settings.get('control.pure_pursuit.crosstrack_gain',0.41) @@ -30,6 +32,12 @@ def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = N self.max_decel = settings.get('vehicle.limits.max_deceleration') # m/s^2 self.pid_speed = PID(settings.get('control.longitudinal_control.pid_p',0.5), settings.get('control.longitudinal_control.pid_d',0.0), settings.get('control.longitudinal_control.pid_i',0.1), windup_limit=20) + if (self.desired_speed_source == 'racing'): + self.look_ahead = settings.get('control.racing.pure_pursuit.lookahead',4.0) + self.look_ahead_scale = settings.get('control.racing.pure_pursuit.lookahead_scale') + self.crosstrack_gain = settings.get('control.racing.pure_pursuit.crosstrack_gain',0.41) + self.pid_speed = PID(settings.get('control.racing.longitudinal_control.pid_p',0.5), settings.get('control.racing.longitudinal_control.pid_d',0.0), settings.get('control.racing.longitudinal_control.pid_i',0.1), windup_limit=20) + self.path_arg = None self.path = None # type: Trajectory #if trajectory = None, then only an untimed path was provided and we can't use the path velocity as the reference @@ -39,6 +47,9 @@ def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = N self.current_traj_parameter = 0.0 self.t_last = None + self.launch_control = launch_control + + def set_path(self, path : Path): if path == self.path_arg: return @@ -90,6 +101,7 @@ def compute(self, state : VehicleState, component : Component = None): #TODO: calculate parameter that is look_ahead distance away from the closest point? #(rather than just advancing the parameter) des_parameter = closest_parameter + self.look_ahead + self.look_ahead_scale * speed + print("desired speed source is:", self.desired_speed_source) print("Closest parameter: " + str(closest_parameter),"distance to path",closest_dist) if closest_dist > 0.1: print("Closest point",self.path.eval(closest_parameter),"vs",(curr_x,curr_y)) @@ -167,6 +179,15 @@ def compute(self, state : VehicleState, component : Component = None): print("Desired speed",desired_speed,"m/s",", trying to reach desired",next_desired_speed,"m/s") feedforward_accel= np.clip(feedforward_accel, -self.max_decel, self.max_accel) print("Feedforward accel: " + str(feedforward_accel) + " m/s^2") + elif self.desired_speed_source == 'racing': + # get radius of the upcoming curve_points + # right now we can only use 3 points + curve_points = 3 + curve_radius = self.path.fit_curve_radius((curr_x,curr_y), curve_points) + # map curve radius to desired_speed. Note curve_radius can be inf if all points are colinear + print("curve_radius: ", curve_radius) + + desired_speed = (.5*curve_radius)**.5 else: #decay speed when crosstrack error is high desired_speed *= np.exp(-abs(ct_error)*0.4) @@ -183,7 +204,9 @@ def compute(self, state : VehicleState, component : Component = None): if desired_speed > self.speed_limit: desired_speed = self.speed_limit + output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel) + if component is not None: component.debug('curr pt',(curr_x,curr_y)) component.debug('curr param',self.current_path_parameter) @@ -210,6 +233,12 @@ class PurePursuitTrajectoryTracker(Component): def __init__(self,vehicle_interface=None, **args): self.pure_pursuit = PurePursuit(**args) self.vehicle_interface = vehicle_interface + launch_control_enabled = self.pure_pursuit.launch_control + if launch_control_enabled: + stage_duration = settings.get('control.racing.launch_control.stage_duration', 0.5) + self.launch_control = LaunchControl(stage_duration, stop_threshold=0.1) + else: + self.launch_control = None def rate(self): return 50.0 @@ -227,7 +256,13 @@ def update(self, vehicle : VehicleState, trajectory: Trajectory): #print("Desired wheel angle",wheel_angle) steering_angle = np.clip(front2steer(wheel_angle), self.pure_pursuit.steering_angle_range[0], self.pure_pursuit.steering_angle_range[1]) #print("Desired steering angle",steering_angle) - self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel,steering_angle, vehicle)) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + if self.launch_control: + cmd = self.launch_control.apply_launch_control(cmd, vehicle.v) + + + self.vehicle_interface.send_command(cmd) def healthy(self): return self.pure_pursuit.path is not None \ No newline at end of file diff --git a/GEMstack/onboard/planning/racing_planning.py b/GEMstack/onboard/planning/racing_planning.py new file mode 100644 index 000000000..bb485e6c1 --- /dev/null +++ b/GEMstack/onboard/planning/racing_planning.py @@ -0,0 +1,1385 @@ +from ...state.trajectory import Trajectory +from ...state.vehicle import VehicleState +from ...state import Obstacle +from ..component import Component + +from ...state.trajectory import Trajectory, compute_headings, Path +from ...state.physical_object import ObjectFrameEnum + +import numpy as np +import matplotlib.pyplot as plt +from scipy.optimize import minimize +import threading + +from typing import Dict + +# -------------------------- +# This is the main code for the racing trajectory planner. +# Contributors: Hua-Ta, Shilan +# -------------------------- + +def scenario_check(vehicle_state, cone_state): + """ + Check the cone state and determine the scenario. + Args: + vehicle_state: The state of the vehicle. + cone_state: The state of the cones. + Returns: + str: The scenario type. + - 'left_pass': cone pointed left, go left + - 'right_pass': cone pointed right, go right + - 'u_turn': cone standing up, make a U-turn + tuple: (cone_x, cone__y) + """ + # Get the closest cone info + cones_ahead = sorted(cone_state, key=lambda c: np.linalg.norm( + np.array([c['x'], c['y']]) - np.array(vehicle_state['position']))) + cone_direction = cones_ahead[0]['orientation'] + cone_position = (cones_ahead[0]['x'], cones_ahead[0]['y']) + + # Check the cone orientation + if cone_direction != 'LEFT' and cone_direction != 'RIGHT' and cone_direction != 'STANDING' and cone_direction != '90turn': + raise ValueError("Unknown cone orientation") + + return cone_direction, cone_position + +def waypoint_generate(vehicle_state, cones, cone_idx, next_cone_idx, prev_cone_idx): + """ + Generate waypoints based on the scenario. + Args: + vehicle_state: The state of the vehicle. + cone_state: The state of the cones. + Returns: + Tuple[str, Tuple[float, float], Tuple[float, float]]: + scenario: detected scenario type + flex_wp: flexible waypoint (used to maneuver) + fixed_wp: fixed waypoint (goal position) + """ + print("cone idx: ", cone_idx, next_cone_idx, prev_cone_idx) + scenario, cone_position = scenario_check(vehicle_state, [cones[cone_idx]]) + car_position = np.array(vehicle_state['position']) + car_heading = vehicle_state['heading'] # in radians + current_heading = car_heading + target_heading = car_heading + + # ===== Parameters ===== + u_turn_radius = 8 # Radius for U-turn + offset = 2.0 # Offset for left/right pass + lookahead_distance = 10.0 # Distance ahead for fixed point + # ====================== + + # Direction vector based on heading + heading_vector = np.array([np.cos(car_heading), np.sin(car_heading)]) + + # Vector perpendicular to heading (to determine left/right) + perpendicular_vector = np.array([-np.sin(car_heading), np.cos(car_heading)]) + + if cone_idx > 0: + if prev_cone_idx == None: + prev_cone_idx = cone_idx - 1 + prev_cone_position = np.array((cones[prev_cone_idx]['x'], cones[prev_cone_idx]['y'])) + cone_position = np.array((cones[cone_idx]['x'], cones[cone_idx]['y'])) + target_heading = np.arctan2(cone_position[1] - prev_cone_position[1], + cone_position[0] - prev_cone_position[0]) + + # If next cone (2nd closest) is available, use it to determine lookahead_distance + if next_cone_idx != None: + next_cone_position = np.array((cones[next_cone_idx]['x'], cones[next_cone_idx]['y'])) + lookahead_distance = np.linalg.norm(next_cone_position - cone_position) / 2 + + if scenario == 'STANDING': + # U-turn: Generate points in a semi-circular arc around the cone + cone = np.array(cone_position) + + # Number of waypoints to generate for the arc + num_arc_points = 9 + + # Generate waypoints in a smooth semi-circular pattern on the RIGHT side + flex_wps_list = [] + + # Create a semi-circular arc from 0 to π + # But negate perpendicular_vector to go to the right side + for i in range(num_arc_points): + # Calculate angle in the semi-circle (0 to π) + angle = i * np.pi * 1 / (num_arc_points - 1) + + # MODIFIED: Negative perpendicular_vector to go to right side of cone + # This creates a counter-clockwise turn around the cone + wp = cone - u_turn_radius * ( + perpendicular_vector * np.cos(angle) - + heading_vector * np.sin(angle) + ) + + flex_wps_list.append(wp) + + # Fixed waypoint: behind the cone after U-turn (also on right side) + fixed_wp = cone + u_turn_radius * perpendicular_vector + + target_heading = target_heading + np.pi + + # For visualization only + # for i, wp in enumerate(flex_wps_list): + # plt.plot(wp[0], wp[1], 'ro') # Plot all waypoints in red + # plt.text(wp[0], wp[1], f'wp{i}', fontsize=9) + + # plt.plot(fixed_wp[0], fixed_wp[1], 'bo') # Plot fixed waypoint in blue + # plt.plot(cone[0], cone[1], 'gx') # Plot cone in green + # plt.plot(car_position[0], car_position[1], 'ks') # Plot car position + # plt.axis('equal') # Equal aspect ratio for better visualization + # plt.grid(True) + # plt.show() + + elif scenario == 'LEFT': + cone = np.array(cone_position) + + # Flexible waypoint: go to the left of the cone + flex_wp1 = cone + offset * perpendicular_vector + flex_wps_list = [flex_wp1] + + # Fixed waypoint: forward after passing the cone + # fixed_wp = flex_wp + heading_vector * lookahead_distance - offset * perpendicular_vector * 2 + fixed_wp = flex_wp1 + heading_vector * lookahead_distance - offset * perpendicular_vector + + elif scenario == 'RIGHT': + cone = np.array(cone_position) + + # Flexible waypoint: go to the right of the cone + flex_wp1 = cone - offset * perpendicular_vector + flex_wps_list = [flex_wp1] + + # Fixed waypoint: forward after passing the cone + # fixed_wp = flex_wp + heading_vector * lookahead_distance + offset * perpendicular_vector * 2 + fixed_wp = flex_wp1 + heading_vector * lookahead_distance + offset * perpendicular_vector + + # TODO: move to a different setting instead of as a scenario + elif scenario == '90turn': + turn_vector = np.array([-np.sin(car_heading + np.pi / 6), np.cos(car_heading + np.pi / 6)]) + cone = np.array(cone_position) + distance_to_cone = np.linalg.norm(car_position - cone) + + flex_wps_list = [] + if distance_to_cone > 7: + steps = int(distance_to_cone // 6) + print("steps:", steps) + + final_flex_wp = cone + 1.0 * perpendicular_vector + heading_vector * 1.0 + fixed_wp = cone - turn_vector * 2.5 + heading_vector * 0.0 + + cone_direction = (final_flex_wp - car_position) / np.linalg.norm(final_flex_wp - car_position) + for i in range(steps - 2): + intermediate_wp = car_position + cone_direction * ((i + 1) * 6) + flex_wps_list.append(intermediate_wp) + + flex_wps_list.append(final_flex_wp) + else: + flex_wp = cone + 1.0 * perpendicular_vector + heading_vector * 1.0 + fixed_wp = cone - turn_vector * 2.5 + heading_vector * 0.0 + flex_wps_list = [flex_wp] + + else: + flex_wps_list = None + fixed_wp = None + + target_heading = (target_heading + np.pi) % (2 * np.pi) - np.pi + current_heading = (current_heading + np.pi) % (2 * np.pi) - np.pi + + if abs(target_heading - current_heading) > np.pi: + if target_heading < current_heading: + target_heading += 2 * np.pi + else: + target_heading -= 2 * np.pi + + return scenario, flex_wps_list, fixed_wp, target_heading + +def trajectory_generation(init_state, final_state, N=30, T=0.1, Lr=1.5, + w_c=10.0, w_eps=0.0, w_vvar=4.0, + w_terminal=10.0, + v_min=3.0, v_max=11.0, + waypoints=None, waypoint_penalty_weight=100.0): + """ + Generate a dynamically feasible trajectory between init_state and final_state + using curvature-based vehicle dynamics and nonlinear optimization. + + Now supports multiple waypoints. + + Parameters: + - init_state (dict): Initial vehicle state with keys 'x', 'y', 'psi', 'c', 'v'. + - final_state (dict): Target vehicle state with keys 'x', 'y', 'psi', 'c'. + - N (int): Number of discrete time steps in the trajectory. + - T (float): Duration of each time step (in seconds). + - Lr (float): Distance from the vehicle's center to the rear axle. + - w_c (float): Weight for penalizing curvature (smoothness of turns). + - w_eps (float): Weight for penalizing curvature rate (reduces sharp steering changes). + - w_vvar (float): Weight for penalizing speed variance (encourages speed smoothness). + - w_terminal (float): Weight for penalizing final state deviation (soft constraint). + - v_min (float): Minimum allowed speed (in m/s). + - v_max (float): Maximum allowed speed (in m/s). + - waypoints (list or None): Optional list of (x, y) coordinates that the trajectory should pass near. + - waypoint_penalty_weight (float): Penalty weight for distance from waypoints (soft constraint). + + Returns: + - x, y, psi, c, v, eps (np.ndarray): Arrays of optimized state and control values. + - final_error (dict): Final state errors in x, y, psi, and c. + """ + final_heading = (final_state['psi'] + np.pi) % (2 * np.pi) - np.pi + init_heading = (init_state['psi'] + np.pi) % (2 * np.pi) - np.pi + + if abs(final_heading - init_heading) > np.pi: + if final_heading < init_heading: + final_heading += 2 * np.pi + else: + final_heading -= 2 * np.pi + + init_state['psi'] = init_heading + final_state['psi'] = final_heading + print("init and final headings: ", init_heading, final_heading) + + def cost(p): + x_, y_, psi_, c_, v_, eps_ = np.split(p, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)]) + c_seq = np.concatenate(([init_state['c']], c_)) + v_seq = np.concatenate(([init_state['v']], v_)) + x_final, y_final, psi_final, c_final = x_[-1], y_[-1], psi_[-1], c_[-1] + + cost_c = w_c * np.sum(c_seq ** 2) + cost_eps = w_eps * np.sum(eps_ ** 2) + v_mean = np.mean(v_seq) + cost_vvar = w_vvar * np.mean((v_seq - v_mean) ** 2) + + cost_terminal = w_terminal * ( + (x_final - final_state['x']) ** 2 + + (y_final - final_state['y']) ** 2 + + (psi_final - final_state['psi']) ** 2 * 100 + + (c_final - final_state['c']) ** 2 * 100 + ) + + cost_waypoints = 0.0 + if waypoints is not None and len(waypoints) > 0: + # Calculate equally spaced indices for each waypoint + num_waypoints = len(waypoints) + indices = [int((i + 1) * (N - 1) / (num_waypoints + 1)) for i in range(num_waypoints)] + + # Sum penalties for each waypoint at its corresponding index + for wp_idx, waypoint in enumerate(waypoints): + traj_idx = indices[wp_idx] + cost_waypoints += waypoint_penalty_weight * ( + (x_[traj_idx] - waypoint[0])**2 + (y_[traj_idx] - waypoint[1])**2 + ) + + return cost_c + cost_eps + cost_vvar + cost_terminal + cost_waypoints + def dynamics_constraints(p): + x_, y_, psi_, c_, v_, eps_ = np.split(p, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)]) + constraints = [] + x_prev, y_prev, psi_prev, c_prev, v_prev = init_state['x'], init_state['y'], init_state['psi'], init_state['c'], init_state['v'] + for k in range(N - 1): + dx = v_prev * np.cos(psi_prev + c_prev * Lr) * T + dy = v_prev * np.sin(psi_prev + c_prev * Lr) * T + dpsi = v_prev * c_prev * T + dc = eps_[k] * T + constraints.extend([ + x_[k] - (x_prev + dx), + y_[k] - (y_prev + dy), + psi_[k] - (psi_prev + dpsi), + c_[k] - (c_prev + dc) + ]) + x_prev, y_prev, psi_prev, c_prev, v_prev = x_[k], y_[k], psi_[k], c_[k], v_[k] + return constraints + + # Initial guesses + x_vals = np.linspace(init_state['x'], final_state['x'], N) + y_vals = np.linspace(init_state['y'], final_state['y'], N) + psi_vals = np.linspace(init_state['psi'], final_state['psi'], N) + c_vals = np.linspace(init_state['c'], final_state['c'], N) + v_vals = np.ones(N) * init_state['v'] + eps_vals = np.zeros(N - 1) + + p0 = np.concatenate([x_vals[1:], y_vals[1:], psi_vals[1:], c_vals[1:], v_vals[1:], eps_vals]) + bounds = [(None, None)] * (4 * (N - 1)) + [(v_min, v_max)] * (N - 1) + [(None, None)] * (N - 1) + + result = minimize(cost, p0, bounds=bounds, + constraints={'type': 'eq', 'fun': dynamics_constraints}, + options={'maxiter': 1000}) + if not result.success: + raise RuntimeError("Optimization failed") + + x_, y_, psi_, c_, v_, eps_ = np.split(result.x, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)]) + x_full = np.concatenate(([init_state['x']], x_)) + y_full = np.concatenate(([init_state['y']], y_)) + psi_full = np.concatenate(([init_state['psi']], psi_)) + c_full = np.concatenate(([init_state['c']], c_)) + v_full = np.concatenate(([init_state['v']], v_)) + + final_error = { + 'x_error': abs(x_full[-1] - final_state['x']), + 'y_error': abs(y_full[-1] - final_state['y']), + 'psi_error': abs(psi_full[-1] - final_state['psi']), + 'c_error': abs(c_full[-1] - final_state['c']), + } + + return x_full, y_full, psi_full, c_full, v_full, eps_, final_error + +def trajectory_generation_dynamics(init_state, final_state, N=30, Lr=1.5, + a_min=-3.0, a_max=3.0, + eps_min=-0.2, eps_max=0.2, + v_min=2.0, v_max=11.0, + T_min = 0.5, T_max = 1000.0, + waypoints=None, waypoint_penalty_weight=10, waypoint_headings=None): + """ + Generate a dynamically feasible trajectory between init_state and final_state + using curvature-based vehicle dynamics and nonlinear optimization. + + Now supports multiple waypoints. + + Parameters: + - init_state (dict): Initial vehicle state with keys 'x', 'y', 'psi', 'c', 'v'. + - final_state (dict): Target vehicle state with keys 'x', 'y', 'psi', 'c'. + - N (int): Number of discrete time steps in the trajectory. + - T (float): Duration of each time step (in seconds). + - Lr (float): Distance from the vehicle's center to the rear axle. + - w_c (float): Weight for penalizing curvature (smoothness of turns). + - w_eps (float): Weight for penalizing curvature rate (reduces sharp steering changes). + - w_vvar (float): Weight for penalizing speed variance (encourages speed smoothness). + - w_terminal (float): Weight for penalizing final state deviation (soft constraint). + - v_min (float): Minimum allowed speed (in m/s). + - v_max (float): Maximum allowed speed (in m/s). + - waypoints (list or None): Optional list of (x, y) coordinates that the trajectory should pass near. + - waypoint_penalty_weight (float): Penalty weight for distance from waypoints (soft constraint). + + Returns: + - x, y, psi, c, v, eps (np.ndarray): Arrays of optimized state and control values. + - final_error (dict): Final state errors in x, y, psi, and c. + """ + def cost(p): + T_total = p[-1] + return T_total + + def dynamics_constraints(p): + # x_, y_, psi_, c_, v_, eps_ = np.split(p, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)]) + # print("x_: " +str(x_)) + split_idx = 5 *(N - 1) + x_, y_, psi_, c_, v_= np.split(p[:split_idx], 5) + + a = p[split_idx:split_idx + (N - 1)] + eps = p[split_idx + (N - 1):-1] + T_total = p[-1] + T_k = T_total / (N - 1) + + constraints = [] + x_prev, y_prev, psi_prev, c_prev, v_prev = init_state['x'], init_state['y'], init_state['psi'], init_state['c'], init_state['v'] + + for k in range(N - 1): + dx = v_prev * np.cos(psi_prev + c_prev * Lr) * T_k + dy = v_prev * np.sin(psi_prev + c_prev * Lr) * T_k + dpsi = v_prev * c_prev * T_k + dv = a[k] * T_k + dc = eps[k] * T_k + constraints.extend([ + x_[k] - (x_prev + dx), + y_[k] - (y_prev + dy), + psi_[k] - (psi_prev + dpsi), + v_[k] - (v_prev + dv), + c_[k] - (c_prev + dc) + ]) + x_prev, y_prev, psi_prev, c_prev, v_prev = x_[k], y_[k], psi_[k], c_[k], v_[k] + return constraints + + def waypoint_penalty(p): + if waypoints is None or len(waypoints) == 0: + return 0.0 + + split_idx = 5 *(N - 1) + x_, y_, psi_, c_, v_= np.split(p[:split_idx], 5) + # x_ = p[:N -1] + # y_ = p[N - 1:2 * (N-1)] + # psi_ = [ 3 * (N - 1)] + # Calculate equally spaced indices for each waypoint + num_waypoints = len(waypoints) + indices = [int((i + 1) * (N - 1) / (num_waypoints + 1)) for i in range(num_waypoints)] + penalty = 0.0 + alignment_threshold = 0.3 + # Sum penalties for each waypoint at its corresponding index + for wp_idx, waypoint in enumerate(waypoints): + traj_idx = indices[wp_idx] + penalty += waypoint_penalty_weight * ( + (x_[traj_idx] - waypoint[0])**2 + (y_[traj_idx] - waypoint[1])**2 + ) + return penalty + + # Initial guesses + x_vals = np.linspace(init_state['x'], final_state['x'], N) + y_vals = np.linspace(init_state['y'], final_state['y'], N) + psi_vals = np.linspace(init_state['psi'], final_state['psi'], N) + c_vals = np.linspace(init_state['c'], final_state['c'], N) + v_vals = np.ones(N) * init_state['v'] + + a_vals = np.zeros(N - 1) + eps_vals = np.zeros(N - 1) + T_guess = 60.0 + + p0 = np.concatenate([x_vals[1:], y_vals[1:], psi_vals[1:], c_vals[1:], v_vals[1:], + a_vals, eps_vals, [T_guess]]) + num_vars = len(p0) + bounds = ([(None, None)] * (4 * (N - 1)) + + [(v_min, v_max)] * (N - 1) + + [(a_min, a_max)] * (N - 1) + + [(eps_min, eps_max)] * (N - 1) + + [(T_min, T_max)] + ) + def total_cost(p): + return cost(p) + waypoint_penalty(p) + + result = minimize(total_cost, + p0, + bounds=bounds, + constraints={'type': 'eq', 'fun': dynamics_constraints}, + options={'maxiter': 1000}) + + # print("result.x:" + str(result.x)) + if not result.success: + raise RuntimeError("Optimization failed") + + split_idx = 5 *(N - 1) + x_, y_, psi_, c_, v_= np.split(result.x[:split_idx], 5) + + a = result.x[split_idx:split_idx + (N - 1)] + eps = result.x[split_idx + (N - 1):-1] + T_total = result.x[-1] + + x_full = np.concatenate(([init_state['x']], x_)) + y_full = np.concatenate(([init_state['y']], y_)) + psi_full = np.concatenate(([init_state['psi']], psi_)) + c_full = np.concatenate(([init_state['c']], c_)) + v_full = np.concatenate(([init_state['v']], v_)) + + final_error = { + 'x_error': abs(x_full[-1] - final_state['x']), + 'y_error': abs(y_full[-1] - final_state['y']), + 'psi_error': abs(psi_full[-1] - final_state['psi']), + 'c_error': abs(c_full[-1] - final_state['c']), + } + + return x_full, y_full, psi_full, c_full, v_full, a, eps, T_total, final_error + +def feasibility_check(trajectory, cone_map, car_width=2.0, safety_margin=0.3, v=10.0, Lr=1.5, T=0.1): + """ + Check if the car trajectory collides with any cones. + + Parameters: + - trajectory: list of (y, psi, c) states + - cone_map: list of (x, y) cone positions + - car_width: width of the vehicle in meters + - safety_margin: buffer around the vehicle + - v: vehicle constant speed (used for x position estimation) + - Lr: distance to rear axle + - T: time step + + Returns: + - feasible: True if no collisions + - collisions: list of indices of cones that were collided with (for plotting purpose) + - x_vals, y_vals: trajectory positions for plotting (for plotting purpose) + """ + y_vals, psi_vals, c_vals = zip(*trajectory) + x_vals = [0.0] + for i in range(1, len(trajectory)): + dx = v * np.cos(psi_vals[i-1] + c_vals[i-1] * Lr) * T + x_vals.append(x_vals[-1] + dx) + + collision_radius = (car_width / 2.0) + safety_margin + collisions = [] + + for j, (cone_x, cone_y) in enumerate(cone_map): + for x, y in zip(x_vals, y_vals): + if np.hypot(x - cone_x, y - cone_y) < collision_radius: + collisions.append(j) + break + + feasible = len(collisions) == 0 + return feasible, collisions, x_vals, y_vals + + +def waypoint_search_optimization(vehicle_state, cones, search_attempts=3): + valid_flex_wps = [] + valid_fixed_wps = [] + current_state = vehicle_state.copy() + cones_copy = cones.copy() + + for i in range(min(search_attempts, len(cones_copy))): + scenario, flex_wps, fixed_wp = waypoint_generate(current_state, cones_copy, cone_idx=i, next_cone_idx=i+1, prev_cone_idx=i-1) + + if flex_wps is None or fixed_wp is None: + break + + for flex_wp in flex_wps: + init_state = { + 'x': current_state['position'][0], + 'y': current_state['position'][1], + 'psi': current_state['heading'], + 'c': 0.0, + 'v': current_state['velocity'] + } + final_state = { + 'x': fixed_wp[0], + 'y': fixed_wp[1], + 'psi': current_state['heading'], + 'c': 0.0 + } + + try: + x, y, psi, c, v, eps, final_error = trajectory_generation(init_state, final_state, waypoints=flex_wp) + trajectory = list(zip(y, psi, c)) + feasible, collisions, x_vals, y_vals = feasibility_check(trajectory, [(cone['x'], cone['y']) for cone in cones]) + # print(f"Checking waypoint: {flex_wp}, Fixed: {fixed_wp}, Feasible: {feasible}, Collisions: {collisions}") + if feasible: + valid_flex_wps.append(flex_wp) + valid_fixed_wps.append(fixed_wp) + current_state['position'] = list(fixed_wp) + break + except: + continue + if len(cones_copy) > 0: + cones_copy.pop(0) + return valid_flex_wps, valid_fixed_wps + + +def to_gemstack_trajectory(x_all, y_all, v_all, T=0.1): + t_vals = np.arange(len(x_all)) * T + combined_xy = [[x, y] for x, y in zip(x_all, y_all)] + curr_path = Path(ObjectFrameEnum.START,combined_xy) + path = compute_headings(curr_path, smoothed=True) + path_normalized = path.arc_length_parameterize() + points = [p for p in path_normalized.points] + return Trajectory(points=points, times=t_vals, frame=ObjectFrameEnum.START) + + +def plan_full_slalom_trajectory(vehicle_state, cones): + x_all, y_all, v_all = [], [], [] + current_pos = np.array(vehicle_state['position']) + current_heading = vehicle_state['heading'] + + for cone_idx, cone in enumerate(cones): + scenario, flex_wps, fixed_wp, target_heading = waypoint_generate(vehicle_state, cones, cone_idx, next_cone_idx=cone_idx+1, prev_cone_idx=cone_idx-1) + print(f"Scenario: {scenario}, Cone: {cone}, Flex WP: {flex_wps}, Fixed WP: {fixed_wp}") + if not flex_wps or fixed_wp is None: + continue + # flex_wp = flex_wps[0] + current_heading = (current_heading + np.pi) % (2 * np.pi) - np.pi + + init_state = { + 'x': current_pos[0], 'y': current_pos[1], 'psi': current_heading, + 'c': 0.0, 'v': vehicle_state['velocity'] + } + final_state = { + 'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': target_heading, 'c': 0.0 + } + + x, y, psi, c, v, eps, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps) + x_all.extend(x) + y_all.extend(y) + v_all.extend(v) + + current_pos = np.array([x[-1], y[-1]]) + current_heading = psi[-1] + current_steering = c[-1] + + vehicle_state = { + 'position': current_pos, + 'heading': current_heading, + 'velocity': v[-1] + } + current_pos = np.array([x[-1], y[-1]]) + + # # Plot overall trajectory + # plt.figure() + # plt.plot(x_all, y_all, label='Overall Trajectory') + + # # Plot cones + # for i, cone in enumerate(cones): + # plt.scatter(cone['x'], cone['y'], color='orange', s=10, label='Cone' if i == 0 else "") + # plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange') + + # plt.title('4-Cone Full Course Trajectory') + # plt.xlabel('X') + # plt.ylabel('Y') + # plt.legend() + # plt.axis('equal') + # plt.grid(True) + # plt.show() + + combined_xy = [[x, y] for x, y in zip(x_all, y_all)] + # print(combined_xy) + path = Path(ObjectFrameEnum.START,combined_xy) + path = compute_headings(path) + path = path.arc_length_parameterize() + # print(path) + return path.racing_velocity_profile() + # return to_gemstack_trajectory(x_all, y_all, v_all) + + +def no_cone_planning(vehicle_dict): + temp_points = [] + vehicle_x, vehicle_y = vehicle_dict['position'][0], vehicle_dict['position'][1] + vehicle_heading = vehicle_dict['heading'] + vehicle_velocity = vehicle_dict['velocity'] + step_size = 0.00001 + for i in range(10): + temp_points.append([vehicle_x + i * step_size * np.cos(vehicle_heading), + vehicle_y + i * step_size * np.sin(vehicle_heading)]) + + path = Path(ObjectFrameEnum.START,temp_points) + path = compute_headings(path) + path = path.arc_length_parameterize() + # print(path) + return path.racing_velocity_profile() + +def got_new_cone(current_cones, prev_cones): + if current_cones is None: + return False + if prev_cones is None: + return True + prev_ids = {cone['id'] for cone in prev_cones} + + for cone in current_cones: + if cone['id'] not in prev_ids: + return True # Found a new cone not in previous list + + return False +################################################ +# Main Racing Trajectory Planner Class +################################################ +class SlalomTrajectoryPlanner(Component): + def __init__(self, **kwargs): + # You can accept args here if needed + self.prev_vehicle_position = None + self.trajectory = None + self.prev_cones = None + self.prev_cone_idx = None # Used to set heading in waypoint_generate + self.travelled_distance = 0.0 + self.cones = [] + # ---------------------------- + self.run_fake_plan = False # Set to True if you want to run the Predifined-Cones Simulation + self.onboard = True # Set to False if you want to run the Predifined-Cones Simulation + # ---------------------------- + # Planner runs on different thread + self.plan_thread = None + self.plan_lock = threading.Lock() + self.plan_pending = False + + self.DEBUG_MODE = True + + def state_inputs(self): + return ['obstacles', 'vehicle'] # Receive VehicleState & AgentState input + + def state_outputs(self): + return ['trajectory'] # Return trajectory output + + def rate(self): # Setup the update rate + return 1.0 + + def update(self, cone_obstacles: Dict[str, Obstacle], vehicle: VehicleState): + # Running on real vehicle + if self.onboard: + # Get all current detected cones + cones = [] + n = 0 + for id, detected_cone in cone_obstacles.items(): + if n > 3: + break + if n % 4 == 0: + curr_activity = 'LEFT' + elif n % 4 == 1: + curr_activity = 'RIGHT' + elif n % 4 == 2: + curr_activity = 'LEFT' + else: + curr_activity = 'STANDING' + c = { + 'id': id, + 'x': detected_cone.pose.x, + 'y': detected_cone.pose.y, + 'orientation': curr_activity + } + n = n + 1 + if c['id'] not in {cone['id'] for cone in self.cones}: + self.cones.append(c) + + curr_pos = np.array([vehicle.pose.x, vehicle.pose.y]) + if self.prev_vehicle_position is None: + distance_increment = 0.0 + else: + distance_increment = np.linalg.norm(curr_pos - self.prev_vehicle_position) + + self.prev_vehicle_position = curr_pos + + vehicle_dict = { + 'position': [vehicle.pose.x, vehicle.pose.y], + 'heading': vehicle.pose.yaw, + 'velocity': vehicle.v + } + if self.DEBUG_MODE: + print("===================== STATES =====================") + print(f"Vehicle State: {vehicle_dict}") + print(f"Detected Cones: {self.cones}") + print("===================== ====== =====================") + + # self.trajectory = self.online_trajectory_planning(vehicle_dict, self.cones, distance_increment) + if not self.plan_pending: + self.plan_pending = True + vehicle_copy = vehicle_dict.copy() + cones_copy = list(self.cones) + distance_copy = distance_increment + + def plan(): + new_traj = self.online_trajectory_planning(vehicle_copy, cones_copy, distance_copy) + with self.plan_lock: + self.trajectory = new_traj + self.plan_pending = False + + self.plan_thread = threading.Thread(target=plan) + self.plan_thread.start() + + # If no cones detected, drive forward + if len(self.cones) == 0: + self.trajectory = no_cone_planning(vehicle_dict) + + # Testing with predefined fake generated cone positions + elif self.run_fake_plan: + # Example Test - Slalom + cones = [ + {'x': 10, 'y': 0.0, 'orientation': 'LEFT'}, + {'x': 30, 'y': 1.0, 'orientation': 'RIGHT'}, + {'x': 50, 'y': 0.0, 'orientation': 'LEFT'}, + {'x': 70, 'y': 1.0, 'orientation': 'STANDING'}, + {'x': 50, 'y': 0.0, 'orientation': 'RIGHT'}, + {'x': 30, 'y': 1.0, 'orientation': 'LEFT'}, + {'x': 10, 'y': 0.0, 'orientation': 'RIGHT'} + ] + vehicle_dict = { + 'position': [vehicle.pose.x, vehicle.pose.y], + 'heading': vehicle.pose.yaw, + 'velocity': vehicle.v + } + + # Example Test - Racing Circle + # cones = [ + # {'x': -60.083, 'y': -33.118, 'orientation': '90turn'}, + # {'x': -6.392, 'y': -5.147, 'orientation': '90turn'}, + # {'x': -5.625, 'y': -13.637, 'orientation': '90turn'}, + # {'x': -56.258, 'y': -41.080, 'orientation': '90turn'} + # ] + # vehicle_dict = { + # 'position': [-60.0, -39.0], + # 'heading': np.pi * 2 / 3, + # 'velocity': 0.0 + # } + + self.trajectory = plan_full_slalom_trajectory(vehicle_dict, cones) + self.run_fake_plan = False + + # Update output + with self.plan_lock: + return self.trajectory + + def online_trajectory_planning(self, vehicle_state, cones, distance_increment, replan_threshold=100.0): + if not hasattr(self, 'prev_cones'): + self.prev_cones = None + + if not hasattr(self, 'no_cone_ahead'): + self.no_cone_ahead = False + + if not hasattr(self, 'visited_cone_ids'): + self.visited_cone_ids = set() + + stitch_idx = -1 + + def got_new_cone(current, prev): + if prev is None: + return True + prev_ids = {c['id'] for c in prev} + return any(c['id'] not in prev_ids for c in current) + + self.travelled_distance += distance_increment + new_cone_detected = got_new_cone(self.cones, self.prev_cones) + + # Plan at the beginning or when new cones detected or after threshold distance + if self.trajectory is None or new_cone_detected or not self.no_cone_ahead or True: + self.prev_cones = self.cones + + if self.trajectory is None: + current_position = vehicle_state['position'] + init_state = { + 'x': current_position[0], + 'y': current_position[1], + 'psi': vehicle_state['heading'], + 'c': 0.0, + 'v': vehicle_state['velocity'] + } + else: + stitch_idx, init_point, heading = self.get_future_point_on_trajectory(self.trajectory, vehicle_state['position'], lookahead_distance=500.0) + init_state = { + 'x': init_point[0], + 'y': init_point[1], + 'psi': heading, + 'c': 0.0, + 'v': vehicle_state['velocity'] + } + vehicle_state['position'] = np.array([init_point[0], init_point[1]]) + vehicle_state['heading'] = heading + + print("all cones: ", self.cones) + current_cone_idx, next_cone_idx, updated_cones = self.get_current_cone_idx(self.cones, init_state) + self.cones = updated_cones + print("init state: ", init_state) + print("current cone: ", current_cone_idx, next_cone_idx) + + # No cone ahead + if current_cone_idx == None: + self.no_cone_ahead = True + return self.trajectory + else: + self.no_cone_ahead = False + + # No need to plan if there is no new cone detected and no cone ahead + if not new_cone_detected and self.no_cone_ahead: + return self.trajectory + + self.visited_cone_ids.add(self.cones[current_cone_idx]['id']) + scenario, flex_wps, fixed_wp, target_heading = waypoint_generate(vehicle_state, self.cones, current_cone_idx, next_cone_idx, self.prev_cone_idx) + self.prev_cone_idx = current_cone_idx + + if flex_wps and fixed_wp is not None: + final_state = { + 'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': target_heading, 'c': 0.0 + } + + # Stitch from current vehicle position to new plan start + if self.trajectory is not None: + print("init and final state: ", init_state, final_state) + # 1. Plan new trajectory from init_state onward + x_new, y_new, psi_new, _, v_new, _, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps) + + # 2. Cut old trajectory up to init_state (e.g., index `stitch_idx`) + old_points = self.trajectory.points[:stitch_idx] + old_x = [p[0] for p in old_points] + old_y = [p[1] for p in old_points] + old_v = [vehicle_state['velocity']] * len(old_x) # or extract from old trajectory if available + + # 3. Combine old + new + x_full = np.concatenate([old_x, x_new]) + y_full = np.concatenate([old_y, y_new]) + v_full = np.concatenate([old_v, v_new]) + + # if current_cone_idx == 6: + # # Plot overall trajectory + # plt.figure() + # plt.plot(x_full, y_full, label='Overall Trajectory') + + # # Plot cones + # for i, cone in enumerate(self.cones): + # plt.scatter(cone['x'], cone['y'], color='orange', s=10, label='Cone' if i == 0 else "") + # plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange') + + # # Plot fixed waypoint + # if fixed_wp is not None: + # plt.plot(fixed_wp[0], fixed_wp[1], 'ro', label='Fixed Waypoint') + # plt.text(fixed_wp[0], fixed_wp[1] + 0.5, 'Fixed', fontsize=9, color='red') + + # plt.title('4-Cone Full Course Trajectory') + # plt.xlabel('X') + # plt.ylabel('Y') + # plt.legend() + # plt.axis('equal') + # plt.grid(True) + # plt.show() + + # 4. Create trajectory + self.trajectory = to_gemstack_trajectory(x_full, y_full, v_full) + else: + x, y, _, _, v, _, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps) + self.trajectory = to_gemstack_trajectory(x, y, v) + + self.travelled_distance = 0.0 + + return self.trajectory + + @staticmethod + def get_future_point_on_trajectory(trajectory, vehicle_position, lookahead_distance=80.0): + """ + Finds a point `lookahead_distance` ahead of the current vehicle position along the trajectory. + """ + traj_points = trajectory.points + current_pos = np.array(vehicle_position) + + # Step 1: Find the closest point on trajectory + dists = [np.linalg.norm(current_pos - np.array([p[0], p[1]])) for p in traj_points] + closest_idx = np.argmin(dists) + + # Step 2: Accumulate distance from closest_idx forward + accumulated = 0.0 + heading = 0 + for i in range(closest_idx + 1, len(traj_points)): + p1 = np.array([traj_points[i - 1][0], traj_points[i - 1][1]]) + p2 = np.array([traj_points[i][0], traj_points[i][1]]) + heading = np.arctan2(p2[1] - p1[1], p2[0] - p1[0]) + segment = np.linalg.norm(p2 - p1) + accumulated += segment + if accumulated >= lookahead_distance: + return i, traj_points[i], heading # Return future point + + p1 = np.array([traj_points[-2][0], traj_points[-2][1]]) + p2 = np.array([traj_points[-1][0], traj_points[-1][1]]) + heading = np.arctan2(p2[1] - p1[1], p2[0] - p1[0]) + # If not enough length, return the last point + return -1, traj_points[-1], heading + + def get_current_cone_idx(self, cones, init_state, forward_dist=50.0, angle_thresh=np.pi): + """ + Get the index of the nearest cone in front of the init_state. + If a 'STANDING' cone is found, previous cones are flipped and returned with the index of the standing cone. + + Args: + cones: List of cones. + init_state: Dict with keys 'x', 'y', 'psi'. + forward_dist: Max distance to search for cones ahead. + angle_thresh: Angle threshold to filter cones roughly in front. + + Returns: + idx: Index of the cone in front (after STANDING logic if needed). + updated_cones: Possibly updated cones with flipped orientations. + """ + pos = np.array([init_state['x'], init_state['y']]) + heading = init_state['psi'] + heading_vec = np.array([np.cos(heading), np.sin(heading)]) + + best_idx = None + next_idx = None # 2nd cone in front of init_state + min_dist = float('inf') + + # Search in the list of cones, which one is nearest ahead of init_state + for i, cone in enumerate(cones): + if cone['id'] in self.visited_cone_ids: + continue # Skip already visited cones + cone_pos = np.array([cone['x'], cone['y']]) + vec_to_cone = cone_pos - pos + dist = np.linalg.norm(vec_to_cone) + if dist > forward_dist: + continue + angle = np.arccos(np.clip(np.dot(heading_vec, vec_to_cone / (dist + 1e-8)), -1.0, 1.0)) + if angle < angle_thresh and dist < min_dist: + next_idx = best_idx + best_idx = i + min_dist = dist + print("getting cone idx.............") + + # If STANDING cone is ahead, flip previous cone directions + if best_idx is not None and cones[best_idx]['orientation'] == 'STANDING': + updated_cones = cones[:best_idx + 1] + [ + self.flip_cone_orientation(c) for c in cones[:best_idx][::-1] + ] + cones[best_idx + 1:] + print("updated cones: ", updated_cones) + return best_idx, next_idx, updated_cones + else: + return best_idx, next_idx, cones + + @staticmethod + def flip_cone_orientation(cone): + """ + Flip cone orientation LEFT↔RIGHT + """ + flipped = cone.copy() + flipped['id'] = cone['id'] + 'flipped' + if cone['orientation'] == 'LEFT': + flipped['orientation'] = 'RIGHT' + elif cone['orientation'] == 'RIGHT': + flipped['orientation'] = 'LEFT' + return flipped + +######################################################################################################################## +######################################################################################################################## + +# ------------ Test Code START -------------- +if __name__ == "__main__": + ########## Testing Block 1 ############## + # Hardcoded slalom testing without simulation + def test_4_cone_slalom(): + vehicle_state = { + 'position': [0.0, 0.0], + 'heading': 0.0, + 'velocity': 5.0 + } + + cones = [ + {'x': 10, 'y': 0.0, 'orientation': 'left'}, + {'x': 30, 'y': 1.0, 'orientation': 'right'}, + {'x': 50, 'y': 0.0, 'orientation': 'left'}, + {'x': 70, 'y': 1.0, 'orientation': 'standing'} + ] + + flex_wps, fixed_wps = waypoint_search_optimization(vehicle_state, cones, search_attempts=1) + + current_pos = np.array(vehicle_state['position']) + + x_all, y_all = [], [] + + for idx, (flex_wp, fixed_wp) in enumerate(zip(flex_wps, fixed_wps)): + init_state = { + 'x': current_pos[0], 'y': current_pos[1], 'psi': vehicle_state['heading'], + 'c': 0.0, 'v': vehicle_state['velocity'] + } + final_state = { + 'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': vehicle_state['heading'], 'c': 0.0 + } + x, y, psi, c, v, eps, final_error = trajectory_generation(init_state, final_state, waypoints=flex_wp) + + print(f"\nSegment {idx + 1}:") + print(f" Waypoint: {flex_wp}") + print(f" Final Errors:") + for k, e in final_error.items(): + print(f" {k}: {e:.4f}") + + x_all += list(x) + y_all += list(y) + + plot_trajectory(x, y, v, c, eps, waypoint=flex_wp) + + current_pos = np.array(fixed_wp) + + # Plot overall trajectory + plt.figure() + plt.plot(x_all, y_all, label='Overall Trajectory') + + # Plot cones + for i, cone in enumerate(cones): + plt.scatter(cone['x'], cone['y'], color='orange', s=80, label='Cone' if i == 0 else "") + plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange') + + plt.title('4-Cone Full Course Trajectory') + plt.xlabel('X') + plt.ylabel('Y') + plt.legend() + plt.axis('equal') + plt.grid(True) + plt.show() + # Hardcoded course testing without simulation + def test_fixed_course(): + # steering angle c is relative to car heading!! + vehicle_state = { + 'position': [-60.0, -39.0], + 'heading': np.pi * 2 / 3, + 'velocity': 0.0 + } + + cones = [ + {'x': -60.083, 'y': -33.118, 'orientation': '90turn'}, + {'x': -6.392, 'y': -5.147, 'orientation': '90turn'}, + {'x': -5.625, 'y': -13.637, 'orientation': '90turn'}, + {'x': -56.258, 'y': -41.080, 'orientation': '90turn'} + ] + + current_pos = np.array(vehicle_state['position']) + current_heading = vehicle_state['heading'] + current_steering = 0.0 + + x_all, y_all = [], [] + + for cone_idx, cone in enumerate(cones): + print("current cone: ", cone) + scenario, flex_wps, fixed_wp = waypoint_generate(vehicle_state, [cone], cone_idx, next_cone_idx=cone_idx+1, prev_cone_idx=cone_idx-1) + if not flex_wps or fixed_wp is None: + continue + + # Setting target_heading in final_state + if cone_idx > 0: + prev_cone_position = np.array((cones[cone_idx - 1]['x'], cones[cone_idx - 1]['y'])) + current_cone_position = np.array((cones[cone_idx]['x'], cones[cone_idx]['y'])) + target_heading = np.arctan2(current_cone_position[1] - prev_cone_position[1], + current_cone_position[0] - prev_cone_position[0]) - np.pi / 2 + np.pi / 18 + + else: # no previous cone + target_heading = current_heading - np.pi / 2 + np.pi / 18 + + # mod to -pi to pi range + target_heading = (target_heading + np.pi) % (2 * np.pi) - np.pi + current_heading = (current_heading + np.pi) % (2 * np.pi) - np.pi + + if abs(target_heading - current_heading) > np.pi: + if target_heading < current_heading: + target_heading += 2 * np.pi + else: + target_heading -= 2 * np.pi + + init_state = { + 'x': current_pos[0], 'y': current_pos[1], 'psi': current_heading, + 'c': current_steering, 'v': vehicle_state['velocity'] + } + + final_state = { + 'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': target_heading, 'c': 0.0 + } + + estimated_N = int(((current_pos[0]-fixed_wp[0]) ** 2 + (current_pos[1]-fixed_wp[1]) ** 2) ** 0.5 / 0.7) + + x, y, psi, c, v, eps, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps, N=estimated_N) + x_all.extend(x) + y_all.extend(y) + + plot_trajectory(x, y, v, c, eps) + + current_pos = np.array([x[-1], y[-1]]) + current_heading = psi[-1] + current_steering = c[-1] + + vehicle_state = { + 'position': current_pos, + 'heading': current_heading, + 'velocity': v[-1] + } + + # Plot overall trajectory + plt.figure() + plt.plot(x_all, y_all, label='Overall Trajectory') + + # Plot cones + for i, cone in enumerate(cones): + plt.scatter(cone['x'], cone['y'], color='orange', s=10, label='Cone' if i == 0 else "") + plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange') + + plt.title('4-Cone Full Course Trajectory') + plt.xlabel('X') + plt.ylabel('Y') + plt.legend() + plt.axis('equal') + plt.grid(True) + plt.show() + ######################################### + + ########## Testing Block 2 ############## + # Trajectory plot + def plot_trajectory(x, y, v, c, eps, waypoint=None): + plt.figure(figsize=(12, 10)) + + plt.subplot(4, 1, 1) + plt.plot(x, y, label="Trajectory") + plt.scatter([x[0], x[-1]], [y[0], y[-1]], color='red', label="Start/End") + + if waypoint is not None: + plt.scatter(*waypoint, color='purple', s=60, marker='X', label="Waypoint") + plt.annotate("Waypoint", (waypoint[0], waypoint[1]), textcoords="offset points", xytext=(5,5), color='purple') + + plt.axis('equal') + plt.ylabel("y (m)") + plt.title("Trajectory") + plt.grid(True) + plt.legend() + + # Speed plot + plt.subplot(4, 1, 2) + plt.plot(v, label="Speed (v)", color="blue") + plt.ylabel("Speed (m/s)") + plt.grid(True) + plt.legend() + + # Curvature plot + plt.subplot(4, 1, 3) + plt.plot(c, label="Curvature (c)", color="orange") + plt.ylabel("Curvature (1/m)") + plt.grid(True) + plt.legend() + + # Curvature rate plot + plt.subplot(4, 1, 4) + plt.plot(eps, label="Curvature Rate (ε)", color="green") + plt.xlabel("Step") + plt.ylabel("ε (1/m²)") + plt.grid(True) + plt.legend() + + plt.tight_layout() + plt.show() + + # --- Trajectory Generation Test case 1. One cone in slalom --- + def trajectory_generation_test1(): + # Init and final + init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0} + final_state = {'x': 15.0, 'y': 0.0, 'psi': np.pi / 20000000, 'c': np.pi / 20000000} + waypoint = [(8.0, 6.0)] + + # Solve + x, y, psi, c, v, eps, final_error = trajectory_generation( + init_state, final_state, waypoints=waypoint + ) + plot_trajectory(x, y, v, c, eps, waypoint) + + # Error + print("\nFinal State Errors:") + for k, e in final_error.items(): + print(f"{k}: {e:.6f}") + + # --- Trajectory Generation Test case 2: 90 degree turn --- + def trajectory_generation_test2(): + # Init and final + init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0} + final_state = {'x': 15.0, 'y': 15.0, 'psi': np.pi / 2, 'c': np.pi / 2} + waypoint = [(13.0, 3.0)] + + # Solve + x, y, psi, c, v, eps, final_error = trajectory_generation( + init_state, final_state, waypoints=waypoint + ) + plot_trajectory(x, y, v, c, eps, waypoint) + + # Error + print("\nFinal State Errors:") + for k, e in final_error.items(): + print(f"{k}: {e:.6f}") + + # Testing feasibility check function + def test_feasibility_check(): + N = 50 + y_traj = np.linspace(0, 10, N) + psi_traj = np.linspace(0, 0.1, N) + c_traj = np.linspace(0, 0.2, N) + trajectory = list(zip(y_traj, psi_traj, c_traj)) + + # Cone map near the path + cone_map = [(5.0, 1.0), (10.0, 1.5), (15.0, 2.0), (25.0, 4.0), (25.0, 10.0), (16.0, 9.0), (40.0, 5.0)] + + # Run check + feasible, collisions, x_vals, y_vals = feasibility_check(trajectory, cone_map) + + # Plot + plt.figure(figsize=(10, 6)) + plt.plot(x_vals, y_vals, label="Trajectory", linewidth=2) + for i, (cx, cy) in enumerate(cone_map): + color = 'red' if i in collisions else 'green' + plt.scatter(cx, cy, color=color, s=100, label=f'Cone {i}' if i == 0 else "") + plt.xlabel("x (m)") + plt.ylabel("y (m)") + plt.title("Trajectory and Cone Map") + plt.legend() + plt.axis("equal") + plt.grid(True) + plt.show() + + print("Feasible:", feasible) + print("Collisions with cones:", collisions) + ######################################### + + ########## Testing Block 3 ############## + # Waypoint generation plotting + def plot_results(vehicle_state, cones, wpt_flexes=None, wpt_fixed=None, scenario_label=""): + plt.figure(figsize=(10, 5)) + ax = plt.gca() + ax.set_aspect('equal') + + all_x = [cone['x'] for cone in cones] + [vehicle_state['position'][0]] + all_y = [cone['y'] for cone in cones] + [vehicle_state['position'][1]] + + if wpt_flexes is not None: + all_x += [pt[0] for pt in wpt_flexes] + all_y += [pt[1] for pt in wpt_flexes] + + if wpt_fixed is not None: + all_x.append(wpt_fixed[0]) + all_y.append(wpt_fixed[1]) + + # Compute axis limits with padding + padding = 2 + x_min, x_max = min(all_x) - padding, max(all_x) + padding + y_min, y_max = min(all_y) - padding, max(all_y) + padding + plt.xlim(x_min, x_max) + plt.ylim(y_min, y_max) + + # Plot cones + for cone in cones: + plt.scatter(cone['x'], cone['y'], c='orange', label='Cone' if cone == cones[0] else "") + plt.text(cone['x'], cone['y'] + 0.5, cone['orientation'][0].upper(), fontsize=10, ha='center') + + # Plot vehicle + vx, vy = vehicle_state['position'] + plt.plot(vx, vy, 'bo', label='Vehicle Start') + plt.arrow(vx, vy, np.cos(vehicle_state['heading']) * 2, np.sin(vehicle_state['heading']) * 2, + head_width=0.25, color='blue') + + # Plot flexible waypoint + if wpt_flexes is not None: + for wpt_flex in wpt_flexes: + plt.plot(wpt_flex[0], wpt_flex[1], 'go', label='Flexible Waypoint') + plt.text(wpt_flex[0], wpt_flex[1] + 0.5, 'Flex', fontsize=9, color='green') + + # Plot fixed waypoint + if wpt_fixed is not None: + plt.plot(wpt_fixed[0], wpt_fixed[1], 'ro', label='Fixed Waypoint') + plt.text(wpt_fixed[0], wpt_fixed[1] + 0.5, 'Fixed', fontsize=9, color='red') + + plt.title(scenario_label) + plt.xlabel("X") + plt.ylabel("Y") + plt.grid(True) + plt.legend() + plt.show() + return + # Vehicle State Test Update + def drive(vehicle_state): + # Update vehicle state + vehicle_state['position'][0] += vehicle_state['velocity'] * np.cos(vehicle_state['heading']) + vehicle_state['position'][1] += vehicle_state['velocity'] * np.sin(vehicle_state['heading']) + return vehicle_state + # Generate Fake Cones + def generate_test_cones(case='slalom'): + if case == 'slalom': + cones = [] + for i in range(3): + x = (i+1) * 10 + y = 0 if i % 2 == 0 else 1 + orientation = 'left' if i % 2 == 0 else 'right' + cones.append({'x': x, 'y': y, 'orientation': orientation}) + return cones + + elif case == 'u_turn': + return [{'x': 10, 'y': 0, 'orientation': 'standing'}] + # Generate Fake Vehicle + def get_test_vehicle_state(vehicle_state=None): + if vehicle_state is not None: + return vehicle_state + return { + 'position': [0, 0], + 'heading': 0.0 * 180/np.pi, # Facing right + 'velocity': 10.0 + } + # Testing Scenario + Waypoints + def test_waypoint_generation(case='slalom', test_loop=1): + vehicle_state = get_test_vehicle_state() + cones = generate_test_cones(case) + + for i in range(test_loop): + scenario = scenario_check(vehicle_state, cones) + scenario_label = f"Scenario: {scenario}" + scenario, wpt_flexes, wpt_fixed = waypoint_generate(vehicle_state, cones, cone_idx=i, next_cone_idx=i+1, prev_cone_idx=i-1) + plot_results(vehicle_state, cones, wpt_flexes, wpt_fixed, scenario_label) + vehicle_state = drive(vehicle_state) + if case == 'slalom': + cones.pop(0) + # Comprehensive Testing + def test_planning(case='slalom', test_loop=2): + vehicle_state = get_test_vehicle_state() + cones = generate_test_cones(case) + + for i in range(test_loop): + # Way Points + scenario = scenario_check(vehicle_state, cones) + scenario_label = f"Scenario: {scenario}" + scenario, wpt_flexes, wpt_fixed = waypoint_generate(vehicle_state, cones, cone_idx=i, next_cone_idx=i+1, prev_cone_idx=i-1) + plot_results(vehicle_state, cones, wpt_flexes, wpt_fixed, scenario_label) + # Trajectory + init_state = {'y': wpt_flexes[0][1], 'psi': 0.0, 'c': 0.0} + final_state = {'y': wpt_fixed[1], 'psi': 0.0, 'c': 0.0} + + y_traj, psi_traj, c_traj, eps_traj = trajectory_generation(init_state, final_state) + # plot_trajectory(y_traj, psi_traj, c_traj, label="Generated trajectory") + # plot_dynamics(psi_traj, c_traj, eps_traj) + + # Iterate + vehicle_state = drive(vehicle_state) + if case == 'slalom': + cones.pop(0) + ######################################### +# ------------ Test Code END -------------- \ No newline at end of file diff --git a/GEMstack/onboard/planning/stanley.py b/GEMstack/onboard/planning/stanley.py index 2bc8579ab..986084746 100644 --- a/GEMstack/onboard/planning/stanley.py +++ b/GEMstack/onboard/planning/stanley.py @@ -1,476 +1,561 @@ -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.""" +import numpy as np +import rospy +from math import sin, cos, atan2 + +# These imports match your existing project structure +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, compute_headings +from ..interface.gem import GEMVehicleCommand +from ..component import Component +from .launch_control import LaunchControl + +##################################### +# 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): + """ + A Stanley controller that handles lateral control (steering) + plus a basic longitudinal control (PID + optional feedforward). + It has been modified to reduce oscillations by: + 1) Lower gains + 2) Steering damping + 3) Low-pass filter on steering + 4) Gentler speed logic when cornering + """ + + def __init__( + self, + control_gain=None, + softening_gain=None, + desired_speed=None, + launch_control=None + ): + """ + :param control_gain: Stanley lateral control gain k (lowered from the default to reduce overshoot). + :param softening_gain: Softening gain k_soft. + :param yaw_rate_gain: Yaw-rate gain k_yaw_rate (helps reduce oscillations). + :param steering_damp_gain: Steering damping gain k_damp_steer. + :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 for longitudinal speed tracking + 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) + + # Speed source: numeric or derived from path/trajectory + 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 + + if (self.desired_speed_source == 'racing'): + self.k = settings.get('control.racing.stanley.control_gain') + self.k_soft = settings.get('control.racing.stanley.softening_gain') + p = settings.get('control.racing.longitudinal_control.pid_p') + d = settings.get('control.racing.longitudinal_control.pid_d') + i = settings.get('control.racing.longitudinal_control.pid_i') + + # 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 + + self.launch_control = launch_control + + 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 set_racing_path(self, path: Path): + if path == self.path_arg: + return + self.path_arg = path + if len(path.points[0]) > 2: + path = path.get_dims([0,1]) + if not isinstance(path, Trajectory): + # path = Path(ObjectFrameEnum.START,path) + path = compute_headings(path, True) + self.path = path.arc_length_parameterize() + self.trajectory = self.path.racing_velocity_profile() + self.current_traj_parameter = 0.0 + if self.desired_speed_source not in ['racing']: + raise ValueError("Racing: desired speed must be set to racing, currently set to: " + str(self.desired_speed_source)) + 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 + + # Current vehicle states + 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 - 10.0 + search_end = self.current_path_parameter + 10.0 + closest_dist, closest_parameter = self.path.closest_point_local((fx, fy), [search_start, search_end]) + self.current_path_parameter = closest_parameter + + # 2) Path heading + # 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 + desired_x = target_x + desired_y = 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) + + if (self.desired_speed): + desired_speed = abs(self.desired_speed) + else: + desired_speed = None + 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', 'racing']: + 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): + """ + A trajectory-tracking Component that uses the above Stanley controller + for lateral control plus PID-based longitudinal control. + It now includes measures to mitigate oscillations. + """ + + def __init__(self, vehicle_interface=None, **kwargs): + """ + :param vehicle_interface: The low-level interface to send commands to the vehicle. + :param kwargs: Optional parameters to pass into the Stanley(...) constructor. + """ + self.stanley = Stanley(**kwargs) + self.vehicle_interface = vehicle_interface + self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path') + + launch_control_enabled = self.stanley.launch_control + if launch_control_enabled: + stage_duration = settings.get('control.racing.launch_control.stage_duration', 0.5) + self.launch_control = LaunchControl(stage_duration, stop_threshold=0.1) + else: + self.launch_control = None + + def rate(self): + """Control frequency in Hz.""" + return 50.0 + + def state_inputs(self): + """ + Required state inputs: + - Vehicle state + - Trajectory + """ + return ["vehicle", "trajectory"] + + def state_outputs(self): + """No direct output state here.""" + return [] + + def update(self, vehicle: VehicleState, trajectory: Trajectory): + """ + Per control cycle: + 1) Set the path/trajectory + 2) Compute (acceleration, front wheel angle) + 3) Convert front wheel angle to steering wheel angle (if necessary) + 4) Send command to the vehicle + """ + # path to trajectory if racing enabled and using route instead of planner + if self.desired_speed_source in ['racing'] and not isinstance(trajectory, Trajectory): ## conditional needed for no racing + self.stanley.set_racing_path(trajectory) + else: + self.stanley.set_path(trajectory) + + accel, f_delta = self.stanley.compute(vehicle, self) + + # If your low-level interface expects steering wheel angle: + 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) + ) + + cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle) + if self.launch_control: + cmd = self.launch_control.apply_launch_control(cmd, vehicle.v) + + 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/GEMstack/onboard/planning/velocity_profile.py b/GEMstack/onboard/planning/velocity_profile.py new file mode 100644 index 000000000..f2ec97a62 --- /dev/null +++ b/GEMstack/onboard/planning/velocity_profile.py @@ -0,0 +1,345 @@ +import numpy as np +import sys +import os +import matplotlib.pyplot as plt +from matplotlib.collections import LineCollection +from scipy.interpolate import splprep, splev +from ...knowledge.vehicle.geometry import steer2front + +from ...knowledge.vehicle import dynamics +from ...knowledge.vehicle import geometry +from ...utils import settings +import pandas as pd + +def parse_route_csv(filename): + """ + Parses the route file and extracts: + - X position desired (from column index 0) + - Y position desired (from column index 1) + """ + + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + x_desired = data[:, 0] + y_desired = data[:, 1] + return x_desired, y_desired + +def compute_spline_curvature(x, y, s=0.0, num=1000): + """ + Computes curvature from 2D path (x, y) using parametric splines. + + Parameters: + x, y (np.array): path coordinates + s (float): Smoothing factor for spline (0 = interpolating spline) + num (int): Number of points to evaluate spline and curvature at + + Returns: + kappa (float): curvature profile along the trajectory + """ + # Fit parametric spline + nums = len(x) + tck, _ = splprep([x, y], s=s) + u_fine = np.linspace(0, 1, nums) + + # Evaluate spline and its first and second derivatives + dx, dy = splev(u_fine, tck, der=1) + ddx, ddy = splev(u_fine, tck, der=2) + + # Compute curvature: κ = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2) + denom = (dx**2 + dy**2)**1.5 + 1e-8 # Avoid divide by zero + kappa = (dx * ddy - dy * ddx) / denom + + return kappa + +def lateral_speed_limit(kappa, ay_max, v_max): + """ + Limits the velocity based on curvature and lateral acceleration limits. + + Parameters: + kappa (np.ndarray): curvature profile along the trajectory + ay_max (float): maximum lateral acceleration + v_max (float): maximum velocity + + Returns: + np.minimum(v_lat, v_max): velocity profile limited by lateral acceleration + """ + v_lat = np.sqrt(np.maximum(ay_max / (np.abs(kappa) + 1e-8), 0)) + return np.minimum(v_lat, v_max) + +def limit_ax_for_friction_ellipse(v, kappa, ax_limit, ay_max): + """ + Longitudinal acceleration limit based on current lateral acceleration. + + Parameters: + v (float): velocity + kappa (float): curvature + ax_limit (float): maximum longitudinal acceleration + ay_limit (float): maximum lateral acceleration + + Returns: + ax_limit * ax_ratio: maximum allowed longitudinal acceleration + """ + # Compute a_y at current v + ay = v**2 * kappa + ay_ratio = ay / ay_max + ay_ratio = np.clip(ay_ratio, -1.0, 1.0) + # Remaining fraction for ax + ax_ratio = np.sqrt(np.maximum(1.0 - ay_ratio**2, 0.0)) + return ax_limit * ax_ratio + +def apply_trail_braking(ds, v_lat, kappa, ax_max, ax_min, ay_max): + """ + Limits the velocity based on lateral and longitudinal acceleration limits. + + Parameters: + ds (np.ndarray): distance diff along the trajectory. + v_lat (np.ndarray): velocity profile along the trajectory. + kappa (float): curvature profile along the trajectory + ax_max (float): maximum longitudinal acceleration + ax_min (float): minimum longitudinal acceleration (braking) + ay_max (float): maximum lateral acceleration + + Returns: + v_profile (np.ndarray): acceleration limited velocity profile. + forward_vs (np.ndarray): positive acceleration limited velocity profile + forward_vs, (np.ndarray): negative acceleration limited velocity profile + + """ + N = len(ds) + v_profile = v_lat.copy() + v_profile[0] = 0.0 + forward_vs = v_lat.copy() + backward_vs = v_lat.copy() + + # Forward (accelerating out of curves) + for i in range(1, N): + ax_limit = limit_ax_for_friction_ellipse(v_profile[i-1], kappa[i-1], ax_max, ay_max) + ax_limit = np.clip(ax_limit, ax_min, ax_max) + v_allowed = np.sqrt(v_profile[i-1]**2 + 2 * ax_limit * ds[i-1]) + v_profile[i] = min(v_profile[i], v_allowed) + forward_vs[i] = v_allowed + + # Backward (trail braking into corners) + for i in range(N - 1, 0, -1): + ax_limit = limit_ax_for_friction_ellipse(v_profile[i], kappa[i], abs(ax_min), ay_max) + arg = v_profile[i]**2 + 2 * ax_limit * ds[i] + v_allowed = np.sqrt(max(arg,0.0)) + v_profile[i-1] = min(v_profile[i-1], v_allowed) + + backward_vs[i-1] = v_allowed + + return v_profile, forward_vs, backward_vs + +def limit_velocity_by_steering_rate(ds, velocity, wheelbase, max_steering_rate, kappa): + """ + Limits the velocity based on steering angle rate of change. + + Parameters: + ds (np.ndarray): distance diff along the trajectory. + velocity (np.ndarray): velocity profile along the trajectory. + wheelbase (float): vehicle's wheelbase (L). + max_steering_rate (float): maximum allowable steering angle rate (rad/s). + kappa (float): curvature profile along the trajectory + + Returns: + limited_velocity (np.ndarray): steering rate limited velocity profile. + """ + # Compute steering angle delta: delta = arctan(L * kappa) + steering_angle = np.arctan(wheelbase * kappa) + + # Compute steering rate: d(delta)/dt + ddelta = np.gradient(steering_angle) + dt = ds / (velocity + 1e-6) # Add small value to prevent division by zero + ddelta_dt = ddelta / (dt + 1e-6) + + # max steering wheel rate to steering rate + max_steering_rate = steer2front(max_steering_rate) + + # Limit velocity where steering rate exceeds max + limited_velocity = np.copy(velocity) + for i in range(len(velocity)): + if abs(ddelta_dt[i]) > max_steering_rate: + ddelta_dt[i] = max_steering_rate + limited_velocity[i] = max(0, min( + velocity[i], + abs((ds[i] / (ddelta[i] / ddelta_dt[i])+ 1e-6) - 1e-6) + )) + return limited_velocity + +def compute_time_profile(ds, v_profile): + """ + Compute updated trajectory times based on a path and velocity profile + + Parameters: + ds (np.ndarray): distance diff along the trajectory. + v_profile (np.ndarray): velocity profile along the trajectory. + + Returns: + t (np.ndarray): time profile for trajectory. + """ + + v_profile = np.array(v_profile) + + # Use average speed between points to estimate time between them + v_mid = (v_profile[:-1] + v_profile[1:]) / 2 + v_mid = np.clip(v_mid, 0.1, None) # Avoid divide-by-zero + + dt = ds[:-1] / v_mid + t = np.insert(np.cumsum(dt), 0, 0.0) # cumulative time starting from 0 + + return t + +def plot_speed_profile_gradient(fig, axis, xs, ys, velocity, cmap='jet'): + """ + Plotting color gradient velocity profile on the path + + Parameters: + fig, axis: figure and axis on which to plot + xs, ys (np.ndarray): path coordinates + velocity (np.ndarray): velocity profile + cmap: color map + """ + xs = np.array(xs) + ys = np.array(ys) + velocity = np.array(velocity) + + # Build segments between points + points = np.array([xs, ys]).T.reshape(-1, 1, 2) + segments = np.concatenate([points[:-1], points[1:]], axis=1) + + # Create line collection + lc = LineCollection(segments, cmap=cmap, linewidth=2) + lc.set_array(velocity[:-1]) # color values + lc.set_linewidth(2) + + # fig, ax = plt.subplots(figsize=(8, 6)) + line = axis.add_collection(lc) + fig.colorbar(line, ax=axis, label='Speed (m/s)') + axis.set_xlim(xs.min(), xs.max()) + axis.set_ylim(ys.min(), ys.max()) + axis.set_aspect('equal', adjustable='box') + axis.set_title('Speed Profile Along Path') + axis.set_xlabel('X [m]') + axis.set_ylabel('Y [m]') + plt.grid(True) + +def plot_x_y(axis, x, y, x_label, y_label): + """ + Plotting x vs y variables + + Parameters: + axis: axis on which to plot + x, y (np.ndarray): variables to plot + x_label, y_label (str): axis labels + """ + axis.plot(x, y) + axis.set_xlabel(x_label) + axis.set_ylabel(y_label) + axis.grid(True) + +def compute_velocity_profile(points, plot=False): + """ + Compute a vehicle dynamics limited velocity profile for a set path + + Parameters: + points: path x,y coordinates + plot (bool): visualization plots + + Returns: + t (np.ndarray): time profile for trajectory. + v_profile (np.ndarray): velocity profile for trajectory. + """ + # Vehicle dynamics limits + v_max = settings.get('vehicle.limits.max_speed') + ax_max = settings.get('vehicle.limits.max_longitudinal_acceleration') + ax_min = settings.get('vehicle.limits.min_longitudinal_acceleration') + ay_max = settings.get('vehicle.limits.max_lateral_acceleration') + max_steering_rate = settings.get('vehicle.limits.max_steering_rate') + wheelbase = settings.get('vehicle.geometry.wheelbase') + + points = np.array(points) + xs, ys = points[:,0], points [:,1] + dx = np.diff(xs) + dy = np.diff(ys) + ds = np.hypot(dx, dy) + ds = np.append(ds, ds[-1]) + + # curvature profile of the path + kappa = compute_spline_curvature(xs, ys) + + # max speeds from lateral acceleration limits + v_lat = lateral_speed_limit(kappa, ay_max, v_max) + + # max speeds from steering limits + v_steer = limit_velocity_by_steering_rate(ds, v_lat, wheelbase, max_steering_rate, kappa) + + #max speeds from longitudinal acceleration limits + v_profile, forward_vs, backward_vs = apply_trail_braking(ds, v_steer, kappa, ax_max, ax_min, ay_max) + + t = compute_time_profile(ds, v_profile) + + if plot is False: + return t, v_profile + else: + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + + dv = np.diff(v_profile) + dt = np.diff(t) + + a = dv/dt + + plot_x_y(axs[0, 0], t, v_profile, "t", "v") + plot_x_y(axs[1, 0], t[:-1], a, "t", "a") + # plot_x_y(axs[1, 1], t, forward_vs, "t", "forward & backward vs") + # plot_x_y(axs[1, 1], t, backward_vs, "t", "forward & backward vs") + plot_x_y(axs[0, 1], t, kappa, "t", "kappa") + + plot_speed_profile_gradient(fig, axs[1, 1], xs, ys, v_profile) + + # Save to CSV + df = pd.DataFrame({'x': xs, 'y': ys, 'v': v_profile, 't': t}) + df.to_csv('v_profile.csv', index=False) + + + plt.tight_layout() + plt.show() + + return t, v_profile + + +if __name__=='__main__': + ### offline velocity profile plotting from a route + if len(sys.argv) != 2: + print("Usage: python velocity_profile.py ") + sys.exit(1) + + path_file = sys.argv[1] + + # if behavior.json doesn't exist, print error and exit + if not os.path.exists(path_file): + print("Error: route file not found.") + sys.exit(1) + + xs, ys = parse_route_csv(path_file) + + points = list(zip(xs,ys)) + + t, v_profile = compute_velocity_profile(points, plot=True) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + + dv = np.diff(v_profile) + dt = np.diff(t) + + a = dv/dt + + plot_x_y(axs[0, 0], t, v_profile, "t", "v") + # plot_x_y(axs[0, 1], t[:-1], a, "t", "a") + # plot_x_y(axs[1, 0], xs, ys, "x", "y") + # plot_x_y(axs[1, 1], t, forward_vs, "t", "forward & backward vs") + # plot_x_y(axs[1, 1], t, backward_vs, "t", "forward & backward vs") + # plot_x_y(axs[1, 1], t, xs, "t", "x & y") + # plot_x_y(axs[1, 1], t, ys, "t", "x & y") + + plot_speed_profile_gradient(fig, axs[1, 0], xs, ys, v_profile) + plt.tight_layout() + plt.show() \ No newline at end of file diff --git a/GEMstack/state/trajectory.py b/GEMstack/state/trajectory.py index af0f70122..b8d1f4390 100644 --- a/GEMstack/state/trajectory.py +++ b/GEMstack/state/trajectory.py @@ -4,7 +4,11 @@ from ..mathutils import transforms,collisions from .physical_object import ObjectFrameEnum, convert_point import math +import numpy as np from typing import List,Tuple,Optional,Union +from ..onboard.planning.velocity_profile import compute_velocity_profile +from scipy.interpolate import splprep, splrep, splev + @dataclass @register @@ -79,6 +83,13 @@ def arc_length_parameterize(self, speed = 1.0) -> Trajectory: times.append(times[-1] + d/speed) return Trajectory(frame=self.frame,points=points,times=times) + def racing_velocity_profile(self) -> Trajectory: + """Returns a timed trajectory with max velocity profile parametrized by path radius""" + times = [0.0] + points = self.points + times, velocities = compute_velocity_profile(points) + return Trajectory(frame=self.frame,points=points,times=times) + def closest_point(self, x : List[float], edges = True) -> Tuple[float,float]: """Returns the closest point on the path to the given point. If edges=False, only computes the distances to the vertices, not the @@ -162,6 +173,99 @@ def trim(self, start : float, end : float) -> Path: s = self.eval(start) e = self.eval(end) return replace(self,points=[s]+self.points[sind+1:eind]+[e]) + + + def fit_curve_radius(self, vehicle_position: List[float], n_points: int) -> float: + """ + Calculates the curve radius over the next 3 intervals and returns the largest. + + Args: + vehicle_position: Current position of the vehicle [x, y, ...] + n_points: Number of points to sample ahead. Must be >= 5 to allow 3 intervals. + + Returns: + float: Min radius among 3 consecutive 3-point intervals. Returns float('inf') if not enough data. + """ + _, closest_point_idx_float = self.closest_point(vehicle_position) + closest_point_idx = int(math.ceil(closest_point_idx_float)) + + # Require at least 5 points to form 3 overlapping triplets + total_needed = max(n_points, 5) + end_idx = min(closest_point_idx + total_needed, len(self.points)) + + if end_idx - closest_point_idx < 5: + return float('0') + + # Get the next chunk of points + points_to_check = self.points[closest_point_idx:end_idx] + + # Convert to 2D + points_2d = [[p[0], p[1]] for p in points_to_check] + + radii = [] + for i in range(len(points_2d) - 2): # Slide a window of 3 + triplet = points_2d[i:i+3] + try: + _, _, r = self._fit_circle_to_points(triplet) + radii.append(r) + except: + radii.append(float('inf')) # Treat failed fits as straight line + + return min(radii) if radii else float('0') + + def _fit_circle_to_points(self, points: List[List[float]]) -> Tuple[float, float, float]: + """ + Approximates a circle from a set of 2D points using the circumcircle of the first three points. + + Args: + points: List of [x, y] coordinates. Must contain at least 3 points. + + Returns: + Tuple[float, float, float]: Center coordinates (h, k) and radius r + """ + if len(points) < 3: + raise ValueError("At least 3 points are required to fit a circle") + + p1, p2, p3 = points[0], points[1], points[2] + + # Build matrices for determinant calculation + def det(matrix): + return np.linalg.det(np.array(matrix)) + + A = [ + [p1[0], p1[1], 1], + [p2[0], p2[1], 1], + [p3[0], p3[1], 1] + ] + D = det(A) + if abs(D) < 1e-10: + raise ValueError("Points are collinear or nearly collinear") + + a = p1[0]**2 + p1[1]**2 + b = p2[0]**2 + p2[1]**2 + c = p3[0]**2 + p3[1]**2 + + M11 = [ + [a, p1[1], 1], + [b, p2[1], 1], + [c, p3[1], 1] + ] + M12 = [ + [a, p1[0], 1], + [b, p2[0], 1], + [c, p3[0], 1] + ] + M13 = [ + [a, p1[0], p1[1]], + [b, p2[0], p2[1]], + [c, p3[0], p3[1]] + ] + + h = 0.5 * det(M11) / D + k = -0.5 * det(M12) / D + r = math.sqrt(h**2 + k**2 + det(M13) / D) + + return h, k, r @dataclass @@ -262,8 +366,13 @@ def closest_point_local(self, x : List[float], time_range=Tuple[float,float], ed """ param_range = [self.time_to_parameter(time_range[0]),self.time_to_parameter(time_range[1])] #print("Searching within time range",time_range,"= param range",param_range) + if len(self.points[0]) > 2: + heading_points = self.points + self.points = [(x, y) for x, y, z in self.points] distance, closest_index = Path.closest_point_local(self,x,param_range,edges) closest_time = self.parameter_to_time(closest_index) + if len(self.points[0]) > 2: + self.points = heading_points return distance, closest_time def trim(self, start : float, end : float) -> Trajectory: @@ -274,9 +383,6 @@ def trim(self, start : float, end : float) -> Trajectory: e = self.eval(end) return replace(self,points=[s]+self.points[sind+1:eind]+[e],times=[start]+self.times[sind+1:eind]+[end]) - - - def compute_headings(path : Path, smoothed = False) -> Path: """Converts a 2D (x,y) path into a 3D path (x,y,heading) or a 3D (x,y,z) path into a 5D path (x,y,z,heading,pitch). @@ -285,7 +391,25 @@ def compute_headings(path : Path, smoothed = False) -> Path: estimate good tangent vectors. """ if smoothed: - raise NotImplementedError("Smoothing not done yet") + # raise NotImplementedError("Smoothing not done yet") + points = np.array(path.points) + x = points[:, 0] + y = points[:, 1] + + dx = np.diff(x) + dy = np.diff(y) + ds = np.hypot(dx, dy) + s = np.insert(np.cumsum(ds), 0, 0) + + tck_x = splrep(s, x, s=0.5) + tck_y = splrep(s, y, s=0.5) + + s_fine = np.linspace(0, s[-1], 200) + x_smooth = splev(s_fine, tck_x) + y_smooth = splev(s_fine, tck_y) + + path.points = np.vstack((x_smooth, y_smooth)).T + if len(path.points) < 2: raise ValueError("Path must have at least 2 points") derivs = [] diff --git a/GEMstack/utils/generate_circle.py b/GEMstack/utils/generate_circle.py new file mode 100644 index 000000000..bc09cf195 --- /dev/null +++ b/GEMstack/utils/generate_circle.py @@ -0,0 +1,19 @@ +import numpy as np +import pandas as pd + +# Parameters +radius = 20 # radius of the circle in meters +num_points = 100 # number of points around the circle + +# Generate angles from 0 to 2π +angles = np.linspace(0, 2 * np.pi, num_points, endpoint=False) + +# Compute x and y coordinates +x = radius * np.cos(angles) +y = radius * np.sin(angles) + +# Create a DataFrame +circle_df = pd.DataFrame({'x': x, 'y': y}) + +# Save to CSV +circle_df.to_csv('circle_points.csv', index=False) \ No newline at end of file diff --git a/GEMstack/utils/generate_figure8.py b/GEMstack/utils/generate_figure8.py new file mode 100644 index 000000000..d93d778f5 --- /dev/null +++ b/GEMstack/utils/generate_figure8.py @@ -0,0 +1,28 @@ +import numpy as np +import pandas as pd + +# Parameters +radius_left = 10 # Radius scaling on the left side +radius_right = 20 # Radius scaling on the right side +num_points = 200 # Total number of points +rotation_angle_deg = -45 # Rotation angle in degrees + +# Convert degrees to radians +theta = np.radians(rotation_angle_deg) + +# Generate parametric figure-8 (lemniscate-style) +t = np.linspace(-np.pi/2, 3*np.pi/2, num_points) +x = np.sin(t) +y = np.sin(t) * np.cos(t) + +# Apply asymmetric scaling +x_scaled = np.where(x < 0, x * radius_left, x * radius_right) +y_scaled = np.where(x < 0, y * radius_left, y * radius_right) + +# Apply rotation +x_rotated = x_scaled * np.cos(theta) - y_scaled * np.sin(theta) +y_rotated = x_scaled * np.sin(theta) + y_scaled * np.cos(theta) + +# Create DataFrame and save +figure8_df = pd.DataFrame({'x': x_rotated, 'y': y_rotated}) +figure8_df.to_csv('../GEMstack/GEMstack/knowledge/routes/figure8.csv', index=False) \ No newline at end of file diff --git a/launch/dash_fixed_route.yaml b/launch/dash_fixed_route.yaml new file mode 100644 index 000000000..09bf44c48 --- /dev/null +++ b/launch/dash_fixed_route.yaml @@ -0,0 +1,81 @@ +description: "Drive the GEM vehicle along a fixed route (currently xyhead_highbay_backlot_p.csv)" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/forward_50m.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 + args: {desired_speed: 2.5} #approximately 5mph + print: False +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','trajectory_tracking'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 + # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False + #auto_plot : True +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_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/launch/fixed_route.yaml b/launch/fixed_route.yaml index 17abf89ee..732e82adf 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -16,6 +16,7 @@ drive: planning: route_planning: type: StaticRoutePlanner + # args: [!relative_path '../GEMstack/knowledge/routes/race_track_7.csv'] # 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'] @@ -33,7 +34,7 @@ drive: # type: pure_pursuit.PurePursuitTrajectoryTracker type: stanley.StanleyTrajectoryTracker # type: mpc.MPCTrajectoryTracker - args: {desired_speed: 2.5} #approximately 5mph + args: {desired_speed: 2.5} #approximately 5mph {launch_control: 1} print: False log: # Specify the top-level folder to save the log files. Default is 'logs' @@ -105,4 +106,4 @@ variants: # visualization: !include "mpl_visualization.yaml" log_ros: log: - ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/launch/racing_slalom.yaml b/launch/racing_slalom.yaml new file mode 100644 index 000000000..7ec27602c --- /dev/null +++ b/launch/racing_slalom.yaml @@ -0,0 +1,91 @@ +description: "Use the sensors to detect cones and drive a slalom route around them." +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking: + type: recovery.StopTrajectoryTracker + print: False +# Driving behavior for the GEM vehicle following a fixed route +drive: + perception: + state_estimation : GNSSStateEstimator + obstacle_detection : + type: cone_detection.ConeDetector3D + args: + camera_name: front #[front, front_right] + camera_calib_file: ./GEMstack/knowledge/calibration/cameras.yaml + enable_tracking: True # True if you want to enable tracking + visualize_2d: False # True to see 2D detection visualization + use_cyl_roi: False # True to use a cylinder ROI + save_data: False # True to save sensor input data + orientation: False # True to detect flipped cones + use_start_frame: True # True to output in START frame + + perception_normalization : StandardPerceptionNormalizer + + planning: + motion_planning: + type: racing_planning.SlalomTrajectoryPlanner + trajectory_tracking: + #type: pure_pursuit.PurePursuitTrajectoryTracker # to use pure pursuit comment below + type: stanley.StanleyTrajectoryTracker # to use stanley comment above + print: True + +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : [] + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','trajectory_tracking','obstacle_detection'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 + # If True, then make plots of the recorded data specifically from PurePursuitTrajectoryTracker_debug.csv and report metrics. Default False + #auto_plot : True +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + # For replaying sensor data, try !include "../knowledge/defaults/standard_sensor_ros_topics.yaml" + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +after: + show_log_folder: True #set to false to avoid showing the log folder + +#on load, variants will overload the settings structure +variants: + #sim variant doesn't execute on the real vehicle + #real variant executes on the real robot + sim: + run: + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/racing_sim_slalom.yaml' + + drive: + perception: + state_estimation : OmniscientStateEstimator + agent_detection : OmniscientAgentDetector + visualization: !include "klampt_visualization.yaml" + #visualization: !include "mpl_visualization.yaml" + log_ros: + log: + ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" diff --git a/testing/test_gg_diagram.py b/testing/test_gg_diagram.py new file mode 100644 index 000000000..160286d3d --- /dev/null +++ b/testing/test_gg_diagram.py @@ -0,0 +1,309 @@ +import numpy as np +import matplotlib.pyplot as plt + +import sys +import os +sys.path.append(os.getcwd()) + +import json +import matplotlib.pyplot as plt +import numpy as np +# from GEMstack.mathutils.transforms import lat_lon_to_xy + +from pyproj import Proj, Transformer +from scipy.spatial import ConvexHull + +def latlon_to_xy(latitudes, longitudes, origin_lat, origin_lon): + """ + Converts lat/lon to local X/Y coordinates in meters using a local ENU projection. + + latitudes, longitudes: arrays of GPS coordinates + origin_lat, origin_lon: reference point (usually the first point) + + Returns: xs, ys (in meters, relative to origin) + """ + # WGS84 to ENU local projection (east-north-up) + transformer = Transformer.from_crs( + "epsg:4326", # WGS84 lat/lon + f"+proj=tmerc +lat_0={origin_lat} +lon_0={origin_lon} +k=1 +x_0=0 +y_0=0 +ellps=WGS84", + always_xy=True + ) + + xs, ys = transformer.transform(longitudes, latitudes) + return xs, ys + + +def parse_behavior_log(filename): + """ + Parses the behavior log file and extracts the following data: + - vehicle time + - vehicle acceleration + - vehicle heading rate + - speed + """ + times = [] + accelerations = [] + heading_rates = [] + speeds = [] + xs = [] + ys = [] + ref_lat = 0.0 + ref_long = 0.0 + + with open(filename, 'r') as f: + for idx, line in enumerate(f): + try: + entry = json.loads(line) + except json.JSONDecodeError: + print(f"Skipping invalid JSON line: {line.strip()}") + continue + # Process vehicle state data + 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") + speed = vehicle_data.get("v") + # print(speed) + # pose_data = vehicle_data["pose"].get("pose") + frame = vehicle_data["pose"].get("frame") + x = vehicle_data["pose"].get("x") + y = vehicle_data["pose"].get("y") + if idx == 0: + ref_lat = y + ref_long = x + x, y = latlon_to_xy(y,x,ref_lat,ref_long) + # Only add if all fields are available + if None not in (t, acceleration, heading_rate, speed) and frame != 3: + times.append(t) + accelerations.append(acceleration) + heading_rates.append(heading_rate) + speeds.append(speed) + + xs.append(x) + + ys.append(y) + # print(len(speeds)) + # print(len(times)) + return np.array(times), np.array(xs), np.array(ys), np.array(speeds), np.array(accelerations), np.array(heading_rates) + +def parse_tracker_csv(filename): + """ + Parses the pure pursuit tracker log file and extracts the following data: + - vehicle time (from column index 19) + - X position actual (from column index 2) + - Y position actual (from column index 5) + - X position desired (from column index 11) + - Y position desired (from column index 14) + """ + + data = np.genfromtxt(filename, delimiter=',', skip_header=1) + vehicle_time = data[:, 19] + cte = data[:, 20] + x_actual, y_actual = data[:, 2], data[:, 5] + # x_desired, y_desired = data[:, 11], data[:, 14] ### PP + x_desired, y_desired = data[:, 8], data[:, 11] #Stanley + speed_actual = data[:, -1] + des_speed = data[:, -13] + return vehicle_time, cte, x_actual, y_actual, x_desired, y_desired, speed_actual, des_speed + +def compute_derivative(times, values): + """ + Computes the derivative of array with respect to time. + Returns the time array and derivative values. + """ + dt = np.diff(times) + dv = np.diff(values) + derivative = dv / dt + return times[1:], derivative + +def moving_average(data, window_size=5): + """ + Simple moving average smoother for 1D array. + Pads edges by repeating border values. + """ + pad = window_size // 2 + padded = np.pad(data, (pad, pad), mode='edge') + smoothed = np.convolve(padded, np.ones(window_size) / window_size, mode='valid') + return smoothed + +def remove_duplicate_positions(times, xs, ys): + """ + Removes consecutive duplicate (x, y) points. + Keeps only unique steps in position. + """ + xs = np.array(xs) + ys = np.array(ys) + times = np.array(times) + + # Compute deltas + dx = np.diff(xs) + dy = np.diff(ys) + + # Find where movement occurs + moved = (dx != 0) | (dy != 0) + + # Always keep the first point + keep_idx = np.insert(moved, 0, True) + + return times[keep_idx], xs[keep_idx], ys[keep_idx] + +def compute_gs(times, xs, ys, speeds, accelerations, heading_rates): + # vxs = np.gradient(xs, times) + # vys = np.gradient(ys, times) + times = times - times[0] + dx = np.diff(xs) + dy = np.diff(ys) + dt = np.diff(times) + + vxs = dx/dt + vys = dy/dt + vs = np.sqrt(vxs**2 + vys**2) + + vs = moving_average(vs, window_size=7) + + valid_mask = vs > 1e-6 # small threshold to avoid float noise + vs = vs[valid_mask] + times = times[:-1] + times = times[valid_mask] + + long_accels = np.gradient(vs, times) + + accel_valid_mask = np.abs(long_accels) < 5 + + psis = np.arctan2(vys, vxs) + psis = psis[valid_mask] + psi_mask = psis > 1e-6 + psis = psis[psi_mask] + times = times[psi_mask] + vs = vs[psi_mask] + long_accels = long_accels[psi_mask] + + yaw_rates = np.gradient(psis, times) + + lat_accels = yaw_rates * vs + g = 9.81 + longitudinal_gs = long_accels / g + lateral_gs = lat_accels / g + + print("max long accel: " + str(np.max(long_accels))) + print("min long accel: " + str(np.min(long_accels))) + print("max lat accel: " + str(np.max(lat_accels))) + + + return longitudinal_gs, lateral_gs, vs, lat_accels, long_accels, times, valid_mask + +def plot_position(axis, x_actual, y_actual, x_desired=None, y_desired=None, safe_thresh=1, unsafe_thresh=2.5): + """Plots vehicle actual and desired positions vs. time""" + # position_error = np.sqrt((x_desired - x_actual) ** 2 + (y_desired - y_actual) ** 2) + + axis.plot(y_desired, x_desired, linestyle='--', color='blue', label='Desired') + axis.plot(y_actual, x_actual, color="black", linewidth=0.8, alpha=0.5, label='Actual') + + axis.set_xlabel("Y Position (m)") + axis.set_ylabel("X Position (m)") + axis.set_title("GEM Position") + axis.legend() + axis.grid(True) + +def plot_speeds(axis, time, speed_actual, comptued_speed = None): + if comptued_speed is not None: + axis.plot(time, comptued_speed, label='computed speed') + axis.plot(time, speed_actual, linestyle="--")#, label='current speed') + axis.set_xlabel("time") + axis.set_ylabel("speed m/s") + axis.set_title("GEM Speed") + # axis.legend() + axis.grid(True) + +def plot_accelerations(axis, accelerations, time): + axis.plot(time, accelerations, linestyle='--')#,label='acceleration') + axis.set_xlabel("time") + axis.set_ylabel("accel m/s^2") + axis.set_title("GEM Accelerations") + # axis.legend() + axis.grid(True) + +def plot_gg_diagram(axis, longitudinal_gs, lateral_gs): + """Plots gg diagram""" + # Plot G-G diagram + axis.scatter(lateral_gs, longitudinal_gs, alpha=0.5, label="Data Points") + + # draw op envelope + gy_full = np.concatenate([longitudinal_gs, longitudinal_gs]) + gx_full = np.concatenate([lateral_gs, -lateral_gs]) + + points = np.vstack((gx_full, gy_full)).T + hull = ConvexHull(points) + for simplex in hull.simplices: + axis.plot(points[simplex, 0], points[simplex, 1], 'r-') + axis.fill(points[hull.vertices, 0], points[hull.vertices, 1], + 'r', alpha=0.1, label='Operating Envelope') + + axis.axhline(0, color='black', linewidth=0.8) + axis.axvline(0, color='black', linewidth=0.8) + axis.set_xlabel("Lateral Acceleration (g)") + axis.set_ylabel("Longitudinal Acceleration (g)") + axis.set_title("G-G Diagram and Operating Envelope") + axis.legend() + axis.grid() + + + +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, "StanleyTrajectoryTracker_debug.csv") + # tracker_file = os.path.join(log_dir, "PurePursuitTrajectoryTracker_debug.csv") + + # if behavior.json doesn't exist, print error and exit + if not os.path.exists(behavior_file): + print("Error: behavior.json file is missing in log folder.") + sys.exit(1) + + # Parse behavior log file and compute metrics + times, xs, ys, speeds, 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) + + times, xs, ys = remove_duplicate_positions(times, xs, ys) + longitudinal_gs, lateral_gs, vs, lat_accels, long_accels, times, valid_mask = compute_gs(times, xs, ys, speeds, accelerations, heading_rates) + + vehicle_time, cte, x_actual, y_actual, x_desired, y_desired, speed_actual, des_speed = parse_tracker_csv(tracker_file) + + fig, axs = plt.subplots(2, 2, figsize=(12, 8)) + plot_gg_diagram(axs[0, 0], longitudinal_gs, lateral_gs) + # plot_position(axs[0, 1], xs, ys) #, x_desired, y_desired) + plot_position(axs[0, 1], x_actual, y_actual, x_desired, y_desired) + # plot_speeds(axs[1, 0], times, vs) + plot_speeds(axs[1, 0], vehicle_time, des_speed, speed_actual) + + plot_accelerations(axs[1, 1], long_accels, times) + plt.tight_layout() + name = "gg_diagram.png" + file_path = os.path.join(log_dir, name) + + plt.savefig(file_path, dpi=300, format='png') + print(f"Plots saved as {name}") + + plt.show() + + # # Pure pursuit tracker file exists: parse and plot all metrics + # if os.path.exists(tracker_file): + + # longitudinal_gs, lateral_gs, calculated_speed, lat_accels, long_accels, times, valid_mask = compute_gs(vehicle_time, x_actual, y_actual, speed_actual) + # fig, axs = plt.subplots(2, 2) + # plot_gg_diagram(axs[0, 0], longitudinal_gs, lateral_gs) + # plot_position(axs[0, 1], x_actual, y_actual, x_desired, y_desired) + # plot_speeds(axs[1, 0], speed_actual[valid_mask], calculated_speed, times) + + # plot_accelerations(axs[1, 1], long_accels, times) + + # plt.show() + # # Pure pursuit tracker file is missing: plot only behavior.json metrics + # else: + # print("Tracker file is missing. Skipping cross track error and vehicle position plots.") diff --git a/tuning logs/log_plot.py b/tuning logs/log_plot.py new file mode 100644 index 000000000..8d3121069 --- /dev/null +++ b/tuning logs/log_plot.py @@ -0,0 +1,136 @@ +import matplotlib.pyplot as plt +import pandas as pd +import os +import numpy as np + +def create_plot(t, actual, desired, xlabel, ylabel, title, legend, save_path=None): + plt.figure() + plt.plot(t, actual) + plt.plot(t, desired) + overshoot = np.max(np.array(actual) - np.array(desired)) + rmse = np.sqrt(np.mean((np.array(actual) - np.array(desired))**2)) + print(f'{title} - Maximum Overshoot: {overshoot}, RMSE: {rmse}') + plt.xlabel(xlabel) + plt.ylabel(ylabel) + plt.title(f'{title} (Max Overshoot: {overshoot:.2f}, RMSE: {rmse:.2f})') + plt.legend(legend) + plt.grid(True) + if save_path: + plt.savefig(save_path, dpi=600) + +def create_error_plot(t, error, xlabel, ylabel, title, save_path=None): + plt.figure() + plt.plot(t, error) + plt.xlabel(xlabel) + plt.ylabel(ylabel) + plt.title(title) + plt.grid(True) + if save_path: + plt.savefig(save_path, dpi=600) + +def main(): + log_folder = '2025-04-17_17-44-12' + df = pd.read_csv(log_folder + '/StanleyTrajectoryTracker_debug.csv') + # df = pd.read_csv(log_folder + '/PurePursuitTrajectoryTracker_debug.csv') + save_figures =True + + plots_folder = os.path.join(log_folder, 'plots') + os.makedirs(plots_folder, exist_ok=True) + + # pure pursuit + # t = df['curr pt[0] vehicle time'].tolist() + # x = df['curr pt[0]'].tolist() + # y = df['curr pt[1]'].tolist() + # v = df['current speed (m/s)'].tolist() + # yaw = df['current yaw (rad)'].tolist() + + # xd = df['desired pt[0]'].tolist() + # yd = df['desired pt[1]'].tolist() + # vd = df['desired speed (m/s)'].tolist() + # yawd = df['desired yaw (rad)'].tolist() + + # cte = df['crosstrack error'].tolist() + # front_wheel_angle = df['front wheel angle (rad)'].tolist() + # accel = df['output accel (m/s^2)'].tolist() + + #stanley + t = df['curr pt[0] vehicle time'].tolist() + x = df['curr pt[0]'].tolist() + y = df['curr pt[1]'].tolist() + v = df['Stanley: current speed (m/s)'].tolist() + yaw = df['Stanley: current yaw (rad)'].tolist() + + xd = df['desired_x'].tolist() + yd = df['desired_y'].tolist() + vd = df['Stanley: desired_speed (m/s)'].tolist() + # yawd = df['Stanley: desired yaw (rad)'].tolist() + + cte = df['crosstrack error'].tolist() + # front_wheel_angle = df['Stanley: front wheel angle (rad)'].tolist() + accel = df['Stanley: output_accel (m/s^2)'].tolist() + + + rmse_cte = np.sqrt(np.mean(np.array(cte)**2)) + print(f'RMSE (cte): {rmse_cte}') + + max_accel_error = np.max((np.array(accel) - 1.0)**2) + print(f'Maximum (acceleration - 1.0)^2: {max_accel_error}') + + rms_forward_acceleration = np.sqrt(np.mean(np.array(accel)**2)) + print(f'RMS of Acceleration: {rms_forward_acceleration}') + + dt = np.mean(np.diff(t)) + jerk = np.gradient(np.array(accel), dt) + rms_jerk = np.sqrt(np.mean(jerk**2)) + print(f'RMS of Jerk: {rms_jerk}') + + w1, w2 = 0.7, 0.3 + comfort_index = w1 * rms_forward_acceleration + w2 * rms_jerk + print(f'Comfort Index: {comfort_index}') + + + create_plot(t, y, yd, '$t$ (s)', '$y(t)$, $y_{d}(t)$ (m)', 'Actual and Desired y', ['Actual $y(t)$', 'Desired $y_{d}(t)$'], os.path.join(plots_folder, 'y_vs_yd.png') if save_figures else None) + create_error_plot(t, np.array(y) - np.array(yd), '$t$ (s)', 'Error in $y(t)$ (m)', 'Error between Actual and Desired $y(t)$', os.path.join(plots_folder, 'error_y.png') if save_figures else None) + + create_plot(t, x, xd, '$t$ (s)', '$x(t)$, $x_{d}(t)$ (m)', 'Actual and Desired x', ['Actual $x(t)$', 'Desired $x_{d}(t)$'], os.path.join(plots_folder, 'x_vs_xd.png') if save_figures else None) + create_error_plot(t, np.array(x) - np.array(xd), '$t$ (s)', 'Error in $x(t)$ (m)', 'Error between Actual and Desired $x(t)$', os.path.join(plots_folder, 'error_x.png') if save_figures else None) + + create_plot(t, v, vd, '$t$ (s)', '$v(t)$, $v_{d}(t)$ (m/s)', 'Actual and Desired v', ['Actual $v(t)$', 'Desired $v_{d}(t)$'], os.path.join(plots_folder, 'v_vs_vd.png') if save_figures else None) + create_error_plot(t, np.array(v) - np.array(vd), '$t$ (s)', 'Error in $v(t)$ (m/s)', 'Error between Actual and Desired $v(t)$', os.path.join(plots_folder, 'error_v.png') if save_figures else None) + + plt.figure() + plt.plot(t, cte) + rmse_cte = np.sqrt(np.mean(np.array(cte)**2)) + print(f'RMSE (cte): {rmse_cte}') + plt.xlabel('$t$ (s)') + plt.ylabel('Crosstrack Error (m)') + plt.title(f'Crosstrack Error over Time (RMSE: {rmse_cte:.2f})') + plt.grid(True) + if save_figures: + plt.savefig(os.path.join(plots_folder, 'cte.png'), dpi=600) + + # plt.figure() + # plt.plot(t, front_wheel_angle) + # plt.xlabel('$t$ (s)') + # plt.ylabel('Front Wheel Angle (rad)') + # plt.title('Front Wheel Angle over Time') + # plt.grid(True) + # if save_figures: + # plt.savefig(os.path.join(plots_folder, 'front_wheel_angle.png'), dpi=600) + + plt.figure() + plt.plot(t, accel) + plt.xlabel('$t$ (s)') + plt.ylabel('Acceleration (m/s^2)') + plt.title('Acceleration over Time') + plt.grid(True) + if save_figures: + plt.savefig(os.path.join(plots_folder, 'accel.png'), dpi=600) + + plt.show() + + + + +if __name__ == '__main__': + main() diff --git a/tuning logs/plot_json.py b/tuning logs/plot_json.py new file mode 100644 index 000000000..6396d974c --- /dev/null +++ b/tuning logs/plot_json.py @@ -0,0 +1,43 @@ +import json +import matplotlib.pyplot as plt +import matplotlib.dates as mdates +from datetime import datetime + +# Read the NDJSON data +data = [] +with open('test1/behavior.json', 'r') as f: + for line in f: + if line.strip(): + data.append(json.loads(line)) + +# Extract fields +x_vals = [] +y_vals = [] +speeds = [] +times = [] + +for entry in data: + # Skip if position is missing + if 'position' not in entry or 'vehicle_interface_reading' not in entry: + continue + + pos = entry['position'] + x_vals.append(pos.get('x', 0.0)) + y_vals.append(pos.get('y', 0.0)) + + speeds.append(entry['vehicle_interface_reading']['data'].get('speed', 0.0)) + times.append(datetime.fromtimestamp(entry['time'])) + +# Plotting +fig, ax = plt.subplots(figsize=(10, 6)) +sc = ax.scatter(x_vals, y_vals, c=speeds, cmap='plasma', s=30, edgecolor='k') +cbar = plt.colorbar(sc, ax=ax) +cbar.set_label('Speed (m/s)') + +ax.set_title('Vehicle Position Colored by Speed') +ax.set_xlabel('X Position (m)') +ax.set_ylabel('Y Position (m)') +ax.grid(True) + +plt.tight_layout() +plt.show() \ No newline at end of file