package Ġca . warp 7 . f rc . trajectory Ċ Ċ import Ġca . warp 7 . f rc . geometry . Arc Pose 2 D Ċ import Ġca . warp 7 . f rc . linear Interpol ate Ċ import Ġca . warp 7 . f rc . squared Ċ import Ġkotlin . math . abs Ċ import Ġkotlin . math . asin Ċ import Ġkotlin . math . sqrt Ċ Ċ fun ĠList < Arc Pose 2 D >. timed Trajectory ( ĊĠĠĠĠĠĠĠ Ġwheel base Radius : ĠDouble , ĊĠĠĠĠĠĠĠ Ġstart Velocity : ĠDouble , ĊĠĠĠĠĠĠĠ Ġend Velocity : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Velocity : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Acceleration : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Cent ri pet al Acceleration : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax J erk : ĠDouble Ċ ): ĠList < Trajectory Point > Ġ{ ĊĠĠĠ Ġ// ĠCreate Ġlist Ġof Ġstates Ġwith Ġeverything Ġset Ġto Ġmax , Ġthen Ġlimit Ġit Ġafterwards ĊĠĠĠ Ġval Ġstates Ġ= Ġmap Ġ{ ĠTra jectory Point ( it , Ġmax Velocity , Ġmax Acceleration ) Ġ} ĊĠĠĠ Ġ// ĠCompute Ġarc Ġlength Ġbetween Ġeach Ġpair Ġof Ġpoints Ġin Ġthe Ġpath ĊĠĠĠ Ġval Ġarc Lengths Ġ= Ġzip With Next Ġ{ Ġa , Ġb Ġ-> ĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġabs ( a . cur v ature ) ĊĠĠĠĠĠĠĠ Ġ// Ġget Ġthe Ġch ord Ġlength Ġ( translation al Ġdistance ) ĊĠĠĠĠĠĠĠ Ġval Ġch ord Length Ġ= Ġa . translation . distance To ( b . translation ) ĊĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠGo ing Ġstraight , Ġarc Length Ġ= Ġch ord Length ĊĠĠĠĠĠĠĠĠĠĠĠ Ġk Ġ< Ġ 1 E - 6 Ġ-> Ġch ord Length ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠTurn ing Ġin Ġplace , Ġarc Length Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġk . isIn finite () Ġ-> Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠMoving Ġon Ġa Ġradius : ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġarc Length Ġ= Ġtheta Ġ* Ġr Ġ= Ġtheta Ġ/ Ġk ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġtheta Ġ= Ġas in ( half _ ch ord Ġ/ Ġr ) Ġ* Ġ 2 Ġ= Ġas in ( half _ ch ord Ġ* Ġk ) Ġ* Ġ 2 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġarc Length Ġ= Ġas in ( half _ ch ord Ġ* Ġk ) Ġ* Ġ 2 Ġ/ Ġk ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġas in (( ch ord Length Ġ/ Ġ 2 ) Ġ* Ġk ) Ġ* Ġ 2 Ġ/ Ġk ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// ĠCompute Ġisolated Ġvelocity Ġconstraints Ġat Ġa Ġgiven Ġcur v ature ĊĠĠĠ Ġval Ġisolated Constraints Ġ= Ġmap Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġabs ( it . cur v ature ) ĊĠĠĠĠĠĠĠ Ġ// ĠTurn ing Ġin Ġplace , Ġv Ġ= Ġ 0 ĊĠĠĠĠĠĠĠ Ġif Ġ( k . isIn finite ()) Ġreturn @ map Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ/* ĠVe locity Ġconstr ained Ġby Ġcur v ature Ġequations ĊĠĠĠĠĠĠĠĠ Ġ* Ġeq n Ġ 1 . Ġw Ġ= Ġ( right Ġ- Ġleft ) Ġ/ Ġ( 2 Ġ* ĠL ) ĊĠĠĠĠĠĠĠĠ Ġ* Ġeq n Ġ 2 . Ġv Ġ= Ġ( left Ġ+ Ġright ) Ġ/ Ġ 2 ĊĠĠĠĠĠĠĠĠ Ġ* Ġ 1 . ĠRe arrange Ġequation Ġ 1 : Ġw ( 2 Ġ* ĠL ) Ġ= Ġright Ġ- Ġleft ; Ġ Ġleft Ġ= Ġright Ġ- Ġw ( 2 Ġ* ĠL ) ĊĠĠĠĠĠĠĠĠ Ġ* Ġ 2 . ĠAss uming Ġthe Ġright Ġside Ġis Ġat Ġmax Ġvelocity : Ġright Ġ= ĠV _ max ; Ġ Ġleft Ġ= ĠV _ max Ġ- Ġw ( 2 Ġ* ĠL ) ĊĠĠĠĠĠĠĠĠ Ġ* Ġ 3 . ĠSubstitute Ġleft Ġand Ġright Ġinto Ġequation Ġ 2 : Ġv Ġ= Ġ( 2 Ġ* ĠV _ max Ġ- Ġw ( 2 Ġ* ĠL )) Ġ/ Ġ 2 ĊĠĠĠĠĠĠĠĠ Ġ* Ġ 5 . ĠSubstitute Ġw Ġ= Ġv Ġ* Ġk Ġinto Ġequation Ġ 2 , Ġv Ġ= Ġ( 2 Ġ* ĠV _ max Ġ- Ġv Ġ* Ġk Ġ* Ġ 2 Ġ* ĠL ) Ġ/ Ġ 2 ĊĠĠĠĠĠĠĠĠ Ġ* Ġ 6 . ĠSolve : Ġv Ġ= ĠV _ max Ġ- Ġv Ġ* Ġk Ġ* ĠL ; Ġv Ġ+ Ġv Ġ* Ġk Ġ* ĠL Ġ= ĠV _ max ; Ġv Ġ* Ġ( 1 Ġ+ Ġk Ġ* ĠL ) Ġ= ĠV _ max ; Ġv Ġ= ĠV _ max Ġ/ Ġ( 1 Ġ+ Ġk Ġ* ĠL ) */ ĊĠĠĠĠĠĠĠ Ġval Ġdrive Kin ematic Constraint Ġ= Ġmax Velocity Ġ/ Ġ( 1 Ġ+ Ġk Ġ* Ġwheel base Radius ) ĊĠĠĠĠĠĠĠ Ġ// ĠVe locity Ġconstr ained Ġinverse ly Ġpro portional Ġto Ġthe Ġcur v ature ĊĠĠĠĠĠĠĠ Ġval Ġcent ri pet al Acceleration Constraint Ġ= Ġif Ġ( k Ġ> Ġ 1 E - 6 ) Ġmax Cent ri pet al Acceleration Ġ/ Ġk Ġelse Ġmax Velocity ĊĠĠĠĠĠĠĠ Ġreturn @ map Ġmin Of ( drive Kin ematic Constraint , Ġcent ri pet al Acceleration Constraint ) ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// ĠAssign Ġthe Ġinitial Ġvelocity ĊĠĠĠ Ġstates . first (). velocity Ġ= Ġstart Velocity ĊĠĠĠ Ġ// ĠForward Ġpass ĊĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize Ġ- Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġarc Length Ġ= Ġarc Lengths [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġcurrent State Ġ= Ġstates [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġnext State Ġ= Ġstates [ i Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġval Ġconstr ained Velocity Ġ= Ġisolated Constraints [ i Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġ// ĠApply Ġkin ematic Ġequation Ġvf ^ 2 Ġ= Ġvi ^ 2 Ġ+ Ġ 2 ax , Ġsolve Ġfor Ġvf ĊĠĠĠĠĠĠĠ Ġval Ġmax Reach able Velocity Ġ= Ġsqrt ( currentState . velocity . squared Ġ+ Ġ 2 Ġ* Ġmax Acceleration Ġ* Ġarc Length ) ĊĠĠĠĠĠĠĠ Ġ// ĠLimit Ġvelocity Ġbased Ġon Ġcur v ature Ġconstraint Ġand Ġforward Ġacceleration ĊĠĠĠĠĠĠĠ Ġnext State . velocity Ġ= Ġmin Of ( next State . velocity , Ġconstr ained Velocity , Ġmax Reach able Velocity ) ĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġforward Ġdt ĊĠĠĠĠĠĠĠ Ġnext State . t Ġ= Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( currentState . velocity Ġ+ Ġnext State . velocity ) ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// ĠAssign Ġthe Ġfinal Ġvelocity ĊĠĠĠ Ġstates . last (). velocity Ġ= Ġend Velocity ĊĠĠĠ Ġ// ĠReverse Ġpass ĊĠĠĠ Ġfor Ġ( i Ġin Ġsize Ġ- Ġ 1 Ġdown To Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġarc Length Ġ= Ġarc Lengths [ i Ġ- Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġval Ġcurrent State Ġ= Ġstates [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġnext State Ġ= Ġstates [ i Ġ- Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġ// ĠApply Ġkin ematic Ġequation Ġvf ^ 2 Ġ= Ġvi ^ 2 Ġ+ Ġ 2 ax , Ġsolve Ġfor Ġvf ĊĠĠĠĠĠĠĠ Ġval Ġmax Reach able Velocity Ġ= Ġsqrt ( currentState . velocity . squared Ġ+ Ġ 2 Ġ* Ġmax Acceleration Ġ* Ġarc Length ) ĊĠĠĠĠĠĠĠ Ġ// ĠLimit Ġvelocity Ġbased Ġon Ġreverse Ġacceleration ĊĠĠĠĠĠĠĠ Ġnext State . velocity Ġ= Ġmin Of ( next State . velocity , Ġmax Reach able Velocity ) ĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġreverse Ġdt ĊĠĠĠĠĠĠĠ Ġcurrent State . t Ġ= Ġmax Of ( currentState . t , Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( currentState . velocity Ġ+ Ġnext State . velocity )) ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// Ġcalculate Ġacceleration Ġbased Ġon Ġfinal Ġdt ĊĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize Ġ- Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġcurrent State Ġ= Ġstates [ i Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġval Ġlast State Ġ= Ġstates [ i ] ĊĠĠĠĠĠĠĠ Ġcurrent State . acceleration Ġ= Ġ( currentState . velocity Ġ- Ġlast State . velocity ) Ġ/ Ġcurrent State . t ĊĠĠĠĠĠĠĠ Ġcurrent State . j erk Ġ= Ġ( currentState . acceleration Ġ- Ġlast State . acceleration ) Ġ/ Ġcurrent State . t ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// ĠLimit Ġj erk Ġif Ġit 's Ġenabled ĊĠĠĠ Ġif Ġ( max J erk . is Finite ()) Ġ{ ĊĠĠĠĠĠĠĠ Ġ// ĠFind Ġa Ġlist Ġof Ġpoints Ġthat Ġexceeds Ġthe Ġmax Ġj erk ĊĠĠĠĠĠĠĠ Ġval Ġj erk Points Ġ= Ġmutable ListOf < Int >() ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 1 Ġuntil Ġsize ) Ġif Ġ( abs ( states [ i ]. j erk ) Ġ> Ġmax J erk ) Ġj erk Points . add ( i ) ĊĠĠĠĠĠĠĠ Ġ// ĠLimit Ġj erk Ġat Ġeach Ġof Ġthese Ġpoints ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġj erk Points . size ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġstate Index Ġ= Ġj erk Points [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġa Ġrange Ġof Ġpoints Ġto Ġspread Ġout Ġthe Ġrequired Ġacceleration ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġrange Ġ= Ġabs ( states [ state Index ]. j erk Ġ/ Ġ( 2 Ġ* Ġmax J erk )). toInt () Ġ* Ġ 3 Ġ+ Ġ 1 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġbounds Ġof Ġthe Ġactual Ġrange Ġwith Ġrespect Ġto Ġother Ġj erk Ġpoints ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġstart Ġ= Ġmax Of ( j erk Points . get OrNull ( i Ġ- Ġ 1 ) Ġ?: Ġ 0 , Ġstate Index Ġ- Ġrange ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġend Ġ= Ġmin Of ( j erk Points . get OrNull ( i Ġ+ Ġ 1 ) Ġ?: Ġstates . size Ġ- Ġ 1 , Ġstate Index Ġ+ Ġrange ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġacc Start Ġ= Ġstates [ start ]. acceleration ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġacc End Ġ= Ġstates [ end ]. acceleration ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġindividual Ġstep Ġsize ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmax Time Ġ= Ġstates . sub List ( start , Ġend Ġ+ Ġ 1 ). sum By Double Ġ{ Ġit . t Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġt Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( j Ġin Ġstart .. end ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġnext Ġ= Ġstates [ j Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcurrent Ġ= Ġstates [ j ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġt Ġ+= Ġcurrent . t ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġinterpol ant Ġ= Ġt Ġ/ Ġmax Time ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠInterpol ate Ġthe Ġacceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent . acceleration Ġ= Ġlinear Interpol ate ( acc Start , Ġacc End , Ġinterpol ant ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠApply Ġkin ematic Ġequation Ġvf ^ 2 Ġ= Ġvi ^ 2 Ġ+ Ġ 2 ax , Ġsolve Ġfor Ġvf ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġarc Length Ġ= Ġarc Lengths [ j ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . velocity Ġ= Ġmin Of ( next . velocity , Ġsqrt ( current . velocity . squared Ġ+ Ġ 2 Ġ* Ġcurrent . acceleration Ġ* Ġarc Length )) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠSet Ġthe Ġnew Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . t Ġ= Ġmax Of ( next . t , Ġ 2 Ġ* Ġarc Length ) Ġ/ Ġabs ( current . velocity Ġ+ Ġnext . velocity ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĠĠĠ Ġ// ĠSet Ġthe Ġendpoints ' Ġacceleration Ġto Ġ 0 Ġto Ġallow Ġthe Ġmotor Ġto Ġstop ĊĠĠĠ Ġstates . first (). acceleration Ġ= Ġ 0 . 0 ĊĠĠĠ Ġstates . last (). acceleration Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ// ĠIntegr ate Ġdt Ġarray Ġinto Ġtimestamps ĊĠĠĠ Ġfor Ġ( i Ġin Ġ 1 Ġuntil Ġstates . size ) Ġstates [ i ]. t Ġ+= Ġstates [ i Ġ- Ġ 1 ]. t ĊĠĠĠ Ġreturn Ġstates Ċ }