From 8a963d1999bf6236bbdef0564025a5e78bb34d3e Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 14:34:31 +0100
Subject: [PATCH] dual arm stuff

---
 python/examples/.clik.py.swp               | Bin 0 -> 12288 bytes
 python/examples/clik.py                    |   7 ++-
 python/ur_simple_control/clik/.clik.py.swp | Bin 0 -> 40960 bytes
 python/ur_simple_control/clik/clik.py      |  65 +++++++++++++++++++++
 python/ur_simple_control/managers.py       |  63 ++++++++++++++------
 5 files changed, 117 insertions(+), 18 deletions(-)
 create mode 100644 python/examples/.clik.py.swp
 create mode 100644 python/ur_simple_control/clik/.clik.py.swp

diff --git a/python/examples/.clik.py.swp b/python/examples/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..170a055deeb26d0d8c333e4a1a4337f2328b14a5
GIT binary patch
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|{fbj!3r`s$uAx$H0)DUtEx%l2`<i!iRGci?cKH^6)7E
z%hkaQ)X&JzO*PVob8|}bQuGUo5-T!`^$Uvfvr>~wiuFs2;)^qL3vyE9lk@XRit=;x
z3o1)8^7HglD-uD%#rnxPnb~>;m0+_+iO~=k4FNnMP+F3vYr)H4Y-DHvQl+e<s30s9
z3Sy4p(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fe{h{B?XKO^$ZLQOi=$CLup1d8p<7|
zMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E1VU<f3pFfhDgXJEL*0h#}Y
z_5WY+Gca7`XJ9zV&%m&epMhZtKLbM<KLdj#KLf)mJ_d$pJ_d#;J_d$JJ_ZI)J_ZIg
zJ_d&8ybKI;co`U~c^McKco`U0@h~v7@-Q%@@h~ui@h~ui@-Q%1@-Q$c^Dr>5@Gvku
z;AUXh$j!je#m&GF!Og&6$j!i@&dtE^hl_#X3Ks*zCN2hs30w>eZCnftQCtiRdRz<)
zUpW~V?r<_N9N}bOSi;G`(8bBX(8<ZbkjBZtV8hA4V9v?FpvcL<@Rx&u;W7u*Uqdin
zMx8tw0;3@?8UmvsFd71*AwW_HC@3f>DCgwo7sTf#<|U@57U>n2<QI4(=B4DM7HRn9
z=cVc>fJjXSkV+6P%1_EK(aXtCN9af_OZCZ5*MO+WOjAfKN-x$cPAp4}&&f}>0_y}5
z2<_RKIXS+Gd6@;JIf*6tMPZr6rHMJ2RjEa2hLpi1K$gK&TQQ)S26A!`$XN*8dHE#}
zXB1@S<tHa+Wah``=jBvF90Tz}enD!Udr@XV0o+!IcydmDaVn|=$l<y9WvM<crHMI?
zMY$RvAL=N8)$1turspT-=qP~5_>!W;yyCR{qFhK|fHb0t>w#6|B$j06=S3UDD%dI*
z=o!NOgQ`}+R-qs>PcPWjSkE&hHLoPIq!Q}G)STi}h+jZ9fSrcLO%OL^ra>I42X=0J
zUSe*lf}*X0Qe|mwrV`j@<>dU_f}G66yb@oKCvlntvJ3241zV_*DXD3hd8zLCi8%rJ
znRzA7KAu<|2+|0{)k<I^loYHW0apugB}^LZNIiJG7l9%kk}gy8Qk?U1a})DYG{CL~
z>3}6RxO$LsL^=T}0b#I>3bqRAsU<;)c`5n1IhF3Id8tK-C8;SO$Af(gaR<!ABB;9f
zbg063P~1Sm9PCB~TLo~M^M&LaBqKnH17cY`h^e8;keQ|s4-WYFcm-Qq1*Q1-+{Dbh
z_;@8N1xQ*hN-ZfZ$^)lZu-`!XK(U`#RGeC*R{)}5W`X>ZSd<Qqvec4Py~NUzd>BUq
ztg;B=6Ocj-oRXTBSejE}>z0^PoT{Uck(yIrtCX3hP*PNys-pn%v;xe-3W?w(t)q~c
zrjQ0wu27j@s!&o{kgAYbqL7)VP?njfP?A~!Vi%N_C_!w6xIiy4B?aW*(%jU%5)E}-
zU69G3VAqA2t_uoIbsdGoWKd49O)N?W>(q0~&(BFs%=0e*2`A<_f~9r9fdW=g1PNK^
zoXl*;qV!@EALpkjlqD8r=9d;LWM}536zeJYCT6E96qgpIg592-mtU?>o{?ChP?C`f
z_MAdyu|iU6T0T@kQE8r{I+9&_DXGQDMVVl?DA+2f2bJb2fQyF|1yGr(keOGOT2!10
zvMn_?u_QCOSRn>UJH)BJnR%JHpkgCE02Gg)A}%F04U`r^AyllPX~n?6kXDqRtAMeJ
zTd%YvGe-}S$>JeF2u_9ynYjh|MI|6Ju%>5R#(@`sg9s!8U?HgkG7ZFWhO8jZNiEV*
zfUYL@$<HqUN$DtniyDwPa)E)%I%t(rjAR>nVCX17tpiyB%}P2b84F@~T4`Q#Nq&9~
z+^mAcqLR$S90q7eAms=hP&NXm6__Nv4p2xe29>BV@siBkR2Vz2G`FA<B%fEnpsWxO
Z8RFsZ7w;J4?(FXy;Nu$N8t?DT000eAvA6&L

literal 0
HcmV?d00001

diff --git a/python/examples/clik.py b/python/examples/clik.py
index 332ab56..5d72800 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -31,7 +31,12 @@ if __name__ == "__main__":
     else:
         Mgoal = robot.defineGoalPointCLI()
     #compliantMoveL(args, robot, Mgoal)
-    moveL(args, robot, Mgoal)
+    if robot.robot_name != "yumi"
+        moveL(args, robot, Mgoal)
+    else:
+        goal_transform = pin.SE3.Identity()
+        goal_transform.translation[0] = 0.1
+        moveLDualArm(args, robot, Mgoal, goal_transform)
     robot.closeGripper()
     robot.openGripper()
     if not args.pinocchio_only:
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..af83ac7f5436cadd1f4a612479c3a2b4869838d2
GIT binary patch
literal 40960
zcmYc?2=nw+u+TGNU|?VnU|_Ivj!3r`s$uAx$H0)DUtEx%l2`<i!iRGci?cKH^6)7E
z%hkaQ)X&JzO*PVob8|}bQuGUo5-T!`^$Uvfvr>~wiuFs2;)^qL3vyE9lk@XRit=;x
z3o1)8^7F7rB<Eyi>w^frf=aMWqr_+kjD`S82$Ytj>00nI7#kTHfW(!R6cvPpLP5+?
zJQ@O{Aut*OqaiRF0;3@?8UmvsFd71*AuvKhprn9<p`L+(feGs0ASlg<Msq;<sZcr?
zO2gE#K>3DHdIyw-$+JTFAyE1Nl!nQ(LHU|cS`aD$r5HG&;=WKi2r2=k7}%lW@ld)5
zDgmV!n4#jHQ2HWN0!l$eN2$>e7!85Z5Eu=C(GVC7fzc2c4S~@R7!85Z5Eu=C(GVD(
zA&{8Dz`(=6z)%3{2n#SU!216$_!$_k@-r};<Y!>m$<M$ro1cNfnV*5doS%U~f}eqb
zgP(!nJ0AnXc|Hb)Og;vN3_b=1e?A5VcRmINb3O(JH9iK02fPdnTX-25@_88;%y}6Y
zKJzdz%;RBTaOGiO_{7b?Fo&Cgp@N%%p@5r#L4uos;V>5iLmd|bLoF8rLkbrIgEtog
zgAo@4!yirthIO0_45ge544Rw_41$~t3^zC!80K&=Fz9hGFz9kHF#Kg_U^vUpz%ZAc
zfgy*TfkBU*fkA|wf#C@o1H*bY28O9@3=C7)7#IrJ7#Mum7#K9z7#N<hGB9+rGBCKY
zGBA8%VPKfY!oc9k!oc9d!oXm}!oXn2!ocv7nSo&;GXp~;GXp~?GXsMoGXsMrGXuj9
zCI*JHObiSwnHU(Vm>3vrnHU(pGBPl1W@KO}Wn^F|WQ2riJ}6v~la7*-5<_`LW^#r?
za(-TMNl|HXNwGpnMk<I4TIG<FTBMLxl%K1Rn5U3flwO*fnpdJwk`GprmYI{P#{g2C
zm|Rktm;=+JkdvQZppckWl3JutnO~}2ma0%tTB49yqL7)Vke*uNoRgXD3|T~xlUk(1
zPz*9Iu>_=6AtgUgy+k1+u`Cs2OL=}#QKdp+QhsTPLPmZ$NJSCM7Ldy_ixn~w^HOqB
zQxr1u7)mnoi&GU6i=fu!DU@X<Dx{U>C70yq=M?J|Bo>uqCg$L9L}r>oBC^pLiNy-V
z`MIeIi76?WC7Jnoi8=59DAr-fO-;<p%u5HkNFg~fPocOxu>ce(AP1BvlxODT=rLrb
zq$cL%RO%?CmZcU|mSlj`r{)x=Dr6QbBq|i-Bqpb3<maS-f}jYjQlX@%G$*H0A*-~w
z1Z0>(YGG+2$ZoJTAdeN5=7Fr$W6;G87@&(&a#D+m71C4lQi~FE7(j8XOdY_Wtbh+7
z+m272G6Qv-qs+hnvZg4tq_ikcp&+rSIJF2Y0wzFGAe@n!Q(&u<SDKrYTBMMlrjVOh
zp#WN*qmWsWS_BT?VuhsClJeA4kQpTz3I+L@c_qb4ItnSNX^EvdCAMyfImM}(U_&96
z=q0A4#KZD~hPtk9S!R`PNosM4Zb4#6hPsYIVlpV+Y!i#p!5Z|O^7C_26Z8BFK*EVR
zj$mmWg&44zAV-2Q<qkD8FfhRD&fLTbU1%cG1%-hwA`H}Z6iO-!Qf)KyO2~0oNj@lV
zgK{mXL`cglDlUNpBvLqnoJZJwnR%HdL^zH>5(0;;vVsCALKM>Sb8_;_K^d?hu_!S&
zwFHzu6qF%q@EZh4VxZ^&VOWYV&@(WE1T;t#gdvfalV7e|kXT%-kd~PPD*uvFOA<i|
z6Jn<lEMUQjq6i}ufmFkyAuT6A5hEf9S)Z0)l$@%NmYSN9l$e~YP*9X#kY5Bzatg`$
zscC7M$(gBnC4}6Om{X9Eh}9wZ!wN+f61WNq5H}U4<`sj&KQS)_$$g|o2C@a%BSt+X
zF}EN!C7?L9G$lVXuPn8wI8|LoAvq&IGdZ=`HX4V#dRAg`eo|&)UPw`5UU5M_Sd)5Y
zUYU1hUT$E3dMvicE>6u!O)gP@)}J|<$=M2tIqCUDnI##y;3B&`k#Japb0MPOgoS!>
zNf9iuC@X~cyZD1DB5<Wplv-Q@4qH&F17S$Y1Nj>iz{SalIf+G}TCOBBJ0m|Yzf7Sh
zHN7+^u_&_&T-YJ!$5dToh{tdwNSG12Ak$$^!<sT7j(}u+P+eJ&Ukt7`6;g|e^1<x@
zaLt^NSWr-@P@Y+mf$9t+yv|6^Pt4H;8-&#j$f*Lx28WQc0;HgamDI%wc=9|XJ;5uu
zwEUvn#FCQKqWI*T#NuK^QSJzB2Dv~ghhmS^oB}tPVu&~3hUBLylqD8r=9d;LWM}53
z6zeJYCT6E96qgpIf)hw~UVgbkc}60nx(9g^)Hp~=P0NQWC@RfURM&xc1`<aKwxD(h
zn5_qMN&vX#(@04zPA<v>S24EgL8W;L$vOGOsVR{57&I?{+>x4_Sdy7s46S`Y%>z&}
z$}b01fAG>Vu^3kUDdgv+7VClCke82WoE4SkDI_WsmlmWJDdgnmr7I-o=jJBnr4;Kx
z8ZgQE1(l$(uq0KXBqOsJ(h|$e&r<*u0iXt9Wqv87wl2>|%~OEZN}ymV0u@~8MTxn3
z3Z7{o0R>P-0#z-^8Hsu6sS1e-DMg9JC7H<ziMjcuc_pA)y(A+wR|jM^*rAR&IpEAv
zoS2si^G7ix1*R6Ig4KZy2DRN_hUQf&AQ}lE;}jB$k=wmsgG(~<OVcyJW+&&TfLp0)
znRzK7fm{WRq|y?FqEv9xPC;E)9h}e=)Z^7b*)g{?rzBIiJijOf6oKHDbg?ERSkZim
zk}8T*;YkRQj8pQVft+5HTA%=SSW;?oVkxL`S)yL70E(x))a2CS;>4oNoJx@E6p9l;
zt=VD)P!qH~Bef_MVz(Ql$^chapz<TL7}N|d&P+)yN=*T`i9rqO#3EQA7sLAo;B-|C
zwmbwB!R3h%i$DbwG)5G_VFWS~)cS!~=n85LgY%_sQEE<NNotBhZen_7GBogY6w-1M
z(?JobP@Gy)0(BB7y=3MoAe@cd{slV$+OcpfN{2WJl&n)y(?E?9P=^B~t^v+9MVeNS
ztPbwiV65xb12q`pA#G<+&8b(AlbDy6TBMMf3#v=O?Fl27_>%nilFZ!Hl=xs*V;!(y
z0I1dIR+OI`kXVut0IJy-GSi^Cax#<jN=q_x^gwAPzPKPYSwp=zBe5Vgr&3+hiUDdg
zgv!ZGf(gKyj|^a^78Vre=YT3SsG$mAZhT<@OgYFw5T%&D%1z8mOb4}kAeJaVJ77Nf
z`31fZDIJBN{G|L6I4__mAC!Qge2BU5-W1FU&`uT11#kyLZ2^@5FuA<a+=5C_mdb-W
zxF9nxKRG!gGan>ekeSB->i;(|F)$neO^^#PFu>-=FY_}n^zt(>Wb-pHSU~&zI{XX_
ziu?==ulN`kUh*+8+~#9oIK#)lFqe;kVJaU3LkJ%OgCHLR!wFsnhCRFt3@yA23|71h
z3<A6i3@>;X7>@EVFf8I>V5s9^U<l)3V9@4aV0g&Qz_5y&fuWh3fgzcjfx(xXfx(=c
zfq{>kf#DPv1H&>d28MPn1_mcC1_ntk28Q>X3=Bs(85kyWGBA{IGBAX5GB6l&GB6l$
zGBEt*U|=}K!N4$`gMp!ugMq=4gMr~AI|IXVb_RxYb_NDpb_NC(b_RysYzz$bYzz$M
zYzzz^SQ!|Ou`)0eure@&ure?NvobIQu`)22u`)0yure?(vNAB-Vqsud!@|Ij$-=-O
z&%(fPof#5d`$1ub+_xWPkA?u9Ljcr{0FC~{LyH@|qS8DKNSg>*Siu%c`J+(;Dt>f8
zwGU|I2~;xafNIKO9fj&z5QdnetWch+UX%*1;!2@iib{oo#NrZ#l*E!mg?vzJBt1Pd
zFI`6=BQ-A>)cQzONX;!MsRWq@sdvFO2DH%)vPi)e)JX#k4{0EYg6a(&1(>sPGP7ZO
ze{xcbAdUew-jM_qY+=<6w0O-)Ez(FVN-u_}_60ZaY#}D<rKF~1=B2vlC+2`_4rd=v
zXh?xARIo)Uj*(0OnFp~5Gy+kqp$V#H;^XrYb5rBv6>M!4l;Y!a6EpMT<CUxw7$8+0
zbc7@eR7J&?6eWW8H02jnf`cKiK(9ElEVZPfL_<kWA6<!Fa&eiGjsm(cXlx`WGZ!>^
zV5_d9uA`8aTVkuOs%M&}4)Lq9LQX2IB?2DKD@x2u$<GD#i&H@jvHa4!6i9nO8QcH{
zjgdiG3#G-WpdrZ2Vo+1KD7CmCKd(45DF@ut%E?a$k0YjmNKm7&SRu0n+B^pLlB^Vx
zGg6bY6*AKl%2Pq3Fr}a&IEB2_)D%c76x8_4OarH$qSV}c&^Tj8szPyMS!P~3I5?CQ
z@{9Fy6SGrOGK&;+3qWm6P_hE?Kzczz1!_)#8j;{;ZDz4TX?hMMD3uj75_8}!#7uAt
z8Z?@bnp3Gzo?nz*tWcDho>8IzYW0H*%Pa<sb|xjJKwERUnJKW)19$l#gPTy-Ddd7W
z1)yQt%;J*Nypo(s{meWN13Wkhbpd)jDcCCH73gIYmn0@<YiN|{C=?cJLaZo(NabYa
z6&EBXr)n5LGMipWVyS_SLQZO)21u!1Q7X8dqM-p5(J_STS4QL}1r1PmLBcmbFI54Q
zz`(5|9Z*Q+fk%Hq#wyq%q(Cx7sn8GydDcn+VF;x4m7@c0qC<v+L6HsW0i>iBq~@h4
z<mV~mmt>?CDL|4H)Er33P*4hKHYO^h<Q6F8m!%d#oK~C<P7^srsfj6-&@twWRE3hv
zTu>tuEDAQKuox0FiA6<;m7q`ni^4jV#L*Bf3ZSTi6hQe&C5f4NsVR`0qzevnaA>3^
zm*f{!;_H~=wh@xKL4~feLP1V`3B;fId7!{m0F5a_iW;zEK&lFh^%4s};ir+8S{`2*
zZDocwuAvNSLh2Rgq^1^V80Z-om_pJDNG}Ff0uPfaDOf3l#FxjX>VevN8k)8EbbuAY
zbwKhh%v^A(r&n5#l30=&mRY4ytyBn733e!|O0X#kwhG0Wx$zKP>8T}PL1+OAGg2AU
zL&;Xi%qz)PD9bD^P0Y!xO4Tc<R7lG&Qb<WHNzBYC2BjoW2VA{aA+anmGbb@A2a+m5
zhJYpoiqebq$}+3sL8H6zpuT|>JUGE_LvctUG!!6RGLR8qHxwx3<fntnvqXiI%;XXs
zFsYE8T3M`+lL;CqElMp;Eh<YbR>;f)b#?NKQlKN*ATvP4l#d3;yP&dP2jOuY1<*(p
zB*Z~}0`WjyO@uQ+njm2mUtE$}01Y67XrY3wLSa0pjDorx>^ZnRdMyN!0J#BTC5)$+
zn^+MKstUm4OQ75X9_Uq2h7?&P8Hsrcsd=D&Gu(8LA>e8!9#o{;Du74W6)@}e;>=t~
z8dk_JEdg~(Asx$1NFNXG0C?<z8XlPir8$Wu`9%s)V^J*yWeafO6c5TV;Ql36b!myk
zC7><8AhSTD7Pbl@MWv9W0k;<3e~Hh}%c)erqC~+~0hBNFQc_b3Kn$dM9OPMr?9|i(
zg+%CJ1*Aj+1q~?8CMH9M`II3gU3q44szP~cW)Zj|%S$f?&oU{12GmmX;5BI~w5rQj
zNG?iEECCIb<fWFQG$kN|5{R(`P{JwB%!T?BlGR|fH)ucsJf;VlMa#_3D@HFt^}uCv
zzCv+6IF!LfXLKx>tqktNgKFWTR8TcnP?TB>8jS)KPno%?3gA&6(8LX>^aW22mgGZ*
zTZ$4Z6+i<6&_<TBLUK-OVi9y0BPkWu(t}J_f`-yS!vYE#nR&$}sYNBAW<$P)p{7D{
zNoGzC%s=I+(78qM;0kCmF*h*}JYG?h3JJ-iR2_xle1$}XoXosz(5M+GV`dgZvNC8i
z0hASW!CieTh2+GdlGNhNM9`2~NrpmtW?5<;DCd<V7H2D@fd<jQVSpH=0CVAi4k@^G
zA(cTqxH3R$mVv?)GGhTQ$RUP<@-!rxA%=ntgtY4*rhyt1o(g&3hEzIu0u?l_2p)g|
z`6;bLx3mD%Yy!1~pu>46V@BX4TndUIP{o=G2{`}K5>R;r84LwSD73nSG-tqiKnVy`
zgn}Ils&c`0rdAYyHqL{c2}z5fSx<2LL;>t!<k}BpL}rN|xc{%n&cMJ3THr6hzyMpn
ze~_Pnp_8A1!H=JT!I+<cL4coufuEm&fsdboVJ#m6!%{v5hAut^hB7_|20K0m21Y&x
zhO@j3486Pz48^<*49>g^41&B244lw;|A#yb3}<*480PXYFihiNU`XX*V9?}YV9?-U
zVE7K2`{!n0IK<7s(96xh5XQ~G5X#NKaD|J3VK;Ow01p=f!zpOLKa!Jy!IqPOL4%Wl
zL79_*;UfnF!zK;}hAIvQ273+$1{Ds7e|k9>7&15*7(6)`82+#`FkE40VCZ9KU?^i}
zU~pn*VEDoY84D<9V_>jnV_;BYV_^8s%D`}%m4RU<D+5CVbR6Ir3j@O;76yj-EDQ`?
zEDQ`0EDQ`<EDQ|nEDQ{6EDQ{Hm>C$ZFf%ZK#sju8GcZ&!GcXh}GcfotGcd?8GcX)t
zVqmCXVqgehVqlPGVqgI6J=n#_z%YZ6fnhrO`T%JE5tQ@`iZb&`G?bk4b8<3^K_y~J
zYDsExNotCsq7tM&K<?sEvFDeQpAM>Ba$%N0c+u*G#p<z;mReP6QGT%oqywp!o1c=J
zqnB5x18u0nv@6&uRI8U4rRF7Ps4G~3n#B-pW}3C&L<%a8Y1y#^)#9OfC7C&(zF}f=
z35Y7n&jEEmAx&?1SF^YvH8n*CJVc7rxrBJ1v>s<-aWSYw1kLMefKAl|brACk^zu@`
zty|Q>EhRs-7;LLTDaf6m!Xhm-H3zZ~0<wMqG;;&a=V|&S3ZOZ?e8><!xJ&{EBf2-i
zf}r9PG#HC639b%N{S6zPg%oWHNkyrN*={+B>7Wq+9Z)+8oEE^Q!eTZW7OkM<0EuBl
zzr_qa9l#S#Awm(PiG-eVAfm;oc`1P@8imlVSz!vOPDdRK(D2exfCMktUCN-Tkb+Y1
z#78Qmj8`a2&B;&BEXjn;q<Ddb95VCtlJj#xD&0U;h8J`W5;-A514j=uwxOd?2;-)v
z#)E5_ctnq}0AZRJsGkor4OAcImL_K?q-3V0r51q~@PJxmpnBa|TU!A%q72H;IjK2d
z*MsspJk*ifG$09RApq&y7bjMNduSP%CE&F(kR*~*nv)7G5Hiygz%i_c+ECI2S0oyl
z3U&&n1_qjl1~p26=N0HdGAKk87CF#d4RQ=2SAyIHO0Y^gpxmC5nU|Q8u9ufzl&b+r
zB+;OwVq&2Nf(8W&^l&OtumkmXi}gUmLy5^H@sPqVGcP?JJiiU`gEFWAkp`N51r2?G
zs>sw71<)uDWWp&EBwA3ET2fM}P@JCzT1b@-snV4}c@4ZQ$k4z*6*O9vS(KTan4<$}
ztbw8mJa&|n3TiBsfGT#dVc?ZQiAkVgjJ*8JV(65PvVxy~h^rNNBrP$y1T-cEZs&qs
z3Q7)%C^IhLSO*0Ks4D|1dK46G6%6zYz~agZB^jmQW*@k&PX?_*f-J~_gdw=gq5xTX
zm6BPM3SQWvker`il#-d3Sdt29s4IipMj5FJ2qmDEVMVD5>G>cZWx`vq3JMBD<a}j}
zp*>Ky3hH3UC;(^*3^J?(o;A`@0L{&T+CHGx6=+xqG<6ROSjPZQ9Y{HdC{I!fi{q1X
zQ^560VG53Nq71WSNrBc$kR+R)S`wI|p-H@UsBVG<6WF5=JFt~bg(>klsc9t&+6sj!
z@!%0Bum%_hTloc1rURO-k1qocNkBv(l@826TX5}wR3~Y8fo($32eKNX4SzYPkdz2&
zZ06^I(`iwD4roPRWqv7WEVno{1?*%mh$|>7D~sYWN=t~dDKj<)(^#B^5~v`|LoHoW
zi;7?&4Gu8{8%Rf257htD1LgL3@QMd$VToHeID$2CYKKmCB3TF40QGxLe!7`Puq)Um
zNLXv?LE;~hC6Kg%T!~E=NF1RF9Bn8Df~pYE01?D^y~N}a&%81X@K`uR2$EP}dO*g(
zwSYLNIzS?tkPZ)$5el$Db+8ynhXyzVOF*l6K%Eyo=<){*XlV~t2Tr>LwL+Yy3<(eL
zFcVmx4rJI1;sv-NXd_Xe;T1?e1(gNR(HB@T18R^$nx4oN8F(ZF9EzYu6DUQ2I(C^Q
zprIj9RSj7_T2PRa394K2L1UY#&|&4oqFhLI0xH(vUI!KUDXGPo>7da^P)7?S3kpVs
z#G+hCFRvtDAt@Eq_YQV7R>)5R%??0UCV<q!O#%<^fQ<u<@`4ug<|pQ;7eo5H3g8e2
z$%5RQkq9aHLF`0OPcN?+6b%p)KqCMdprSu92i$&0%qh+X=X#jy6H638(FZaNG87D2
zBmkZRPgDR+`Q$(~gDXN<kpb~aN<L)OA*hrG4^S%P=7Xm3!DD`@iOCrtUxV8ppdba=
z0<jd_fJG4C2msaf&=$ILPG+`CX=09JQ7%{t6gQv-Ep{c42AqNw%D4+ggI)*hNl?EZ
z5;!^vnV^*f#U=5e#vZ6047O?{RfnMYe_2KbhEC7{00ImQu>JY}`574Y@-r|j=VxG;
z%Fn>iz|X*t3|$8R8V7j7$H1_XkAYzV9|MB}9|MCt9|HqFbPQlAF9Sn8F9Sm)F9U-M
zF9U-;F9QQFF9X9lXx~4Yhk?O?hk-$shk-$chk;=yHv_{)ZU%;l+zbr0+zbq6+zbrw
zxEL6YaWOFL<6>Y~!NtH(%*DWv$i=|m!^Oa$$i=|G$i={LkduL-o|Azgij#rCnv;Ry
z9R~x$F%AZX257(EgoA<M89M{RT6PA8Om+qaLv{uR33djCmuw6Sd)XKmy4e^QilBP|
zZm=>iY++?!n8?b&P{Yc=;Kj<oAj8VQ@RNms;Rg!?!#NfPhGQ%Y3>#S(7?!gzFyyc>
zFr=_BFxas$FbJ?PFl=FFV8~)-U~pn)U=U<xU^vXgz_5mifuV~D8n)1W{V0Vi1V9BZ
zw6ilh;z8zAI(lz^bi^aM1l%8n_TpiK2GAxN5d}T0=`cFtp`f6wFgoG^ZXh5PRih&w
zc_l+_!~@hsPAe?}O}c<rcz_3X5<%1V#rb)l>1c(-;^NX=@Um_NSc_LZCr1Gk+2Hm$
zXlEHXB|wH`kUJyc;PGx~$3q!SK0ZCQ1fsw*&(#&C2sDP3S)u?Q?g1^yC<c%7f|uWb
z7xI9tgc_6yS!qxVT3HMpjs&|x8Puo;jie_+ms+G}f>zgnhmFCbq_DvtS678hy;MDg
zr2LdhNGAfKA8I<RPX~7;+=bx&5_pXd*n6OULSB9zXtWSCr~n!`gD!_5vj3&50G<^D
zuZjja9<)$g2jaGrd{ED!Jh3PRY2`fFnP5kNMy65bnL!-`aE1g8!RnUe>!Nf*pxp(~
zx`YBqzaujbJah^lh0RX`hf`@@CZw}h0$N#}oL`!g0_virR+OaXrGVBlfhH=!eWT1|
zuxp@f*jNhK29V3aBQ^>e;FUz6kv4EoAJjQY0}Y2l`%0-L$<Y2Dq6Y+KfsMvfjf3Su
zUIz7r@Tfzs^)R|c;531ycZA*>0tG7(b>!&SpaOIx3ES8p(rPgH0K2iKjzVDybYw&s
zzR@T*vl!IbMw+@%NJ>o32FE$BSrEi{5p385I>7->)sWd=#8e0<x2A#@b3ro(wjn?T
zTLrIZD;;Ai*s_A;QqW$6G_YpK=49CBEwIz5J+lHzcF1`hz6=C&#sJCpxW?lk69ou`
zSVq(kgJ~eEq2pq(Au-U{7HIqoGOFYY8o+>sG-S^uWc_1#Mk=U6fv%NB+Ft|?BJd~{
zay0=KfEKSrmYK*w0vWRcl>yLr1Wjs!hD;oba{cpiDk0+`L}YPL{~t7m{up$?o&W;_
z?EJm6{0t07`572?^D{7P<Y!=5$j`t~!_UBA$IrlE#?Qd;mydzrA|C_8Vm=0j1U?1^
zF+K(cRz3!X8@vn*t9Th0DtH+f@^~2-6rgJWICvQt_VF+<Y~x{ISOeV$Fo}nOA(w}N
zL7#_#L7j(z;T?1hz!Yu<1}km`h7Vi}4EMPh7&dV+Ftl(nFa&WiFnDk=F#O_VU;w%I
z5hnw~8BPX<U7QRIi#Qn=7IHE$^l&mT<a07G1adMkC~`6|C~z_`Jmg?tIK;uguz-Vs
zp__w&A(w-J!JLDEfrEpA;SxIoLq9tMLkv3ugDX1&gAO|bgEl(@13Nnd!$~#<hBD|r
z0W&rR1}-)RhP$i`4E?MO3?-}#3^A+>47RKc3_Pq13@=$27*?_{Fsy*C58z~BV0g&P
zz_5*(fnfqO14At{149%u1A_-M1A`DV1H(-w28R7i3=F%N7#KR37#KX67#NJ17#LWX
z7#NtD7#J49)&+p$7`FBWw9&;4wzD=ovm_$`vQ-9eX%AY*5)a!`W2Jz)%LFv&1xoao
z#XM-;Z9Hg|cRXmO2W=u4ZP_ApXD*_%G+@&*WvMx!c_c^<g>n>Z6+pWw(e|i<ms=X?
zD0m^yrD#G1os|(f;X6`6Q_hG|L`NYf$u=iRN1-&ywloQ1U1?IZA!tI}K+nLy5S;Bx
zli+zCGMfr5i7S0TE4jf+a-f=Y5xR0fx^xY-ASDhijR?1pTcUvyMzE`Kw5}m^Q7UX2
zGe4;ew1YCI61-{~qzq|4H)vKb6TUyNI2E+oHWR!$F25)n)ZhWF4Fh|Pn)L$MKyX3@
z?OD)+FFz~-tv>|qv(f|Az0Qygks8%XAV(`%f!B@&yBb5+$RbvkLYJn3G{Y7zLh1>~
z)?X_H*Dy~X1xG&@1rNtyg%E!QCs&0aS6}}yR~H4hAb(#457!`9u$#a;Pa)%01qFKG
zk^3C*pgSZ@D1*lT3kpCDaFCONU5%mkK(!>6WEi=`gI0g!rb5>{K-Ga_6(Wng^9`EQ
zL2BX)@^e7L*T^R5fX-To2hH=vmq8r>>A@7Gf>r~j=D`jdNzDUomWQ@|L2K)vo-x#f
ztO_@C0f!xE5h!Q@XmoUiflj4Ctd5?ZUaSISHVCu^6=D-)O%U=-KF*{YZDj;cY{eO{
zR(3&ADzw#-0&bi``URkr25yaKg7#NJ#PU)TArml3nd#}Eg^2}4`AIpckU0WSs31ZU
z;wtc}8x2$$qzPhOLmeY%I}D@-TbP22kCH@)r$I>&xgthxn}8+}K&yR}Qu6asl_2p9
zQcq&p3erMhNs4L&v6I{&U66<a<)y$B4S0JMA_^&q^2(s$1yT;ekd&;Ho)6k)0ZBE*
z`3iaY3ZUZvK>JD|C$T_RsDO09@?lD9QfYcTSbs4jxH8iqdx~sr75wt^QXztna0QzT
ztH<=xQ%hVDOA?_iC}mLB6x@vm^_7c3y<;Ot0){4Q$np%Z3UK!@C$+d3)F&%SO-{^7
z2Cc*e6=SJjJ0PdWfOaXQLQ^^<Wk9N0a5>`!+N6<MTmmm|)C)i>+zO!OOaZ*@h*Tbc
zQa;!<;HDqEULZ7o3D$yI7bB)IVaWs`f}A!XjYLp@gFC07!VX+g7L`(Kix|v)NKpf_
z2{OG3Vu7$aY*`2L@;|i11erwysRm(?4p30yN_uFj3yX0pF4Tl5hB*=xNZ`<<^}0i}
zCOLE#6FvluwxAGwQW6rDXo?`)&p-`)=#W0@J~v2^`hxl<ptUCX1(k^P?;yc=aH5Bp
ztc<)w5lIqMa)HKo5i7?ED$9!!3-p3rjlsbN8g&3gQ6}Vs0LU&Oq~%FP`6b|r4^#(b
zr-Ig3!6Q+_SjQMzYJiFsg+v9!yf<hkGRPT_krBuk4R~E7XmrB|!bcq%@qs%B)Hnt!
zgN8A*b_53p#;6C#5=cN`1UYm*fG*2vK|wg0OLAzr?GqgTdLuYH5Sb4&t-$O5Wf&M3
zwt)JZ0t^hW^YiEOGcfe<Gcd&SGcXwOGcagC_W*3;V_=xi$G|X;kAWeKkAcCDkAcAo
zdfwlCUIvEg(DVM%c^Mc2p>zN0ybKJVco-P&@-Q$Q;9+1`$iu*p#lyg$#>2qC$-}^K
zg`0sPkDGzPgqwju2HNjG!NtHZmy3ZRiHm{3lZ%1jCnp2LRZa$mwVVtLE}RSux||FQ
z+?)&yA2=8oZgVg&T;pJ1Si-@;P{hH&U<cjzzloiJp@p4+!5?}S;7>LNhEr^ie*FqI
z28Kd528K*F28J-`nSeZO3=E%H85qv7GB8YFWnk!FWnl1UWnfTdWnlOUn&W3-V8~)&
zV2EU4V9;S<VBlb3V7SH1z_5myfngCd14B16149P1pAWil;0qI^pMRc-fnhTf1H&dJ
z28NAH3=EY_3=BR@3=A$z3=E1)3=IDm85rI$GBDg`gv0~r3_)D8<REcS2?7}f0B!In
zD9SI(1dUKT!$t|ftr$q%2QE0F1h~`$pC<v{znWPB*+L7I#X*5Jfhr@o>7X$P&%Ba+
z@RDZ`2do5EIe^>-R*pDs5W3w>2Xp{mUTQpO%bcba1E}Z*D}=ckWE6_I$cY%z&jFp!
zglL9Co7KpLHrOek<5nOIN04KXrIEHsWTt`6zkw)2R-!=SMtRhh8Df+KQrChU0K#bH
zJ!HB?9c4=dB)(9MgSS|)m;oALA-+{rl30qnp;QKKD1mGNVc6n+WaG`y)eQQUSAK3m
zPG({r`0z`V?H-V*0i`{pb+uG#V1Y*ihCl<WB%=tlJ5&L*sRVX-9XRuVTV9~PI5_Nl
z^UG3w6m%656~L`5$dYc*0)Oy+3y_jz(4PN{%+xaI`eAS}0yYWN5U>!akO1d<Slock
zN6qKRt+b-jJX=r)ThodGGD(uCP?7=adgbS&fT!9(y*)@~f)oqTA_3kB(gAM=$32Qk
z<3;VDTn8EuEJ-XSen~pSKj4M%5GFz^mYp@AVgO_Xq{#zX^Gd|}ci12ocy>7{za#?^
zvB|J)v7o2}ZKi>CWE4P4Src;<axx*!dib~vI8v1rkWOGoEKw+j4eF$o=7GA{Aj3d=
z7%~;oOH+#>i&T|CJ0zfoMZjjeDs{l4ji4xno@)l4sDkdWfOX=C-%A8`93)skeGb$O
zMcUvq-HP?V=PoK}DU>9Zg4HXRB$k2>O-L*SZPiB(dDtlo5CvX(Aqox(koW;>1W(UF
zL=g+eGxN$cyc8T1K;qh<9WkkusTy8-;PV}#4Ptc^5Nu5aEy#8k*a3j?pdo8b1qY-J
z0$_`jA!of6<dlMjR#HHfY9-P}BhbllP<5by2gM>NMA7E~A>Ar!4$y)s4iJXtJ<u2k
zZ2$Jqow$QUv4X8a6l#1&K}Hu-G85B5!(@<A#%M!5104k%1Z>+A+(huUgCx*#rCQLG
zq@b;!VF)U2K}R5fWI(ARuRt%cEVU>xJr!XQWLhS%BoVPJO;bk!bbq!j^t4yhh6F+x
z#_#~dIgmOL6n_vt#CM?e9mN`E5J}89geV1VgaCUbH#09Yw=@^JQ46$=CLX;1DIT-|
z4ARm9jl973%Y)7dNd_JG4Bkiu8o?`1h0iM_=YviXO-}`<Wr#({LuQagG|<sM(gxH(
zo+NAe32Kmn+Gdbu9js_Vh#=<y#2_4~_5)cC9*Y?s3ss;M1Z?sd99bZ5LMnOit?ghr
zSbYx~z(TI>F~(@Htw2F;o+Gv3k%g(-YR47PpvZ$nIfw<qghngD0RmD8!l3Qb$W3ZU
z-2@#}$2nMu6vW^eA@Gsv8lV{)@M#F3UK6A*n3<Q3G$RZhCIp2(O5+#Ok%A4QYl8d#
zQVa|Xr=b0R*!_N>bN^HM85qL(85qL&85j)s85qQ%=l-+vGcY{hV_-PW$G|Y1kAb0|
zkAb0)kAb0>kAWeXkAcCKkAXppkAdMIF9X9RUIvEEybKI8plbqbco`Tpc^McOL2^6{
z49j^K80PaZFqH5xFnB}91Fms1Fr4IOVA#gZz_5m!fuV_;fuWe2fgy#PfgzlmfkB#^
zf#D7p1H(Bk28Od-3=A8fdjuwNF)-9{F)+AsF)(m)F)*y+WMD|*WMJUuWMFv3!NBl{
zgMncU2LnS62Lpoz2LppR2Lr=9b_Rx>><kQ(*clj-*clic*clkOq4x&tU}IpI&&I%z
z#m2zk%*MdL$Hu_$oRxuL9V-JvGb;l_6LcMb1uFxCIrOZ6H!KVcPgocjF0e2#>|tSG
zC}m+_&}Cs@U}a%o_{+?|@SK@};VSg}z{SiA4CTxW4F1dv3@XeF3@@1&7%ng|Ff=eR
zFxW9MFo-cRFfcMPF#KVJ#Lr1k9DxP^uwqaWgB3+oY9(RWeTXty4o==w*mDR<*oeK<
z6z|Lig#>aD4C+FFor+n7Atup5)fgm4f-5s{Hw3(N4^nd>6ha+~ypM0Ft{(yg6sWic
z(Ic={%E`=ztdqj1hR_BF@b43Yp2(^JYOtfNF@Y4ldZ=xBa8gu;oskUbxPr$GpuJc4
zij&~XymZI1{LB>J#JtP`-^2=t&dfB}n$w)rBJe4Mps5=r+}fa1(fI9fjnDQa(R`2=
zV$Jp>-E2>&*`U;k9yDOnVAIHGv4t8MP(!eIJ|1#py=Puod`4nU8rVo>{DF(6g&4C*
zGZwdrfdTO3;2NJzL2yDk|DfZ9b5g;V%Yc@{C#HZdbOA3J1sx_0b~IAB!<Ku(&*}#&
zz!&})I-ov7GL)=*gU?PZ84zkKtdN5odstqSS&|A_*#cg&3BN)BGB^QQD*#@20$&CW
zIk7wiG@enMpPLFfcLOxg4C=OImVk~=hnyvsSOB^Lq9_w|5*YYIEm%mEWTZlmKuoPD
zDN0l*E-6h*gPlT|2tFqYy0||nH9Zr2=?vs33TQBa&4G20Km#?9wL7`sZ32nO$*Bb;
z;OHv`Eq+LZo|s<@T~iMp0)U=%0^W)X5&+k33Xr|$knsRd&~X`w3Q3tI;1f1=z^sz|
z67Y$hh?xVh3I5)o)38fHJAEOV9Tkd8ia_hXV3vb62|!M_%2$A1+yL6e3~@K?<P7k5
zAu<8>C-i7>Xd^ZUdNLm<7C}7=3|VlO0&OK@A>=#*@VE@@*lvvg@PRJwItm#&3XVDo
zNuX7Hpe1~ubI3pkecLJ(mL{eY6y>KwSF0$Ksy{V3zZj}FLjkgg1?(eb@KQj~nidEb
zyv7f~g&w5}-dSCgpOlycI=?hAuS5ZS<ZChbKn<`)N6_MIFblLu5YZ|D3%MiP2wGJP
z8an`=sTpk$tDprRZZeG3QAhzDQDm!>l3$vX13Nti+&wQ&1>YQ1lCNL@wiYrrSe~i?
z9rgnqjt07h0O_26usW~{log6olk-7m9aSnowzX%Lf%ngYbV2TiDg-MGfQ&F84~XG%
z6)4(4#Rdw_Nm8(}RVYjWk)=rr8u{SeI*B=uu?iHWULa+*3P~VA4bXiFprZ;aH9=ej
z$Z}5z6H2;+l*i<x7Aj<5Gq$)Cyjx$P1akf!R1=)iRw#@M0hs`I5w;LPxDq^Ql3A6i
zV5o1TP?!=Iq7VSm4Gtsd2?|KtU=#uaAk9&@eIN!TEGi&D0oDl;g=0sB3I(Y5;bIU5
zrdoG}3I(X&AxhvRTrHLWK=DKc*bfz;vrytf6e=Jg4BIRNb3Z)CgN=nZUBEn0e1oz%
zTolX7TTspcWn5bj12R33Q=G2@KKM2>Pa!7>Tt<~9F+gfNE3gEj<OUy5S_oR$2r38z
z10ansXd?r;<N%G_q71=;9EMT1p*j#xK?E_5>;Yobb|mW9Fvwvkm^blgpy)wg?F^a!
z2dzn;!NS16Ai%)j3+-Ei_W!@-XJFXF&%m&SpMe3i9-xh%fgy;WfkB&}f#D|~1H(o>
z28KdD1_l*A1_n_+1_oX}1_l;B1_ow628Iq^28IS+28JkJ1_ldW1_sdie>Zs;7!LC=
zFl^*uV3^6nz%YY{fuWg)fgz2Dfx&}^fkA?YfkB*yf#C)><V=9+(7r#Y->=8b!0?}o
zf#D<<14A1Z14A$u1A`401A{Ub1A`J51H(T~28IKi3=FxP3=DeE^#P2W3=CH|7#P|(
z7#OlS7#M;%7#Kij0x)whFg##qU|7J;z%U=WK7fIpf#C)l1H)Q228MPv28I$g28KX3
z$hm-=Yzzz+SQ!|$u`)1hWMyEeXJue;U}a#?Vr5|X&BDO&i-my!9DdNS8>L1=U^E0M
z3jt7ZMsSc5-0p*}tHkKTBQM%j7~S>)yP9ou+Y2aRjBa}w-Sz@LQX1UiR8|<>_JXmM
zXmr~PWN;XEyz%I^mz1Hl?FH161a)9j^HOwE)6&4FY-$v1LN_ylhRZUGz>8%;f{Dq=
z;3HJiL4$nY)729dpqq$vVY_FG3qWVs<(ES434nKEu`$2}3&9o)SQW^bM74LI<6)pl
zUGkegn3EZxAOc;X3wI1iAQ5!Yk3xDr^gP)T(2bFad5{YeKnG=j4loAEf_657PP@+o
z9cP#VI?5k%a{%OC5zxItsS5c;pgVBEn?t~Mlz@)Y1@*j(OF-8gf(D>LSIod}Fa_N)
z4!<TS7ezZ{s4)|C2_|SAF=&$swi8o8<BYKJO341l)I9K7lwy7G(Jly2DHN8b7J~*c
zL8F!6ST6x>T!Rd~BDouSem~^+U(iTrBKTgRoXosb_*K@R8!RCX1i1-x<9K382IRbi
z<kBL@InepwBM3pah=4C^g^bUF0|he?frY^fFX1!%KACxtB{8rxgDH)iGN2n>z;k=x
RQ4+`icxG+^WbB`T0RY6SP-*}G

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index a9eb3f1..5e5ec88 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -187,6 +187,50 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
     # hence the empty dict
     return breakFlag, {}, log_item
 
+
+def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform, i, past_data):
+    """
+    controlLoopClikDualArm
+    ---------------
+    do point to point motion for each arm and its goal.
+    that goal is generated from a single goal that you pass,
+    and a transformation on the goal you also pass.
+    the transformation is done in goal's frame (goal is and has
+    to be an SE3 object).
+    the transform is designed for the left arm,
+    and its inverse is applied for the right arm.
+    """
+    breakFlag = False
+    log_item = {}
+    q = robot.getQ()
+    T_w_e_left, T_w_e_right = robot.getT_w_e()
+    # 
+    Mgoal_left = robot.Mgoal.act(goal_transform)
+    Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+
+    SEerror_left = T_w_e_left.actInv(Mgoal_left)
+    SEerror_right = T_w_e_right.actInv(Mgoal_right)
+
+    err_vector_left = pin.log6(SEerror_left).vector 
+    err_vector_right = pin.log6(SEerror_right).vector 
+
+    if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+        breakFlag = True
+    J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
+    J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
+    # compute the joint velocities based on controller you passed
+    qd_left = clik_controller(J_left, err_vector_left)
+    qd_right = clik_controller(J_right, err_vector_right)
+    qd = qd_left + qd_right
+    robot.sendQd(qd)
+    
+    log_item['qs'] = q.reshape((robot.model.nq,))
+    log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+    log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+    # we're not saving here, but need to respect the API, 
+    # hence the empty dict
+    return breakFlag, {}, log_item
+
 def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
     breakFlag = False
     log_item = {}
@@ -291,6 +335,27 @@ def moveL(args, robot : RobotManager, goal_point):
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
     loop_manager.run()
 
+def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform):
+    """
+    moveL
+    -----
+    does moveL.
+    send a SE3 object as goal point.
+    if you don't care about rotation, make it np.zeros((3,3))
+    """
+    #assert type(goal_point) == pin.pinocchio_pywrap.SE3
+    robot.Mgoal = copy.deepcopy(goal_point)
+    clik_controller = getClikController(args)
+    controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
+    # we're not using any past data or logging, hence the empty arguments
+    log_item = {
+            'qs' : np.zeros(robot.model.nq),
+            'dqs' : np.zeros(robot.model.nv),
+            'dqs_cmd' : np.zeros(robot.model.nv),
+        }
+    loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+    loop_manager.run()
+
 # TODO: implement
 def moveLFollowingLine(args, robot, goal_point):
     """
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index a9cd447..1360ab9 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -563,20 +563,37 @@ class RobotManager:
         return self.v_q.copy()
 
     def getT_w_e(self, q_given=None):
-        if q_given is None:
-            return self.T_w_e.copy()
+        if self.robot_name != "yumi":
+            if q_given is None:
+                return self.T_w_e.copy()
+            else:
+                assert type(q_given) is np.ndarray
+                # calling these here is ok because we rely
+                # on robotmanager attributes instead of model.something
+                # (which is copying data, but fully separates state and computation,
+                # which is important in situations like this)
+                pin.forwardKinematics(self.model, self.data, q_given, 
+                                      np.zeros(self.model.nv), np.zeros(self.model.nv))
+                # NOTE: this also returns the frame, so less copying possible
+                #pin.updateFramePlacements(self.model, self.data)
+                pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+                return self.data.oMf[self.ee_frame_id].copy()
         else:
-            assert type(q_given) is np.ndarray
-            # calling these here is ok because we rely
-            # on robotmanager attributes instead of model.something
-            # (which is copying data, but fully separates state and computation,
-            # which is important in situations like this)
-            pin.forwardKinematics(self.model, self.data, q_given, 
-                                  np.zeros(self.model.nv), np.zeros(self.model.nv))
-            # NOTE: this also returns the frame, so less copying possible
-            #pin.updateFramePlacements(self.model, self.data)
-            pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-            return self.data.oMf[self.ee_frame_id].copy()
+            if q_given is None:
+                return self.T_w_e_left.copy(), self.T_w_e_right.copy().copy()
+            else:
+                assert type(q_given) is np.ndarray
+                # calling these here is ok because we rely
+                # on robotmanager attributes instead of model.something
+                # (which is copying data, but fully separates state and computation,
+                # which is important in situations like this)
+                pin.forwardKinematics(self.model, self.data, q_given, 
+                                      np.zeros(self.model.nv), np.zeros(self.model.nv))
+                # NOTE: this also returns the frame, so less copying possible
+                #pin.updateFramePlacements(self.model, self.data)
+                pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+                pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+                return self.data.oMf[self.l_ee_frame_id].copy(), self.data.oMf[self.r_ee_frame_id].copy()
 
 
     # this is in EE frame by default (handled in step which
@@ -650,8 +667,14 @@ class RobotManager:
         # (includes forward kinematics, all jacobians, all dynamics terms, energies)
         #pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
         pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
-        pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-        self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+        if self.robot_name != "yumi":
+            pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+            self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+        else:
+            pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+            pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+            self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+            self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
         # wrench in EE should obviously be the default
         self._getWrenchInEE(step_called=True)
         # this isn't real because we're on a velocity-controlled robot, 
@@ -750,8 +773,14 @@ class RobotManager:
         q = np.array(q)
         self.q = q
         pin.forwardKinematics(self.model, self.data, q)
-        pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-        self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+        if self.robot_name != "yumi":
+            pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+            self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+        else:
+            pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+            pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+            self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+            self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
 
     def _getQd(self):
         """
-- 
GitLab