Merge branch 'main' of https://github.com/StanfordASL/trunk-stack

StanfordASL · Oct 29, 2024 · 68ce68e · 68ce68e
2 parents 7ed5364 + b5e8493
commit 68ce68e
Show file tree

Hide file tree

Showing 2 changed files with 157 additions and 3 deletions.
diff --git a/stack/main/data/trajectories/steady_state/control_inputs_circle_seed0.csv b/stack/main/data/trajectories/steady_state/control_inputs_circle_seed0.csv
@@ -0,0 +1,121 @@
+ID,u1,u2,u3,u4,u5,u6
+0,0.4048813503927325,0.1715189366372419,-0.20623001333894525,0.1294883182996897,-0.26744214120144116,0.014589411306665613
+1,0.39149264121032407,0.21594337775795303,-0.1822002088680435,0.08962278720199118,-0.21321923239929966,-0.03962790228365572
+2,0.39776581203653993,0.2440889888965039,-0.2809332634657386,0.03512267320192259,-0.2702076163097129,-0.05129106489705593
+3,0.4075747184823376,0.26101037949936823,-0.1969483665859198,0.08059095464319577,-0.20340129113915761,-0.09757776718013396
+4,0.32608135172885866,0.2579429930696024,-0.2844744061955993,0.06883317580464501,-0.17242319401828415,-0.17381868030829892
+5,0.3210771937679851,0.2885519250702908,-0.25437512212600066,0.004624785129592129,-0.1958094662718991,-0.19130277042794827
+6,0.3322942274446276,0.2870409301217697,-0.203963279556258,-0.01059633596882235,-0.13314470727033545,-0.24484374803546638
+7,0.3136727799379534,0.24246925436153452,-0.22719557816202424,-0.03794826465459017,-0.11811489265964029,-0.3084321047423611
+8,0.24494738671843383,0.2806714054063265,-0.22962356492809127,-0.08676227748089511,-0.009363275601090924,-0.34082382686765256
+9,0.20080263969524625,0.2649503608978004,-0.2114221391972672,-0.12999016145017064,-0.02995765603448711,-0.35287855116310735
+10,0.15971730649761207,0.26099026071558185,-0.19862076740345144,-0.16500460944793555,-0.025017310092380885,-0.36303271674668813
+11,0.1876289931741218,0.2573976271943035,-0.16555240987527972,-0.19125703168810465,0.08482325911227126,-0.3716596709833951
+12,0.16315288775488218,0.3025338862848206,-0.15825503692144832,-0.21728267919825933,0.04688963393270833,-0.4209491592234853
+13,0.053729509387060295,0.24497490524633025,-0.1811533831582292,-0.1981699639858426,0.055677945695482484,-0.3738264379000064
+14,0.038574587641890756,0.2474862211927016,-0.13955089410167298,-0.24644881595151596,0.13638741232683146,-0.3557951830546786
+15,-0.02879131332997751,0.27246350798506097,-0.1560124031932171,-0.19812446827149777,0.13580366531136834,-0.43153910824866937
+16,-0.044438917432480435,0.1896109847308915,-0.06226418136416768,-0.28071202596179423,0.2009376818377404,-0.41946033508238645
+17,-0.07141127773618638,0.26300473919993,-0.0951706845349154,-0.2323708454328168,0.21657796244230354,-0.3813467667267967
+18,-0.1633873831831502,0.2387901697191672,-0.04942762646980593,-0.21143157308861865,0.2491898103351781,-0.39653641158290004
+19,-0.14354314621329445,0.15770262121380235,0.02037611563428145,-0.2412429154587145,0.28668613034464946,-0.3404716930607111
+20,-0.18964293526593293,0.14081716703924563,0.05448105526864057,-0.23544348114578467,0.25735306337830116,-0.32845444974307597
+21,-0.2950905261475434,0.0918808780389048,0.05139641027533953,-0.26847622332504506,0.29022187747196226,-0.32174375222648044
+22,-0.31550520203174887,0.06132442576863478,0.06848105347875628,-0.23323409846978857,0.2961510218648083,-0.27126261413361274
+23,-0.2927232663717936,0.043487449085133995,0.12654808321622052,-0.24765698013746737,0.28933185597734584,-0.21610533795636094
+24,-0.3027061687283974,0.039013440436607,0.07134043637423781,-0.18190070346496998,0.3208454467348786,-0.1712353254876908
+25,-0.38999599955682923,0.023505140403224628,0.10073009056142833,-0.19919250789109444,0.2620867052322911,-0.14963617764750703
+26,-0.34180257510843426,-0.03817865998868163,0.1468384551003013,-0.2383433999424533,0.3163352003435448,-0.15030996183332215
+27,-0.36370158757877163,-0.03972089640275635,0.22354270075893432,-0.14197902546801847,0.24129083799713144,-0.11924321924317213
+28,-0.37190828564613204,-0.13960137697187316,0.19597059250331664,-0.20241300259806305,0.2503590195548461,-0.11177321896156744
+29,-0.3700633079349627,-0.16356740890808108,0.1940818917733232,-0.0935376464775321,0.2878641610893664,-0.06810497698374073
+30,-0.4329636626209135,-0.151467090838933,0.23057512627459434,-0.13951061595764847,0.2948774682368389,0.032685465533461
+31,-0.391344061586042,-0.1804251640829289,0.25664500883934865,-0.10777578770038758,0.22246242448109368,0.034609773305140965
+32,-0.4172878469377728,-0.168633992650342,0.26572127082045477,-0.06458672663459275,0.18393012310770726,0.08067667386804771
+33,-0.4167315549485479,-0.24087045682974825,0.22833985739031729,-0.017939595993571707,0.17511933438248997,0.11362459785709225
+34,-0.4024729571834956,-0.29617319403704956,0.26769319980597917,-0.015719869989059658,0.16503487762342153,0.22410412988376444
+35,-0.2842916728933356,-0.29692905936747727,0.26582558429331626,-0.00814727725747649,0.08564639711818245,0.2469578915280914
+36,-0.32593187828971587,-0.2929494111535185,0.25528618089767996,0.07505951680839937,0.11717029271645224,0.29256273900266744
+37,-0.30169840497243045,-0.2609809661222706,0.20916234191380395,0.11319266000805403,0.09253030252913763,0.2581334638306818
+38,-0.20227278692958434,-0.27389520017829155,0.2074111097362344,0.06443402804628957,0.044274939224351655,0.2671868941265102
+39,-0.24142135570378612,-0.3073424544073474,0.20822329680215865,0.11359481647529375,-0.011589899214944905,0.3477722058016852
+40,-0.1385373675912191,-0.3374910576705571,0.20883173918138168,0.10325018324210175,0.00040492322705780806,0.3493308223492171
+41,-0.12915341526136132,-0.32695894478146986,0.15551967827052754,0.16006835187111923,-0.06740692341288675,0.42032330119369465
+42,-0.06839815456774952,-0.2631175326644659,0.21318344840385983,0.13899714745102357,-0.07883595167205266,0.39701956420146084
+43,-0.006896573058766016,-0.2982031356180038,0.12671290145664266,0.15802958598666814,-0.16254685818393727,0.4394140640808219
+44,0.006343446083980941,-0.2351933352177114,0.10889222392260195,0.22174307494254322,-0.18663510070828937,0.3984210040428433
+45,0.07966395619567775,-0.20936091590117312,0.09114319749302935,0.27194640886978066,-0.19708067669089102,0.44365668505386824
+46,0.11825326010549539,-0.19759525738953237,0.09576412373718277,0.2733472411215828,-0.2162400128663724,0.42796800536830026
+47,0.1272644481276006,-0.25411806433253975,0.04189746652244506,0.20818853135341311,-0.1704347348517088,0.38080584604880113
+48,0.15526880673919433,-0.16709299533312885,0.01783955445540512,0.211762943514391,-0.20700450743076748,0.3375095823384411
+49,0.19495224638770545,-0.18215578260188248,-0.03577850676758483,0.28617977390812777,-0.20981252271766143,0.39598869961908467
+50,0.27057051400788046,-0.09897845277511493,-0.0388427625639003,0.20712631325655034,-0.2813554489511876,0.3005445245803621
+51,0.22665331510181352,-0.14140419641387028,-0.02594111308478645,0.19639301945589005,-0.2569138155135774,0.2657229724824931
+52,0.25186186860390225,-0.107054158515827,-0.057089509903795244,0.21324231031745755,-0.3005927529798993,0.27706161364023835
+53,0.35713977255526463,-0.011829612783872224,-0.12221345724922332,0.19260455529102452,-0.3424682728471447,0.21870680686349508
+54,0.32083344748149023,-0.00704827502867926,-0.1047455717081774,0.23716967466961006,-0.32163630175896996,0.191059015725548
+55,0.33236900531565555,0.05809409215027586,-0.1898218808510853,0.2229921332865405,-0.2707021423296944,0.1930256304922266
+56,0.35570129982188803,0.04819143165908426,-0.15753105928407,0.16395649413801872,-0.30668696104924653,0.09533611282734353
+57,0.3869469443027474,0.04615292200353361,-0.15521162492105933,0.12563674892136156,-0.2837607282594801,0.0652340594063694
+58,0.4054882149671924,0.16747769646663857,-0.18743608906070464,0.10084130830867011,-0.2812395379509696,0.04351907949077126
+59,0.40361774947034523,0.16813925106038377,-0.23874674117293299,0.08788605654663215,-0.27054005348062216,0.04564057227959498
+60,0.3687130891750845,0.1903983954928237,-0.21212575593837696,0.12069114216457671,-0.2216034801123426,-0.004139603823141316
+61,0.42015068374520914,0.11143689455360627,-0.16158867421620926,0.16630756675224376,-0.2543175764767241,0.02528943441465813
+62,0.41663922020083555,0.10529779742283633,-0.2111891860401502,0.13399498647122393,-0.2560050956908995,0.13046970975537203
+63,0.37557292611335685,0.06990853439819661,-0.13051875838435342,0.18239664923095772,-0.24858479328776176,0.13320580101326682
+64,0.3963306212120968,0.07012391276089783,-0.11395430661755246,0.16923385545893735,-0.2856818380176347,0.155128300169319
+65,0.300892927759857,-0.01025910651974303,-0.1472052985740047,0.2525109609485754,-0.346657723762517,0.24896459232188334
+66,0.30662154001957825,-0.04886335001475076,-0.147613397504724,0.19797950788553015,-0.3078797548089036,0.28147608514567224
+67,0.25387115336735383,-0.02148430859177959,-0.037345437584523467,0.23412182902292825,-0.30997661147108313,0.24459914743575234
+68,0.27485102227370267,-0.14343945544500225,-0.09693994306116151,0.23812984584321073,-0.327164482966383,0.2762224067670649
+69,0.20203105562037094,-0.15106406667269362,-0.059051754645238715,0.2002197965475161,-0.31053113637869356,0.3391691363145426
+70,0.24124596941486487,-0.10555298534326701,-0.00465754567968207,0.21625606477061998,-0.24322829588332154,0.3489357684676584
+71,0.2044706479177712,-0.2245098262578232,0.05930453767382468,0.2269127660789805,-0.2650744187425347,0.3847752483014329
+72,0.09008309743494465,-0.18885583835251094,0.05890496644945198,0.23578334410218155,-0.21314889296739223,0.36167396733676527
+73,0.09477296026214342,-0.23602982919141516,0.06877388609909288,0.2687841359146619,-0.15304535012183287,0.3476743809257381
+74,0.005177141504209611,-0.26215950348357586,0.1388239502463695,0.27433853617996057,-0.12335366887097629,0.4392196373672928
+75,-0.03099258206870708,-0.21450750207857777,0.10566121745285216,0.17369267381213377,-0.09793536621484808,0.37320026999619066
+76,-0.034113384405713876,-0.32158225239919985,0.17570093337784562,0.23617221299350233,-0.13694613159648844,0.3843668220093457
+77,-0.10750106161635445,-0.26310506889550733,0.14659219868317036,0.14784021381158602,-0.08912603379551952,0.3690187980436159
+78,-0.10177663422134697,-0.32151274580131384,0.19127384808511597,0.20552108377601683,-0.039568427423416236,0.4130090472146598
+79,-0.1309019592480655,-0.27416775707812524,0.22705133448844542,0.18683992926800913,0.028159034072566383,0.3549070203354637
+80,-0.2550813854294762,-0.32053159813844617,0.18604176933750913,0.10898591151841386,-0.026223836997092492,0.34950555155795654
+81,-0.2587290400626914,-0.2574421924264863,0.278796412656582,0.09779132047648359,0.01993722481804499,0.3238436641972407
+82,-0.23937649263155306,-0.30011231492995216,0.2458065751524058,0.045078771071474164,0.06932588983535881,0.27675404965728584
+83,-0.3330922468636655,-0.2807091621881775,0.2274925521403005,0.02925815767285507,0.1067691525827638,0.2803571757508267
+84,-0.34997482105387856,-0.22620288114524828,0.2857496186668964,-0.029610485333638702,0.10894554959729186,0.21573357741039742
+85,-0.3944780411483813,-0.2680505582800247,0.27376367501789073,0.006961893669618401,0.1736153993691498,0.20548984085196478
+86,-0.4020421022849653,-0.2501423078990871,0.26478451741865333,0.0003495314280150541,0.19171195583605138,0.12393419519568244
+87,-0.4328836217252082,-0.19203754875131404,0.19255460166637006,-0.0763647005142499,0.21424238176665314,0.14745217186772774
+88,-0.4359578873852478,-0.19882192952155764,0.18606412585868748,-0.10470918881418874,0.244863424143464,0.04153219323048858
+89,-0.4284580833051413,-0.2020446076562,0.23056629181621174,-0.0937728975046307,0.2686517825039418,0.06617497200892214
+90,-0.44916275688135454,-0.12124021232471548,0.21958594741339688,-0.12747181482706238,0.31369975288103735,-0.06960170259986753
+91,-0.3752590988183928,-0.07539647972783088,0.19484765154506212,-0.1744503144723075,0.3094433833879466,-0.10390110584438639
+92,-0.3917035837528139,-0.07436389153853544,0.1953861913521005,-0.21847078650403862,0.26289537973241733,-0.11421086942199596
+93,-0.3602050036789223,-0.006392655728399213,0.168002742201839,-0.23191395239447365,0.3446503363078167,-0.10848565662557974
+94,-0.3546687539080962,0.0289273328460665,0.13861887646950444,-0.16841785753056254,0.33269444380768126,-0.13760835955030074
+95,-0.29134728655519015,-0.027772036174572693,0.0787949480591017,-0.23493549712931877,0.3446355865095095,-0.2411171341968845
+96,-0.3418868289832024,0.08897607215039259,0.08153286517943449,-0.1936256077068244,0.2617684100827975,-0.2391859223071182
+97,-0.2850262331710447,0.03914487007606025,0.08113087687387985,-0.2675814990406905,0.24268004923780218,-0.3305882662943688
+98,-0.290901992271798,0.15249669211995381,0.059367448308981136,-0.20767774832471597,0.29567856570173023,-0.3117266294179687
+99,-0.23172372681315917,0.19048095397944276,-0.004908402309742885,-0.25277760614735745,0.2527920760148294,-0.29114107822232255
+100,-0.20745708964404158,0.15085084721991185,0.0003006625246654432,-0.2930494408884373,0.25923753156031826,-0.3619602017808461
+101,-0.1572955487710019,0.1673565966598745,-0.04268889000169299,-0.2592796822820273,0.2248938719296932,-0.3925323684919894
+102,-0.047881310588848106,0.1801298278627466,-0.11036560356907378,-0.2556474059955089,0.21647645940216875,-0.3726543101208812
+103,-0.06336453619668454,0.2875280295043593,-0.10996888466659974,-0.2090409067850537,0.1969142976111967,-0.36515574607685686
+104,0.03697435777027213,0.2947018356332468,-0.09273599199788546,-0.19993234090152526,0.1404128729842382,-0.43508968662871034
+105,-0.011821957472785784,0.24513744251890557,-0.14864755133021307,-0.20364323966436187,0.1561011181767007,-0.35285071050664074
+106,0.03584753767332691,0.243802547370285,-0.15398222879627171,-0.1650564560425803,0.13407823409530253,-0.3494904418153295
+107,0.16381940316191268,0.28202950353276857,-0.19414787650852117,-0.20642059075396416,0.08710238154342331,-0.3672926195007185
+108,0.191735929774449,0.2574132990937614,-0.19956920984262594,-0.14275867951029467,0.011333016614923523,-0.4016222505352491
+109,0.22977843177502225,0.2558062388227072,-0.21719163595817148,-0.1672395044527084,0.001030329872750502,-0.3019090011785536
+110,0.22228559943131007,0.3345319008597575,-0.26500141532548765,-0.12819411402796244,-0.036209459253384285,-0.3373398963623345
+111,0.28054289949303074,0.278772141049615,-0.2152665551288776,-0.0667037389045879,-0.06828454488153624,-0.30785229517702345
+112,0.305362430326259,0.24345092621307202,-0.21163157587102197,-0.03966993550504242,-0.06651890232524085,-0.2676324343996074
+113,0.28213236627155996,0.26585109176398897,-0.25780072912286484,-0.046674066874849,-0.16610046483723911,-0.2148215191557528
+114,0.3056000783637725,0.27175936943137763,-0.22966840570093394,0.011260022536096752,-0.10177422111773712,-0.24273650462702212
+115,0.4009706250524852,0.19686991442174132,-0.24531804944379557,0.0147906458936125,-0.1721896401539155,-0.1787748866013503
+116,0.34881573488135936,0.17592145441955526,-0.24299521943007876,0.08495278449000764,-0.2122276317899714,-0.1533443030072815
+117,0.3490145562743311,0.16006041737910814,-0.26589722465625004,0.03641114932392404,-0.24572548621729728,-0.12793810324015426
+118,0.3542944068050474,0.21239369564202326,-0.2623544588472631,0.10724687590175822,-0.21504618176266307,-0.0468764377250395
+119,0.3653368877859346,0.1199596142120114,-0.22320793031799158,0.1278234089178536,-0.27486359193047816,0.028147960023466135
diff --git a/stack/main/scripts/control_inputs_sampling.py b/stack/main/scripts/control_inputs_sampling.py
@@ -98,6 +98,36 @@ def beta_sampling(control_variables, seed, sample_size=100):
 
     return control_inputs_df
 
