From 7834c2cb8f82f0fe3531c8f2350852099fb82bff Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 13 Mar 2026 15:20:48 +0100 Subject: [PATCH 01/16] Add anal test package Signed-off-by: Florian Vahl --- src/bitbots_motion/wolfgang_ik/CMakeLists.txt | 58 +++ .../wolfgang_ik/docs/_static/logo.png | Bin 0 -> 11686 bytes src/bitbots_motion/wolfgang_ik/docs/conf.py | 187 +++++++++ src/bitbots_motion/wolfgang_ik/docs/index.rst | 21 + .../wolfgang_ik/include/wolfgang_ik/ik.h | 371 ++++++++++++++++++ src/bitbots_motion/wolfgang_ik/package.xml | 18 + src/bitbots_motion/wolfgang_ik/src/main.cpp | 150 +++++++ src/bitbots_motion/wolfgang_ik/src/ros.cpp | 203 ++++++++++ 8 files changed, 1008 insertions(+) create mode 100644 src/bitbots_motion/wolfgang_ik/CMakeLists.txt create mode 100644 src/bitbots_motion/wolfgang_ik/docs/_static/logo.png create mode 100644 src/bitbots_motion/wolfgang_ik/docs/conf.py create mode 100644 src/bitbots_motion/wolfgang_ik/docs/index.rst create mode 100644 src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h create mode 100644 src/bitbots_motion/wolfgang_ik/package.xml create mode 100644 src/bitbots_motion/wolfgang_ik/src/main.cpp create mode 100644 src/bitbots_motion/wolfgang_ik/src/ros.cpp diff --git a/src/bitbots_motion/wolfgang_ik/CMakeLists.txt b/src/bitbots_motion/wolfgang_ik/CMakeLists.txt new file mode 100644 index 000000000..6be314cff --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(wolfgang_ik) + +# Add support for C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Build with release optimizations and debug symbols by default +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebug) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(backward_ros REQUIRED) +find_package(bitbots_docs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_compile_options(-Wall -Werror -Wno-unused -Wfatal-errors) + +set(INCLUDE_DIRS include) +include_directories(${INCLUDE_DIRS}) + +add_executable(ik src/main.cpp ) +add_executable(node src/ros.cpp) + +ament_target_dependencies( + ik + ament_cmake + ament_index_cpp + bitbots_docs + tf2_eigen + Eigen3) + +ament_target_dependencies( + node + ament_cmake + ament_index_cpp + bitbots_docs + tf2_eigen + sensor_msgs + Eigen3) + + + +enable_bitbots_docs() + +install(TARGETS ik DESTINATION lib/${PROJECT_NAME}) +install(TARGETS node DESTINATION lib/${PROJECT_NAME}) + +#install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +#install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/bitbots_motion/wolfgang_ik/docs/_static/logo.png b/src/bitbots_motion/wolfgang_ik/docs/_static/logo.png new file mode 100644 index 0000000000000000000000000000000000000000..f8afdd5d066232e2dba94b758c6c44fe72b42820 GIT binary patch literal 11686 zcmXw92RN1Q|9;Jm5r>RpWG7@(BpJsDp=`1phm^hd-h0QfqL3Xzj!i~JRv}wfGBUII zKi}VV{om_2=klKSea`bfgp&VAb|Sc&F*NFgB@tx<-Z;YfUD5ico@ud|H^*oUSla|u+Kdso~i3-C87>d;4E{yfHSMIeSYOAHl#@;HngrMIR za(p~|u$?+a2JyvKOCTfT;B1gttk6vv#`gO?`gt~(xpedquJ|CL=|PKn?m9DD=mk^Y z`pvEr56IzTL$yfX%#R0!t^&gc*Om=4+dse|CnvY0%~O{v^gtfA1liK!FWrLd>t?nK z23GeY3%FpxffosI;X?I+nzx%0qG}G}@BNLfK7d%1$HcNQ z7^x>Pa})}-B+n*Pxp@XIxVsR(qwlQ3YSMhQs<^3m)2aPq+MK?l*ksrL8ST zs;06I?Xet3C`*od*ZRSb87}nw`*$TZH8lp$TxlfCiI+zxR+Pcfu z^bsrm_VzY{iAj-*s?#FwyAnt6-k#IAQqWj*6bG+pi&z_s5(nii z>D=|vk&!^nqv67*Z;)Rt_vO;jEt+E?cPupMWMMzwa`g|~)2lq>d(UJ%c~?R-Q&rul zI5l|P%aQTMxTraTkawWJ;U;L+RU)5@n2YZj@g_E&gqHvC54JT}$clI6@mgnrFp{cU zD<>z1!P3Aa^^9*I*c>6Kc8jq$X#KS2kuN`fNXM7HzCP1G*efyWD#-{1tqI0nzo~8a z20ofwfm@Mq1&Rk*Hw(DUe_v5;`x;4!( zoWhx;;6;k~C{99CCnLi`M~Qz`k?C<~hKZTk8V?U|VN!HEH(E!6C~dpYw@~uh5zL?Q zoktde5|NIaq$f~)R7dky`~wjg4GAiReDz|NhlPigDienFKeTmp>gJdCF3N_i zYIz5Q`35MF`6Jl+`UfS+`09#+^j{p;1qYe4TwXW4!P)S*Q@&qO`e8LSqH+_4k6VJl zV8%n?DzN+wR}2|B`9hv5=fI0$t{5;|6)i0-m)(Z;jp!dMn-OHUk@IbB`iN*U^Uo+m zPXN6Pjsw9dYtZ32rLJy;5f$eOMNIdgag)26kP_K#3D?uRs?@MTu-%4(UCFJE(m&mFL@8hlNXs10|83`h z$+%XA+3?o2CV#jS>N!0FvW~s8PevAkOZ?#`Cv`AW)rjgcZ0 zUclAg_I+Z)KN0?Gt{T;wE!PUyqC+8~6e)z&5JyKJcRu4yL{rPC-4qnYo&G`KeIj^b z2A7JLi<>dljF`Z_is2`*q#I#~VckCNx|m&W-)Uv|P>Ok>q$A|L2=(OoQeKp3>90A; zWEP#TodtU3_U_Cz-L7gq`=bs`KFaxKNE!77&qX0>@F5!`S#MrleZ4<-j(W^Yy&c~0 z!AD+_+K2l^_coOzh;|dCh>ddDqoJ2uYsDxAmm3ArRH6pb`MQ3T=}{!~CMj_^t_{*D9|{p zG$PtwQPwv-8=F;ypgi)4H?8>K1UMfR^wM=_|BeL%C zIDZ)O*5++&CA&nN;y}&rT9yM_rYaj#!g7Dwy_c_F%dsWFTTK4?C36^y8H&^jBhy#g zD$2@Ur*jZJ^4JMwfhiS_gUIFP=7wNq&g04gz1x}+@|0wP+S96qLXUUC8UwB`NHfUt z$k(neVSndh?x7Smu@8k1F|4lV+k$z156W)z2OhQKDP3MJERX%>g4&n&7Ekt;BfIA@ zI}u3MlS}{SDoN_a9Zrn%r{6p}t!nctD=QU1KXNOsg0(4-n5?WUF-jy04EMrcn#pU0 zLR-&zZjW60VmFjBrgvG~`;ZHSGRTaBUcq;mDI?*r?gYuYxzsyM94^Y9%-MUkm@qzaCpE0M4I1Cb)>rmD9DO1SljWj9 z>gi2}nz%L1iNTQ=IuzdcCcpT9YeuiEtZXx=oOeZ>%%qAoRa5eK*2S^WC{mscq@t4_ zRsIB3lO#x(t8s6lP3OhgB<;l*WafXli4Y#*W~WAU4JAKwkEzN)*AF0L)g_< ziSceRx%i{khLmLLJvkAlQs}51mI94PGLeHE$&-c)o%ptTD`;GOdfq}%l7c?P`aXG- zqkge?L%MybD99t;k?Y#?}o=ij^tCTQb2PbL}yRR|(~+6uNF@g1g>2y|8>z8g0M zGB}izK86-uJAJo=4p-$h{p~Jxm%1FA<`Tgk&LhVZE%ha_dtRNcFF7~D6MBUnJRr;( z(tZAH@FNIf@!{d&BvN@*nako})>H}?-J7bdhUgLDLG$w_q@<*(ygJj{?q6;Z8QFm) zIQG<~t*;NZo3#M>`L+M~(S9_Ecc8CyaogiYEUhBu=q%29k)T3q~dx=?zC(E_PrU(LnN#Kc4%PqeUHzzSb@^F4OTD4ArA zDcc9x47U~5ervpKZ53QCR>xt!5)_mo%iv5@#pS0pGcd4{JRlV`9MKbmc@5B%AJMjv z3Nl5#J(gn*KGF-&`P@z(0P|fFG4L*mk;=W%Y_gcOfG@S5)H%#7MLK*}I*P1;VZ)Wa zv{X2uS1xdi5mn##TsTt{+thUY2kURPe?(Uhwfwnh%iTxzjyiYFuV)sqL7JGj_a7>9 za)?Y%RVFN(ed!Rl33_@UouRASWOz5oaW^ON&W~yRTU(i|?6yOV_z6*=?Pu*bh!-w) zJc6G2vOSU@*^T%!*u|kt#+OOqrA) zd=n&fWQ=&phU2o(+8Jizi365Qjp@9t;Lujgf(J7kM#W#_z+4a|-{BLMsI6l*UX&?D zcq2&qLXJiE3y&5rq3`K{jKkkSA4&3h=R4XuIgw>NgrRWIxYXrYEE6q$e&YxFBx)VT zURFg|Inw{lp3|aMUVr~D3pg2`$AauoU%x5RQQq5Oq zo1eXpc%b$TSBv4?$C2F@00x$ia6BK6X3j}TQnHMT*@&0UBe&vK9b%pA)jIR{E_Mk` zET9uw>M9n!w_dzFn3)_U&r4{86+OV(Sk1`lKIDlGo>#7U#FW6EsoJ^Z7`X10n0?QJ zxZkx74|<<@uU!-l4WlO`$|(`ao6FoJPs0!zu9BL$6u7-Oa&jUhv%W6o4o+QzVB@E(!d!-?s%=-0rR^x9Flz++1#?(^#A?@AHU+ zK^=>OxHI+WNA*FBu4FY*r5AfV3(JdefpLEI+yp~Ny5 zBs_2^tM*pQM(AyEXp+Obb4l>epFg`v3BLHuAecEV$B9|I9Q)u(gzNZwWbi@jep=_5 z?Sm8FcGHXQ!OgE6zeh&CL}nyV#0tG1w~>FRai1o}CZh-D|6)lB+1FEDUHu5>0*_Vx zT`DG3v&V^%8b3&nDol^6+ooZrB=df**PC&ovt{P4i-ET6Y3v-Ct{}!C>`qE zmcP`^3desVUMJuCB1me~|G{pr%+DzXICC;fs7b?Wf_?x{Fjgu0itw< z4Y&cv9__Sk8Fl)^-1oEi&c~!L%qQvuZ`1P2#CEq+3o762;u2qWm&k#ecoD31$F!H) z;M^6Phh>Z7Y|Y#-Y7|c6SE+3+K@SA3=iQjL9&CZs@UHvVKjeMNttfp}3KCk9tHh-g z)MO+5I-aWZ;eBN~7AJ9~}xgpP%q+F=^f-^ z*q8!OyE&}eeHr(JX|zH3UJpZ97BhW6mRdc#ueIC;_u*;`xgXc2Zx)&;ec=;PPNwSD znMwc^vDxZ~+Ce-BYC9otTv5`E6(Zc4!f9T<-M%wdOHY3|V5&IhGMq1U7b%iSip65d zN=vaZa7}`@BtJitX&{vpsT7pc(La|QBi);f*St)~!jO;Oy?b~2RN^iI&_?fPZf-{& z&fL_T?Fc!}6dcqbKraUe4CgTQ8zzr3O%N02JG z!%PA}DrMF%Q+4Ls+NM@KK4AACOD!D@a;19uD$;=RP>I8N0l>UGLT5m)`x+Mf&%as< zH%jE>1|)b0XUYp_`aXoiE}q)iZ8tD5Fr*o+ia!#@y*%6~|6st?X%1s}u!zNAYC&07H27(nHCopH!P9!&4c#VHrIeKnA_pM3vB_n zP`q+DG8(Bv4fEcc`xY6H<+}nyaL}Oyp0FipUr7{F4Y@!1?DLmzDDF8donU9)XVkVq zfGaprGnyvr=0!zA`WgyD*3AUhQGtG zJ(Z7|NkR6tFp>X(70Ws$H_@K>=i^q|);Fe#1D`ag;}qQnYI1QM=YX8V5PkHhq%=A` zod$r#ofesR3j2npJG!xk;y2=#pPQQg{W!CI6mIT&moiE<^WlB$+{Wd*UJnLZ^ zV*zM!%V1Y*A-1PXZ}fBE+n^1{d&IiCk}ElnIxa_`k z8!KP*?CL`Ot}cd?(dRh$`lL4~N!Ij zxbI31?~81owj;VmM&O_`fwPjxNw=2b;^N;YS_bJ(|F(WEf!H{SK(WfgTN#MMr2ZbC z7jCTTeii=J8*jcM$BY~k?T~oXF+JUm!N}m^B<3CYB->cpSNj6?z2f7gg#F9vN8 zyDVSMEi5xH-GFn=<0CD)WCvM`q*De3- zSwaT_#n_t!`r0}=I;s{c*VC552u6g!+@)cu$`Tq`!edH6v#glRi@J2Motnr zUp8C;VE9;PHH1+`q24`SpKf1VT-0afgaRu!J#LbcUMQxo#&-`69lM*kx{BV8QzWOd zz=Z^RD1mJEjzP@6{FR*@wWWa~_+8V_jpvi8>k1;8KWv`$uJQC9 zToB|9-^dzDPl%~2DV6Ao+}PgkK0R3XE8w!nb7lwYSIlARBaTYYlqL{{o95!RdBRfe zJ|ipCEb-)}+02YV^i1ifY&>bgVsj1aUg;bKJSlrA>|CN$wSo0#K-qp9In?KMUu_S< zqup|Ea_+h*COWh2{((SPvHsJix22`T#n1BWapx)3;{F+AzYaJ54ITxSEAI5GTZUy9 zhT=6M-2{$79pg|viOPzH>jN!$2ST73k#-w z(*E;Oc+<_UjA~LP?yx!g8?5hfJYy)XeyXbXWVl_N?Bx;S2Hha0$-o1T@uc)ugimd%wae4|yw^51OWW?}^GLL3?h~mFVge|B*(%4( zN|+Ai=(xM28JR(*xL^ih@1f2oyTBw}qff@alfb*XCnzYlo%q=|8J50bF)=_Kn=!Trn_n`QMWS zZ^u?=*kD>}=khvXx$D+wpmFd;7nB>qgdj76iK_c*O{YK_dDGj|lP>z#z1l1`zo9`) zLR9pyz#Jha(nB0dl1`|gF)>gxD?D@QyDo%M(&_u}A=b z6@fqmwuRcg=w{1Ne^*pQ7+t{9CJhq)q-nrqou{PKx_pIdqR|4iJpVH;_Ed+?Q+7J^{;1jCuhb=CcqMO5FbpkZ8)y%h$FK~FZs0vw* zAIAHrjei>Muepc`3JRJG?pQpBw6#>cWZ@Yc%FSHSH5|(BvT!B!2}4ZL9UYyaml&%J zth%$PsZ}iBxZ5RR@%X7fw7|dX&xGt#huR8K{j=bjT?oUE_-3&ie0+ReQ2Ma0A{?J# zKQ@uWMqLpg99*H})gU+<&WEG(OKBT$Ks5vLnW}{9#kUH$IyaBz*Y-SHdz`XK+V@mB z5AHTvpQ>_dF(>pgK%$x|hT`#t9f+q{MP8XgG?3%Xc9uXmDwYDNtDJo3@@C*D3h<+a zHylJjODNGMPSph3Te)eQzlW*m0=Bial{!`Pl|lLq8I8*9)3>Fsg5ua6WT_M$LF5h% zGn-zydG`swh^l=koi-#YfB>y5*u-jSq`CqSLrEOw2-sGqs2X5WMlIgdn-dkACr4EQ zEwRg=Q%g%5peb^w(e?tTm@Jxy+G>;8lvl(nE$VB{5e}iqd;??oM$3J-RJf*~d8G{oY^e6BsSJat z5`QuiPwwrB;qb>q`BUIxIY_JHoqL>E+i~;-D$zS6TE)vumgTz z&(oI;J;HG&!(F$RIz~q`J~f5dX}k8nI4qXl{`>cD-W@sx1N{3dvxt(ty}iO4aeNQ+V(nbH`674g>jFvqV5jPRu6?I z!I7%tR9?9b1cCo12z@VKn?fkkyvxffbIIgDa|EkpLs=$cua0Pr2#Oc$zCEo4y?9_h z8~OANzxh5sX5A~(nNbWa4>-T=d@~)W5Ydz2Oo$U=X(3OS)5c(U8IW-xi{^#kAi zdGhx4EQ@kjXkzj|KU!oZnIoeZ#hU>V$$E#GP&-30^{sCOsaVL94#ipc>DT(+;qkGw z^Y4-Tr7U}_O}OVTarHHqTjfN6VRQ*Pg3SAF&$IbU##y5*_YoXP3W3^q4%E&Y9?`ac zGcq`^z}KQDk~r!PU^bFSc~ovVensVq6)yqmmo zX&=VF?oTsKEYyYVd0upa9GGA`hdsg@?-C|rec*o<3CG77mOL2D1i4h8}Fep5{q z$y7~V_u|h{Kfdkh<(%xG1d3)~Yj}e+`zeq#pO3gU%Q-?{Zn#RkFMS0{W>&-WnEF=ra~>G0=z5n| z7r@qWy~|DG8;27%=xO21!I*IK3ntc;KIE|fKmsg&IX9|z8QLYsR$kjW(Gfz@yN5zI2@lWE_)zt|!&S@oQ+`Cv&VAavl(9yXA;?I@Y@y;BP@gxcq{36cJ z&WP{w@_sMV``iC4yUocjhLT#YPkg1fYazEa5Z}J1u1odO)5>QkeEh;K94rbwM za^J3ri;v$gEk*XoP=-eO*D7>h(QX@&c$r=F45&>GWm=AOI zv;cDJx=w!u2%R8?Qx-llI_e}xWT$)9$K3n)c(#_it=0MJ!mAnRFhJ7}H)mla%LDsu zaz6%Ht!~0hXS9?MzwoT)M$B8dcVubwyUG!Ir|1+ zSzy1z)=X9ZjqfE|4zDnT12wKvARX-g9(`8~mWh~F$@fKz2UqTqMYH%dE4?&h zRzgagDU&ePhC7r|*LW=bK>tv9ME@C@CmVhLEiLmioaT=%H$WAvPyi|KFQNY0x7h-A z`TY_irkwS-!@Q^Vg9?xJeO2T_KRslOfAcUt#fFBl*GaP1i6u0nk4L8;u?(uvs^gaF zKnpVuS z<7U@~6OD{obMA+KORK6JoSWuuF$w(VR`Hf1$?SGwT=nv2du*hh{r&jxaFC8jh(SF5 z^^0nY?xU0{H7RPTg$dW{w6B(qtNm&H5%k}J3N1rbxpQjjWw^sQlut{pj4W!5_Ou;~ zzxig35wWs_YVUmd$@bAw0@xwKQrj{sjZO_FTpBZ`7St6T8P~bQ(%o=4Z4F}kDonL~ z)hzgV>DXbcl>}S-d}VS5ndwUQ#(qxE+H&UClJCpg{A9;njVY>8)U=nsa2F01D&jSh zPyLa9sybB2R$PA1U**zBU-LFal)Q>mKW~~!hv-lZ+d~A(mxGe~?9(lltPu}SPefmM z7(ZQlEJZrHkunNjMR_qpdTBs($e9lzL`5unFSXo{W~Ek2Ks2^ko3}6?J~KVN|Fs|* zclbcR*6Jp{EZ4o26is@=R$nh=y3{+$NI-G93aNh%>hq?LO>$g&j8e!vKz^LH9_Y~I z*{m@wHxi**H8gB%iG7>OztY{mJyK)*`{{O&aX--<(xiO$G*Dy5?5yH81&LQR!DsQo zYx6Inyu01$MxvMG?8NVk6jS*yA3r_CfBLGGzcF54X?9Dq!f?IX(r7p#5~RVtlamuO z`$yD9{7q&3%gJABUl6gKTre*ZBB&H^$x}pR-BEkNd+WSl12S!td!r9kMGr zId&%T?`&U+TZVRy3PSqnX}VP1K(3qFo#DlZL%^~GYU|IU3#M3=HJ_mT;;z3WS?4_Q z-}^Ma>@@K-`4iBa1!F@}5YH=XauGmqUJ&f@%o>_7Pg(7`dwn{*U3InfD1IWHuz`1A zyN0)+taQ4<=7qSPzcO95Wn5;_#vtEV?X1*&$Y3e&>xbuePeVAXR?xz9zp*Cic=1At zJ-xjLnf84vpG!MlcO{}J!uQgpKvR~nnT@HGB+|0)@xeMF?Q(Q?bL&`g)K5pF_kUUk zcfcRmoeM%NNrRkYW1@UFWomgz=H}*DLaC%yyqSJuuYkSC0vMk3)4Ns22L*e~^huBX z)`n|bGdZE;;f41&f*?4XVb+qZ$W4Stf#C}^G&FP?$DZ)y-KYo%Wb{!zKVNbMFC_`l z#P!@tQhGll7+Zyh;0%3x$47gw!+BbibUAq8Z^BCOE!BvwJ@V30lM%Pa$tqoF`{!-2eho!Z$}rg7z`_yWIQ;my{6_FZD|JF(o#htRuf1cY|p!YThDiXK)|(?U?vy| zuw{OJeg|Ir!QuF`03HM>rhsd$fnYo=+?OV%(DiKf-8Vj2cqq6m6v?_ZRgIT-by&Q& z7()J^lj`W`2o#*LXafld&=0QuSXJxT4S=Q+_Rz^327q?Z3YdJkE44>7>#^WB)!@Jk z9A=Boji7`c(NM4)@MvxkTj{ZS^$A}VPTB>EvZmx!lQDI#Cy~hwkdIJbmi-#NIBceCEy1M!-g*}(TXayxChzDwXlR4%r z5QN$~kDC+!>F8WGcuH38OSUBoO)$amKL%X;gOV;Vw}A?C$PMy>vN96JUb|$pmmg1> z1sMJ9NA6qVV<7+8a;yGSaCqC&;Z{p$<>27q=IXjQk}oIjyaKat zInvF3=wkJ&odk*=UiW#z0{^YfnL-k+3k+3ZW`ZZtT}%2}#>U$~BjW=OEdTwKJTqX! zIaw75005!dyo(U15mmr-#+l;zJ3m4K#(KM|<==E4ko6g$K}i*`SiV!p!uGsc=A(|* zT6~TzTwSSv|IOEz(Y}GtcY3v^X*%EAN|`F1`aODiCpi6b_7X?;}t+^)JyXx8PO$(Rybs+pKz`qeBfGOW|r+50<8?QTlbSsDZsF{@b^2 zQP%%S0f3wSaM3oU;4OW*f9hN2C$*5;OaqJ~_=QyL>_1(}gIN~C8SFEz7;PV9{XX(u zAX+>M8)F@Cw${>0F@=XlQ56NVsBSn$bOqgiJu$fZx^d3FucrBTEEq%9Xp4ix6x)N_ z)lSPj!NUNHQcDhZS=9zDf5xbcK|A-!-fifrx@*dgQj?UeVkzn)8 zZ5T=~sw^sUWL|dxP%zXeeSiQUG_0@EknOtVVA^rIY4E`;O&yN$WSM|zD=8@0J$drpi&#BZ(jX~x zBQ!A=IAaSyIe*C!0c@+ET{kp4cAw?00~2DlI*@;9=@}TD0M=KKwlc4#>kw5F0So4I zV`F?x&fcP7aq9vCG`Q;M__%AswR8QTWBp(WyIGlaK-Yr+@-$HSZk1@i8r9pRaT8%@ z{c>+$pR;YDt!v4V*Q069ziDo#I5j#Qt-+FI9|X6^ETa1%%7;-YEGW_DDc$sT;GjG; z;h@Ys2(4n= num_files_cpp else "cpp" + +# Tell sphinx what the pygments highlight language should be. +highlight_language = primary_domain + +if num_files_cpp > 0: + extensions += [ + "breathe", + "exhale", + ] + + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} + breathe_default_project = project + + def specifications_for_kind(kind): + # Show all members for classes and structs + if kind == "class" or kind == "struct": + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] + # An empty list signals to Exhale to use the defaults + else: + return [] + + exhale_args = { + # These arguments are required + "containmentFolder": "cppapi", + "rootFileName": "library_root.rst", + "rootFileTitle": "C++ Library API", + "doxygenStripFromPath": "..", + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), + # Suggested optional arguments + "createTreeView": True, + "exhaleExecutesDoxygen": True, + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), + } + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "sphinx_rtd_theme" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") + + +# -- Options for intersphinx extension --------------------------------------- + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {"https://docs.python.org/": None} + +# -- Options for todo extension ---------------------------------------------- + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = True + +# -- RST Standard variables --------------------------------------------------- +rst_prolog = f".. |project| replace:: {project}\n" +rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/src/bitbots_motion/wolfgang_ik/docs/index.rst b/src/bitbots_motion/wolfgang_ik/docs/index.rst new file mode 100644 index 000000000..e76aa433a --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/docs/index.rst @@ -0,0 +1,21 @@ +Welcome to |project|'s documentation! +================================================ + +Description +----------- + +|description| + +.. toctree:: + :maxdepth: 2 + + cppapi/library_root + pyapi/modules + + +Indices and tables +================== + +* :ref:`genindex` +* |modindex| +* :ref:`search` diff --git a/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h b/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h new file mode 100644 index 000000000..b49bc8515 --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h @@ -0,0 +1,371 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// ── Model constants ──────────────────────────────────────────────────────── +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] + + + +// Motor offsets (left leg) +constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; +constexpr double KNEE_OFFSET = 0.26180265358979327; + + + +using Mat4 = Eigen::Matrix4d; +using Mat3 = Eigen::Matrix3d; +using Vec3 = Eigen::Vector3d; + +constexpr double BASE_LINK_SIDE_OFFSET = 0.055; +constexpr double BASE_LINK_Z_OFFSET = -0.0414; +constexpr double SOLE_X_OFFSET = 0.0216683; +constexpr double SOLE_Y_OFFSET = 0.0152219; +constexpr double SOLE_Z_OFFSET = -0.0529; + +// ── Custom exception ─────────────────────────────────────────────────────── +class SolverError : public std::runtime_error { +public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} +}; + +// ── Elementary rotation matrices ─────────────────────────────────────────── +static Mat3 Rx(double t) { + Mat3 R = Mat3::Identity(); + R(1,1) = std::cos(t); R(1,2) = -std::sin(t); + R(2,1) = std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Ry(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,2) = std::sin(t); + R(2,0) = -std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Rz(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,1) = -std::sin(t); + R(1,0) = std::sin(t); R(1,1) = std::cos(t); + return R; +} + +// ── Euler composition helpers ────────────────────────────────────────────── + +/** + * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) + */ +static Mat3 euler2mat_sxyz(double a, double b, double c) { + return Rz(c) * Ry(b) * Rx(a); +} + +/** + * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) + * b = arcsin(-M[2,0]) + * a = arctan2( M[2,1], M[2,2]) + * c = arctan2( M[1,0], M[0,0]) + */ +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); + double a = std::atan2( M(2,1), M(2,2)); + double c = std::atan2( M(1,0), M(0,0)); + return {a, b, c}; +} + +/** + * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) + */ +static Mat3 euler2mat_rzxy(double a, double b, double c) { + return Rz(a) * Rx(b) * Ry(c); +} + +/** + * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) + * b = arcsin( M[2,1]) + * a = arctan2(-M[0,1], M[1,1]) + * c = arctan2(-M[2,0], M[2,2]) + */ +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); + double a = std::atan2(-M(0,1), M(1,1)); + double c = std::atan2(-M(2,0), M(2,2)); + return {a, b, c}; +} + +/** + * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) + */ +static Mat3 euler2mat_rxyz(double a, double b, double c) { + return Rx(a) * Ry(b) * Rz(c); +} + +/** + * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) + * b = arcsin( M[0,2]) + * a = arctan2(-M[1,2], M[2,2]) + * c = arctan2(-M[0,1], M[0,0]) + */ +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); + double a = std::atan2(-M(1,2), M(2,2)); + double c = std::atan2(-M(0,1), M(0,0)); + return {a, b, c}; +} + +// ── axangle2mat ──────────────────────────────────────────────────────────── +/** + * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). + */ +static Mat3 axangle2mat(Vec3 axis, double angle) { + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + K << 0, -axis(2), axis(1), + axis(2), 0, -axis(0), + -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; +} + +// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── +/** + * Returns a 4×4 transform T whose rotation aligns a reference frame to + * the given leg_vector direction. + * + * Mirrors the Python: + * roll = arctan2(v[1], v[2]) + * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) + * yaw = 0 + * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) + * = Ry(pitch)*Rx(roll) + */ +static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { + Vec3 v = leg_vector_in.normalized(); + + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError( + "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } + + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); + double yaw = 0.0; + + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); + return T; +} + +// ── Joint angle map type ─────────────────────────────────────────────────── +using JointAngles = std::map; + +// ── Forward kinematics ───────────────────────────────────────────────────── +/** + * Mirrors calculate_fk() exactly. + * Joint order (all around standard axes): + * HipYaw → Rz + * HipRoll → Rx + * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) + * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) + * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) + * AnkleRoll→ Rx + */ +static Mat4 calculate_fk(const JointAngles& q) { + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + Mat4 T = Mat4::Identity(); + + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); + + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); + + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); + + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; +} + +// ── Inverse kinematics ───────────────────────────────────────────────────── +static JointAngles calculate_ik(const Mat4& target_transform, bool left) { + JointAngles joint_angles = { + {"HipYaw", 0.0}, + {"HipRoll", 0.0}, + {"HipPitch", 0.0}, + {"Knee", 0.0}, + {"AnklePitch",0.0}, + {"AnkleRoll", 0.0} + }; + + std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + double virtual_leg_length = leg_vec.norm(); + std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3,3>(0,0) = + euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() + * T_hip_axis_intersection + * T_hip_pitch_origin; + std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); + std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = (real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + + real_leg_length*real_leg_length + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH + + real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) + / (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; +} diff --git a/src/bitbots_motion/wolfgang_ik/package.xml b/src/bitbots_motion/wolfgang_ik/package.xml new file mode 100644 index 000000000..8a19b7a99 --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/package.xml @@ -0,0 +1,18 @@ + + + + wolfgang_ik + 0.0.0 + TODO: Package description + florian + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/bitbots_motion/wolfgang_ik/src/main.cpp b/src/bitbots_motion/wolfgang_ik/src/main.cpp new file mode 100644 index 000000000..91e0b3095 --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/src/main.cpp @@ -0,0 +1,150 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#include + +// ── Main ─────────────────────────────────────────────────────────────────── +int main() { + const double PI = M_PI; + + // ── Test foot subproblem ─────────────────────────────────────────────── + { + Vec3 leg_vector(1.0, 1.0, 0.0); + Mat4 T = foot_to_leg_from_vec_subproblem(leg_vector); + auto euler = mat2euler_sxyz(T.block<3,3>(0,0)); + std::cout << "Ankle Joint Angles from Foot Vector:\n"; + std::cout << "Roll: " << euler[0] * 180.0/PI << "\n"; + std::cout << "Pitch: " << euler[1] * 180.0/PI << "\n"; + std::cout << "Yaw: " << euler[2] * 180.0/PI << "\n\n"; + } + + // ── Zero configuration test ──────────────────────────────────────────── + Eigen::VectorXd q(6); + q << 0.0, 0.0, 0.0, -0.5, 0.0, 0.0; + q *= PI; + + JointAngles joint_angles = { + {"HipYaw", q(0)}, + {"HipRoll", q(1)}, + {"HipPitch", q(2)}, + {"Knee", q(3)}, + {"AnklePitch",q(4)}, + {"AnkleRoll", q(5)} + }; + + std::cout << "Joint Angles:\n"; + for (auto& [k,v] : joint_angles) + std::cout << " " << k << ": " << v << "\n"; + + // Test goal leg length (knee-to-foot in leg frame) + { + Mat4 T_knee = Mat4::Identity(); + T_knee.block<3,3>(0,0) = axangle2mat({0,1,0}, q(3)); + T_knee.block<3,1>(0,3) = Vec3(0, 0, -UPPER_LEG_LENGTH); + Mat4 T_lower = Mat4::Identity(); + T_lower.block<3,1>(0,3) = Vec3(0, 0, -LOWER_LEG_LENGTH); + Mat4 T_combined = T_knee * T_lower; + std::cout << "\nGoal leg length: " << T_combined.block<3,1>(0,3).norm() << "\n\n"; + } + + // FK + Mat4 T_fk = calculate_fk(joint_angles); + std::cout << "Forward Kinematics:\n" << T_fk << "\n"; + auto fk_euler = mat2euler_sxyz(T_fk.block<3,3>(0,0)); + std::cout << "Foot orientation (sxyz): " + << fk_euler[0] << ", " << fk_euler[1] << ", " << fk_euler[2] << "\n"; + std::cout << "Goal distance: " << T_fk.block<3,1>(0,3).norm() << "\n\n"; + + // IK + JointAngles q_ik = calculate_ik(T_fk, true); + std::cout << "\nInverse Kinematics Joint Angles:\n"; + for (auto& [k,v] : q_ik) + std::cout << " " << k << ": " << v << "\n"; + + // Verify FK from IK result + Mat4 T_fk_ik = calculate_fk(q_ik); + std::cout << "\nForward Kinematics from IK:\n" << T_fk_ik << "\n"; + double pos_err = (T_fk.block<3,1>(0,3) - T_fk_ik.block<3,1>(0,3)).norm(); + double rot_err = (T_fk.block<3,3>(0,0) - T_fk_ik.block<3,3>(0,0)).norm(); + std::cout << "Position error: " << pos_err << "\n"; + std::cout << "Orientation error: " << rot_err << "\n\n"; + + // ── Random test (1000 iterations) ───────────────────────────────────── + std::cout << "Running 1000 random IK tests...\n"; + std::srand(42); + std::vector errors; + int skipped = 0; + for (int iter = 0; iter < 1000; ++iter) { + Eigen::VectorXd q_rand(6); + for (int i = 0; i < 6; ++i) + q_rand(i) = (static_cast(std::rand()) / RAND_MAX * 2.0 - 1.0) * 0.4 * PI; + + JointAngles ja = { + {"HipYaw", q_rand(0)}, + {"HipRoll", q_rand(1)}, + {"HipPitch", q_rand(2)}, + {"Knee", q_rand(3)}, + {"AnklePitch",q_rand(4)}, + {"AnkleRoll", q_rand(5)} + }; + + Mat4 T_rand = calculate_fk(ja); + + JointAngles q_ik_rand; + try { + q_ik_rand = calculate_ik(T_rand, true); + } catch (const SolverError& e) { + ++skipped; + continue; + } + + Mat4 T_ik_rand = calculate_fk(q_ik_rand); + double err = (T_rand.block<3,1>(0,3) - T_ik_rand.block<3,1>(0,3)).norm() + + (T_rand.block<3,3>(0,0) - T_ik_rand.block<3,3>(0,0)).norm(); + + if (std::isnan(err)) { + std::cout << "NaN error at iteration " << iter << ", skipping.\n"; + ++skipped; + continue; + } + if (err > 0.001) { + std::cout << "High error " << err << " at iteration " << iter + << " for q = " << q_rand.transpose() << "\n"; + break; + } + errors.push_back(err); + } + + if (!errors.empty()) { + double sum = 0.0, max_err = 0.0; + for (double e : errors) { sum += e; max_err = std::max(max_err, e); } + std::cout << "Mean error: " << sum / errors.size() << "\n"; + std::cout << "Max error: " << max_err << "\n"; + } + std::cout << "Skipped: " << skipped << " / 1000\n"; + + return 0; +} diff --git a/src/bitbots_motion/wolfgang_ik/src/ros.cpp b/src/bitbots_motion/wolfgang_ik/src/ros.cpp new file mode 100644 index 000000000..6c059f3b9 --- /dev/null +++ b/src/bitbots_motion/wolfgang_ik/src/ros.cpp @@ -0,0 +1,203 @@ +/** + * leg_ik_node.cpp + * + * ROS2 node that: + * 1. Looks up the current l_sole → base_link TF transform. + * 2. Runs the analytic IK solver on that target. + * 3. Subscribes to /joint_states and prints IK solution vs. actual joint + * angles side-by-side in a loop. + * + * Build (add to CMakeLists.txt): + * find_package(Eigen3 REQUIRED) + * find_package(rclcpp REQUIRED) + * find_package(tf2_ros REQUIRED) + * find_package(tf2_eigen REQUIRED) + * find_package(sensor_msgs REQUIRED) + * + * add_executable(leg_ik_node leg_ik_node.cpp) + * ament_target_dependencies(leg_ik_node rclcpp tf2_ros tf2_eigen sensor_msgs) + * target_include_directories(leg_ik_node PRIVATE ${EIGEN3_INCLUDE_DIRS}) + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +using JointAngles = std::map; + + +// ═══════════════════════════════════════════════════════════════════════════ +// ROS2 node +// ═══════════════════════════════════════════════════════════════════════════ + +// Joint names in the order we want to print them. +// Adjust these to match your robot's URDF joint names. +static const std::vector JOINT_NAMES = { + "LHipYaw", "LHipRoll", "LHipPitch", + "LKnee", + "LAnklePitch", "LAnkleRoll" +}; + +class LegIKNode : public rclcpp::Node { +public: + LegIKNode() : Node("leg_ik_node") { + // TF + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Joint state subscriber + js_sub_ = create_subscription( + "/joint_states", rclcpp::SystemDefaultsQoS(), + [this](sensor_msgs::msg::JointState::SharedPtr msg) { + last_js_ = msg; + }); + + // Timer — runs the main loop at ~10 Hz + timer_ = create_wall_timer(100ms, [this]() { timerCb(); }); + + RCLCPP_INFO(get_logger(), "leg_ik_node started. Waiting for TF and joint states..."); + } + +private: + void timerCb() { + // ── 1. Look up l_sole → base_link ────────────────────────────────── + geometry_msgs::msg::TransformStamped ts; + try { + ts = tf_buffer_->lookupTransform( + "base_link", // target frame + "l_sole", // source frame + tf2::TimePointZero); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "TF lookup failed: %s", ex.what()); + return; + } + + // { + // geometry_msgs::msg::TransformStamped ts1; + // try { + // ts1 = tf_buffer_->lookupTransform( + // "l_hip_1", // target frame + // "l_foot", // source frame + // tf2::TimePointZero); + // } catch (const tf2::TransformException& ex) { + // RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + // "TF lookup failed: %s", ex.what()); + // return; + // } + + + // std::cout << "TF lookup successful: base_link → l_foot" << ts1.transform.translation.z << "\n"; + // } + + + // Convert to Eigen 4×4 + Eigen::Isometry3d iso = tf2::transformToEigen(ts); + Mat4 T_target = iso.matrix(); + + // ── 2. Run IK ─────────────────────────────────────────────────────── + JointAngles ik_solution; + try { + ik_solution = calculate_ik(T_target, true); + + // Apply joint offsets to match actual robot configuration (if needed) + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + + } catch (const SolverError& e) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "IK solver error: %s", e.what()); + return; + } + + // ── 3. Extract actual joint angles from /joint_states ─────────────── + JointAngles actual; + if (last_js_) { + for (const auto& name : JOINT_NAMES) { + auto it = std::find(last_js_->name.begin(), last_js_->name.end(), name); + if (it != last_js_->name.end()) { + size_t idx = std::distance(last_js_->name.begin(), it); + if (idx < last_js_->position.size()) + actual[name] = last_js_->position[idx]; + } + } + } + + // ── 4. Print comparison ───────────────────────────────────────────── + const int W = 12; + const double RAD2DEG = 180.0 / M_PI; + + std::cout << "\n══════════════════════════════════════════════════════\n"; + std::cout << " TF base_link → l_sole\n"; + std::cout << " pos [m]: x=" << std::fixed << std::setprecision(4) + << ts.transform.translation.x << " y=" << ts.transform.translation.y + << " z=" << ts.transform.translation.z << "\n"; + std::cout << " quat: x=" << ts.transform.rotation.x + << " y=" << ts.transform.rotation.y + << " z=" << ts.transform.rotation.z + << " w=" << ts.transform.rotation.w << "\n"; + std::cout << "──────────────────────────────────────────────────────\n"; + std::cout << std::setw(16) << "Joint" + << std::setw(W) << "IK [deg]" + << std::setw(W) << "IK [rad]" + << std::setw(W) << "Act [deg]" + << std::setw(W) << "Act [rad]" + << std::setw(W) << "Δ [deg]" << "\n"; + std::cout << "──────────────────────────────────────────────────────\n"; + + for (const auto& name : JOINT_NAMES) { + + //Remove first character (L) for printing + + double ik_rad = ik_solution.count(name.substr(1)) ? ik_solution.at(name.substr(1)) : -100.0; + double act_rad = actual.count(name) ? actual.at(name) : -100.0; + double delta = (ik_rad - act_rad) * RAD2DEG; + + std::cout << std::setw(16) << name + << std::setw(W) << std::fixed << std::setprecision(3) << ik_rad * RAD2DEG + << std::setw(W) << std::setprecision(4) << ik_rad + << std::setw(W) << std::setprecision(3) << act_rad * RAD2DEG + << std::setw(W) << std::setprecision(4) << act_rad + << std::setw(W) << std::setprecision(3) << delta << "\n"; + } + std::cout << "══════════════════════════════════════════════════════\n"; + } + + // TF + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Joint states + rclcpp::Subscription::SharedPtr js_sub_; + sensor_msgs::msg::JointState::SharedPtr last_js_; + + // Loop timer + rclcpp::TimerBase::SharedPtr timer_; +}; + +// ─── main ────────────────────────────────────────────────────────────────── +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 4b17884f4960db5904adddea53409381ca14d74f Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 13 Mar 2026 17:08:05 +0100 Subject: [PATCH 02/16] Fix r ankle roll angle Signed-off-by: Florian Vahl --- src/bitbots_robot/wolfgang_description/urdf/robot.urdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/bitbots_robot/wolfgang_description/urdf/robot.urdf b/src/bitbots_robot/wolfgang_description/urdf/robot.urdf index b964ae9a6..49fdd84a5 100644 --- a/src/bitbots_robot/wolfgang_description/urdf/robot.urdf +++ b/src/bitbots_robot/wolfgang_description/urdf/robot.urdf @@ -2200,7 +2200,7 @@ - + From e91b16fa617c27837e18f155ed329ac21f717cdb Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 13 Mar 2026 18:11:49 +0100 Subject: [PATCH 03/16] First working analytical walk Signed-off-by: Florian Vahl --- .../bitbots_quintic_walk/CMakeLists.txt | 2 +- .../include/bitbots_quintic_walk/ik.hpp | 396 ++++++++++++++++++ .../include/bitbots_quintic_walk/walk_ik.hpp | 20 +- .../bitbots_quintic_walk/src/walk_ik.cpp | 111 +++-- .../bitbots_quintic_walk/src/walk_node.cpp | 10 +- src/bitbots_motion/wolfgang_ik/src/ros.cpp | 33 +- 6 files changed, 500 insertions(+), 72 deletions(-) create mode 100644 src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp diff --git a/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt b/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt index 26c03b7a1..9b234e980 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt +++ b/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt @@ -58,7 +58,7 @@ rosidl_generate_interfaces( include_directories(include ${PYTHON_INCLUDE_DIRS}) -add_compile_options(-Wall -Werror -Wno-unused -Wextra -Wpedantic) +add_compile_options(-Wall -Werror -Wno-unused -Wextra -Wpedantic -Wfatal-errors) set(SOURCES src/walk_visualizer.cpp src/walk_engine.cpp src/walk_stabilizer.cpp src/walk_ik.cpp src/walk_node.cpp) diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp new file mode 100644 index 000000000..f124dc979 --- /dev/null +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp @@ -0,0 +1,396 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// ── Model constants ──────────────────────────────────────────────────────── +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] + + + +// Motor offsets (left leg) +constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; +constexpr double KNEE_OFFSET = 0.26180265358979327; + + + +using Mat4 = Eigen::Matrix4d; +using Mat3 = Eigen::Matrix3d; +using Vec3 = Eigen::Vector3d; + +constexpr double BASE_LINK_SIDE_OFFSET = 0.055; +constexpr double BASE_LINK_Z_OFFSET = -0.0414; +constexpr double SOLE_X_OFFSET = 0.0216683; +constexpr double SOLE_Y_OFFSET = 0.0152219; +constexpr double SOLE_Z_OFFSET = -0.0529; + +// ── Custom exception ─────────────────────────────────────────────────────── +class SolverError : public std::runtime_error { +public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} +}; + +// ── Elementary rotation matrices ─────────────────────────────────────────── +static Mat3 Rx(double t) { + Mat3 R = Mat3::Identity(); + R(1,1) = std::cos(t); R(1,2) = -std::sin(t); + R(2,1) = std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Ry(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,2) = std::sin(t); + R(2,0) = -std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Rz(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,1) = -std::sin(t); + R(1,0) = std::sin(t); R(1,1) = std::cos(t); + return R; +} + +// ── Euler composition helpers ────────────────────────────────────────────── + +/** + * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) + */ +static Mat3 euler2mat_sxyz(double a, double b, double c) { + return Rz(c) * Ry(b) * Rx(a); +} + +/** + * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) + * b = arcsin(-M[2,0]) + * a = arctan2( M[2,1], M[2,2]) + * c = arctan2( M[1,0], M[0,0]) + */ +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); + double a = std::atan2( M(2,1), M(2,2)); + double c = std::atan2( M(1,0), M(0,0)); + return {a, b, c}; +} + +/** + * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) + */ +static Mat3 euler2mat_rzxy(double a, double b, double c) { + return Rz(a) * Rx(b) * Ry(c); +} + +/** + * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) + * b = arcsin( M[2,1]) + * a = arctan2(-M[0,1], M[1,1]) + * c = arctan2(-M[2,0], M[2,2]) + */ +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); + double a = std::atan2(-M(0,1), M(1,1)); + double c = std::atan2(-M(2,0), M(2,2)); + return {a, b, c}; +} + +/** + * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) + */ +static Mat3 euler2mat_rxyz(double a, double b, double c) { + return Rx(a) * Ry(b) * Rz(c); +} + +/** + * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) + * b = arcsin( M[0,2]) + * a = arctan2(-M[1,2], M[2,2]) + * c = arctan2(-M[0,1], M[0,0]) + */ +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); + double a = std::atan2(-M(1,2), M(2,2)); + double c = std::atan2(-M(0,1), M(0,0)); + return {a, b, c}; +} + +// ── axangle2mat ──────────────────────────────────────────────────────────── +/** + * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). + */ +static Mat3 axangle2mat(Vec3 axis, double angle) { + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + K << 0, -axis(2), axis(1), + axis(2), 0, -axis(0), + -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; +} + +// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── +/** + * Returns a 4×4 transform T whose rotation aligns a reference frame to + * the given leg_vector direction. + * + * Mirrors the Python: + * roll = arctan2(v[1], v[2]) + * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) + * yaw = 0 + * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) + * = Ry(pitch)*Rx(roll) + */ +static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { + Vec3 v = leg_vector_in.normalized(); + + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError( + "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } + + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); + double yaw = 0.0; + + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); + return T; +} + +// ── Joint angle map type ─────────────────────────────────────────────────── +using JointAngles = std::map; + +// ── Forward kinematics ───────────────────────────────────────────────────── +/** + * Mirrors calculate_fk() exactly. + * Joint order (all around standard axes): + * HipYaw → Rz + * HipRoll → Rx + * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) + * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) + * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) + * AnkleRoll→ Rx + */ +static Mat4 calculate_fk(const JointAngles& q) { + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + Mat4 T = Mat4::Identity(); + + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); + + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); + + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); + + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; +} + +// ── Inverse kinematics ───────────────────────────────────────────────────── +static JointAngles calculate_ik(const Mat4& target_transform, bool left) { + JointAngles joint_angles = { + {"HipYaw", 0.0}, + {"HipRoll", 0.0}, + {"HipPitch", 0.0}, + {"Knee", 0.0}, + {"AnklePitch",0.0}, + {"AnkleRoll", 0.0} + }; + + std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + double virtual_leg_length = leg_vec.norm(); + std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3,3>(0,0) = + euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() + * T_hip_axis_intersection + * T_hip_pitch_origin; + std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); + std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = (real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + + real_leg_length*real_leg_length + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH + + real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) + / (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; +} + +static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, false); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + + return ik_solution; +} + +static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, true); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + + return ik_solution; +} diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp index 20da32c54..c6c2a73ac 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp @@ -2,9 +2,12 @@ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #include #include +#include #include -#include #include +#include +#include + namespace bitbots_quintic_walk { class WalkIK : public bitbots_splines::AbstractIK { @@ -16,17 +19,18 @@ class WalkIK : public bitbots_splines::AbstractIK { void reset() override; void setConfig(walking::Params::Node::Ik config); - const std::vector& getLeftLegJointNames(); - const std::vector& getRightLegJointNames(); + const std::vector getLeftLegJointNames(); + const std::vector getRightLegJointNames(); - moveit::core::RobotStatePtr get_goal_state(); + const geometry_msgs::msg::Pose get_right_goal(); + const geometry_msgs::msg::Pose get_left_goal(); private: rclcpp::Node::SharedPtr node_; - moveit::core::RobotStatePtr goal_state_; - const moveit::core::JointModelGroup* legs_joints_group_; - const moveit::core::JointModelGroup* left_leg_joints_group_; - const moveit::core::JointModelGroup* right_leg_joints_group_; + + geometry_msgs::msg::Pose right_foot_goal_; + geometry_msgs::msg::Pose left_foot_goal_; + walking::Params::Node::Ik config_; }; } // namespace bitbots_quintic_walk diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp index d4e8fa1d3..2e895a0e5 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp @@ -5,13 +5,6 @@ namespace bitbots_quintic_walk { WalkIK::WalkIK(rclcpp::Node::SharedPtr node, walking::Params::Node::Ik config) : node_(node), config_(config) {} void WalkIK::init(moveit::core::RobotModelPtr kinematic_model) { - legs_joints_group_ = kinematic_model->getJointModelGroup("Legs"); - left_leg_joints_group_ = kinematic_model->getJointModelGroup("LeftLeg"); - right_leg_joints_group_ = kinematic_model->getJointModelGroup("RightLeg"); - - goal_state_.reset(new moveit::core::RobotState(kinematic_model)); - goal_state_->setToDefaultValues(); - if (config_.reset) { reset(); } @@ -23,65 +16,95 @@ bitbots_splines::JointGoals WalkIK::calculate(const WalkResponse& ik_goals) { tf2::Transform trunk_to_flying_foot_goal = trunk_to_support_foot_goal * ik_goals.support_foot_to_flying_foot; // make pose msg for calling IK - geometry_msgs::msg::Pose left_foot_goal_msg; - geometry_msgs::msg::Pose right_foot_goal_msg; + geometry_msgs::msg::Transform left_foot_goal, right_foot_goal; + + auto transform_to_geo_tf = [](const tf2::Transform& transform) { + geometry_msgs::msg::Transform msg; + msg.translation.x = transform.getOrigin().x(); + msg.translation.y = transform.getOrigin().y(); + msg.translation.z = transform.getOrigin().z(); + msg.rotation.x = transform.getRotation().x(); + msg.rotation.y = transform.getRotation().y(); + msg.rotation.z = transform.getRotation().z(); + msg.rotation.w = transform.getRotation().w(); + return msg; + }; // decide which foot is which if (ik_goals.is_left_support_foot) { - tf2::toMsg(trunk_to_support_foot_goal, left_foot_goal_msg); - tf2::toMsg(trunk_to_flying_foot_goal, right_foot_goal_msg); + left_foot_goal = transform_to_geo_tf(trunk_to_support_foot_goal); + right_foot_goal = transform_to_geo_tf(trunk_to_flying_foot_goal); } else { - tf2::toMsg(trunk_to_support_foot_goal, right_foot_goal_msg); - tf2::toMsg(trunk_to_flying_foot_goal, left_foot_goal_msg); + right_foot_goal = transform_to_geo_tf(trunk_to_support_foot_goal); + left_foot_goal = transform_to_geo_tf(trunk_to_flying_foot_goal); } - // call IK two times, since we have two legs - bool success; - - // we have to do this otherwise there is an error - goal_state_->updateLinkTransforms(); + auto transform_to_geo_pose = [](const geometry_msgs::msg::Transform& transform) { + geometry_msgs::msg::Pose msg; + msg.position.x = transform.translation.x; + msg.position.y = transform.translation.y; + msg.position.z = transform.translation.z; + msg.orientation.x = transform.rotation.x; + msg.orientation.y = transform.rotation.y; + msg.orientation.z = transform.rotation.z; + msg.orientation.w = transform.rotation.w; + return msg; + }; + + left_foot_goal_ = transform_to_geo_pose(left_foot_goal); + right_foot_goal_ = transform_to_geo_pose(right_foot_goal); - success = goal_state_->setFromIK(left_leg_joints_group_, left_foot_goal_msg, config_.timeout, - moveit::core::GroupStateValidityCallbackFn()); - goal_state_->updateLinkTransforms(); + bitbots_splines::JointGoals result; - success &= goal_state_->setFromIK(right_leg_joints_group_, right_foot_goal_msg, config_.timeout, - moveit::core::GroupStateValidityCallbackFn()); + // call IK + Eigen::Isometry3d right_iso = tf2::transformToEigen(right_foot_goal); + Mat4 T_right = right_iso.matrix(); + auto result_right = calculate_ik_right_leg(T_right); - if (!success) { - RCLCPP_ERROR(node_->get_logger(), "IK failed with no solution found"); + for (const auto& joint : getRightLegJointNames()) { + result.first.push_back(joint); + result.second.push_back(result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint } - std::vector joint_names = legs_joints_group_->getActiveJointModelNames(); - std::vector joint_goals; - goal_state_->copyJointGroupPositions(legs_joints_group_, joint_goals); + Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal); + Mat4 T_left = left_iso.matrix(); + auto result_left = calculate_ik_left_leg(T_left); + + for (const auto& joint : getLeftLegJointNames()) { + result.first.push_back(joint); + result.second.push_back(result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + } - /* construct result object */ - bitbots_splines::JointGoals result; - result.first = joint_names; - result.second = joint_goals; return result; } void WalkIK::reset() { - // we have to set some good initial position in the goal state, since we are using a gradient - // based method. Otherwise, the first step will be not correct - std::vector names_vec = {"LHipPitch", "LKnee", "LAnklePitch", "RHipPitch", "RKnee", "RAnklePitch"}; - std::vector pos_vec = {0.7, 1.0, -0.4, -0.7, -1.0, 0.4}; - for (size_t i = 0; i < names_vec.size(); i++) { - // besides its name, this method only changes a single joint position... - goal_state_->setJointPositions(names_vec[i], &pos_vec[i]); - } } void WalkIK::setConfig(walking::Params::Node::Ik config) { config_ = config; } -const std::vector& WalkIK::getLeftLegJointNames() { return left_leg_joints_group_->getJointModelNames(); } +const std::vector WalkIK::getLeftLegJointNames() { + return { + "LHipYaw", "LHipRoll", "LHipPitch", + "LKnee", + "LAnklePitch", "LAnkleRoll" + }; +} + +const std::vector WalkIK::getRightLegJointNames() { + return { + "RHipYaw", "RHipRoll", "RHipPitch", + "RKnee", + "RAnklePitch", "RAnkleRoll" + }; +} -const std::vector& WalkIK::getRightLegJointNames() { - return right_leg_joints_group_->getJointModelNames(); +const geometry_msgs::msg::Pose WalkIK::get_right_goal() { + return right_foot_goal_; } -moveit::core::RobotStatePtr WalkIK::get_goal_state() { return goal_state_; } +const geometry_msgs::msg::Pose WalkIK::get_left_goal() { + return left_foot_goal_; +} } // namespace bitbots_quintic_walk diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index 46f775fd2..3beed946d 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -318,17 +318,11 @@ geometry_msgs::msg::PoseArray WalkNode::step_open_loop(double dt, } geometry_msgs::msg::Pose WalkNode::get_left_foot_pose() { - moveit::core::RobotStatePtr goal_state = ik_.get_goal_state(); - geometry_msgs::msg::Pose pose; - tf2::convert(goal_state->getGlobalLinkTransform("l_sole"), pose); - return pose; + return ik_.get_left_goal(); } geometry_msgs::msg::Pose WalkNode::get_right_foot_pose() { - moveit::core::RobotStatePtr goal_state = ik_.get_goal_state(); - geometry_msgs::msg::Pose pose; - tf2::convert(goal_state->getGlobalLinkTransform("r_sole"), pose); - return pose; + return ik_.get_right_goal(); } std::array WalkNode::get_step_from_vel(const geometry_msgs::msg::Twist::SharedPtr msg) { diff --git a/src/bitbots_motion/wolfgang_ik/src/ros.cpp b/src/bitbots_motion/wolfgang_ik/src/ros.cpp index 6c059f3b9..1ac782fde 100644 --- a/src/bitbots_motion/wolfgang_ik/src/ros.cpp +++ b/src/bitbots_motion/wolfgang_ik/src/ros.cpp @@ -49,9 +49,9 @@ using JointAngles = std::map; // Joint names in the order we want to print them. // Adjust these to match your robot's URDF joint names. static const std::vector JOINT_NAMES = { - "LHipYaw", "LHipRoll", "LHipPitch", - "LKnee", - "LAnklePitch", "LAnkleRoll" + "RHipYaw", "RHipRoll", "RHipPitch", + "RKnee", + "RAnklePitch", "RAnkleRoll" }; class LegIKNode : public rclcpp::Node { @@ -81,7 +81,7 @@ class LegIKNode : public rclcpp::Node { try { ts = tf_buffer_->lookupTransform( "base_link", // target frame - "l_sole", // source frame + "r_sole", // source frame tf2::TimePointZero); } catch (const tf2::TransformException& ex) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, @@ -114,15 +114,26 @@ class LegIKNode : public rclcpp::Node { // ── 2. Run IK ─────────────────────────────────────────────────────── JointAngles ik_solution; try { - ik_solution = calculate_ik(T_target, true); + bool left = true; - // Apply joint offsets to match actual robot configuration (if needed) - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; - ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; + ik_solution = calculate_ik(T_target, left); + + // Apply joint offsets to match actual robot configuration (if needed) + if (left){ + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + } else { + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + } } catch (const SolverError& e) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "IK solver error: %s", e.what()); From 62991ac6875b1afab491ba323f871dca2073df56 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 13 Mar 2026 19:02:25 +0100 Subject: [PATCH 04/16] Remove old pkg Signed-off-by: Florian Vahl --- src/bitbots_motion/wolfgang_ik/CMakeLists.txt | 58 --- .../wolfgang_ik/docs/_static/logo.png | Bin 11686 -> 0 bytes src/bitbots_motion/wolfgang_ik/docs/conf.py | 187 --------- src/bitbots_motion/wolfgang_ik/docs/index.rst | 21 - .../wolfgang_ik/include/wolfgang_ik/ik.h | 371 ------------------ src/bitbots_motion/wolfgang_ik/package.xml | 18 - src/bitbots_motion/wolfgang_ik/src/main.cpp | 150 ------- src/bitbots_motion/wolfgang_ik/src/ros.cpp | 214 ---------- 8 files changed, 1019 deletions(-) delete mode 100644 src/bitbots_motion/wolfgang_ik/CMakeLists.txt delete mode 100644 src/bitbots_motion/wolfgang_ik/docs/_static/logo.png delete mode 100644 src/bitbots_motion/wolfgang_ik/docs/conf.py delete mode 100644 src/bitbots_motion/wolfgang_ik/docs/index.rst delete mode 100644 src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h delete mode 100644 src/bitbots_motion/wolfgang_ik/package.xml delete mode 100644 src/bitbots_motion/wolfgang_ik/src/main.cpp delete mode 100644 src/bitbots_motion/wolfgang_ik/src/ros.cpp diff --git a/src/bitbots_motion/wolfgang_ik/CMakeLists.txt b/src/bitbots_motion/wolfgang_ik/CMakeLists.txt deleted file mode 100644 index 6be314cff..000000000 --- a/src/bitbots_motion/wolfgang_ik/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(wolfgang_ik) - -# Add support for C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -# Build with release optimizations and debug symbols by default -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebug) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(backward_ros REQUIRED) -find_package(bitbots_docs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(sensor_msgs REQUIRED) - -add_compile_options(-Wall -Werror -Wno-unused -Wfatal-errors) - -set(INCLUDE_DIRS include) -include_directories(${INCLUDE_DIRS}) - -add_executable(ik src/main.cpp ) -add_executable(node src/ros.cpp) - -ament_target_dependencies( - ik - ament_cmake - ament_index_cpp - bitbots_docs - tf2_eigen - Eigen3) - -ament_target_dependencies( - node - ament_cmake - ament_index_cpp - bitbots_docs - tf2_eigen - sensor_msgs - Eigen3) - - - -enable_bitbots_docs() - -install(TARGETS ik DESTINATION lib/${PROJECT_NAME}) -install(TARGETS node DESTINATION lib/${PROJECT_NAME}) - -#install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) - -#install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/bitbots_motion/wolfgang_ik/docs/_static/logo.png b/src/bitbots_motion/wolfgang_ik/docs/_static/logo.png deleted file mode 100644 index f8afdd5d066232e2dba94b758c6c44fe72b42820..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11686 zcmXw92RN1Q|9;Jm5r>RpWG7@(BpJsDp=`1phm^hd-h0QfqL3Xzj!i~JRv}wfGBUII zKi}VV{om_2=klKSea`bfgp&VAb|Sc&F*NFgB@tx<-Z;YfUD5ico@ud|H^*oUSla|u+Kdso~i3-C87>d;4E{yfHSMIeSYOAHl#@;HngrMIR za(p~|u$?+a2JyvKOCTfT;B1gttk6vv#`gO?`gt~(xpedquJ|CL=|PKn?m9DD=mk^Y z`pvEr56IzTL$yfX%#R0!t^&gc*Om=4+dse|CnvY0%~O{v^gtfA1liK!FWrLd>t?nK z23GeY3%FpxffosI;X?I+nzx%0qG}G}@BNLfK7d%1$HcNQ z7^x>Pa})}-B+n*Pxp@XIxVsR(qwlQ3YSMhQs<^3m)2aPq+MK?l*ksrL8ST zs;06I?Xet3C`*od*ZRSb87}nw`*$TZH8lp$TxlfCiI+zxR+Pcfu z^bsrm_VzY{iAj-*s?#FwyAnt6-k#IAQqWj*6bG+pi&z_s5(nii z>D=|vk&!^nqv67*Z;)Rt_vO;jEt+E?cPupMWMMzwa`g|~)2lq>d(UJ%c~?R-Q&rul zI5l|P%aQTMxTraTkawWJ;U;L+RU)5@n2YZj@g_E&gqHvC54JT}$clI6@mgnrFp{cU zD<>z1!P3Aa^^9*I*c>6Kc8jq$X#KS2kuN`fNXM7HzCP1G*efyWD#-{1tqI0nzo~8a z20ofwfm@Mq1&Rk*Hw(DUe_v5;`x;4!( zoWhx;;6;k~C{99CCnLi`M~Qz`k?C<~hKZTk8V?U|VN!HEH(E!6C~dpYw@~uh5zL?Q zoktde5|NIaq$f~)R7dky`~wjg4GAiReDz|NhlPigDienFKeTmp>gJdCF3N_i zYIz5Q`35MF`6Jl+`UfS+`09#+^j{p;1qYe4TwXW4!P)S*Q@&qO`e8LSqH+_4k6VJl zV8%n?DzN+wR}2|B`9hv5=fI0$t{5;|6)i0-m)(Z;jp!dMn-OHUk@IbB`iN*U^Uo+m zPXN6Pjsw9dYtZ32rLJy;5f$eOMNIdgag)26kP_K#3D?uRs?@MTu-%4(UCFJE(m&mFL@8hlNXs10|83`h z$+%XA+3?o2CV#jS>N!0FvW~s8PevAkOZ?#`Cv`AW)rjgcZ0 zUclAg_I+Z)KN0?Gt{T;wE!PUyqC+8~6e)z&5JyKJcRu4yL{rPC-4qnYo&G`KeIj^b z2A7JLi<>dljF`Z_is2`*q#I#~VckCNx|m&W-)Uv|P>Ok>q$A|L2=(OoQeKp3>90A; zWEP#TodtU3_U_Cz-L7gq`=bs`KFaxKNE!77&qX0>@F5!`S#MrleZ4<-j(W^Yy&c~0 z!AD+_+K2l^_coOzh;|dCh>ddDqoJ2uYsDxAmm3ArRH6pb`MQ3T=}{!~CMj_^t_{*D9|{p zG$PtwQPwv-8=F;ypgi)4H?8>K1UMfR^wM=_|BeL%C zIDZ)O*5++&CA&nN;y}&rT9yM_rYaj#!g7Dwy_c_F%dsWFTTK4?C36^y8H&^jBhy#g zD$2@Ur*jZJ^4JMwfhiS_gUIFP=7wNq&g04gz1x}+@|0wP+S96qLXUUC8UwB`NHfUt z$k(neVSndh?x7Smu@8k1F|4lV+k$z156W)z2OhQKDP3MJERX%>g4&n&7Ekt;BfIA@ zI}u3MlS}{SDoN_a9Zrn%r{6p}t!nctD=QU1KXNOsg0(4-n5?WUF-jy04EMrcn#pU0 zLR-&zZjW60VmFjBrgvG~`;ZHSGRTaBUcq;mDI?*r?gYuYxzsyM94^Y9%-MUkm@qzaCpE0M4I1Cb)>rmD9DO1SljWj9 z>gi2}nz%L1iNTQ=IuzdcCcpT9YeuiEtZXx=oOeZ>%%qAoRa5eK*2S^WC{mscq@t4_ zRsIB3lO#x(t8s6lP3OhgB<;l*WafXli4Y#*W~WAU4JAKwkEzN)*AF0L)g_< ziSceRx%i{khLmLLJvkAlQs}51mI94PGLeHE$&-c)o%ptTD`;GOdfq}%l7c?P`aXG- zqkge?L%MybD99t;k?Y#?}o=ij^tCTQb2PbL}yRR|(~+6uNF@g1g>2y|8>z8g0M zGB}izK86-uJAJo=4p-$h{p~Jxm%1FA<`Tgk&LhVZE%ha_dtRNcFF7~D6MBUnJRr;( z(tZAH@FNIf@!{d&BvN@*nako})>H}?-J7bdhUgLDLG$w_q@<*(ygJj{?q6;Z8QFm) zIQG<~t*;NZo3#M>`L+M~(S9_Ecc8CyaogiYEUhBu=q%29k)T3q~dx=?zC(E_PrU(LnN#Kc4%PqeUHzzSb@^F4OTD4ArA zDcc9x47U~5ervpKZ53QCR>xt!5)_mo%iv5@#pS0pGcd4{JRlV`9MKbmc@5B%AJMjv z3Nl5#J(gn*KGF-&`P@z(0P|fFG4L*mk;=W%Y_gcOfG@S5)H%#7MLK*}I*P1;VZ)Wa zv{X2uS1xdi5mn##TsTt{+thUY2kURPe?(Uhwfwnh%iTxzjyiYFuV)sqL7JGj_a7>9 za)?Y%RVFN(ed!Rl33_@UouRASWOz5oaW^ON&W~yRTU(i|?6yOV_z6*=?Pu*bh!-w) zJc6G2vOSU@*^T%!*u|kt#+OOqrA) zd=n&fWQ=&phU2o(+8Jizi365Qjp@9t;Lujgf(J7kM#W#_z+4a|-{BLMsI6l*UX&?D zcq2&qLXJiE3y&5rq3`K{jKkkSA4&3h=R4XuIgw>NgrRWIxYXrYEE6q$e&YxFBx)VT zURFg|Inw{lp3|aMUVr~D3pg2`$AauoU%x5RQQq5Oq zo1eXpc%b$TSBv4?$C2F@00x$ia6BK6X3j}TQnHMT*@&0UBe&vK9b%pA)jIR{E_Mk` zET9uw>M9n!w_dzFn3)_U&r4{86+OV(Sk1`lKIDlGo>#7U#FW6EsoJ^Z7`X10n0?QJ zxZkx74|<<@uU!-l4WlO`$|(`ao6FoJPs0!zu9BL$6u7-Oa&jUhv%W6o4o+QzVB@E(!d!-?s%=-0rR^x9Flz++1#?(^#A?@AHU+ zK^=>OxHI+WNA*FBu4FY*r5AfV3(JdefpLEI+yp~Ny5 zBs_2^tM*pQM(AyEXp+Obb4l>epFg`v3BLHuAecEV$B9|I9Q)u(gzNZwWbi@jep=_5 z?Sm8FcGHXQ!OgE6zeh&CL}nyV#0tG1w~>FRai1o}CZh-D|6)lB+1FEDUHu5>0*_Vx zT`DG3v&V^%8b3&nDol^6+ooZrB=df**PC&ovt{P4i-ET6Y3v-Ct{}!C>`qE zmcP`^3desVUMJuCB1me~|G{pr%+DzXICC;fs7b?Wf_?x{Fjgu0itw< z4Y&cv9__Sk8Fl)^-1oEi&c~!L%qQvuZ`1P2#CEq+3o762;u2qWm&k#ecoD31$F!H) z;M^6Phh>Z7Y|Y#-Y7|c6SE+3+K@SA3=iQjL9&CZs@UHvVKjeMNttfp}3KCk9tHh-g z)MO+5I-aWZ;eBN~7AJ9~}xgpP%q+F=^f-^ z*q8!OyE&}eeHr(JX|zH3UJpZ97BhW6mRdc#ueIC;_u*;`xgXc2Zx)&;ec=;PPNwSD znMwc^vDxZ~+Ce-BYC9otTv5`E6(Zc4!f9T<-M%wdOHY3|V5&IhGMq1U7b%iSip65d zN=vaZa7}`@BtJitX&{vpsT7pc(La|QBi);f*St)~!jO;Oy?b~2RN^iI&_?fPZf-{& z&fL_T?Fc!}6dcqbKraUe4CgTQ8zzr3O%N02JG z!%PA}DrMF%Q+4Ls+NM@KK4AACOD!D@a;19uD$;=RP>I8N0l>UGLT5m)`x+Mf&%as< zH%jE>1|)b0XUYp_`aXoiE}q)iZ8tD5Fr*o+ia!#@y*%6~|6st?X%1s}u!zNAYC&07H27(nHCopH!P9!&4c#VHrIeKnA_pM3vB_n zP`q+DG8(Bv4fEcc`xY6H<+}nyaL}Oyp0FipUr7{F4Y@!1?DLmzDDF8donU9)XVkVq zfGaprGnyvr=0!zA`WgyD*3AUhQGtG zJ(Z7|NkR6tFp>X(70Ws$H_@K>=i^q|);Fe#1D`ag;}qQnYI1QM=YX8V5PkHhq%=A` zod$r#ofesR3j2npJG!xk;y2=#pPQQg{W!CI6mIT&moiE<^WlB$+{Wd*UJnLZ^ zV*zM!%V1Y*A-1PXZ}fBE+n^1{d&IiCk}ElnIxa_`k z8!KP*?CL`Ot}cd?(dRh$`lL4~N!Ij zxbI31?~81owj;VmM&O_`fwPjxNw=2b;^N;YS_bJ(|F(WEf!H{SK(WfgTN#MMr2ZbC z7jCTTeii=J8*jcM$BY~k?T~oXF+JUm!N}m^B<3CYB->cpSNj6?z2f7gg#F9vN8 zyDVSMEi5xH-GFn=<0CD)WCvM`q*De3- zSwaT_#n_t!`r0}=I;s{c*VC552u6g!+@)cu$`Tq`!edH6v#glRi@J2Motnr zUp8C;VE9;PHH1+`q24`SpKf1VT-0afgaRu!J#LbcUMQxo#&-`69lM*kx{BV8QzWOd zz=Z^RD1mJEjzP@6{FR*@wWWa~_+8V_jpvi8>k1;8KWv`$uJQC9 zToB|9-^dzDPl%~2DV6Ao+}PgkK0R3XE8w!nb7lwYSIlARBaTYYlqL{{o95!RdBRfe zJ|ipCEb-)}+02YV^i1ifY&>bgVsj1aUg;bKJSlrA>|CN$wSo0#K-qp9In?KMUu_S< zqup|Ea_+h*COWh2{((SPvHsJix22`T#n1BWapx)3;{F+AzYaJ54ITxSEAI5GTZUy9 zhT=6M-2{$79pg|viOPzH>jN!$2ST73k#-w z(*E;Oc+<_UjA~LP?yx!g8?5hfJYy)XeyXbXWVl_N?Bx;S2Hha0$-o1T@uc)ugimd%wae4|yw^51OWW?}^GLL3?h~mFVge|B*(%4( zN|+Ai=(xM28JR(*xL^ih@1f2oyTBw}qff@alfb*XCnzYlo%q=|8J50bF)=_Kn=!Trn_n`QMWS zZ^u?=*kD>}=khvXx$D+wpmFd;7nB>qgdj76iK_c*O{YK_dDGj|lP>z#z1l1`zo9`) zLR9pyz#Jha(nB0dl1`|gF)>gxD?D@QyDo%M(&_u}A=b z6@fqmwuRcg=w{1Ne^*pQ7+t{9CJhq)q-nrqou{PKx_pIdqR|4iJpVH;_Ed+?Q+7J^{;1jCuhb=CcqMO5FbpkZ8)y%h$FK~FZs0vw* zAIAHrjei>Muepc`3JRJG?pQpBw6#>cWZ@Yc%FSHSH5|(BvT!B!2}4ZL9UYyaml&%J zth%$PsZ}iBxZ5RR@%X7fw7|dX&xGt#huR8K{j=bjT?oUE_-3&ie0+ReQ2Ma0A{?J# zKQ@uWMqLpg99*H})gU+<&WEG(OKBT$Ks5vLnW}{9#kUH$IyaBz*Y-SHdz`XK+V@mB z5AHTvpQ>_dF(>pgK%$x|hT`#t9f+q{MP8XgG?3%Xc9uXmDwYDNtDJo3@@C*D3h<+a zHylJjODNGMPSph3Te)eQzlW*m0=Bial{!`Pl|lLq8I8*9)3>Fsg5ua6WT_M$LF5h% zGn-zydG`swh^l=koi-#YfB>y5*u-jSq`CqSLrEOw2-sGqs2X5WMlIgdn-dkACr4EQ zEwRg=Q%g%5peb^w(e?tTm@Jxy+G>;8lvl(nE$VB{5e}iqd;??oM$3J-RJf*~d8G{oY^e6BsSJat z5`QuiPwwrB;qb>q`BUIxIY_JHoqL>E+i~;-D$zS6TE)vumgTz z&(oI;J;HG&!(F$RIz~q`J~f5dX}k8nI4qXl{`>cD-W@sx1N{3dvxt(ty}iO4aeNQ+V(nbH`674g>jFvqV5jPRu6?I z!I7%tR9?9b1cCo12z@VKn?fkkyvxffbIIgDa|EkpLs=$cua0Pr2#Oc$zCEo4y?9_h z8~OANzxh5sX5A~(nNbWa4>-T=d@~)W5Ydz2Oo$U=X(3OS)5c(U8IW-xi{^#kAi zdGhx4EQ@kjXkzj|KU!oZnIoeZ#hU>V$$E#GP&-30^{sCOsaVL94#ipc>DT(+;qkGw z^Y4-Tr7U}_O}OVTarHHqTjfN6VRQ*Pg3SAF&$IbU##y5*_YoXP3W3^q4%E&Y9?`ac zGcq`^z}KQDk~r!PU^bFSc~ovVensVq6)yqmmo zX&=VF?oTsKEYyYVd0upa9GGA`hdsg@?-C|rec*o<3CG77mOL2D1i4h8}Fep5{q z$y7~V_u|h{Kfdkh<(%xG1d3)~Yj}e+`zeq#pO3gU%Q-?{Zn#RkFMS0{W>&-WnEF=ra~>G0=z5n| z7r@qWy~|DG8;27%=xO21!I*IK3ntc;KIE|fKmsg&IX9|z8QLYsR$kjW(Gfz@yN5zI2@lWE_)zt|!&S@oQ+`Cv&VAavl(9yXA;?I@Y@y;BP@gxcq{36cJ z&WP{w@_sMV``iC4yUocjhLT#YPkg1fYazEa5Z}J1u1odO)5>QkeEh;K94rbwM za^J3ri;v$gEk*XoP=-eO*D7>h(QX@&c$r=F45&>GWm=AOI zv;cDJx=w!u2%R8?Qx-llI_e}xWT$)9$K3n)c(#_it=0MJ!mAnRFhJ7}H)mla%LDsu zaz6%Ht!~0hXS9?MzwoT)M$B8dcVubwyUG!Ir|1+ zSzy1z)=X9ZjqfE|4zDnT12wKvARX-g9(`8~mWh~F$@fKz2UqTqMYH%dE4?&h zRzgagDU&ePhC7r|*LW=bK>tv9ME@C@CmVhLEiLmioaT=%H$WAvPyi|KFQNY0x7h-A z`TY_irkwS-!@Q^Vg9?xJeO2T_KRslOfAcUt#fFBl*GaP1i6u0nk4L8;u?(uvs^gaF zKnpVuS z<7U@~6OD{obMA+KORK6JoSWuuF$w(VR`Hf1$?SGwT=nv2du*hh{r&jxaFC8jh(SF5 z^^0nY?xU0{H7RPTg$dW{w6B(qtNm&H5%k}J3N1rbxpQjjWw^sQlut{pj4W!5_Ou;~ zzxig35wWs_YVUmd$@bAw0@xwKQrj{sjZO_FTpBZ`7St6T8P~bQ(%o=4Z4F}kDonL~ z)hzgV>DXbcl>}S-d}VS5ndwUQ#(qxE+H&UClJCpg{A9;njVY>8)U=nsa2F01D&jSh zPyLa9sybB2R$PA1U**zBU-LFal)Q>mKW~~!hv-lZ+d~A(mxGe~?9(lltPu}SPefmM z7(ZQlEJZrHkunNjMR_qpdTBs($e9lzL`5unFSXo{W~Ek2Ks2^ko3}6?J~KVN|Fs|* zclbcR*6Jp{EZ4o26is@=R$nh=y3{+$NI-G93aNh%>hq?LO>$g&j8e!vKz^LH9_Y~I z*{m@wHxi**H8gB%iG7>OztY{mJyK)*`{{O&aX--<(xiO$G*Dy5?5yH81&LQR!DsQo zYx6Inyu01$MxvMG?8NVk6jS*yA3r_CfBLGGzcF54X?9Dq!f?IX(r7p#5~RVtlamuO z`$yD9{7q&3%gJABUl6gKTre*ZBB&H^$x}pR-BEkNd+WSl12S!td!r9kMGr zId&%T?`&U+TZVRy3PSqnX}VP1K(3qFo#DlZL%^~GYU|IU3#M3=HJ_mT;;z3WS?4_Q z-}^Ma>@@K-`4iBa1!F@}5YH=XauGmqUJ&f@%o>_7Pg(7`dwn{*U3InfD1IWHuz`1A zyN0)+taQ4<=7qSPzcO95Wn5;_#vtEV?X1*&$Y3e&>xbuePeVAXR?xz9zp*Cic=1At zJ-xjLnf84vpG!MlcO{}J!uQgpKvR~nnT@HGB+|0)@xeMF?Q(Q?bL&`g)K5pF_kUUk zcfcRmoeM%NNrRkYW1@UFWomgz=H}*DLaC%yyqSJuuYkSC0vMk3)4Ns22L*e~^huBX z)`n|bGdZE;;f41&f*?4XVb+qZ$W4Stf#C}^G&FP?$DZ)y-KYo%Wb{!zKVNbMFC_`l z#P!@tQhGll7+Zyh;0%3x$47gw!+BbibUAq8Z^BCOE!BvwJ@V30lM%Pa$tqoF`{!-2eho!Z$}rg7z`_yWIQ;my{6_FZD|JF(o#htRuf1cY|p!YThDiXK)|(?U?vy| zuw{OJeg|Ir!QuF`03HM>rhsd$fnYo=+?OV%(DiKf-8Vj2cqq6m6v?_ZRgIT-by&Q& z7()J^lj`W`2o#*LXafld&=0QuSXJxT4S=Q+_Rz^327q?Z3YdJkE44>7>#^WB)!@Jk z9A=Boji7`c(NM4)@MvxkTj{ZS^$A}VPTB>EvZmx!lQDI#Cy~hwkdIJbmi-#NIBceCEy1M!-g*}(TXayxChzDwXlR4%r z5QN$~kDC+!>F8WGcuH38OSUBoO)$amKL%X;gOV;Vw}A?C$PMy>vN96JUb|$pmmg1> z1sMJ9NA6qVV<7+8a;yGSaCqC&;Z{p$<>27q=IXjQk}oIjyaKat zInvF3=wkJ&odk*=UiW#z0{^YfnL-k+3k+3ZW`ZZtT}%2}#>U$~BjW=OEdTwKJTqX! zIaw75005!dyo(U15mmr-#+l;zJ3m4K#(KM|<==E4ko6g$K}i*`SiV!p!uGsc=A(|* zT6~TzTwSSv|IOEz(Y}GtcY3v^X*%EAN|`F1`aODiCpi6b_7X?;}t+^)JyXx8PO$(Rybs+pKz`qeBfGOW|r+50<8?QTlbSsDZsF{@b^2 zQP%%S0f3wSaM3oU;4OW*f9hN2C$*5;OaqJ~_=QyL>_1(}gIN~C8SFEz7;PV9{XX(u zAX+>M8)F@Cw${>0F@=XlQ56NVsBSn$bOqgiJu$fZx^d3FucrBTEEq%9Xp4ix6x)N_ z)lSPj!NUNHQcDhZS=9zDf5xbcK|A-!-fifrx@*dgQj?UeVkzn)8 zZ5T=~sw^sUWL|dxP%zXeeSiQUG_0@EknOtVVA^rIY4E`;O&yN$WSM|zD=8@0J$drpi&#BZ(jX~x zBQ!A=IAaSyIe*C!0c@+ET{kp4cAw?00~2DlI*@;9=@}TD0M=KKwlc4#>kw5F0So4I zV`F?x&fcP7aq9vCG`Q;M__%AswR8QTWBp(WyIGlaK-Yr+@-$HSZk1@i8r9pRaT8%@ z{c>+$pR;YDt!v4V*Q069ziDo#I5j#Qt-+FI9|X6^ETa1%%7;-YEGW_DDc$sT;GjG; z;h@Ys2(4n= num_files_cpp else "cpp" - -# Tell sphinx what the pygments highlight language should be. -highlight_language = primary_domain - -if num_files_cpp > 0: - extensions += [ - "breathe", - "exhale", - ] - - breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} - breathe_default_project = project - - def specifications_for_kind(kind): - # Show all members for classes and structs - if kind == "class" or kind == "struct": - return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] - # An empty list signals to Exhale to use the defaults - else: - return [] - - exhale_args = { - # These arguments are required - "containmentFolder": "cppapi", - "rootFileName": "library_root.rst", - "rootFileTitle": "C++ Library API", - "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), - # Suggested optional arguments - "createTreeView": True, - "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), - } - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = "sphinx_rtd_theme" - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ["_static"] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# -# The default sidebars (for documents that don't match any pattern) are -# defined by theme itself. Builtin themes are using these templates by -# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', -# 'searchbox.html']``. -# -# html_sidebars = {} - -html_logo = os.path.join("_static", "logo.png") -html_favicon = os.path.join("_static", "logo.png") - - -# -- Options for intersphinx extension --------------------------------------- - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {"https://docs.python.org/": None} - -# -- Options for todo extension ---------------------------------------------- - -# If true, `todo` and `todoList` produce output, else they produce nothing. -todo_include_todos = True - -# -- RST Standard variables --------------------------------------------------- -rst_prolog = f".. |project| replace:: {project}\n" -rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format( - ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" -) diff --git a/src/bitbots_motion/wolfgang_ik/docs/index.rst b/src/bitbots_motion/wolfgang_ik/docs/index.rst deleted file mode 100644 index e76aa433a..000000000 --- a/src/bitbots_motion/wolfgang_ik/docs/index.rst +++ /dev/null @@ -1,21 +0,0 @@ -Welcome to |project|'s documentation! -================================================ - -Description ------------ - -|description| - -.. toctree:: - :maxdepth: 2 - - cppapi/library_root - pyapi/modules - - -Indices and tables -================== - -* :ref:`genindex` -* |modindex| -* :ref:`search` diff --git a/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h b/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h deleted file mode 100644 index b49bc8515..000000000 --- a/src/bitbots_motion/wolfgang_ik/include/wolfgang_ik/ik.h +++ /dev/null @@ -1,371 +0,0 @@ -/** - * Leg Inverse Kinematics Solver - C++ port of the Python implementation. - * - * Euler angle conventions used (all are from transforms3d, verified analytically): - * - * euler2mat / mat2euler 'sxyz': - * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) - * Decompose: b = arcsin(-R[2,0]) - * a = arctan2( R[2,1], R[2,2]) - * c = arctan2( R[1,0], R[0,0]) - * - * euler2mat / mat2euler 'rzxy': - * R = Rz(a) * Rx(b) * Ry(c) - * Decompose: b = arcsin( R[2,1]) - * a = arctan2(-R[0,1], R[1,1]) - * c = arctan2(-R[2,0], R[2,2]) - * - * euler2mat / mat2euler 'rxyz': - * R = Rx(a) * Ry(b) * Rz(c) - * Decompose: b = arcsin( R[0,2]) - * a = arctan2(-R[1,2], R[2,2]) - * c = arctan2(-R[0,1], R[0,0]) - * - * All matrix indices are row-major (M[row][col]). - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -// ── Model constants ──────────────────────────────────────────────────────── -constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] -constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] -constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] - - - -// Motor offsets (left leg) -constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; -constexpr double KNEE_OFFSET = 0.26180265358979327; - - - -using Mat4 = Eigen::Matrix4d; -using Mat3 = Eigen::Matrix3d; -using Vec3 = Eigen::Vector3d; - -constexpr double BASE_LINK_SIDE_OFFSET = 0.055; -constexpr double BASE_LINK_Z_OFFSET = -0.0414; -constexpr double SOLE_X_OFFSET = 0.0216683; -constexpr double SOLE_Y_OFFSET = 0.0152219; -constexpr double SOLE_Z_OFFSET = -0.0529; - -// ── Custom exception ─────────────────────────────────────────────────────── -class SolverError : public std::runtime_error { -public: - explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} -}; - -// ── Elementary rotation matrices ─────────────────────────────────────────── -static Mat3 Rx(double t) { - Mat3 R = Mat3::Identity(); - R(1,1) = std::cos(t); R(1,2) = -std::sin(t); - R(2,1) = std::sin(t); R(2,2) = std::cos(t); - return R; -} -static Mat3 Ry(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,2) = std::sin(t); - R(2,0) = -std::sin(t); R(2,2) = std::cos(t); - return R; -} -static Mat3 Rz(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,1) = -std::sin(t); - R(1,0) = std::sin(t); R(1,1) = std::cos(t); - return R; -} - -// ── Euler composition helpers ────────────────────────────────────────────── - -/** - * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) - */ -static Mat3 euler2mat_sxyz(double a, double b, double c) { - return Rz(c) * Ry(b) * Rx(a); -} - -/** - * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) - * b = arcsin(-M[2,0]) - * a = arctan2( M[2,1], M[2,2]) - * c = arctan2( M[1,0], M[0,0]) - */ -static std::array mat2euler_sxyz(const Mat3& M) { - double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); - double a = std::atan2( M(2,1), M(2,2)); - double c = std::atan2( M(1,0), M(0,0)); - return {a, b, c}; -} - -/** - * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) - */ -static Mat3 euler2mat_rzxy(double a, double b, double c) { - return Rz(a) * Rx(b) * Ry(c); -} - -/** - * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) - * b = arcsin( M[2,1]) - * a = arctan2(-M[0,1], M[1,1]) - * c = arctan2(-M[2,0], M[2,2]) - */ -static std::array mat2euler_rzxy(const Mat3& M) { - double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); - double a = std::atan2(-M(0,1), M(1,1)); - double c = std::atan2(-M(2,0), M(2,2)); - return {a, b, c}; -} - -/** - * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) - */ -static Mat3 euler2mat_rxyz(double a, double b, double c) { - return Rx(a) * Ry(b) * Rz(c); -} - -/** - * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) - * b = arcsin( M[0,2]) - * a = arctan2(-M[1,2], M[2,2]) - * c = arctan2(-M[0,1], M[0,0]) - */ -static std::array mat2euler_rxyz(const Mat3& M) { - double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); - double a = std::atan2(-M(1,2), M(2,2)); - double c = std::atan2(-M(0,1), M(0,0)); - return {a, b, c}; -} - -// ── axangle2mat ──────────────────────────────────────────────────────────── -/** - * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). - */ -static Mat3 axangle2mat(Vec3 axis, double angle) { - double n = axis.norm(); - if (n < 1e-12) return Mat3::Identity(); - axis /= n; - // Rodrigues' formula - Mat3 K; - K << 0, -axis(2), axis(1), - axis(2), 0, -axis(0), - -axis(1), axis(0), 0; - return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; -} - -// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── -/** - * Returns a 4×4 transform T whose rotation aligns a reference frame to - * the given leg_vector direction. - * - * Mirrors the Python: - * roll = arctan2(v[1], v[2]) - * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) - * yaw = 0 - * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) - * = Ry(pitch)*Rx(roll) - */ -static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { - Vec3 v = leg_vector_in.normalized(); - - if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { - throw SolverError( - "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); - } - - double roll = std::atan2(v(1), v(2)); - double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); - double yaw = 0.0; - - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); - return T; -} - -// ── Joint angle map type ─────────────────────────────────────────────────── -using JointAngles = std::map; - -// ── Forward kinematics ───────────────────────────────────────────────────── -/** - * Mirrors calculate_fk() exactly. - * Joint order (all around standard axes): - * HipYaw → Rz - * HipRoll → Rx - * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) - * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) - * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) - * AnkleRoll→ Rx - */ -static Mat4 calculate_fk(const JointAngles& q) { - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - - Mat4 T = Mat4::Identity(); - - // Base link transform (translation from base_link to hip axis intersection) - T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); - - // HipYaw: rotation about Z - T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); - - // HipRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); - - // HipPitch: rotation about Y + offset along x - T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); - - // Knee: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); - - // AnklePitch: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); - - // AnkleRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); - - // End effector offset - T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - - return T; -} - -// ── Inverse kinematics ───────────────────────────────────────────────────── -static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - JointAngles joint_angles = { - {"HipYaw", 0.0}, - {"HipRoll", 0.0}, - {"HipPitch", 0.0}, - {"Knee", 0.0}, - {"AnklePitch",0.0}, - {"AnkleRoll", 0.0} - }; - - std::cout << "Target transformation matrix:\n" << target_transform << "\n"; - - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - - // Remove the ends of the kinematic chain up to the first and last joint - Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; - - if (left) { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } else { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } - - auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); - - std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; - - // Axis-intersection transforms (identity = at origin) - Mat4 T_hip_axis_intersection = Mat4::Identity(); - Mat4 T_ankle_axis_intersection = target_transform_leg_only; - - // ankle-to-hip transform - Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; - std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; - - // ankle-to-leg alignment (inverse of foot_to_leg subproblem) - Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); - Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); - - // hip-to-leg alignment - Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; - std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; - - // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) - auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); - joint_angles["HipYaw"] = hip_euler[0]; - joint_angles["HipRoll"] = hip_euler[1]; - // hip_euler[2] would be HipPitch, but we compute it properly below - std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] - << ", " << joint_angles["HipRoll"] << "\n"; - - // Virtual leg length (distance from ankle axis intersection to hip axis intersection) - double virtual_leg_length = leg_vec.norm(); - std::cout << "Leg length: " << virtual_leg_length << "\n"; - - // Hip transform without pitch component - Mat4 T_hip_without_pitch = Mat4::Identity(); - T_hip_without_pitch.block<3,3>(0,0) = - euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); - - // Hip pitch offset transform - Mat4 T_hip_pitch_offset = Mat4::Identity(); - T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); - - Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; - - // Real leg: from ankle to hip-pitch origin - Mat4 T_real_leg = T_ankle_axis_intersection.inverse() - * T_hip_axis_intersection - * T_hip_pitch_origin; - std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; - - double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); - std::cout << "Real leg length: " << real_leg_length << "\n"; - - // Ankle angles from real-leg direction - Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); - Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); - - // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) - auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); - joint_angles["AnkleRoll"] = -ankle_euler[0]; - joint_angles["AnklePitch"] = -ankle_euler[1]; - std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] - << ", " << joint_angles["AnklePitch"] << "\n"; - - // Knee from cosine rule - double knee_arg = (real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); - joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); - - // HipPitch base from rzxy[2] - Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; - auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); - double hip_pitch_base = hip_euler_real[2]; - - // Hip pitch offset (cosine rule) - double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - + real_leg_length*real_leg_length - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * real_leg_length); - double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; - - // Ankle pitch offset (cosine rule) - double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH - + real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) - / (2.0 * LOWER_LEG_LENGTH * real_leg_length); - double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; - - // Solution i=0 (matches Python solutions[0]) - joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; - joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract - - return joint_angles; -} diff --git a/src/bitbots_motion/wolfgang_ik/package.xml b/src/bitbots_motion/wolfgang_ik/package.xml deleted file mode 100644 index 8a19b7a99..000000000 --- a/src/bitbots_motion/wolfgang_ik/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - wolfgang_ik - 0.0.0 - TODO: Package description - florian - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/bitbots_motion/wolfgang_ik/src/main.cpp b/src/bitbots_motion/wolfgang_ik/src/main.cpp deleted file mode 100644 index 91e0b3095..000000000 --- a/src/bitbots_motion/wolfgang_ik/src/main.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/** - * Leg Inverse Kinematics Solver - C++ port of the Python implementation. - * - * Euler angle conventions used (all are from transforms3d, verified analytically): - * - * euler2mat / mat2euler 'sxyz': - * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) - * Decompose: b = arcsin(-R[2,0]) - * a = arctan2( R[2,1], R[2,2]) - * c = arctan2( R[1,0], R[0,0]) - * - * euler2mat / mat2euler 'rzxy': - * R = Rz(a) * Rx(b) * Ry(c) - * Decompose: b = arcsin( R[2,1]) - * a = arctan2(-R[0,1], R[1,1]) - * c = arctan2(-R[2,0], R[2,2]) - * - * euler2mat / mat2euler 'rxyz': - * R = Rx(a) * Ry(b) * Rz(c) - * Decompose: b = arcsin( R[0,2]) - * a = arctan2(-R[1,2], R[2,2]) - * c = arctan2(-R[0,1], R[0,0]) - * - * All matrix indices are row-major (M[row][col]). - */ - -#include - -// ── Main ─────────────────────────────────────────────────────────────────── -int main() { - const double PI = M_PI; - - // ── Test foot subproblem ─────────────────────────────────────────────── - { - Vec3 leg_vector(1.0, 1.0, 0.0); - Mat4 T = foot_to_leg_from_vec_subproblem(leg_vector); - auto euler = mat2euler_sxyz(T.block<3,3>(0,0)); - std::cout << "Ankle Joint Angles from Foot Vector:\n"; - std::cout << "Roll: " << euler[0] * 180.0/PI << "\n"; - std::cout << "Pitch: " << euler[1] * 180.0/PI << "\n"; - std::cout << "Yaw: " << euler[2] * 180.0/PI << "\n\n"; - } - - // ── Zero configuration test ──────────────────────────────────────────── - Eigen::VectorXd q(6); - q << 0.0, 0.0, 0.0, -0.5, 0.0, 0.0; - q *= PI; - - JointAngles joint_angles = { - {"HipYaw", q(0)}, - {"HipRoll", q(1)}, - {"HipPitch", q(2)}, - {"Knee", q(3)}, - {"AnklePitch",q(4)}, - {"AnkleRoll", q(5)} - }; - - std::cout << "Joint Angles:\n"; - for (auto& [k,v] : joint_angles) - std::cout << " " << k << ": " << v << "\n"; - - // Test goal leg length (knee-to-foot in leg frame) - { - Mat4 T_knee = Mat4::Identity(); - T_knee.block<3,3>(0,0) = axangle2mat({0,1,0}, q(3)); - T_knee.block<3,1>(0,3) = Vec3(0, 0, -UPPER_LEG_LENGTH); - Mat4 T_lower = Mat4::Identity(); - T_lower.block<3,1>(0,3) = Vec3(0, 0, -LOWER_LEG_LENGTH); - Mat4 T_combined = T_knee * T_lower; - std::cout << "\nGoal leg length: " << T_combined.block<3,1>(0,3).norm() << "\n\n"; - } - - // FK - Mat4 T_fk = calculate_fk(joint_angles); - std::cout << "Forward Kinematics:\n" << T_fk << "\n"; - auto fk_euler = mat2euler_sxyz(T_fk.block<3,3>(0,0)); - std::cout << "Foot orientation (sxyz): " - << fk_euler[0] << ", " << fk_euler[1] << ", " << fk_euler[2] << "\n"; - std::cout << "Goal distance: " << T_fk.block<3,1>(0,3).norm() << "\n\n"; - - // IK - JointAngles q_ik = calculate_ik(T_fk, true); - std::cout << "\nInverse Kinematics Joint Angles:\n"; - for (auto& [k,v] : q_ik) - std::cout << " " << k << ": " << v << "\n"; - - // Verify FK from IK result - Mat4 T_fk_ik = calculate_fk(q_ik); - std::cout << "\nForward Kinematics from IK:\n" << T_fk_ik << "\n"; - double pos_err = (T_fk.block<3,1>(0,3) - T_fk_ik.block<3,1>(0,3)).norm(); - double rot_err = (T_fk.block<3,3>(0,0) - T_fk_ik.block<3,3>(0,0)).norm(); - std::cout << "Position error: " << pos_err << "\n"; - std::cout << "Orientation error: " << rot_err << "\n\n"; - - // ── Random test (1000 iterations) ───────────────────────────────────── - std::cout << "Running 1000 random IK tests...\n"; - std::srand(42); - std::vector errors; - int skipped = 0; - for (int iter = 0; iter < 1000; ++iter) { - Eigen::VectorXd q_rand(6); - for (int i = 0; i < 6; ++i) - q_rand(i) = (static_cast(std::rand()) / RAND_MAX * 2.0 - 1.0) * 0.4 * PI; - - JointAngles ja = { - {"HipYaw", q_rand(0)}, - {"HipRoll", q_rand(1)}, - {"HipPitch", q_rand(2)}, - {"Knee", q_rand(3)}, - {"AnklePitch",q_rand(4)}, - {"AnkleRoll", q_rand(5)} - }; - - Mat4 T_rand = calculate_fk(ja); - - JointAngles q_ik_rand; - try { - q_ik_rand = calculate_ik(T_rand, true); - } catch (const SolverError& e) { - ++skipped; - continue; - } - - Mat4 T_ik_rand = calculate_fk(q_ik_rand); - double err = (T_rand.block<3,1>(0,3) - T_ik_rand.block<3,1>(0,3)).norm() - + (T_rand.block<3,3>(0,0) - T_ik_rand.block<3,3>(0,0)).norm(); - - if (std::isnan(err)) { - std::cout << "NaN error at iteration " << iter << ", skipping.\n"; - ++skipped; - continue; - } - if (err > 0.001) { - std::cout << "High error " << err << " at iteration " << iter - << " for q = " << q_rand.transpose() << "\n"; - break; - } - errors.push_back(err); - } - - if (!errors.empty()) { - double sum = 0.0, max_err = 0.0; - for (double e : errors) { sum += e; max_err = std::max(max_err, e); } - std::cout << "Mean error: " << sum / errors.size() << "\n"; - std::cout << "Max error: " << max_err << "\n"; - } - std::cout << "Skipped: " << skipped << " / 1000\n"; - - return 0; -} diff --git a/src/bitbots_motion/wolfgang_ik/src/ros.cpp b/src/bitbots_motion/wolfgang_ik/src/ros.cpp deleted file mode 100644 index 1ac782fde..000000000 --- a/src/bitbots_motion/wolfgang_ik/src/ros.cpp +++ /dev/null @@ -1,214 +0,0 @@ -/** - * leg_ik_node.cpp - * - * ROS2 node that: - * 1. Looks up the current l_sole → base_link TF transform. - * 2. Runs the analytic IK solver on that target. - * 3. Subscribes to /joint_states and prints IK solution vs. actual joint - * angles side-by-side in a loop. - * - * Build (add to CMakeLists.txt): - * find_package(Eigen3 REQUIRED) - * find_package(rclcpp REQUIRED) - * find_package(tf2_ros REQUIRED) - * find_package(tf2_eigen REQUIRED) - * find_package(sensor_msgs REQUIRED) - * - * add_executable(leg_ik_node leg_ik_node.cpp) - * ament_target_dependencies(leg_ik_node rclcpp tf2_ros tf2_eigen sensor_msgs) - * target_include_directories(leg_ik_node PRIVATE ${EIGEN3_INCLUDE_DIRS}) - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; - -using JointAngles = std::map; - - -// ═══════════════════════════════════════════════════════════════════════════ -// ROS2 node -// ═══════════════════════════════════════════════════════════════════════════ - -// Joint names in the order we want to print them. -// Adjust these to match your robot's URDF joint names. -static const std::vector JOINT_NAMES = { - "RHipYaw", "RHipRoll", "RHipPitch", - "RKnee", - "RAnklePitch", "RAnkleRoll" -}; - -class LegIKNode : public rclcpp::Node { -public: - LegIKNode() : Node("leg_ik_node") { - // TF - tf_buffer_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - // Joint state subscriber - js_sub_ = create_subscription( - "/joint_states", rclcpp::SystemDefaultsQoS(), - [this](sensor_msgs::msg::JointState::SharedPtr msg) { - last_js_ = msg; - }); - - // Timer — runs the main loop at ~10 Hz - timer_ = create_wall_timer(100ms, [this]() { timerCb(); }); - - RCLCPP_INFO(get_logger(), "leg_ik_node started. Waiting for TF and joint states..."); - } - -private: - void timerCb() { - // ── 1. Look up l_sole → base_link ────────────────────────────────── - geometry_msgs::msg::TransformStamped ts; - try { - ts = tf_buffer_->lookupTransform( - "base_link", // target frame - "r_sole", // source frame - tf2::TimePointZero); - } catch (const tf2::TransformException& ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, - "TF lookup failed: %s", ex.what()); - return; - } - - // { - // geometry_msgs::msg::TransformStamped ts1; - // try { - // ts1 = tf_buffer_->lookupTransform( - // "l_hip_1", // target frame - // "l_foot", // source frame - // tf2::TimePointZero); - // } catch (const tf2::TransformException& ex) { - // RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, - // "TF lookup failed: %s", ex.what()); - // return; - // } - - - // std::cout << "TF lookup successful: base_link → l_foot" << ts1.transform.translation.z << "\n"; - // } - - - // Convert to Eigen 4×4 - Eigen::Isometry3d iso = tf2::transformToEigen(ts); - Mat4 T_target = iso.matrix(); - - // ── 2. Run IK ─────────────────────────────────────────────────────── - JointAngles ik_solution; - try { - bool left = true; - - - ik_solution = calculate_ik(T_target, left); - - // Apply joint offsets to match actual robot configuration (if needed) - if (left){ - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; - ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; - } else { - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; - ik_solution["Knee"] *= -1; - ik_solution["AnklePitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; - } - } catch (const SolverError& e) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "IK solver error: %s", e.what()); - return; - } - - // ── 3. Extract actual joint angles from /joint_states ─────────────── - JointAngles actual; - if (last_js_) { - for (const auto& name : JOINT_NAMES) { - auto it = std::find(last_js_->name.begin(), last_js_->name.end(), name); - if (it != last_js_->name.end()) { - size_t idx = std::distance(last_js_->name.begin(), it); - if (idx < last_js_->position.size()) - actual[name] = last_js_->position[idx]; - } - } - } - - // ── 4. Print comparison ───────────────────────────────────────────── - const int W = 12; - const double RAD2DEG = 180.0 / M_PI; - - std::cout << "\n══════════════════════════════════════════════════════\n"; - std::cout << " TF base_link → l_sole\n"; - std::cout << " pos [m]: x=" << std::fixed << std::setprecision(4) - << ts.transform.translation.x << " y=" << ts.transform.translation.y - << " z=" << ts.transform.translation.z << "\n"; - std::cout << " quat: x=" << ts.transform.rotation.x - << " y=" << ts.transform.rotation.y - << " z=" << ts.transform.rotation.z - << " w=" << ts.transform.rotation.w << "\n"; - std::cout << "──────────────────────────────────────────────────────\n"; - std::cout << std::setw(16) << "Joint" - << std::setw(W) << "IK [deg]" - << std::setw(W) << "IK [rad]" - << std::setw(W) << "Act [deg]" - << std::setw(W) << "Act [rad]" - << std::setw(W) << "Δ [deg]" << "\n"; - std::cout << "──────────────────────────────────────────────────────\n"; - - for (const auto& name : JOINT_NAMES) { - - //Remove first character (L) for printing - - double ik_rad = ik_solution.count(name.substr(1)) ? ik_solution.at(name.substr(1)) : -100.0; - double act_rad = actual.count(name) ? actual.at(name) : -100.0; - double delta = (ik_rad - act_rad) * RAD2DEG; - - std::cout << std::setw(16) << name - << std::setw(W) << std::fixed << std::setprecision(3) << ik_rad * RAD2DEG - << std::setw(W) << std::setprecision(4) << ik_rad - << std::setw(W) << std::setprecision(3) << act_rad * RAD2DEG - << std::setw(W) << std::setprecision(4) << act_rad - << std::setw(W) << std::setprecision(3) << delta << "\n"; - } - std::cout << "══════════════════════════════════════════════════════\n"; - } - - // TF - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // Joint states - rclcpp::Subscription::SharedPtr js_sub_; - sensor_msgs::msg::JointState::SharedPtr last_js_; - - // Loop timer - rclcpp::TimerBase::SharedPtr timer_; -}; - -// ─── main ────────────────────────────────────────────────────────────────── -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} From d978e605c1697c8fa629d8c973edcc997fb28922 Mon Sep 17 00:00:00 2001 From: Valerie Date: Fri, 13 Mar 2026 20:29:27 +0100 Subject: [PATCH 05/16] add ik to dynup --- .../include/bitbots_dynup/dynup_ik.hpp | 10 +- .../include/bitbots_dynup/ik.hpp | 396 ++++++++++++++++++ .../bitbots_dynup/src/dynup_ik.cpp | 138 ++++-- 3 files changed, 499 insertions(+), 45 deletions(-) create mode 100644 src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp index 6dbdfb0ac..fcb0321b2 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp @@ -4,11 +4,13 @@ #include #include #include +#include #include #include #include -#include #include +#include +#include #include "dynup_utils.hpp" @@ -23,13 +25,13 @@ class DynupIK : public bitbots_splines::AbstractIK { moveit::core::RobotStatePtr get_goal_state(); void set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state); + const std::vector getLeftLegJointNames(); + const std::vector getRightLegJointNames(); + private: rclcpp::Node::SharedPtr node_; - moveit::core::JointModelGroup* all_joints_group_; moveit::core::JointModelGroup* l_arm_joints_group_; - moveit::core::JointModelGroup* l_leg_joints_group_; moveit::core::JointModelGroup* r_arm_joints_group_; - moveit::core::JointModelGroup* r_leg_joints_group_; moveit::core::RobotStatePtr goal_state_; sensor_msgs::msg::JointState::ConstSharedPtr joint_state_; DynupDirection direction_; diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp new file mode 100644 index 000000000..f124dc979 --- /dev/null +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp @@ -0,0 +1,396 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// ── Model constants ──────────────────────────────────────────────────────── +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] + + + +// Motor offsets (left leg) +constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; +constexpr double KNEE_OFFSET = 0.26180265358979327; + + + +using Mat4 = Eigen::Matrix4d; +using Mat3 = Eigen::Matrix3d; +using Vec3 = Eigen::Vector3d; + +constexpr double BASE_LINK_SIDE_OFFSET = 0.055; +constexpr double BASE_LINK_Z_OFFSET = -0.0414; +constexpr double SOLE_X_OFFSET = 0.0216683; +constexpr double SOLE_Y_OFFSET = 0.0152219; +constexpr double SOLE_Z_OFFSET = -0.0529; + +// ── Custom exception ─────────────────────────────────────────────────────── +class SolverError : public std::runtime_error { +public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} +}; + +// ── Elementary rotation matrices ─────────────────────────────────────────── +static Mat3 Rx(double t) { + Mat3 R = Mat3::Identity(); + R(1,1) = std::cos(t); R(1,2) = -std::sin(t); + R(2,1) = std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Ry(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,2) = std::sin(t); + R(2,0) = -std::sin(t); R(2,2) = std::cos(t); + return R; +} +static Mat3 Rz(double t) { + Mat3 R = Mat3::Identity(); + R(0,0) = std::cos(t); R(0,1) = -std::sin(t); + R(1,0) = std::sin(t); R(1,1) = std::cos(t); + return R; +} + +// ── Euler composition helpers ────────────────────────────────────────────── + +/** + * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) + */ +static Mat3 euler2mat_sxyz(double a, double b, double c) { + return Rz(c) * Ry(b) * Rx(a); +} + +/** + * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) + * b = arcsin(-M[2,0]) + * a = arctan2( M[2,1], M[2,2]) + * c = arctan2( M[1,0], M[0,0]) + */ +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); + double a = std::atan2( M(2,1), M(2,2)); + double c = std::atan2( M(1,0), M(0,0)); + return {a, b, c}; +} + +/** + * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) + */ +static Mat3 euler2mat_rzxy(double a, double b, double c) { + return Rz(a) * Rx(b) * Ry(c); +} + +/** + * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) + * b = arcsin( M[2,1]) + * a = arctan2(-M[0,1], M[1,1]) + * c = arctan2(-M[2,0], M[2,2]) + */ +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); + double a = std::atan2(-M(0,1), M(1,1)); + double c = std::atan2(-M(2,0), M(2,2)); + return {a, b, c}; +} + +/** + * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) + */ +static Mat3 euler2mat_rxyz(double a, double b, double c) { + return Rx(a) * Ry(b) * Rz(c); +} + +/** + * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) + * b = arcsin( M[0,2]) + * a = arctan2(-M[1,2], M[2,2]) + * c = arctan2(-M[0,1], M[0,0]) + */ +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); + double a = std::atan2(-M(1,2), M(2,2)); + double c = std::atan2(-M(0,1), M(0,0)); + return {a, b, c}; +} + +// ── axangle2mat ──────────────────────────────────────────────────────────── +/** + * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). + */ +static Mat3 axangle2mat(Vec3 axis, double angle) { + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + K << 0, -axis(2), axis(1), + axis(2), 0, -axis(0), + -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; +} + +// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── +/** + * Returns a 4×4 transform T whose rotation aligns a reference frame to + * the given leg_vector direction. + * + * Mirrors the Python: + * roll = arctan2(v[1], v[2]) + * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) + * yaw = 0 + * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) + * = Ry(pitch)*Rx(roll) + */ +static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { + Vec3 v = leg_vector_in.normalized(); + + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError( + "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } + + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); + double yaw = 0.0; + + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); + return T; +} + +// ── Joint angle map type ─────────────────────────────────────────────────── +using JointAngles = std::map; + +// ── Forward kinematics ───────────────────────────────────────────────────── +/** + * Mirrors calculate_fk() exactly. + * Joint order (all around standard axes): + * HipYaw → Rz + * HipRoll → Rx + * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) + * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) + * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) + * AnkleRoll→ Rx + */ +static Mat4 calculate_fk(const JointAngles& q) { + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + Mat4 T = Mat4::Identity(); + + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); + + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); + + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); + + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; +} + +// ── Inverse kinematics ───────────────────────────────────────────────────── +static JointAngles calculate_ik(const Mat4& target_transform, bool left) { + JointAngles joint_angles = { + {"HipYaw", 0.0}, + {"HipRoll", 0.0}, + {"HipPitch", 0.0}, + {"Knee", 0.0}, + {"AnklePitch",0.0}, + {"AnkleRoll", 0.0} + }; + + std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3,3>(0,0) = R; + T.block<3,1>(0,3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + double virtual_leg_length = leg_vec.norm(); + std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3,3>(0,0) = + euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() + * T_hip_axis_intersection + * T_hip_pitch_origin; + std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); + std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = (real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH + + real_leg_length*real_leg_length + - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) + / (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH + + real_leg_length*real_leg_length + - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) + / (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; +} + +static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, false); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + + return ik_solution; +} + +static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, true); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + + return ik_solution; +} diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index 0511f7f8e..cf20a667e 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -6,11 +6,8 @@ DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {} void DynupIK::init(moveit::core::RobotModelPtr kinematic_model) { /* Extract joint groups from kinematics model */ - l_leg_joints_group_ = kinematic_model->getJointModelGroup("LeftLeg"); - r_leg_joints_group_ = kinematic_model->getJointModelGroup("RightLeg"); l_arm_joints_group_ = kinematic_model->getJointModelGroup("LeftArm"); r_arm_joints_group_ = kinematic_model->getJointModelGroup("RightArm"); - all_joints_group_ = kinematic_model->getJointModelGroup("All"); /* Reset kinematic goal to default */ goal_state_ = std::make_shared(kinematic_model); @@ -45,30 +42,62 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { kinematics::KinematicsQueryOptions ik_options; ik_options.return_approximate_solution = true; - geometry_msgs::msg::Pose right_foot_goal_msg, left_foot_goal_msg, right_hand_goal_msg, left_hand_goal_msg; - - tf2::toMsg(ik_goals.r_foot_goal_pose, right_foot_goal_msg); - tf2::toMsg(ik_goals.l_foot_goal_pose, left_foot_goal_msg); + geometry_msgs::msg::Transform left_foot_goal_tf, right_foot_goal_tf; + + auto transform_to_geo_tf = [](const tf2::Transform& transform) { + geometry_msgs::msg::Transform msg; + msg.translation.x = transform.getOrigin().x(); + msg.translation.y = transform.getOrigin().y(); + msg.translation.z = transform.getOrigin().z(); + msg.rotation.x = transform.getRotation().x(); + msg.rotation.y = transform.getRotation().y(); + msg.rotation.z = transform.getRotation().z(); + msg.rotation.w = transform.getRotation().w(); + return msg; + }; + + right_foot_goal_tf = transform_to_geo_tf(ik_goals.r_foot_goal_pose); + left_foot_goal_tf = transform_to_geo_tf(ik_goals.l_foot_goal_pose); + + geometry_msgs::msg::Pose right_hand_goal_msg, left_hand_goal_msg; + tf2::toMsg(ik_goals.r_hand_goal_pose, right_hand_goal_msg); tf2::toMsg(ik_goals.l_hand_goal_pose, left_hand_goal_msg); bool success; goal_state_->updateLinkTransforms(); + // Kann man den einfach löschen? Scheint ja nciht so benutzt zu werden + bio_ik::BioIKKinematicsQueryOptions leg_ik_options; leg_ik_options.return_approximate_solution = true; // Add auxiliary goal to prevent bending the knees in the wrong direction when we go from init to walkready leg_ik_options.goals.push_back(std::make_unique()); - success = goal_state_->setFromIK(l_leg_joints_group_, left_foot_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), leg_ik_options); + // here udpate to analytic IK, feet are set here + bitbots_splines::JointGoals result; - goal_state_->updateLinkTransforms(); + // call IK + Eigen::Isometry3d right_iso = tf2::transformToEigen(right_foot_goal_tf); + Mat4 T_right = right_iso.matrix(); + auto result_right = calculate_ik_right_leg(T_right); - success &= goal_state_->setFromIK(r_leg_joints_group_, right_foot_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), leg_ik_options); + for (const auto& joint : getRightLegJointNames()) { + result.first.push_back(joint); + result.second.push_back(result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint + } + + Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal_tf); + Mat4 T_left = left_iso.matrix(); + auto result_left = calculate_ik_left_leg(T_left); + + for (const auto& joint : getLeftLegJointNames()) { + result.first.push_back(joint); + result.second.push_back(result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + } + // arms if (direction_ != DynupDirection::RISE_NO_ARMS and direction_ != DynupDirection::DESCEND_NO_ARMS) { goal_state_->updateLinkTransforms(); success &= goal_state_->setFromIK(l_arm_joints_group_, left_hand_goal_msg, 0.005, @@ -79,58 +108,85 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { moveit::core::GroupStateValidityCallbackFn(), ik_options); } if (success) { - /* retrieve joint names and associated positions from */ - auto joint_names = all_joints_group_->getActiveJointModelNames(); - std::vector joint_goals; - goal_state_->copyJointGroupPositions(all_joints_group_, joint_goals); - - /* construct result object */ - bitbots_splines::JointGoals result = {joint_names, joint_goals}; // Store the name of the arm joins so we can remove them if they are not needed const auto r_arm_motors = r_arm_joints_group_->getActiveJointModelNames(); const auto l_arm_motors = l_arm_joints_group_->getActiveJointModelNames(); - /* sets head motors to correct positions, as the IK will return random values for those unconstrained motors. */ + // append right arm + for (const auto& joint : r_arm_motors) { + double value = goal_state_->getVariablePosition(joint); + result.first.push_back(joint); + result.second.push_back(value); + } + + // append left arm + for (const auto& joint : l_arm_motors) { + double value = goal_state_->getVariablePosition(joint); + result.first.push_back(joint); + result.second.push_back(value); + } + + // head joints + result.first.push_back("HeadPan"); + result.second.push_back(goal_state_->getVariablePosition("HeadPan")); + + result.first.push_back("HeadTilt"); + result.second.push_back(goal_state_->getVariablePosition("HeadTilt")); + for (size_t i = result.first.size(); i-- > 0;) { - if (result.first[i] == "HeadPan") { - if (direction_ == DynupDirection::WALKREADY) { + if (result.first[i] == "HeadPan") { + if (direction_ == DynupDirection::WALKREADY) { // remove head from the goals so that we can move it freely - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); - } else { - result.second[i] = 0; - } - } else if (result.first[i] == "HeadTilt") { - if (ik_goals.is_head_zero) { - result.second[i] = 0; + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } else { + result.second[i] = 0; + } + } else if (result.first[i] == "HeadTilt") { + if (ik_goals.is_head_zero) { + result.second[i] = 0; } else if (direction_ == DynupDirection::FRONT or direction_ == DynupDirection::FRONT_ONLY) { - result.second[i] = 1.0; + result.second[i] = 1.0; } else if (direction_ == DynupDirection::BACK or direction_ == DynupDirection::BACK_ONLY) { - result.second[i] = -1.5; - } else if (direction_ == DynupDirection::WALKREADY) { + result.second[i] = -1.5; + } else if (direction_ == DynupDirection::WALKREADY) { // remove head from the goals so that we can move it freely - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); - } else { - result.second[i] = 0; - } + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } else { + result.second[i] = 0; + } } // Remove the arm motors from the goals if we have a goal without arms else if ((std::find(r_arm_motors.begin(), r_arm_motors.end(), result.first[i]) != r_arm_motors.end() or std::find(l_arm_motors.begin(), l_arm_motors.end(), result.first[i]) != l_arm_motors.end()) and (direction_ == DynupDirection::RISE_NO_ARMS or direction_ == DynupDirection::DESCEND_NO_ARMS)) { - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); - } + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); } + } return result; } else { // node will count this as a missing tick and provide warning return {}; } } +const std::vector DynupIK::getLeftLegJointNames() { + return { + "LHipYaw", "LHipRoll", "LHipPitch", + "LKnee", + "LAnklePitch", "LAnkleRoll" + }; +} +const std::vector DynupIK::getRightLegJointNames() { + return { + "RHipYaw", "RHipRoll", "RHipPitch", + "RKnee", + "RAnklePitch", "RAnkleRoll" + }; +} moveit::core::RobotStatePtr DynupIK::get_goal_state() { return goal_state_; } void DynupIK::set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state) { From a328bdcfa68adc9209ec2ce9a9e74ce379d3d498 Mon Sep 17 00:00:00 2001 From: Valerie Date: Fri, 13 Mar 2026 21:06:52 +0100 Subject: [PATCH 06/16] fix uninitialzied memory in dynup ik --- src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index cf20a667e..f2bbeb9ca 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -64,7 +64,7 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { tf2::toMsg(ik_goals.r_hand_goal_pose, right_hand_goal_msg); tf2::toMsg(ik_goals.l_hand_goal_pose, left_hand_goal_msg); - bool success; + bool success = true; goal_state_->updateLinkTransforms(); // Kann man den einfach löschen? Scheint ja nciht so benutzt zu werden From c0f4dfd5960ad31ebbaee5e234cc9f79cffa1e2f Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Fri, 13 Mar 2026 22:50:51 +0100 Subject: [PATCH 07/16] Increase walking speeds --- .../config/path_planning_parameters.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml index 777c2c682..8b805ebc2 100644 --- a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml @@ -56,13 +56,13 @@ bitbots_path_planning: bounds<>: [0, 100] max_rotation_vel: type: double - default_value: 0.4 + default_value: 0.7 description: 'The maximum rotation velocity of the robot in rad/s around the z-axis' validation: bounds<>: [0.0, 1.0] max_vel_x: type: double - default_value: 0.12 + default_value: 0.18 description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)' validation: bounds<>: [0.0, 1.0] From 4dd4647118c8460e44d5dd6587eb2eefd3a27f37 Mon Sep 17 00:00:00 2001 From: Valerie Date: Sat, 14 Mar 2026 09:55:50 +0100 Subject: [PATCH 08/16] remove debug prints --- .../include/bitbots_dynup/ik.hpp | 22 +++++++++---------- .../include/bitbots_quintic_walk/ik.hpp | 22 +++++++++---------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp index f124dc979..71b161a95 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp @@ -252,7 +252,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { {"AnkleRoll", 0.0} }; - std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + //std::cout << "Target transformation matrix:\n" << target_transform << "\n"; auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { Mat4 T = Mat4::Identity(); @@ -274,7 +274,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); - std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + //std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; // Axis-intersection transforms (identity = at origin) Mat4 T_hip_axis_intersection = Mat4::Identity(); @@ -282,7 +282,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { // ankle-to-hip transform Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; - std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + //std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; // ankle-to-leg alignment (inverse of foot_to_leg subproblem) Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); @@ -290,19 +290,19 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { // hip-to-leg alignment Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; - std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + //std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); joint_angles["HipYaw"] = hip_euler[0]; joint_angles["HipRoll"] = hip_euler[1]; // hip_euler[2] would be HipPitch, but we compute it properly below - std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] << ", " << joint_angles["HipRoll"] << "\n"; // Virtual leg length (distance from ankle axis intersection to hip axis intersection) double virtual_leg_length = leg_vec.norm(); - std::cout << "Leg length: " << virtual_leg_length << "\n"; + //std::cout << "Leg length: " << virtual_leg_length << "\n"; // Hip transform without pitch component Mat4 T_hip_without_pitch = Mat4::Identity(); @@ -319,10 +319,10 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; - std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + //std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); - std::cout << "Real leg length: " << real_leg_length << "\n"; + //std::cout << "Real leg length: " << real_leg_length << "\n"; // Ankle angles from real-leg direction Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); @@ -332,7 +332,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); joint_angles["AnkleRoll"] = -ankle_euler[0]; joint_angles["AnklePitch"] = -ankle_euler[1]; - std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] << ", " << joint_angles["AnklePitch"] << "\n"; // Knee from cosine rule @@ -353,7 +353,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) / (2.0 * UPPER_LEG_LENGTH * real_leg_length); double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + //std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; // Ankle pitch offset (cosine rule) double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH @@ -361,7 +361,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) / (2.0 * LOWER_LEG_LENGTH * real_leg_length); double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + //std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; // Solution i=0 (matches Python solutions[0]) joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp index f124dc979..71b161a95 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp @@ -252,7 +252,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { {"AnkleRoll", 0.0} }; - std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + //std::cout << "Target transformation matrix:\n" << target_transform << "\n"; auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { Mat4 T = Mat4::Identity(); @@ -274,7 +274,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); - std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + //std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; // Axis-intersection transforms (identity = at origin) Mat4 T_hip_axis_intersection = Mat4::Identity(); @@ -282,7 +282,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { // ankle-to-hip transform Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; - std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + //std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; // ankle-to-leg alignment (inverse of foot_to_leg subproblem) Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); @@ -290,19 +290,19 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { // hip-to-leg alignment Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; - std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + //std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); joint_angles["HipYaw"] = hip_euler[0]; joint_angles["HipRoll"] = hip_euler[1]; // hip_euler[2] would be HipPitch, but we compute it properly below - std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] << ", " << joint_angles["HipRoll"] << "\n"; // Virtual leg length (distance from ankle axis intersection to hip axis intersection) double virtual_leg_length = leg_vec.norm(); - std::cout << "Leg length: " << virtual_leg_length << "\n"; + //std::cout << "Leg length: " << virtual_leg_length << "\n"; // Hip transform without pitch component Mat4 T_hip_without_pitch = Mat4::Identity(); @@ -319,10 +319,10 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; - std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + //std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); - std::cout << "Real leg length: " << real_leg_length << "\n"; + //std::cout << "Real leg length: " << real_leg_length << "\n"; // Ankle angles from real-leg direction Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); @@ -332,7 +332,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); joint_angles["AnkleRoll"] = -ankle_euler[0]; joint_angles["AnklePitch"] = -ankle_euler[1]; - std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] << ", " << joint_angles["AnklePitch"] << "\n"; // Knee from cosine rule @@ -353,7 +353,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) / (2.0 * UPPER_LEG_LENGTH * real_leg_length); double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + //std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; // Ankle pitch offset (cosine rule) double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH @@ -361,7 +361,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) / (2.0 * LOWER_LEG_LENGTH * real_leg_length); double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); - std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + //std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; // Solution i=0 (matches Python solutions[0]) joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; From cfca64730bc7b35928be825d721d464d9fb220c1 Mon Sep 17 00:00:00 2001 From: Valerie Date: Sat, 14 Mar 2026 10:04:42 +0100 Subject: [PATCH 09/16] Fix comments --- .../bitbots_dynup/include/bitbots_dynup/ik.hpp | 10 +++++----- .../include/bitbots_quintic_walk/ik.hpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp index 71b161a95..d6954ba04 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp @@ -155,9 +155,9 @@ static Mat3 axangle2mat(Vec3 axis, double angle) { axis /= n; // Rodrigues' formula Mat3 K; - K << 0, -axis(2), axis(1), - axis(2), 0, -axis(0), - -axis(1), axis(0), 0; + //std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; } @@ -298,7 +298,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { joint_angles["HipRoll"] = hip_euler[1]; // hip_euler[2] would be HipPitch, but we compute it properly below //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] - << ", " << joint_angles["HipRoll"] << "\n"; + // << ", " << joint_angles["HipRoll"] << "\n"; // Virtual leg length (distance from ankle axis intersection to hip axis intersection) double virtual_leg_length = leg_vec.norm(); @@ -333,7 +333,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { joint_angles["AnkleRoll"] = -ankle_euler[0]; joint_angles["AnklePitch"] = -ankle_euler[1]; //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] - << ", " << joint_angles["AnklePitch"] << "\n"; + // << ", " << joint_angles["AnklePitch"] << "\n"; // Knee from cosine rule double knee_arg = (real_leg_length*real_leg_length diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp index 71b161a95..d6954ba04 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp @@ -155,9 +155,9 @@ static Mat3 axangle2mat(Vec3 axis, double angle) { axis /= n; // Rodrigues' formula Mat3 K; - K << 0, -axis(2), axis(1), - axis(2), 0, -axis(0), - -axis(1), axis(0), 0; + //std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; } @@ -298,7 +298,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { joint_angles["HipRoll"] = hip_euler[1]; // hip_euler[2] would be HipPitch, but we compute it properly below //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] - << ", " << joint_angles["HipRoll"] << "\n"; + // << ", " << joint_angles["HipRoll"] << "\n"; // Virtual leg length (distance from ankle axis intersection to hip axis intersection) double virtual_leg_length = leg_vec.norm(); @@ -333,7 +333,7 @@ static JointAngles calculate_ik(const Mat4& target_transform, bool left) { joint_angles["AnkleRoll"] = -ankle_euler[0]; joint_angles["AnklePitch"] = -ankle_euler[1]; //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] - << ", " << joint_angles["AnklePitch"] << "\n"; + // << ", " << joint_angles["AnklePitch"] << "\n"; // Knee from cosine rule double knee_arg = (real_leg_length*real_leg_length From 6cdfc446211adcd8c4e6c221351771e46a996199 Mon Sep 17 00:00:00 2001 From: Valerie Date: Sat, 14 Mar 2026 10:13:50 +0100 Subject: [PATCH 10/16] add path planning default parameters --- .../config/path_planning_parameters_amy.yaml | 0 .../config/path_planning_parameters_default.yaml | 0 .../config/path_planning_parameters_donna.yaml | 0 .../config/path_planning_parameters_jack.yaml | 5 +++++ .../config/path_planning_parameters_rory.yaml | 0 ...arameters.yaml => path_planning_parameters_template.yaml} | 4 ++-- .../bitbots_path_planning/launch/path_planning.launch | 1 + src/bitbots_navigation/bitbots_path_planning/setup.py | 2 +- 8 files changed, 9 insertions(+), 3 deletions(-) create mode 100644 src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_amy.yaml create mode 100644 src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_default.yaml create mode 100644 src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_donna.yaml create mode 100644 src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml create mode 100644 src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_rory.yaml rename src/bitbots_navigation/bitbots_path_planning/config/{path_planning_parameters.yaml => path_planning_parameters_template.yaml} (98%) diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_amy.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_amy.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_default.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_default.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_donna.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_donna.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml new file mode 100644 index 000000000..772a3140a --- /dev/null +++ b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml @@ -0,0 +1,5 @@ +bitbots_path_planning: + ros__parameters: + controller: + max_rotation_vel: 0.7 + max_vel_x: 0.18 diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_rory.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_rory.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml similarity index 98% rename from src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml rename to src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml index 8b805ebc2..520feb7f4 100644 --- a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml +++ b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml @@ -56,13 +56,13 @@ bitbots_path_planning: bounds<>: [0, 100] max_rotation_vel: type: double - default_value: 0.7 + default_value: 0.3 description: 'The maximum rotation velocity of the robot in rad/s around the z-axis' validation: bounds<>: [0.0, 1.0] max_vel_x: type: double - default_value: 0.18 + default_value: 0.14 description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)' validation: bounds<>: [0.0, 1.0] diff --git a/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch index 0774f196a..043ddf64b 100755 --- a/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch +++ b/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch @@ -3,5 +3,6 @@ + diff --git a/src/bitbots_navigation/bitbots_path_planning/setup.py b/src/bitbots_navigation/bitbots_path_planning/setup.py index d18b72a7e..dd2b07cfa 100644 --- a/src/bitbots_navigation/bitbots_path_planning/setup.py +++ b/src/bitbots_navigation/bitbots_path_planning/setup.py @@ -5,7 +5,7 @@ generate_parameter_module( "path_planning_parameters", # python module name for parameter library - "config/path_planning_parameters.yaml", # path to input yaml file + "config/path_planning_parameters_template.yaml", # path to input yaml file ) package_name = "bitbots_path_planning" From 989616ddecde0ba4792e0ed990d7879dbc6cff57 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Sat, 14 Mar 2026 11:55:26 +0100 Subject: [PATCH 11/16] fix(walking): improve rory trunk offset --- .../bitbots_quintic_walk/config/robots/rory.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml b/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml index 7c48aea0f..572e7ad8b 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml +++ b/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml @@ -3,5 +3,4 @@ walking: ros__parameters: engine: - trunk_x_offset: -0.005 - + trunk_x_offset: -0.01 From 14fd961eb61b50e7c6b50e950c3983ccdd5f5029 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 14 Mar 2026 12:15:29 +0100 Subject: [PATCH 12/16] Formatting --- .../include/bitbots_dynup/dynup_ik.hpp | 6 +- .../include/bitbots_dynup/ik.hpp | 442 +++++++++--------- .../bitbots_dynup/src/dynup_ik.cpp | 71 ++- .../include/bitbots_quintic_walk/ik.hpp | 442 +++++++++--------- .../include/bitbots_quintic_walk/walk_ik.hpp | 6 +- .../bitbots_quintic_walk/src/walk_ik.cpp | 29 +- .../bitbots_quintic_walk/src/walk_node.cpp | 8 +- 7 files changed, 475 insertions(+), 529 deletions(-) diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp index fcb0321b2..0a14f6f95 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp @@ -3,14 +3,14 @@ #include #include -#include #include +#include #include #include #include -#include -#include #include +#include +#include #include "dynup_utils.hpp" diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp index d6954ba04..de851a990 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp @@ -35,18 +35,14 @@ #include // ── Model constants ──────────────────────────────────────────────────────── -constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] -constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] -constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] - - +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] // Motor offsets (left leg) constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; constexpr double KNEE_OFFSET = 0.26180265358979327; - - using Mat4 = Eigen::Matrix4d; using Mat3 = Eigen::Matrix3d; using Vec3 = Eigen::Vector3d; @@ -59,28 +55,34 @@ constexpr double SOLE_Z_OFFSET = -0.0529; // ── Custom exception ─────────────────────────────────────────────────────── class SolverError : public std::runtime_error { -public: - explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} + public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} }; // ── Elementary rotation matrices ─────────────────────────────────────────── static Mat3 Rx(double t) { - Mat3 R = Mat3::Identity(); - R(1,1) = std::cos(t); R(1,2) = -std::sin(t); - R(2,1) = std::sin(t); R(2,2) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(1, 1) = std::cos(t); + R(1, 2) = -std::sin(t); + R(2, 1) = std::sin(t); + R(2, 2) = std::cos(t); + return R; } static Mat3 Ry(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,2) = std::sin(t); - R(2,0) = -std::sin(t); R(2,2) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 2) = std::sin(t); + R(2, 0) = -std::sin(t); + R(2, 2) = std::cos(t); + return R; } static Mat3 Rz(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,1) = -std::sin(t); - R(1,0) = std::sin(t); R(1,1) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 1) = -std::sin(t); + R(1, 0) = std::sin(t); + R(1, 1) = std::cos(t); + return R; } // ── Euler composition helpers ────────────────────────────────────────────── @@ -88,9 +90,7 @@ static Mat3 Rz(double t) { /** * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) */ -static Mat3 euler2mat_sxyz(double a, double b, double c) { - return Rz(c) * Ry(b) * Rx(a); -} +static Mat3 euler2mat_sxyz(double a, double b, double c) { return Rz(c) * Ry(b) * Rx(a); } /** * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) @@ -98,19 +98,17 @@ static Mat3 euler2mat_sxyz(double a, double b, double c) { * a = arctan2( M[2,1], M[2,2]) * c = arctan2( M[1,0], M[0,0]) */ -static std::array mat2euler_sxyz(const Mat3& M) { - double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); - double a = std::atan2( M(2,1), M(2,2)); - double c = std::atan2( M(1,0), M(0,0)); - return {a, b, c}; +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2, 0), -1.0, 1.0)); + double a = std::atan2(M(2, 1), M(2, 2)); + double c = std::atan2(M(1, 0), M(0, 0)); + return {a, b, c}; } /** * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) */ -static Mat3 euler2mat_rzxy(double a, double b, double c) { - return Rz(a) * Rx(b) * Ry(c); -} +static Mat3 euler2mat_rzxy(double a, double b, double c) { return Rz(a) * Rx(b) * Ry(c); } /** * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) @@ -118,19 +116,17 @@ static Mat3 euler2mat_rzxy(double a, double b, double c) { * a = arctan2(-M[0,1], M[1,1]) * c = arctan2(-M[2,0], M[2,2]) */ -static std::array mat2euler_rzxy(const Mat3& M) { - double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); - double a = std::atan2(-M(0,1), M(1,1)); - double c = std::atan2(-M(2,0), M(2,2)); - return {a, b, c}; +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2, 1), -1.0, 1.0)); + double a = std::atan2(-M(0, 1), M(1, 1)); + double c = std::atan2(-M(2, 0), M(2, 2)); + return {a, b, c}; } /** * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) */ -static Mat3 euler2mat_rxyz(double a, double b, double c) { - return Rx(a) * Ry(b) * Rz(c); -} +static Mat3 euler2mat_rxyz(double a, double b, double c) { return Rx(a) * Ry(b) * Rz(c); } /** * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) @@ -138,11 +134,11 @@ static Mat3 euler2mat_rxyz(double a, double b, double c) { * a = arctan2(-M[1,2], M[2,2]) * c = arctan2(-M[0,1], M[0,0]) */ -static std::array mat2euler_rxyz(const Mat3& M) { - double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); - double a = std::atan2(-M(1,2), M(2,2)); - double c = std::atan2(-M(0,1), M(0,0)); - return {a, b, c}; +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0, 2), -1.0, 1.0)); + double a = std::atan2(-M(1, 2), M(2, 2)); + double c = std::atan2(-M(0, 1), M(0, 0)); + return {a, b, c}; } // ── axangle2mat ──────────────────────────────────────────────────────────── @@ -150,15 +146,15 @@ static std::array mat2euler_rxyz(const Mat3& M) { * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). */ static Mat3 axangle2mat(Vec3 axis, double angle) { - double n = axis.norm(); - if (n < 1e-12) return Mat3::Identity(); - axis /= n; - // Rodrigues' formula - Mat3 K; - //std::cout << K << 0, -axis(2), axis(1), - // axis(2), 0, -axis(0), - // -axis(1), axis(0), 0; - return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + // std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; } // ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── @@ -174,20 +170,19 @@ static Mat3 axangle2mat(Vec3 axis, double angle) { * = Ry(pitch)*Rx(roll) */ static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { - Vec3 v = leg_vector_in.normalized(); + Vec3 v = leg_vector_in.normalized(); - if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { - throw SolverError( - "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); - } + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError("Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } - double roll = std::atan2(v(1), v(2)); - double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); - double yaw = 0.0; + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1) * v(1) + v(2) * v(2))); + double yaw = 0.0; - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); - return T; + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = euler2mat_sxyz(roll, pitch, yaw); + return T; } // ── Joint angle map type ─────────────────────────────────────────────────── @@ -205,192 +200,181 @@ using JointAngles = std::map; * AnkleRoll→ Rx */ static Mat4 calculate_fk(const JointAngles& q) { - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; - // Base link transform (translation from base_link to hip axis intersection) - T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + Mat4 T = Mat4::Identity(); - // HipYaw: rotation about Z - T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); - // HipRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0, 0, 1}, q.at("HipYaw"))); - // HipPitch: rotation about Y + offset along x - T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("HipRoll"))); - // Knee: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0, 1, 0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); - // AnklePitch: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); - // AnkleRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); - // End effector offset - T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("AnkleRoll"))); - return T; + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; } // ── Inverse kinematics ───────────────────────────────────────────────────── static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - JointAngles joint_angles = { - {"HipYaw", 0.0}, - {"HipRoll", 0.0}, - {"HipPitch", 0.0}, - {"Knee", 0.0}, - {"AnklePitch",0.0}, - {"AnkleRoll", 0.0} - }; - - //std::cout << "Target transformation matrix:\n" << target_transform << "\n"; - - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - - // Remove the ends of the kinematic chain up to the first and last joint - Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; - - if (left) { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } else { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } - - auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); - - //std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; - - // Axis-intersection transforms (identity = at origin) - Mat4 T_hip_axis_intersection = Mat4::Identity(); - Mat4 T_ankle_axis_intersection = target_transform_leg_only; - - // ankle-to-hip transform - Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; - //std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; - - // ankle-to-leg alignment (inverse of foot_to_leg subproblem) - Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); - Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); - - // hip-to-leg alignment - Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; - //std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; - - // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) - auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); - joint_angles["HipYaw"] = hip_euler[0]; - joint_angles["HipRoll"] = hip_euler[1]; - // hip_euler[2] would be HipPitch, but we compute it properly below - //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] - // << ", " << joint_angles["HipRoll"] << "\n"; - - // Virtual leg length (distance from ankle axis intersection to hip axis intersection) - double virtual_leg_length = leg_vec.norm(); - //std::cout << "Leg length: " << virtual_leg_length << "\n"; - - // Hip transform without pitch component - Mat4 T_hip_without_pitch = Mat4::Identity(); - T_hip_without_pitch.block<3,3>(0,0) = - euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); - - // Hip pitch offset transform - Mat4 T_hip_pitch_offset = Mat4::Identity(); - T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); - - Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; - - // Real leg: from ankle to hip-pitch origin - Mat4 T_real_leg = T_ankle_axis_intersection.inverse() - * T_hip_axis_intersection - * T_hip_pitch_origin; - //std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; - - double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); - //std::cout << "Real leg length: " << real_leg_length << "\n"; - - // Ankle angles from real-leg direction - Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); - Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); - - // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) - auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); - joint_angles["AnkleRoll"] = -ankle_euler[0]; - joint_angles["AnklePitch"] = -ankle_euler[1]; - //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] - // << ", " << joint_angles["AnklePitch"] << "\n"; - - // Knee from cosine rule - double knee_arg = (real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); - joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); - - // HipPitch base from rzxy[2] - Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; - auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); - double hip_pitch_base = hip_euler_real[2]; - - // Hip pitch offset (cosine rule) - double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - + real_leg_length*real_leg_length - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * real_leg_length); - double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); - //std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; - - // Ankle pitch offset (cosine rule) - double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH - + real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) - / (2.0 * LOWER_LEG_LENGTH * real_leg_length); - double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); - //std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; - - // Solution i=0 (matches Python solutions[0]) - joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; - joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract - - return joint_angles; + JointAngles joint_angles = {{"HipYaw", 0.0}, {"HipRoll", 0.0}, {"HipPitch", 0.0}, + {"Knee", 0.0}, {"AnklePitch", 0.0}, {"AnkleRoll", 0.0}}; + + // std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = + (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + // std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + // std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3, 1>(0, 3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + // std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3, 3>(0, 0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + // std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + // << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + // double virtual_leg_length = leg_vec.norm(); + // std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3, 3>(0, 0) = euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3, 1>(0, 3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; + // std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3, 1>(0, 3).norm(); + // std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3, 1>(0, 3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3, 3>(0, 0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + // std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + // << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = + (real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3, 3>(0, 0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = + (UPPER_LEG_LENGTH * UPPER_LEG_LENGTH + real_leg_length * real_leg_length - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = + (LOWER_LEG_LENGTH * LOWER_LEG_LENGTH + real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH) / + (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; } static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { - auto ik_solution = calculate_ik(target_transform, false); + auto ik_solution = calculate_ik(target_transform, false); - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; - ik_solution["Knee"] *= -1; - ik_solution["AnklePitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; - return ik_solution; + return ik_solution; } static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { - auto ik_solution = calculate_ik(target_transform, true); + auto ik_solution = calculate_ik(target_transform, true); - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; - ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; - return ik_solution; + return ik_solution; } diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index f2bbeb9ca..0f3e9f336 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -43,7 +43,7 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { ik_options.return_approximate_solution = true; geometry_msgs::msg::Transform left_foot_goal_tf, right_foot_goal_tf; - + auto transform_to_geo_tf = [](const tf2::Transform& transform) { geometry_msgs::msg::Transform msg; msg.translation.x = transform.getOrigin().x(); @@ -55,12 +55,12 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { msg.rotation.w = transform.getRotation().w(); return msg; }; - + right_foot_goal_tf = transform_to_geo_tf(ik_goals.r_foot_goal_pose); left_foot_goal_tf = transform_to_geo_tf(ik_goals.l_foot_goal_pose); - + geometry_msgs::msg::Pose right_hand_goal_msg, left_hand_goal_msg; - + tf2::toMsg(ik_goals.r_hand_goal_pose, right_hand_goal_msg); tf2::toMsg(ik_goals.l_hand_goal_pose, left_hand_goal_msg); @@ -85,7 +85,8 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { for (const auto& joint : getRightLegJointNames()) { result.first.push_back(joint); - result.second.push_back(result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint + result.second.push_back( + result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint } Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal_tf); @@ -94,10 +95,11 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { for (const auto& joint : getLeftLegJointNames()) { result.first.push_back(joint); - result.second.push_back(result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + result.second.push_back( + result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint } - // arms + // arms if (direction_ != DynupDirection::RISE_NO_ARMS and direction_ != DynupDirection::DESCEND_NO_ARMS) { goal_state_->updateLinkTransforms(); success &= goal_state_->setFromIK(l_arm_joints_group_, left_hand_goal_msg, 0.005, @@ -108,7 +110,6 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { moveit::core::GroupStateValidityCallbackFn(), ik_options); } if (success) { - // Store the name of the arm joins so we can remove them if they are not needed const auto r_arm_motors = r_arm_joints_group_->getActiveJointModelNames(); const auto l_arm_motors = l_arm_joints_group_->getActiveJointModelNames(); @@ -135,37 +136,37 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { result.second.push_back(goal_state_->getVariablePosition("HeadTilt")); for (size_t i = result.first.size(); i-- > 0;) { - if (result.first[i] == "HeadPan") { - if (direction_ == DynupDirection::WALKREADY) { + if (result.first[i] == "HeadPan") { + if (direction_ == DynupDirection::WALKREADY) { // remove head from the goals so that we can move it freely - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); - } else { - result.second[i] = 0; - } - } else if (result.first[i] == "HeadTilt") { - if (ik_goals.is_head_zero) { - result.second[i] = 0; + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } else { + result.second[i] = 0; + } + } else if (result.first[i] == "HeadTilt") { + if (ik_goals.is_head_zero) { + result.second[i] = 0; } else if (direction_ == DynupDirection::FRONT or direction_ == DynupDirection::FRONT_ONLY) { - result.second[i] = 1.0; + result.second[i] = 1.0; } else if (direction_ == DynupDirection::BACK or direction_ == DynupDirection::BACK_ONLY) { - result.second[i] = -1.5; - } else if (direction_ == DynupDirection::WALKREADY) { + result.second[i] = -1.5; + } else if (direction_ == DynupDirection::WALKREADY) { // remove head from the goals so that we can move it freely - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); - } else { - result.second[i] = 0; - } + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } else { + result.second[i] = 0; + } } // Remove the arm motors from the goals if we have a goal without arms else if ((std::find(r_arm_motors.begin(), r_arm_motors.end(), result.first[i]) != r_arm_motors.end() or std::find(l_arm_motors.begin(), l_arm_motors.end(), result.first[i]) != l_arm_motors.end()) and (direction_ == DynupDirection::RISE_NO_ARMS or direction_ == DynupDirection::DESCEND_NO_ARMS)) { - result.first.erase(result.first.begin() + i); - result.second.erase(result.second.begin() + i); + result.first.erase(result.first.begin() + i); + result.second.erase(result.second.begin() + i); + } } - } return result; } else { // node will count this as a missing tick and provide warning @@ -173,19 +174,11 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { } } const std::vector DynupIK::getLeftLegJointNames() { - return { - "LHipYaw", "LHipRoll", "LHipPitch", - "LKnee", - "LAnklePitch", "LAnkleRoll" - }; + return {"LHipYaw", "LHipRoll", "LHipPitch", "LKnee", "LAnklePitch", "LAnkleRoll"}; } const std::vector DynupIK::getRightLegJointNames() { - return { - "RHipYaw", "RHipRoll", "RHipPitch", - "RKnee", - "RAnklePitch", "RAnkleRoll" - }; + return {"RHipYaw", "RHipRoll", "RHipPitch", "RKnee", "RAnklePitch", "RAnkleRoll"}; } moveit::core::RobotStatePtr DynupIK::get_goal_state() { return goal_state_; } diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp index d6954ba04..de851a990 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp @@ -35,18 +35,14 @@ #include // ── Model constants ──────────────────────────────────────────────────────── -constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] -constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] -constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] - - +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] // Motor offsets (left leg) constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; constexpr double KNEE_OFFSET = 0.26180265358979327; - - using Mat4 = Eigen::Matrix4d; using Mat3 = Eigen::Matrix3d; using Vec3 = Eigen::Vector3d; @@ -59,28 +55,34 @@ constexpr double SOLE_Z_OFFSET = -0.0529; // ── Custom exception ─────────────────────────────────────────────────────── class SolverError : public std::runtime_error { -public: - explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} + public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} }; // ── Elementary rotation matrices ─────────────────────────────────────────── static Mat3 Rx(double t) { - Mat3 R = Mat3::Identity(); - R(1,1) = std::cos(t); R(1,2) = -std::sin(t); - R(2,1) = std::sin(t); R(2,2) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(1, 1) = std::cos(t); + R(1, 2) = -std::sin(t); + R(2, 1) = std::sin(t); + R(2, 2) = std::cos(t); + return R; } static Mat3 Ry(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,2) = std::sin(t); - R(2,0) = -std::sin(t); R(2,2) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 2) = std::sin(t); + R(2, 0) = -std::sin(t); + R(2, 2) = std::cos(t); + return R; } static Mat3 Rz(double t) { - Mat3 R = Mat3::Identity(); - R(0,0) = std::cos(t); R(0,1) = -std::sin(t); - R(1,0) = std::sin(t); R(1,1) = std::cos(t); - return R; + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 1) = -std::sin(t); + R(1, 0) = std::sin(t); + R(1, 1) = std::cos(t); + return R; } // ── Euler composition helpers ────────────────────────────────────────────── @@ -88,9 +90,7 @@ static Mat3 Rz(double t) { /** * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) */ -static Mat3 euler2mat_sxyz(double a, double b, double c) { - return Rz(c) * Ry(b) * Rx(a); -} +static Mat3 euler2mat_sxyz(double a, double b, double c) { return Rz(c) * Ry(b) * Rx(a); } /** * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) @@ -98,19 +98,17 @@ static Mat3 euler2mat_sxyz(double a, double b, double c) { * a = arctan2( M[2,1], M[2,2]) * c = arctan2( M[1,0], M[0,0]) */ -static std::array mat2euler_sxyz(const Mat3& M) { - double b = std::asin(std::clamp(-M(2,0), -1.0, 1.0)); - double a = std::atan2( M(2,1), M(2,2)); - double c = std::atan2( M(1,0), M(0,0)); - return {a, b, c}; +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2, 0), -1.0, 1.0)); + double a = std::atan2(M(2, 1), M(2, 2)); + double c = std::atan2(M(1, 0), M(0, 0)); + return {a, b, c}; } /** * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) */ -static Mat3 euler2mat_rzxy(double a, double b, double c) { - return Rz(a) * Rx(b) * Ry(c); -} +static Mat3 euler2mat_rzxy(double a, double b, double c) { return Rz(a) * Rx(b) * Ry(c); } /** * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) @@ -118,19 +116,17 @@ static Mat3 euler2mat_rzxy(double a, double b, double c) { * a = arctan2(-M[0,1], M[1,1]) * c = arctan2(-M[2,0], M[2,2]) */ -static std::array mat2euler_rzxy(const Mat3& M) { - double b = std::asin(std::clamp(M(2,1), -1.0, 1.0)); - double a = std::atan2(-M(0,1), M(1,1)); - double c = std::atan2(-M(2,0), M(2,2)); - return {a, b, c}; +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2, 1), -1.0, 1.0)); + double a = std::atan2(-M(0, 1), M(1, 1)); + double c = std::atan2(-M(2, 0), M(2, 2)); + return {a, b, c}; } /** * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) */ -static Mat3 euler2mat_rxyz(double a, double b, double c) { - return Rx(a) * Ry(b) * Rz(c); -} +static Mat3 euler2mat_rxyz(double a, double b, double c) { return Rx(a) * Ry(b) * Rz(c); } /** * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) @@ -138,11 +134,11 @@ static Mat3 euler2mat_rxyz(double a, double b, double c) { * a = arctan2(-M[1,2], M[2,2]) * c = arctan2(-M[0,1], M[0,0]) */ -static std::array mat2euler_rxyz(const Mat3& M) { - double b = std::asin(std::clamp(M(0,2), -1.0, 1.0)); - double a = std::atan2(-M(1,2), M(2,2)); - double c = std::atan2(-M(0,1), M(0,0)); - return {a, b, c}; +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0, 2), -1.0, 1.0)); + double a = std::atan2(-M(1, 2), M(2, 2)); + double c = std::atan2(-M(0, 1), M(0, 0)); + return {a, b, c}; } // ── axangle2mat ──────────────────────────────────────────────────────────── @@ -150,15 +146,15 @@ static std::array mat2euler_rxyz(const Mat3& M) { * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). */ static Mat3 axangle2mat(Vec3 axis, double angle) { - double n = axis.norm(); - if (n < 1e-12) return Mat3::Identity(); - axis /= n; - // Rodrigues' formula - Mat3 K; - //std::cout << K << 0, -axis(2), axis(1), - // axis(2), 0, -axis(0), - // -axis(1), axis(0), 0; - return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + // std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; } // ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── @@ -174,20 +170,19 @@ static Mat3 axangle2mat(Vec3 axis, double angle) { * = Ry(pitch)*Rx(roll) */ static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { - Vec3 v = leg_vector_in.normalized(); + Vec3 v = leg_vector_in.normalized(); - if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { - throw SolverError( - "Leg vector is too close to the x or y axis, cannot determine roll and pitch."); - } + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError("Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } - double roll = std::atan2(v(1), v(2)); - double pitch = std::atan2(-v(0), std::sqrt(v(1)*v(1) + v(2)*v(2))); - double yaw = 0.0; + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1) * v(1) + v(2) * v(2))); + double yaw = 0.0; - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = euler2mat_sxyz(roll, pitch, yaw); - return T; + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = euler2mat_sxyz(roll, pitch, yaw); + return T; } // ── Joint angle map type ─────────────────────────────────────────────────── @@ -205,192 +200,181 @@ using JointAngles = std::map; * AnkleRoll→ Rx */ static Mat4 calculate_fk(const JointAngles& q) { - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; - // Base link transform (translation from base_link to hip axis intersection) - T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + Mat4 T = Mat4::Identity(); - // HipYaw: rotation about Z - T = T * make_T(axangle2mat({0,0,1}, q.at("HipYaw"))); + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); - // HipRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("HipRoll"))); + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0, 0, 1}, q.at("HipYaw"))); - // HipPitch: rotation about Y + offset along x - T = T * make_T(axangle2mat({0,1,0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("HipRoll"))); - // Knee: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0, 1, 0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); - // AnklePitch: rotation about Y + translation down z - T = T * make_T(axangle2mat({0,1,0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); - // AnkleRoll: rotation about X - T = T * make_T(axangle2mat({1,0,0}, q.at("AnkleRoll"))); + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); - // End effector offset - T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("AnkleRoll"))); - return T; + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; } // ── Inverse kinematics ───────────────────────────────────────────────────── static JointAngles calculate_ik(const Mat4& target_transform, bool left) { - JointAngles joint_angles = { - {"HipYaw", 0.0}, - {"HipRoll", 0.0}, - {"HipPitch", 0.0}, - {"Knee", 0.0}, - {"AnklePitch",0.0}, - {"AnkleRoll", 0.0} - }; - - //std::cout << "Target transformation matrix:\n" << target_transform << "\n"; - - auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { - Mat4 T = Mat4::Identity(); - T.block<3,3>(0,0) = R; - T.block<3,1>(0,3) = t; - return T; - }; - - // Remove the ends of the kinematic chain up to the first and last joint - Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; - - if (left) { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } else { - T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); - T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); - } - - auto target_transform_leg_only = (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); - - //std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; - - // Axis-intersection transforms (identity = at origin) - Mat4 T_hip_axis_intersection = Mat4::Identity(); - Mat4 T_ankle_axis_intersection = target_transform_leg_only; - - // ankle-to-hip transform - Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; - //std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; - - // ankle-to-leg alignment (inverse of foot_to_leg subproblem) - Vec3 leg_vec = T_ankle_to_hip.block<3,1>(0,3); - Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); - - // hip-to-leg alignment - Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; - //std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; - - // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) - auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3,3>(0,0)); - joint_angles["HipYaw"] = hip_euler[0]; - joint_angles["HipRoll"] = hip_euler[1]; - // hip_euler[2] would be HipPitch, but we compute it properly below - //std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] - // << ", " << joint_angles["HipRoll"] << "\n"; - - // Virtual leg length (distance from ankle axis intersection to hip axis intersection) - double virtual_leg_length = leg_vec.norm(); - //std::cout << "Leg length: " << virtual_leg_length << "\n"; - - // Hip transform without pitch component - Mat4 T_hip_without_pitch = Mat4::Identity(); - T_hip_without_pitch.block<3,3>(0,0) = - euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); - - // Hip pitch offset transform - Mat4 T_hip_pitch_offset = Mat4::Identity(); - T_hip_pitch_offset.block<3,1>(0,3) = Vec3(HIP_PITCH_OFFSET, 0, 0); - - Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; - - // Real leg: from ankle to hip-pitch origin - Mat4 T_real_leg = T_ankle_axis_intersection.inverse() - * T_hip_axis_intersection - * T_hip_pitch_origin; - //std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; - - double real_leg_length = T_real_leg.block<3,1>(0,3).norm(); - //std::cout << "Real leg length: " << real_leg_length << "\n"; - - // Ankle angles from real-leg direction - Vec3 real_leg_vec = T_real_leg.block<3,1>(0,3); - Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); - - // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) - auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3,3>(0,0)); - joint_angles["AnkleRoll"] = -ankle_euler[0]; - joint_angles["AnklePitch"] = -ankle_euler[1]; - //std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] - // << ", " << joint_angles["AnklePitch"] << "\n"; - - // Knee from cosine rule - double knee_arg = (real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); - joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); - - // HipPitch base from rzxy[2] - Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; - auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3,3>(0,0)); - double hip_pitch_base = hip_euler_real[2]; - - // Hip pitch offset (cosine rule) - double hip_pitch_offset_arg = (UPPER_LEG_LENGTH*UPPER_LEG_LENGTH - + real_leg_length*real_leg_length - - LOWER_LEG_LENGTH*LOWER_LEG_LENGTH) - / (2.0 * UPPER_LEG_LENGTH * real_leg_length); - double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); - //std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; - - // Ankle pitch offset (cosine rule) - double ankle_pitch_offset_arg = (LOWER_LEG_LENGTH*LOWER_LEG_LENGTH - + real_leg_length*real_leg_length - - UPPER_LEG_LENGTH*UPPER_LEG_LENGTH) - / (2.0 * LOWER_LEG_LENGTH * real_leg_length); - double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); - //std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; - - // Solution i=0 (matches Python solutions[0]) - joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; - joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract - - return joint_angles; + JointAngles joint_angles = {{"HipYaw", 0.0}, {"HipRoll", 0.0}, {"HipPitch", 0.0}, + {"Knee", 0.0}, {"AnklePitch", 0.0}, {"AnkleRoll", 0.0}}; + + // std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = + (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + // std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + // std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3, 1>(0, 3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + // std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3, 3>(0, 0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + // std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + // << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + // double virtual_leg_length = leg_vec.norm(); + // std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3, 3>(0, 0) = euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3, 1>(0, 3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; + // std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3, 1>(0, 3).norm(); + // std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3, 1>(0, 3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3, 3>(0, 0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + // std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + // << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = + (real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3, 3>(0, 0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = + (UPPER_LEG_LENGTH * UPPER_LEG_LENGTH + real_leg_length * real_leg_length - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = + (LOWER_LEG_LENGTH * LOWER_LEG_LENGTH + real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH) / + (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; } static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { - auto ik_solution = calculate_ik(target_transform, false); + auto ik_solution = calculate_ik(target_transform, false); - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; - ik_solution["Knee"] *= -1; - ik_solution["AnklePitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; - return ik_solution; + return ik_solution; } static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { - auto ik_solution = calculate_ik(target_transform, true); + auto ik_solution = calculate_ik(target_transform, true); - ik_solution["HipYaw"] *= -1; - ik_solution["HipPitch"] *= -1; - ik_solution["AnkleRoll"] *= -1; - ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; - ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; - return ik_solution; + return ik_solution; } diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp index c6c2a73ac..4db6ba5b9 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp @@ -1,12 +1,12 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #include -#include #include +#include #include -#include -#include #include +#include +#include namespace bitbots_quintic_walk { diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp index 2e895a0e5..07346c299 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp @@ -63,7 +63,8 @@ bitbots_splines::JointGoals WalkIK::calculate(const WalkResponse& ik_goals) { for (const auto& joint : getRightLegJointNames()) { result.first.push_back(joint); - result.second.push_back(result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint + result.second.push_back( + result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint } Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal); @@ -72,39 +73,27 @@ bitbots_splines::JointGoals WalkIK::calculate(const WalkResponse& ik_goals) { for (const auto& joint : getLeftLegJointNames()) { result.first.push_back(joint); - result.second.push_back(result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + result.second.push_back( + result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint } return result; } -void WalkIK::reset() { -} +void WalkIK::reset() {} void WalkIK::setConfig(walking::Params::Node::Ik config) { config_ = config; } const std::vector WalkIK::getLeftLegJointNames() { - return { - "LHipYaw", "LHipRoll", "LHipPitch", - "LKnee", - "LAnklePitch", "LAnkleRoll" - }; + return {"LHipYaw", "LHipRoll", "LHipPitch", "LKnee", "LAnklePitch", "LAnkleRoll"}; } const std::vector WalkIK::getRightLegJointNames() { - return { - "RHipYaw", "RHipRoll", "RHipPitch", - "RKnee", - "RAnklePitch", "RAnkleRoll" - }; + return {"RHipYaw", "RHipRoll", "RHipPitch", "RKnee", "RAnklePitch", "RAnkleRoll"}; } -const geometry_msgs::msg::Pose WalkIK::get_right_goal() { - return right_foot_goal_; -} +const geometry_msgs::msg::Pose WalkIK::get_right_goal() { return right_foot_goal_; } -const geometry_msgs::msg::Pose WalkIK::get_left_goal() { - return left_foot_goal_; -} +const geometry_msgs::msg::Pose WalkIK::get_left_goal() { return left_foot_goal_; } } // namespace bitbots_quintic_walk diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index 3beed946d..4a7ba8f90 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -317,13 +317,9 @@ geometry_msgs::msg::PoseArray WalkNode::step_open_loop(double dt, return pose_array; } -geometry_msgs::msg::Pose WalkNode::get_left_foot_pose() { - return ik_.get_left_goal(); -} +geometry_msgs::msg::Pose WalkNode::get_left_foot_pose() { return ik_.get_left_goal(); } -geometry_msgs::msg::Pose WalkNode::get_right_foot_pose() { - return ik_.get_right_goal(); -} +geometry_msgs::msg::Pose WalkNode::get_right_foot_pose() { return ik_.get_right_goal(); } std::array WalkNode::get_step_from_vel(const geometry_msgs::msg::Twist::SharedPtr msg) { // We have to compute by dividing by step frequency which is a double step From 548d6358c85bc4ee691c8852aa2c00c7d3788e65 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 14 Mar 2026 12:54:37 +0100 Subject: [PATCH 13/16] Reset path_planning_parameters to previous defaults (fixing tests) --- .../config/path_planning_parameters_template.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml index 520feb7f4..777c2c682 100644 --- a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml +++ b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml @@ -56,13 +56,13 @@ bitbots_path_planning: bounds<>: [0, 100] max_rotation_vel: type: double - default_value: 0.3 + default_value: 0.4 description: 'The maximum rotation velocity of the robot in rad/s around the z-axis' validation: bounds<>: [0.0, 1.0] max_vel_x: type: double - default_value: 0.14 + default_value: 0.12 description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)' validation: bounds<>: [0.0, 1.0] From 75960bd34ea444ac9826b6e6afa4002f39505469 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sat, 14 Mar 2026 13:02:13 +0100 Subject: [PATCH 14/16] Fix param get from other node Signed-off-by: Florian Vahl --- .../include/bitbots_dynup/dynup_engine.hpp | 1 + .../bitbots_dynup/src/dynup_engine.cpp | 22 ++++++++++++++----- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp index 86f87e957..dbca5d796 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp @@ -81,6 +81,7 @@ class DynupEngine : public bitbots_splines::AbstractEngine walk_param_executor_; std::shared_ptr walking_param_node_; std::shared_ptr walking_param_client_; diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp index 407f089c0..fa27b3ada 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp @@ -12,8 +12,10 @@ DynupEngine::DynupEngine(rclcpp::Node::SharedPtr node, bitbots_dynup::Params::En pub_debug_marker_ = node_->create_publisher("dynup_engine_marker", 1); // We need a separate node for the parameter client because the parameter client adds the node to an executor // and our dynup node is already added to an executor + + walk_param_executor_ = std::make_shared(); walking_param_node_ = std::make_shared(std::string(node->get_name()) + "_walking_param_node"); - walking_param_client_ = std::make_shared(walking_param_node_, "/walking"); + walking_param_client_ = std::make_shared(walk_param_executor_, walking_param_node_, "/walking"); } void DynupEngine::init(double arm_offset_y, double arm_offset_z) { @@ -660,11 +662,19 @@ void DynupEngine::setGoals(const DynupRequest& goals) { // get parameters from walking. If walking is not running, use default values // we re-request the values every time because they can be changed by dynamic reconfigure // and re-requesting them is fast enough - std::vector walking_params; - // Get params and wait for walking to be ready - walking_params = walking_param_client_->get_parameters( - {"engine.trunk_pitch", "engine.trunk_height", "engine.foot_distance", "engine.trunk_x_offset"}, - std::chrono::seconds(5)); + // We need to do this in another thread, because the sync parameter client runs spin_until_future_complete, + // which is not allowed to be called inside of another callback recusively. + // It should however use a different executor from the beginning, + // but somehow this does not work. + std::vector walking_params = std::async( + std::launch::async, [&]() { + return walking_param_client_->get_parameters({ + "engine.trunk_pitch", + "engine.trunk_height", + "engine.foot_distance", + "engine.trunk_x_offset" + }, std::chrono::seconds(15)); + }).get(); // when the walking was killed, service_is_ready is still true but the parameters come back empty if (walking_params.size() != 4) { From 10996ec26a3dd6af40035d0e8cdbba22efc18223 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Sat, 14 Mar 2026 13:04:03 +0100 Subject: [PATCH 15/16] Apply formatting Signed-off-by: Florian Vahl --- .../bitbots_dynup/src/dynup_engine.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp index fa27b3ada..e6e312262 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp @@ -15,7 +15,8 @@ DynupEngine::DynupEngine(rclcpp::Node::SharedPtr node, bitbots_dynup::Params::En walk_param_executor_ = std::make_shared(); walking_param_node_ = std::make_shared(std::string(node->get_name()) + "_walking_param_node"); - walking_param_client_ = std::make_shared(walk_param_executor_, walking_param_node_, "/walking"); + walking_param_client_ = + std::make_shared(walk_param_executor_, walking_param_node_, "/walking"); } void DynupEngine::init(double arm_offset_y, double arm_offset_z) { @@ -666,15 +667,12 @@ void DynupEngine::setGoals(const DynupRequest& goals) { // which is not allowed to be called inside of another callback recusively. // It should however use a different executor from the beginning, // but somehow this does not work. - std::vector walking_params = std::async( - std::launch::async, [&]() { - return walking_param_client_->get_parameters({ - "engine.trunk_pitch", - "engine.trunk_height", - "engine.foot_distance", - "engine.trunk_x_offset" - }, std::chrono::seconds(15)); - }).get(); + std::vector walking_params = + std::async(std::launch::async, [&]() { + return walking_param_client_->get_parameters( + {"engine.trunk_pitch", "engine.trunk_height", "engine.foot_distance", "engine.trunk_x_offset"}, + std::chrono::seconds(15)); + }).get(); // when the walking was killed, service_is_ready is still true but the parameters come back empty if (walking_params.size() != 4) { From 97fdcdacee9c03dc27f98c31b84f466c25789146 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 14 Mar 2026 13:08:13 +0100 Subject: [PATCH 16/16] Amy extrinsic calibration --- .../bitbots_extrinsic_calibration/config/amy.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml b/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml index 955e68f54..ba020af93 100644 --- a/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml +++ b/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml @@ -6,6 +6,6 @@ /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: 0.295 - offset_y: 0.06 + offset_x: 0.3 + offset_y: 0.047 offset_z: 0.0