package Ġca . warp 7 . f rc . trajectory Ċ Ċ import Ġca . warp 7 . f rc . geometry . Arc Pose 2 D Ċ import Ġca . warp 7 . f rc . geometry . radians Ċ 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 Ċ import Ġkotlin . math . with Sign Ċ Ċ /** Ċ Ġ* ĠGenerates Ġa Ġlist Ġof Ġtimed Ġ[ Trajectory State ] Ġfor Ġa Ġdiffer ential Ġdrive Ġrobot , Ċ Ġ* Ġbased Ġon Ġa Ġlist Ġof Ġ[ Arc Pose 2 D ] Ġand Ġdrive Ġtrain Ġparameters Ċ Ġ* Ċ Ġ* ĠThis Ġalgorithm Ġis Ġinspired Ġby ĠTeam Ġ 2 5 4 , ĠTeam Ġ 5 1 9 0 , Ġand ĠTeam Ġ 6 1 3 5 Ċ Ġ* Ċ Ġ* ĠThe Ġinput Ġpath Ġof Ġthis Ġfunction Ġcan Ġbe Ġgenerated Ġfrom Ġway points Ġwith Ċ Ġ* Ġ[ ca . warp 7 . f rc . path . parameter ized Spl ines Of ] Ċ Ġ* Ċ Ġ* ĠThe Ġpath Ġmay Ġcontain Ġpoints Ġof Ġinfinite Ġcur v ature Ġto Ġindicate Ġturning Ġin Ġplace . ĠIn Ġsuch Ġcases , Ċ Ġ* Ġthe Ġfunction Ġassumes Ġthat Ġthe Ġtranslation Ġfield Ġstays Ġthe Ġsame . ĠThe Ġdirection Ġof Ġturning Ġis Ċ Ġ* Ġdetermined Ġby Ġthe Ġsign Ġof Ġthe Ġcur v ature Ġfield Ċ Ġ* Ċ Ġ* Ġ### ĠTra jectory ĠGeneration ĠSteps Ċ Ġ* Ġ 1 . ĠFind Ġthe Ġarc Ġlength Ġbetween Ġevery Ġconsecutive Ġpoint Ċ Ġ* Ġ 2 . ĠPerform Ġa Ġforward Ġpass Ġstates Ġto Ġsatisfy Ġisolated Ġand Ġpositive Ġacceleration Ġconstraints Ċ Ġ* Ġ 3 . ĠPerform Ġa Ġreverse Ġpass Ġto Ġsatisfy Ġnegative Ġacceleration Ġconstraints Ċ Ġ* Ġ 4 . ĠPerform Ġan Ġacc umulative Ġpass Ġto Ġcalculate Ġhigher - order Ġderivatives Ġof Ġvelocity Ċ Ġ* Ġ 5 . ĠApply Ġj erk Ġlimiting Ċ Ġ* Ġ 6 . ĠIntegr ate Ġdt Ġof Ġeach Ġstate Ġinto Ġtotal Ġtrajectory Ġtime Ċ Ġ* Ċ Ġ* ĠComplex ity Ġbased Ġon Ġpath Ġsize : ĠO ( n ) Ċ Ġ* Ġ[ generate Trajectory ] Ġis Ġa Ġpure Ġfunction ; Ġthe Ġother Ġfunctions Ġin Ġthis Ġfile Ġare Ġnot Ċ Ġ* Ċ Ġ* Ġ@ param Ġpath Ġthe Ġtarget Ġpath Ġof Ġthe Ġtrajectory . Ċ Ġ* Ċ Ġ* Ġ@ param Ġwheel base Radius Ġthe Ġeffect Ġwheel Ġbase Ġradius Ġin Ġmet res Ċ Ġ* Ċ Ġ* Ġ@ param Ġmax Velocity Ġthe Ġmaximum Ġlinear Ġvelocity Ġallowed Ġin Ġthis Ġtrajectory Ġin Ġmet res / second . ĠIt Ġis Ċ Ġ* Ġbetter Ġto Ġset Ġthis Ġvalue Ġto Ġbelow Ġthe Ġactual Ġmaximum Ġof Ġthe Ġrobot Ġfor Ġefficiency Ġand Ġaccuracy . Ċ Ġ* Ġ( At Ġmost Ġ 8 0 % Ġof Ġactual Ġmax Ġvelocity ) Ċ Ġ* Ċ Ġ* Ġ@ param Ġmax Acceleration Ġthe Ġmaximum Ġlinear Ġacceleration Ġallowed Ġin Ġthis Ġtrajectory Ġin Ġmet res / second ^ 2 Ċ Ġ* ĠLower Ġthis Ġfor Ġrobot Ġstability . ĠHowever , Ġthis Ġwould Ġusually Ġincrease Ġtotal Ġtrajectory Ġtime Ġmore Ċ Ġ* Ġsignificantly Ġthan Ġ[ max Velocity ] Ċ Ġ* Ċ Ġ* Ġ@ param Ġmax Cent ri pet al Acceleration Ġthe Ġmaximum Ġcent ri pet al Ġacceleration Ġin Ġh ert z . ĠThis Ġvalue Ċ Ġ* Ġcan Ġreduce Ġmaximum Ġvelocity Ġat Ġhigh Ġcur v atures Ġand Ġis Ġuseful Ġto Ġprevent Ġt ipping Ġwhen Ġthe ĠCo G Ġis Ġhigh Ċ Ġ* Ċ Ġ* Ġ@ param Ġmax J erk Ġthe Ġmaximum Ġlinear Ġj erk Ġallowed Ġin Ġthis Ġtrajectory Ġin Ġmet res / second ^ 3 . ĠThis Ġvalue Ġcan Ċ Ġ* Ġreduce Ġspi kes Ġin Ġvol t ages Ġon Ġthe Ġdrive Ġtrain Ġto Ġincrease Ġstability . Ġ[ Double . POSITIVE _ INFINITY ] Ġmay Ċ Ġ* Ġbe Ġpassed Ġinstead Ġto Ġdisable Ġj erk Ġlimiting Ċ Ġ* Ċ Ġ* Ġ@ see ĠTra jectory State Ċ Ġ* Ġ@ see ĠArc Pose 2 D Ċ Ġ* Ċ Ġ* Ġ@ return Ġa Ġlist Ġof Ġtimed Ġtrajectory Ġpoints Ċ Ġ*/ Ċ fun Ġgenerate Trajectory ( ĊĠĠĠĠĠĠĠ Ġpath : ĠList < Arc Pose 2 D >, Ġ// Ġ((( x , Ġy ), ĠÎ ¸ ), Ġk , Ġdk _ ds ) ĊĠĠĠĠĠĠĠ Ġwheel base Radius : ĠDouble , Ġ// Ġm ĊĠĠĠĠĠĠĠ Ġmax Velocity : ĠDouble , Ġ// Ġm / s ĊĠĠĠĠĠĠĠ Ġmax Acceleration : ĠDouble , Ġ// Ġm / s ^ 2 ĊĠĠĠĠĠĠĠ Ġmax Cent ri pet al Acceleration : ĠDouble , Ġ// Ġs ^ - 1 ĊĠĠĠĠĠĠĠ Ġmax J erk : ĠDouble Ġ// Ġm / s ^ 3 Ċ ): ĠList < Trajectory State > Ġ{ ĊĊĠĠĠ Ġval Ġmax Angular Velocity Ġ= Ġmax Velocity Ġ/ Ġwheel base Radius Ġ// Ġrad / s ĊĠĠĠ Ġval Ġmax Angular Acceleration Ġ= Ġmax Acceleration Ġ/ Ġwheel base Radius Ġ// Ġrad / s ^ 2 ĊĊĠĠĠ Ġval Ġstates Ġ= Ġpath . map Ġ{ ĠTra jectory State ( it ) Ġ} ĊĊĠĠĠ Ġval Ġarc Lengths Ġ= Ġcompute Arc Lengths ( path ) ĊĊĠĠĠ Ġforward Pass ( states , Ġarc Lengths , Ġwheel base Radius , Ġmax Velocity , Ġmax Acceleration , ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmax Angular Velocity , Ġmax Angular Acceleration , Ġmax Cent ri pet al Acceleration ) ĊĊĠĠĠ Ġreverse Pass ( states , Ġarc Lengths , Ġmax Acceleration , Ġmax Angular Acceleration ) ĊĊĠĠĠ Ġacc umulative Pass ( states ) ĊĊĠĠĠ Ġif Ġ( max J erk . is Finite ()) Ġ{ ĊĠĠĠĠĠĠĠ Ġr amp ed Acceleration Pass ( states , Ġarc Lengths , Ġmax J erk ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġintegration Pass ( states ) ĊĊĠĠĠ Ġreturn Ġstates Ċ } ĊĊ Ċ /** Ċ Ġ* ĠCompute Ġarc Ġlength Ġbetween Ġeach Ġpair Ġof Ġpo ses Ġin Ġthe Ġpath Ċ Ġ*/ Ċ private Ġfun Ġcompute Arc Lengths ( ĊĠĠĠĠĠĠĠ Ġpath : ĠList < Arc Pose 2 D > Ġ// Ġ((( x , Ġy ), ĠÎ ¸ ), Ġk , Ġdk _ ds ) Ċ ): ĠList < Double > Ġ= Ġpath . zip With Next Ġ{ Ġcurrent , Ġnext Ġ-> ĊĊĠĠĠ Ġcheck (! current . pose . epsilon Equals ( next . pose )) Ġ{ ĊĠĠĠĠĠĠĠ Ġ" Trajectory ĠGenerator Ġ- ĠTwo Ġconsecutive Ġpoints Ġcontain Ġthe Ġsame Ġpose " ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ// ĠGet Ġthe Ġmagnitude Ġof Ġcur v ature Ġon Ġthe Ġnext Ġstate ĊĠĠĠ Ġ// ĠNote Ġthat Ġthe Ġcur v ature Ġis Ġtaken Ġfrom Ġthe Ġ` next ` Ġstate Ġto Ġpredict ĊĠĠĠ Ġ// Ġchanging Ġfrom Ġand Ġto Ġinfinity Ġso Ġthat Ġit Ġdoesn 't Ġresult Ġin Ġa Ġvery ĊĠĠĠ Ġ// Ġsmall Ġarc Ġlength ĊĠĠĠ Ġval Ġk Ġ= Ġabs ( next . cur v ature ) ĊĊĠĠĠ Ġwhen Ġ{ ĊĊĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġturning Ġin Ġplace , Ġarc Length Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġ// ĠReturns Ġthe Ġmagnitude Ġof Ġthe Ġangular Ġdistance Ġin Ġradians ĊĠĠĠĠĠĠĠ Ġk . isIn finite () Ġ-> Ġabs (( next . rotation Ġ- Ġcurrent . rotation ). radians ) ĊĊĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġmoving Ġin Ġa Ġcurve Ġor Ġstraight Ġline ĊĠĠĠĠĠĠĠ Ġ// ĠReturns Ġthe Ġlinear Ġdistance Ġin Ġmeters ĊĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠGet Ġthe Ġch ord Ġlength Ġ( translation al Ġdistance ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġdistance Ġ= Ġcurrent . translation . distance To ( next . translation ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġcheck ( distance Ġ!= Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ" Trajectory ĠGenerator Ġ- ĠOver lapping Ġpoints Ġwithout Ġinfinite Ġcur v ature " ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠGo ing Ġstraight , Ġarc Length Ġ= Ġdistance ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġk Ġ< Ġ 1 E - 6 Ġ-> Ġdistance ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġ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 (( distance Ġ/ Ġ 2 ) Ġ* Ġk ) Ġ* Ġ 2 Ġ/ Ġk ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ } Ċ Ċ /** Ċ Ġ* ĠForward Ġpass Ċ Ġ*/ Ċ private Ġfun Ġforward Pass ( ĊĠĠĠĠĠĠĠ Ġstates : ĠList < Trajectory State >, ĊĠĠĠĠĠĠĠ Ġarc Lengths : ĠList < Double >, ĊĠĠĠĠĠĠĠ Ġwheel base Radius : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Velocity : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Acceleration : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Angular Velocity : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Angular Acceleration : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Cent ri pet al Acceleration : ĠDouble Ċ ) Ġ{ ĊĊĠĠĠ Ġ// ĠAssign Ġthe Ġinitial Ġlinear Ġand Ġangular Ġvelocity ĊĠĠĠ Ġstates . first (). v Ġ= Ġ 0 . 0 ĊĠĠĠ Ġstates . first (). w Ġ= Ġ 0 . 0 ĊĊĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġstates . size Ġ- Ġ 1 ) Ġ{ ĊĊĠĠĠĠĠĠĠ Ġval Ġarc Length Ġ= Ġarc Lengths [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġcurrent Ġ= Ġstates [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġnext Ġ= Ġstates [ i Ġ+ Ġ 1 ] ĊĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġabs ( next . arc Pose . cur v ature ) ĊĊĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġturning Ġin Ġplace ; Ġusing Ġangular Ġvalues Ġinstead ĊĠĠĠĠĠĠĠĠĠĠĠ Ġk . isIn finite () Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmax Reach able Angular Velocity Ġ= Ġsqrt ( current . w . squared Ġ+ Ġ 2 Ġ* Ġmax Angular Acceleration Ġ* Ġarc Length ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . w Ġ= Ġmin Of ( max Angular Velocity , Ġmax Reach able Angular Velocity ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠMake Ġsure Ġthat Ġthe Ġlinear Ġvelocity Ġis Ġtaken Ġcare Ġof ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . v Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . t Ġ= Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( current . w Ġ+ Ġnext . w ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġmoving Ġin Ġa Ġcurve Ġor Ġstraight Ġline ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġ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 . ĠRe arrange Ġto Ġ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 Ġto Ġslow Ġdown Ġaround Ġturns ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcent ri pet al Acceleration Constraint Ġ= Ġif Ġ( k Ġ> Ġ 1 E - 6 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmax Cent ri pet al Acceleration Ġ/ Ġk ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmax Velocity ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠFind Ġthe Ġtotal Ġconstr ained Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġconstr ained Velocity Ġ= Ġmin Of ( drive Kin ematic Constraint , Ġcent ri pet al Acceleration Constraint ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠApply Ġkin ematic Ġequation Ġvf ^ 2 Ġ= Ġvi ^ 2 Ġ+ Ġ 2 ax , Ġsolve Ġfor Ġvf ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmax Reach able Velocity Ġ= Ġsqrt ( current . v . squared Ġ+ Ġ 2 Ġ* Ġmax Acceleration Ġ* Ġarc Length ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠLimit Ġvelocity Ġbased Ġon Ġcur v ature Ġconstraint Ġand Ġforward Ġacceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . v Ġ= Ġmin Of ( max Velocity , Ġconstr ained Velocity , Ġmax Reach able Velocity ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠMake Ġsure Ġthat Ġthe Ġangular Ġvelocity Ġis Ġtaken Ġcare Ġof ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . w Ġ= Ġnext . v Ġ* Ġk ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġforward Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . t Ġ= Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( current . v Ġ+ Ġnext . v ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ } Ċ Ċ /** Ċ Ġ* ĠReverse Ġpass Ċ Ġ*/ Ċ private Ġfun Ġreverse Pass ( ĊĠĠĠĠĠĠĠ Ġstates : ĠList < Trajectory State >, ĊĠĠĠĠĠĠĠ Ġarc Lengths : ĠList < Double >, ĊĠĠĠĠĠĠĠ Ġmax Acceleration : ĠDouble , ĊĠĠĠĠĠĠĠ Ġmax Angular Acceleration : ĠDouble Ċ ) Ġ{ ĊĊĠĠĠ Ġ// ĠAssign Ġthe Ġfinal Ġlinear Ġand Ġangular Ġvelocity ĊĠĠĠ Ġstates . last (). v Ġ= Ġ 0 . 0 ĊĠĠĠ Ġstates . last (). w Ġ= Ġ 0 . 0 ĊĊĠĠĠ Ġfor Ġ( i Ġin Ġstates . size Ġ- Ġ 1 Ġdown To Ġ 1 ) Ġ{ ĊĊĠĠĠĠĠĠĠ Ġval Ġarc Length Ġ= Ġarc Lengths [ i Ġ- Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġval Ġcurrent Ġ= Ġstates [ i ] ĊĠĠĠĠĠĠĠ Ġval Ġnext Ġ= Ġstates [ i Ġ- Ġ 1 ] ĊĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġabs ( current . arc Pose . cur v ature ) ĊĊĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġturning Ġin Ġplace ; Ġusing Ġangular Ġvalues Ġinstead ĊĠĠĠĠĠĠĠĠĠĠĠ Ġk . isIn finite () Ġ-> Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmax Reach able Angular Velocity Ġ= Ġsqrt ( current . w . squared Ġ+ Ġ 2 Ġ* Ġmax Angular Acceleration Ġ* Ġarc Length ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . w Ġ= Ġmin Of ( next . w , Ġmax Reach able Angular Velocity ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . v Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent . t Ġ= Ġmax Of ( current . t , Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( current . w Ġ+ Ġnext . w )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠRobot Ġis Ġmoving Ġin Ġa Ġcurve Ġor Ġstraight Ġline ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠApply Ġkin ematic Ġequation Ġvf ^ 2 Ġ= Ġvi ^ 2 Ġ+ Ġ 2 ax , Ġsolve Ġfor Ġvf ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġmax Reach able Velocity Ġ= Ġsqrt ( current . v . squared Ġ+ Ġ 2 Ġ* Ġmax Acceleration Ġ* Ġarc Length ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠLimit Ġvelocity Ġbased Ġon Ġreverse Ġacceleration ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . v Ġ= Ġmin Of ( next . v , Ġmax Reach able Velocity ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġnext . w Ġ= Ġnext . v Ġ* Ġk ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġthe Ġreverse Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcurrent . t Ġ= Ġmax Of ( current . t , Ġ( 2 Ġ* Ġarc Length ) Ġ/ Ġ( current . v Ġ+ Ġnext . v )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ } ĊĊ Ċ /** Ċ Ġ* ĠAcc umulative ĠPass Ġfor Ġcalculating Ġhigher - order Ġderivatives Ċ Ġ* Ġ( acceleration Ġand Ġj erk ), Ġas Ġwell Ġas Ġgiving Ġback Ġthe Ġsign Ċ Ġ* Ġof Ġthe Ġcur v ature Ċ Ġ*/ Ċ private Ġfun Ġacc umulative Pass ( states : ĠList < Trajectory State >) Ġ{ ĊĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġstates . size Ġ- Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġcurrent Ġ= Ġstates [ i Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠ Ġval Ġlast Ġ= Ġstates [ i ] ĊĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġacceleration ĊĠĠĠĠĠĠĠ Ġcurrent . dv Ġ= Ġ( current . v Ġ- Ġlast . v ) Ġ/ Ġcurrent . t ĊĊĠĠĠĠĠĠĠ Ġval Ġk Ġ= Ġcurrent . arc Pose . cur v ature ĊĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġangular Ġvelocity ĊĠĠĠĠĠĠĠ Ġwhen Ġ{ ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠCorrect Ġthe Ġsign Ġof Ġangular Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠ Ġk . isIn finite () Ġ-> Ġcurrent . w Ġ= Ġcurrent . w . with Sign ( k ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠMultiply Ġcur v ature Ġby Ġthe Ġlinear Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠ Ġelse Ġ-> Ġcurrent . w Ġ= Ġcurrent . v Ġ* Ġk ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġangular Ġacceleration ĊĠĠĠĠĠĠĠ Ġcurrent . dw Ġ= Ġ( current . w Ġ- Ġlast . w ) Ġ/ Ġcurrent . t ĊĊĠĠĠĠĠĠĠ Ġ// ĠCalculate Ġj erk ĊĠĠĠĠĠĠĠ Ġcurrent . dd v Ġ= Ġ( current . dv Ġ- Ġlast . dv ) Ġ/ Ġcurrent . t ĊĠĠĠĠĠĠĠ Ġcurrent . dd w Ġ= Ġ( current . dw Ġ- Ġlast . dw ) Ġ/ Ġcurrent . t ĊĠĠĠ Ġ} Ċ } ĊĊ Ċ /** Ċ Ġ* ĠLim its Ġj erk Ġin Ġa Ġtrajectory Ċ Ġ*/ Ċ private Ġfun Ġr amp ed Acceleration Pass ( states : ĠList < Trajectory State >, Ġarc Lengths : ĠList < Double >, Ġmax J erk : ĠDouble ) Ġ{ ĊĊĠĠĠ Ġ// ĠFind Ġa Ġlist Ġof Ġpoints Ġthat Ġexceeds Ġthe Ġmax Ġj erk ĊĠĠĠ Ġval Ġj erk Points Ġ= Ġmutable ListOf < Int >() ĊĊĠĠĠ Ġfor Ġ( i Ġin Ġ 1 Ġuntil Ġstates . size ) Ġ{ ĊĊĠĠĠĠĠĠĠ Ġ// ĠLimit Ġj erk Ġat Ġeach Ġof Ġthese Ġpoints ĊĠĠĠĠĠĠĠ Ġif Ġ( abs ( states [ i ]. dd v ) Ġ> Ġmax J erk ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġj erk Points . add ( i ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ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 ]. dd v Ġ/ Ġ( 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 ]. dv ĊĠĠĠĠĠĠĠ Ġval Ġacc End Ġ= Ġstates [ end ]. dv ĊĊĠĠĠĠĠĠĠ Ġ// Ġ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 . dv Ġ= Ġ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 ] ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġj erk Limit ed Velocity Ġ= Ġsqrt ( current . v . squared Ġ+ Ġ 2 Ġ* Ġcurrent . dv Ġ* Ġarc Length ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext . v Ġ= Ġmin Of ( next . v , Ġj erk Limit ed Velocity ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠSet Ġthe Ġnew Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġnext . t Ġ= Ġmax Of ( next . t , Ġ 2 Ġ* Ġarc Length ) Ġ/ Ġabs ( current . v Ġ+ Ġnext . v ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ } Ċ Ċ /** Ċ Ġ* ĠIntegr ate Ġdt Ġinto Ġtrajectory Ġtime Ċ Ġ*/ Ċ fun Ġintegration Pass ( states : ĠList < Trajectory State >) Ġ{ ĊĠĠĠ Ġfor Ġ( i Ġin Ġ 1 Ġuntil Ġstates . size ) Ġstates [ i ]. t Ġ+= Ġstates [ i Ġ- Ġ 1 ]. t ĊĊĠĠĠ Ġ// ĠSet Ġthe Ġendpoints ' Ġacceleration Ġand Ġj erk Ġto Ġ 0 Ġto Ġallow Ġthe Ġmotor Ġto ĊĠĠĠ Ġ// Ġstop Ġif Ġvoltage Ġis Ġapplied Ġfor Ġtor que ĊĠĠĠ Ġstates . first (). apply Ġ{ ĊĠĠĠĠĠĠĠ Ġdv Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġdw Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dv Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dw Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ} ĊĠĠĠ Ġstates . last (). apply Ġ{ ĊĠĠĠĠĠĠĠ Ġdv Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġdw Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dv Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dw Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ} Ċ }