From b43ad3cf2dcedeef0c4d915af7eef02c63f16bf6 Mon Sep 17 00:00:00 2001 From: Boogard Date: Thu, 5 Sep 2024 22:34:15 -0600 Subject: [PATCH 01/13] removing zip file --- IK_Last_version/untitled1.zip | Bin 9647 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 IK_Last_version/untitled1.zip diff --git a/IK_Last_version/untitled1.zip b/IK_Last_version/untitled1.zip deleted file mode 100644 index 3f8a86bda44f6658f96489a3c5be944c6b31aa17..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 9647 zcmaKS1yEhhvNi7R?(PuW-Q9w_!@=F%9fG^Ny9L)kut3n@5}e@r!~NfT$<2HB>#DQs zRGqy}&GdB7>RvO7GN52+Ku}OnKpS;68bE&-!rOO4TXP#zeJeXlTW2Q*fKTG2Z4eVu z@R?*+5Mu%yIC+a#)U2@rUWJZQV3uGndIPg-5rx7J-PK^#7#I|coNT)hfzn2PPgB-< z6Okg@$C3wDX)g)-iO{MZlWVq!LQX3$r+7YUENV__u!zFAahH;x^FZi7wyl^B-fx(MAXc7s2`QCqJvJN( zKTB>K9zdL^!eY667dQ@CWbnxOWfnD2s3LM(kcO6K<{ldM8LwTw4xrrd2A|Qm)owf4_FR0* zc}5I?&>czGRfIBh^k9vZpBh23fc1}HLx#EF0A*nFO9a`^6^Z{)ij=m+3#U`Nbx@|1Ik~^pl@em!T<FZ-|14P-nEi+yO3K0HT&|EelI;N@Ip?^}QpdaSEK`?hi?1kHq?`3#)c5CTJdLKJD zW{wHp_%)l_PT(hr-55jXxq753nwjEWo*6xS$a96}dicn4ixl)ZhSS2!#)4EibX>>E zP8w^M!C395avC!U(Zt5LgeFtvLGLDK4HL*pFMs&rkwaNOIFg z(DXTZ_lfL&C6lR5_dz!{wAsqQr66$;ABD4LusX{%%8c%Ikt{1ZnpqFo+-IggPF5X+ zYE+qeAZl9spVU)K&@UacwVtfsQ$|{uQ<=_ba7$}&IU`hrR()`GRIazA>a!JL#mo4) zN%~V|ks1B_GmQnVF-_CsP+KcK{?{)zGT#q%&tgab+% zf&zb7O0P3E);!0Aa`0CP6V%cU0A;2`uzBQRgAc^S)nm#N2}q5k$QBn!5)}4jiJ<`^ zO1t>3J)@E6VFzLEw~gfXKI<=2=?P zOphb>?fO7S`qb9hsJIY161Jd~@UgxLBZ@0d$|Rp&j{-fmjJUxQU0+|D+O!?(D-6r? z(;axaugZ?*(9|rgUcF|{N48Qa47H{eS3xdY$YT&;vwi^tVe^PNlZBSR_=o-~1Bfda zv)-y+Qz#*4Jl;a|^^zrL6*wKwQ8_qQ!D<7@V;Gvyf$fdanrQ-%R0K!VK>TUgG~i~z z(g9!0ds8}t)PwBoSgt3%hPd;}o6bk>+!{}t3_VIsXzNenZIUG%ORS+qfz%}sLfxXc z=!9Q)G9Ly<77vsA5UagP6bD}-D@k+^YA4tk?JeUn;9hEVM*O!PkDMae|Gv_7yy~cdd>)h z`6U6px*`1I#_ZDc-1s;0YQz)f)?VKuqm7PN0H*#)FX=y8z19S770&hT9kxaz z*)?ZqCRUCwT|2ub8ERKf<~x3!PFX^I;xpPjw|`v{+IZv#svF=Z6Z<5e2P`|+5ZNfz zpND0aMAk?hk?;1LP7eJT1O=<;g(IaR@$$0y3t*GLz3dTm^7Z?n;7{{;=s?3JLil3p z!ChA5X%zMOJLhFXNWL8;y1QG#*!(!%ef>o0D8>m}#28I-JS&>57Mb+Uj?J=tV$7Lw z9b5Z$s)`zxJL**xtK2ejMv!=HX2wt=5qefdlJ4#{*~S{tRp3(7PXipkEOqh+$|Me{ z<|?CO^cuu^rKLGcWa|e~gV!7-@$+yX#C$8s3`9D>9nf)*Myau7<>!{@s1vISWaGoq zAT^|T-1G3StPpOm;&Px^1)8R0eYg#D1xO@`fv6G+(j5=-AgH?|@vkOBNyK!Z* zz>eIF&XdK)Lfi0#d^jYrT)}pVyqt23im;MMfzv5rqZCbz5>s`<8GzKPz@nM^Ti1m+ zjtATcCA|0$zvOG}R7goxONdLtkHqNxT)=bmp^XdyDRifcjM;)fgAFmtSE4%=V@_hi zz%@Ls?$dbk!x~9NIy5$tr~5~4DP$kAGK9t9rdV5R z_zi{#HC&Vv8J58G@gBISn4pZMZcal#ALbV+R%6-~LsMAyEt6}ZN9;;e6`<4CZ5fig_g}_L{vP&OP4xyeuwLZZc3^F;y#C~EHL$w|(+XuwTemiLLJTf0!TNI^OO5Ev;=)~!Q^{LnkMbO8oZ}OcG-^< zX~qR4!z49{)(Yy(*$~fw$5fiCrC*+daoB{AFMjW>q6Z=!ed*p0+#7NH09v{kKXFhXbyqHh$Cn9_j;M z6Cjx|58$HJ>KIoD;2i+C*uDaXxo&SSp1Yo~P7SGd(JSn%@+H*jhP0iy&<>-=ndw|u z45NR>x`kx1%?1qNtiTMlyrkIBo*HW#E~&3hwDdPr-z_cOXxpC{Q-yRO@H)3Q6(klO zdngF47i4$k26vmPv>qLAe&DIvee$Md@q^LAxSefKY@!QK3CTCT61v@czz0P*^zwJv zDTnm9KQ5*uyWWUt=;GmA6~Jlv3O>-RvRzwxo4b3s25_0&;Hx(Hb&gN?uvmM2w+wLM zDQ(t2t4>n?6{)bOHgR%2%pZksX_rF~?2fvb6|qM9IZP|G>v9eriC7%MFA&xF*VWU} z`4jy3UisJ6mY%{IW#!>9Qwk=XVxW zt^jjGc2FGm=2W%kqgsh|)BS;CGJYKcc9?t1ER6F${x(GOpzLK9k2+-{;j#q9d2-;% zlAxa%#ct*8HNBDBj*w3L-ph#(rB`@xP(VOG$^NOFKzb`Dj4e!!t@WKuja?iqoju;V zi&!Cqej+5XTe4Ndl=ykkjG8KU^a4p2w0v-!pwiTm!xFZaA-eU~u}oWM?tHg+p}-#8 zXVsYlW`l8?jbsyOLR4=Pmv84OiJ8}_jo?Q=D9XYlQj>=;OQ4K@)FP2*Bl;`j;YjC! z@fd)LAaoKQMdXql-*;svrkBCc*JbC|&s14cr_AbdWSr!p3e>P&`^k0B(Cx#%Pm0~? z%YEwGkOkhV_5WO{!@j*CY6!45HPM$W{;X=S9l0WjV^L#F)_4`(rE0={egh$wTP1rYw)G&p%QDi>> zFqx{rczk?CGH$knHkIe~Ict-ku<}mg%uXhZ9xCYO?koV*jOhT288NiZQ6hxitv=fv z37_c~HXa6Pj-xw>%_TsQ4Q zhn3qZpS5PZCYsF*|NirIyhG~w;Pq@mrm8Yoxl49Wjv^_dl<}F_O&rw3EYRFPbam~kaaJ*Q7F$0YLHoo4qB!^vW^4~HI|+qkBGwwps+z#x>lGDiVROe zA|%g{k~BkvDPNtX)#koyG?MVVqH$DyOKI}W*;0{1n}uq;%w6`Jwwk+=VQv5Um>50w z^Us!NY^t5z@sF)FOJ9;RzdbCsYPaf^Bw^d3S#Q}~wNvRI2$L7c9uF2fqS{auW%0x) zv|$>0VLrY3dKXM%HeU3194yk5g$VK*hB%*qYlV5)tfjCm z&Gc(EcWfTxzWZ`Nb!-Xe&6kb;=u4P4U&`2;n(Euzy-5Nxw39LnV_&|iC_;@gs@JJ0 zo=?fh#W0RCE=Ycvm;!EeMsb2+L19C8Z=ZF#OhR!&^OkjQrwOpyq+yL_ZT6A%FJfc- z2p$sIqr{M;8+R?Xle1DV)9t-7cPvJzdH|EjdzC2$bSRGqKhvF|qgp*yZom{rX?7}YYJ1#lNWUdUHUA=r5W=t6qMDFLBBGyT7{?~P`_*`#u5~+> zzu;RYICt;;cEzpw!6Z}f`Z{K4N1NU+g{+7jk--6;54}qYjq9(>xr%=DdFqvB6U{)H zBE{bj(<9(8x`uB_0;+8mtmc5^E%exV6lphh5qc(AWCWk8n#!ydGsX(Wi+eNZ=Dd@} zF2Pg6Y0+PTOo?&H0BG?W+0v02qcHh%Z2b4idkstEHsqO!oFAn@!y=EwOER;p%h)@V zn+5OTEM+unSS!MXlP6ItjNk_my#>4XJmi(5?WVga{NgLDN_QtZ0-DD^1Pm1mntP{5 zWuk>jJ26CovFM#+G8dvXScn^QxYg;x(j*X!=r2pEDMzb*iWxQ!q;~m*ur{tm*4tcV zNlaTE89c*{oNtCQ_-SicxH41$vk%$R&gw!QDt&{?@f%o4xZQxlG2?rrMr9vVyS!x= z{o980pCk3H5eEVoTD~>cJ}NSCD?~^=661awDA;y*BL*mCgZh#^T9AUD-=+l^rcF+RGA=h5nEHkmD>VRuX-boH97~XsE~jcfZEkai zIdCt@G@GJGUH7}VsutL=Q0IslD{8BqS}n|xC5We)^Rqb+BZlnqk8T3H!S z@dCbNW$fx*v7VB7Z2v?j;Nhn;OLH_=ao+vB)RhQw&w0h*+TM( z-~2Ik*uMls5fhZWNOpnrwLs{Pe!tai17#{%fIlVWo$;{qaD2%Slw7DwM2O7Wf9Cp7 zFnEU>Ap6Tu`{i<@)A*3?LE#|8$%WeNpp!-Z`I=x+bKbChwf#PIgRxG#^NCn?qR4%B zDQLL@f=8PPgGn&8)Ab>;Idq$fjD3reZ|9Vt!`B5z51#3MLv#5i<&X}Z-lEfqLaA>P-PD-cDEiX4 z?F~UO4V)G2=dmeBk=%gr1Hcfp=fv`fktKajAYZpmd8K zEBbESQ-9#KYqpqbyO~RNHo+_mepv#O(5^d@wD>-lHuaKM3{h`A+;=<>(_R~u{E6~# zJWlzD$xu&I>|$fhf# zU34X=9ABp8kF_Ut7iHXW7}%P=0L9fFByn8DVFsJ&GFKf<%4EpDfV@Zh6bbZk$6K_| zfd5mpBfmwvgNvbwzP+QJy{V(KrRf{30H&B>-<0OiP2EwyJB&rv)In7$23=eWH-|bh zwtRK5=T}xuX~>aIFTysI#Wwe_5pG7`wZb17%+bFZ2@Rpu`)M>LBO8LR=jtF=?W*eL z^QzHOAwu1&i8);;QG|8QVfQhGb(HCFC56KF#|#l2pag02;AlG^C;p}Ns?Ta?>p6V6 zlL&~KFApP5zV|Pe(Vv;COJZ}^(sFWFY(=uS=4pIqYFrkreIuSjvl5=aeEw=P@$&*z z{4O|MeIaO;k5nB#Tl?o)M9=VbI(cTGyg357((+pg_W6u&ib+h(4%)|c zzQx7m(dx44@-pWByJ_&%TXddX-m^pPcO`kcYd8dg}csFDOb#M@>5HM-5 zQ8y$Cg<=NVI4l+WaKzOKbXe$R0Q&6*r5P`tRy4H1T+?V&b@>M-mvA8vMi*_0rlR~s0^G#L%$eGz7I9`f0{iIz@&hVQiH3C#ptGwO@GpES zFD5^aH{8y=+=U%6SFK%sH2Ir(k4diZ%*)YIF3Q#9kBMX^0Ka}4XeFCP_kZ!;=Vd5^ zZ2S@xI^;EV^}|z}H0MRa%GUZjJu#kjleSiEpP8kBgay;~F=V1TmX+H~PIrk}$g7LT zFf6@sPD#45R6-F+^eg@p+1*^cI$0rdAq&wK$*5yavL%@IFfyix-X4&yB9=G#?0DG* z4W=O$)T9gbW?gWenT|gfwkQcKm0W{_#hr7}u25QmS)t%a0t#;d$tLtEwtPY&Z{@io z&b}Hpg?<3*StAJ~P`OIE@sNv_1y-(Nd9X~VG=AQQnPv(ZXSFAs<$jM)6*K;FJr}q0 zIyuf5>pZ_v311S5F@p5^ZOGZ_a1Cv=t^&Cvuuyyt!+7KbCsdzAy#2WI7^%=FRT3!2 zME#*54-hzqMGOdy6~2sJQ*U4+71hnFl1}@*7(lVt_3!Q7kh+wupl(>|nA zC;d#{JWq~R!HCPrUT@0+6Gf^V?Bdd_Bwn5mKmIpIe(!pRf(L znm6=feEwh&R5d$ue)hw$n(*vShBZ`&iVa+l@%QB(<4RMgv)C)LIh~-sk z@e-NHWIR$RnXyO_WD&G!P6baGNIRP^O5-EPqT~>@;NmBft+1zU8$HE!1x1vD{m*f< zt!?^t#O!|KSUn??@%tOcY!3UKglnXC>R;#TGJZZdh~06a z)itRkz(@1H)9+X1Wsjz}Nksl5LA|#{&W?t*Z>0Mvp&KEP3CZ6LWqlJPDUnIhe%ASn z2MNL`%912RF}maKcwG&S2>&YMYQM8Z-!G$*FQ%kXF)$bgELsGBUwJlMoR`SS0*_U? z;_K}m4#reO#bu&_wmo(+NyV~3uxBl)QNM*tQiF6DHCL8@60-3tS+hZdL%RX~npcn# z5&mNh@5XY?ejn=75sj~F$Qe3!->fM*Kbaa2QK_12`VDpDJjY`&3R-J^qq$&N^?={2 z0f#H!bt!!jO5z2myo`^W00;T@0r>meEvQ)W9ezCuWV=Z zzTDNR3U--HXuW5W-jMnv1EM5#mYpR(^D|loI;6f;oG7b>gHc{=ZNaV513sPc8^Zt} zJIfYHgZiE=o!!20ZVVZ7V|_}PGTP8aU8<2tIEJFi)>n$sc6gwN%*`n`$wNmFSeSaU^Zl5pJbYml zEM}Eotisr~v{w!XShIp~M8gWr6$7d{Wq=d_*6A~sp)<*xZrdn|K0mzN_|Gft?()My zJ?-gVKTB?FUkrwK5ys_f%Q2qFMLmr;&yl>Q*=jrGFdVQvj9Jo<=!@4ndd(<-NfUoq zRU;}cyIlRYrM2=cSh|o!j{kJw4HD z$DYQAky$(pAU_Kd^=VL{>_H!i9xxuvXx?dAYAQ!76GT>56N9(M6^2({ePPbA#Xb&m zLTChhn&iNsogoQ4>h0hlBvXATHqQH_CnSRHOC6<|hCwWlAs2NG5MH}S_s2b>L*tv; z((BXwSb}t6ZefK?5}es@q2UOfKfnY`Ak#dd%XAYchH4+h`333= z0niH8uQgzu1Uio2L!tQY*rpc9IzOEuOvCCH1_?SLSs(-wBgF3kSt3j$L25Ypvq8Ft z2PrxsSt0bVBS56259GV>HVMB9<7?&nkt0-33w*<4chu~L$80?Enr~w?GdUZlCM6nG z=j_`V1nq&G-sxVA%8e=(Vlc{Y+3Q;E1~Ma~X2^A?8Ak~uQvJ-?KDyJn6xB6~skfhc zb?li(kiP)cDxQCI%7BAa3L~;9nQ3&P6=5a5BndBip2@^t8kpE6#$y~;glU^R8YBv< zDHTuAmr)c`+faJ`%qO6T?0iWlz<6$)5j|!kv?tsRuydD>3PM=D9{1pgzBk4IM5;)F zHY5@U!Qq(T!W|KBIxfB(gw;_$n!{o8cl z{zcszT zaet~7?{x9ozP{o9OH=zB^k?(;E@=EV57hsbH~z-_*;2jl&A-hY+gNn zZxh4(qYL{R_9suilm2ge#rv-k;BUyE^z!c>*~9-II{h2;XA1oL?6wI04<1sK0soiZ R5P%SXDBd#1iR67?{|{eLxgY=l From 0463fb213072939e3ccba40711db4770460dbe91 Mon Sep 17 00:00:00 2001 From: Boogard Date: Thu, 5 Sep 2024 22:37:49 -0600 Subject: [PATCH 02/13] updating the coordinate systems for solving IK --- IK_Last_version/Damped_LS.m | 28 --- IK_Last_version/FK.m | 95 +++------ IK_Last_version/FK_old.m | 29 --- IK_Last_version/Knee_pos.m | 7 - IK_Last_version/LICENSE | 21 -- IK_Last_version/README.md | 50 ----- IK_Last_version/Visualize_Robot.m | 46 ++--- IK_Last_version/angle_joints.m | 32 ---- IK_Last_version/angle_joints_old.m | 64 ------- IK_Last_version/angle_joints_stefan.m | 265 -------------------------- IK_Last_version/check_security.m | 16 -- IK_Last_version/main.m | 72 ++++--- IK_Last_version/quad_properties.m | 8 - 13 files changed, 88 insertions(+), 645 deletions(-) delete mode 100644 IK_Last_version/Damped_LS.m delete mode 100644 IK_Last_version/FK_old.m delete mode 100644 IK_Last_version/Knee_pos.m delete mode 100644 IK_Last_version/LICENSE delete mode 100644 IK_Last_version/README.md delete mode 100644 IK_Last_version/angle_joints.m delete mode 100644 IK_Last_version/angle_joints_old.m delete mode 100644 IK_Last_version/angle_joints_stefan.m delete mode 100644 IK_Last_version/check_security.m delete mode 100644 IK_Last_version/quad_properties.m diff --git a/IK_Last_version/Damped_LS.m b/IK_Last_version/Damped_LS.m deleted file mode 100644 index 99c1201..0000000 --- a/IK_Last_version/Damped_LS.m +++ /dev/null @@ -1,28 +0,0 @@ -function [q, q_dot] = Damped_LS(q_0, link_lengths, p_global) -deltaT = 1; -k = 1000; - -u = sqrt(0.001); -%% Calculating the jacobian -J = Jacobian(q_0, link_lengths); - - -%% Calculating the forward kinematics -[~, ~, ~, ~, ~, ~, ~, cur_pos] = FK(q_0, link_lengths); - -%% Getting the r vector -r = p_global - cur_pos; - -%% Numerical differentiation -r_dot = r./k; % To decrease the step that is taken by the velocity, k is some large constant - -%% Calculate the J_inverse -J_DLS= J'/(J*J' + u^2*eye(6)); - -%% Calculating the q_dot -q_dot = J_DLS * r_dot; - -%% Integration -q = q_0+ (q_dot .* deltaT)'; -end - diff --git a/IK_Last_version/FK.m b/IK_Last_version/FK.m index afdff81..534ba8a 100644 --- a/IK_Last_version/FK.m +++ b/IK_Last_version/FK.m @@ -1,6 +1,5 @@ -function cur_pos = FK(q,r, leg) -global L_sh L_th_f L_h L_th_h L_th_h_mid L_b W_b - +function pos = FK(q,r) +global L_b W_b r_p = r(1:3); % pelvis position x_p = r_p(1); y_p = r_p(2); @@ -15,69 +14,31 @@ R_p_z = rot(3,psi_p,4); R0 = R_p_z*R_p_y*R_p_x; P_p = trans([],r_p); +P_shd_hr = trans([],[-L_b/2,-W_b/2,0]); + +q1_hl = q(1); +q2_hl = q(2); +q3_hl = q(3); +q4_hl = q(4); + +Rq1_hr = rot(1,q1_hl,4); +Rq2_hr = rot(2,q2_hl,4); +Rq3_hr = rot(2,q3_hl,4); +Rq4_hr = rot(2,q4_hl,4); + +L_h = [53.26, 61.34, 0.17]/1000; +L_th_h_mid = [0.0, 92.88, 201.0]/1000; +L_th_h = [0.0, 52.62, 246.5]/1000; +L_sh = [77.21, 0.0, 190.62]/1000; +P_hip_hr = trans([],[-L_h(1);-L_h(2);-L_h(3)]); +P_knee_mid_hr = trans([],[-L_th_h_mid(1);-L_th_h_mid(2);-L_th_h_mid(3)]); +P_knee_hr = trans([],[-L_th_h(1);-L_th_h(2);-L_th_h(3)]); +P_ankle_hr = trans([],[-L_sh(1);-L_sh(2);-L_sh(3)]); -if leg==1 - q1_hl = q(1); - q2_hl = q(2); - q3_hl = q(3); - q4_hl = q(4); - - Rq1_hl = rot(1,q1_hl,4); - Rq2_hl = rot(2,q2_hl,4); - Rq3_hl = rot(2,q3_hl,4); - Rq4_hl = rot(2,q4_hl,4); - - P_shd_hl = trans([],[-L_b/2,W_b/2,0]); - P_hip_hl = trans([],[-L_h(1);L_h(2);-L_h(3)]); - P_knee_mid_hl = trans([],[-L_th_h_mid(1);L_th_h_mid(2);-L_th_h_mid(3)]); - P_knee_hl = trans([],[-L_th_h(1);L_th_h(2);-L_th_h(3)]); - P_ankle_hl = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]); - cur_pos = P_p*R0*P_shd_hl*Rq1_hl*P_hip_hl*Rq2_hl*P_knee_mid_hl*Rq3_hl*P_knee_hl*Rq4_hl*P_ankle_hl*[0;0;0;1]; -elseif leg==2 - q1_hr = q(1); - q2_hr = q(2); - q3_hr = q(3); - q4_hr = q(4); - - Rq1_hr = rot(1,q1_hr,4); - Rq2_hr = rot(2,q2_hr,4); - Rq3_hr = rot(2,q3_hr,4); - Rq4_hr = rot(2,q4_hr,4); - - P_shd_hr = trans([],[-L_b/2,W_b/2,0]); - P_hip_hr = trans([],[-L_h(1);L_h(2);-L_h(3)]); - P_knee_mid_hr = trans([],[-L_th_h_mid(1);L_th_h_mid(2);-L_th_h_mid(3)]); - P_knee_hr = trans([],[-L_th_h(1);L_th_h(2);-L_th_h(3)]); - P_ankle_hr = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]); - cur_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*Rq4_hr*P_ankle_hr*[0;0;0;1]; -elseif leg==0 - q1_fr = q(1); - q2_fr = q(2); - q3_fr = q(3); - - Rq1_fr = rot(1,q1_fr,4); - Rq2_fr = rot(2,q2_fr,4); - Rq3_fr = rot(2,q3_fr,4); - - P_shd_fr = trans([],[L_b/2,-W_b/2,0]); - P_hip_fr = trans([],[L_h(1);-L_h(2);-L_h(3)]); - P_knee_fr = trans([],[L_th_f(1);-L_th_f(2);-L_th_f(3)]); - P_ankle_fr = trans([],[-L_sh(1);-L_sh(2);-L_sh(3)]); - cur_pos= P_p*R0*P_shd_fr*Rq1_fr*P_hip_fr*Rq2_fr*P_knee_fr*Rq3_fr*P_ankle_fr*[0;0;0;1]; -else - q1_fl = q(1); - q2_fl = q(2); - q3_fl = q(3); - - Rq1_fl = rot(1,q1_fl,4); - Rq2_fl = rot(2,q2_fl,4); - Rq3_fl = rot(2,q3_fl,4); - - P_shd_fl = trans([],[L_b/2,W_b/2,0]); - P_hip_fl = trans([],[L_h(1);L_h(2);-L_h(3)]); - P_knee_fl = trans([],[L_th_f(1);L_th_f(2);-L_th_f(3)]); - P_ankle_fl = trans([],[-L_sh(1);L_sh(2);-L_sh(3)]); - - cur_pos= P_p*R0*P_shd_fl*Rq1_fl*P_hip_fl*Rq2_fl*P_knee_fl*Rq3_fl*P_ankle_fl*[0;0;0;1]; -end +shoulder_pos = P_p*R0*P_shd_hr*[0;0;0;1]; +hip_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*[0;0;0;1]; +knee_mid_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*[0;0;0;1]; +knee_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*[0;0;0;1]; +anlke_pos = P_p*R0*P_shd_hr*Rq1_hr*P_hip_hr*Rq2_hr*P_knee_mid_hr*Rq3_hr*P_knee_hr*Rq4_hr*P_ankle_hr*[0;0;0;1]; +pos = [shoulder_pos(1:3), hip_pos(1:3), knee_mid_pos(1:3), knee_pos(1:3), anlke_pos(1:3)]; end \ No newline at end of file diff --git a/IK_Last_version/FK_old.m b/IK_Last_version/FK_old.m deleted file mode 100644 index 520cd77..0000000 --- a/IK_Last_version/FK_old.m +++ /dev/null @@ -1,29 +0,0 @@ -function Pos = FK(q) -global L1 L2 L3 L4 - -q1 = q(1); -q2 = q(2); -q3 = q(3); -q4 = q(4); -%% Rotation -Rq1 = rot(1,q1,4); -Rq2 = rot(2,q2,4); -Rq3 = rot(2,q3,4); -Rq4 = rot(2,q4,4); - -%% Translation -P_shd = trans([],[0;0;0]); -P_hip = trans([],-L1); -P_knee = trans(3,-L2); -P_knee_mid = trans(3,-L3); -P_ankle = trans([],-L4); - -%% Transformations -r_shd = P_shd*[0;0;0;1]; -r_hip = P_shd*Rq1*P_hip*[0;0;0;1]; -r_knee = P_shd*Rq1*P_hip*Rq2*P_knee*[0;0;0;1]; -r_knee_mid = P_shd*Rq1*P_hip*Rq2*P_knee*Rq3*P_knee_mid*[0;0;0;1]; -r_ankle = P_shd*Rq1*P_hip*Rq2*P_knee*Rq3*P_knee_mid*Rq4*P_ankle*[0;0;0;1]; - -Pos = [r_shd(1:3);r_hip(1:3);r_knee(1:3);r_knee_mid(1:3);r_ankle(1:3)]; -end diff --git a/IK_Last_version/Knee_pos.m b/IK_Last_version/Knee_pos.m deleted file mode 100644 index 4a4c1e0..0000000 --- a/IK_Last_version/Knee_pos.m +++ /dev/null @@ -1,7 +0,0 @@ -function Pos = Knee_pos(ankle_pos,alpha) -global L4 -y = ankle_pos(2)+L4(2); -x = ankle_pos(1)+L4(1)*cos(alpha)-L4(3)*sin(alpha); -z = ankle_pos(3)+L4(1)*sin(alpha)+L4(3)*cos(alpha); -Pos = [x,y,z]; -end diff --git a/IK_Last_version/LICENSE b/IK_Last_version/LICENSE deleted file mode 100644 index b86b789..0000000 --- a/IK_Last_version/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2022 Walid Shaker - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/IK_Last_version/README.md b/IK_Last_version/README.md deleted file mode 100644 index a4c9117..0000000 --- a/IK_Last_version/README.md +++ /dev/null @@ -1,50 +0,0 @@ -[[Demo]](https://youtu.be/QU4yxvBdSNA) - -## 7DOF-KUKA-Linear-Axis-Forward-and-Inverse-Kinematics -KUKA on linear axis includes a further axis to the robot, so it is considerably extending the workspace of the robot. The advantages of the redundant robots are increasing manipulability in specified directions, uniform distribution of velocities and accelerations, minimizing energy consumption, optimizing execution time, etc. However, there are also such drawbacks, as complicated calculations for IK and motion control, and greater structural complexity of construction. - -A manipulator is kinematically redundant when it possesses more degrees of freedom than it is needed to execute a given task. In other words, there are more local coordinate variables than global. The Redundancy Resolution is necessary because it allows to avoid singularity, obstacles and to smooth manipulation around the workspace. The solution for the redundancy is a cost function optimization, where the cost function can be i) energy-based or ii) minimizing the distance. The Redundancy Resolution includes three methods, which are -- Jacobian-based (Damped Least Square and Weighted Pseudoinverse) -- Null Space -- Task Augmentation - -In this repository, the implementation of forward and inverse kinematics by redundancy resolution is presented. - -

