From b248725ea0f4e127ee3cbe97a53af8e46916293a Mon Sep 17 00:00:00 2001 From: Chi Chi Date: Wed, 8 Aug 2018 10:33:29 -0400 Subject: [PATCH] First Commit --- .classpath | 6 ++ .project | 17 +++++ .settings/org.eclipse.jdt.core.prefs | 11 +++ bin/Main/CalculatePath.class | Bin 0 -> 2751 bytes bin/Main/Pathfinder.class | Bin 0 -> 621 bytes bin/Main/Trajectory$Config.class | Bin 0 -> 558 bytes bin/Main/Trajectory$Segment.class | Bin 0 -> 734 bytes bin/Main/Trajectory.class | Bin 0 -> 786 bytes bin/Main/Waypoint.class | Bin 0 -> 404 bytes bin/PartTypes/Acceleration.class | Bin 0 -> 1087 bytes bin/PartTypes/Constant.class | Bin 0 -> 269 bytes bin/PartTypes/Jerk.class | Bin 0 -> 1501 bytes src/Main/CalculatePath.java | 110 +++++++++++++++++++++++++++ src/Main/Pathfinder.java | 7 ++ src/Main/Trajectory.java | 56 ++++++++++++++ src/Main/Waypoint.java | 12 +++ src/PartTypes/Acceleration.java | 48 ++++++++++++ src/PartTypes/Constant.java | 5 ++ src/PartTypes/Jerk.java | 67 ++++++++++++++++ 19 files changed, 339 insertions(+) create mode 100644 .classpath create mode 100644 .project create mode 100644 .settings/org.eclipse.jdt.core.prefs create mode 100644 bin/Main/CalculatePath.class create mode 100644 bin/Main/Pathfinder.class create mode 100644 bin/Main/Trajectory$Config.class create mode 100644 bin/Main/Trajectory$Segment.class create mode 100644 bin/Main/Trajectory.class create mode 100644 bin/Main/Waypoint.class create mode 100644 bin/PartTypes/Acceleration.class create mode 100644 bin/PartTypes/Constant.class create mode 100644 bin/PartTypes/Jerk.class create mode 100644 src/Main/CalculatePath.java create mode 100644 src/Main/Pathfinder.java create mode 100644 src/Main/Trajectory.java create mode 100644 src/Main/Waypoint.java create mode 100644 src/PartTypes/Acceleration.java create mode 100644 src/PartTypes/Constant.java create mode 100644 src/PartTypes/Jerk.java diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..5bc4564 --- /dev/null +++ b/.classpath @@ -0,0 +1,6 @@ + + + + + + diff --git a/.project b/.project new file mode 100644 index 0000000..9f89c2c --- /dev/null +++ b/.project @@ -0,0 +1,17 @@ + + + MyPathfinder + + + + + + org.eclipse.jdt.core.javabuilder + + + + + + org.eclipse.jdt.core.javanature + + diff --git a/.settings/org.eclipse.jdt.core.prefs b/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 0000000..8000cd6 --- /dev/null +++ b/.settings/org.eclipse.jdt.core.prefs @@ -0,0 +1,11 @@ +eclipse.preferences.version=1 +org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled +org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6 +org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve +org.eclipse.jdt.core.compiler.compliance=1.6 +org.eclipse.jdt.core.compiler.debug.lineNumber=generate +org.eclipse.jdt.core.compiler.debug.localVariable=generate +org.eclipse.jdt.core.compiler.debug.sourceFile=generate +org.eclipse.jdt.core.compiler.problem.assertIdentifier=error +org.eclipse.jdt.core.compiler.problem.enumIdentifier=error +org.eclipse.jdt.core.compiler.source=1.6 diff --git a/bin/Main/CalculatePath.class b/bin/Main/CalculatePath.class new file mode 100644 index 0000000000000000000000000000000000000000..1f0281f85ccc7f561eaff08c95f981ecce455383 GIT binary patch literal 2751 zcmc&$OLG)e6#jainWlL-1_+Nx6J9eDWK2K>0tk7aU=o58f`AV?JvWmkGu>lPPe}Hz zSh$qJDtB6xJ4v*lq>Ouif&avX<>R}zdon|+tcs;oW-;gXJ>Na&+;hHjZhrpbhcy6$ zXrvL-ke#p`w>V-|?Pk>q#FQ1xr;*UG6|0t8DPEhsC+t8&s_r>%(4g+lxH@&y zT2@;_8oC;yQWHwMH_}e~mOSVA%ZE#$lDL%ZxpPiMLr-X;12m#E#3bsLPj}iCn!O*n z(2!mlnP)VDicx+ox4x2%s#oW3SJ`Os3~YfRSAzzUNXgYH0~zQ# zS8o`26Q?!oVfZ0`hDC5+2)D%yMw`f8$IATRlrhTc}h zZQwe8WIi)3O3?P!kWI~2rMI}v+O@fY3?zK^^(+H)T(niu+NE+ydZz5m%|so& z3A8dk%Gt14?B)acC#L1#*}P?m{{LexExUoWWC}?%lY9!i1>u@~xxVA(*ixP4X-Jld zr8>tvsb|dv!Z+oN3Xyu|-x|5PbC5c)K=CqQfD}`8`@j10*XKBMYXw73asFGv<0`T~mE>+3*vZu{UTHV4*@JHE;cpL2>{IRwp$lA8 z#i27?;;lGFu^+=c6_eu|BV5HXO6y&S{f=%;OA*DFEeur~U~%O=*Y>VrYy|@=7)#t8 z{0?u2Yq_edV!T5$xvrU{=K6|MUiVf}THn9XfxhwZ3tU`5=^5T{>mNQs@A_9{PkEgr zI-7Wgk5*7j+-33}<5=fsyhGKwFAEfAZ@_JH+(yL5nI~pj(+f{WvL=N(Pq?i@9>^07|LzW;cl1vGW6%cbTE<= zhEjWp)?&yFg%Umw2x`@z;(|v$d-0?Stk~`NJXGWPvt!#+139!B%L&Us3F|t_Bp~9j zx$8__Y8%dIVgm)rEU1!(4EeV?8R;u7^V0&Hh>@YXOu5j)U_5wJ-w{_boWGDP^)%ck zgL$oJLfhq&iJ0ivXQ)IH6BYATIt~|?Npd719vU^!8R&+9Mn76Vy*ha| zls4!M(;&bKy=GiMc9pz5)=;2G2tKD~8rf{~d{yfk!8QvE;!bf>5rnD>rCXWCu}+N%q)~L~C%Gts3qQb* z63?^+K}=xIea*e+T;}Wh;}gIhwj87c3unrNjjLGow4X%r%XTvgZ%x-hMo{djC)Eg4 z*lk>NY+jJM;UUKY_dz{hKj|RyP4dE0>nh*ZfsR#TqEIktLp>ee3$lkMG|7>`sU7pH zw*{GIbffb~W5z`u1wp!2Z|5K=xtKuKTC*;kku~pPa%3&K$RR5zv`nbaA0Ik8zEYh) z3(Bp?S3z6F#`@!6Cb=_xL8bM-$^$|EGJ1@CePV2n!tWk-t#-;h4MQC_1J&>AzJm?c z{AopXqm^NkoYP!whDZstD0qydKlDj{h#ZV5=3RQPypntHY T1gtQg!YbB?dDaWoDJb^?V;N$d literal 0 HcmV?d00001 diff --git a/bin/Main/Trajectory$Segment.class b/bin/Main/Trajectory$Segment.class new file mode 100644 index 0000000000000000000000000000000000000000..12121c684fa2a1bcb1e53fc81065bed085f8bfa8 GIT binary patch literal 734 zcmZ`%O>fgc6r4@mI3F9_mO|a~)k2|(DG3nZ0^-sN2`Rv#RZd(s&MMvD+Q@b*`m;D# zNF4Y9-1tL?SvwLE5|(yn-rKj&JG1-q_xB$Fo?_slCb0QVnRs}Ts)Y{oB)!}_*7GwR z=Pn$9)rGoH!$`&R;fJX$5U9;Ki6f3L9G3#k&q-!-lfdX>PV-mT5%RS z{UqR?YBe+QT%hsF#3p|&(CHm5zy3tPIZS4{jXKsPuHdRbz1N?#AaG4$1r5tKCESW_ zNi-{VL!wo&TN3Sx-I3^2Y*#{7?4Cpy4FPX#V*UR7Y^u`}HI1~u`Zx(yG*PLs@iOV; zrzR8F8vnSLf9< zi4;r5#U)Z`8B=D76j>rAmPmmmQeKII_IcdD%IFunt`SGUH>?Hw1vY|yf$M?4Qos*< zx4?Gbw+h@0{7!+}f!{5#6Zl?%yI;AjhCR*!eFYxD!GK;5+5Ry$@C4g@Njn(f0iNLi z&l$bIeO7*C6@SV6KG8BONtDcrvO-7p{+PGS3UfaxK5d~^13Cn0zICo$Eb!r O&jp^0PZ1A!y4D|_LUO$T literal 0 HcmV?d00001 diff --git a/bin/Main/Trajectory.class b/bin/Main/Trajectory.class new file mode 100644 index 0000000000000000000000000000000000000000..1a95a11b8e1305cf4a601d5ec53c84ee57024f97 GIT binary patch literal 786 zcmZ{hUuzRV6vfXZ+e{O8n>MYfCjDy#lNOBhwc<;l5QzHFk{6#Q>2y0~I}5v8(eLDg zg^D2958#Iq&;GH7#j@rVunVR)t+sN#_e z30I)n>rd(sSaPv|8l8=e)$gvaraC%R(?AQf#^Fo_6BU^>FE*Xz!qC)8*%Q4uy-|N6 z@mOH}0mJkgXzM2z0%Al9zGkZ%l$B|79WEGqraxtVDQTR9*U?NLo77)%MiPts)|nln!Ut&Vf9HGf z4c-m23Yz>bWy%f}UDj@3nS1TL8Y^fqqb{SSbcxl1IVzZ5#_ayY+JCTH#BRgI&K%ae z$9lYIoj8Ffcv@hO3as3}h1Wg&%G-0FH5LGSb0B#SlIRqmHoM4se#JFa)KbvhmlEei e#&PqCf>UAl>1~_X;@V=g>JOWsU1I)c*jEDv6W; literal 0 HcmV?d00001 diff --git a/bin/Main/Waypoint.class b/bin/Main/Waypoint.class new file mode 100644 index 0000000000000000000000000000000000000000..7cd26bac4544a720d1c47684aec578604a6a600c GIT binary patch literal 404 zcmY*U!A`cPYA&hFbcv-A1&{sG_^`yMQX@KT$sf2Wu8++?K(M`3fKpLKt#vxolmXp)Xg1@*#6 z7)ym3AEv2-dulRMo)PqtB#DL!&Ut>H)?p(I;A2C3@e#`DmVG``^7M3iX?OQH;|IBVWTmBS+!^2UN6a zR)PmGL*!SL;;gj9Q|rv@>}K5bSljU%g7~mPBaSL;#obzk?N>fnXft<6CPWS$_~MiV hEfRD%jX5O&cF<)NB*JQiL|RU1i*wEucG+3j`v&keJ>viX literal 0 HcmV?d00001 diff --git a/bin/PartTypes/Acceleration.class b/bin/PartTypes/Acceleration.class new file mode 100644 index 0000000000000000000000000000000000000000..69470415f18d047058ff039f86b842cd2e2cede4 GIT binary patch literal 1087 zcmZuvU2hUm5IvU#7UdxS4~A2O7y^?JRr#Vx0{>v5UT^^O-eU1!hgZ|E(C=%(8fDJV#5NMe#f zsp_pHGMHA8C9Ic+YM4Qs&>rwNod8SNLke@aq9XTCBXxJwwS|T}3Jepy;MU^yh=5bE zdJ+ut8dSukc2z^-LR-)flSQT)mLr~z4!XkI<&=nFy5XAKZgJ0&_s|&iUs*&?zHvFb z8^jXP<@sYQX~OA^U30yV(2nnRx}sycN5T_5Vw0#RzGU*t>7eB4rq9j67C#K(%=Lvo zKHZ>U!WXUTFKL3QraSUX@x+q-WXB(_$_=2YKXV-6ZQ6V|6hjqvh=>1b6RwLn;yUrB z(1OW7QAQGy&fp|6>5R~eBvK?N0CtbODYBjzU!dug)AvXh)H6)2Cq7^z&|a@6boLpu zr|+<0C#AM7w6JK=Rj3$E`V|?>PiNDC(w|5~;4eq3SefzT3S?l}O?r(qp^b3IJamPZR zK<2Itqs^nHYR$i`!n%dHKz2_a$oYo!YxD2!>?t=An2>JNmW|4;=W~{>s~Relkr(&^ zVvR0UuN6t(RTVclP@$>{q$6)%ReYtYdNqC;!1KMxV~7LQ2wX2Z61dP4y1SoT)OVK! zF6YRh)=)R4oaf#8J!J$Bcc@@@@EB$9q)3|Ek zN)LZ)L3^j6Y+S>2GEz0GE!k)*lj2$m0y!J~NNQBDVRg_A8;K6OX(QD^^EUHhx{&mJB_;KHZE*_q`T#S??8iL141(2Zw&;Oo?7GFRBMk@~f)n zNgTIH-u1d^^%Pr&&h{jSbV<{GGA3f5rIS&Hj58`3&o)mvTrjV;; zKKmSl6X_>-s$@S#+MvUd%@fBCN*O2NWa^n*-N}q%>_<3tH{;l5HDy+%tdn(855K@i zjwQaw<%f^3RCF@Om?=8hW8{m4V-$-{;uy1>bHyi^VF>e;x?Y?`J;vN)+&;(C>iC@F zQZq3vdVa@xVtAJG7T*&via5qt@i<1{5KUl;uVx09@Ft$YU0lI?xQY+>BzAEfb)I~P z96muFpQ3ZBr)E s1-dUXsU;@3j7f%_#tO+ distance) { + if(accelJerkUp.getJerkValues()[2] == config.max_acceleration) { + acceleration = new Acceleration(totalAccelDist, totalAccelDist, config); + }else { + + } + }else { + + + + } + + + if(points[waypoint + 2] == null) { + if(distance > remainingDistance) { + AccelSeconds = (segments[i].velocity - difVelocity) / config.max_acceleration; + AccelDist = remainingDistance; + }else if(distance == remainingDistance){ + AccelSeconds = 0; + AccelDist = 0; + }else { + JerkSeconds = 0; + JerkDist = 0; + AccelSeconds = (segments[i].velocity - 0) / config.max_acceleration; + AccelDist = distance; + } + //segTime[portion] = + }else { + if() { + + }else if(){ + + }else { + + } + } + + + }else { //curve which is part of circle and two clothoids + + } + } +} diff --git a/src/Main/Pathfinder.java b/src/Main/Pathfinder.java new file mode 100644 index 0000000..611526d --- /dev/null +++ b/src/Main/Pathfinder.java @@ -0,0 +1,7 @@ +package Main; + +public class Pathfinder { + public static Trajectory generate(Waypoint[] waypoints, Trajectory.Config config) { + return CalculatePath.generateTrajectory(waypoints, config); + } +} diff --git a/src/Main/Trajectory.java b/src/Main/Trajectory.java new file mode 100644 index 0000000..8c89580 --- /dev/null +++ b/src/Main/Trajectory.java @@ -0,0 +1,56 @@ +package Main; + +public class Trajectory { + + public static class Config { + + public double dt, max_velocity, max_acceleration, max_jerk; + + /** + * Create a Trajectory Configuration + * @param dt The time delta between points (in seconds) + * @param max_velocity The maximum velocity the body is capable of traveling at (in meters per second) + * @param max_acceleration The maximum acceleration to use (in meters per second per second) + * @param max_jerk The maximum jerk (acceleration per second) to use + */ + public Config(double dt, double max_velocity, double max_acceleration, double max_jerk) { + this.dt = dt; + this.max_velocity = max_velocity; + this.max_acceleration = max_acceleration; + this.max_jerk = max_jerk; + } + } + + public Segment[] segments; + + public Trajectory(Segment[] segments) { + this.segments = segments; + } + + public Trajectory(int length) { + this.segments = new Segment[length]; + } + + public Segment get(int index) { + return segments[index]; + } + + public int length() { + return segments.length; + } + + public static class Segment { + public double dt, x, y, position, velocity, acceleration, jerk, heading; + + public Segment(double dt, double x, double y, double position, double velocity, double acceleration, double jerk, double heading) { + this.dt = dt; + this.x = x; + this.y = y; + this.position = position; + this.velocity = velocity; + this.acceleration = acceleration; + this.jerk = jerk; + this.heading = heading; + } + } +} diff --git a/src/Main/Waypoint.java b/src/Main/Waypoint.java new file mode 100644 index 0000000..525233c --- /dev/null +++ b/src/Main/Waypoint.java @@ -0,0 +1,12 @@ +package Main; + +public class Waypoint { + + double x, y, angle; + + public Waypoint(double x, double y, double angle) { + this.x = x; + this.y = y; + this.angle = angle; + } +} diff --git a/src/PartTypes/Acceleration.java b/src/PartTypes/Acceleration.java new file mode 100644 index 0000000..ece3da3 --- /dev/null +++ b/src/PartTypes/Acceleration.java @@ -0,0 +1,48 @@ +package PartTypes; + +import Main.Trajectory; + +public class Acceleration { + + double accelSeconds, accelDistance, final_velocity; + + /** + * Create Acceleration + * @param start_velocity The velocity before the constant acceleration (in used length units per second) + * @param end_velocity The velocity before the constant acceleration (in used length units per second) leave "null" if unknown + * @param distance_to_be_covered The distance covered by the acceleration (in used length units) leave "null" if unknown + * @param config The configuration for the trajectory/path + */ + public Acceleration(double start_velocity, double end_velocity, double distance_to_be_covered, Trajectory.Config config) { + double max_acceleration; + if(end_velocity > start_velocity) { + max_acceleration = config.max_acceleration; + }else { + max_acceleration = -config.max_acceleration; + } + + if(end_velocity == (Double)null) { + accelDistance = distance_to_be_covered; + final_velocity = (((accelDistance - (1/2) * config.max_acceleration) / start_velocity) * max_acceleration) + start_velocity; + accelSeconds = (final_velocity - start_velocity) / max_acceleration; + } + + if(distance_to_be_covered == (Double)null) { + accelSeconds = (end_velocity - start_velocity) / max_acceleration; + accelDistance = start_velocity * accelSeconds + (1/2) * config.max_acceleration; + final_velocity = end_velocity; + } + } + /** + * + * @return

