From d4703f9e896f2f2500a4cbfe484439442955f2db Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 12 Feb 2025 11:42:04 +0100 Subject: [PATCH] Tangle matlab files --- matlab/mat/nano_hexapod.mat | Bin 0 -> 2406 bytes matlab/mat/nano_hexapod_model_conf_log.mat | Bin 0 -> 223 bytes matlab/mat/nano_hexapod_model_controller.mat | Bin 0 -> 261 bytes matlab/nano_hexapod_model.slx | Bin 60009 -> 60081 bytes matlab/nhexa_1_stewart_platform.m | 72 +++ matlab/nhexa_2_model.m | 293 +++++++++++ matlab/nhexa_3_control.m | 486 ++++++++++++++++++ ..._hexapod.slx => nano_hexapod_simscape.slx} | Bin 8 files changed, 851 insertions(+) create mode 100644 matlab/mat/nano_hexapod.mat create mode 100644 matlab/mat/nano_hexapod_model_conf_log.mat create mode 100644 matlab/mat/nano_hexapod_model_controller.mat create mode 100644 matlab/nhexa_1_stewart_platform.m create mode 100644 matlab/nhexa_2_model.m create mode 100644 matlab/nhexa_3_control.m rename matlab/subsystems/{nano_hexapod.slx => nano_hexapod_simscape.slx} (100%) diff --git a/matlab/mat/nano_hexapod.mat b/matlab/mat/nano_hexapod.mat new file mode 100644 index 0000000000000000000000000000000000000000..320b631360729ab18a60847df5377c909a90b616 GIT binary patch literal 2406 zcmV-s37PgyK~zjZLLfCRFd$7qR4ry{Y-KDUP;6mzW^ZzBIv__(PFO)UG%O%Pa%Ew3 zWn>_4ZaN@Fa%mt&Wnv&8Hy|-EIx;ajG&3MFFfuhDARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr(B00000000000ZB~{0001N2mk_6Z2BI`0Q<(SL?&3+H3i7Kfdm;&mJZ| z9U{ITT^wr8em698K>m2ah}|DNP=7EZi6i@G1CX=FJNfyor_bzR;Uo4opu9Wk*?zSf z!O8;Or|su6*jI8?zO!GDuC1f-^|Za3M-NxN=TrNMYc85gc3$4k{PRJM#JQ*TXy!DC zLd>yOgqX7*X3lI>bN2T_&0&F>V|BhI&gA-gxH$)1*K)hH?6g-lo$01AW0U=&x`kOB zuYmTa8U1@={CvMt*Y~&Cl^gd9I=s!8ANbs!6>5&J!4u|tf-m6ys+^qsN?Yvd{*?!& z-gtleqP^|Qz42y$-`MY9GgR1@bjAK|*x4ROsmJzHFLTUNmAbtjhdB*UbL^Kx%-Ih! zXFIAn`zNEBgXS-W^ALaS*N6CP|3aw0CPV$@4e^(~Fw|eL@BziQ0iJY=p5K^&ED#%{ zuYy2&XJ$Z6AE5kZcX%Dc&g*mF?nVcMKWb*q&Vd)gtTM zA6B*4Q(C_H`o}vl_$Gl^K}mEd-;n)%fPi_}%D26C;=7&I=kDiFJ}Meyyc<@|?B4_t zx93C?hpMwb0F{S{M+E;@YkaxcJ{(HJ#G!n+JWQM&YVJuib4JUz(ee#B-Hw)T)GyyG zNG#vDp!^;NXn_U>-ifd<@h*a~oZ({5a53*RII9FM3m0>Si}^CZ)xlYL@fDR-U{*yY zOsu#hwIDt%GY8@fO41Lh`pC6!R3B~hMc~oL#(-Mifzr7O1Mz?!=Htu9AoGyRFD4{4 z#OTLW4$PVf0y{CvFBUZYFaXmpfNbvwFF%;b)JLv;qxwe64{YWS)$(HpQv05i{*@#U z=YRykg$P=Q3d#HgJpO`(IkLY%{`inkm)VxR!u~y!eha1Fq0umTO4=Rdg!`YBA20bP z1yI=jDSFXkxhnB*#!_Dgvy3g;>i#y8&c0E zLnycS^u)|OhFpkTE=15LwU7Y@=+&MF`OCqHpDg=~$aE*U{v$^p$bWn%Rq5tGkbSa~ zyvedJh0JiFBtN3`hbh#bfJZ;B@(Kbb?19i7PAhF!#gz}|9@MP9vR!vL*=-2CHx85}`nlOt#Z z*9pXO04M zAJ>M7qmOID#7E;}c*F;;`2t+y|LEiS00000|NmqC`5;H)+*A9zVP|_Br5@W)z05I7 zRqFPBR#fr*lhMSzyS~58uH0xZ?C>^Ye&BO^7N|HM0|UbWAZ7z%4k!%*tWZ8PRE!ZS z&I!a5fLP6=hb!Onsr|$?7tJL*FYjlDng6C@4o<{?foI*_R*Ja+Aav(Vjl&e_eY`e zBcXh#y8Yya>mo0k2OX^zj`>n`bE+5K+IuAQ>AFLIH`}hL&zp1b`P{vEQobE}_XH10 z-CUsh_2MpDw=1uh=4#*CyAY~x;jvi}dzmlTLe)76RXR>y_HVE4hW7!>svP(2-KX%G zzs+H@?Vbj~cDACMw((mJ%bPp8Ix2kNso%2kh;8rbH9z<>H|%YQd0ut4eevEWVTsael9P{q;1>4xtd673*K00K}E()8#R%Oe<>S&bByaet(fzFcr?}v8VE>`26 z@8rEi;|SSMjzQ*q;~9M?2g%ca||vp zZQZj^=*b#6AxSrvgTiskg;8ChTXn5(}b29H}2lP@c$>C zykhHASNAEKYa^{6MemlntI+~CuShwps9tI6?zr{BEgKmG?EmmNh5u*XX`QuPq1Qui zzctK#SyN7xHEgQgUAZ$+-i5c=dY`S^tmlhbtz)42M4q!w?LKh8+O1uXXT|Ob)-UBW z87xIScVCo$6U#3iWGxx(UKaCkgZ0bzLhi>O?6YPsaZ9_h_qQD=TrH5w8z!`HWn@4t zZ&(>vQ1}3%UfX~V0001L0001Zoa19)VDMmMU|<8%96-#(00FE}J|hwasVPp(Eyzg) z$+LjffW$?BxTLZmmB9m~GYCZFLXBbI17ZUPg!ycE%x3`G&j@CN^jVOgj}t0x3A-ET*8l(j literal 0 HcmV?d00001 diff --git a/matlab/mat/nano_hexapod_model_conf_log.mat b/matlab/mat/nano_hexapod_model_conf_log.mat new file mode 100644 index 0000000000000000000000000000000000000000..76ad1910009d00de469353e00a3e0eba2c045bb5 GIT binary patch literal 223 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NSfNGBUI>F;XxxFfvsj5-`93qo*%FkR1-h6>}aZCnQ)f9?5Yy z!+6ApN8kY0Q)8t>0eee(eO)_s`?*saS=X?soMSzrI+b-LkK$1tg9GeG*gT$XdT2PE fF-KoHWci^xJee($z|YX8!jlI8nw&c} literal 0 HcmV?d00001 diff --git a/matlab/mat/nano_hexapod_model_controller.mat b/matlab/mat/nano_hexapod_model_controller.mat new file mode 100644 index 0000000000000000000000000000000000000000..f5e93fef8a57b087ad51f425cf7f2f07b8df931a GIT binary patch literal 261 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NSJjhAoWhcnuqzU)OJcRqDb3KqQ+ S6dXKBX!7R9R}3yH;y(diS5%S! literal 0 HcmV?d00001 diff --git a/matlab/nano_hexapod_model.slx b/matlab/nano_hexapod_model.slx index 4b49cd4c65311aff4288c3f614512fe025f61f47..8f001a50af3c19c22d8a5fa3d2de9fb35486a6f5 100644 GIT binary patch delta 11209 zcmZ9y1yG$a*DZXI;#OP>6gjvRiWGMaUaZC4odTuEgS(Z3JH?8-ySux)yF2`Szwe(r z_q~}(W@Tq3lgUgn*?X^pX}F3RxbkMycT5@6Dz?xLR0g1v4Yz0Q0atddt`-g#pDu(r zotm95`=Z2xwY3+eBSH^J=I>DvNbL%e;i&kQS#?v@ZH-hqXb+G$9p}X;* zG@aSKcH!^O!n92A-z7Jaj&a+Hn$ge5FKG!^|UTMKf8h{ zp?v2YoohbKraI6+$>wHa` z3zIcbDGIeq^oa;z>V@D2pP!yReng@gtBi>8=53mJ*83 zw|<;h6?{r`)pMRWNzrq88o8OiB!BQr$U%6e3hv^4?Jgn=-9Z8grbd zx{LX;X1A{!snb(d@0D}D8G8s&NEZCY&Sg@MKIh4k%(zs4s|7}XQU~tUGO2EGP{|(E zkeiS^7zykl)pvO}q8spf3hRu6M z^BY$Ni}#N*b?k=!d!opgpN4xo)piHN=m~E7x!$TY!R&eqoMztC${@`khdX zqGR^Qr4y-!uNr{BYawlr%sA0aN!gx)yPhhvf#;h_cdNH&@RjE1(LnzZ$A{#ly9bFL zJ1~woRq0m|q{;9nzHc3kf+S6F!JpsJr3!xd`$gH&Ag66htCe) zN=G-Kmb*)$B_?X!&k$j{8bo&%wIP=-`Uj12u#}61|!B4Bo0|b8Gp7Q{jIp6U|(Q} zMU_OC_BE_@)ov!*zV8Z!;Ld-HdlgSUOUiNS1UNC5?#!m<oh@b0Y@gcF{8tGRLvuUFV;QJ*pYR* zIs^?hKMMTCF7R84KjD_X0PNm zBhc+RV!8}lmDF39C(2^(3Y-8|!fQ$q?D&0u*wj;aw1`Ll_UFSOTCu_1>Q~14Kb81x zsav&833x$nA-dpJw#Q3{_ho8eMbIn4?p{{22Su6A6*=wpdien z3%2CMD2PbuGv|4HXY#%97xQT3AL|kA1jtO z1T)F+{rs@vf;eV;rjLpd{I#CdUNXi)T4XT;Gx=op0>OH{?W?;zzEe>xxwL;bJWnJX zO>FO!MrEW9r%F_Us*v%wKuF9YEQdiunnfmi_k3-Hs*j!&wQ!x>_7|%!U0?!NQGhx& zrBGt*Xh4tW`2)syGg6281S(gWVXM`0da_Zf#?OerdfX6htm#CE7Yli^6$QcC@RPEs zLsMMwIm0rFTEf9Ecr$hAd5}zTTqRy$9Fp3l!@`^oQG!dESm*4Csj8k;V=->vQl8x( z-g3uar$~gW`hZy~I4s|vV%B*~bHJP835rcV_Vs+{tY@L{6YnjzQ^=o*b9dw(h7_dZ zb8?`ygZ4bkv{ce+3HO~Db`zqORMEJ69(||eLN`-H!1ZiqsPw&bO?T!YXpWlkbY?8t zyEn}3%VbUD1l4HE)30P+`V@JZcdu3r%U(uFy{xy^Fib^pawS;3$q+x@A^>H7nsJ>V zi&oVdekS}J3p@1dm`?qc1Iq1tVM*D6-f8|svtK+=1XfyBmnFWP7?PnRGg;7?7=$1= zW7Ab6f&56V3cb>9wY~c*|1`cFSDmb)OvUqO5Wn-209V#Q8dn-Kn|0|RI-*P7CU<#R ztpDMoJ8d90Vs7HXmt6AURDk0rdy(t!6mMyh(Hp0`B(2#$12^FLF=YyDEA- zqX5^Vm@2oI94W5n9yXAKyo^%>+}f>4oYy>?HZk^=qaJUn8Kr_nwCyTb5Cf- z>p??DQ1!LFEA0(b^-X4q967pM6n+gB1oD7#S_l9WkG@FOhxz^81m%bGNhhkthfx_g z$+04$2fE#Y?>)xS&Klf392yCR62D_o;Nd+l3;vp`WnRysh#ea{QI)b&JVDK$d7*Zw z%%n8^Fh@Z}>N{0geA?gG!JfLrZNZN(>dycEE8~)d>%?CS`;S;^M_(DjiK*?w4} z+?z%L-c9=rhmN=i3w-_+f5()HX{5|TQgL*OuzoZlF5+|8FJ`3j8{b(qFIY$vJXZ^i zV4Bk~u{{<22RaxIIk&$NhRK@fYbp$P-t^(GD}8tUK`s zAY+M0XLyatKjZcGuj!{*iD$z0aM-Z;g>2EN2lM;24NcJl&^m}=qlqxiCJ2ZnmJB@3 zI23PQWZai(87AW9$nSbxMcxiw&U5)hB2smSYF!jKFN;aIDJ>cB& z5jKgr9@u}gpDBH56jfn-H#oN8&$C2VlOqyP>x{7>K zQe@^hmp|%oL8#WQzr>GpNUCDe3!(bxEb1uZDmZNm6LE7yq=JI^o0&P67Z7wl=ww$5 z4m!DH%QzeCOs%HHcEUp@V!W1@mMre9B|k{Rz?&P*;%nOaa1C4GJSS;tW)>ljvp@dH zB|3@w%kPJgMUKH7F6u*bFpm)|4z6I+8BSqxfBsu`A*1#Y-XIHO$Gf0Cq^4Y3l=1sX zPa7D>nU>5j1AVEBRzd|!;gi=Rauu6(B?)>Cb|Vi88VGdD4FXX>+kHr&D8a-)C7IoV z_uTS>2#n03Eu$3rBvRfVjhv3p3xh!0hY0x2aQOSwm8GH_QYJ}(m(%q0^os>nMITmy zb!r;r`_?S7B6Q>+h3y#o+t!NA^kv&;$Jbr!YB_q0(KYGG1>r_97_YVZ5K?gi^{r=a z`7bfaCsZ=r=ugsVXoFYjegb|(tVYW`XNImD>wB4Rp?}sQlDd^ZwYkBC#=|6*fcsF~(#->|*VP%oJ*F zY*LG&_AnvWvAT%v09_a_OlbobiYXKKB`r*z%(hgO$!L;kfu&VSjep*-cRLDU;k}QU zeET;UEFCo;&lZbzM12HT3r(&+oX%AZFD}7tEcVF@jW_|gLK9v*eaKmKKkOH_b9ssM zr4f<6DDEo={uD&5#NgNfnRoWEqft8JN`qWKTE7nYTgXrwiAs%e^ z@L!JwcpsR_oq4Dp%mov%U4?`R(b%v0TAO_L+U$fomA1w*+Q=^uQ36Lh?#mR|fpscG zMDMryJ7*scfH%~j-Nc6%)|BT*q)7$EDFS|RVdI2tjq%iHt;#H*@tZA`T0mKR9@U)9 zLgOH18+nUdvVlx;YS_~|=I$?=RyKZR25)mm4kGtKmcl(Yo`pn_$C`PRB(+=pHgyBI zlN?NViULJ59t8*tJtS+GuUH$)@c{xk2bCN3k=7JTarr7UF>2WdKIWEMDlMtM6WkXM zJq1{8j1P3D#44?TU+qdm7sgAR1O!N+dQBb0?saS?91VT-Pm4mXpE`B zFH#CVAUuZkpaSO|WW8W8l*Z(5Q6t~Vb*+cP@;b)sKq~4Em1Blqo_d56eDKmrxWxfTDtFE+oIf~T77vyEjm{yeu&?v_au83Y!?Hn z4dKhV(^sFM`GaN1p?lp0$%6!IWWs*bzCB60e{fwt60F-vS&e%on!2@x(QS&6)C@3b zMOVQFh*+0m(DbLi8y#bYw62T8_pXKS2K(sHu&KjIw8AKX|a(#7N`3h7TH)TnRI-#sx%z3E$ZD9 z$S3c6%kFy>R{Na9444jOEL@Ha#V3SSL-o7)@FW_wbIw);by9b`G$$`@aR$j;Egtn)R3Wm&zKcBRs2{qj!u_8Fx3k5v+#?m7<;W6j2kU(W7DNz9^Xm2q+B z>M#FHWewP#ncZL}Fau-9nl+M68v_~7LDaWgLI3Ws7?9{GCF(#ET``Id!uk6v$u+xZ zZ|Vg>Ta6uZ1ho33I5?V@x>lS$FQRRPN^}6y&&@)3n`BHJ&;0&)jS72GOKF)$gz}Ce zdUTBKntr!&4eifc|}ocC4o@q}0eX7K@MGVHU|S!iQ=B4R`VWRG~%~ z24OZAAzb+1GpYhH&(ZiICG`mCIy}L$%mz3iwnx+D z2nK)WPAOGognnF1MW;oxH`XPNQYDZj(QL!;xV4Ls63FXLT-#2cOz%_DupAEv?@?&D zc&il5#Q`P;Y9W>IQ%O?(S(17p>Q(cr)5fJ z)X5{!`3;_(RIs2?soM`J8wZBH%FWf<*~09R48BkJV8c#2lMBAY_PD>RAr;m}v7R9% zctz{7L>^KKkEzb%8Gz|9>2;b{_r^hbiQ;-BnqmCgdMV2MsiPiPO1UoeXXsX%&yQZu zHg;>UH%sI=n?*P9XQ88pfuu5S`g!wD1XoGIw4;4v(%=g8HwiH1>IaYtKXY~k6zPT2 zy0+-n&6T88li$(UM?N1Ka(dNppkqg!LB5yeswRCe(@wIGuc8~Nj{mNglC)%}*D5mr zFH3{lXjeIyFmqS%6}z!jatZD{$VLvM`gr!hBe>I?qfe0zA)j8(YAJ3{lGvI;)+NAf zBG;$V!Un3tVXi3wRt1*STu^KGf!+m=@1LN!p-6~5uurh(RNbqz9>{3dwPeL?NDH>f z&Y%2Ioz(Xlxbb$rhMOv>HBfQ*LAXu4MFuEmsa;hF^6V5=aCQY@ZrC-0`^k5M*fk&D zJIYEWJf<(0nAy>PskFYt89KgplzQv(B71v6YC5&n!E)1njR7cSzkTm#3?<-YpUZ|9 zL>o;MP^^E9O;fMe8^5ff#A00fVzV-9L6k=^SEg2M=4v}li|UXfU|F*Gnx<@gCRX3% zcOO&c<{?8xDw>s4+7vay_nK@I?>dji;b)R*dLjUx!r}p4wK!MDE|}8Dy<_n7Y$$4` zpvEYc51jc$!VK((=m!v&v!FTsi06@LT{L+|34LHj)<7DK^~HHFtA%Wm@?8suOeBl` z_Nti`rCKi0y5nz5o_?(}T~#`THDnk?Jp?WZuf}(1_|Tob$yCNhT@f8P_##|CR9G5SDXV2zOFm7UZ)EfXDmM)eB1xuD4CVQ{OmWiJn z#4f~3HQYGiR!d{j@lAoGGc3)=MkE&yd@I>_h=m}zoEF^_ibWpDh4NRHy4;kdLY9fu zxuR+YtIDCY8igL`H%(oWBDIF_rn8N!#sH z%Q5hmT^#t5Qp4`jS8q~mUE%O)BcOJCrp4OejZVvamqm*dD-3Z_6z5n%L$FW-u@~RT z_ubw{G^%8qmrzLGc)C+(!jP+=z>C&h>!;VfOt4Oron1~x&`^q()TRcqiNwS7F&)Hp zdB-loNKN^d>vKu9L)^u`a$n&_O7qEtUp25L65!AGvHJ^w;B1Z?CG6JGuZ`>pGR)k`_H zVKJ6aK6xA(g;2=xXkzKh-6vXVY98Ta0cOnp?!Bh7*0XxQj-Dfq&$>v8A0>m@`9ArG znD?8$*Fh6u*ww+*z6bjqXnpLT6k-qWn7_wn8o*kvWW&0|JqNz2_X+FvdC#A^q74PU z02zL(KZp2S*(kH3Z+DXlyvVX`)ow((Zy1rMm=1%P{iJ(MuO3PlB1cx1KEb>O4^KuP zC$E00ZAKRD;5lcuKDAZs|Frc{3r;8`-hy-@|1^Bqj}zbQ`MWB}i`X+9KWYt5rZ(zNWzt?!L&q$g*HlnSFPg<|9yQ0i0t& z7iyw3W9Q~iP8~UQEodfN+ay_iFE>*&Sva?K|8|M7)6=;y(_0_! zI#zn>rSH4)4t2c^Z-Ep6KfZlavV30KujemJu?pO0Pt4g@gu*2||IlMD)$Py163)%F zKFQzxCI`@q`kfCmKPn2^l}D+a#R9VMM_GKUZGoW>r4`A)NOqaO(3PH~G>%x*-Bn;B zHT-6(u3%3H(+f58KNN(mO^jg_?LctaOi+!8na#1lxnLzK;nf~xG1LTlhy;^V=$7%O zeqSl8n>^VHn*x4n?hluqO(q)!(>Dq7v7a`lTX-SqTWC%!7(dA5hX!hXmjS}%BmM!W zzd9F9PyFLrj(9e?IRBzCFfB{Q9*e*j=zZGD=a1EzoTHcRX^_le4>MN?qYq)1wa|48 zxn^&h=;@j@Z9O1mK#|3e{hTe1pFljH|6YK#<225%R@nQnxk+JnW%3vKH41|juSEZH zmqlc2$aLx62T0*}XpD9ClL}zDK^C_=@nN8Gyl_HJZL%V#v(?nwj-W6vW{$$($FYFv};YbYICf8>_vPT8aTpv)1PBuwDm}ixC1UPo5m@tx$n}-B%LM3F-L3MoW-00NLIB|TGE`Ug_V8=z>}_Lb%1lq_rSD=d zL;JxQYu2Xi`T%I=^}5)-xnX6)a(bD}^G#^0PVjnL(`$V`a8+!pTJp3B!R7o@X&L|R zEA8SV+UEnZ@zcdRmPd+?FGA^nBACFuroM|8vgOV_0^>L&1_4K~yBIDUw?|$TD zwH-KxHODB3C9Rl$gKS74qS3GKim?FEMOdxK_RaYA4g%}7uY+kYEG9EA6f6X$kZ@%q z-;dS!4gr1)QiGW;=OO*0Nvfieyx@UAgg>f}Fmtu7D}Jf;Ahvm}g;Uh7Y`84ap90<6 z24cr!tiWs6g-~Qh8N$M>z!|>&M7MaJFCg)!GIj{ci{ClHm`|X_?IZWahfWNvt96STh>Oy$$Ci49;}^#TMbx}^xUILwqt${*i1CF} zc>n7H!Hu~Sqodeb30Flch>ixN$4`A@bVP=_WMYc8%$)zj@Ho+@0`t7M4md-mrV%7pnteF!bh;Z6~vx##BXP zq5ZJC-JsKu0SyvAWkw|YeG9|XIPS*-m*4l=6o=O;p*wR(GLHE!hn>I~0>I*{!rhV_ zCwC>y6p(ElSF#jG8!9q06Q4tuH(6d-twd<_KGPS39K_#SOO~oPLZd`)S|rhf^r~5V zW)Q`%CBNCm%I4R?hIJIdKDgUZU_r$;WAdhA@7as3N@KE}%1%J+_;JbQyligv$gXoT z=b1nDWOxV83Ac*J9r-Jp6mYk|pqIIDOaTvV7A{Cs`Lf@OV05AZ8yOT%o>$0 z@bWNnQ3M{-^SU#_Ot)u+pR-)R_3ZjNFy)efCdw_f3tyKxBL;n+9!lg$WDgwC zD6K4GX7iI7aHew*aIHa2d@nxW5{*S=F)@RT9z?_?epN_EL{06Z>Nvn;!kJJ}jw7|} zik`z+eX4&a!iHFZsvELkYxa>#tSC`i4weQ_v7It7G8ltj6~zl2ueONUaGsv5&PES> zV3(~6u+bY{4)P=Hc9IcyGWZ%otYOwn`qe*rct0dB_|HN7HZ@ifT`+xRth6Zx`Xr3a zm!Fz=Q1Wz2;3^H)EDM%8&%sg?MuaWD`_g6YpOx)mmoA7T5f|Q&gxVyKL!6T-meV_^4^x4Ib z7=#%B{2bV+k|%6xKL-=ZnKEUO?8=$Sq907eqd&7yBNr2}4rcqIVK0{o$8{PK9wY6y z=Pd_xbtZdaKfz95yoMnHCY_fr!3HWZ+SRY&>6`OUu7)b4gZb(%w~WSj4_yJ zCQg^a7X2G{s}?=gCg^yO&HYk$@HkH+PM9eiU{BuKci6pA>XKQwMMsW(S{Sn}zoDkU zL2#POq*k1U4~3B)p(SRFt+$XEixVv->TO``A|z+fY84y5mdZk;p8hp%AHPlnmUXl% zM;-UrP0M{RdVL$f`ePYgV+lS>M&=eB$$5J*ZDkt!%R%9XhOIc2n;e;=EXj)M>j3xUyhT1Y#&RYx=rvcyL(cRNky$DfY+ zTYF;_;b%xA(1BQBOCg8~m-MD4w^Ho!!v;TQm*mrC6S8zcMFwBeK78PbU;OK;{ zwqe7_UugKTa;UdLlZgvIa>wo-AfGf2{32(SK|Y>I)3zWtt@>FRv`(>$djdzyW=@@3 zXahF~{GpVhhuoHmBJe&%eV`=+n@o<}op2WpE4B({ql|deUVYyC)SS)inL4` z-pnTRXJcyNsrs&f?*RwN$_xIv4M`W4>L)!5T_umMB!x=L8B$-|l%t+u?+Xkw0l8aV zx#~=^7w;TPt+fc(nM{(X&4LWxLrUN#3(W#Y?k`H{83kON63+98H z4m?$^_E*(?UkQ?3>M00q>tAV^#pc+cfZ`cQ?g8e%>mc*VDvPy?oU$MNLvB z8?o~%3P3u#AE_k^RHGP?i^-p#04_{eweuHxvdpt`J#m+lJ+Exfm&+H~ie#>Ew#Mxk z(WNtHibWD970!MN2@YY=Pz43W+#*{-|-~I3N1At+v&yV_2 z(NTJR7u%xv<=FtYbA0%@PKNxXpN`xPWTm%%GZ`eyhDTK}y)U35Exan}B))2`HDyHK zqgo11lSbCywh$fNR;{yUU1^z`?J#8Q} zGiPPKakY_M`;F{r%j+sB446C4HnvM|41#{DGqU2m&ze>!;7vBLlCpHM)*lYny;nzx z%z>>gKE-ohSarS){i)BKurBb4O$H{HY2K$o=HL)DJYu@&+NvS4m5^xJjp3trIVZu7 zy%1RI&wsstQ~UIi)n4~Rm-5Rd$94%K&I^CIM{1vyTE*x%Hu|C~I0P6na_N@&h%`rI zs*3a`!*Sc#I~|v>1(M2779b10H5K=1rYCK=nAy3@=6_L3ERw%fZH^LPY38J3m02+^Y>(RZg>u`gxA?LD`?Nu^wLa= znbf-yrjyXuJqyd#beNm!F6Z7GL3gyYzVb7L7!~I48k6UJ6gvPV%n~JVe!^Kt72nj| z{d6l#P4(_ta<~R#GWC`{XSsTrN8gmTsb#j?WYBAu*=1bj7!ROKFjbqzrNnRyIUx#d z;iDfL4#_aE&WW>Q?Jdt2Za-eK%-mjr2#^edKF_)RP|EzwFP1x`a>`d(aaDZ% zSq#Yf>S|((DG8#?^xTt4`XqgQCa6T$T}Utc67<_mQ2r>x2&F#U$WmQ0MWt+8wN z#AHtQdKW%5K_IaIPF$B8-lrvsf6qJvWlKpGnuoq@2*LGG0vmry|9YFZ`IpoqoJ&bm zSx#W8|ED@_ii!}7TN&9qX=;$pvy3Jlt~*>x$)*um@1hIgDSdmD<021R%br)rM_i^BnHZp%l|GJju@A}$}Ja@3qXzi z06ZRdE1Xwd zo-d~M#@aj|SE3T?US=ZNE=G>FG&1--udWBBp5NXw-fo75xZlq1z+-EqZI6#9zYI;NxpM^NMu=rhcnjJ(TeE(uX-nsKzq-4+%V_?&#Z@n- zRrm06F@OmGPB#R9tzQCI6?FgkB9|E?mv#?IfX4)7T%90aK8(v{CvI{L&!4I zdwaS6d4ygcxQBvjhaI@;_@g98Ewn65!?uiS#HhTI6yl&o%^_5;FPdMMEAf7$XtaMsOG&f^{UIrAO z1-i%7@PLcodi?GWw?}Z?E}$yd~I=om5@n8VgXIHXa7kvaj z=`}t-KW_@HrJs%XxdX>SLSHn`FHSTx&K(}-v4A@RFpsh|!MLt`kRm3%&`8jl>GLa_XILNF7$=y23X5Rq^-*I8vE+2Y?rrCv zECX}x4EFz5hQe*?!m6P|?a;}g+?!ZX3kzvBLkBSUD}a{5rfk2 z>cPU=K|^;Tu+Mf-gMCuygbdkv(=;BIo~s(f{Cs%m2W&J$9HW==*&Im=&o0 zz5;9>0PXN0gO2R8!B+VEgH%w7e?LQx17g@Tzkl5iQ0oIc(*H{H|JT;mgZ`0-p>+p@ vr2i+bhxtzn@Lwdl*#993$09+|53xx8-yp<)xc_^Iolu@bMg)zu|5*M%BG{n( delta 11164 zcmZ8{Wl&ztvNaamf(CaD?(XjH9vmJ#B#Zn zYi9TK^z5qs(Y;pf0`&1bbWJ(ZtCKCf1zp2ZI*}16L$tx?g|l} z!vaeN-q$GGiE#v#-L4~oL(!%NXfdVriDosGUG@w3GX8l`A6EGlFe3n6&8IIY+Kzt) z*jEEJFX-2`V6V4F{Z}vVM|{^^$b<(jD~3|;DxAwQj1WZZu_Qr}&7sk?(#26hwlPyh zu*Xp5y<%||SlJ^=SI?ts4{v!o*S3aM=~}>BVlJA zBwTHzmh+Le>GUba{zm*`6+*o&rjf>^Yq`=Jw>4k1EdE>rJRJ=OK@QwG$`cljyG6a1 zeYy&pT@Fbh>Tx4j5bh|kNrpLEiRiUH)cy~&iaaz776b$Y{NF(a5lsainSg+(w?$!O zf#Dv27zbwv(f}h4Yb@v;N1u^5Y}7F5)Pk!ZK&^xk`NJ-#&B!3KRni&IY&Elme%*ev z4BGj?A~H#+Tsv~$E!9Hz35AhCcs?o6s>z8Nr7}=nzQ?R-Y=V!4+(Io-f*LZiGyn!+i~%aG zhC|W6F6H1m;#l*Xa9%8Wu}``_}tLk zb+^2Z8Nl;}jy!};qDo7qIFW)ttvp{NG`vXKNXU0{qeH`UZBg?LN#SsnsPcE3Yd2S2 zz%B~?mh^bHv)oKiYX>Y-)LB5$r;EGkW-@F);Qo_OTxQiKS3@cH2% z^ElWPFu3G)q4}Au?vp1oWu=mElOrhaUO%uV5Mu4t;_}8MCaWn2Jk`j#PfPQ-FIg+G zz5^A+fF`()GL_*^mE+;$g%C51t8-0pm=B~^#EstMsqDxp8*%#D{iEc7BMqhmR2Rgx_5;tJ;%iq0~6my}yO#f^XPUn_ z@KtA-+my-1Rf3-*s7}D2pZ-+E=eRGA@Vj*BO+fna{7d(Z|L(<5k*8 z5Zt+mc9q2Z6~FH8#ZR07Z1cp#Dk+gVH01W-!-c}B?xp_B6YVIl$#DNNXg&1JG5bTa z>Yx!Veipkdls_8DU02~~wjBM!D`B!*Uj8%Xmli9z!F#cZ^icH_rL_E(ps`tL{;N@fVZjJzPz?PElz$_ zaQR^if0|o^VsKXnYtR<+=yM+)_9Mr(3CdwNu|j%WuGg%M7fscVd#qFJWz0U<%ddQJ zhSjca4@~V^s-u-;qQhW$6IXg(_V?{bQ#Z&Am-SZ97f7zaDW==Rth zD)o4SZG#78yt|pbb^`rbGMWClgBrW8kJIbu77v!2v21dUdL!x83HoFwYzDF0)72|~y zmIar2^@1JL6yv!RN-QlHD4Sq4B?6?&sRGKO4KOR4wS>iy@h=ZH6qfkJVrlO$PIc2| zSL{EJd^3(rI_FNdeKC`kH?gTQw+&{Q(ZG+=)f8zn0wBBKIg8AYG}wE^!|3D$zwPJ} zTbYnRnl~rdi%kT26oZXL{VZG z5C!!pxVE_@VpTX&bgMUQsbi*Z>dsk**96mZ6E3$zY;Jd|VU(XeK1U7?LV1noukiR;ufB-^FXQ2Jft%wmfBLMUWhR?h#eJlc^=Xz^DhwuD|50G6Lmep`&QuYwk(v^=0Qs zP@jdLFc+a9AZ$?}AV?s<(FbH;LmON`bqn2J@ij=n8?quMYengUCh(Feq6Ukgcu#&8 z?&N#nJ+-kHIoGKpG42UdR=Rz3HxpV6f<%MK^LBTj;qwM;jd`N)aKra_QCM!8Vexh$ zGD9qW!Ywk!Y_E4J=I5cW?$yTWttiz;!&>;YNnDCPjA$?>g;>Lr$$()$Z94;~>xPOG z1RoLk>2e!y9OMdDJ`EtdM(argf#Dml6vmUM2IeRYhq=WL_+OJ?cn;0jLO;ZO@YsC0 zSv`CthGv+kJ{FapOn`4zni!Q@``P{xX4#Tx3{eQG2reK!86#(s#}d2QA1gKy;+RHY zT8dVaD1-zdiPHqJ;n~`S8QL2d7~RuQAOD4EUg|OpZ!W>Rn3U=tlpSXF4N^40(v^-} z;n5l|@I0zT)^yFZlC(OVIh7ZaN>m%^xc=O`FJYGs+E{#DwqbeDbJVcVc4{pC19PNo zL6GX3313&*7YC-(p+nxM|5m_#GM(4li@aR|bRDaKDh!G|VB|=t~CTg~L?BeFEMEi8O!sSfzN6IQ$ z{y}+a7SWzauPl`TQlVFp0fTLPM|3wM1SXz#iREf1b-&SX=lV!-UR$zem6UBW=j%PI zrDt}>H1z`Zbp(j%bSIS$NDvSU++dMJV(|MhJeW8P16Yn}vhTLLc**sn{MwXM0BZxI zJ`*St2&zkZ_Z4KvfM>_pc6E2GPH?IDbo-~x%hPkvs!Jf)|Jo0n*gx}}?p(QqMc7g+ zoqzni8t`OWe>e7+QNP1h#3KL{&|3K%pawtqoKw4h4A%X5vxU4TQ^p;O)n~u_+a|4Q z%1RG_2J}cjnWR9q`&Qln(sB!PT&N0d%FBi<$bZ^G0Cw#6s_S@CKOkA>O#ZGVe4M79wO|~lae?zABFrFv{(|(=% z;n+pAU5%9KqTjR^wo9`bX~{!|CCS8;-p&#VE!B{-4HrPt_Awo18G2bJWOE3ChxblI z94MQfew1VUQcRT=g+SEGPME*?czM~$Waf#uj}f4Lb+F%LKaU+{Ua(>!*ZvA&%F;Vd z;seDU=&AcT6`yIA^w7;xZ(3QdgEO~yiz%gzw_ z;fkxq#E`qnvLJ7ju`N%pHFSY`_UpsRCxH68U21Ip!~T6iiYz;=4A|MZE4Sslv|SbG zH|MWcPr*@ycNLUh%{EHNW150PjT#jPt+sO&?7iU zBzVQPE!h=%r3Yo;@Qcz`&3jRf4x3-TgTrHjpxe&$6lxcRt_zBVdLNV0ohRF|MxX{U zbDltHPPEhUsNI|g;uV);fCS@uyv#CjQkjGCz|@nFoX;x?%zRu+5UTgSABigAl^EB%SLZuiD94sS5HTq34zOFVU9JehQ5m_>6&qdY%?BU;msp zahoUPnm?+-8}d66t_^a{vlvk_9RlQwlXFglapCTGUh+i*PT-Q&YJ~KuFnumfhE9IJ zCt2i3_6`X;hauwbE!3sl4%!2cu0UE0aRScaMCRdkbW`+<3UNrbFQUI)BNLc;;{C(9 z`|!+6v=O2-MdBSHG?psJ2IHV!R(T(`GhtFOcn708VktjZjDJWWT@mPJfdNzpq@2Y} zTgp;5$U3`B_>gr3p*6MUxN_42Bo{3uH7m;WbM7;J(Ui)~l}9*0ot21eji&qF8`N5n;kS`*&4L|wm}m_B92he_}R+t1Y8`}4y; z3+}6nrKo5V%Nu%NRzlsT-2nnxXk*DKr}j1{)ya&Cf#BX4{}l%nQ?9Sd&0ibU%R!)Y<2jw@=Sz8x6hgI7Oi=>erv4Eh@;i)1VUwa)bOjy zm4X=8I?=4ID7@%b%I@a5%#CW1ud!AwRjd(w?Sy9?hGpK#I!PLKH*%$V5|I_rQJU8Z=vqN< z_0;5ZnkA`~^9c(hp&<)ehejJ|G-_!C2 zJ1lI0auW}s*}d(@7D1I9kO4 zs1%o+I9UEWIwZ6m{-7!iwY(8GEx(Lic!{qTaTGER42y-br}$K+gddx*NXJSan5gNP zmgF#=tOP-A)Mv6Or#%O1U{;`^|n1Id|y zRUV^QIgE*4penM`kWxG{UiSdy0YseVSDT9ts+qXm7C-9|&{a4mj&4qU0NQV4zKBp` zT-qMNl#*DrUu}}#i9%VUkH*YAx!r8yS90<=lpHGNYhVG>(xnSBO)hP)s@w~fFlQj04iF-GXP6exYpAxV~j8ogdN4ry6E)l750ZMFhD z`_J77g>OUnuTdx18pgVLgNiIC8}36JpUV(gvF_+7A9!?)nA+)z$J-R;@$--nW&JSj zBVb5#WjL-YhrRkf8*6Qq=!yP1!3Uvr zlQgpOPS@#|+ z@hJyz<}#iizr*tL2#Wjs0s1Rwh!$BaA>%i7h@0TyLDn~QHL8gjtOzVh-_^W(n9Um^<9Z?|p(Q7;}0^CjmEy{`c{j!6DG-eV14WlKy~ z60o|oYDcP9e|^PSVZ5{EpkH&>%5Fg0xj@~O@4&e#LUx9^wP!vv}O4-veaJsE2AMkNQsxd8Q_s6+VEf!OP6#aEacAZG; z>rkEIbzPY)ml%#G*b>fS)=*!Um4bb-?#Kej+a!94+JZhxFJj^5^B3Q8nWhBLzs4Az zOe^a;Xl4wYoF;&BE#(FSwoB%SVg~yF(Y8iI7xtc@MrftFWd^KL_Z? zTNiA4SvLS#`#qNtc?8Gppup0@#GaK#*NG{gw%B_pMVl}ADigETpcCoW@fP3x`Hb+h z;en@{SHT})gUvsy5zV5dwyPzc`g42*G2iWoPSY1VFddVk38G?SV~1jI_juB53;el$ zH7?+xK+7+wrK6`1dvUZ$JUYk$%3CbGXM)eYIjq;$Rm<8iSPPfWYT==N`FON?6HGAC;7rS?Jb(g ze#S!xkFYMe+W|H?D`oCXiN*m0y%F~@x>(w0x#_1}Xu3z$b#9G6C*IR0-+;LlW4?y! z-TndHqn(AmHs`{cgmDi2ssw%IcZwO%k2}bW8K2?VnU=4aKSxLmL>8 z)`|1pUwm&v`ZV(KW+c&ND^~2+?8gpwe8T;Od*?X%*NS5knSKPusl$5r%{G->UL~+G z4RtH$OX5rKADBm2iQ?a=H|}%#aTiR@ygYh!v+Bc~N%H*s3S(#aSsVPPOs3$Wf~6x2 zuzAS0P!2b)l+AIm?HL`RqRO1%8AP@w|1qs0!RBvZh`@>SfSCt_<$^8Xs6eBdR}~XP zU3@C*p~%{RvJf%E&ozLj&s8~P>DMI@B`YQm$^GdyYnKwNO6|AxfZ{R8unURctT{27 z?g$wP)w_VI4gSpK*X7B5lB_GxJA~W?NIgtpBV?NiY`CSfS8nXBpi{0qXiwS-V;1EU zl`I~m+^a{W=m8RI;utMuSDtW8m%v3cbPl;fTPu| zS7pN30b`ekQ{xW3{D(rPgHvP3E;wQwgWg3iH=o+5O(bbCx6WmP-sOzCbQt>?K=JfY z#ukUk$XmVW3W=as$Y@9^r7TkoyHY|>j=KqV&o}2#gRV=gKVIX zl6Ogxl=jB^t8|Gm9ka;+|ClOkfVYKf*A}KNvu2u3Sx4Pbz*O>*FY5aT6kS+O90!Vk zZ@GTc>X|o~I1zL^r>Vs+3S_gXQj{f==tMy zbmfGA^lpT+|1>g+eyn*u?8kDKcS zy2JOEsT^jXuqVecVIP<4xvR*z>hmr$rHq_ke>|C)=pA29PaFr5j30029J@An0=~VE z8Y{(Ytx8|nwYWH0;R1I@DZ=kN4|syLa(vhL(H4) zoyX4=+grMl%`ZtEe@@@ut`C&Eyq>Z)&g?!H#(hy6*Qk-vNUj6L3;Vuz>|8%d)20T# z;(snO$ltG|7K4W-t*epK8r2tGS=u7%;&}s;oFa8q`&0#j5A54s==7>@Ko}mIrWK=DZT+L z^F3832#IL^X237MJ0q4pSFL*@v~ox{4cF>2)Q_zSkHwaJBte~=J{DDGpuu)h)&-JG zZW^t&@m6{uX%eYmsC6OVLq~=W#xIUG<12;A85}2`Gd#6I-D^{;lPM+zNB+&K`zJ~D z2+Y2b_WkD}@aw84&j;6nW~UfSB6j+?(_%tsH8rmRDgbWa{k2)S`*Yv^g zl=1!xnV0bTL?KnPNNE8PkJmGmM6BG3>&o)QI%x_q@ae>XkmH;VzkiWZNqI3AV`$c= znO+b`aRN!YSn;1Y4pIAe`oq8-zjf+1cjb6#omk_)98D=1luWVt)eRCU zL?yXBat7))W;nmq3*RrBanMYY&o16v3lbmKT0i8OS9c-EgE1~u8sW5wKVipH^_N<@ zeTkVw)G$)7UlE0;_V8C@t#FW#;k;l`kQC4$f16;QQZgbI^m@Aan}EvMT`w0y!~#1; zVZg&e9o>S$hDELQHK>rE5zwesiu@jmoP=7 zzV(ccutg_yGnh4nkilAVN`g}r*8ET<6MzhNr&P&~D7rwrFU#Hs^j%v}?5T;4p%_c9 z+HHP#qKWo!2&@S>8z+$VO*ty!STr0c7zrZuE#}E=>0>&};S@J*qhYV}je#3*J%s&C zl$LE-;q%Bo5&ifI|UoqCYua!WrMcL zgQ6^Uwtd1BWhv>njuly9VL`{U*L%;TRG~NAwNr{|Pd(g<{Br`|FL4E56P7Mq%v#1r z(C;9I%W^b98C7y017o)8{@o2i#}*qV_s?;!Y}#+CWCP+1<94_?_yD{gmID#Fp@wFg z>DgTy)ZAP0>I%W^)cWD$&ukR5s~WR|Utr8Y1gVMi)I}PQ!^kdSBNU_kydrSR)J3CH zJz61zB4-OhV3r&lz(#b=An5H2Mo6O63Ax@k7E#MxCJ81eUn_-u4>fTK_@cKUydO6t zipyT5^G^;nO>pa?PBkmXs%Esr))y*;gmD6&aM|%z-e8Z$Y=ZG3~3?V%+ey9qgyE(D~^=}>|%WLfSfYaIsjvh0$;#XvxgeUC0 z;$r!@weA)7;UCELG?zu=4xX9_{k@+G zcw>mIjL5BVDm>X77&E-iANd3vPsecd zhaFlK935lutuSImq;c79Az)!rQ!t3E!R|SwXUQ*By)76*$%#&TlhkW3Cn8kcUnL}V z8wrmm-I;Pl(s;q6nkmf)0yCmkDU(%b$)xZBTopn~D-tgY?Ef{^- ze`YTSrpK-L9bJX$mlktFB~jUf6X$hR88=~Nq@XyW`3+JvQ-9zkDe9zW;8j~L7-b5s zmjg|WN9gxQGfGDD6kQO$<)3K5xP*~ep8Jrp3yqxwm*t||!~^QWt}G}bpLmzL_+3n# z)c7*@?lMc-F!cr9#OmhQ%j#wXM)#a7FSic?o7f^Ij&mnbmUXAszvm>1b7L>(RQXoZ z$Bhq!O4twT)^;_}m+mt@jpMKt@TG6TY9oDIyvhLaBUgx|r%@``7L-gIH7nIeiqDA2 zXYe<3J|Avux0wDk)dC;V4CZZV;l~M8=mzGCo~&eFMUH^C6SA6ZQrL;T{O0ZTx8{HX zp14=_i@v#ihL*&RFk-zN|4Da0nJ7ut6fAkY^ymQ0E{?>J9i0u6Py3T;W;ANpv6^3on1!9uI?s=o27>U^%?Go%iq zCntnrGs-0V4l6{2;kDP8;S7+SCj(HFI(l1(Nw;%CtvH;Pd6Ily)b=ebbLPcQ+%+&@ zjmqu9`jS?3J0yv_e*1^96J{7SRqd4M{0yrzp+s9_(dBTEP*Wy91t|xwouNsglLdr8 zMN&$5n2Ig)CgDWQPYOCT(NaXiYWZzQx>W($c$ zZ+*i8ImNeAtRD;pSvfx!Oq6Y1;g{WASlg(X{We8PKob80MP5x1(7h?0?b@Im>|9zT zI|u8)N`)w-@~I9_na)eySp!uF5r;O9wm93yT;MZ!qAbU+eJMs7iW0+*>5Z}qF`~nN zLhGs)^~>MXy2skqD)gN{ZXIdpmSk1tj6&b)qv35+#2Fug%02I zMBul#6JsT#oCib4cPQ_dlZWGsz1SQf;O%_w(zm_+s@mZt+R=!l>uLWuqlEGDb>k*Q zc`LL1!E^xb`pe_L>I!amWbKQ-9HoC14xq04{}dHSG|Zdd!#~dfi$1=D;T{r&w2c)h-!cz=E!x#1qF?tFV5Yr0G8`u=$K@OXMaLd*GR1-k*9O z1K~Bam_Cx%=a*Thm#Ys(qe&>jT@O=DM_xDEnoz-kCyN{ac)Hhhux% z)yhU)yuR*#Zluup$w8HCd4R{o;*WS8>`j3cHe%A;}9R=)+LIMVM(ZP18=-`uGL@0gKze@z* zk9*uuGHCxmi#=XQWpK_O9l^ikhSh&{7#*+>5O{ypi2r&Z|9OXk_x7Y9L16lQ87O0M zu#Y$%IC>wQ;D6*@f4S}d-hc&e-j{;Z0l)7{K-pU-9AJTStl`0$C+GzKE%x}w`+p80 z0!JMvK(c^W4rHNHK;SMAIaunD9+DgEd}s`H=Kv;lB>k5MFrpJW^8beR+Yx;Eoe<1- z#0rJ)@((m}fdj`K(Lx+hnn&H$6(+WD^xo0 z7ZQLs0C+ILF+Eg~|36c+V>YPxz<*%v@h8InrSTykNdF!Ae?p%R{cD8(m)QSA_&<$2 kwEy1-G4UTGqr`vzi2vJg`1D`i%D{dnpJCCq{> + +% The developed multi-body model of the Stewart platform is represented schematically in Figure ref:fig:nhexa_stewart_model_input_outputs, highlighting the key inputs and outputs: actuator forces $\bm{f}$, force sensor measurements $\bm{f}_n$, and relative displacement measurements $\bm{\mathcal{L}}$. +% The frames $\{F\}$ and $\{M\}$ serve as interfaces for integration with other elements in the multi-body system. +% A three-dimensional visualization of the model is presented in Figure ref:fig:nhexa_simscape_screenshot. + +% #+attr_latex: :options [b]{0.6\linewidth} +% #+begin_minipage +% #+name: fig:nhexa_stewart_model_input_outputs +% #+caption: Nano-Hexapod plant with inputs and outputs. Frames $\{F\}$ and $\{M\}$ can be connected to other elements in the multi-body models. +% #+attr_latex: :scale 1 :float nil +% [[file:figs/nhexa_stewart_model_input_outputs.png]] +% #+end_minipage +% \hfill +% #+attr_latex: :options [b]{0.35\linewidth} +% #+begin_minipage +% #+name: fig:nhexa_simscape_screenshot +% #+caption: 3D representation of the multi-body model +% #+attr_latex: :width 0.90\linewidth :float nil +% [[file:figs/nhexa_simscape_screenshot.jpg]] +% #+end_minipage + +% The validation of the multi-body model is performed using the simplest Stewart platform configuration, enabling direct comparison with the analytical transfer functions derived in Section ref:ssec:nhexa_stewart_platform_dynamics. +% This configuration consists of massless universal joints at the base, massless spherical joints at the top platform, and massless struts with stiffness $k_a = 1\,\text{N}/\mu\text{m}$ and damping $c_a = 10\,\text{N}/({\text{m}/\text{s}})$. +% The geometric parameters remain as specified in Table ref:tab:nhexa_actuator_parameters. + +% While the moving platform itself is considered massless, a $10\,\text{kg}$ cylindrical payload is mounted on top with a radius of $r = 110\,mm$ and a height $h = 300\,mm$. + +% For the analytical model, the stiffness, damping and mass matrices are defined in eqref:eq:nhexa_analytical_matrices. + +% \begin{subequations}\label{eq:nhexa_analytical_matrices} +% \begin{align} +% \bm{\mathcal{K}} &= \text{diag}(k_a,\ k_a,\ k_a,\ k_a,\ k_a,\ k_a) \\ +% \bm{\mathcal{C}} &= \text{diag}(c_a,\ c_a,\ c_a,\ c_a,\ c_a,\ c_a) \\ +% \bm{M} &= \text{diag}\left(m,\ m,\ m,\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{2}mr^2\right) +% \end{align} +% \end{subequations} + +% The transfer functions from actuator forces to strut displacements are computed using these matrices according to equation eqref:eq:nhexa_transfer_function_struts. +% These analytical transfer functions are then compared with those extracted from the multi-body model. +% The multi-body model yields a state-space representation with 12 states, corresponding to the six degrees of freedom of the moving platform. + +% Figure ref:fig:nhexa_comp_multi_body_analytical presents a comparison between the analytical and multi-body transfer functions, specifically showing the response from the first actuator force to all six strut displacements. +% The close agreement between both approaches across the frequency spectrum validates the multi-body model's accuracy in capturing the system's dynamic behavior. + + +%% Plant using Analytical Equations +% Stewart platform definition +k = 1e6; % Actuator stiffness [N/m] +c = 1e1; % Actuator damping [N/(m/s)] + +stewart = initializeSimplifiedNanoHexapod(... + 'Mpm', 1e-3, ... + 'actuator_type', '1dof', ... + 'actuator_k', k, ... + 'actuator_kp', 0, ... + 'actuator_c', c ... +); + +% Payload: Cylinder +h = 300e-3; % Height of the cylinder [m] +r = 110e-3; % Radius of the cylinder [m] +m = 10; % Mass of the payload [kg] +initializeSample('type', 'cylindrical', 'm', m, 'H', h, 'R', r); + +% Mass Matrix +M = zeros(6,6); +M(1,1) = m; +M(2,2) = m; +M(3,3) = m; +M(4,4) = 1/12*m*(3*r^2 + h^2); +M(5,5) = 1/12*m*(3*r^2 + h^2); +M(6,6) = 1/2*m*r^2; + +% Stiffness and Damping matrices +K = k*eye(6); +C = c*eye(6); + +% Compute plant in the frame of the struts +G_analytical = inv(ss(inv(stewart.geometry.J')*M*inv(stewart.geometry.J)*s^2 + C*s + K)); + +% Compare with Simscape model +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m] + +G_simscape = linearize(mdl, io); +G_simscape.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_simscape.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'}; + +%% Comparison of the analytical transfer functions and the multi-body model +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 +plot(freqs, abs(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:), 0.5], ... + 'DisplayName', sprintf('$l_%i/f_1$ - Multi-Body', i)) +end +for i = 1:6 +plot(freqs, abs(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', [colors(i,:)], ... + 'DisplayName', sprintf('$l_%i/f_1$ - Analytical', i)) +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +leg = legend('location', 'northwest', 'FontSize', 6, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:),0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', colors(i,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Nano Hexapod Dynamics +% <> + +% Following the validation of the multi-body model, a detailed analysis of the nano-hexapod dynamics has been performed. +% The model parameters are set according to the specifications outlined in Section ref:ssec:nhexa_model_def, with a payload mass of $10\,kg$. +% Transfer functions from actuator forces $\bm{f}$ to both strut displacements $\bm{\mathcal{L}}$ and force measurements $\bm{f}_n$ are derived from the multi-body model. + +% The transfer functions relating actuator forces to strut displacements are presented in Figure ref:fig:nhexa_multi_body_plant_dL. +% Due to the system's symmetrical design and identical strut configurations, all diagonal terms (transfer functions from force $f_i$ to displacement $l_i$ of the same strut) exhibit identical behavior. +% While the system possesses six degrees of freedom, only four distinct resonance frequencies are observed in the frequency response. +% This reduction from six to four observable modes is attributed to the system's symmetry, where two pairs of resonances occur at identical frequencies. + +% The system's behavior can be characterized in three frequency regions. +% At low frequencies, well below the first resonance, the plant demonstrates good decoupling between actuators, with the response dominated by the strut stiffness: $\bm{G}(j\omega) \xrightarrow[\omega \to 0]{} \bm{\mathcal{K}}^{-1}$. +% In the mid-frequency range, the system exhibits coupled dynamics through its resonant modes, reflecting the complex interactions between the platform's degrees of freedom. +% At high frequencies, above the highest resonance, the response is governed by the payload's inertia mapped to the strut coordinates: $\bm{G}(j\omega) \xrightarrow[\omega \to \infty]{} \bm{J} \bm{M}^{-T} \bm{J}^T \frac{-1}{\omega^2}$ + +% The force sensor transfer functions, shown in Figure ref:fig:nhexa_multi_body_plant_fm, display characteristics typical of collocated actuator-sensor pairs. +% Each actuator's transfer function to its associated force sensor exhibits alternating complex conjugate poles and zeros. +% The inclusion of parallel stiffness introduces an additional complex conjugate zero at low frequency, a feature previously observed in the three-degree-of-freedom rotating model. + + +%% Multi-Body model of the Nano-Hexapod +% Initialize 1DoF +initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof'); +initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N] + +% With no payload +G = linearize(mdl, io); +G.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Multi-Body model of the Nano-Hexapod without parallel stiffness +% Initialize 1DoF +initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof', 'actuator_kp', 0); + +% With no payload +G_no_kp = linearize(mdl, io); +G_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_no_kp.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Transfer function from actuator force inputs to displacement of each strut +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$l_i/f_i$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$l_i/f_j$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', [colors(1,:),0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Transfer function from actuator force inputs to force sensor in each strut +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G(6+i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G(7,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$') +plot(freqs, abs(squeeze(freqresp(G_no_kp(7,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$f_{ni}/f_i$ (no $k_p$)') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_no_kp(6+i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G(7,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$f_{ni}/f_j$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:)); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(6+i,i), freqs, 'Hz'))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); diff --git a/matlab/nhexa_3_control.m b/matlab/nhexa_3_control.m new file mode 100644 index 0000000..83a31fa --- /dev/null +++ b/matlab/nhexa_3_control.m @@ -0,0 +1,486 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +%% Path for functions, data and scripts +addpath('./mat/'); % Path for Data +addpath('./src/'); % Path for functions +addpath('./subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './mat/'; + +% Simulink Model name +mdl = 'nano_hexapod_model'; + +%% Colors for the figures +colors = colororder; + +%% Frequency Vector +freqs = logspace(0, 3, 1000); + +% Control in Cartesian Space + +% Alternatively, control can be implemented directly in Cartesian space, as shown in Figure ref:fig:nhexa_control_cartesian. +% Here, the controller processes Cartesian errors $\bm{\epsilon}_{\mathcal{X}}$ to generate forces and torques $\bm{\mathcal{F}}$, which are then mapped to actuator forces through the transpose of the inverse Jacobian matrix. + +% The plant behavior in Cartesian space, illustrated in Figure ref:fig:nhexa_plant_frame_cartesian, reveals interesting characteristics. +% Some degrees of freedom, particularly the vertical translation and rotation about the vertical axis, exhibit simpler second-order dynamics. +% A key advantage of this approach is that control performance can be individually tuned for each direction. +% This is particularly valuable when performance requirements differ between degrees of freedom - for instance, when higher positioning accuracy is required vertically than horizontally, or when certain rotational degrees of freedom can tolerate larger errors than others. + +% However, significant coupling exists between certain degrees of freedom, particularly between rotations and translations (e.g., $\epsilon_{R_x}/\mathcal{F}_y$ or $\epsilon_{D_y}/\bm\mathcal{M}_x$). + +% For the conceptual validation of the nano-hexapod, control in the strut space has been selected due to its simpler implementation and the beneficial decoupling properties observed at low frequencies. +% More sophisticated control strategies will be explored during the detailed design phase. + + +%% Identify plant from actuator forces to external metrology +stewart = initializeSimplifiedNanoHexapod(); +initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External Metrology [m, rad] + +% With no payload +G = linearize(mdl, io); +G.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +%% Plant in the Cartesian Frame +G_cart = G*inv(stewart.geometry.J'); +G_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + +%% Plant in the frame of the struts +G_struts = stewart.geometry.J*G; +G_struts.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; + +%% Bode plot of the plant projected in the frame of the struts +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_struts(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_struts(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:),0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Bode plot of the plant projected in the Cartesian frame +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_cart(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_cart(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{D_x}/\mathcal{F}_x$ [m/N]') +plot(freqs, abs(squeeze(freqresp(G_cart(2,2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{D_y}/\mathcal{F}_y$ [m/N]') +plot(freqs, abs(squeeze(freqresp(G_cart(3,3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$\epsilon_{D_z}/\mathcal{F}_z$ [m/N]') +plot(freqs, abs(squeeze(freqresp(G_cart(4,4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$\epsilon_{R_x}/\mathcal{M}_x$ [rad/Nm]') +plot(freqs, abs(squeeze(freqresp(G_cart(5,5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$\epsilon_{R_y}/\mathcal{M}_y$ [rad/Nm]') +plot(freqs, abs(squeeze(freqresp(G_cart(6,6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$\epsilon_{R_z}/\mathcal{M}_z$ [rad/Nm]') +plot(freqs, abs(squeeze(freqresp(G_cart(1,5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.5], ... + 'DisplayName', 'Coupling') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 4e-3]); +leg = legend('location', 'southwest', 'FontSize', 7, 'NumColumns', 3); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i,i), freqs, 'Hz'))), 'color', colors(i,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:nhexa_decentralized_iff_schematic +% #+caption: Schematic of the implemented decentralized IFF controller. The damped plant has a new inputs $\bm{f}^{\prime}$ +% #+RESULTS: +% [[file:figs/nhexa_decentralized_iff_schematic.png]] + + +% \begin{equation}\label{eq:nhexa_kiff} +% \bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix} +% K_{\text{IFF}}(s) & & 0 \\ +% & \ddots & \\ +% 0 & & K_{\text{IFF}}(s) +% \end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} +% \end{equation} + + +% In this section, the stiffness in parallel with the force sensor has been omitted since the Stewart platform is not subjected to rotation. +% The effect of this parallel stiffness will be examined in the next section when the platform is integrated into the complete NASS system. + +% The Root Locus analysis, shown in Figure ref:fig:nhexa_decentralized_iff_root_locus, reveals the evolution of the closed-loop poles as the controller gain $g$ varies from $0$ to $\infty$. +% A key characteristic of force feedback control with collocated sensor-actuator pairs is observed: all closed-loop poles are bounded to the left-half plane, indicating guaranteed stability [[cite:&preumont08_trans_zeros_struc_contr_with]]. +% This property is particularly valuable as the coupling is very large around resonance frequencies, enabling control of modes that would be difficult to include within the bandwidth using position feedback alone. + +% The bode plot of an individual loop gain (i.e. the loop gain of $K_{\text{IFF}}(s) \cdot \frac{f_{ni}}{f_i}(s)$), presented in Figure ref:fig:nhexa_decentralized_iff_loop_gain, exhibits the typical characteristics of integral force feedback of having a phase bounded between $-90^o$ and $+90^o$. +% The loop-gain is high around the resonance frequencies, indicating that the decentralized IFF provides significant control authority over these modes. +% This high gain, combined with the bounded phase, enables effective damping of the resonant modes while maintaining stability. + + +%% Identify the IFF Plant +stewart = initializeSimplifiedNanoHexapod('actuator_kp', 0); % Ignoring parallel stiffness for now +initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N] + +% With no payload +G_iff = linearize(mdl, io); +G_iff.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff.OutputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'}; + +%% IFF Controller Design +Kiff = -500/s * ... % Gain + eye(6); % Diagonal 6x6 controller (i.e. decentralized) + +Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'}; +Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; + +%% Root Locus plot of the Decentralized IFF Control +gains = logspace(-2, 1, 200); + +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); +nexttile(); +hold on; +plot(real(pole(G_iff)), imag(pole(G_iff)), 'x', 'color', colors(1,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(G_iff, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(G_iff, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'kx', ... + 'DisplayName', '$g_{opt}$'); +hold off; +axis equal; +xlim([-600, 50]); ylim([-50, 600]); +xticks([-600:100:0]); +yticks([0:100:600]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); + +%% Loop gain for the Decentralized IFF +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(-G_iff(1,1)*Kiff(1,1), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +% leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +% leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(-G_iff(1,1)*Kiff(1,1), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); + + + +% #+name: fig:nhexa_hac_iff_schematic +% #+caption: HAC-IFF control architecture with the High Authority Controller being implemented in the frame of the struts +% #+RESULTS: +% [[file:figs/nhexa_hac_iff_schematic.png]] + +% The effect of decentralized IFF on the plant dynamics can be observed by comparing two sets of transfer functions. +% Figure ref:fig:nhexa_decentralized_hac_iff_plant_undamped shows the original transfer functions from actuator forces $\bm{f}$ to strut errors $\bm{\epsilon}_{\mathcal{L}}$, characterized by pronounced resonant peaks. +% When decentralized IFF is implemented, the transfer functions from modified inputs $\bm{f}^{\prime}$ to strut errors $\bm{\epsilon}_{\mathcal{L}}$, shown in Figure ref:fig:nhexa_decentralized_hac_iff_plant_damped, exhibit significantly attenuated resonances. +% This damping of structural resonances serves two purposes: it reduces vibrations in the vicinity of resonances and simplifies the design of the high authority controller by providing a simpler plant dynamics. + + +%% Identify the IFF Plant +initializeController('type', 'iff'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External Metrology [m,rad] + +% With no payload +G_hac = linearize(mdl, io); +G_hac.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_hac.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +%% Plant in the frame of the struts +G_hac_struts = stewart.geometry.J*G_hac; +G_hac_struts.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; + +%% Bode plot of the plant projected in the frame of the struts +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_struts(i,j), freqs, 'Hz'))), 'color', [0,0,0,0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_struts(1,2), freqs, 'Hz'))), 'color', [0,0,0,0.1], ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Bode plot of the plant projected in the frame of the struts +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_hac_struts(i,j), freqs, 'Hz'))), 'color', [0,0,0,0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$') +plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i^\prime$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_hac_struts(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,2), freqs, 'Hz'))), 'color', [0,0,0,0.1], ... + 'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j^\prime$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.2]); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_struts(i,i), freqs, 'Hz'))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:nhexa_decentralized_hac_iff_plant +% #+caption: Plant in the frame of the strut for the High Authority Controller. +% #+attr_latex: :options [htbp] +% #+begin_figure +% #+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_plant_undamped}Undamped plant in the frame of the struts} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nhexa_decentralized_hac_iff_plant_undamped.png]] +% #+end_subfigure +% #+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_plant_damped}Damped plant with Decentralized IFF} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nhexa_decentralized_hac_iff_plant_damped.png]] +% #+end_subfigure +% #+end_figure + +% Building upon the damped plant dynamics shown in Figure ref:fig:nhexa_decentralized_hac_iff_plant_damped, a high authority controller is designed with the structure given in eqref:eq:nhexa_khac. +% The controller combines three elements: an integrator providing high gain at low frequencies, a lead compensator improving stability margins, and a low-pass filter for robustness to unmodeled high-frequency dynamics. +% The loop gain of an individual control channel is shown in Figure ref:fig:nhexa_decentralized_hac_iff_loop_gain. + +% \begin{equation}\label{eq:nhexa_khac} +% \bm{K}_{\text{HAC}}(s) = \begin{bmatrix} +% K_{\text{HAC}}(s) & & 0 \\ +% & \ddots & \\ +% 0 & & K_{\text{HAC}}(s) +% \end{bmatrix}, \quad K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}} +% \end{equation} + +% The stability of the MIMO feedback loop is analyzed through the /characteristic loci/ method. +% Such characteristic loci, shown in Figure ref:fig:nhexa_decentralized_hac_iff_root_locus, represent the eigenvalues of the loop gain matrix $\bm{G}(j\omega)\bm{K}(j\omega)$ plotted in the complex plane as frequency varies from $0$ to $\infty$. +% For MIMO systems, this method generalizes the classical Nyquist stability criterion: with the open-loop system being stable, the closed-loop system is stable if none of the characteristic loci encircle the -1 point [[cite:&skogestad07_multiv_feedb_contr]]. +% As seen in Figure ref:fig:nhexa_decentralized_hac_iff_root_locus, all loci remain to the right of the -1 point, confirming the stability of the closed-loop system. Additionally, the distance of the loci from the -1 point provides information about stability margins for the coupled system. + + +%% High Authority Controller - Mid Stiffness Nano-Hexapod +% Wanted crossover +wc = 2*pi*20; % [rad/s] + +% Integrator +H_int = wc/s; + +% Lead to increase phase margin +a = 2; % Amount of phase lead / width of the phase lead / high frequency gain +H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +% Low Pass filter to increase robustness +H_lpf = 1/(1 + s/2/pi/200); + +% Gain to have unitary crossover at 5Hz +H_gain = 1./abs(evalfr(G_hac_struts(1, 1), 1j*wc)); + +% Decentralized HAC +Khac = H_gain * ... % Gain + H_int * ... % Integrator + H_lpf * ... % Low Pass filter + eye(6); % 6x6 Diagonal + +%% Plot of the eigenvalues of L in the complex plane +Ldet = zeros(6, length(freqs)); +Lmimo = squeeze(freqresp(G_hac_struts*Khac, freqs, 'Hz')); +for i_f = 2:length(freqs) + Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end + +figure; +hold on; +for i = 1:6 +plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), ... + '.', 'color', colors(1, :), ... + 'HandleVisibility', 'off'); +plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), ... + '.', 'color', colors(1, :), ... + 'HandleVisibility', 'off'); +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real Part'); ylabel('Imaginary Part'); +axis square +xlim([-1.8, 0.2]); ylim([-1, 1]); + +%% Loop gain for the Decentralized HAC_IFF +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,1)*Khac(1,1), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); + +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_struts(1,1)*Khac(1,1), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); diff --git a/matlab/subsystems/nano_hexapod.slx b/matlab/subsystems/nano_hexapod_simscape.slx similarity index 100% rename from matlab/subsystems/nano_hexapod.slx rename to matlab/subsystems/nano_hexapod_simscape.slx