From f2204f3cb47ed703d0cc7e897ebd15d7352bff22 Mon Sep 17 00:00:00 2001 From: Igor Brylev Date: Wed, 4 Jun 2025 17:23:37 +0300 Subject: [PATCH] init --- LICENSE | 22 ++ config/gello_config.yaml | 7 + franka_gello_state_publisher/__init__.py | 0 .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 191 bytes .../__pycache__/driver.cpython-312.pyc | Bin 0 -> 12629 bytes .../gello_publisher.cpython-312.pyc | Bin 0 -> 7270 bytes franka_gello_state_publisher/driver.py | 274 ++++++++++++++++++ .../gello_publisher.py | 129 +++++++++ launch/main.launch.py | 22 ++ package.xml | 24 ++ resource/franka_gello_state_publisher | 0 setup.cfg | 4 + setup.py | 26 ++ test/test_copyright.py | 25 ++ test/test_flake8.py | 33 +++ test/test_pep257.py | 25 ++ 16 files changed, 591 insertions(+) create mode 100644 LICENSE create mode 100644 config/gello_config.yaml create mode 100644 franka_gello_state_publisher/__init__.py create mode 100644 franka_gello_state_publisher/__pycache__/__init__.cpython-312.pyc create mode 100644 franka_gello_state_publisher/__pycache__/driver.cpython-312.pyc create mode 100644 franka_gello_state_publisher/__pycache__/gello_publisher.cpython-312.pyc create mode 100644 franka_gello_state_publisher/driver.py create mode 100644 franka_gello_state_publisher/gello_publisher.py create mode 100755 launch/main.launch.py create mode 100644 package.xml create mode 100644 resource/franka_gello_state_publisher create mode 100644 setup.cfg create mode 100644 setup.py create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..54aac19 --- /dev/null +++ b/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Original Copyright (c) 2023 Philipp Wu +Modified Copyright (c) 2025 Franka Robotics GmbH + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. \ No newline at end of file diff --git a/config/gello_config.yaml b/config/gello_config.yaml new file mode 100644 index 0000000..1203ee1 --- /dev/null +++ b/config/gello_config.yaml @@ -0,0 +1,7 @@ +usb-FTDI_USB__-__Serial_Converter_FTA7NPA1-if00-port0: + side: "sample-config" + num_joints: 7 + joint_signs: [1, 1, 1, 1, 1, -1, 1] + gripper: true + best_offsets: [6.283, 4.712, 3.142, 3.142, 3.142, 3.142, 4.712] + gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file diff --git a/franka_gello_state_publisher/__init__.py b/franka_gello_state_publisher/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/franka_gello_state_publisher/__pycache__/__init__.cpython-312.pyc b/franka_gello_state_publisher/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bce0c2279694123f8f1f30645d9df4cd3637d9d1 GIT binary patch literal 191 zcmX@j%ge>Uz`)>S=8yrRAA<;V{F#M;fnhpBC4(lT-%5reCI$wE&mc9w;`B4}b5r#b zOOx{BP4!bU;>(NmlS(slQuNb`67#YX`;|ofYax#lEQj73Q#>Z#o sWtPOp>lIY~;;_lhPbtkwwJTy}U|?WmU|=W)F+MUgGBOr1GcYg!0EQVe^#A|> literal 0 HcmV?d00001 diff --git a/franka_gello_state_publisher/__pycache__/driver.cpython-312.pyc b/franka_gello_state_publisher/__pycache__/driver.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8a6369e9ec945bbec9f207e150262124f1c0b78b GIT binary patch literal 12629 zcmX@j%ge>Uz`*d;&LJaBg@NHQhy%l{P{wB_Mh1rI3@HpLj5!QZAet$MF_$TdDVI5l zIhQ4hg%KpioWq*S7R8p!9>oskGv;taai%b&u;g&%az}B4#aMHAa(Sb8!ECl1zFht& zelVLoM<7=)N)XKE$PvmFjuOrli4w^bjS|fjixSHfj}m8ONa2iO<{RG@_+MKDz!E~)?)6>4Ef5k?YK zOc7~eh*C;bP7%%4U|=ZXU}8vBS`9IXfsvt-v64|!>?Mfrr^$Sa)wL`&ujCesPkwUt zEw+%1qSV9`O{QBM0Y&*G`N{b?w>W}R3rkb;l2bJqZ}GYp<(C!&SLP)Lfs`@c;)4r> z7iE@!l>{UvXQ!5UB<7{$q!wv1-r^3(FM@MfZ}B+$`})QQhdMjE1_$5ba*6PX_waOz z3~`0A!~KI?AZ#Chgt(7CNc^P(0|Ud$5)i=xB3eKMBLf3NGRQkH3=K4f&oK<3$WCR5 zVoYI(VoG65Wlm+0WME)OV@hFaVToc*VNPXBV@hFZVToc-VFk0QLdR$xz zP~cdUUTg*Dfds)OK0~9ox(6A}VhlVsX=wabflCJ;_5sUy~0tZMD$UD%q2niN`P;!7c zG9Ki}Ds^1pf~HUx6ejS1A(&c9-Jy9ixwNP#HLnCE-za3}DHJ89WG3bz=Sr+8B(Fd( zFD0?4D6!H?!7)z(%)*lm!KtB087(bvr4x})5Nb?G^Qxe8PJVGJxXgiOO+W0;U`+-o ztdhkQ;$Uf8klR6}M<(3sfb6SqP#8 z6dho04dZH1sl>p*P{IKgU|?Wa!?+q0!wd`zH4O0}Tfh=2Of?MgY+xn>14AW)CbQp4 z##^i@C6xuKZkp`3SmHBNii?auamiYhT9jXWi#aDX?-p}rUdb(9NHqYh!EW)#L$f%f zG)d8f76)dq#H9c&B{|~bGxIV_;^V7iump)7Ox6jc50r`;7;cD&cd+zu-jS7`A2Bmx zN%9Ki#Ti?YugF?;uw3Dgx*@O9!E%*D1`_L_$UrGNK_LYSx6k{)AzA_r83qQVP(p|? zWI;m-7A7DX7Mj`S3=BowC5X_2hjSBS4MP?@96@>@xR|?y6T)I(NMVAR)5KW94HM1c zfw2%Y!euoK@$gs$$IdUSkc?D?oYcJZk_>p61dRfP+|uF_h1|rFcrn(jt11hN)B6WgGzizYr-9*$sgQOxWmoYQFEDF zW(o5Gm*t)dJs0y`;?}z(By&T+=QA^tFy99TCSJY|YzzV-*ZGw%@hjgElDi=9eObuo z0*?SFK{SB+y$yEQIabt*ud@*0w-700-%zi z$O9C>o*+UU6ks6buuukNmMUdz4g!TT)KQ=cI2fc8)CX%|xFN3!3MEM9L8)OuCW9RU z%D}h+C<~+l9$XLxtOpYeajYg2I8oeUNzN~*#0W%CrGPzUAnL|2kaIz$c>@DD02MFt zD_-YUyU4G0nP1}qhXyz#QQQu42*_oymOI2HSZl~4P>ob2i7nK?bz>yRa!`(MV7S4- z1GdUflfMYuG|>Y^gFc7=Wzr&0HChDf_!Jp{#6UG-ks*i$uMoi{X^}BV4CHaJ-4IuS zs$EEJ2yO|02uG03pbP=34VhV0XLu|Kp6UCUftgkH0~b5139NQxXEpgC#K@|$faNm- zBdf{>Hbz#p1t}o58i=h4s~s6xH9xa4vhsrcPz168rFH~)0TgbZ+rSAA)*0huND)AE z$WoY6L7g#{G`19$7S<@X6xLMcRCZ`h3921AQrJ^D)0k2qwIe5(%?0j?af7;IDLgF< zQM@VaseDxy6x`SejCl zSdy9yD&t}1fcycDdl_a>rxG;-rGV;FP}T>lMwH~RG+V5FmYC7z$%L0{q zV6_OM2Cl1^56LW+8s^yybKx-nu4xgP;4Bx0SZxM|T9z7yEHStsoRI}8Ea5x|qlN|M z$|A-dc_bTIYgkZiWMN2Qt6_+T#|I~mW;3L4)UeKGm<#tA zBSR!Z3S%v69czz0gf3?+XQ*I~WGH9Uhvjt)Nj{QUvONW~3^F20JN$#Y*xOlJoQOQj<$S z?G(_^OJY$jqVEaP4=V>0G&1u_p!(rrps|_c{M_8qyv*dplFa-(gkHaNaL_R@I8=$d zC1!#~6-x3I@(WV)!0jiH^;Pl+iR6sLymUxF!A#dtsM13yNK8plC`c?y%uOvxEdu)+ z9-s>4nI#zto-PV*Rcs)0trV)5bQCnDi$D!5a0OWe%H+3L<8$(pvx`94{1#6*1SOWF-r`71 zNeKW22Bh@_${4peT`Q7P3qX-@i?yIAGq0owRNCI+i7zh6FNiMzk7?c#1r1FVmBfPv zI5YFop~9MMkkJKD`Ck+WszHN5RliS_bBd&N=^>zVCmud!p0!0v4Z8Yj4nuA@{XML3g^pm z1|W{~9XahAJObBwWH0i_f*JA`dF1c#DBj^w_{_;F&jnWWg@-{w4=jtOOzsYk>}O6^ z1+X%i4+7w}tinZph3ov97x^`>@N0t`wHB9!EHCg_J^-~FA+kCb`E^!AT;VqZ$tyY> zkT}2vZ~J~=V-S~~o;fk|69WsUP>0(MVe#w2Y8Qpot_$m56xQFsc2(H?j+oL7F_!~j z2Rtr__FCFPsbllAjnDIW_L^2u^UDVKPH)0n-A*1Gl8W)u`!6KgG*qy z17!y?E{OOemB7lvoI)RT7^LN|OX*#d(z`BYeo@N&vXo^9{|#=bIXRcOwLnF_ywhbN z=L9~6G(B66xcxVBX z{EI=_AVYSLp$<9pxPp!-K*#%bg3_lKhyZ0g@GJmIK@Lg*U{|DoXLf5Ci`YQ(gW$Zy zkjGjA8gBq|YME*nu#In;GBARQ0#G#%R)-*57-Ewd7;2eo7_vaNASA%7EKp+#%t8<~ z3|PnAm=VQs4HK+i1TMTmMI1{BD06`g0Tn65Y&EPYj9?aHk0YXRMij}63`nBzLWhw7 zQ81@4q4#cB7&t*Q+YBiTutJiN0o;d$`=XLTnZb~un6aD*)VgN`g$k%wizE+i-!uCa zfdUo1Xo5r=V!8+(t?)LgCeJP25dWaSP}g`@KSw7Y*IRs0u8U_dh+h-}if3@s6*M?= ziw|6Q89G&#q=r-kRTPDS3K2-75~>km4!GzkN(5;JH5kDyLs-KO)FZsb192RryaAU* zMfo7JI6>t}Drf)_qnQV4SXRlyJppaz6{nWK#-l+aUVA`=38(~bV7RHFH??+)UkA?( zUcvsT&Zv(JT%2Mzq~zvn&eZ(Gz{x8%f$fH_;dNbyi@FZib^R~u`riD%3TRE>Um*E`jX^|tNzNr$KL~C9VQMn_f zc0vB=o<%+y3s0k}`LrtrL z1yYJ$;ZO!QXdxx45~NuRE>TfJ96r3n0vTSyI&O?_SabqoaU^2k3EQMW3Otq=K?NR` zA`Mg&;-5k&0W}6d5dks*UTlE{zyz!XgcxpG!vrgEYC)43@WBqSaxjqvZ&`x5U;-B3 zwM;ckC7=cbSQJz!7D<$V<^jO`6lT;`9V3=Pu4h>ZytP=vfUPwQbF&LWY!|dm%?uv! z!RA&ht-~4?M4PZg3G6V?@C+#bfJ&s`8U}1@Nw5x8;Weq26)A4iVD=zrBw0}5g^&QV zYTy+G%)hm)HLNv^HO!z1nVz~5_`p*N3+6x-xT=5;fTe(|5?H)~Qxn(@R03aWEoPd) z*b`C7fLZDEs6(q6R=@N=D?eWHO$x9tvS46fs1hqrEXvEwOV?2VwEz{;z#}VG3RQ-P z25&mrbQ6v?vZi=Z5~%oR%PfvBPA$2`0-AKX#af(`npyyeftDak&sd=Tjsi1-5Tilt&j%A6NIiM9DMd2W0B0%=Br>B;_ zMUZAUaGUpmkl1x0<%>ef*M+n$3Ta&x(z(MU(C^jhHKFthkL(RW(dkkXr9LtU^BUh# zP+HEhkYkPTijeh@Do%hLYNKCDV&arq`7mE-E=(R&tuae?wRaTv^=_ zk-Q*dw88#@k@E#9mn$N!H$-H#mz2?o86F*yP{xv#oYS~Gn0ha2Q~&p)$0nz7Zr@JD_CDtu)ZOq zbVbGTiZ!T_t0*;t^$Qz=qB7X2LhB`0N^UT{tZ8{!!Di1G+ZeUM^MGoHY5ML_AhI>@Q448r2bVRT(c=Yo*V zcNUNw$Q!?kg&7za_BtxMnlT?zchYnS~Xp3WdxMW1k3YP{Kd=j#gx&<}Wa> zfw4ykyV+TwDg~?tLBNYu#u~;HCeSbdVkRHk$UJB$As94!pFV3Q2<)_n4s9-Q&}0Lr z*rEnd>TU!Pz90geaWWVf7&IaC^tYJevyfNqfR}4v_RkQra<4&Y6*RZm!0>=i=sKUm zMLvZKd>XJ+THNFRfti6{7$R|zPvZj{1E0`!Uipi>^4ED)F7m2e=2h!py1_3v!L74; zPSE_&nV}c?l`e27Vb-~zmI|np{=5L(%E1wgAQgxZ1hZhl2rl_ROm{# zib3Tocp~yThvY>L$?F`l7dd3FaL6OITof;ID8kw@5H-pdIh3z*sDi`QPm`&NM?WRC zOuwY0GBnu9AhV%GjzLoh=Y-}h=Hikf0<)T+=2#J^j3Z`#6O?3%KtwDk0fEZS7KR6Y zp&yw|SXFNbOV4ngX!`-o*3gE~9^Rip{0}*d%B)(jNl#@~tq)#|$g`e&kXcVYR<%zG z%&e-PxWrkFKbWwwD#PYK*;ti73ox^)fx`jp0+74mi2+n*gObo^C2;>Vg)yD6mVuK2 zxk(MLkQr+jvp|hmxMm0=1wMcV>a$^00k7OaO(0H&8b*9cu$BpFV2rJXVKxI~mIrx$ zr$VJH${L~(Np(;8-21E7{MlA$LKHvgl{Py(7$06SfUp-33)X3)qHSOh^J z5_cp+B?C;iCWl`UXcV$a3Y?mfOZ7^NlZsP|G81!j-9pU6oqR)`G#SAo^&xImqF$gy zw#dt6K{G>DOjZh-?BJ-lCFqu!lj@gW;+9{UmjYR80%{zAiy=t0E$E$EnUtScl;W9J zl3G+$T2KO>Ki6c2%n7qW7Q%r?kc&YPsQ?{my2X;4n3-1u8c+HON^%RqOEDgB^K>XT zIDcScX7yyeBdD-I__CmC2ipfO23GFtEK(O)q-L00VNtlj!Ug4-USU!Ez{bWZeuGcw zhP2{!X`PGGI+vyOI(+W%2wo7;T%o)|bVbqznGJjwgzT^IIDBAb;1~Y!MVNs{_7ejm z%#sV*?pFjoy4XH2gM@y6k%CH#-{9cu=jr6>;{(Zv|0)KxPg}Vi#TYg#I~p+@6ccmQ zXFjOM2qKLno%om!@iIE`F@u)rfqWMquc-nq;csz5W>GTp(!p~p;8`U|$BV6`vH&Cu zO6bs~dwHd~1(o1TdrLM2wmv$(I3-&TJV_K^T$z^)Zlypbn&fb*0r&qQb4fC&iXih< z@fnb1=8#z|DKsVdMI~?rpka(#lBf!b@{{sQGK=F=Qqv$K2%uC78Q%nTb0A}$rJ!0Z z3PePM2zXc4;&K7f-PI7JkJ7jl3G#vsK4s0;Fo!zMRBr8Fniu4o=~ zrBLy71_p)?%#4hTw;8zaGiY99(7ex}`j|oJE`#}92Ftq)W_KBkzB92gGJR!WV`Tau z#2_YdfkEhoD3}xxy}%%NLqhrjgXj%$i3e=uNR;pwQl#4LG}g`?H&12Y2) HWPuX^Tyl>< literal 0 HcmV?d00001 diff --git a/franka_gello_state_publisher/__pycache__/gello_publisher.cpython-312.pyc b/franka_gello_state_publisher/__pycache__/gello_publisher.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e745e64510bceadfacf390880044d20c822112f1 GIT binary patch literal 7270 zcmX@j%ge>Uz`!uy-XUX=5Cg+w5C?`?p^VRK7#J9)Go&!2Fy=5sL1@M(#uSDWrW~eR z<|t-Hh!{&0OA12@a}H}RTNE2qj6I4Yg(ZbGhclNeiVG~pmcyOP6U75&v*+;U@HCgUwGul&rslHiiW5|C8bI> z8Hq)y@hO=_smUezMU}}QYhaiK%J`hYz`)SXFr6Wl0Tf74OdSkqj46yQ98t_EOsOns zOexGQEK#f}EMPWk3riGR3R??96niR13R5a)8dC~;3k%FcQCuk;EeuiIDV(W1DNLz6 zX)GyREi6&I9SjwWQGCG+n%uYe+*5OM@&ihfax#lEQj3y7euO%O0ThLxp!@8`#K16> zaXLc@OopM8xrU*am4P9JaWzy2BSQ^C7DxwN4TJ%6OD$s!Ll!$k44F(}s$pEs1Q7w7 z!3kj_lPOFvTe7&3#UX5%4Ou)8J~COu5D&Mrh9Qd&Sq{R^5`@smWC}CPz7!To1_p*2 zhIqKEI2lSrU~03(U@Qck1=4`vf>|ZfV2Xi(A%zugGFu8eEMe9%)i7kq!qgz>8o0k1 zL7^*;kb$vE6kx0rjucLqc{L32a9`ChWMR{%0@ufdqE8qmUc->331cJZ6z&>^EG+~d z#)8FQ4MRLUOwt)rcos3%FvN?)R8=x)^7`H4PX{Hr_ySm(yTy^5pBrD0UsUpoQUB%t z|NsBrl1VE{%*#%Uho~$DjDH>XNiA1tPy1J7x zC^Ih|!Ycx${adWX1v!}|w^%@-eTyl-_!dh+VoAm=7LfmnI2jliZi#_R0_!i1Pb@6OsStXgdsYSO0 zp(z1sNLGIOEmnvOH$-DzVs2`&Cd)0B;?$h9TLKut!wqp_JlHFo@$tzyiN(e7@x`Dr zRY5_a;a94DMt*Lpeqw1-e!QuEN=AHnv3^o%W=@JezI29P5}eKue$p$bERtbhV5kyD zEl~Af5iG#Kz)<{~fq~&i1H&B--cI(591<5KwXSezKaiHcE^T#D+Ul~j%>@pL8yq~> zIpi*K$jzv_plyAH!{&yB%ykK^ixOHZtS(EKU6=5>DB*Qk!sog~v4D5}6!%|38zp#)l~Ffe3+w4h-`rBlmT z!&szV!;l5ii>3-*EnumXK$XsHhPlYaD`N^XsN@a?6}0$F1EqR2n^IU{g*qccC95Wz zUzLe|N@|&YacWU!Vvc@NrEX@5era)%u3LzUXMAX|Q+&K`e0(riHr_cuuPn8wB(+GZ zO46~kBwrU)4kVYPrYOL&f3cN95fdnB+4$un1QlTF^b`VeQWJ|)6^aW|lQYvQ6-qKv z;mQM=n z0+BTg@o+Oa8EWvU2dBdthIn|oC7>SGM1kAS$xy-%ww{53p#1aTfGYCrK^*0A7oM5h~VTgzO zfq?%xLHcSK;z1=W*d9($9##f3QHa%09wS2~gEE65Los7H6C*<;10w??L(iiM=17Ke zMoliiTZ~1wxbjMKA@x!5Eod!NoSB|ie2X1grxk%>C$phNjzN>D2xQkSj-ZG zra%l50NIUHtrizKf^wV_D3P<|WEPj)Vg(yjpfR`UQ{*PP;*h$1yrTT-rz8Jpl7lnWP9Ys$cuXR2Y4^)`GF)PZg6Bi zkX2sbvfOi_=S5k)6_podEjw6xcy4f*J`s_aBRZpSfy#3Ih5DBzjV_BAcW~YjQ(cg` zA!U2+#@rL>SH!|PSbDfY^`ea2e2q# zG;_b;5q;4t1|%bQgTv?nsE(Gp$RTx=L*~0K$P*y%{VE3aPI|>$SeTEnu)6TFqf~LA ziUU-@eBOap#nmumAs1KBM2g4=*b1X;4F-l{CNG92#uAXZU_JQFabbv+V_=A3VqmCc ztYxZUN?`<SfaSV^!9@jw%VLI~7#KOD z7(3lMoF@q05EhxiG}(Mc;sV3jX%~1RS8#7|-e7vcBIH77*oBbr3*iwL)FLnQL|zb9 z`N#~?2Cj8bVi**QAeVmzWz6ZILI@l$s6-J{3S%B)3KO_L4AOxr%}~Qw#lXN2%%I8a z2M%O#)vw7^1j_zJpkRYl%RH$SB}IwQLJFmB23cPvi56g>LIs?=iz7gx1?o38Fg%b^ znC~;oXJX-s#15Vt{KD7yRW9tw3l1v#MAb7;2eIKsgW0ho#qAP}Ri?7D6Fl@>%fe z1tATW6GVu?STMO77HpMhJgAQeR*JpCtYJY^k|~Th%z(SLhS`OopEa2&ogtYimY0E{ zmbC=b-h!A9A3>PN)WgrhkjzxeTEl{w! zLFMKxPEZw=oRgoNeTz9Szx)?oF{JbJi>ICQ0TT<{kKQ%EWzqBO2Bp)`kaEm1+u_Unw zG`4b!9oq9P0*$C>azM&JzU0!PqSQQSpSt*#2#k;1B)BC36NPv#J~_V#G_07Ka!UwF z9drcb7AsiQEfJWrz+9N508A9-kz$ZXKvh2oL#lsJR8=XU6)8~H$3slZ%1@64l`O)H zpjQ43N!j_*Go`Of>R*)9zbt9k!FNMm_ky152A1mvt``klFUY%H;E=hYVzI;Iy0ytis{i25Z1&@f!8j%-x z6t429KG4y>z@u=5M-$vjSGvfdbe%)%B8S!mZQCmxc3?r3iySJ~Idm>^=v>fsyvpJ9 z^OG=xdNgA(sO&ssDCAbYU)DELL_k* z6Ox;;m&ypeRoo0Hot+fs5>R6gYzU~{hj$2|wNo&ICaWK~8iG_vF$@e0>0s3iAM7=m zi@@zm=G?@JTgnxa%t8yw`-TfESr zspQn0ocMT6#v&t-k3d5m4v<_1s>F&w1gLH*js^`DwJ>~Pl4s@m;KazzslMCz`h3ChSG=uB|{MYybYWb5xr$>jno>3HAoWxh-3t>dKi)PgE9al z*}%yfB;7TPS)c((xEzE5Gl`J_$s~AX3sD0i5$Y-#G?_q?H!O)o>BX9Cw^)mka|$X! z0~?toVDH{yDK5y&yTy}|T3k|;Um2g5pOSiuqd23qBqhH*Pm`&r78GNk${4v-b&Dl8 zF*C2InSp_!0u*f);DmUGg|pqO(QAUsRTjxRVk(!#)EfMuVjcBYSmbZ82w!JWxX7Y# zg+&P@?$_vdl|=*`^q}MhG7^*${4_<2KqK-+AX9I#l~fjF=A{>bqM@h`RP}Iy{h$YO zX%VR9bc;2wG`FC#$QR^NP(@b+3fx=##i@D4`9+`_tXMC%I2|%X%TrvEf)E0g?M3b& zL!}dQLDfuVUP@|3d_iSNMt+_ibgHTtJQfTNEwGP^N*Nd!Rx*Iy4Nj`RIBXyp#IC4^ zfq?;(hKv0e7#Kb bool: + """Check if torque is enabled for the Dynamixel servos. + + Returns: + bool: True if torque is enabled, False if it is disabled. + """ + ... + + def set_torque_mode(self, enable: bool): + """Set the torque mode for the Dynamixel servos. + + Args: + enable (bool): True to enable torque, False to disable. + """ + ... + + def get_joints(self) -> np.ndarray: + """Get the current joint angles in radians. + + Returns: + np.ndarray: An array of joint angles. + """ + ... + + def close(self): + """Close the driver.""" + + +class FakeDynamixelDriver(DynamixelDriverProtocol): + def __init__(self, ids: Sequence[int]): + self._ids = ids + self._joint_angles = np.zeros(len(ids), dtype=int) + self._torque_enabled = False + + def set_joints(self, joint_angles: Sequence[float]): + if len(joint_angles) != len(self._ids): + raise ValueError( + "The length of joint_angles must match the number of servos" + ) + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + self._joint_angles = np.array(joint_angles) + + def torque_enabled(self) -> bool: + return self._torque_enabled + + def set_torque_mode(self, enable: bool): + self._torque_enabled = enable + + def get_joints(self) -> np.ndarray: + return self._joint_angles.copy() + + def close(self): + pass + + +class DynamixelDriver(DynamixelDriverProtocol): + def __init__( + self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 2000000 + ): + """Initialize the DynamixelDriver class. + + Args: + ids (Sequence[int]): A list of IDs for the Dynamixel servos. + port (str): The USB port to connect to the arm. + baudrate (int): The baudrate for communication. + """ + self._ids = ids + self._joint_angles = None + self._lock = Lock() + + # Initialize the port handler, packet handler, and group sync read/write + self._portHandler = PortHandler(port) + self._packetHandler = PacketHandler(2.0) + self._groupSyncRead = GroupSyncRead( + self._portHandler, + self._packetHandler, + ADDR_PRESENT_POSITION, + LEN_PRESENT_POSITION, + ) + self._groupSyncWrite = GroupSyncWrite( + self._portHandler, + self._packetHandler, + ADDR_GOAL_POSITION, + LEN_GOAL_POSITION, + ) + + # Open the port and set the baudrate + if not self._portHandler.openPort(): + raise RuntimeError("Failed to open the port") + + if not self._portHandler.setBaudRate(baudrate): + raise RuntimeError(f"Failed to change the baudrate, {baudrate}") + + # Add parameters for each Dynamixel servo to the group sync read + for dxl_id in self._ids: + if not self._groupSyncRead.addParam(dxl_id): + raise RuntimeError( + f"Failed to add parameter for Dynamixel with ID {dxl_id}" + ) + + # Disable torque for each Dynamixel servo + self._torque_enabled = False + try: + self.set_torque_mode(self._torque_enabled) + except Exception as e: + print(f"port: {port}, {e}") + + self._stop_thread = Event() + self._start_reading_thread() + + def set_joints(self, joint_angles: Sequence[float]): + if len(joint_angles) != len(self._ids): + raise ValueError( + "The length of joint_angles must match the number of servos" + ) + if not self._torque_enabled: + raise RuntimeError("Torque must be enabled to set joint angles") + + for dxl_id, angle in zip(self._ids, joint_angles): + # Convert the angle to the appropriate value for the servo + position_value = int(angle * 2048 / np.pi) + + # Allocate goal position value into byte array + param_goal_position = [ + DXL_LOBYTE(DXL_LOWORD(position_value)), + DXL_HIBYTE(DXL_LOWORD(position_value)), + DXL_LOBYTE(DXL_HIWORD(position_value)), + DXL_HIBYTE(DXL_HIWORD(position_value)), + ] + + # Add goal position value to the Syncwrite parameter storage + dxl_addparam_result = self._groupSyncWrite.addParam( + dxl_id, param_goal_position + ) + if not dxl_addparam_result: + raise RuntimeError( + f"Failed to set joint angle for Dynamixel with ID {dxl_id}" + ) + + # Syncwrite goal position + dxl_comm_result = self._groupSyncWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + raise RuntimeError("Failed to syncwrite goal position") + + # Clear syncwrite parameter storage + self._groupSyncWrite.clearParam() + + def torque_enabled(self) -> bool: + return self._torque_enabled + + def set_torque_mode(self, enable: bool): + torque_value = TORQUE_ENABLE if enable else TORQUE_DISABLE + with self._lock: + for dxl_id in self._ids: + dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx( + self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value + ) + if dxl_comm_result != COMM_SUCCESS or dxl_error != 0: + print(dxl_comm_result) + print(dxl_error) + raise RuntimeError( + f"Failed to set torque mode for Dynamixel with ID {dxl_id}" + ) + + self._torque_enabled = enable + + def _start_reading_thread(self): + self._reading_thread = Thread(target=self._read_joint_angles) + self._reading_thread.daemon = True + self._reading_thread.start() + + def _read_joint_angles(self): + # Continuously read joint angles and update the joint_angles array + while not self._stop_thread.is_set(): + time.sleep(0.001) + with self._lock: + _joint_angles = np.zeros(len(self._ids), dtype=int) + dxl_comm_result = self._groupSyncRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print(f"warning, comm failed: {dxl_comm_result}") + continue + for i, dxl_id in enumerate(self._ids): + if self._groupSyncRead.isAvailable( + dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION + ): + angle = self._groupSyncRead.getData( + dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION + ) + angle = np.int32(np.uint32(angle)) + _joint_angles[i] = angle + else: + raise RuntimeError( + f"Failed to get joint angles for Dynamixel with ID {dxl_id}" + ) + self._joint_angles = _joint_angles + # self._groupSyncRead.clearParam() # TODO what does this do? should i add it + + def get_joints(self) -> np.ndarray: + # Return a copy of the joint_angles array to avoid race conditions + while self._joint_angles is None: + time.sleep(0.1) + # with self._lock: + _j = self._joint_angles.copy() + return _j / 2048.0 * np.pi + + def close(self): + self._stop_thread.set() + self._reading_thread.join() + self._portHandler.closePort() + + +def main(): + # Set the port, baudrate, and servo IDs + ids = [1] + + # Create a DynamixelDriver instance + try: + driver = DynamixelDriver(ids) + except FileNotFoundError: + driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB") + + # Test setting torque mode + driver.set_torque_mode(True) + driver.set_torque_mode(False) + + # Test reading the joint angles + try: + while True: + joint_angles = driver.get_joints() + print(f"Joint angles for IDs {ids}: {joint_angles}") + # print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}") + except KeyboardInterrupt: + driver.close() + + +if __name__ == "__main__": + main() # Test the driver diff --git a/franka_gello_state_publisher/gello_publisher.py b/franka_gello_state_publisher/gello_publisher.py new file mode 100644 index 0000000..c1a9e57 --- /dev/null +++ b/franka_gello_state_publisher/gello_publisher.py @@ -0,0 +1,129 @@ +import os +import glob +from typing import Tuple +import rclpy +from rclpy.node import Node +import numpy as np + +from .driver import DynamixelDriver +from sensor_msgs.msg import JointState +from std_msgs.msg import Float32 +import yaml +from ament_index_python.packages import get_package_share_directory + + +class GelloPublisher(Node): + def __init__(self): + super().__init__("gello_publisher") + + default_com_port = self.determine_default_com_port() + self.declare_parameter("com_port", default_com_port) + self.com_port = self.get_parameter("com_port").get_parameter_value().string_value + self.port = self.com_port.split("/")[-1] + """The port that GELLO is connected to.""" + + config_path = os.path.join( + get_package_share_directory("franka_gello_state_publisher"), + "config", + "gello_config.yaml", + ) + self.get_values_from_config(config_path) + + self.robot_joint_publisher = self.create_publisher(JointState, "/gello/joint_states", 10) + self.gripper_joint_publisher = self.create_publisher( + Float32, "/gripper_client/target_gripper_width_percent", 10 + ) + + self.timer = self.create_timer(1 / 25, self.publish_joint_jog) + + self.joint_names = [ + "fr3_joint1", + "fr3_joint2", + "fr3_joint3", + "fr3_joint4", + "fr3_joint5", + "fr3_joint6", + "fr3_joint7", + ] + + def determine_default_com_port(self) -> str: + matches = glob.glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") + if matches: + self.get_logger().info(f"Auto-detected com_ports: {matches}") + return matches[0] + else: + self.get_logger().warn("No com_ports detected. Please specify the com_port manually.") + return "INVALID_COM_PORT" + + def get_values_from_config(self, config_file: str): + with open(config_file, "r") as file: + config = yaml.safe_load(file) + + self.num_robot_joints: int = config[self.port]["num_joints"] + """The number of joints in the robot.""" + + self.joint_signs: Tuple[float, ...] = config[self.port]["joint_signs"] + """Depending on how the motor is mounted on the Gello, its rotation direction can be reversed.""" + + self.gripper: bool = config[self.port]["gripper"] + """Whether or not the gripper is attached.""" + + joint_ids = list(range(1, self.num_joints)) + self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=2000000) + """The driver for the Dynamixel motors.""" + + self.best_offsets = np.array(config[self.port]["best_offsets"]) + """The best offsets for the joints.""" + + self.gripper_range_rad: Tuple[float, float] = config[self.port]["gripper_range_rad"] + """The range of the gripper in radians.""" + + self.__post_init__() + + def __post_init__(self): + assert len(self.joint_signs) == self.num_robot_joints + for idx, j in enumerate(self.joint_signs): + assert j == -1 or j == 1, f"Joint idx: {idx} should be -1 or 1, but got {j}." + + @property + def num_joints(self) -> int: + extra_joints = 1 if self.gripper else 0 + return self.num_robot_joints + extra_joints + + def publish_joint_jog(self): + current_joints = self.driver.get_joints() + current_robot_joints = current_joints[: self.num_robot_joints] + current_joints_corrected = (current_robot_joints - self.best_offsets) * self.joint_signs + + robot_joint_states = JointState() + robot_joint_states.header.stamp = self.get_clock().now().to_msg() + robot_joint_states.name = self.joint_names + robot_joint_states.header.frame_id = "fr3_link0" + robot_joint_states.position = [float(joint) for joint in current_joints_corrected] + + gripper_joint_states = Float32() + if self.gripper: + gripper_position = current_joints[-1] + gripper_joint_states.data = self.gripper_readout_to_percent(gripper_position) + else: + gripper_joint_states.position = 0.0 + self.robot_joint_publisher.publish(robot_joint_states) + self.gripper_joint_publisher.publish(gripper_joint_states) + + def gripper_readout_to_percent(self, gripper_position: float) -> float: + gripper_percent = (gripper_position - self.gripper_range_rad[0]) / ( + self.gripper_range_rad[1] - self.gripper_range_rad[0] + ) + return max(0.0, min(1.0, gripper_percent)) + + +def main(args=None): + rclpy.init(args=args) + gello_publisher = GelloPublisher() + rclpy.spin(gello_publisher) + gello_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/launch/main.launch.py b/launch/main.launch.py new file mode 100755 index 0000000..9d6c8d2 --- /dev/null +++ b/launch/main.launch.py @@ -0,0 +1,22 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + args = [] + args.append( + DeclareLaunchArgument(name="com_port", default_value=None, description="Default COM port") + ) + nodes = [ + Node( + package="franka_gello_state_publisher", + executable="gello_publisher", + name="gello_publisher", + output="screen", + parameters=[{"com_port": LaunchConfiguration("com_port")}], + ), + ] + + return LaunchDescription(args + nodes) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..43cdf8a --- /dev/null +++ b/package.xml @@ -0,0 +1,24 @@ + + + + franka_gello_state_publisher + 0.1.0 + Publisher of joint states of gello + Franka Robotics GmbH + MIT + + rclpy + std_msgs + sensor_msgs + control_msgs + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/resource/franka_gello_state_publisher b/resource/franka_gello_state_publisher new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..0b65838 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/franka_gello_state_publisher +[install] +install_scripts=$base/lib/franka_gello_state_publisher diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..ae7f096 --- /dev/null +++ b/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup +import glob + +package_name = "franka_gello_state_publisher" + +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", glob.glob("config/*.yaml")), + ("share/" + package_name + "/launch", ["launch/main.launch.py"]), + ], + install_requires=["setuptools", "dynamixel_sdk"], + zip_safe=True, + maintainer="Franka Robotics GmbH", + maintainer_email="support@franka.de", + description="Publishes the state of the GELLO teleoperation device.", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["gello_publisher = franka_gello_state_publisher.gello_publisher:main"], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..95f0381 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..75c3f19 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,33 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest +from pathlib import Path + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + config_path = Path(__file__).resolve().parents[3] / ".flake8" + excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/driver.py" + print(excluded_file) + rc, errors = main_with_errors( + argv=["--config", str(config_path), "--exclude", str(excluded_file)] + ) + assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) + + +if __name__ == "__main__": + test_flake8() diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..647b3c3 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest +from pathlib import Path + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/driver.py" + rc = main(argv=[".", "test", "--exclude", str(excluded_file)]) + assert rc == 0, "Found code style errors / warnings"