interpolated traj

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

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

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)