accelSeconds [0] The time (in seconds) needed to accelerate + *

accelDistance [1] The distance covered (in used length units) during this constant acceleration + */ + public double[] getAccelerationValues() { + return new double[] { + accelSeconds, + accelDistance, + final_velocity + }; + } +} diff --git a/src/PartTypes/Constant.java b/src/PartTypes/Constant.java new file mode 100644 index 0000000..ab793ac --- /dev/null +++ b/src/PartTypes/Constant.java @@ -0,0 +1,5 @@ +package PartTypes; + +public class Constant { + +} diff --git a/src/PartTypes/Jerk.java b/src/PartTypes/Jerk.java new file mode 100644 index 0000000..29486fc --- /dev/null +++ b/src/PartTypes/Jerk.java @@ -0,0 +1,67 @@ +package PartTypes; + +import java.math.MathContext; + +import Main.Trajectory; + +public class Jerk { + + double actual_final_acceleration, distance_covered, time_needed, actual_initial_velocity, actual_final_velocity; + + /** + * Create Jerk + * @param known_initial_acceleration The initial acceleration of the jerk (in used length units per second squared) + * @param expected_final_acceleration The wanted acceleration after the jerk (in used length units per second squared) + * @param known_initial_velocity The initial velocity of the jerk (in used length units per second) leave "null" if unknown + * @param known_final_velocity The final velocity of the jerk (in used length units per second) leave "null" if unknown + * @param part_remaining_distance The distance left of the part at the beginning of the jerk (in used length units) + * @param config The configuration for the trajectory/path + */ + public Jerk(double known_initial_acceleration, double expected_final_acceleration, double known_initial_velocity, double known_final_velocity, double part_remaining_distance, Trajectory.Config config) { + double max_jerk; + if(expected_final_acceleration - known_initial_acceleration < 0) { + max_jerk = config.max_jerk; + }else { + max_jerk = -config.max_jerk; + } + + double test_jerk_time = (expected_final_acceleration - known_initial_acceleration) / max_jerk; + double test_jerk_distance = known_initial_velocity * test_jerk_time + (1/2) * known_initial_acceleration * Math.pow(test_jerk_time, 2) + (1/6) * max_jerk * Math.pow(test_jerk_time, 3); + + if(test_jerk_distance > part_remaining_distance) { + //time_needed = ; + }else { + if(known_final_velocity == (Double)null) { + time_needed = test_jerk_time; + distance_covered = test_jerk_distance; + actual_final_acceleration = expected_final_acceleration; + actual_initial_velocity = known_initial_velocity; + actual_final_velocity = known_initial_velocity + known_initial_acceleration * time_needed + (1/2) * config.max_jerk * Math.pow(time_needed, 2); + }else { + time_needed = test_jerk_time; + distance_covered = test_jerk_distance; + actual_final_acceleration = expected_final_acceleration; + actual_initial_velocity = known_final_velocity -(known_initial_acceleration * time_needed + (1/2) * config.max_jerk * Math.pow(time_needed, 2)); + actual_final_velocity = known_final_velocity; + } + } + } + + /** + * + * @return

time_needed [0] The time required for the jerk portion + *

distance_covered [1] The length of the path traversed during the jerk portion + *

acutal_final_acceleration [2] The acceleration after the jerk portion + *

actual_initial_velocity [3] The velocity before the jerk portion + *

actual_final_velocity [4] The velocity after the jerk portion + */ + public double[] getJerkValues() { + return new double[] { + time_needed, + distance_covered, + actual_final_acceleration, + actual_initial_velocity, + actual_final_velocity + }; + } +}