+def circle_sampling(control_variables, random_seed):
+    np.random.seed(random_seed)
+    tip_radius, mid_radius, base_radius = 0.40, 0.30, 0.25 # always fits within check control inputs with 0.45, 0.35, 0.30 - change these for bigger/smaller circles
+    noise_amplitude = 0.05 # was 0.05
+
+    num_samples_on_circle = 60
+    sampled_angles_fwd = np.linspace(0, 2*np.pi, num_samples_on_circle)
+    sampled_angles_bkwd = sampled_angles_fwd[::-1] # flip it
+    sampled_angles = np.concatenate((sampled_angles_fwd, sampled_angles_bkwd))
+
+    angle_offset = (1/6)*np.pi #30 degrees
+
+    # set control inputs based on geometry of cable arrangement
+    u1s = tip_radius * np.cos(sampled_angles)
+    u6s = - tip_radius * np.sin(sampled_angles)
+    u5s = - mid_radius * np.cos(sampled_angles + angle_offset) 
+    u2s = mid_radius * np.sin(sampled_angles + angle_offset)
+    u4s = base_radius * np.cos(sampled_angles + 2 * angle_offset) #todo add offset
+    u3s = - base_radius * np.sin(sampled_angles + 2 * angle_offset)
+    print(u1s.shape)
+
+    circle_samples = np.column_stack((u1s, u2s, u3s, u4s, u5s, u6s))
+    print(circle_samples.shape)
+    circle_samples += np.random.uniform(-noise_amplitude, noise_amplitude, (num_samples_on_circle*2, circle_samples.shape[1]))
+    # we are not checking the circle values with check_control_inputs
+
+    control_inputs_df = pd.DataFrame(circle_samples, columns=control_variables)
+    control_inputs_df.insert(0, 'ID', np.arange(0, len(circle_samples)))
+
+    return control_inputs_df
 
 def targeted_sampling(control_variables, random_seed):
     # Load data
@@ -164,7 +194,7 @@ def targeted_sampling(control_variables, random_seed):
 
 def check_control_inputs(u_opt, u_opt_previous):
     # reject vector norms of u that are too large
-    tip_range, mid_range, base_range = 0.55, 0.35, 0.3 #changed tip range to 0.55 to account for circle samples
+    tip_range, mid_range, base_range = 0.45, 0.35, 0.3 #changed tip range to 0.55 to account for circle samples
 
     u1, u2, u3, u4, u5, u6 = u_opt[0], u_opt[1], u_opt[2], u_opt[3], u_opt[4], u_opt[5]
 
@@ -219,6 +249,7 @@ def visualize_samples(control_inputs_df):
 
 def main(data_type, sampling_type, seed=None):
     control_variables = ['u1', 'u2', 'u3', 'u4', 'u5', 'u6']
+    # data_dir for mark's mac starts with '/Users/asltrunk/trunk-stack/stack/main/data'
     data_dir = os.getenv('TRUNK_DATA', '/home/trunk/Documents/trunk-stack/stack/main/data')
     if seed is not None:
         control_inputs_file = os.path.join(data_dir, f'trajectories/{data_type}/control_inputs_{sampling_type}_seed{seed}.csv')
@@ -233,6 +264,8 @@ def main(data_type, sampling_type, seed=None):
         control_inputs_df = beta_sampling(control_variables, seed)
     elif sampling_type=='targeted':
         control_inputs_df = targeted_sampling(control_variables, seed)
+    elif sampling_type =='circle':
+        control_inputs_df = circle_sampling(control_variables, seed)
     else:
         raise ValueError(f"Invalid sampling_type: {sampling_type}")
 
@@ -242,6 +275,6 @@ def main(data_type, sampling_type, seed=None):
 
 if __name__ == '__main__':
     data_type = 'steady_state'           # 'steady_state' or 'dynamic'
-    sampling_type = 'targeted'           # 'beta', 'targeted', 'uniform' or 'sinusoidal'
-    seed = 4                             # choose integer seed number
+    sampling_type = 'circle'           # 'circle', 'beta', 'targeted', 'uniform' or 'sinusoidal'
+    seed = 0                             # choose integer seed number
     main(data_type, sampling_type, seed)