Commit 46905628 authored by andreadelprete's avatar andreadelprete
Browse files

Merge (nothing really changed)

parents 68850d7e a06f758f
22 serialization::archive 10 0 0 0 0 0 0 8 0 0 0 3 3 0 0 3 4 0.058647807531589033 0.045523958093326537 275.85168933420789 -0.95738676904681641 1.1997306559838488 1.4817394802799084 1.7619040859020514 -12.336145168360446 -25.120533312996972 -0.22773306821095601 12.352777788916733 23.946592119617122 0 0.67333333333333334 3 3 3 4 -6.3840322242271155 16.411629237249151 302.51904980370625 22.054592684926405 -66.555213365225441 -89.113614969954355 -23.785944061646529 80.45747626096626 66.660846868589644 8.0611304888577795 -29.624635624503252 0.11172777570799924 0.67333333333333334 1.3399999999999999 3 3 3 4 -30.868834719386193 129.80691722494981 -8437.7297309191745 56.883356055441141 -229.61383444990236 16607.725887388369 -34.710984906837588 133.69085940873629 -10229.42858919898 6.9932577794817838 -25.667835745011278 2017.4990391172944 1.3399999999999999 2 3 3 3 4 -1.2929669650133775e-11 -6.047755497332257e-11 1.3621539945427214e-12 1.7894840117718233e-11 8.6212684749206229e-11 -1.9417939922205136e-12 -8.2375635856822819e-12 -4.0854518519214146e-11 9.2017849631346923e-13 1.2612804696783058e-12 6.4337824439708484e-12 -1.4490999941944655e-13 2 2.3534999999999999 3 3 3 4 -1.0133271959758456e-11 -1.1489402011166182e-10 -4.1696674896314755e-08 1.1801982916011253e-11 1.3425556114845385e-10 5.0929881691172319e-08 -4.5712465595351048e-12 -5.218129509892185e-11 -2.0693594847187947e-08 5.8895013491535877e-13 6.7472258437218281e-12 2.7964385899583263e-09 2.3534999999999999 2.7000000000000002 3 3 3 4 124.05959950933651 -1196.8824935879561 53642.858439117583 -123.18901588631248 1245.5222344881056 -54700.576354863966 40.705899696700314 -427.67641243104066 18406.658024542277 -4.480783691776562 48.353071538503102 -2039.1118874103554 2.7000000000000002 3.3733333333333335 3 3 3 4 417.33223684599517 948.33613681912448 1601.9034282973073 -340.54841527179263 -827.21250367525863 -687.03857499017954 92.206913270568677 235.85483069912988 93.604361487627429 -8.2867073858820373 -22.082443575106844 -1.242358402587737 3.3733333333333335 4.04 3 3 3 4 257.95893506101174 715.94922967525031 1321.5887590948823 -175.0083739159497 -488.95235651455505 -773.00396176117317 39.511551927492235 110.97784011800178 187.92567251856039 -2.9687125738136388 -8.3727056433997884 -15.07119227073569 4.04 4.7000000000000002 8 0 3 3 3 4 -0.14066797165637829 0.002877198553619779 0.0057543358184551077 0.052464208377291738 -0.080285392542056064 -0.01824371631190766 3.2612822928327017 0.36917917625743557 0.043944864249973625 -3.3148150949478978 -0.23705210938383536 -0.056139269066664846 0 0.67333333333333334 3 3 3 4 1.7868670455280522 -1.3848393452848715 -0.01895463652802689 -7.1007024833950227 4.982107384315964 0.12726985251998463 11.709921002154823 -5.4509124834068521 -0.22581027890695182 -6.398578785440832 1.7862449676369534 0.10448069746634218 0.67333333333333334 1.3399999999999999 3 3 3 4 241.15538115173248 5.9509709397080144 0.22869370929657365 -452.31076230347054 -11.820883647300636 -0.70440590435897898 275.59064317804666 7.3743942080924816 0.60379677283247535 -54.862053657122217 -1.475847559684583 -0.15438362398856462 1.3399999999999999 2 3 3 3 4 -3.3348301053053542e-12 5.3751817075278017e-28 8.6519426384291074e-16 4.7539067458609163e-12 -7.2622135835748237e-28 -1.168932675617554e-15 -2.2527841668818932e-12 3.2606353499716199e-28 5.2483490880466667e-16 3.5476916013888734e-13 -4.8666199253307956e-29 -7.8333568478308779e-17 2 2.3534999999999999 3 3 3 4 4.2193852036066772e-10 5.3327435090646544e-09 7.6275575402174123e-10 -5.1570539598885795e-10 -6.5135953026414463e-09 -9.316559643293573e-10 2.0966911197711413e-10 2.6465672060154479e-09 3.7854518248667815e-10 -2.8350443403135394e-11 -3.5764421702918155e-10 -5.1154754390101185e-11 2.3534999999999999 2.7000000000000002 3 3 3 4 -1529.2062904409338 26.884222503341956 -40.636983027131841 1532.3566746399604 -26.533907162094899 40.216074365255615 -509.062915072968 8.6751306550844784 -13.216290184684482 56.033730620077115 -0.93910357370581587 1.4428866375700626 2.7000000000000002 3.3733333333333335 3 3 3 4 -529.03498984020155 -11.523851325649719 -67.702607093922936 397.03249501634076 9.7340782481198094 55.11281068159952 -99.338180547219793 -2.704360791599532 -14.902108937425185 8.2887340653349231 0.24766018467446915 1.3386157803632253 3.3733333333333335 4.04 3 3 3 4 -81.393413771092725 -11.903119501729705 -67.326328429683954 54.407828351468602 8.4725169389157262 46.132770418370313 -11.992860924217013 -2.0088509780459192 -10.518587325677135 0.87200887660426174 0.15854978538216766 0.79807140351315564 4.04 4.7000000000000002 22 serialization::archive 10 0 0 0 0 8 0 0 0 6 3 0 0 6 4 0.058647807531589033 0.045523958093326537 275.85168933420789 -0.14066797165637829 0.002877198553619779 0.0057543358184551077 -0.95738676904681641 1.1997306559838488 1.4817394802799084 0.052464208377291738 -0.080285392542056064 -0.01824371631190766 1.7619040859020514 -12.336145168360446 -25.120533312996972 3.2612822928327017 0.36917917625743557 0.043944864249973625 -0.22773306821095601 12.352777788916733 23.946592119617122 -3.3148150949478978 -0.23705210938383536 -0.056139269066664846 0 0.67333333333333334 6 3 6 4 -6.3840322242271155 16.411629237249151 302.51904980370625 1.7868670455280522 -1.3848393452848715 -0.01895463652802689 22.054592684926405 -66.555213365225441 -89.113614969954355 -7.1007024833950227 4.982107384315964 0.12726985251998463 -23.785944061646529 80.45747626096626 66.660846868589644 11.709921002154823 -5.4509124834068521 -0.22581027890695182 8.0611304888577795 -29.624635624503252 0.11172777570799924 -6.398578785440832 1.7862449676369534 0.10448069746634218 0.67333333333333334 1.3399999999999999 6 3 6 4 -30.868834719386193 129.80691722494981 -8437.7297309191745 241.15538115173248 5.9509709397080144 0.22869370929657365 56.883356055441141 -229.61383444990236 16607.725887388369 -452.31076230347054 -11.820883647300636 -0.70440590435897898 -34.710984906837588 133.69085940873629 -10229.42858919898 275.59064317804666 7.3743942080924816 0.60379677283247535 6.9932577794817838 -25.667835745011278 2017.4990391172944 -54.862053657122217 -1.475847559684583 -0.15438362398856462 1.3399999999999999 2 6 3 6 4 -1.2929669650133775e-11 -6.047755497332257e-11 1.3621539945427214e-12 -3.3348301053053542e-12 5.3751817075278017e-28 8.6519426384291074e-16 1.7894840117718233e-11 8.6212684749206229e-11 -1.9417939922205136e-12 4.7539067458609163e-12 -7.2622135835748237e-28 -1.168932675617554e-15 -8.2375635856822819e-12 -4.0854518519214146e-11 9.2017849631346923e-13 -2.2527841668818932e-12 3.2606353499716199e-28 5.2483490880466667e-16 1.2612804696783058e-12 6.4337824439708484e-12 -1.4490999941944655e-13 3.5476916013888734e-13 -4.8666199253307956e-29 -7.8333568478308779e-17 2 2.3534999999999999 6 3 6 4 -1.0133271959758456e-11 -1.1489402011166182e-10 -4.1696674896314755e-08 4.2193852036066772e-10 5.3327435090646544e-09 7.6275575402174123e-10 1.1801982916011253e-11 1.3425556114845385e-10 5.0929881691172319e-08 -5.1570539598885795e-10 -6.5135953026414463e-09 -9.316559643293573e-10 -4.5712465595351048e-12 -5.218129509892185e-11 -2.0693594847187947e-08 2.0966911197711413e-10 2.6465672060154479e-09 3.7854518248667815e-10 5.8895013491535877e-13 6.7472258437218281e-12 2.7964385899583263e-09 -2.8350443403135394e-11 -3.5764421702918155e-10 -5.1154754390101185e-11 2.3534999999999999 2.7000000000000002 6 3 6 4 124.05959950933651 -1196.8824935879561 53642.858439117583 -1529.2062904409338 26.884222503341956 -40.636983027131841 -123.18901588631248 1245.5222344881056 -54700.576354863966 1532.3566746399604 -26.533907162094899 40.216074365255615 40.705899696700314 -427.67641243104066 18406.658024542277 -509.062915072968 8.6751306550844784 -13.216290184684482 -4.480783691776562 48.353071538503102 -2039.1118874103554 56.033730620077115 -0.93910357370581587 1.4428866375700626 2.7000000000000002 3.3733333333333335 6 3 6 4 417.33223684599517 948.33613681912448 1601.9034282973073 -529.03498984020155 -11.523851325649719 -67.702607093922936 -340.54841527179263 -827.21250367525863 -687.03857499017954 397.03249501634076 9.7340782481198094 55.11281068159952 92.206913270568677 235.85483069912988 93.604361487627429 -99.338180547219793 -2.704360791599532 -14.902108937425185 -8.2867073858820373 -22.082443575106844 -1.242358402587737 8.2887340653349231 0.24766018467446915 1.3386157803632253 3.3733333333333335 4.04 6 3 6 4 257.95893506101174 715.94922967525031 1321.5887590948823 -81.393413771092725 -11.903119501729705 -67.326328429683954 -175.0083739159497 -488.95235651455505 -773.00396176117317 54.407828351468602 8.4725169389157262 46.132770418370313 39.511551927492235 110.97784011800178 187.92567251856039 -11.992860924217013 -2.0088509780459192 -10.518587325677135 -2.9687125738136388 -8.3727056433997884 -15.07119227073569 0.87200887660426174 0.15854978538216766 0.79807140351315564 4.04 4.7000000000000002
22 serialization::archive 10 0 0 0 0 0 0 8 0 0 0 3 3 0 0 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 3 3 3 4 9.482271872173513e-13 5.6623900429007159e-11 -1.5247116567962003e-10 -1.2811154550702546e-12 -7.6502503771105649e-11 2.0599827703523207e-10 5.7520345444602025e-13 3.434858604566324e-11 -9.2490430969077564e-11 -8.5851261857615335e-14 -5.1266546336811042e-12 1.3804541935683271e-11 2 2.3534999999999999 3 3 3 4 -7.1619032169829432e-09 -1.6706599435019551e-08 -1.4016532388586326e-07 8.747793527443618e-09 2.0406011918270987e-08 1.7120272027039792e-07 -3.5543539933065611e-09 -8.2912553573224765e-09 -6.9562121070759028e-08 4.8031810720367904e-10 1.1204399131518955e-09 9.4002866311854057e-09 2.3534999999999999 2.7000000000000002 3 3 3 4 -9.8734063800080964e-08 -7.0102694226260359e-10 -2.2663294093295283e-08 9.5304798905842088e-08 5.7276001499794555e-10 2.3702314985459429e-08 -3.0489854111373606e-08 -1.4712732764859671e-10 -8.2173892067154791e-09 3.2353880620952551e-09 1.153955801079207e-11 9.4354574757459106e-10 2.7000000000000002 3.3733333333333335 3 3 3 4 -2.5171409489153563e-09 2.4811199897237183e-09 -5.6142237790899955e-08 1.9958346939685745e-09 -1.9347273676221064e-09 4.451503839242483e-08 -5.2547498914967523e-10 5.0021869943839829e-10 -1.1720178723692e-08 4.5959911587435478e-11 -4.2905374832038349e-11 1.0250885181071794e-09 3.3733333333333335 4.04 3 3 3 4 4.793214825261435e-12 -1.2919744173598912e-09 0 -3.3966336813445467e-12 8.7010058352175788e-10 0 8.003470048736577e-13 -1.9479503092009114e-10 0 -6.2690365917004568e-14 1.450087078809554e-11 0 4.04 4.7000000000000002 8 0 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 3 3 3 4 -6.1378142659858052e-11 4.9276883213665044e-12 -6.6339156394178715e-15 8.2925788487255254e-11 -6.6700723848764915e-12 9.4568584647022538e-15 -3.7232553718519819e-11 3.000705421374594e-12 -4.4814217351388362e-15 5.5570975699283558e-12 -4.4879565463898723e-13 7.0573570632108947e-16 2 2.3534999999999999 3 3 3 4 5.0890362222126624e-09 -1.5043894052885172e-08 1.5741291182861055e-08 -6.2159228876527448e-09 1.8375143508500199e-08 -1.9226952072310235e-08 2.5256186338305854e-09 -7.4660909390401535e-09 7.8121866886572601e-09 -3.4129981538257528e-10 1.0089320492576025e-09 -1.0557009372521919e-09 2.3534999999999999 2.7000000000000002 3 3 3 4 -1.8379713819122696e-08 7.004405734447105e-09 -1.3889058824451179e-08 1.7810454386143594e-08 -6.6420986959249386e-09 1.3272575302509379e-08 -5.7219246241098431e-09 2.0835764236596784e-09 -4.1995627208698892e-09 6.0988251213208138e-10 -2.164310005552416e-10 4.4037584427258733e-10 2.7000000000000002 3.3733333333333335 3 3 3 4 -2.571399730650364e-09 -3.4282513986297977e-09 3.7275394759056953e-09 2.038856344816477e-09 2.717525046149647e-09 -2.955649181886437e-09 -5.3680198009778369e-10 -7.1528110184412936e-10 7.782066119871932e-10 4.6950610504179676e-11 6.2542147067080952e-11 -6.8067122794617675e-11 3.3733333333333335 4.04 3 3 3 4 0 2.8852116030727174e-11 3.6741498625107384e-12 0 -1.9430913380988538e-11 -2.4744142735037302e-12 0 4.3501239334137559e-12 5.5396308661850331e-13 0 -3.2383056576279769e-13 -4.1237946894676031e-14 4.04 4.7000000000000002 22 serialization::archive 10 0 0 0 0 8 0 0 0 6 3 0 0 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 6 3 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 6 3 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 6 3 6 4 9.482271872173513e-13 5.6623900429007159e-11 -1.5247116567962003e-10 -6.1378142659858052e-11 4.9276883213665044e-12 -6.6339156394178715e-15 -1.2811154550702546e-12 -7.6502503771105649e-11 2.0599827703523207e-10 8.2925788487255254e-11 -6.6700723848764915e-12 9.4568584647022538e-15 5.7520345444602025e-13 3.434858604566324e-11 -9.2490430969077564e-11 -3.7232553718519819e-11 3.000705421374594e-12 -4.4814217351388362e-15 -8.5851261857615335e-14 -5.1266546336811042e-12 1.3804541935683271e-11 5.5570975699283558e-12 -4.4879565463898723e-13 7.0573570632108947e-16 2 2.3534999999999999 6 3 6 4 -7.1619032169829432e-09 -1.6706599435019551e-08 -1.4016532388586326e-07 5.0890362222126624e-09 -1.5043894052885172e-08 1.5741291182861055e-08 8.747793527443618e-09 2.0406011918270987e-08 1.7120272027039792e-07 -6.2159228876527448e-09 1.8375143508500199e-08 -1.9226952072310235e-08 -3.5543539933065611e-09 -8.2912553573224765e-09 -6.9562121070759028e-08 2.5256186338305854e-09 -7.4660909390401535e-09 7.8121866886572601e-09 4.8031810720367904e-10 1.1204399131518955e-09 9.4002866311854057e-09 -3.4129981538257528e-10 1.0089320492576025e-09 -1.0557009372521919e-09 2.3534999999999999 2.7000000000000002 6 3 6 4 -9.8734063800080964e-08 -7.0102694226260359e-10 -2.2663294093295283e-08 -1.8379713819122696e-08 7.004405734447105e-09 -1.3889058824451179e-08 9.5304798905842088e-08 5.7276001499794555e-10 2.3702314985459429e-08 1.7810454386143594e-08 -6.6420986959249386e-09 1.3272575302509379e-08 -3.0489854111373606e-08 -1.4712732764859671e-10 -8.2173892067154791e-09 -5.7219246241098431e-09 2.0835764236596784e-09 -4.1995627208698892e-09 3.2353880620952551e-09 1.153955801079207e-11 9.4354574757459106e-10 6.0988251213208138e-10 -2.164310005552416e-10 4.4037584427258733e-10 2.7000000000000002 3.3733333333333335 6 3 6 4 -2.5171409489153563e-09 2.4811199897237183e-09 -5.6142237790899955e-08 -2.571399730650364e-09 -3.4282513986297977e-09 3.7275394759056953e-09 1.9958346939685745e-09 -1.9347273676221064e-09 4.451503839242483e-08 2.038856344816477e-09 2.717525046149647e-09 -2.955649181886437e-09 -5.2547498914967523e-10 5.0021869943839829e-10 -1.1720178723692e-08 -5.3680198009778369e-10 -7.1528110184412936e-10 7.782066119871932e-10 4.5959911587435478e-11 -4.2905374832038349e-11 1.0250885181071794e-09 4.6950610504179676e-11 6.2542147067080952e-11 -6.8067122794617675e-11 3.3733333333333335 4.04 6 3 6 4 4.793214825261435e-12 -1.2919744173598912e-09 0 0 2.8852116030727174e-11 3.6741498625107384e-12 -3.3966336813445467e-12 8.7010058352175788e-10 0 0 -1.9430913380988538e-11 -2.4744142735037302e-12 8.003470048736577e-13 -1.9479503092009114e-10 0 0 4.3501239334137559e-12 5.5396308661850331e-13 -6.2690365917004568e-14 1.450087078809554e-11 0 0 -3.2383056576279769e-13 -4.1237946894676031e-14 4.04 4.7000000000000002
22 serialization::archive 10 0 0 0 0 0 0 8 0 0 0 3 3 0 0 3 4 -0.058647807531589054 -0.045523958093326933 272.36678306309233 0.824640751404629 1.183854118368697 0.72768440214012819 -1.2194264116302225 -3.9463139716642579 14.127633235627393 0.12104112926040846 4.217238913671471 -11.916814908047296 0 0.67333333333333334 3 3 3 4 3.7721877140376008 -45.729647745946671 225.30388284696761 -12.890895241603967 161.01624154127026 159.13013992242853 14.069041796803653 -175.07564067236007 -143.40515686639975 -4.8807750844170572 55.472275047191609 26.814133553766368 0.67333333333333334 1.3399999999999999 3 3 3 4 32.71514051819517 696.66644685976394 8840.0504682974024 -61.408639039539857 -1339.301670187991 -16344.89776921902 38.004424636691425 822.60781727438859 10075.251843898082 -7.7513895422479351 -162.05984547694496 -1988.0091338644404 1.3399999999999999 2 3 3 3 4 34.546649291449015 -946.82208699103296 469.36083175782841 -50.403479384171355 1379.2156198148718 81.444603261019282 24.020874123704566 -655.08734574726179 -26.54887883457501 -3.7398427962533609 102.59448026451437 2.6418389493268783 2 2.3534999999999999 3 3 3 4 -36.119184338785082 1733.0506728140979 633.24423092781637 42.60067903758425 -2040.7957997459657 -129.01760384110162 -16.743642615371201 799.76741070731305 63.541148484573156 2.2109083884093392 -103.70133598872872 -10.212298446047981 2.3534999999999999 2.7000000000000002 3 3 3 4 -350.94396817694411 -3601.3911495119592 -52306.622923151866 352.9391724033851 3665.1279160029649 53888.874919495422 -117.56001872152572 -1228.2281706912031 -18130.143592281591 12.97462497451729 135.72027546273313 2007.9515536728543 2.7000000000000002 3.3733333333333335 3 3 3 4 -416.67403485634838 402.34796727180827 458.70074423261593 335.67339800553452 -377.38553253707727 -530.75926556985246 -89.905693372504203 113.55556451480587 232.42961226282409 8.0062880579729949 -11.093675184666678 -27.780984084240448 3.3733333333333335 4.04 3 3 3 4 -327.31023616944856 807.88790049541763 482.17832245893442 223.90629218557856 -551.01646350831061 -87.844854068237083 -50.963890777171109 124.86645442310154 8.4730458593819566 3.8597795862346609 -9.4054759009225091 0.1611351183991486 4.04 4.7000000000000002 8 0 3 3 3 4 -0.1898238987725889 -0.0039455293049555119 0.0053887476125384073 0.03251718911786624 -0.082597937984857941 -0.018311914503725272 3.0375859264475449 0.40413720291581029 0.069296181630102899 -3.0866088588308527 -0.26974736048106102 -0.080689500385642557 0 0.67333333333333334 3 3 3 4 0.14951412681309967 -1.3577738276687434 -0.097994587883084835 -1.5232661854999086 4.8759293549174982 0.42103038644410429 5.4144043775975188 -5.3331036618357341 -0.55089320661793273 -4.2965897790175198 1.7485980345937291 0.20999316215152256 0.67333333333333334 1.3399999999999999 3 3 3 4 282.99460495032451 -3.5659005993678328 -0.55250248365511956 -530.15180555715426 6.5461194954612552 0.81203173875888346 321.05612267040169 -4.112174945409147 -0.37041316097479254 -62.999597793233967 0.82497302502848324 0.046440385770931576 1.3399999999999999 2 3 3 3 4 -1139.6575588501212 158.58151017025025 27.897501879964487 1597.6757438579673 -221.69870011602291 -39.159820855272741 -740.4368204174383 102.53406537916349 18.194523952357326 113.62150687196136 -15.705368580589074 -2.7993155678396739 2 2.3534999999999999 3 3 3 4 1848.5261727053191 -16.136456774896423 -41.924261760626123 -2214.5715954147695 19.277729828252944 50.469260871928242 880.75128278136219 -7.6398268621502421 -20.156113113476785 -116.18605285410241 1.0045069944764704 2.6703637731724195 2.3534999999999999 2.7000000000000002 3 3 3 4 -1685.979187941686 -1.8830404555895637 -42.492361791452176 1715.2278822448989 2.0492853788299672 42.090798159955099 -576.31778108410901 -0.70909639327837914 -13.851049433656822 63.973488198846226 0.076719491354958771 1.5133264885748461 2.7000000000000002 3.3733333333333335 3 3 3 4 -379.60509881569681 59.566013445372505 -48.395872695434171 273.57582658515918 -48.680579446028844 39.195954748285523 -65.661824152646346 13.163125477719245 -10.568799603692195 5.250871170747736 -1.1783465543226752 0.94851013399905104 3.3733333333333335 4.04 3 3 3 4 -96.462531145950791 -37.471591072480805 -70.176395978356851 65.145270124679556 26.311144573886082 48.115868531662947 -14.523178759928991 -6.1277961933671863 -10.975271921390741 1.0690287479465208 0.47362484367299013 0.83292300701434463 4.04 4.7000000000000002 22 serialization::archive 10 0 0 0 0 8 0 0 0 6 3 0 0 6 4 -0.058647807531589054 -0.045523958093326933 272.36678306309233 -0.1898238987725889 -0.0039455293049555119 0.0053887476125384073 0.824640751404629 1.183854118368697 0.72768440214012819 0.03251718911786624 -0.082597937984857941 -0.018311914503725272 -1.2194264116302225 -3.9463139716642579 14.127633235627393 3.0375859264475449 0.40413720291581029 0.069296181630102899 0.12104112926040846 4.217238913671471 -11.916814908047296 -3.0866088588308527 -0.26974736048106102 -0.080689500385642557 0 0.67333333333333334 6 3 6 4 3.7721877140376008 -45.729647745946671 225.30388284696761 0.14951412681309967 -1.3577738276687434 -0.097994587883084835 -12.890895241603967 161.01624154127026 159.13013992242853 -1.5232661854999086 4.8759293549174982 0.42103038644410429 14.069041796803653 -175.07564067236007 -143.40515686639975 5.4144043775975188 -5.3331036618357341 -0.55089320661793273 -4.8807750844170572 55.472275047191609 26.814133553766368 -4.2965897790175198 1.7485980345937291 0.20999316215152256 0.67333333333333334 1.3399999999999999 6 3 6 4 32.71514051819517 696.66644685976394 8840.0504682974024 282.99460495032451 -3.5659005993678328 -0.55250248365511956 -61.408639039539857 -1339.301670187991 -16344.89776921902 -530.15180555715426 6.5461194954612552 0.81203173875888346 38.004424636691425 822.60781727438859 10075.251843898082 321.05612267040169 -4.112174945409147 -0.37041316097479254 -7.7513895422479351 -162.05984547694496 -1988.0091338644404 -62.999597793233967 0.82497302502848324 0.046440385770931576 1.3399999999999999 2 6 3 6 4 34.546649291449015 -946.82208699103296 469.36083175782841 -1139.6575588501212 158.58151017025025 27.897501879964487 -50.403479384171355 1379.2156198148718 81.444603261019282 1597.6757438579673 -221.69870011602291 -39.159820855272741 24.020874123704566 -655.08734574726179 -26.54887883457501 -740.4368204174383 102.53406537916349 18.194523952357326 -3.7398427962533609 102.59448026451437 2.6418389493268783 113.62150687196136 -15.705368580589074 -2.7993155678396739 2 2.3534999999999999 6 3 6 4 -36.119184338785082 1733.0506728140979 633.24423092781637 1848.5261727053191 -16.136456774896423 -41.924261760626123 42.60067903758425 -2040.7957997459657 -129.01760384110162 -2214.5715954147695 19.277729828252944 50.469260871928242 -16.743642615371201 799.76741070731305 63.541148484573156 880.75128278136219 -7.6398268621502421 -20.156113113476785 2.2109083884093392 -103.70133598872872 -10.212298446047981 -116.18605285410241 1.0045069944764704 2.6703637731724195 2.3534999999999999 2.7000000000000002 6 3 6 4 -350.94396817694411 -3601.3911495119592 -52306.622923151866 -1685.979187941686 -1.8830404555895637 -42.492361791452176 352.9391724033851 3665.1279160029649 53888.874919495422 1715.2278822448989 2.0492853788299672 42.090798159955099 -117.56001872152572 -1228.2281706912031 -18130.143592281591 -576.31778108410901 -0.70909639327837914 -13.851049433656822 12.97462497451729 135.72027546273313 2007.9515536728543 63.973488198846226 0.076719491354958771 1.5133264885748461 2.7000000000000002 3.3733333333333335 6 3 6 4 -416.67403485634838 402.34796727180827 458.70074423261593 -379.60509881569681 59.566013445372505 -48.395872695434171 335.67339800553452 -377.38553253707727 -530.75926556985246 273.57582658515918 -48.680579446028844 39.195954748285523 -89.905693372504203 113.55556451480587 232.42961226282409 -65.661824152646346 13.163125477719245 -10.568799603692195 8.0062880579729949 -11.093675184666678 -27.780984084240448 5.250871170747736 -1.1783465543226752 0.94851013399905104 3.3733333333333335 4.04 6 3 6 4 -327.31023616944856 807.88790049541763 482.17832245893442 -96.462531145950791 -37.471591072480805 -70.176395978356851 223.90629218557856 -551.01646350831061 -87.844854068237083 65.145270124679556 26.311144573886082 48.115868531662947 -50.963890777171109 124.86645442310154 8.4730458593819566 -14.523178759928991 -6.1277961933671863 -10.975271921390741 3.8597795862346609 -9.4054759009225091 0.1611351183991486 1.0690287479465208 0.47362484367299013 0.83292300701434463 4.04 4.7000000000000002
22 serialization::archive 10 0 0 0 0 0 0 8 0 0 0 3 3 0 0 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 3 3 3 4 4.1141927399909127e-11 4.7263600659075799e-12 2.0078730694786874e-10 -5.5585369997749786e-11 -6.3856141315985593e-12 -2.7127646789765333e-10 2.4957076829279608e-11 2.8670540915872342e-12 1.2179945283392757e-10 -3.724936840191001e-12 -4.2791852113242472e-13 -1.8179022811034044e-11 2 2.3534999999999999 3 3 3 4 1.577177449314478e-08 -4.1056613201976668e-08 -4.1106726145000385e-08 -1.9264184763104551e-08 5.0147951507547351e-08 5.0209161170027033e-08 7.8273146051888665e-09 -2.037583204699656e-08 -2.0400702410886729e-08 -1.0577452169176124e-09 2.753490817162211e-09 2.7568516771473718e-09 2.3534999999999999 2.7000000000000002 3 3 3 4 6.6042881341319885e-07 -3.8363187813455244e-07 3.7197574868982623e-07 -6.8536758801395549e-07 3.9857695977167173e-07 -3.868937674445222e-07 2.3589872391797851e-07 -1.3733561161264347e-07 1.3344785221674965e-07 -2.6908399285555155e-08 1.5681079886562508e-08 -1.5251608964574542e-08 2.7000000000000002 3.3733333333333335 3 3 3 4 1.473579866047682e-06 -8.6993034891981394e-07 8.5649163547713781e-07 -1.168397749938804e-06 6.8976557409647674e-07 -6.7911005218668768e-07 3.0762256855590338e-07 -1.8160550002437865e-07 1.7880005211987463e-07 -2.6905764013636063e-08 1.5883862975893295e-08 -1.563848852360431e-08 3.3733333333333335 4.04 3 3 3 4 6.8120933062878599e-17 7.3988866939800279e-17 5.4592085784193435e-17 -4.7312935283673291e-17 -5.1388469239020077e-17 -3.7916565519208943e-17 1.0898530898824641e-17 1.1837329822950653e-17 8.7340778670945501e-18 -8.3194892357443669e-19 -9.0361296358405917e-19 -6.6672350130495695e-19 4.04 4.7000000000000002 8 0 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 3 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 3 3 3 4 -3.5189560773041442e-11 2.205171743029803e-11 -1.1475202941089038e-12 4.7543342746556153e-11 -2.9793277804764463e-11 1.5503731633173528e-12 -2.1346315724706139e-11 1.3376777436215291e-11 -6.9609651159164365e-13 3.1860172723442133e-12 -1.9965339457037823e-12 1.0389500173009649e-13 2 2.3534999999999999 3 3 3 4 -6.0583211453523511e-10 1.2750529196470506e-11 7.9645849826874086e-10 7.3998406424729467e-10 -1.5573932430680493e-11 -9.7282164879181655e-10 -3.0066614003739619e-10 6.3279121475392571e-12 3.9527139058673359e-10 4.0630559464520591e-11 -8.5512326318114089e-13 -5.3415052782001017e-11 2.3534999999999999 2.7000000000000002 3 3 3 4 7.2331666163665223e-09 -3.7604918772497322e-09 2.9293827128609108e-09 -6.9705740434641952e-09 3.6252200998652068e-09 -3.2058186517699531e-09 2.2260717825313216e-09 -1.1581585975151848e-09 1.1570139083242398e-09 -2.3577096955070505e-10 1.2271370134122686e-10 -1.3759608518852535e-10 2.7000000000000002 3.3733333333333335 3 3 3 4 -1.6148151052814676e-10 4.5951728395177172e-11 1.1581485432811521e-08 1.2803828140231988e-10 -3.6435009258593332e-11 -9.1829305166471054e-09 -3.3710664883194228e-11 9.5928215661778203e-12 2.4177354608474118e-09 2.9484546545068054e-12 -8.390222944178912e-13 -2.1146374293126187e-10 3.3733333333333335 4.04 3 3 3 4 0 0 6.752632012737688e-13 0 0 -4.7851427837764874e-13 0 0 1.1275206731661168e-13 0 0 -8.8317546201012439e-15 4.04 4.7000000000000002 22 serialization::archive 10 0 0 0 0 8 0 0 0 6 3 0 0 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 6 3 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.67333333333333334 1.3399999999999999 6 3 6 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1.3399999999999999 2 6 3 6 4 4.1141927399909127e-11 4.7263600659075799e-12 2.0078730694786874e-10 -3.5189560773041442e-11 2.205171743029803e-11 -1.1475202941089038e-12 -5.5585369997749786e-11 -6.3856141315985593e-12 -2.7127646789765333e-10 4.7543342746556153e-11 -2.9793277804764463e-11 1.5503731633173528e-12 2.4957076829279608e-11 2.8670540915872342e-12 1.2179945283392757e-10 -2.1346315724706139e-11 1.3376777436215291e-11 -6.9609651159164365e-13 -3.724936840191001e-12 -4.2791852113242472e-13 -1.8179022811034044e-11 3.1860172723442133e-12 -1.9965339457037823e-12 1.0389500173009649e-13 2 2.3534999999999999 6 3 6 4 1.577177449314478e-08 -4.1056613201976668e-08 -4.1106726145000385e-08 -6.0583211453523511e-10 1.2750529196470506e-11 7.9645849826874086e-10 -1.9264184763104551e-08 5.0147951507547351e-08 5.0209161170027033e-08 7.3998406424729467e-10 -1.5573932430680493e-11 -9.7282164879181655e-10 7.8273146051888665e-09 -2.037583204699656e-08 -2.0400702410886729e-08 -3.0066614003739619e-10 6.3279121475392571e-12 3.9527139058673359e-10 -1.0577452169176124e-09 2.753490817162211e-09 2.7568516771473718e-09 4.0630559464520591e-11 -8.5512326318114089e-13 -5.3415052782001017e-11 2.3534999999999999 2.7000000000000002 6 3 6 4 6.6042881341319885e-07 -3.8363187813455244e-07 3.7197574868982623e-07 7.2331666163665223e-09 -3.7604918772497322e-09 2.9293827128609108e-09 -6.8536758801395549e-07 3.9857695977167173e-07 -3.868937674445222e-07 -6.9705740434641952e-09 3.6252200998652068e-09 -3.2058186517699531e-09 2.3589872391797851e-07 -1.3733561161264347e-07 1.3344785221674965e-07 2.2260717825313216e-09 -1.1581585975151848e-09 1.1570139083242398e-09 -2.6908399285555155e-08 1.5681079886562508e-08 -1.5251608964574542e-08 -2.3577096955070505e-10 1.2271370134122686e-10 -1.3759608518852535e-10 2.7000000000000002 3.3733333333333335 6 3 6 4 1.473579866047682e-06 -8.6993034891981394e-07 8.5649163547713781e-07 -1.6148151052814676e-10 4.5951728395177172e-11 1.1581485432811521e-08 -1.168397749938804e-06 6.8976557409647674e-07 -6.7911005218668768e-07 1.2803828140231988e-10 -3.6435009258593332e-11 -9.1829305166471054e-09 3.0762256855590338e-07 -1.8160550002437865e-07 1.7880005211987463e-07 -3.3710664883194228e-11 9.5928215661778203e-12 2.4177354608474118e-09 -2.6905764013636063e-08 1.5883862975893295e-08 -1.563848852360431e-08 2.9484546545068054e-12 -8.390222944178912e-13 -2.1146374293126187e-10 3.3733333333333335 4.04 6 3 6 4 6.8120933062878599e-17 7.3988866939800279e-17 5.4592085784193435e-17 0 0 6.752632012737688e-13 -4.7312935283673291e-17 -5.1388469239020077e-17 -3.7916565519208943e-17 0 0 -4.7851427837764874e-13 1.0898530898824641e-17 1.1837329822950653e-17 8.7340778670945501e-18 0 0 1.1275206731661168e-13 -8.3194892357443669e-19 -9.0361296358405917e-19 -6.6672350130495695e-19 0 0 -8.8317546201012439e-15 4.04 4.7000000000000002
...@@ -76,7 +76,8 @@ namespace dynamicgraph { ...@@ -76,7 +76,8 @@ namespace dynamicgraph {
/* --- SIGNALS --- */ /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector); DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector); DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
...@@ -91,6 +92,9 @@ namespace dynamicgraph { ...@@ -91,6 +92,9 @@ namespace dynamicgraph {
void playSpline(const std::string& fileName); void playSpline(const std::string& fileName);
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */ /** Print the current value of the specified component. */
void getValue(const int& id); void getValue(const int& id);
...@@ -169,6 +173,7 @@ namespace dynamicgraph { ...@@ -169,6 +173,7 @@ namespace dynamicgraph {
double m_t; /// current control loop time. double m_t; /// current control loop time.
unsigned int m_n; /// size of ouput vector unsigned int m_n; /// size of ouput vector
unsigned int m_iterLast; /// last iter index unsigned int m_iterLast; /// last iter index
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component std::vector<JTG_Status> m_status; /// status of the component
std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* > m_currentTrajGen; std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* > m_currentTrajGen;
......
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
""" """
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.core.switch import Switch
from dynamic_graph.sot.torque_control.force_torque_estimator import ForceTorqueEstimator from dynamic_graph.sot.torque_control.force_torque_estimator import ForceTorqueEstimator
from dynamic_graph.sot.torque_control.numerical_difference import NumericalDifference from dynamic_graph.sot.torque_control.numerical_difference import NumericalDifference
from dynamic_graph.sot.torque_control.joint_torque_controller import JointTorqueController from dynamic_graph.sot.torque_control.joint_torque_controller import JointTorqueController
...@@ -92,6 +93,20 @@ def create_com_traj_gen(conf, dt): ...@@ -92,6 +93,20 @@ def create_com_traj_gen(conf, dt):
com_traj_gen.init(dt,3); com_traj_gen.init(dt,3);
return com_traj_gen ; return com_traj_gen ;
def create_force_traj_gen(name, initial_value, dt):
force_traj_gen = NdTrajectoryGenerator(name);
force_traj_gen.initial_value.value = initial_value;
force_traj_gen.init(dt,6);
return force_traj_gen ;
def create_trajectory_switch():
traj_sync = Switch("traj_sync");
return traj_sync ;
def connect_synchronous_trajectories(switch, list_of_traj_gens):
for traj_gen in list_of_traj_gens:
plug(switch.out, traj_gen.trigger);
def create_free_flyer_locator(ent, robot_name="robot"): def create_free_flyer_locator(ent, robot_name="robot"):
from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
ff_locator = FreeFlyerLocator("ffLocator"); ff_locator = FreeFlyerLocator("ffLocator");
...@@ -296,6 +311,10 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot') ...@@ -296,6 +311,10 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.com_traj_gen.dx, ctrl.com_ref_vel); plug(robot.com_traj_gen.dx, ctrl.com_ref_vel);
plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc); plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc);
plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias # rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result # it is better to compute directly their product in python and pass the result
# to the C++ entity, because otherwise we get a loss of precision # to the C++ entity, because otherwise we get a loss of precision
......
...@@ -6,6 +6,7 @@ ...@@ -6,6 +6,7 @@
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories, create_force_traj_gen
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator, create_com_traj_gen, create_encoders from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator, create_com_traj_gen, create_encoders
from dynamic_graph.sot.torque_control.create_entities_utils import create_imu_offset_compensation, create_estimators, create_imu_filter from dynamic_graph.sot.torque_control.create_entities_utils import create_imu_offset_compensation, create_estimators, create_imu_filter
from dynamic_graph.sot.torque_control.create_entities_utils import create_base_estimator, create_position_controller, create_torque_controller from dynamic_graph.sot.torque_control.create_entities_utils import create_base_estimator, create_position_controller, create_torque_controller
...@@ -63,6 +64,10 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None): ...@@ -63,6 +64,10 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
robot.traj_gen = create_trajectory_generator(robot.device, dt); robot.traj_gen = create_trajectory_generator(robot.device, dt);
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt); robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt);
robot.rf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt);
robot.lf_force_traj_gen = create_force_traj_gen("lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt);
robot.traj_sync = create_trajectory_switch();
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf"); robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf");
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf"); robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf");
robot.rf_traj_gen.init(dt); robot.rf_traj_gen.init(dt);
...@@ -74,6 +79,11 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None): ...@@ -74,6 +79,11 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
robot.imu_filter = create_imu_filter(robot, dt); robot.imu_filter = create_imu_filter(robot, dt);
robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator); robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator);
connect_synchronous_trajectories(robot.traj_sync,
[robot.com_traj_gen,
robot.rf_force_traj_gen, robot.lf_force_traj_gen])
#robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt); robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt);
robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt); robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt); robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt);
......
...@@ -36,7 +36,7 @@ def get_sim_conf(): ...@@ -36,7 +36,7 @@ def get_sim_conf():
conf.motor_params = motor_params; conf.motor_params = motor_params;
return conf; return conf;
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, start_sot=True): def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, startSoT=True):
# BUILD THE STANDARD GRAPH # BUILD THE STANDARD GRAPH
conf = get_sim_conf(); conf = get_sim_conf();
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf); robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf);
...@@ -76,7 +76,7 @@ def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=Fals ...@@ -76,7 +76,7 @@ def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=Fals
plug(robot.q.sout, robot.inv_dyn.q); plug(robot.q.sout, robot.inv_dyn.q);
plug(robot.v.sout, robot.inv_dyn.v); plug(robot.v.sout, robot.inv_dyn.v);
if(start_sot): if(startSoT):
start_sot(); start_sot();
# RESET FORCE/TORQUE SENSOR OFFSET # RESET FORCE/TORQUE SENSOR OFFSET
sleep(10*robot.timeStep); sleep(10*robot.timeStep);
......
...@@ -37,7 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc'); ...@@ -37,7 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc');
ent.ctrl_manager.setCtrlMode('all','torque') ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(0.0); robot.base_estimator.set_imu_weight(0.0);
robot.com_traj_gen.playSpline("${SOTTORQUECONTROL_SOURCE_DIR}/data/traj_com_y_0.08/com_spline.curve") robot.com_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/com_spline.curve", -1)
robot.lf_force_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/lf_force.curve", -1)
robot.rf_force_traj_gen.setSpline("${SOT_TORQUE_INSTALL_DIR}/data/traj_com_y_0.08/rf_force.curve", -1)
robot.traj_sync.turnOn()
create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'right_foot_force_des');
create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'left_foot_force_des');
sleep(5.0) sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5) robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7) sleep(2.0*7)
......
...@@ -7,13 +7,13 @@ RF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rf_force.curve" ...@@ -7,13 +7,13 @@ RF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rf_force.curve"
LF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lf_force.curve" LF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lf_force.curve"
RH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rh_force.curve" RH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rh_force.curve"
LH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lh_force.curve" LH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lh_force.curve"
VERIFY=True VERIFY=False
WRITE_OUTPUT =True WRITE_OUTPUT =False
PLOT =True #PLOT =True
####CONFIG###################### ####CONFIG######################
from parametriccurves import spline, forcecurve from parametriccurves import spline, spline6
from locomote import ContactSequenceHumanoid from locomote import ContactSequenceHumanoid
from numpy import polyfit from numpy import polyfit
from numpy.linalg import lstsq from numpy.linalg import lstsq
...@@ -67,20 +67,20 @@ cs.loadFromXML(CONTACT_SEQUENCE_WHOLEBODY_FILE,CONTACT_SEQUENCE_XML_TAG) ...@@ -67,20 +67,20 @@ cs.loadFromXML(CONTACT_SEQUENCE_WHOLEBODY_FILE,CONTACT_SEQUENCE_XML_TAG)
traj_times = [] traj_times = []
poly_com_list = []; poly_dcom_list = []; poly_ddcom_list = [] poly_com_list = []; poly_dcom_list = []; poly_ddcom_list = []
poly_L_list = []; poly_dL_list = [] poly_L_list = []; poly_dL_list = []
poly_control_list_rf_lin = []; poly_control_list_rf_ang = [] poly_control_list_rf = [];
poly_control_list_lf_lin = []; poly_control_list_lf_ang = [] poly_control_list_lf = [];
poly_control_list_rh_lin = []; poly_control_list_rh_ang = [] poly_control_list_rh = [];
poly_control_list_lh_lin = []; poly_control_list_lh_ang = [] poly_control_list_lh = [];
time_vector = np.zeros(len(cs.ms_interval_data)) time_vector = np.zeros(len(cs.ms_interval_data))
control_list = np.array([]) control_list = np.array([])
for spl in cs.ms_interval_data: for spl in cs.ms_interval_data:
x = np.array([]); x = np.array([]);
y_com = np.array([]); y_dcom = np.array([]); y_ddcom = np.array([]); y_com = np.array([]); y_dcom = np.array([]); y_ddcom = np.array([]);
y_L = np.array([]); y_dL = np.array([]); y_L = np.array([]); y_dL = np.array([]);
y_control_lf_lin = np.array([]); y_control_lf_ang = np.array([]); y_control_lf = np.array([]);
y_control_rf_lin = np.array([]); y_control_rf_ang = np.array([]); y_control_rf = np.array([]);
y_control_rh_lin = np.array([]); y_control_rh_ang = np.array([]); y_control_rh = np.array([]);
y_control_lh_lin = np.array([]); y_control_lh_ang = np.array([]); y_control_lh = np.array([]);
for t in spl.time_trajectory: for t in spl.time_trajectory:
x = np.append(x,t) x = np.append(x,t)
if x.size==0: break; if x.size==0: break;
...@@ -106,38 +106,27 @@ for spl in cs.ms_interval_data: ...@@ -106,38 +106,27 @@ for spl in cs.ms_interval_data:
control_list = y control_list = y
else: else:
control_list = np.append(control_list, y, axis=1) control_list = np.append(control_list, y, axis=1)
if y_control_rf_lin.size==0:
y_control_rf_lin = y[0:3] if y_control_rf.size==0:
y_control_rf_ang = y[3:6] y_control_rf = y[0:6]
y_control_lf_lin = y[6:9] y_control_lf = y[6:12]
y_control_lf_ang = y[9:12] y_control_rh = y[12:18]
y_control_rh_lin = y[12:15] y_control_lh = y[18:24]
y_control_rh_ang = y[15:18]
y_control_lh_lin = y[18:21]
y_control_lh_ang = y[21:24]
else: else:
y_control_rf_lin = np.append(y_control_rf_lin, y[0:3], axis=1) y_control_rf = np.append(y_control_rf, y[0:6], axis=1)
y_control_rf_ang = np.append(y_control_rf_ang, y[3:6], axis=1) y_control_lf = np.append(y_control_lf, y[6:12], axis=1)
y_control_lf_lin = np.append(y_control_lf_lin, y[6:9], axis=1) y_control_rh = np.append(y_control_rh, y[12:18], axis=1)
y_control_lf_ang = np.append(y_control_lf_ang, y[9:12], axis=1) y_control_lh = np.append(y_control_lh, y[18:24], axis=1)
y_control_rh_lin = np.append(y_control_rh_lin, y[12:15], axis=1)
y_control_rh_ang = np.append(y_control_rh_ang, y[15:18], axis=1)
y_control_lh_lin = np.append(y_control_lh_lin, y[18:21], axis=1)
y_control_lh_ang = np.append(y_control_lh_ang, y[21:24], axis=1)
traj_times.append(x) traj_times.append(x)
poly_com_list.append(dd_polyfit(x, y_com, y_dcom, y_ddcom, deg_y=5, eps=1e-20)) poly_com_list.append(dd_polyfit(x, y_com, y_dcom, y_ddcom, deg_y=5, eps=1e-20))
#poly_L_list.append(array_polyfit(x, y_L, deg=4, full=True)) #poly_L_list.append(array_polyfit(x, y_L, deg=4, full=True))
#poly_dL_list.append(array_polyfit(x, y_dL, deg=3, full=True)) #poly_dL_list.append(array_polyfit(x, y_dL, deg=3, full=True))
#Fitting for control vector is the same? #Fitting for control vector is the same?
poly_control_list_rf_lin.append(array_polyfit(x, y_control_rf_lin, deg=3, full=True, eps=1e-18)) poly_control_list_rf.append(array_polyfit(x, y_control_rf, deg=3, full=True, eps=1e-18))
poly_control_list_rf_ang.append(array_polyfit(x, y_control_rf_ang, deg=3, full=True, eps=1e-18)) poly_control_list_lf.append(array_polyfit(x, y_control_lf, deg=3, full=True, eps=1e-18))
poly_control_list_lf_lin.append(array_polyfit(x, y_control_lf_lin, deg=3, full=True, eps=1e-18)) poly_control_list_rh.append(array_polyfit(x, y_control_rh, deg=3, full=True, eps=1e-18))
poly_control_list_lf_ang.append(array_polyfit(x, y_control_lf_ang, deg=3, full=True, eps=1e-18)) poly_control_list_lh.append(array_polyfit(x, y_control_lh, deg=3, full=True, eps=1e-18))
poly_control_list_rh_lin.append(array_polyfit(x, y_control_rh_lin, deg=3, full=True, eps=1e-18))
poly_control_list_rh_ang.append(array_polyfit(x, y_control_rh_ang, deg=3, full=True, eps=1e-18))
poly_control_list_lh_lin.append(array_polyfit(x, y_control_lh_lin, deg=3, full=True, eps=1e-18))
poly_control_list_lh_ang.append(array_polyfit(x, y_control_lh_ang, deg=3, full=True, eps=1e-18))
time_vector = np.zeros(len(traj_times)+1) time_vector = np.zeros(len(traj_times)+1)
for ts in xrange(len(traj_times)): for ts in xrange(len(traj_times)):
...@@ -145,14 +134,11 @@ for ts in xrange(len(traj_times)): ...@@ -145,14 +134,11 @@ for ts in xrange(len(traj_times)):
time_vector[-1] = traj_times[-1][-1] time_vector[-1] = traj_times[-1][-1]
com_spline = spline(poly_com_list, time_vector) com_spline = spline(poly_com_list, time_vector)
rf_force = forcecurve(spline(poly_control_list_rf_lin, time_vector), rf_force = spline6(poly_control_list_rf, time_vector)
spline(poly_control_list_rf_ang, time_vector)) lf_force = spline6(poly_control_list_lf, time_vector)
lf_force = forcecurve(spline(poly_control_list_lf_lin, time_vector), rh_force = spline6(poly_control_list_rh, time_vector)
spline(poly_control_list_lf_ang, time_vector)) lh_force = spline6(poly_control_list_lh, time_vector)
rh_force = forcecurve(spline(poly_control_list_rh_lin, time_vector),
spline(poly_control_list_rh_ang, time_vector))
lh_force = forcecurve(spline(poly_control_list_lh_lin, time_vector),
spline(poly_control_list_lh_ang, time_vector))
print "Trajectories created" print "Trajectories created"
if WRITE_OUTPUT: if WRITE_OUTPUT:
com_spline.save_to_file(COM_SPLINE_OUTPUT_FILE) com_spline.save_to_file(COM_SPLINE_OUTPUT_FILE)
......
...@@ -50,10 +50,12 @@ namespace dynamicgraph ...@@ -50,10 +50,12 @@ namespace dynamicgraph
NdTrajectoryGenerator(const std::string& name) NdTrajectoryGenerator(const std::string& name)
: Entity(name) : Entity(name)
,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector) ,CONSTRUCT_SIGNAL_IN(trigger,bool)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT) ,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT) ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
,m_firstIter(true) ,m_firstIter(true)
,m_splineReady(false)
,m_initSucceeded(false) ,m_initSucceeded(false)
,m_n(1) ,m_n(1)
,m_t(0) ,m_t(0)
...@@ -61,7 +63,8 @@ namespace dynamicgraph ...@@ -61,7 +63,8 @@ namespace dynamicgraph
{ {
BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector); BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN); Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
<<m_triggerSIN);
/* Commands. */ /* Commands. */
addCommand("init", addCommand("init",
...@@ -87,10 +90,11 @@ namespace dynamicgraph ...@@ -87,10 +90,11 @@ namespace dynamicgraph
"(double) final value", "(double) final value",
"(double) time to reach the final value in sec"))); "(double) time to reach the final value in sec")));
addCommand("playSpline", addCommand("setSpline",
makeCommandVoid1(*this, &NdTrajectoryGenerator::playSpline, makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
docCommandVoid1("Load serialized spline from file", docCommandVoid2("Load serialized spline from file",
"(string) filename"))); "(string) filename",
"(double) time to initial conf")));
/* addCommand("startTriangle", /* addCommand("startTriangle",
makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle, makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
...@@ -198,6 +202,7 @@ namespace dynamicgraph ...@@ -198,6 +202,7 @@ namespace dynamicgraph
} }
else if(iter == m_iterLast) else if(iter == m_iterLast)
{ {
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE) if(m_status[0]==JTG_TEXT_FILE)
{ {
s = (*m_textFileTrajGen)(m_t); s = (*m_textFileTrajGen)(m_t);
...@@ -215,6 +220,7 @@ namespace dynamicgraph ...@@ -215,6 +220,7 @@ namespace dynamicgraph
m_iterLast = iter; m_iterLast = iter;
m_t += m_dt; m_t += m_dt;
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE) if(m_status[0]==JTG_TEXT_FILE)
{ {
if(!m_textFileTrajGen->checkRange(m_t)) if(!m_textFileTrajGen->checkRange(m_t))
...@@ -241,7 +247,8 @@ namespace dynamicgraph ...@@ -241,7 +247,8 @@ namespace dynamicgraph
m_noTrajGen[i]->setInitialPoint(s(i)); m_noTrajGen[i]->setInitialPoint(s(i));
m_status[i] = JTG_STOP; m_status[i] = JTG_STOP;
} }
SEND_MSG("Spline trajectory ended.", MSG_TYPE_INFO); m_splineReady =false;
SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
m_t =0; m_t =0;
} }
else else
...@@ -378,7 +385,7 @@ namespace dynamicgraph ...@@ -378,7 +385,7 @@ namespace dynamicgraph
} }
} }
void NdTrajectoryGenerator::playSpline(const std::string& filename) void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
{ {
if(!m_initSucceeded) if(!m_initSucceeded)
return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR); return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
...@@ -390,9 +397,16 @@ namespace dynamicgraph ...@@ -390,9 +397,16 @@ namespace dynamicgraph
if(!m_splineTrajGen->loadFromFile(filename)) if(!m_splineTrajGen->loadFromFile(filename))
return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR); return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
// check current configuration is not too far from initial configuration // check current configuration is not too far from initial configuration
bool needToMoveToInitConf = false; bool needToMoveToInitConf = false;
const VectorXd& xInit = (*m_splineTrajGen)(0.002); if(timeToInitConf == -1)
{
m_splineReady = true;
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
return;
}
const VectorXd& xInit = (*m_splineTrajGen)(0.0);
assert(xInit.size() == m_n); assert(xInit.size() == m_n);
for(unsigned int i=0; i<m_n; i++) for(unsigned int i=0; i<m_n; i++)
if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001) if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
...@@ -410,7 +424,7 @@ namespace dynamicgraph ...@@ -410,7 +424,7 @@ namespace dynamicgraph
m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]); m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
m_minJerkTrajGen[i]->setFinalPoint(xInit[i]); m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
m_minJerkTrajGen[i]->setTimePeriod(4.0); m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
m_status[i] = JTG_MIN_JERK; m_status[i] = JTG_MIN_JERK;
m_currentTrajGen[i] = m_minJerkTrajGen[i]; m_currentTrajGen[i] = m_minJerkTrajGen[i];
SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING); SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
...@@ -418,7 +432,13 @@ namespace dynamicgraph ...@@ -418,7 +432,13 @@ namespace dynamicgraph
m_t = 0.0; m_t = 0.0;
return; return;
} }
m_splineReady = true;
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
}
void NdTrajectoryGenerator::startSpline()
{
if(m_status[0]==JTG_SPLINE) return;
m_t = 0.0; m_t = 0.0;
for(unsigned int i=0; i<m_n; i++) for(unsigned int i=0; i<m_n; i++)
{ {
...@@ -577,6 +597,7 @@ namespace dynamicgraph ...@@ -577,6 +597,7 @@ namespace dynamicgraph
unsigned int i = id; unsigned int i = id;
m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]); m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
m_status[i] = JTG_STOP; m_status[i] = JTG_STOP;
m_splineReady = false;
m_currentTrajGen[i] = m_noTrajGen[i]; m_currentTrajGen[i] = m_noTrajGen[i];
m_t = 0.0; m_t = 0.0;
} }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment