From 5ca212a20cc4eafb96d1c619eff58ec99308a468 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Tue, 17 Feb 2015 23:15:36 -0800 Subject: [PATCH] Added a free body diagram of the plant model. --- figures/free-body-diagram.pdf | Bin 0 -> 11021 bytes figures/free-body-diagram.svg | 1205 +++++++++++++++++++++++++++++++++ paper.tex | 259 ++++--- src/indirect_shooting.py | 41 +- src/sample_id.py | 2 +- src/utils.py | 21 +- 6 files changed, 1424 insertions(+), 104 deletions(-) create mode 100644 figures/free-body-diagram.pdf create mode 100644 figures/free-body-diagram.svg diff --git a/figures/free-body-diagram.pdf b/figures/free-body-diagram.pdf new file mode 100644 index 0000000000000000000000000000000000000000..04e965b202d4df43084144eeb3c1c0899f7fa528 GIT binary patch literal 11021 zcma*NWmFv9)-8-XH13ebErG@wcPBW(wQ={x-CY6%Pl5(_x8M%J-66PpAdpL*^Pcmb z``tg^9zD8h&sux0HP@^jU8BaNQ<0Q@$Ii-)M%Qp!`Gm#^1OXjQY|sP+fouxq4wi0K zK(1Fv4GjncvPoOpxtY7X9_=7*=91>7j%Ma)!op~-ZZ75!do<7N9D~t#IzETBL9M=J zQN!cs#fgC~7{k~OyRn{Tcg5Zz{V~s}?~b2c8KqXoN^P3h($sCr}^ZWeXHstqUYHM!t67wFo{X{^rS<{oluu56FDqe_owmE%AA;(JI}=D z#;z*NO0mJgd`_iwe!Cj?X^bW!IQp8MY{aF)5|DsX?vcQO**7?Z3bZU+oXJlM_18i6 zZe(Ue=lnvkR&sa9r&6_SN7SwxnbNju#SE-?nSBTDvbL`Y{@6%SrFtL?1WU? zfav`-2B{1at*;aYhoV}{tUhsuH@3Kh$JZy&UHTkU?k{uQ>pe47f|Es1kfKs0XmWxS zbK5tdcFd)693+;X01jcQ>Aye;YHmU?xmDnb6O|se3o!8xl~>dKhGNUfzl1@X@A@`@ zi%$qK8Ox{d@p z1N(IKi@8!|kQD~Oj_LB#m-K6|m&Y)J?-W~oe_l?FVsmMTmmhLTTAqGo5j-!w{Mi^} z3*c@9><3W({Jn5}Zn;b5_+a|>5v5|@IP_^TZbc=TyP8M>fvGD_7cg~zp!N_Qxv7!0 z(LiqplfZ8u-S8G;r9j8h&j2Q#q$(B1zQ^Xs)*j|l=E4ySm;4)_piMCtjRx_>Z0=)Q zgz6@@3S8mppU1}t)pw>9_+9RLEZ%{s1#@t`Pljy2@2nO+@+}Ja{LsQ2H2HE5?R|{9 zZE;#$qFk7UMaXUN#8RsabE|msv9Yfq7^d!4*EaHca%g>vHYHxI zvLO_C&U?QsYVxS!*6cc{u@cc7;8N@(1?hY=RBZ%;e0=1VJ{I3i>#Bi&b?lq6F&p7E znRClcc2ve%+O3_3nb%3TK8^5SAk4JR6Q++!j_c|pS+Xu1)h=`q*m^B#O-4UFFJc53 z8T(-Ve)>gJS>vfrN+(n_K2v|2lRw8awR^)CXKYy$Pb_2+K07o|k-w;ra<6P0$#r2U zcn!0Z;JSQ5=14NulG9s?aDds{t=V7A)Rbg->${j&?8{B|Bf-vAsKfzQ3y@6q@Kz6L z0bkGRZpsdQ*b|`#byAZ-+l<&ehOn@P&0D)K4Y@fh+XOH$w3+Cz(*RK`m2vNhOtk0H zRw2L)VbpHQS=BsAl}{qI_kO>{)v&!>_n{*z))6`Jkanz?-Pu}29*mj#TQxQ{ggsse zR7)I;V!yjW-C~N;8lq8AV?$eugkgqfpz+3pMRQKlDerAfRlV6ex+7AcZ3pQB!X3=< zSK{NF+*ZW=CXpm)@*!5N)$$3+$$TxQzXzNV+!O;0t(n7zoq1q0eMma_GJ>PUP7bWK zbzhF<$E!Mj%%|k?OE`;S@C>+-_SAr-P<~|ADZHrRb2)OH{4xsMaGISBSq6D%jiQZ(pFt<1diOK_q66>PS3b;d{VT1xnl(p&nh`uUbxg_I2X;5n6?nUbc@ z+Ml$A21ZfegYP{jV@uOr-kxmD*s|cL^O;dVJXQWo&eE5klu1*MDDUJ`a~sOg>Y2-C z$kHP*pQH~HYbe#Z5_8n$g)8`U&3e)B1=|wnmC$=qBVh;G^{CMTbd2hR-qYW2r(ATm z*6KhUX>L_aHUJnTCY&LZ2DB($8AurED<~$}DpqW|oQ?99aHF%sBt71waUagX!;Or% z<$_~kaOXFLrex*sec#nNAT{AH2CDFHULsJF2ZDXCe(%m=RbVMnBFG|cwQCG%U5vc7 zfnHEq&Lapkc+o-2J``cTS#z9@oW!I{MgbeCC=B;=L%E4_VMw%8eg*3QM{R?_Xbz0X zcrhJG%FN}VXd1|6mjpr-Z<ysG8BqJc>NN@B|@N?3ou-L`RkuYj%&l>o(K$tQs zW`!cbu(#dla1E!}54VcLhnhfz@Th*iZ|Bktr35wnWFG&Nlhpn;hfvYI-&x&8hU-_q zLGi5am~MCFyj_e7G<;(;mA{)?uU;x{kJDM)lIQDdJ z>5%c9XR|Un4@LGCy|IuWN#wMUT+OrA_yLcS$}`_&{IpJ*vFmA1^5iDXL%m&OkW3C)x^Icg$@cy#44T%2a ze7gppikFwo6(JLwTxk$VgKF`OL<&~d5phdPaPJ<6GXwN3bf~{jE}XGv`PkVI4Tpuu z2pUIhx+AQ@pH#Lg?58i!o>>vv-K-g!a$sF16^VT&Pq-ycS0>LUPubx4wjafrU|TJJ zpv5(@P@hEY=$rDvujqtAQbpS-a(W$lVG+ktXBYo8o0LBHIbKq!J?9eXorR=NHe3`i zE!(YA_>y@od&@XvdOCH#6+#o&m#6T5$fmi}Kcjw1u$5s6eR00ELNVKT#POt+xA6WH z`*3%w73MyZpw|y!WAO7l{i)zUgo1~XkCuiuX0W0YT(%`0llZa;!Slt}O)w6}LLgfc zcLG4za;zySKyq|4iWwM9DiHCN5h~N}1+#h>vR4ZSG(Vir;JaD~RZ z;Mzq!{Vtpv3J@6n7aCkrR$KA2V|4|idz=Nf>O2CO*%+6!y;TtrvN=u@HHrLeEMz`> z%FrrM;>z+OZuNsEcb5#<42d-Kk^9Vw-o^DN*bkpy^b5$9NIiR z8KB^Ftky{UQBmX$r_f5eK?IZ;K`nV`Q4WV{N*wMVCisO5juT_8k3iHCO}pA@)KLMM zrg3T?Ax28mwc#g1j1%Fz47KvyDPBwM(Q10dPFl2gvYV*LA+{0XWqq2Q^9Y1@b{t)t zew4@u?=NkH>>ABL6b-aTDq|dPqr}j^WJma+HSI}Z5u)V4tt7IHYmcM@L%>D!28BG) zh%{?~#Bvm_@FQxArU0Fm47Lwb11$7C#ztb_gS9yK$)wd0LWp#;minCw>(V}vHE_M< z&FW(vR0uBzDJFGX-%W|CzfsGQUnPCBIwYRnmp9nx8WtJkP))&0ETDctscnHr5PX8Z z41?+mz?8}{N+yKUZPMvCR!izP#JO_XvISo&Hs^+_p?rmQvUjc}JNjlE#Zt7?sR>qy-pk>fTWZN z&8SO_4|hnK@jKweAuIGl827EgEZd1*CO+j8enQCe zoILgTA}(GKmcUkA>Omp6i4o5_B5r z$0PXcv`VU%iuQCbwJ0IuoAf8Z3h$>QL&m6{S66YTTSNwE7qujweC_tIl%@@5E0ac9 z=XsW_F`b#`pb|WiDA2&#As^6QFCB&`^0makx@+GicAp=bMLC-Jd438Y$Y?rDVsV+wxqty>Zeknt<1yHPI=3#hW8K|c3hW+*sg4N6iyS;-=u!tL+ z#-&r)lapHvAeeWeND{!*Sim9i*(e0MP%Y*96`;m$O|~CLWIs4-0le%j3&9YM$~Oz70Z$(?X6y-&8|VLRdg-M6#zB;kRlE|5cSKfYU7>a1#?l4KLxQe$XVqw7>e^crK;6a6sP8 z*l63t8CXj@eAw@VI1R!?pf^@R`AxFl5&pr7K#ejB9Kqhg=sO=2Kk=?=5|bT#!T3YB4TSHN?-P*+#I z4qE~LvQyt>NRVcU8NxcED)t-7lT(GksA{hKB1BxWdRHs<(!3 zS0P^BP&CvM6ktllq0xMiNGws8$()D5OrDo3QfvZO87YpIynvRLa*)HDuzR(ZyLWxi zVKg-)cDju{3UL|CSZ=4m-RntO)!@1+JtOk|aOfVjQMf&B9EOiN?snlQBlriyIdojV z+M%->aV{oGnSNtTIMWQ90z!b6c1D_(q`%+yKd#bM_mq5xgJB7DfoTNAW5(}N} zLx2jQLtl|cAgQ0J#CM%Kt&7yO4E9t}U2PI;=utxq?a()+KZk9&kAK84)L%lPm|D91 z5SSW344t04W$wSQqPLCp?c-}`N2yB0ImrP>J+bkhlpGvWCw7=E^ounvg`JUP62+?d zj71m)96M-*gm1HMKmuiV%sG~!#5gR3y;T$r@n3M^+^eZu=yydX5bc-1%a=p5j5nXU z`*`?d>xWdx+rmp;_OM56z;YCj6;zRGH}O<0m)IIo`lAT#&>#)L^rMg8btlqZf~mhC zUCV<|9M%hufRunYF*ix5-5?s)ILXx)MC;Kf*Qp4qG5aR@gX0bsslI(>z^9V=# zhoL$(^D}(i_g=4Poa=6uzRy(C9ZyKkbLWvkd?v>cQSqS#QKI+cZK@LsO4A~bB`YF^ zyaAEKZp4V;z9JyOl~_ENlS!K(3%{iCs7V7N3ec$|_Gz^@mgKxbb-UdBcg|@G3kFc0 zdn)*p%suxqemZ)DB%(vi(bB?tc0!r%G+6VGY*^aMqlsLj+v7IEiW#ZjdZqdGPB%@X z`SCBs&xe44R{r&B#e=TjyV54u$`%oePeh9`)W6+Fe$>s0UeD<94e=AY zrDmvM;MNU8%Qs5$kK;LwUdrj_kr6Z!7`^$Xyp~p&Etqk^Fz<4-dK-9#CqK-7<;W4s zyt3Ra^J7vyR4&fmvYGPtV&#N`R>;9dUHO(+D6j7fgUG>1xBKQh#S`ZB!{qik&+l`S z?g09`Gz-J=lNB57jt83^n!;X$yA|#co)x^_!Yjg((TkEMTqE0#4O{N?hKSggs_~9H z)GYt-9m*8G)Ty(`Q2wj$wHm|tiefQGjVX)!%wsn>tRFNqGN>Zado2wPnayFEKnBGQ zDwLEeNc`z+qnSyt#-wxo_eJ7pnoZ6ubmx$3Ic!=ZCId>yE9qqK z&oE{_d$*%CI$q2odgCXV-igU%sB*HrKMLP#rwXHHTgBN9dH)W_1f5#uGyLPqCPpV3 zi^wtwJF|C%S3AnS&~F+iO=&{U6CM4k4fD#-BE5SvEKs6Zp24o`!_vm+PUf+4@cgYT7!Y37)* zIgj+iw)lp96n2g+)QPP>BVLX%?3c=l(1B!m#5?aHi&mpK?8Zejz>xSM@@2j@F|z2T z)@|wuYiWWybH{4_FwX-r=D>~6c)tYNGfCor3VRasobE`@`g7tJab>#L&zl3XmZ zCSH3@N}(L0->(fT8g?BU%l7!J!4v!km3`X(Ib0_rDO*-2N>f5dXqqRG5gTkHbVdoI zoZ9m(D<3!TGF6bAU&P9QU67v+2kxGhXr#V^f<~f*2pkqDIZaX+gEe90+z+RdSx;x?IseKvmFFAod}lz?;g7pFOz_AGUk3N%OrA55tb0 zv+QuYO<-FW7-b4LeYPKIfPKh|`~ZGG9oSW1PZW5OHg}`(4AIhZp~vVN;Roo8ipZgB&I)wQ=K^v!C8d#qMPI%k||9fFr^Z+jyfW5!qQy4rio z*@J+mr03U2NSS`tKL_#nym=%fg)_J+no_$Po8mg{hLX_x@tH`!V{}mImCnS* z%>uv`Qw>bE&LWlaQNOb5WHu54?8hx1JgB3_5Y?2hHG7fk${6x7BE3g%FYA7;HK&CU zrDFtl7*3-dhIaps5lhK()a zLe1O?2dH-=Fqrq)H2HbEj_Xif6h2GRJPr|YHZBpD+`ZW_mCiTMR49l&L5ZE@O%>D{ zjzwALs!F@xH`R#E>G@3TB0*cr zROiugAUiJ1BOPdYrp0NRQ3jzF`ToHp_kPgG_we0^vHh3tnFJ!1CfXB0Oz5%?`}7zS zAM)|9)R6Z$4?U$O1a~raH=l-SO+{*onp14vcE0$t5nm?;i90kv(@X4=mahH5MbKJ& z&eOt!WV5l+wlBU`F5)zsTy?8+UGVFzP+C*QUd*yaR#A@`tFlC&pPn6zn-#*2#B#PT z^lm2(^WE3lZww0C0(`Fvo6g$(5C6Q$m?rf!B$LU1A7r-&k&5gJ_)YY@`JH7cu{W$Q zUUKD5BBN69p`77++Ph0rEoQY(TOj{7(g0I&-!N8j6X}YS+1!qKpMa7jf7`fFF*M;BDp)WF(U*w~eTmYmW#VxOAp0HSwL%!km zxuwE=oObF*fYp<1MBnQ61yh5U%=>=*(ihDxP?3QOZe*QkkrhWi5n|)0ZC$hhok%^r zIY@s@)D*G9=&K4k1={5g(@hD)%FJ#ZAW`j8mPs8D_@A)0Pcjfm`7xYf;f>ofE4RcM*5J1(e0 zMFsPL;?S&icBCsNB3E3Yb|AXTocdVVX_I_1-c(fFY^fjUbqgWZTFG&w#uGb_!H?VK zIlEcBbSLEAyfGZLOI{9<9EYYgGCStDWEbOmocP@OUmhm3I!m30?;y6SxsBc99fvEWtx4^l0~%{;SQdX_O~`&! zcK7usBJE@bZI*~Q&)@zf)GZbG%%eYU9;x32Y;F9BfemxQnW=QjJB6R`H%^3s3@Mvb zAs}@pp1y^55Fs98Wc&OSkDa8XNBBc{A&#DxXt>V1OniX^&z5a~tlW)WunIX$x?Jor zAW3=63kmN7w755WRTcZZvH>c+xeh(+j@THUv=Rv@SI;?#SAx>Rk!w$cJjoU<->0%? zx*=eaJPpx?iACaR#2psKrt_NZSBG_^a&9CH)@@T>7C`9FFPh5 z-xWqnz&>~|Ch}ZX;h;I*C6Cfug3WB=^&YxSLe>}#V%RhrBfvXHpDC9-#pL5LjW26I z)<=j}pF6uyd1`%qIGS{J63vdj*2Aim(aDmr?&SRJD07Kvk*fW`dk&!fp@`^+sHNx| z{Nr|0CF}Bqe|0$UGFZ8QXlHkjIesi_l}+Y>1OVx`&ih^e0yCIc*C{-RIUL@xmGXOT z2l2j{s%%ELbA-q&@+T4r%XbkJ=KWQkks5(r93OHPha48*p_kUx@Jp1~O~#{Hyn=UjLE9mnzT#oZjlf5;2&o@Gf{M z6p1>&pq(%&8>nGhL~KWn)D1SFDd05&$E>sY-+zpu+9hBjYzzva@w_4Kob34wH1ZVF zOlUCpnZ)#9);y-p$WI@a7jI}~ot)osoxLY+YQ$dm258%j4d~6u^b*=<5?;2<%9W!P#GHz61xfpKw5n*Q@P6R?rdI+Z=ylMu z=~5~PL?w^H={}EIqC7aesTto=2|e%=3+z+(;g$8x?St1NnqR!3pJ=I>>r!q!(%`PH z5kYzL_B$w*s!VD@2yx(i-g_VZ9n30fJ^zOF(mJ-*d`}2dYg>uf%V3(kfviF4q2@BQ z1)o}vZ56}+oMSn=VAs)(uXN&rN6w=*Li}=Iw9GFmUT;q=*h=`147RGg2k}*kFI^!- zT1;4qTXWeC#bZYpXMonIj58t2tlZ*doXiiO}=uVvOcYgKlR zHTavV+i>O|E2|fi+-cQ{u-DD`vz_myXJn446-rhB>+;>#`x)K$P&ui@Fd1_RF1Hnp z&CBL4U(mW^f7Vkdte4>gRn9z}VZSPo1x&beghj-gQL0J?RSz+CWX7T3KxSge5$|Rv ziknEzGThja4M1Q;`D}&%}IYfq@Q6Z`X7p0fDUp1 ztqIU|94y_9+{Z@BAt#4cGSl9n(0D|W&;9Od=}TKK*5#yu{VX30{o&aMe>exA=>$Fl zWl8eZ_bX6i(yK8nmiyXzp$ii|CF@!6x6IE8gK?ROU?uM=?3}w7?iH5|JgE$mxz}p@ z2~w-AD^2uu;PLNs%HutM6GL7!j=F<~RZtuwZOLjm^z-7yd}LY^Fn_gc^-^bF2qSbn zazVW|jyTQ>NSf5DXNm9|a$6`9mDDYbd5(xo4AyZP-$NuFy_E|&6c$RVy~TAc5KehQwIUoYg1UzT(%mlk&tgx9;snqf<-)j6w?kOq=2;M<9Trd zAwejrjk?Vz8f%2+)YoRo8FXtAK<}k5+C?0R*FWwdkF(Q$|K7xLliMFeV_V03lN80zkXU9+v(`iH zYo}&0kgz~|@UY4))5>O^dZgJ|f)a>LX0yaba%Zwj&w~hXWc(JUoz~-#R#YV1ZMy2& zK$65Npy_>|c{bKDwi`ZOrI}^xk>R!9#R}iS0H8DwC89PUHnVQkGq%}ZPRWgRjz63V zK$G;$mE&04O3zerkVX0##-2o!*=&f3N!5?@R>cqOIrjmjRf>D`4>IqPdmawHVArCX z&tMG;?HTbm*6AC)i>`!mkCrb81@^Ac(v&FU$Qc@=ckDU1TcTfkVx@E^S}#Z=b=JeZ zR^Wb$)Q~}Vjf4+toLKS_exbdllfR#uN|32ciiJMXnc2u`sMXyHB*Mu1#knC z$Ur{ahiO{hmQa#rpc6Zd{WOg%bH-H{j3*#v2IPy4-LfN?k>n>I1UbxSG=ulaSzB`3FV8Gv2 zSj3!fv(tM`@OR>-|sav?~gp7G}k-&zY=;2Y^3mfW*o`vbsgA zu`m5ZaWCcL+?&ejwJ)1FX*@UMnmh<#mx8v~FCMF&LO%23-MzfaV1EQ= z%7xMUmdR^TqTT^w#a(W#M2qIth3{fX8(C-IrNWt{4!Pj8Ru~7h_>rom{J`AC zghalNok4jWi@?Vkk6xhqx3)`h`+SL1sC`;mPl8)6Z5~tnhQO(~@pBs~sbZn2v4j-0 zqvtBi417P0liVL+^wrh0_oU*Y zX+yE+IAgv=Z8HpnZPuq{QQBV1anAYmecbGC;6`linJp<=H|z2=&VIYxU2lv1=GRe0 zY(1U&pO+tFF4_mis2061q#i)5^%_l(X=EfcXc1v!IysLLyW;dyS62Cvgod+?^375i z=!u&a6i#%gzrFK?raAGZ7n9`=ML6*u+8^CP%)8&Io*VJs$^W1uQ1MBlnLC*MkI(M) z?5{)buPyKI@zt2e4g&G;{EhfK=lG9el&71Fy4$N?4=5=37kC|P5YX!c$R;5M z1hI1Raj9YY8q82Un-RiA}x!4a))DT%iAy^q0TH zYm}t9yS1sgnvD40kbjY4Q!{sUgu0lTy8{0W^ zzgN-RUl4opat+>7(0lY^Yu3TJAe-V!Pxm;75l%i*9rS;d;Tws9mEdi z`+qP_&}*gtpEz#5|2K|@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/paper.tex b/paper.tex index 93ee5ba..19c9de6 100644 --- a/paper.tex +++ b/paper.tex @@ -1,10 +1,20 @@ \documentclass{article} +% for images: png, pdf, etc \usepackage{graphicx} -\usepackage{booktabs} % for table /toprule, /midrule, etc + +% for nice table formatting, i.e. /toprule, /midrule, etc +\usepackage{booktabs} + +% for nice units +\usepackage{siunitx} + \usepackage{amsmath} -\title{Controller parameter identification using direct collocation} +\title{Quiet Standing Controller Parameter Identification: A Comparison of +Methods} + +\author{Jason K. Moore and Antonie van den Bogert} \begin{document} @@ -12,117 +22,183 @@ \section{Introduction} -Given a mathematical model of a system operating inside a closed loop system -and experimental measurements of data collected from that system, it is common -to attempt to identify parameters of the mathematical model that cause the -models response to the measured inputs to best fit the measured output data. -For linear systems, there are a variety of solutions to these identification -problems, but for non linear systems the solution amounts to solving a -optimization problem which is most certinaly laden with many local optima. - -We are particularly interested in identifying the parameters of the controller -in a closed loop system. It is theorized that creatures operate, at least -partially, under a closed loop during locomotion. Humans for example have -proprioceptive, vestibular, etc sensors that provide a rich set of internal -measurements that guide our choice for muscle activation. If the map from -sensastion to acuatation can be identified, then the engineer will have a -mathematical model of the highly evolved control system human's employ that can -be used for biomicry in robotic controls. This is potentially useful for -humanoid robots and assitive devices for both able body and disabled -locomoters. - -In this paper, we present the use of direct collocation to identify the -controller parameters of controled inverted pendulum that is excited by -pseudo-random external inputs. The method is compared to more traditional -shooting optimization methods and also a direct identification method. - -Direct collocation is more commonly used to find the open loop controls that -drive a mathematical model to track a particular trajectory. - -\section{Closed Loop System Model Description} - -Herein we make use of a well known and studied planar inverted pendulum on a -cart as a our plant model, except that we allow any number of links. The cart -with mass, $m_o$ can move laterally but is restricted to the origin by a linear -spring and damper. The links are masseless and there is a point mass located at -each link joint. The external loads acting on the system are a lateral force -which acts on the cart and ``joint torques'' at each joint which apply an equal -and opposite torque between the adjacent joints. - -% TODO : m_{n+1} and q_{n+1} should be m_n and q_n in the figure. And l_n -% should be l_{n-1} if n is the number of links. +It is hypothesized that a human operating during the quiet standing task uses +feedback to remain upright in the face of perturbations. For various reasons, +it is desirable to obtain mathematical models that predict a human's actuation +patterns given measured estimates of the sensory information available to the +human. Reasonably good models of the human's open loop musculoskeletal system +exist but models of the human's control system and the system process noises +are still less than adequate. The control model can possibly be derived from +first principles, but high level understanding of the human's sensory +neurological feedback patterns are difficult to derive from the low level +neurological first principles. These high level control descriptions may be +more easily arrived at through identification and learning techniques. + +Here we present a numerical study comparing three methods of identifying the +parameters of a human quiet standing state feedback system. The first method, +direct identification, is by far the least computationally intensive but +suffers from bias due to the unknown processes in the modeled +system~\cite{Kooij2010}. The second method, single shooting, is a typical +method for parameter identification but is the most computationally intensive +and often suffers from extreme sensitivity to initial guesses. Finally, the +third method, which has not been used for control parameter identification in +biological systems, is direct collocation. We aim to show that direct +collocation is better suited to control parameter identification because it +does not suffer from bias, computation times are very reasonable, and it is +much less sensitive to initial guesses than shooting. + +\section{Musculoskeletal Model Description} + +We make use of the widely used planar two link inverted pendulum model of human +quiet standing. In particular, our model matches that described in +\cite{Park2004}. Figure~\ref{fig:free-body-diagram} shows the open loop system. +The human is modeled by two rigid bodies: the legs and the torso. These are +connected to each other at the hip joint, modeled as an ideal pin. The legs can +rotate about a pin joint relative to the ``platform'' and the platform/ankle +point can be laterally accelerated. The centers of mass of the legs and torso +are located line connecting the respective pin joints. The muscles are modeled +as simple joint torque actuators. The orientation of the bodies are described +by the generalized coordinates $\theta_a$ and $\theta_h$. Gravity acts on the +bodies in the $-y$ direction. The equations of motion were formed symbolically +using Kane's Method \cite{Kane1985} using SymPy~\cite{Gede2013}. The model +derivation is included in the \verb|src/model.py| file and implemented in a +class named \verb|QuietStandingModel|. + +The numerical values of the model constants were estimated using Yeadon's +method and the software package \verb|yeadon|~\cite{Dembia2014}. The +measurements are included in \verb|raw-data/yeadon-measurements.yml|. +Table~\ref{tab:model-constants} reports the computed constants for the model. + +% TODO : This is too long! +\begin{equation} + \input{eoms.tex} +\end{equation} \begin{figure} \centering - \includegraphics{figures/n-pendulum-with-cart.pdf} - \caption{Free body diagram of the n-link pendulum on a cart used in this - study.} + \includegraphics{figures/free-body-diagram.pdf} + \caption{Free body diagram of musculoskeletal model used in this study.} \label{fig:free-body-diagram} \end{figure} \begin{table} \centering \caption{Constant parameters in the plant} - \begin{tabular}{llll} - \toprule - Variable & Description & Value & Units \\ - \midrule - $n$ & number of links & 1-4 & NA \\ - $m$ & mass & $75.0 / (n + 1)$ & kg \\ - $l$ & length of each link & $1.75 / n$ & m \\ - $c$ & lateral spring damping coefficient & & Ns/m \\ - $k$ & lateral spring stiffness coefficient & & N/m \\ - \bottomrule - \end{tabular} + \input{tables/constants-table.tex} + \label{tab:model-constants} \end{table} -To close the loop, we assume full state feedback and make use of an infinite -horizon continous time linear quadratic regulator to compute the optimal -feedback gains to stabilize the linearized form of the equations about the -nominal operating point, $q,u=0$. Since we are not concerned with performance, -we simply set the $Q$ and $R$ matrices to the appropriate sized identify -matrix. +\section{Control Model Description} -% TODO : Maybe include a table of gains for the the 1, 2, 3 and 4 link -% pendulums in the Appendix. +To close the loop, we assume the human's sensors allow for full state feedback. +There is physicoolgical backing for this assumption but we mostly choose it for +computational simplicity. Given the state vector: -For example the open loop equations of motion for the 1 link pendulum take the -following form: +\begin{equation} + x = \left[\theta_a \theta_h \omega_a \omega_h \right]^T +\end{equation} + +we can close the loop with \begin{equation} - \begin{bmatrix} - 0 \\ 0 \\ 0 \\ 0 - \end{bmatrix} - = - \begin{bmatrix} - \dot{q}_{0} - u_{0} \\ - \dot{q}_{1} - u_{1} \\ c u_{0} + k q_{0} + l_{0} m_{1} u^{2}_{1} - \operatorname{sin}\left(q_{1}\right) - l_{0} m_{1} - \operatorname{cos}\left(q_{1}\right) \dot{u}_{1} + \left(m_{0} + - m_{1}\right) \dot{u}_{0} - F \\ - -g l_{0} m_{1} \operatorname{sin}\left(q_{1}\right) + l_{0}^{2} m_{1} \dot{u}_{1} - l_{0} m_{1} \operatorname{cos}\left(q_{1}\right) \dot{u}_{0} - T_{1} - \end{bmatrix} + T = K * (x_ref - x) \end{equation} -% TODO : Show a block diagram of the system +Therefore we create a gain matrix: -The loop is closed by replacing the joint torque with +\begin{equation} + \begin{bmatrix} + k_{00} & k_{01} & k_{02} & k_{03} \\ + k_{10} & k_{11} & k_{12} & k_{13} \\ + \end{bmatrix} +\end{equation} -\begin{align} - u^{con} = \mathbf{K} (x_{eq} - x) \\ - u^{con} = T_{1} = -k_{00} q_0 - k_{01} q_1 - k_{02} u_0 - k_{03} u_1 -\end{align} +The acceleration $a$ is treated as an exogenous input. + +We consider a process noise, in our case we add noise to the reference. This +primarily represents the controller's error in estimatign the state error, but +can also + +\section{Data Measurement} + +Several variables are useful as measurements, $y_m$, for identification +purposes. To obtain the state trajectory under the two exongenous inputs $a$ +and $x_n$, we simulate the system by integrating the first order form of the +equations of motion forward in time. We use the variable step integration +routine available in odepack's lsoda routine and acces it through SciPy's +integration wrappers. The measurements, $y$ are + +\begin{equation} + y_m(t) = [a(t), x(t), T(x)] + v(t) +\end{equation} + +where v(t) is a random process vector with a mean of zero and standard +deviation. + +\section{Direct Identification} + +The direct approach can be used to identify the gains in the controller. The +accuracy of this approach relies heavily on the ratio of the system's process +noise and the applied pertrubations \cite{Kooij2005}. To implement the inputs +to the controller (-x) and the outputs of the controller (T) are assumed to be +measured. A linear identifcation model is constructed and linear least squares +can be used to compute the optimal gains for a set of measurments. + +For the identification we assume that the controller model is: -We excite the system at $F$ with a sum of sines to create a pseudo-random -input. And add two types of noise to the system: process $w$ and measurement -$y$. +\begin{equation} + u = -K * x +\end{equation} + +i.e. not affected by reference noise or deviating reference + +Measuered states and joint torques +$\mathbf{X}$ : N x n +$\mathbf{T}$ : N x m + +Unknown gains +$\tilde{\mathbf{K}}$: n x m + +\begin{equation} + -x \tilde{K} = \mathbf{T} +\end{equation} + +The least squares estimation + +\section{Indirect Identification: Shooting} + +Indirect identification is based on minimizing the following cost function: \begin{align} - \dot{x} = f(x, u) + w(t) \\ - y = x + v(t) + J(p) = \int_{t_0}^{t_f} [x_m(t) - x(t, p)]^2 dt \end{align} -\section{Direct Collocation} +the state at any time is determined by integrating the eqations of motion + +\begin{equation} + x = \int_{t_0}^{t_f} f(x, t, r_m, p_m, p) dt +\end{equation} + +$r_m$ : measured exongenous input +$p_m$ : measured constant parameters +p : unknown constant parameters + +given the initial state $x_0$ \footnote{Here we assume that the initial state is +zero and do not include it in the objective's unknowns}. + +To test shooting we make use both a gradient based sovler and a gradient free +evolutionary algorithm. + +The quasi-Newton method of Broyden, Fletcher, Goldfarb, and Shanno is a common +general purpose minimizer for unconstrained problems. And we use the CMAES +algorithm which is has been successfully used for control identification +purposes \cite{Wang2010}. + +\begin{equation} + J(\tilde{K}) = h \sum_1^N (\bar{x}_i - x_i)^2 +\end{equation} + +\section{Indirect Identification: Direct Collocation} We make use of direct collocation to transform the parameter identifaction problem into a large scale non-linear programming problem. Do do so we first @@ -158,6 +234,9 @@ \section{Direct Collocation} \end{align} The goal is to estimate the uknown parameters $p$, the controller gains in our -case, given noisy measurements, $y_m_i$. +case, given noisy measurements, $y_{m_i}$. + +\bibliographystyle{unsrt} +\bibliography{references} \end{document} diff --git a/src/indirect_shooting.py b/src/indirect_shooting.py index b9c3a06..7890f9f 100644 --- a/src/indirect_shooting.py +++ b/src/indirect_shooting.py @@ -19,6 +19,10 @@ from scipy.optimize import minimize import cma +# TODO : Make sure that we are simulating with the MEASURED platform +# acceleration. The identification simluations should be using the measured +# values not the actual values. + def sum_of_squares(measured_states, simulated_states, interval=1.0): """Returns the sum of the squares of the difference in the measured @@ -73,12 +77,38 @@ def objective(gain_matrix, model, rhs, initial_conditions, time_vector, return s -def identify(time, measured_states, rhs, rhs_args, model, method='SLSQP'): +def identify(time, measured_states, rhs, rhs_args, model, method='SLSQP', + initial_guess=None, tol=1e-8): + """ + Parameters + ========== + time : ndarray, shape(n,) + The monotonically increasing time vector. + measured_states : ndarray, shape(n, 4) + The measured state variables. + rhs : function + A function, f(x, t, r, p), that evaluates the right hand side of the + ordinary differential equations describing the closed loop system. + rhs_args : tuple + The specified input and the constants. + model : QuietStandingModel + method : string, optional + Any method available in scipy.optimize.minimize or 'CMA'. + initial_guess : ndarray, shape(8,), optional + The initial guess for the gains. + + Returns + ======= + gains : ndarray, shape(8,) + The flattend gain matrix. + + """ x0 = np.zeros(4) - #initial_guess = np.zeros_like(model.scaled_gains.copy()) - initial_guess = model.scaled_gains.copy() + if initial_guess is None: + initial_guess = np.zeros_like(model.scaled_gains.copy()) + #initial_guess = model.scaled_gains.copy() if method == 'CMA': sigma = 0.125 @@ -95,11 +125,13 @@ def obj(gains): # This method of parallelization is taken from the cma.py docstring # for CMAEvolutionStrategy. - es = cma.CMAEvolutionStrategy(initial_guess.flatten(), sigma) + es = cma.CMAEvolutionStrategy(initial_guess.flatten(), sigma, + {'tolx': tol}) pool = mp.Pool(es.popsize) while not es.stop(): + # TODO : This gains is a group of gains for each iteration. gains = es.ask() f_values = pool.map_async(obj, gains).get() es.tell(gains, f_values) @@ -111,6 +143,7 @@ def obj(gains): method=method, args=(model, rhs, x0, time, rhs_args, measured_states), + tol=tol, options={'disp': True}) gains = result.x.flatten() diff --git a/src/sample_id.py b/src/sample_id.py index a6bd78c..0124fc5 100644 --- a/src/sample_id.py +++ b/src/sample_id.py @@ -46,7 +46,7 @@ print('Indirect Identification via Shooting.') indirect_sh_gains = indirect_shooting.identify(data.time, data.measured['x'], data.rhs, - (data.r, data.p), h) + (data.r, data.p), h, method="CMA") print_gains(h.numerical_gains.flatten(), direct_gains, diff --git a/src/utils.py b/src/utils.py index 9d50e06..42e1771 100644 --- a/src/utils.py +++ b/src/utils.py @@ -23,7 +23,7 @@ def timed(*args, **kw): #print '%r (%r, %r) %2.2f sec' % \ #(method.__name__, args, kw, t_delta) - return result + (t_delta,) + return (result, ) + (t_delta,) return timed @@ -41,13 +41,13 @@ def print_gains(actual, *identified): def config_paths(): - """Returns the full path to the directories specified in the config.yml + """Returns the full paths to the directories specified in the config.yml file. Returns ------- - processed_dir : string - Absolute path to the processed data directory. + paths : dictionary + Absolute paths to the various directories. """ @@ -62,10 +62,13 @@ def config_paths(): with open(os.path.join(root_dir, 'default-config.yml'), 'r') as f: config = yaml.load(f) - processed_dir_name = os.path.join(root_dir, config['processed_data_dir']) - processed_dir = os.path.join(root_dir, processed_dir_name) + paths = {} + for name, dir_name in config.items(): + dir_path = os.path.join(root_dir, dir_name) + if not os.path.exists(dir_path): + os.makedirs(dir_path) + paths[name] = dir_path - if not os.path.exists(processed_dir): - os.makedirs(processed_dir) + paths['project_root'] = root_dir - return processed_dir + return paths