diff --git a/PathGen/L1.csv b/PathGen/L1.csv new file mode 100644 index 0000000..1be5147 --- /dev/null +++ b/PathGen/L1.csv @@ -0,0 +1,66 @@ +1.540398999999999852e+00,1.320082000000000200e+00 +1.543049391304347706e+00,1.254956913043478561e+00 +1.545699782608695561e+00,1.189831826086956923e+00 +1.548350173913043193e+00,1.124706739130434840e+00 +1.551000565217391047e+00,1.059581652173913202e+00 +1.553650956521738902e+00,9.944565652173915637e-01 +1.556301347826086534e+00,9.293314782608697033e-01 +1.558951739130434611e+00,8.642063913043479539e-01 +1.561602130434782243e+00,7.990813043478262045e-01 +1.564252521739130319e+00,7.339562173913045662e-01 +1.566902913043478396e+00,6.688311304347828168e-01 +1.569553304347826028e+00,6.037060434782609564e-01 +1.572203695652173661e+00,5.385809565217393180e-01 +1.574854086956521737e+00,4.734558695652175686e-01 +1.577504478260869369e+00,4.083307826086957082e-01 +1.580154869565217002e+00,3.432056956521740143e-01 +1.582805260869565078e+00,2.780806086956522649e-01 +1.585455652173912933e+00,2.129555217391305433e-01 +1.588106043478260787e+00,1.478304347826088772e-01 +1.590756434782608641e+00,8.270534782608701674e-02 +1.593406826086956496e+00,1.758026086956529510e-02 +1.596057217391304128e+00,-4.754482608695641266e-02 +1.598707608695651983e+00,-1.126699130434782870e-01 +1.601357999999999837e+00,-1.777949999999999808e-01 +1.601357999999999837e+00,-1.777949999999999808e-01 +1.538633463414633917e+00,-1.772236097560975498e-01 +1.475908926829267998e+00,-1.766522195121951189e-01 +1.413184390243902078e+00,-1.760808292682926601e-01 +1.350459853658536602e+00,-1.755094390243902291e-01 +1.287735317073170682e+00,-1.749380487804877982e-01 +1.225010780487804762e+00,-1.743666585365853672e-01 +1.162286243902438843e+00,-1.737952682926829084e-01 +1.099561707317073145e+00,-1.732238780487804775e-01 +1.036837170731707225e+00,-1.726524878048780465e-01 +9.741126341463413052e-01,-1.720810975609756155e-01 +9.113880975609753854e-01,-1.715097073170731568e-01 +8.486635609756094656e-01,-1.709383170731706980e-01 +7.859390243902436568e-01,-1.703669268292682670e-01 +7.232144878048777370e-01,-1.697955365853658360e-01 +6.604899512195121503e-01,-1.692241463414634051e-01 +5.977654146341462305e-01,-1.686527560975609741e-01 +5.350408780487803106e-01,-1.680813658536585153e-01 +4.723163414634146684e-01,-1.675099756097561121e-01 +4.095918048780487486e-01,-1.669385853658536534e-01 +3.468672682926828843e-01,-1.663671951219511946e-01 +2.841427317073169645e-01,-1.657958048780487914e-01 +2.214181951219510447e-01,-1.652244146341463327e-01 +1.586936585365851249e-01,-1.646530243902438739e-01 +9.596912195121920508e-02,-1.640816341463414707e-01 +3.324458536585361834e-02,-1.635102439024390120e-01 +-2.947995121951230146e-02,-1.629388536585365810e-01 +-9.220448780487822127e-02,-1.623674634146341500e-01 +-1.549290243902441411e-01,-1.617960731707316913e-01 +-2.176535609756100054e-01,-1.612246829268292603e-01 +-2.803780975609756476e-01,-1.606532926829268293e-01 +-3.431026341463414564e-01,-1.600819024390243983e-01 +-4.058271707317073762e-01,-1.595105121951219673e-01 +-4.685517073170732960e-01,-1.589391219512195086e-01 +-5.312762439024392158e-01,-1.583677317073170776e-01 +-5.940007804878048026e-01,-1.577963414634146189e-01 +-6.567253170731707224e-01,-1.572249512195121879e-01 +-7.194498536585366422e-01,-1.566535609756097569e-01 +-7.821743902439025620e-01,-1.560821707317073259e-01 +-8.448989268292684818e-01,-1.555107804878048949e-01 +-9.076234634146340685e-01,-1.549393902439024362e-01 +-9.703479999999999883e-01,-1.543680000000000052e-01 diff --git a/PathGen/L2.csv b/PathGen/L2.csv new file mode 100644 index 0000000..e8a88b3 --- /dev/null +++ b/PathGen/L2.csv @@ -0,0 +1,102 @@ +-9.703479999999999883e-01,-1.543680000000000052e-01 +-9.092070588235293682e-01,-1.551153970588235453e-01 +-8.480661176470588591e-01,-1.558627941176470577e-01 +-7.869251764705882390e-01,-1.566101911764705978e-01 +-7.257842352941176189e-01,-1.573575882352941102e-01 +-6.646432941176471099e-01,-1.581049852941176503e-01 +-6.035023529411764898e-01,-1.588523823529411627e-01 +-5.423614117647058697e-01,-1.595997794117647028e-01 +-4.812204705882353051e-01,-1.603471764705882152e-01 +-4.200795294117646850e-01,-1.610945735294117553e-01 +-3.589385882352941204e-01,-1.618419705882352955e-01 +-2.977976470588235003e-01,-1.625893676470588356e-01 +-2.366567058823528802e-01,-1.633367647058823202e-01 +-1.755157647058824821e-01,-1.640841617647058881e-01 +-1.143748235294118620e-01,-1.648315588235294282e-01 +-5.323388235294113091e-02,-1.655789558823529406e-01 +7.907058823529378166e-03,-1.663263529411764807e-01 +6.904799999999999827e-02,-1.670737500000000209e-01 +1.301889411764706184e-01,-1.678211470588235610e-01 +1.913298823529411274e-01,-1.685685441176470734e-01 +2.524708235294118586e-01,-1.693159411764705857e-01 +3.136117647058824787e-01,-1.700633382352941259e-01 +3.747527058823529877e-01,-1.708107352941176660e-01 +4.358936470588237189e-01,-1.715581323529411784e-01 +4.970345882352941169e-01,-1.723055294117646907e-01 +5.581755294117645150e-01,-1.730529264705882309e-01 +6.193164705882351351e-01,-1.738003235294117710e-01 +6.804574117647056442e-01,-1.745477205882353111e-01 +7.415983529411763753e-01,-1.752951176470588512e-01 +8.027392941176471064e-01,-1.760425147058823359e-01 +8.638802352941176155e-01,-1.767899117647058760e-01 +9.250211764705880135e-01,-1.775373088235294161e-01 +9.861621176470587447e-01,-1.782847058823529562e-01 +1.047303058823529476e+00,-1.790321029411764686e-01 +1.108443999999999985e+00,-1.797795000000000087e-01 +1.169584941176470272e+00,-1.805268970588235211e-01 +1.230725882352941225e+00,-1.812742941176470612e-01 +1.291866823529411512e+00,-1.820216911764706014e-01 +1.353007764705882243e+00,-1.827690882352941415e-01 +1.414148705882352530e+00,-1.835164852941176539e-01 +1.475289647058823483e+00,-1.842638823529411662e-01 +1.536430588235293992e+00,-1.850112794117647064e-01 +1.597571529411764946e+00,-1.857586764705882465e-01 +1.658712470588235233e+00,-1.865060735294117589e-01 +1.719853411764706186e+00,-1.872534705882353268e-01 +1.780994352941176473e+00,-1.880008676470588391e-01 +1.842135294117647426e+00,-1.887482647058823515e-01 +1.903276235294117713e+00,-1.894956617647058916e-01 +1.964417176470588000e+00,-1.902430588235294040e-01 +2.025558117647058509e+00,-1.909904558823529719e-01 +2.086699058823529018e+00,-1.917378529411764843e-01 +2.147839999999999971e+00,-1.924852499999999966e-01 +2.208980941176470036e+00,-1.932326470588235368e-01 +2.270121882352940990e+00,-1.939800441176470769e-01 +2.331262823529411499e+00,-1.947274411764706170e-01 +2.392403764705882452e+00,-1.954748382352941294e-01 +2.453544705882352961e+00,-1.962222352941176418e-01 +2.514685647058823470e+00,-1.969696323529411819e-01 +2.575826588235293979e+00,-1.977170294117647220e-01 +2.636967529411764932e+00,-1.984644264705882621e-01 +2.698108470588235441e+00,-1.992118235294117745e-01 +2.759249411764705950e+00,-1.999592205882353146e-01 +2.820390352941176015e+00,-2.007066176470588548e-01 +2.881531294117646969e+00,-2.014540147058823671e-01 +2.942672235294117478e+00,-2.022014117647059073e-01 +3.003813176470587987e+00,-2.029488088235294196e-01 +3.064954117647058496e+00,-2.036962058823529598e-01 +3.126095058823529005e+00,-2.044436029411764999e-01 +3.187235999999999958e+00,-2.051910000000000123e-01 +3.187235999999999958e+00,-2.051910000000000123e-01 +3.156340875000000157e+00,-2.596481875000000161e-01 +3.125445749999999911e+00,-3.141053750000000200e-01 +3.094550625000000110e+00,-3.685625624999999683e-01 +3.063655500000000309e+00,-4.230197499999999722e-01 +3.032760375000000064e+00,-4.774769374999999760e-01 +3.001865249999999818e+00,-5.319341250000000354e-01 +2.970970125000000017e+00,-5.863913124999999837e-01 +2.940074999999999772e+00,-6.408484999999999321e-01 +2.909179874999999971e+00,-6.953056874999999915e-01 +2.878284749999999725e+00,-7.497628749999999398e-01 +2.847389624999999924e+00,-8.042200624999999992e-01 +2.816494500000000123e+00,-8.586772499999999475e-01 +2.785599375000000322e+00,-9.131344374999998958e-01 +2.754704250000000076e+00,-9.675916249999999552e-01 +2.723809124999999831e+00,-1.022048812499999793e+00 +2.692914000000000030e+00,-1.076505999999999963e+00 +2.662018875000000229e+00,-1.130963187499999911e+00 +2.631123750000000427e+00,-1.185420375000000082e+00 +2.600228624999999738e+00,-1.239877562499999808e+00 +2.569333499999999937e+00,-1.294334749999999756e+00 +2.538438375000000136e+00,-1.348791937499999927e+00 +2.507543250000000334e+00,-1.403249124999999875e+00 +2.476648125000000089e+00,-1.457706312499999823e+00 +2.445753000000000288e+00,-1.512163499999999994e+00 +2.414857875000000043e+00,-1.566620687499999942e+00 +2.383962750000000241e+00,-1.621077874999999890e+00 +2.353067624999999996e+00,-1.675535062499999839e+00 +2.322172500000000195e+00,-1.729992249999999787e+00 +2.291277374999999950e+00,-1.784449437499999735e+00 +2.260382250000000148e+00,-1.838906624999999684e+00 +2.229487124999999903e+00,-1.893363812499999854e+00 +2.198592000000000102e+00,-1.947820999999999803e+00 diff --git a/PathGen/interpolatedPath.py b/PathGen/interpolatedPath.py new file mode 100644 index 0000000..ac0ed26 --- /dev/null +++ b/PathGen/interpolatedPath.py @@ -0,0 +1,39 @@ +import numpy as np + +def generate_interpolated_points(p1, p2, num_points): + p1 = np.array(p1) + p2 = np.array(p2) + + # Generate evenly spaced values from 0 to 1 + t = np.linspace(0, 1, num_points) + + # Interpolate between p1 and p2 + interpolated_points = p1[np.newaxis, :] * (1 - t)[:, np.newaxis] + p2[np.newaxis, :] * t[:, np.newaxis] + + return interpolated_points.tolist() + +L1 = np.array([[1540.399, 1320.082], [1601.358, -177.795], [-970.348, -154.368]]) / 1000.0 +L2 = np.array([[-970.348, -154.368], [3187.236, -205.191], [2198.592, -1947.821]]) / 1000.0 + +def main(path, V): + interpolated_path = [] + + for i, p in enumerate(path): + if i == 0: + continue + N = int(np.linalg.norm(path[i-1] - path[i]) / V) + points = generate_interpolated_points(path[i-1], path[i], N) + interpolated_path.extend(points) + return interpolated_path + +if __name__=="__main__": + dt = 0.03 # s + v = 2 # m/s + path = main(L2, v * dt) + + np.savetxt('L2.csv', path, delimiter=',') + + print(len(path)) + + # from pprint import pprint + # pprint(path) \ No newline at end of file