- - -

- -### Forward Kinematics can be written as follows: -![image](https://user-images.githubusercontent.com/90580636/171468322-838cf1ab-ec6d-4d20-b146-d6a899b7f772.png) - -## Redundancy Resolution -

- - -

- -

- - -

- -

- - -

- -Full video is attached [Demo](https://youtu.be/QU4yxvBdSNA) - -## Accuracy and Execution Time -

- - -

- -## How to run -First open **IK.m** file and select the preferable redundancy resolution approach as shown below. -![image](https://user-images.githubusercontent.com/90580636/176931082-95879408-ef2a-4dbc-ab4c-08d87c0bbe02.png) - -Then run **main_redundancy_resolution.m** file for calculations and visualization. - diff --git a/IK_Last_version/Visualize_Robot.m b/IK_Last_version/Visualize_Robot.m index ef750b3..f54232d 100644 --- a/IK_Last_version/Visualize_Robot.m +++ b/IK_Last_version/Visualize_Robot.m @@ -1,10 +1,15 @@ -function [] = Visualize_Robot(Pos, p_global, color_list) +function [] = Visualize_Robot(body_pos, foot_Pos, p_global, color_list) limit = 1.3; -r_shd = Pos(1:3); -r_hip = Pos(4:6); -r_knee_mid = Pos(7:9); -r_knee = Pos(10:12); -r_ankle = Pos(13:15); +r_shd = foot_Pos(1:3); +r_hip = foot_Pos(4:6); +r_knee_mid = foot_Pos(7:9); +r_knee = foot_Pos(10:12); +r_ankle = foot_Pos(13:15); + +r_shd_hr = body_pos(1:3); +r_shd_hl = body_pos(4:6); +r_shd_fr = body_pos(7:9); +r_shd_fl = body_pos(10:12); %% Plotting hold on @@ -19,7 +24,6 @@ config_joint_3 = r_knee; config_joint_4 = r_ankle; -% if flag==0 line([config_joint_0(1) config_joint_1(1)],[config_joint_0(2) config_joint_1(2)],... [config_joint_0(3) config_joint_1(3)],'linewidth', 1,'Color',color_list{1}) %, 'LineStyle', '--' line([config_joint_1(1) config_joint_2(1)],[config_joint_1(2) config_joint_2(2)],... @@ -28,16 +32,6 @@ [config_joint_2(3) config_joint_3(3)],'linewidth', 1,'Color',color_list{3}) line([config_joint_3(1) config_joint_4(1)],[config_joint_3(2) config_joint_4(2)],... [config_joint_3(3) config_joint_4(3)],'linewidth', 1,'Color',color_list{4}) -% else -% line([config_joint_0(1) config_joint_1(1)],[config_joint_0(2) config_joint_1(2),],... -% [config_joint_0(3) config_joint_1(3)],'linewidth', 1,'Color',color_list{1},'LineStyle', '--') %, 'LineStyle', '--' -% line([config_joint_1(1) config_joint_2(1)],[config_joint_1(2) config_joint_2(2)],... -% [config_joint_1(3) config_joint_2(3)],'linewidth', 1,'Color',color_list{2},'LineStyle', '--') -% line([config_joint_2(1) config_joint_3(1)],[config_joint_2(2) config_joint_3(2)],... -% [config_joint_2(3) config_joint_3(3)],'linewidth', 1,'Color',color_list{3},'LineStyle', '--') -% line([config_joint_3(1) config_joint_4(1)],[config_joint_3(2) config_joint_4(2)],... -% [config_joint_3(3) config_joint_4(3)],'linewidth', 1,'Color',color_list{4},'LineStyle', '--') -% end plot3(config_joint_0(1),config_joint_0(2),config_joint_0(3),'go','linewidth', 4,'MarkerSize',4) plot3(config_joint_1(1),config_joint_1(2),config_joint_1(3),'go','linewidth', 4,'MarkerSize',4) @@ -53,28 +47,12 @@ plot3(p_global(1),p_global(2),p_global(3),'r*') -% text(1.5, 0, 2.8, 'Joint state', 'color', 'red', 'FontSize', 14) -% str = {string(q(1)), string(q(2)), string(q(3)), string(q(4))}; -% text(1.6, 0, 1.5, str) -% -% text(2.5, 0, 2.4,'Joint state velocities', 'color', 'blue', 'FontSize', 14) -% str = {string(q_dot(1)), string(q_dot(2)), string(q_dot(3)), string(q_dot(4))}; -% text(2.8, 0, 1.05, str) -% % -% % text(1.5, 0, 0, {'End-effector','position'}, 'color', 'red', 'FontSize', 14) -% % str = {string(cur_pos(1)), string(cur_pos(2)), string(cur_pos(3)), string(cur_pos(4)), string(cur_pos(5)), string(cur_pos(6))}; -% % text(1.65, 0, -1.35, str) -% -% text(2.6, 0, -0.35, {'End-effector','desired position'}, 'color', 'blue', 'FontSize', 14) -% str = {string(p_global(1)), string(p_global(2)), string(p_global(3))}; -% text(2.9, 0, -1.75, str) - +plot3([r_shd_hr(1) r_shd_fr(1) r_shd_fl(1) r_shd_hl(1) r_shd_hr(1)],[r_shd_hr(2) r_shd_fr(2) r_shd_fl(2) r_shd_hl(2) r_shd_hr(2)],[r_shd_hr(3) r_shd_fr(3) r_shd_fl(3) r_shd_hl(3) r_shd_hr(3)],'k','LineWidth',2); xlim([-3,3]) ylim([-3,3]) zlim([-3,3]) xlabel('x') ylabel('y') zlabel('z') -%title('KUKA on Linear Axis', 'FontSize', 14) end diff --git a/IK_Last_version/angle_joints.m b/IK_Last_version/angle_joints.m deleted file mode 100644 index 288440c..0000000 --- a/IK_Last_version/angle_joints.m +++ /dev/null @@ -1,32 +0,0 @@ -function q = angle_joints(q1,p,dy,dz) - -global L1 L2 L3 -P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); - -%% Front Right -Rq1 = rot(1,q1,4); -r_0_hip= Rq1*P_sho_hip*[0;0;0;1]; - -delta_x= r_0_hip(1)-p(1); -delta_y= r_0_hip(2)-p(2); -delta_z= r_0_hip(3)-p(3); -r_0 = sqrt(dy^2+dz^2-L1(2)^2); - -h_1 = sqrt(delta_x^2+r_0^2); -phi = asin(delta_x/h_1); -if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1 - ratio = 1; -else - ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2); -end -q2 = phi-acos(ratio); -if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1 - ratio1 = -1; -else - ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3); -end -q3 = pi-acos(ratio1); - -q = [q2,q3]; -% pause -end \ No newline at end of file diff --git a/IK_Last_version/angle_joints_old.m b/IK_Last_version/angle_joints_old.m deleted file mode 100644 index 11ce586..0000000 --- a/IK_Last_version/angle_joints_old.m +++ /dev/null @@ -1,64 +0,0 @@ -function q = angle_joints(q1,p,dy,dz) - -global L1 L2 L3 - -% %% Rotation Matrix -% % Pelvis -% R_p_x = rot(1,alpha_p,4); -% R_p_y = rot(2,beta_p,4); -% R_p_z = rot(3,gamma_p,4); -% R0=R_p_y*R_p_z*R_p_x; % Pelvis Rotation Matrix - -% % Front Right -% Rq1 = rot(1,q1,4); -% %% Translation Matrix -% P_p = trans([],r_p); -% % Front Right -% P_b_sho_fr = trans([],0.5*[L_b,-W_b,0]); -% P_b_sho_fl = trans([],0.5*[L_b,W_b,0]); -% P_b_sho_hr = trans([],0.5*[-L_b,-W_b,0]); -% P_b_sho_hl = trans([],0.5*[-L_b,W_b,0]); -P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); -% P_sho_hip_fl = trans([],[L_h(1),L_h(2),-L_h(3)]); -% P_sho_hip_hr = trans([],[-L_h(1),-L_h(2),-L_h(3)]); -% P_sho_hip_hl = trans([],[-L_h(1),L_h(2),-L_h(3)]); - -% r_0_sho_fr = P_p*R0*P_b_sho_fr*[0;0;0;1]; -% r_0_sho_fl = P_p*R0*P_b_sho_fl*[0;0;0;1]; -% r_0_sho_hr = P_p*R0*P_b_sho_hr*[0;0;0;1]; -% r_0_sho_hl = P_p*R0*P_b_sho_hl*[0;0;0;1]; -%% Front Right -% delta_y = -p(2); -% delta_z = -p(3); -% -% h = sqrt(delta_y^2+delta_z^2); -% q1= (atan(delta_y/delta_z)-asin(L1(2)/h)); -% r_0 = sqrt(delta_y^2+delta_z^2-L1(2)^2); - -Rq1 = rot(1,q1,4); -r_0_hip= Rq1*P_sho_hip*[0;0;0;1]; - -delta_x= r_0_hip(1)-p(1); -delta_y= r_0_hip(2)-p(2); -delta_z= r_0_hip(3)-p(3); -r_0 = sqrt(dy^2+dz^2-L1(2)^2); - -% delta_x_hip = -r_0_hip(1)+p(1); -h_1 = sqrt(delta_x^2+r_0^2); -phi = asin(delta_x/h_1); -if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1 - ratio = 1; -else - ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2); -end -q2 = phi-acos(ratio); -if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1 - ratio1 = -1; -else - ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3); -end -q3 = pi-acos(ratio1); - -q = [q2,q3]; -% pause -end \ No newline at end of file diff --git a/IK_Last_version/angle_joints_stefan.m b/IK_Last_version/angle_joints_stefan.m deleted file mode 100644 index 9035d8e..0000000 --- a/IK_Last_version/angle_joints_stefan.m +++ /dev/null @@ -1,265 +0,0 @@ -function q = angle_joints(p) -r_p = p(1:3); % Base position -x_p = r_p(1); -y_p = r_p(2); -z_p = r_p(3); - -q_p = p(4:6); % Base rotation -theta_p = q_p(1); -phi_p = q_p(2); -psi_p = q_p(3); - -r_fr = p(7:9); % Front Right foot position -x_fr = r_fr(1); -y_fr = r_fr(2); -z_fr = r_fr(3); - -r_fl = p(10:12); % Front Left foot position -x_fl = r_fl(1); -y_fl = r_fl(2); -z_fl = r_fl(3); - -r_hr = p(13:15); % Hind Right foot position -x_hr = r_hr(1); -y_hr = r_hr(2); -z_hr = r_hr(3); - -r_hl = p(16:18); % Hind Left foot position -x_hl = r_hl(1); -y_hl = r_hl(2); -z_hl = r_hl(3); - -%% Global -global L_h L_b W_b % Base (Body) dimensions -global redundant -% global L_sh L_th_f L_th_h L_th_h_mid -%% Rotation Matrix -% Body Rotation Matrix -R_p_x = rot(1,theta_p,4); -R_p_y = rot(2,phi_p,4); -R_p_z = rot(3,psi_p,4); -R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix - -%% Translation Matrix -% Base in the World frame -P_p = trans([],r_p); - -% Translation from Base to Shoulder joint -P_b_sho_fr = trans([],0.5*[L_b,-W_b,0]); -P_b_sho_fl = trans([],0.5*[L_b,W_b,0]); -P_b_sho_hr = trans([],0.5*[-L_b,-W_b,0]); -P_b_sho_hl = trans([],0.5*[-L_b,W_b,0]); - -% Translation from Shoulder to Hip joint -P_sho_hip_fr = trans([],[L_h(1),-L_h(2),-L_h(3)]); -P_sho_hip_fl = trans([],[L_h(1),L_h(2),-L_h(3)]); -P_sho_hip_hr = trans([],[-L_h(1),-L_h(2),-L_h(3)]); -P_sho_hip_hl = trans([],[-L_h(1),L_h(2),-L_h(3)]); -% Position of Shoulder joint in the World frame -r_0_sho_fr = P_p*R0*P_b_sho_fr*[0;0;0;1]; -r_0_sho_fl = P_p*R0*P_b_sho_fl*[0;0;0;1]; -r_0_sho_hr = P_p*R0*P_b_sho_hr*[0;0;0;1]; -r_0_sho_hl = P_p*R0*P_b_sho_hl*[0;0;0;1]; -%% Front Right -L2_f = 246.5/1000; % Vertical distance between Shoulder and Knee Joints -L3_f = 280.67/1000; % Vertical distance between Knee and Foot Joints -% off_y = 143.34/1000; - -delta_y = r_0_sho_fr(2)-y_fr; % SV: removed abs() -% delta_y = abs(r_0_sho_fr(2)-y_fr); -delta_z = r_0_sho_fr(3)-z_fr; - -h_fr = sqrt(delta_y^2+delta_z^2); -q1_fr= -atan2(delta_y,delta_z)+asin(L_h(2)/h_fr); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different) -% q1_fr = atan(delta_y/delta_z)-asin(L_h(2)/h); -r_0 = sqrt(h_fr^2-L_h(2)^2); - -Rq1_fr = rot(1,q1_fr,4); -r_0_hip= P_p*R0*P_b_sho_fr*Rq1_fr*P_sho_hip_fr*[0;0;0;1]; - -delta_x_hip = x_fr-r_0_hip(1); - -h_1 = sqrt(delta_x_hip^2+r_0^2); -phi = asin(delta_x_hip/h_1); -q2_fr = acos((h_1^2+L2_f^2-L3_f^2)/(2*h_1*L2_f))-phi; -q3_fr = acos((L3_f^2+L2_f^2-h_1^2)/(2*L3_f*L2_f))-pi; -%% Front Left -delta_y = r_0_sho_fl(2)-y_fl; % SV: removed abs() -% delta_y = abs(r_0_sho_fl(2)-y_fl); -delta_z = r_0_sho_fl(3)-z_fl; - -h_fl = sqrt(delta_y^2+delta_z^2); -q1_fl= -atan2(delta_y,delta_z)-asin(L_h(2)/h_fl); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) -% q1_fl = atan(delta_y/delta_z)-asin(L_h(2)/h_fl); -% q1_fl =0; -r_0 = sqrt(h_fl^2-L_h(2)^2); - -Rq1_fl = rot(1,q1_fl,4); -r_0_hip= P_p*R0*P_b_sho_fl*Rq1_fl*P_sho_hip_fl*[0;0;0;1]; - -delta_x_hip = x_fl-r_0_hip(1); - -h_1 = sqrt(delta_x_hip^2+r_0^2); -phi = asin(delta_x_hip/h_1); -q2_fl = acos((h_1^2+L2_f^2-L3_f^2)/(2*h_1*L2_f))-phi; -q3_fl = acos((L3_f^2+L2_f^2-h_1^2)/(2*L3_f*L2_f))-pi; - -%% Hind Right -if redundant == 1 % Hind legs with redundancy - alpha = 10*pi/180; % alpha = q2+q3+q4 - L2_h = 201/1000; % Vertical distance between Shoulder and Knee Joint (Hind leg) - L3_h = 246.5/1000; % Vertical distance between Knee and Ankle Joint (Hind leg) - - delta_y = r_0_sho_hr(2)-y_hr; % SV: removed abs() -% delta_y = abs(r_0_sho_hr(2)-y_hr); - delta_z = abs(r_0_sho_hr(3)-z_hr); - - h = sqrt(delta_y^2+delta_z^2); - q1_hr= -atan2(delta_y,delta_z)+asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different) -% q1_hr= atan(delta_y/delta_z)-asin((L_h(2))/h); - Rq1_hr = rot(1,q1_hr,4); - r_0_hip= P_p*R0*P_b_sho_hr*Rq1_hr*P_sho_hip_hr*[0;0;0;1]; - - L4 = [0 0 190.62]/1000; % Vertical distance from Knee to foot - - % SV: new x-r plane calcs - dx = x_hr - r_0_hip(1); % SV: hip to foot dx - dy = y_hr - r_0_hip(2); % SV: hip to foot dy - dz = z_hr - r_0_hip(3); % SV: hip to foot dz - - dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip - - dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx - dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr - - h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane - phi = asin(dx_ankle/h_1); % angle to h_1 from r - - % knee_hr = Knee_pos(r_hr,alpha,1,L4); - % delta_x= r_0_hip(1)-knee_hr(1); - % dy=abs(r_0_sho_hr(2)-knee_hr(2)); - % dz=abs(r_0_sho_hr(3)-knee_hr(3)); - % r_0 = sqrt(dy^2+dz^2-L_h(2)^2); - % h_1 = sqrt(delta_x^2+r_0^2); - % phi = asin(delta_x/h_1); - % L2 = 201/1000; - % L3 = 246.5/1000; - - if abs((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))>1 - ratio = 1; - else - ratio = (h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h); - end - q2_hr = -phi-acos(ratio); % SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip - % q2_hr = phi-acos(ratio); - if abs((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))>1 - ratio1 = 1; % SV: changed sign, should be >0 now like ratio above - else - ratio1 = (L3_h^2+L2_h^2-h_1^2)/(2*L2_h*L3_h); - end - q3_hr = pi-acos(ratio1); - q4_hr= -q2_hr-q3_hr-alpha; - %% Hind Left - delta_y = r_0_sho_hl(2)-y_hl; % SV: removed abs() -% delta_y = abs(r_0_sho_hl(2)-y_hl); - delta_z = abs(r_0_sho_hl(3)-z_hl); - - h = sqrt(delta_y^2+delta_z^2); - q1_hl = -atan2(delta_y,delta_z)-asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) -% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h); - Rq1_hl = rot(1,q1_hl,4); - r_0_hip= P_p*R0*P_b_sho_hl*Rq1_hl*P_sho_hip_hl*[0;0;0;1]; - - % SV: new x-r plane calcs - dx = x_hl - r_0_hip(1); % SV: hip to foot dx - dy = y_hl - r_0_hip(2); % SV: hip to foot dy - dz = z_hl - r_0_hip(3); % SV: hip to foot dz - - dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip - - dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx - dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr - - h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane - phi = asin(dx_ankle/h_1); % angle to h_1 from r - - % knee_hl = Knee_pos(r_hl,alpha,0,L4); - % delta_x= r_0_hip(1)-knee_hl(1); - % dy=abs(r_0_sho_hl(2)-knee_hl(2)); - % dz=abs(r_0_sho_hl(3)-knee_hl(3)); - % r_0 = sqrt(dy^2+dz^2-L_h(2)^2); - % h_1 = sqrt(delta_x^2+r_0^2); - % phi = asin(delta_x/h_1); - % L2 = 201/1000; - % L3 = 246.5/1000; - if abs((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))>1 - ratio = 1; % SV: changed sign, should be >0 now like right leg above - % ratio = -1; - else - ratio = (h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h); - end - q2_hl = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip - % q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%% - if abs((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))>1 - ratio1 = 1; - else - ratio1 = (L3_h^2+L2_h^2-h_1^2)/(2*L2_h*L3_h); - end - q3_hl = pi-acos(ratio1); - q4_hl= -q2_hl-q3_hl-alpha; -else - %% Hind Right - L2_h = 246.5/1000; - L3_h = 190.62/1000; -% off_h = 206.84/1000; -% off_h = 145/1000; - - delta_y = r_0_sho_hr(2)-y_hr; % SV: removed abs() -% delta_y = abs(r_0_sho_hr(2)-y_hr); - delta_z = abs(r_0_sho_hr(3)-z_hr); - - h = sqrt(delta_y^2+delta_z^2); - q1_hr= -atan2(delta_y,delta_z)+asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip below shoulder (right and left now different) -% q1_hr= atan(delta_y/delta_z)-asin((L_h(2))/h); - r_0 = sqrt(delta_y^2+delta_z^2-(L_h(2))^2); - - Rq1_hr = rot(1,q1_hr,4); - r_0_hip= P_p*R0*P_b_sho_hr*Rq1_hr*P_sho_hip_hr*[0;0;0;1]; - - delta_x_hip = x_hr-r_0_hip(1); - - h_1 = sqrt(delta_x_hip^2+r_0^2); - phi = asin(delta_x_hip/h_1); - q3_hr = acos((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))-phi+pi/2; % - q4_hr = acos((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))-pi; - q2_hr= -pi/2; - %% Hind Left - delta_y = r_0_sho_hl(2)-y_hl; % SV: removed abs() -% delta_y = abs(r_0_sho_hl(2)-y_hl); - delta_z = r_0_sho_hl(3)-z_hl; - - h = sqrt(delta_y^2+delta_z^2); - q1_hl = -atan2(delta_y,delta_z)-asin(L_h(2)/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) -% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h); - r_0 = sqrt(delta_y^2+delta_z^2-(L_h(2))^2); - - Rq1_hl = rot(1,q1_hl,4); - r_0_hip= P_p*R0*P_b_sho_hl*Rq1_hl*P_sho_hip_hl*[0;0;0;1]; - - delta_x_hip = x_hl-r_0_hip(1); - - h_1 = sqrt(delta_x_hip^2+r_0^2); - phi = asin(delta_x_hip/h_1); - q3_hl = acos((h_1^2+L2_h^2-L3_h^2)/(2*h_1*L2_h))-phi+pi/2; - q4_hl = acos((L3_h^2+L2_h^2-h_1^2)/(2*L3_h*L2_h))-pi; - q2_hl= -pi/2; - -end -%% Output -q_fr = [q1_fr q2_fr q3_fr]'; % Right Angles -q_fl = [q1_fl q2_fl q3_fl]'; % Right Angles -q_hr = [q1_hr q2_hr q3_hr q4_hr]'; % Right Angles -q_hl = [q1_hl q2_hl q3_hl q4_hl]'; % Right Angles - -q = [q_fr;q_fl;q_hr;q_hl]; -end \ No newline at end of file diff --git a/IK_Last_version/check_security.m b/IK_Last_version/check_security.m deleted file mode 100644 index cf10dc5..0000000 --- a/IK_Last_version/check_security.m +++ /dev/null @@ -1,16 +0,0 @@ -function y = check_security(q) -y= zeros(2,4); -global lower_joint_limits upper_joint_limits -for i=1:4 - if (q(i) - upper_joint_limits(i) >= 0) - y(1,i) = 0; - else - y(1,i)=1; - end - if (q(i) - lower_joint_limits(i) <= 0) - y(2,i) = 0; - else - y(2,i)=1; - end -end -end \ No newline at end of file diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index dd80b85..202c227 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -3,36 +3,59 @@ close all; clc; -global L1 L2 L3 L4 +global L1 L2 L3 L4 L_b W_b -L1= [53.26 61.84 0.1]/1000; % Length of hip -L2= 201/1000; % Length of hind thigh [0 46.5 201] -L3= 246.5/1000; % Length of middle hind thigh [0 63.5 246.5] -% L4=[-77.18 -145 -280.52]/1000; % Length of shank -L4=[0 0 190.62]/1000; % Length of shank 280.52 190.62 +L1 = [53.26 206.84 0.1]/1000; % Length of hip +L2 = 201/1000; % Length of hind thigh +L3 = 246.5/1000; % Length of middle hind thigh +L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 +L_b = 527/1000; % Length of body +W_b = 245.81/1000; % Width of body color_list = {'blue', 'red','green', 'black'}; -% p_global = [-53.2600 -61.8400 -638.2200]'/1000; % zero position -728.12 -% p_global = [-130.61 -95.84 -620.05]'/1000; -% p_global = [-226.55 -153.84 -463.83]'/1000; -p_global = [192.46 -206.84 18.8]'/1000; - -r_0_sho = [-0,-0,0]'/1000; -alpha =10*pi/180; -offset_y = 145/1000; - -delta_y = r_0_sho(2)-p_global(2); % SV: removed abs() -% delta_y = abs(r_0_sho_hl(2)-y_hl); -delta_z = abs(r_0_sho(3)-p_global(3)); +%% Position of the base +r_p = [0.0 0.0 0.0]'/1000; +angle_p = [0.0 0.0 0.0]'*pi/180; +R_p_x = rot(1,angle_p(1),4); +R_p_y = rot(2,angle_p(2),4); +R_p_z = rot(3,angle_p(3),4); +R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix + +base_pos = [r_p, angle_p]; +%% +P_p = trans([],r_p); + +r_b_sho_hr = trans([],[-L_b/2,-W_b/2,0]); +r_b_sho_hl = trans([],[-L_b/2,W_b/2,0]); +r_b_sho_fr = trans([],[L_b/2,-W_b/2,0]); +r_b_sho_fl = trans([],[L_b/2,W_b/2,0]); + +r_w_sho_hr = P_p*R0*r_b_sho_hr*[0;0;0;1]; +r_w_sho_hl = P_p*R0*r_b_sho_hl*[0;0;0;1]; +r_w_sho_fr = P_p*R0*r_b_sho_fr*[0;0;0;1]; +r_w_sho_fl = P_p*R0*r_b_sho_fl*[0;0;0;1]; + +body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)]; +%% Position of foot +% p_global = [-393.96 -329.74 -638.29]'/1000; % zero position +% p_global = [-393.96 -229.74 -638.29]'/1000; +p_global = [-323.96 -229.74 -438.29]'/1000; +% p_global = [-323.96 -229.74 -258.29]'/1000; + +%% +alpha = 0*pi/180; +%offset_y = 145/1000; + +delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs() +delta_z = abs(r_w_sho_hr(3)-p_global(3)); h = sqrt(delta_y^2+delta_z^2); q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) -% q1_hl= atan(delta_y/delta_z)-asin((L_h(2))/h); Rq1 = rot(1,q1,4); P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); -r_0_hip= Rq1*P_sho_hip*[0;0;0;1]; +r_0_hip= P_p*R0*r_b_sho_hr*Rq1*P_sho_hip*[0;0;0;1]; % SV: new x-r plane calcs dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx @@ -41,8 +64,9 @@ dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip -dx_ankle = dx - L4(3)*sin(alpha); % hip to ankle dx -dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr +% rr = sqrt(L4(1)^2+L4(3)^2); +dx_ankle = dx - L4(3)*sin(alpha) + L4(1)*cos(alpha); % hip to ankle dx +dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr L4(3)*cos(alpha) h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane phi = asin(dx_ankle/h_1); % angle to h_1 from r @@ -66,9 +90,9 @@ q4= -q2-q3-alpha; q_new =[q1,q2,q3,q4]; -Pos = FK(q_new); %q_new +foot_Pos = FK(q_new, base_pos); %q_new figure view(3) -Visualize_Robot(Pos, p_global, color_list) %Pos(:,3) +Visualize_Robot(body_pos,foot_Pos, p_global, color_list) %Pos(:,3) pause(0.1); diff --git a/IK_Last_version/quad_properties.m b/IK_Last_version/quad_properties.m deleted file mode 100644 index 14ea7ca..0000000 --- a/IK_Last_version/quad_properties.m +++ /dev/null @@ -1,8 +0,0 @@ -% Biped Properties -global L1 L2 L3 L4 -%% Length OK -L1= [53.26 61.84 0.1]/1000; % Length of hip -L2= 201/1000; % Length of hind thigh [0 46.5 201] -L3= 246.5/1000; % Length of middle hind thigh [0 63.5 246.5] -%L4=[77.18 145 280.52]/1000; % Length of shank -L4=[77.18 145 190.62]/1000; % Length of shank From a9658d172f7e7d17cb606d3ce0b919db44abae76 Mon Sep 17 00:00:00 2001 From: Boogard Date: Mon, 9 Sep 2024 19:22:13 -0600 Subject: [PATCH 03/13] add a for loop for checking the best alpha value --- IK_Last_version/main.m | 203 +++++++++++++++++++++-------------------- 1 file changed, 105 insertions(+), 98 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 202c227..bfeeaff 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -1,98 +1,105 @@ -%% Redundancy Resolution -clear all; -close all; -clc; - -global L1 L2 L3 L4 L_b W_b - -L1 = [53.26 206.84 0.1]/1000; % Length of hip -L2 = 201/1000; % Length of hind thigh -L3 = 246.5/1000; % Length of middle hind thigh -L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 -L_b = 527/1000; % Length of body -W_b = 245.81/1000; % Width of body - -color_list = {'blue', 'red','green', 'black'}; - -%% Position of the base -r_p = [0.0 0.0 0.0]'/1000; -angle_p = [0.0 0.0 0.0]'*pi/180; -R_p_x = rot(1,angle_p(1),4); -R_p_y = rot(2,angle_p(2),4); -R_p_z = rot(3,angle_p(3),4); -R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix - -base_pos = [r_p, angle_p]; -%% -P_p = trans([],r_p); - -r_b_sho_hr = trans([],[-L_b/2,-W_b/2,0]); -r_b_sho_hl = trans([],[-L_b/2,W_b/2,0]); -r_b_sho_fr = trans([],[L_b/2,-W_b/2,0]); -r_b_sho_fl = trans([],[L_b/2,W_b/2,0]); - -r_w_sho_hr = P_p*R0*r_b_sho_hr*[0;0;0;1]; -r_w_sho_hl = P_p*R0*r_b_sho_hl*[0;0;0;1]; -r_w_sho_fr = P_p*R0*r_b_sho_fr*[0;0;0;1]; -r_w_sho_fl = P_p*R0*r_b_sho_fl*[0;0;0;1]; - -body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)]; -%% Position of foot -% p_global = [-393.96 -329.74 -638.29]'/1000; % zero position -% p_global = [-393.96 -229.74 -638.29]'/1000; -p_global = [-323.96 -229.74 -438.29]'/1000; -% p_global = [-323.96 -229.74 -258.29]'/1000; - -%% -alpha = 0*pi/180; -%offset_y = 145/1000; - -delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs() -delta_z = abs(r_w_sho_hr(3)-p_global(3)); - -h = sqrt(delta_y^2+delta_z^2); -q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) -Rq1 = rot(1,q1,4); -P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); - -r_0_hip= P_p*R0*r_b_sho_hr*Rq1*P_sho_hip*[0;0;0;1]; - -% SV: new x-r plane calcs -dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx -dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy -dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz - -dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip - -% rr = sqrt(L4(1)^2+L4(3)^2); -dx_ankle = dx - L4(3)*sin(alpha) + L4(1)*cos(alpha); % hip to ankle dx -dr_ankle = dr_foot - L4(3)*cos(alpha); % hip to ankle dr L4(3)*cos(alpha) - -h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane -phi = asin(dx_ankle/h_1); % angle to h_1 from r - - -if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1 - ratio = 1; % SV: changed sign, should be >0 now like right leg above - q2 =0; -else - ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2); - q2 = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip -end -% q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%% -if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1 - ratio1 = 1; - q3=0; -else - ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3); - q3 = pi-acos(ratio1); -end - -q4= -q2-q3-alpha; -q_new =[q1,q2,q3,q4]; -foot_Pos = FK(q_new, base_pos); %q_new -figure -view(3) -Visualize_Robot(body_pos,foot_Pos, p_global, color_list) %Pos(:,3) -pause(0.1); - +%% Redundancy Resolution +clear all; +close all; +clc; + +global L1 L2 L3 L4 L_b W_b + +L1 = [53.26 206.84 0.1]/1000; % Length of hip +L2 = 201/1000; % Length of hind thigh +L3 = 246.5/1000; % Length of middle hind thigh +L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 +L_b = 527/1000; % Length of body +W_b = 245.81/1000; % Width of body + +color_list = {'blue', 'red','green', 'black'}; + +%% Position of the base +r_p = [0.0 0.0 0.0]'/1000; +angle_p = [0.0 0.0 0.0]'*pi/180; +R_p_x = rot(1,angle_p(1),4); +R_p_y = rot(2,angle_p(2),4); +R_p_z = rot(3,angle_p(3),4); +R0=R_p_z*R_p_y*R_p_x; % Body Rotation Matrix + +base_pos = [r_p, angle_p]; +%% +P_p = trans([],r_p); + +r_b_sho_hr = trans([],[-L_b/2,-W_b/2,0]); +r_b_sho_hl = trans([],[-L_b/2,W_b/2,0]); +r_b_sho_fr = trans([],[L_b/2,-W_b/2,0]); +r_b_sho_fl = trans([],[L_b/2,W_b/2,0]); + +r_w_sho_hr = P_p*R0*r_b_sho_hr*[0;0;0;1]; +r_w_sho_hl = P_p*R0*r_b_sho_hl*[0;0;0;1]; +r_w_sho_fr = P_p*R0*r_b_sho_fr*[0;0;0;1]; +r_w_sho_fl = P_p*R0*r_b_sho_fl*[0;0;0;1]; + +body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)]; +%% Position of foot +% p_global = [-393.96 -329.74 -638.29]'/1000; % zero position +% p_global = [-393.96 -229.74 -638.29]'/1000; +% p_global = [-323.96 -229.74 -438.29]'/1000; +p_global = [-393.96 -329.74 -38.29]'/1000; +% p_global = [-323.96 -229.74 -188.29]'/1000; + +%% + +alpha = 0*pi/180:-0.2*pi/180:-75*pi/180; +delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs() +delta_z = abs(r_w_sho_hr(3)-p_global(3)); + +h = sqrt(delta_y^2+delta_z^2); +q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) +Rq1 = rot(1,q1,4); +P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); + +r_0_hip= P_p*R0*r_b_sho_hr*Rq1*P_sho_hip*[0;0;0;1]; + +% SV: new x-r plane calcs +dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx +dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy +dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz +dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip + +for i = 1: length(alpha) + dx_ankle = dx - L4(3)*sin(alpha(i)) + L4(1)*cos(alpha((i))); % hip to ankle dx + dr_ankle = dr_foot - L4(3)*cos(alpha(i)); % hip to ankle dr L4(3)*cos(alpha) + + h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane + phi = asin(dx_ankle/h_1); % angle to h_1 from r + + + if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1 + ratio = 1; % SV: changed sign, should be >0 now like right leg above + q2 =0; + else + ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2); + q2 = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip + end + % q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%% + if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1 + ratio1 = 1; + q3=0; + else + ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3); + q3 = pi-acos(ratio1); + end + + q4= -q2-q3-alpha(i); + q_new(i,:) =[q1,q2,q3,q4]; + foot_Pos = FK(q_new(i,:), base_pos); %q_new + error(i) = sqrt((foot_Pos(1,5)-p_global(1))^2+(foot_Pos(2,5)-p_global(2))^2+(foot_Pos(3,5)-p_global(3))^2); % root square error +end +plot(alpha*180/pi, error) +[minError, minIndex] = min(error); +minAlpha = alpha(minIndex); +foot_Pos_min = FK(q_new(minIndex,:), base_pos); + +figure +view(3) +Visualize_Robot(body_pos,foot_Pos_min, p_global, color_list) %Pos(:,3) +pause(0.1); + + From cb807f4100581ef617974537bb1ebd664cd4dda9 Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 10 Sep 2024 12:19:18 -0600 Subject: [PATCH 04/13] SV comments on frame changes --- IK_Last_version/FK.m | 4 ++++ IK_Last_version/main.m | 8 +++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/IK_Last_version/FK.m b/IK_Last_version/FK.m index 534ba8a..eec60ff 100644 --- a/IK_Last_version/FK.m +++ b/IK_Last_version/FK.m @@ -26,6 +26,10 @@ Rq3_hr = rot(2,q3_hl,4); Rq4_hr = rot(2,q4_hl,4); +% Why is L_h(3)=0.17 mm here, vs. 0.1 mm elsewhere (e.g., in main.m)? Also, +% is it important to split the y-offset for links 1-3 here vs. grouping in +% link 1 as done for IK? Should I add to my writeup? This means the FK +% positions of frames 1-2 are different than IK. L_h = [53.26, 61.34, 0.17]/1000; L_th_h_mid = [0.0, 92.88, 201.0]/1000; L_th_h = [0.0, 52.62, 246.5]/1000; diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index bfeeaff..320fcaf 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -5,10 +5,16 @@ global L1 L2 L3 L4 L_b W_b +% SV: is the L1(3)=0.1 mm accurate and necessary (vs. setting to 0)? I +% had (maybe mistakenly) neglected this in my writeup as I thought link +% 1 lay in the x1-y1 plane in zero pose. If necessary I can add. L1 = [53.26 206.84 0.1]/1000; % Length of hip L2 = 201/1000; % Length of hind thigh L3 = 246.5/1000; % Length of middle hind thigh -L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 +% SV: I neglected the x-component of L4 in my writeup. To be corrected. To +% discuss: do we prefer to have x and z component in zero pose, or instead +% make zero pose when foot is directly under ankle (x = 0, z adjusted)? +L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 L_b = 527/1000; % Length of body W_b = 245.81/1000; % Width of body From 4ba0c47c5989a9726539b8a88ba786cb8149957f Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 10 Sep 2024 14:11:20 -0600 Subject: [PATCH 05/13] Update q1 calc to joint 1 frame --- IK_Last_version/main.m | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 320fcaf..8edd17e 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -53,11 +53,25 @@ %% alpha = 0*pi/180:-0.2*pi/180:-75*pi/180; -delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs() -delta_z = abs(r_w_sho_hr(3)-p_global(3)); +% TODO: cleanup commented code after testing +% delta_y = r_w_sho_hr(2)-p_global(2); % SV: removed abs() +% delta_z = abs(r_w_sho_hr(3)-p_global(3)); + +% vector from joint 1 to 5 (foot) in joint 1_hr frame: +p_1_5_hr = transpose(R0)*(p_global - r_w_sho_hr); + +% h = sqrt(delta_y^2+delta_z^2); +% q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign +% => q1 should be positive when hip above shoulder (right and left now different) + +% h using joint 1 frame instead of delta_y & z: +h = sqrt(p_1_5_hr(2)^2 + p_1_5_hr(3)^2); + +% hr q1 using joint 1 frame and with q1 positive when hip above shoulder +% (right and left now different). atan2 discontinuity rotated to an +% unattainable point (along y_1 axis) (by -pi/2): +q1 = atan2(-p_1_5_hr(3), -p_1_5_hr(2))+ asin(L1(2)/h) - pi/2; -h = sqrt(delta_y^2+delta_z^2); -q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign => q1 should be positive when hip above shoulder (right and left now different) Rq1 = rot(1,q1,4); P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); From 853d37b28339e3a7094433e0fcd3ccf10e8fe9b3 Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 10 Sep 2024 15:47:22 -0600 Subject: [PATCH 06/13] Correction to q1 comp - R0 size. --- IK_Last_version/main.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 8edd17e..0e5dfad 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -58,7 +58,7 @@ % delta_z = abs(r_w_sho_hr(3)-p_global(3)); % vector from joint 1 to 5 (foot) in joint 1_hr frame: -p_1_5_hr = transpose(R0)*(p_global - r_w_sho_hr); +p_1_5_hr = transpose(R0(1:3,1:3)) * (p_global - r_w_sho_hr(1:3)); % h = sqrt(delta_y^2+delta_z^2); % q1 = -atan2(delta_y,delta_z)+asin((L1(2))/h); % SV: updated atan2 and sign From 47ad69c2881ed9d9662149a27d26ff6960e98954 Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 10 Sep 2024 15:49:54 -0600 Subject: [PATCH 07/13] Initial update of IK using new method - issue Shows error between FK and IK with body rotation not zero. And alpha may not be as expected. Still working on it. --- IK_Last_version/main.m | 65 ++++++++++++++++++++++++++---------------- 1 file changed, 41 insertions(+), 24 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 0e5dfad..0220fac 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -72,39 +72,56 @@ % unattainable point (along y_1 axis) (by -pi/2): q1 = atan2(-p_1_5_hr(3), -p_1_5_hr(2))+ asin(L1(2)/h) - pi/2; -Rq1 = rot(1,q1,4); -P_sho_hip = trans([],[-L1(1),-L1(2),-L1(3)]); +% Renamed several for consistency with writeup notation & clarity. +R_1_2_hr = rot(1,q1,4); +R_0_2_hr = R0 * R_1_2_hr; -r_0_hip= P_p*R0*r_b_sho_hr*Rq1*P_sho_hip*[0;0;0;1]; +P_1_2_hr = trans([],[-L1(1),-L1(2),-L1(3)]); +p_0_2_hr = P_p*R0*r_b_sho_hr*R_1_2_hr*P_1_2_hr*[0;0;0;1]; -% SV: new x-r plane calcs -dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx -dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy -dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz -dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip - -for i = 1: length(alpha) - dx_ankle = dx - L4(3)*sin(alpha(i)) + L4(1)*cos(alpha((i))); % hip to ankle dx - dr_ankle = dr_foot - L4(3)*cos(alpha(i)); % hip to ankle dr L4(3)*cos(alpha) - - h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane - phi = asin(dx_ankle/h_1); % angle to h_1 from r +% Vector from frame 2 to 5 in joint 2 frame: +p_2_5_hr = transpose(R_0_2_hr(1:3,1:3)) * (p_global - p_0_2_hr(1:3)); +% % SV: new x-r plane calcs +% dx = p_global(1) - r_0_hip(1); % SV: hip to foot dx +% dy = p_global(2) - r_0_hip(2); % SV: hip to foot dy +% dz = p_global(3) - r_0_hip(3); % SV: hip to foot dz +% dr_foot = sqrt(dy^2+dz^2); % positive and increasing as foot moves away from hip - if abs((h_1^2+L2^2-L3^2)/(2*h_1*L2))>1 - ratio = 1; % SV: changed sign, should be >0 now like right leg above - q2 =0; +%% +for i = 1: length(alpha) + % Vector from frame 5 to 4 (notice order) in joint 2 frame as a function of alpha: + p_2_5_4 = [L4(1)*cos(alpha(i)) - L4(3)*sin(alpha(i)); 0; L4(1)*sin(alpha(i)) + L4(3)*cos(alpha(i))]; + + p_2_4_hr = p_2_5_hr + p_2_5_4; % Vector from frame 2 to 4 in joint 2 frame. + +% Angle between z_2 axis and p_2_4_hr, with dextral sign. Renamed from +% phi to avoid confusion with body rotation angle. + beta = atan2(-p_2_4_hr(1)^2, -p_2_4_hr(3)^2); + +% dx_ankle = dx - L4(3)*sin(alpha(i)) + L4(1)*cos(alpha((i))); % hip to ankle dx +% dr_ankle = dr_foot - L4(3)*cos(alpha(i)); % hip to ankle dr L4(3)*cos(alpha) + +% h_1 = sqrt(dx_ankle^2+dr_ankle^2); % hyp. to ankle in x-r plane +% phi = asin(dx_ankle/h_1); % angle to h_1 from r + +% norm(p_2_4_hr) is the magnitude (2-norm) of the vector p_2_4_hr, +% equivalent to h_1 previously used: +% Also, I don't know if the domain check is still necessary: + if abs((norm(p_2_4_hr)^2+L2^2-L3^2)/(2*norm(p_2_4_hr)*L2))>1 + ratio = 1; + q2 = 0; else - ratio = (h_1^2+L2^2-L3^2)/(2*h_1*L2); - q2 = -phi-acos(ratio); %% SV: -phi because dx>0 in same direction as x, q2>0 when knee behind hip + ratio = (norm(p_2_4_hr)^2+L2^2-L3^2)/(2*norm(p_2_4_hr)*L2); + q2 = -acos(ratio) + beta; % Note change in sign because of orientation of beta. end % q2_hl = -acos(ratio)+phi; %%%%%%%%%%%%% - if abs((L3^2+L2^2-h_1^2)/(2*L3*L2))>1 + if abs((L3^2+L2^2-norm(p_2_4_hr)^2)/(2*L3*L2))>1 ratio1 = 1; - q3=0; + q3 = 0; else - ratio1 = (L3^2+L2^2-h_1^2)/(2*L2*L3); - q3 = pi-acos(ratio1); + ratio1 = (L3^2+L2^2-norm(p_2_4_hr)^2)/(2*L3*L2); + q3 = -acos(ratio1) + pi; end q4= -q2-q3-alpha(i); From 0f9fecf3564b50144562e84a6007b1bd8baef248 Mon Sep 17 00:00:00 2001 From: TheIndoorDad Date: Wed, 11 Sep 2024 12:04:49 -0600 Subject: [PATCH 08/13] Modify L4 so zero pose with foot directly below frame 4. --- IK_Last_version/main.m | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 0220fac..00dada1 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -5,16 +5,13 @@ global L1 L2 L3 L4 L_b W_b -% SV: is the L1(3)=0.1 mm accurate and necessary (vs. setting to 0)? I -% had (maybe mistakenly) neglected this in my writeup as I thought link -% 1 lay in the x1-y1 plane in zero pose. If necessary I can add. L1 = [53.26 206.84 0.1]/1000; % Length of hip L2 = 201/1000; % Length of hind thigh L3 = 246.5/1000; % Length of middle hind thigh -% SV: I neglected the x-component of L4 in my writeup. To be corrected. To -% discuss: do we prefer to have x and z component in zero pose, or instead -% make zero pose when foot is directly under ankle (x = 0, z adjusted)? -L4 = [77.21 0 190.62]/1000; % Length of shank dx = 77.21/1000 +% SV: As discussed, will set the zero position to the point with frame 5 +% directly under frame 4 (on z4 axis). Length between frame 4 and 5 then equal +% to sqrt(77.21^2 + 190.62^2) = 205.66 mm. +L4 = norm([77.21 0 190.62])/1000; % Length of shank dx = 77.21/1000 L_b = 527/1000; % Length of body W_b = 245.81/1000; % Width of body @@ -91,7 +88,7 @@ %% for i = 1: length(alpha) % Vector from frame 5 to 4 (notice order) in joint 2 frame as a function of alpha: - p_2_5_4 = [L4(1)*cos(alpha(i)) - L4(3)*sin(alpha(i)); 0; L4(1)*sin(alpha(i)) + L4(3)*cos(alpha(i))]; + p_2_5_4 = [-L4*sin(alpha(i)); 0; L4*cos(alpha(i))]; p_2_4_hr = p_2_5_hr + p_2_5_4; % Vector from frame 2 to 4 in joint 2 frame. From 6a57b34d1bea65b54a66887d3258070989d1025e Mon Sep 17 00:00:00 2001 From: TheIndoorDad Date: Wed, 11 Sep 2024 12:15:12 -0600 Subject: [PATCH 09/13] Remove SV comment resolved after discussion. --- IK_Last_version/FK.m | 4 ---- 1 file changed, 4 deletions(-) diff --git a/IK_Last_version/FK.m b/IK_Last_version/FK.m index eec60ff..534ba8a 100644 --- a/IK_Last_version/FK.m +++ b/IK_Last_version/FK.m @@ -26,10 +26,6 @@ Rq3_hr = rot(2,q3_hl,4); Rq4_hr = rot(2,q4_hl,4); -% Why is L_h(3)=0.17 mm here, vs. 0.1 mm elsewhere (e.g., in main.m)? Also, -% is it important to split the y-offset for links 1-3 here vs. grouping in -% link 1 as done for IK? Should I add to my writeup? This means the FK -% positions of frames 1-2 are different than IK. L_h = [53.26, 61.34, 0.17]/1000; L_th_h_mid = [0.0, 92.88, 201.0]/1000; L_th_h = [0.0, 52.62, 246.5]/1000; From 90ae6c666a262b0e9341e55dd4cec24d3f743c08 Mon Sep 17 00:00:00 2001 From: TheIndoorDad Date: Wed, 11 Sep 2024 12:35:13 -0600 Subject: [PATCH 10/13] Adjusted L_sh to put foot under frame 4 in zero pose. --- IK_Last_version/FK.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/IK_Last_version/FK.m b/IK_Last_version/FK.m index 534ba8a..8faf694 100644 --- a/IK_Last_version/FK.m +++ b/IK_Last_version/FK.m @@ -29,7 +29,8 @@ L_h = [53.26, 61.34, 0.17]/1000; L_th_h_mid = [0.0, 92.88, 201.0]/1000; L_th_h = [0.0, 52.62, 246.5]/1000; -L_sh = [77.21, 0.0, 190.62]/1000; +% Adjusted L_sh to put foot under frame 4 in zero pose, as discussed. +L_sh = [0.0, 0.0, norm([77.21, 190.62])]/1000; P_hip_hr = trans([],[-L_h(1);-L_h(2);-L_h(3)]); P_knee_mid_hr = trans([],[-L_th_h_mid(1);-L_th_h_mid(2);-L_th_h_mid(3)]); P_knee_hr = trans([],[-L_th_h(1);-L_th_h(2);-L_th_h(3)]); From 28a6d5cbeb2a936ffdf368bb2ff25121d82997da Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 17 Sep 2024 12:21:16 -0600 Subject: [PATCH 11/13] Corrected beta calc. Mistakenly squared parameters previously. IK working now for alpha = 0 and +-45 at 30 deg. roll pitch OR yaw (haven't tested combinations), but some alpha angles break calculation (e.g., alpha > 50). --- IK_Last_version/main.m | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 00dada1..3a657c7 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -41,10 +41,12 @@ body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)]; %% Position of foot +% p_global = [-393.96+77.21 -329.74 -638.29+(norm([77.21, 190.62])-190.62)]'/1000; % adjusted zero position +p_global = [-393.96+77.21 -329.74 -438.29+(norm([77.21, 190.62])-190.62)]'/1000; % p_global = [-393.96 -329.74 -638.29]'/1000; % zero position % p_global = [-393.96 -229.74 -638.29]'/1000; % p_global = [-323.96 -229.74 -438.29]'/1000; -p_global = [-393.96 -329.74 -38.29]'/1000; +% p_global = [-393.96 -329.74 -38.29]'/1000; % p_global = [-323.96 -229.74 -188.29]'/1000; %% @@ -94,7 +96,7 @@ % Angle between z_2 axis and p_2_4_hr, with dextral sign. Renamed from % phi to avoid confusion with body rotation angle. - beta = atan2(-p_2_4_hr(1)^2, -p_2_4_hr(3)^2); + beta = atan2(-p_2_4_hr(1), -p_2_4_hr(3)); % dx_ankle = dx - L4(3)*sin(alpha(i)) + L4(1)*cos(alpha((i))); % hip to ankle dx % dr_ankle = dr_foot - L4(3)*cos(alpha(i)); % hip to ankle dr L4(3)*cos(alpha) From 23a3c3f6e11ebc15464b43b5a6a460e1c1917cfd Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 17 Sep 2024 13:39:55 -0600 Subject: [PATCH 12/13] Corrected zero position and added printout. --- IK_Last_version/main.m | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 3a657c7..4daeb5a 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -41,9 +41,13 @@ body_pos= [r_w_sho_hr(1:3),r_w_sho_hl(1:3),r_w_sho_fr(1:3),r_w_sho_fl(1:3)]; %% Position of foot -% p_global = [-393.96+77.21 -329.74 -638.29+(norm([77.21, 190.62])-190.62)]'/1000; % adjusted zero position -p_global = [-393.96+77.21 -329.74 -438.29+(norm([77.21, 190.62])-190.62)]'/1000; -% p_global = [-393.96 -329.74 -638.29]'/1000; % zero position +p_global = [-393.96+77.21 -329.74 -638.29-(norm([77.21, 190.62])-190.62)]'/1000; % adjusted zero position +% p_global = [-393.96+77.21 -329.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000; +% p_global = [-393.96+77.21 -329.74 -138.29-(norm([77.21, 190.62])-190.62)]'/1000; +% p_global = [-393.96+77.21 -429.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000; +% p_global = [-193.96+77.21 -329.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000; + +% p_global = [-393.96 -329.74 -638.29]'/1000; % old zero position % p_global = [-393.96 -229.74 -638.29]'/1000; % p_global = [-323.96 -229.74 -438.29]'/1000; % p_global = [-393.96 -329.74 -38.29]'/1000; @@ -131,6 +135,7 @@ plot(alpha*180/pi, error) [minError, minIndex] = min(error); minAlpha = alpha(minIndex); +fprintf("alpha = %.2f deg\nerror = %.2e m", minAlpha*180/pi, minError) foot_Pos_min = FK(q_new(minIndex,:), base_pos); figure From 86c560a9a6d56fe83d03e7dfe76d5320440a39d3 Mon Sep 17 00:00:00 2001 From: TheIndoorDad <167908515+TheIndoorDad@users.noreply.github.com> Date: Tue, 17 Sep 2024 13:49:25 -0600 Subject: [PATCH 13/13] Minor update. --- IK_Last_version/main.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/IK_Last_version/main.m b/IK_Last_version/main.m index 4daeb5a..506a5c6 100644 --- a/IK_Last_version/main.m +++ b/IK_Last_version/main.m @@ -46,6 +46,7 @@ % p_global = [-393.96+77.21 -329.74 -138.29-(norm([77.21, 190.62])-190.62)]'/1000; % p_global = [-393.96+77.21 -429.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000; % p_global = [-193.96+77.21 -329.74 -438.29-(norm([77.21, 190.62])-190.62)]'/1000; +% p_global = [-393.96+77.21 -329.74 600.00]'/1000; % p_global = [-393.96 -329.74 -638.29]'/1000; % old zero position % p_global = [-393.96 -229.74 -638.29]'/1000; @@ -135,7 +136,7 @@ plot(alpha*180/pi, error) [minError, minIndex] = min(error); minAlpha = alpha(minIndex); -fprintf("alpha = %.2f deg\nerror = %.2e m", minAlpha*180/pi, minError) +fprintf("alpha = %.2f deg\nerror = %.2e m\n", minAlpha*180/pi, minError) foot_Pos_min = FK(q_new(minIndex,:), base_pos); figure