From df0a9c5aa422ba742218c7383f00299912ca05f2 Mon Sep 17 00:00:00 2001
From: gg <marko.guberina@control.lth.se>
Date: Wed, 11 Dec 2024 12:42:59 +0100
Subject: [PATCH] ajde

---
 python/examples/crocoddyl_mpc.py              |   7 ++++
 python/examples/smc_yumi_challenge.py         |  14 ++++++--
 .../__pycache__/managers.cpython-310.pyc      | Bin 35320 -> 35761 bytes
 python/ur_simple_control/clik/.clik.py.swp    | Bin 40960 -> 0 bytes
 python/ur_simple_control/clik/clik.py         |  31 ++++++++++++++++--
 python/ur_simple_control/managers.py          |  11 ++++++-
 .../.crocoddyl_optimal_control.py.swp         | Bin 16384 -> 0 bytes
 .../optimal_control/crocoddyl_mpc.py          |   4 ++-
 8 files changed, 61 insertions(+), 6 deletions(-)
 delete mode 100644 python/ur_simple_control/clik/.clik.py.swp
 delete mode 100644 python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp

diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index d29008e..f5f8e14 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -35,6 +35,13 @@ if __name__ == "__main__":
     # TODO: put this back for nicer demos
     #Mgoal = robot.defineGoalPointCLI()
     Mgoal = pin.SE3.Random()
+    if robot.robot_name == "yumi":
+        Mgoal.rotation = np.eye(3)
+        Mgoal_transform = pin.SE3.Identity()
+        Mgoal_transform.translation[1] = 0.1
+        Mgoal_left = Mgoal_transform.act(Mgoal)
+        Mgoal_right = Mgoal_transform.inverse().act(Mgoal)
+        Mgoal = (Mgoal_left, Mgoal_right)
 
     robot.Mgoal = Mgoal.copy()
     if args.visualize_manipulator:
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py
index 83fe5d3..15f442c 100644
--- a/python/examples/smc_yumi_challenge.py
+++ b/python/examples/smc_yumi_challenge.py
@@ -10,7 +10,7 @@ from abb_python_utilities.names import get_rosified_name
 import numpy as np
 import argcomplete, argparse
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP, moveLDualArm
 from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
 from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
 
@@ -44,11 +44,13 @@ class ROSCommHandlerMYumi(Node):
         self.loop_manager = loop_manager
         self.args = args
         
+        # give me the latest thing even if there wasn't an update
         qos_prof = rclpy.qos.QoSProfile(
             reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
             durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
             history = rclpy.qos.HistoryPolicy.KEEP_LAST,
             depth = 1)
+
         # MASSIVE TODO: you want to be subsribed to a proper localization thing,
         # specifically to output from robot_localization
         self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, qos_prof)
@@ -143,7 +145,15 @@ def main(args=None):
     args = get_args()
     robot = RobotManager(args)
     goal = robot.defineGoalPointCLI()
-    loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+    if robot.robot_name == "yumi":
+        goal.rotation = np.eye(3)
+        goal_transform = pin.SE3.Identity()
+#        goal_transform.translation[1] = 0.1
+#        goal_left = goal_transform.act(goal)
+#        goal_left = goal_transform.inverse().act(goal)
+#        goal = (goal_left, goal_right)
+    #loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+    loop_manager = moveLDualArm(args, robot, goal, goal_transform, run=False)
     
     executor = MultiThreadedExecutor()
     ros_comm_handler = ROSCommHandlerMYumi(args, robot, loop_manager)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 7d1daab77b3d534c89f472a45ca60728517b18a6..45e06998fec1cdb20af8d2e092947181e3091db4 100644
GIT binary patch
delta 726
zcmew{nQ7y6Ccb=LUM>a(28Iu1k?9WaH}XkqGrpPJE3Cn2IXPI{kkN8-tF|Je)#T;c
z@uH?FW-Sa+vMJ^%7A*`>aw#0a44T%Pxpf{pFn*r=K1YgAqd2uhp)55gJ}I#{HGVQ*
zZXDz1$+@|@2FXkyE1(#}24QCg28JS0Mg|5=Cci4U)|}L|5|CC|<y%4prAaxN#TltZ
z@gPG%GDZ0eAoWE-AiWZdlkIa=c|qc4EDQ`q;*1OolS^|IO+g}rt<;3MwkR__17c_7
zEn!qU!BQxeN-;7p6j_3-2NPBd3=Fr#i&IPDv1qh{xvENKa(S*iKg6_MATKbwY06K2
z+a@)6Uv3Gv7|2XX5FtI;I1d!up?T(P9DE!sJS?00^0+w|6(`RsTgIp|*|FT7F?w=i
zc>$XoBLhP+)8zMl;+y#@CNVOqPF`3k$0#@XK&3e2#K|`*T^KDl%U1cbu~~u4wVhnl
zpw2jN^Q?v?OiJ1y5nT|W$H>5Ni#;*9ur#wM^%i?kYEEimacYt6<b-BD#)Fgln-BP^
zfYhmj)Ul@K7L-)p;z~~~iO<U~Ps}VSG6qS43@Ne)u?)b<a`F>Xii`9?0y-eV0YvzK
zLd<t^K!yY-dwOb#OJYglrOkU=su|h97HCbjYf}(V1F19u5vm}<W^!Je4&$B4v)gh+
kWk3Q+AVcgxEJqOGI$5S&k<os$UHex?MuW*Forf810OX{^T>t<8

delta 410
zcmdluo$1G9Ccb=LUM>a(1_s-s5$Si|Zse2JX1p`GS6GA5baJq^A*1Q!R&7N_v&qY~
z<5@X^88po|OY1y#U@V^eKSzpHzBsi+Ve;xc*~u=sj@-p83=9lxj6950%99oI<@u5l
zi&KksF)%P_GP-HXPWI35V-%hIA|GTLSAjVP4_{F~0|P_yL_x{TX$5cC8Kox&S1e<c
zoBXT7o-t^$Rb>IA#OBqN>lhj3Cr4GwO;*ekW-Cf!U|{H&Jh9q^(RlN<YJWC16GjGx
zBJ;`GP3nwOHwQN@VNy~BiKv4J4MqlrTkMI+g{7HAskhjRQgc#4PBNb?*{a95W3or<
z0X8|1I>pI+ZQ^RWASOslktK+w1=7r#lb@JUT%-vSPy-QGlfB!t7*B34YpZ62XjYm0
zy<I^-0VJsdBIH4Y*<|$&9mdO(LppM##6SWdcNbZJSk@rI9z+yRcI*~sD+*v>U@+VK
Qz2hq*qt@iNU56Qs0o5aM^8f$<

diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
deleted file mode 100644
index 43cc7c57dc1da9d88354597a6af9ed841edb7245..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 40960
zcmYc?2=nw+u+TGNU|?VnU|>ioicEiUD273nk%1vSzqlYjC9w!3g%9T@7H4PX<>6BT
zmaBsqs9#zXU!0j+kdqppoS#=xl%JztP+5|ZpNB;vIVUq)A4KRCRDw+zB}PMFGz3Tv
zfzpyRT?<|YV<STYkX~gaMFnA@P!MwzkA}c#2#kinXb6mkz-S1JhQMeDjE2By2#k;r
zC@J7zsAphcV1oKL2ud@e(VS3zDwGa}(lB)_P`)&j-T|dy@~lw4GnBpzrD5`HP<{-Q
z7KTbdDF$|^cs!KOhe|*x1`eor9F#r>m4H$V%uw-gD6IqaFiajQJW7p*z-S1JhQMeD
zjE2By2#kinXb6mkz-S1JhQMeDjE2Cl41vTH1_mAm28IGqM_7P?0oMP2!Oy^Om7js(
zBtHYgPJRZ4+58L)&io7v=KKr{68sDd9Q+In-}x9A&hs%aWb!dEWbiRC`13I^xbrbE
znDa3(sPQo{Jm6(u*uu-ekk8A&V9v|H@R^5!VIB_ygDVdM!zXSAhB@2}3>DlA3<cZ_
z3=-T742QWG80xqf7;3o~7*e<x7`(X{7>u|W82)fFFs$QbU?}BeV9?}bU=ZYFV7S4-
zz%Yk{fkBUhfkBspf#EMZ1H)N%28Oxp3=BE!3=Deg3=AUd3=B`$7#P;GF)&PJV_=xV
z#=ua(#=zji#=xM##=!85m4Ts~m4U&Hm4V?C3j@PE76t}S76t|v76t|*76t}G76yiw
z%nS?*nHd-&nHd;DnHd-ynHd-~nHd;<FflNkWny4h$;7}=#l*m1%f!I&m63sAGa~~-
zDI)_zAtNMAVdE6=#G|C7#894*nVg}JoS#=*QdF8;QmjytkqY91Rzl>Y7Ad3^<>x9S
z<|!l=rI+TW=9MUv<b#!@W#**nF@O{&CYO{Z=D_qQ<mBfUC?uwpq!uYu=9j9Mr79GZ
zmMCPFC}id-q^FiR=VWF(Ll#=(q!#Hg6oZUQECH!iNXgGrFHy)yEK3F1Ql4K_RH=}d
zlwVq+kda>wQc(o61>~~KVug&vyp){O6ot$@hLVi@;#7siBB*tF3T2sz3TdTz$tC&u
zImLPfiA5!ui8(kNk(s8Dh-`F5VzEMTer~ElVoFM8NoIatVh%h2igg%rQxo$t^U^^s
zQb<nBQz$M^EC2-x$N?n^<(WA-dJLH<sfjr`l{yNkWvNA#B^e;~sX4`|3Yo<Ui3$Zd
ziOH!M`8g?|ASeQ>R46Gb&B>`$$SN%^0U4%{T3DJ0vKwp-$YVvNc_3@`7<91%2IvBo
zoYbOXh4j?C)S|>322dO;QwK08E8qjjw&PQ$%s?IIC^ImCtSL$@DJ{xVC`c?SPAvk9
zfC-Qk2xp|`6xb@|mF6a;7AfSXDdZ+rD1esmC}ftT7J<XJSRpC3q&zhhWJXDbLP36J
zUP-Z%jzUUmT4HHViLF~=PI0Ox*ieWidWk71@v!`$p{}c2mRY4+l3HA%TaZ|ip{}El
zm<)<H+r*-Dum(M+{QR8M#6153kZ@v-BUoBTAqH$F$dMpSxkC*N3=HtPGdHn97n+E4
zL1Cba2m^H;g_6pGRNKtF5^@|?k`Kz;pj-<o5z;b?ic25?i4=|?=Mi>aW?p6q5so8}
zguo%Ite^mj5QVh-oSgh}PzEeWEK1BxEdk{Z1!agD{02di7$|x`7?vUo^b8Ck0Syub
zVMyfV<d^FfBo-Gdq-ExS%D<%4l0;C#gxIMB3s`WXD8fiZAl0yFNXyAj#E1w&)~Dqc
zC8sK+rKYANB_?Mp6cptb<QGAboI-MbYFb)ma%O5?2_bhR<`iTkVs!}qutJfA1g?St
z#7)JidBvdcPs~d}av!OYfouWxh*3{T%q>Vw2`El2P07#9D@!dZPF2@YNY2R5OinGf
zjm9Cbo|TxKpOl%H7gCg%S6q+})})@9SLU6Wmm3(M9*Zrqi&Jw_lS>q!^=D3Ia<)QZ
zPI`V(W=TdaxX3O~BplY@T!<(*VWD1JQUpsZ$_gR=F8-j32wW)?r52Zf!xoh4Kp2wp
zK>h{=aB*^CPGS+LmMh83&dATpFH<N=O)t$!EXu3`7k0?`F;&+X;xQZv5@v)h$aI*~
zu%=9iBOqBHR96<{7lW%!h18;=d~iDeTr+1R78Fz}lxLP?pgO|{uQSr~6LWOI24Qsr
za;ku_!6Brq04eBUC3Ud^o;(jpPw)yZEx#x?v7{umC_XtSvA7sflsiJ3K`xNWq1Yoe
zr@#%S7~&1MA^B+vWr;<Z`K84Q*_nAM#d-?9iP@<N#id25-~^JLmtU?>o{<Qt?m^xJ
zH4c(e)AFGTic0eo)pa1Afy9x5EvOv=X6wP65&*9GG*VKFlZ!IJRgA5AP-&h*a!!76
zY6_%12F(i~cckVfmSiRuLu(&U^8l2L^2<ThAG~x-EQVEo3i)}d#d=^j<mDq8XGNuX
z3W*BEr3I-)3OV_C=?cmDxw(mXDaAUF2265(K_#dxEJ;-;$;d2*w8S#=^Atcu0H{G&
znO_R2t;;h~^Aw=95-3=TKm}KNQDUy1f@c~?KmnAIKvhd}Mq*xiszRbdN>O5QNoKM_
zVs3tEUJ0mHFUd&F)d86ecBo@c4mh(EC+4NX{80=^fvH8QV0B=FL2Wmfp?Q@Gh(-d)
zIEBPw<aRIE;F65|()0|l*~$4S;8to{W?l+NAXh;nskB6)C>7kaQ&87c2Pbp|^>}qq
zcFZl!Daq6=&o4>=MIg8(U91TSRy1Ftq>AEHcoIS+<CJ`8Ag33l7ASxnmXw;DSPE)f
zmZ%pifZ{1HH957oII$=*rxN5kh2lg|YqnSc)C4WhNG(c**zE?XGQiapsQkz*1~r3=
zGgDHFQd7WfVo-xRu?QB(#qhoXI9(NkEe`=jaCsucB2WPZjS&TK7=ergwSFKLx`JB6
z;C!iDl$w)RlA5BBo0y)N3=Mo8g|wW+bWlVp6sMMyK%E3iFPV7?2xlX=f5A?Gb}Sr=
z(jiU)CF_*bG*F`i)ZqY$Yk+f2k){<StAqPB7;C@vKn;d?NZT1ybLti3B<AI%7Aa)r
zg6dLmd&0;iz9c`sBr`WPB|g~ISO+W^0BSY573Jp!B$i|ZfNC~|%rvO3oXjM>(vr*^
zJy2SSFD^(;)=)3bNGwRrsZ`grVt^VAp>i^lU;?n_BLmo}g$2d=IiLy+YN!I38(&xe
zQx0+vL@B1Pauf3s(?P8sh$RZp4wz4Vet|DUN=G3mKPkTi&I>5Y2PGgVA7U=NHwAM7
zv{MCh0o=h*TR>$1OfIi9x1bV~rSjkoF38NwPfpIr%m)b<Wacq|`v1HP3=FN%{y%Jf
zd>cOlLp?tOLj*qqg9SeWgAzXj0}np~!(%=MhWUI94Ap!L3^9BR4B~tY4DWau7>@8V
zFwEj*V94NQVDRK+V9?=ZU|{2AU|{8CV7SD?z%ZYOfgy*7fgy~Cfq{>Qf#Dc8149ir
z1A`4W1A`DZ1H(-&28Ib-3=AP$3=BM63=Bs(85m}BGBCJ8`}`W53=D@j7#MbOFfh#G
zU|?wDU|^`@U|`7LU|<O2U|?|MU|<m9U|`_pU|_h#&cHAaX)M5koq@reoq>Ugoq^#r
z8w0~BHU<VyHU<U*HU@^9tPBkMSs578Ss56-Ss56-SQ!{RSs57ESs55EurM%8V_{&Z
zVqstiVPRmfXJKHlVqsw5U}0c5$;`kok(q%ZjG2MKnwfz?f|-FqoSA{)HxmQHQziz6
zV@wPT^O+bJ3Yi!f^q3eJo-i^n%wdFta|<Y};R6B;kh}rv!pB3?j9yV`o(801Ll#!B
z#gd3Lia=>d2ULWBh7Ul=O9xck73(Ne*Mcy_9A$;_RP~}%a5+^9Z5LK56eJdxD5NBo
zBr4>Cs+9Ef%)E3Rg^bj^WKb27s*svnP*Mpp4N`c5vpuxs1hPoM7Sz%P_3<^3L_wKf
zM*-%noXl+4CXbxdB8X!^EifcO1zT8|0nH;hsYM!zMd`&5)xO|b+7@D>UP@|OW?rg$
zeqs(d`#bx1LPH8{p@J<+o<cGOWFEvKP&cwzLlab1#K-3)=BCESE7;m9D8<L;CT8Zv
z$17PWK%xZN%S|duP0V)7NlXWYrj9~RetLXnNop=QFqAO@D<v}-(pW?e@#4fXP_YUs
zOm!4MRaRbVY6_@&ElLFyO(o#!!ZE;82U4@ZtcX@mDJ+gp&P`E|Rj^ekOwlV!1(#tO
zkVx0d%}+_q(aS5-(S(E)iaxL&aG>a=r<Me!XlN3zy|5UpyAX$tLYRf%Fo4)moSK&s
zn4(db0&z-Vih`{|UV&alaY<rwwuVMtfnHT=QGT(8v8IkfVG7j2U{!@F@j0n!B?{UK
zg(>kxndupj@Ply_Y{69*QhL$wf+*7gHFe|5Qj<&aiy$H}yCDW*(+IK&MIT52qD>jp
zo+>B>x1LfJvOtxKLRo4~esX3>W@@oQQX;5jm!AjAX`tb-%KTDL9x6^vf#zbU%M@%C
zpsZ*s9b>Cla5zClK$#{pPcJz?7i5<ksQKpw?Fu1BD>Mh`fpVvgLLrP>6rY+Jp9azt
zp9u|WFPLv33=|tdzN5^-983#AZC7}vP_R`9DJo3``!F+20Tj_WnR$sh>3VtjMY$TO
zMMba}0=Y#~!3LU-LHSV+l(XZ(-3(1|`vZ?|aKgZ?9ooV`vJR{P>i3-dbTf@$SFlac
zl%fYoH;@d6qz&XsY`Q?=ns7~E@1ht83Wxae_*97TdWp#;o_S>&;Ia!M1W9BtJs{)Y
zT0k6B9Uu`+NGXJ5gaWJ(1dD-mXn;eo1k{BFb)WR0eGUz1{sOCmB@akG1q)%(3UQt?
zBs@aEJ^?vGM*+fyg(k$1^wbitFtq3`ggXpW(m(}ag^_}-LUk=TypYojcz_Qq3Tifj
zQWSFgQdd_OEQeOBxRfU5I2Pp=`{(6Ug5^PaK}}9P>L7Va!3w2b&_OR!bQDknQb!>Z
z)W$3>i3jCBSo;-PA%Kg9_>!W;EO5~SaY|l+UU6bsYDq<jhLWB>x)QzQ;xZ*2NTGtH
z6ExtNlbH(|60}v<QP)vO%Pp~0SJg93Q-{SzPAaT>0Uo0-O3X{i&jk&3rGk1_`K5U&
zkVd{TxK|4r_=a==N{drL^9q^8p#CkWn#s>A&P>Vy_n&g|)4`JiX&@5R(<@fUEP?iU
z!9yQb3dtF%$=M2-X$s}3prO@L(73t+q}BrUJU~6l%rtOoq9`>tA2fN8k*WZx=D;zd
ztdL)<mz$WKnvz+hpj!ayK!TbXARb6BD5yY{9;jys?yF`NE0m@~V^vu}BQXcw?aKsr
zi$Oz_sX3Jj<@rU~#R`zr2<pm%49hGAO<^P@ra-%Bxv-KEI*tRGv4FZxAs5us2hF`?
z7MG;vmE=_FXXb$z;28&~3(%tvwJs~sQ79~i=CBfoR8D4IaY15os)hli@ugRiSZbi7
zkdvCH0aA*nN1-A*hEV;=h}M;Y1}MBBUHAOFR0U8119y3JKp~X}p3(sst6+<e0?8CX
z3SWq4trQT3K)O^pI^cdcWS$2U*`RhuN@_uBUW!6~o<e>}Mrx4)Bw0aY4$@LAD24O|
z6BSZ&3l#FpQi~u?E6xX}iJYR;#FR?t#8F17LP=&WsEGp>1)Eb?3<;XVqN2n~P$+;!
zVWUOF(GV>Pps0g1Nb{3Q5;OBsQy@7>7aZo`(17Gbe4|acZG@DwpfX5Vp&%z8)Edhy
z289i1a1At31gWyXjsdAEEY?db0EM4MUTS%KVYHPQ+F+$Js9&g8oRgYbpkbhAU|<T#
z%pkoOSP4A$p`>61DQNXT9XJimT6{XdMK4?jB;Uf!g_O^w1u2OosbQH_8r4dLAeCT;
zqN)U&qF}2~oS7RB(S=s3!;Dl04Lf8jWagFRE0kpxmnP<9R;B6{RD#Bt6;e`55;Jqa
zp#>UMQ!iFXEKAJHNleOtq)Lz>pqaj+^kTiT%&K_MlubOS!*2x-PO#fh98w4k1xRHC
zG6L*|0#Mr<T%IK=fSTDlU{WDFwX#?tClfULQIuMoT2z)=tdN-p8o$UdN`X#Kfy@9E
zQ$89X?}FMMItY*JC=}$wW}DLTi@-e4_z}XHAWe`kiZ3onEr8Z=2vJZ4SQrl~qo6Ja
zdk!v--YWq~fZPCyWEf8`H?acLvw+Qz*(!i%ZxoavMOH~hVxB^39%wKOZaT;iaJ3T;
zD$;Egz_qFZa?b<O*DKD<g`{DH{L&K8NE2kVD6<&c|F7a^U|<3*0uW$efUVy@$j`vg
z$<M&x$IrlE%+J6ez|X+I&(FZX$Irm9mXCp9DIWtv7as#d86N|K9UlV&BOe39SzZQ)
zUS0-<VqOLYXI=&dL0$$1PUyV<Lmmc(Gdv6ob9opTrtvT^r1CH@X!1bpmE~bzc*D)W
zu!Ea{VLLYiLlZXxgEuz=gBLdg!$~d%h7DW{43S(642)b14Es457&<r^82mUH7>qa>
z7-Tsa7$i6u7@l!3Ff8X_V94iSU@+leV36csVED$)!0?rwfnhUrETEg6fuW3@fgzHe
zfkBs@fq|8sf#DV#1H*ha28K2^1_m=W1_n_!1_o9(28P3|3=Feb85l}g85sOo85pcs
z85m4i85o#Z85oYRFfjD6FfatNFff?0Ffa(SFfa(QFfgoVW?)#(%)rpj%)n60%)p?-
z%)s!3iGg7c69dB*CI*IiObiT}ObiU+ObiT8ObiU-ObiU)85tP1qmKt5j}2gFgVM7y
zs9Xp2=@X%4ae5}WmkBDhiVHv;%*;Gc{h;9Ls*tIds;7{YpHis+?mK`JkaBrZYF=^%
za@#E&ECNm5$_h!Ko-eo&02T+eB=Yj}K>avSZzwYv)F=QO1xo*r{DHRtsH^}TXibF<
zwkLw>f3Vw9@<GK(d16rt(s()8Y_KETKqHjU^bHmTwfsPV04_asOY(KW429f$&<G-|
ziv}vV3Lwp+%sj~A0?=q4Xh=Li4IDJ3d6^&)P~`#|GtDo}Nl{2jRY<KUNzF@vbkP$b
z)B2goV0*!JYGyKu4Ir0;8<Ywf;HCzs9t3waKs9X|s51namsLnDNrsL;V{7b!%BRr0
zlFS^?C`e**35Y7n&jAgzfUO338PwRtqYgaY0qrVcG<m^k0!zymPfHgb_M<}|(6JV(
z4SguXrx$ZGi@^hOC}R!D+2A+_50Ib@muYw*4`4v*5X#0bKpDvkG-iP`cA%_~Tnd_j
zNCRc{RLGJf*pw~UX&?;+<PT(E3}b*C0Z(tBt})W6LSBI$_CW=l0|Jod1)^V%rAv?K
z&4a9lcFaK*gF3&U9x|u{4=L5aon}}_LsnWqI!NUiso?fAq{)mls|XGvSQ86k2wL-s
z$TAZ-NZdfHVi-VW09uFFu_zbRCx-NON3%F+L>$lHD3wMxhsU@jDD8VeMyQ~9ga#v%
z6qFM~c>EC*w4jp1UeP`)H6_y)G#~&EX-Js>lFk8@HRuC*l;}ka0@^FuD`;rNYiX4w
z=9H!uTPfrxfyS(~;<a=Xic<@$6#NTHKsG5C6lLa>Xec2$N(tJG&;vUWQucwFa37$K
zm?4H9!QMeK30W^_ND(v(lb@Ia8V1WN29H`MCYL~pE@aCFa3~Tys8j;#MM4^fpyi_l
z1v#0iDd46asDTX`fl4gOg)}cgLhw;oP&+v#wKy{!G*AKRP=aK^K2I#lg>=G8@<Gka
z#5{#yS7T_TKwE|&wQ!Tb%ZI?mfd+&?i+S@CbJU9=jd%rcngPjz+?$aInS}?j6G1~F
zkg$cAkPqnyfMN{PX;4VaDb5E^T)<qPSOOjZ1sMiz;er<lfakyy6+n%XoK%Q>325aE
zY}Nzfm6UvF>$U{OfHuNGK2J?d&QM4!0<C6FQOGPQ2Acx06k8J*ty+VQ_k!zESnC(N
z5=ipFX!4@9oCuHdf~|rM^TKQ1;oSHI&Ht;iFfjOnHs}j5Fu?Zb-{xmv*uu}iu$iBM
zp$R$;;0s;*{}8mcpO1kdo{xdSjgNuhJud^pbY2FAP+kTGIbH^aCp-)cCwLebHuEqr
ztm9!|sNrE?u;pQ3VC7+8IK|Dtu%4TNp@*A+p_`k5!IPVT!I+zYL6Mt*frp!c;XM~*
zAHZxb28IGI1_o0u1_l`}28J`73=FHGYXTxU85j&X85p=Z85o{&Ffd%;U|=}S!N4$+
zgMlH6gMmSbgMr~WI|IWRb_Rx(><kPm*cljH*%=sY*%=sQ*%=r(*%=rPu`w`Iu`w_N
zu`w`su`w{1urV<FXJue`%gVrTgO!0{Eh_^<FDoQmELj;C)Ii~by)Qm0IvN6_Auy~%
z0Nh3b)trdoHY`GwSj9$1<wi&4iqear&1sA#%IK(EK`P<(OjPQ!6I_rqIx2^@xCm6F
z!5hM$!LiX%IrJbI9hCzuhXk)x1g{!|bp1gSK?AYoaCB6zB$3GVf+KTOE<FRZW(VZ=
zp}Z;(He!b~B&VdKfW(80x}hoqjX}XxfjFAr`F{xp28Jb|IcNa}2H5#~`}i3cX7V#I
zRPi$~=<+i#u<|o7?B`=(SkA}5P|U}`pvuR<@P?OxVFfP(Ljf-XLpU!3g9a}H!yix|
zpND~ABM$>ZIu8SbI1dBEXKn_D3)~D0ySW({I=LAbqPZCuEVvmM%()pDc)1xESh*P(
zK5#KGJmX?uSk1-2u$T*S27o^o1H)%d28NZK3=EB&3=9FB3=B-13=FS17#L1+Ffg3p
zU|^Wa!NAbV!NAbW!N5?-!N8Eo!NB0c!N9=A!NBm3oq^#fI|IW^b_RxQb_NCwb_Rxf
zYzz!%*%%lWurV+cu`w_>K<E8mure@Q1g-gJWngGyWniddWnd^_Wnjo+WnhSc&i#L9
zVPJUA!oaYNg@K`qg@GZ4g@J*Ig@IuQbl$&;nSsHFnSnuynSnuonStRD69dC)CI*I8
zObiS&m>3w+nHU)2nHU&6m>3ugnHU)UGBPk+V}ykLVo><w=tF@j9MG^}VQFeHczQdt
z7`)gIG`*aX583jRUj*7_3Lfo)?i>T}X-`xrE-6X`t*KVX$;?ZI@45zUwt@`9ftQPc
zcFdM!KqmQ;ON)x2XAXeJ3t{^Tb-;@VzyoAxLwB&%nxKhkA2-;>U!Tl8$Pysf$Q`CM
z@^m$HYz{n)uW7{qTg?W(B?5E?0%VaE@^%O+?vjA5RfS|4P<TUDtb$k|3}1(@U<F##
z4q5?G3>k8SPcvykMngfWK^UY%op{xS#kdt0LXCzw611Qe91WQ3d=m3$xTga;8Vl{n
zV3}nEB^Sh~J|ryBMqoi>|LEsYfKvfPHKgePF2z90{__he^-@w(3&3mQz@|fz2gLV@
z#l@*bCEzXe;F0inh$Lt)2dH$1Z*__<s4Oo^EYJ&fH3n-0rAJT{WkPm3LKa3QgARLu
zoO@A}UjoiYI*_H5nI(uQ*D%&GhA!L$?RQW}L>j*b?Wh8|5i%MM$y_P<sl^K5bPwT!
zGY;4hU_meo<Q&kHE?5K_#^4}<1~f*G5o8I(uh2XLpDuu{Qy$GFsksA{OQ5lakv$*@
zd^9_dlpTo72e5PxoyBy{&&kOw2Cec+Ni9iDE=f&MR8)e5KV5PLte}oohYnl7H$S1)
z8HKQIR4@&oG^-9-m4{Ltm}x?)BU%^updtvz3N*;Fc6j2(K57LCW75(%aw!frRTETz
z=N0JXrGQtCL)HL;3Q16z4K7eY7ATa0R?30)FQlcW=0Hvkf*kk*I_dztwl7V;L;-XN
zMm}WEJ8IdB>`jbf7+DfI3F;_7TnOE4M8rHHEM}u&(F)qg2Z>?Co&YoSbO28{1UKM8
zDojvagueF<6i6t0=3tHo`xgItF-T=a?FC-S3ZQBky1K**w7m|t?+rBVoLiclp^%c9
zmX-=>eS%gygBrcg+S&@BS$I%(&PmMyyB?GhuntOrB%p-=WZz72VkKzUULhm11ibze
zl0<S!b5fxN0%*$`coj9GxsaciQwiOUmZ@N;U}|8X30X}BG94wr;rSXO3R@xwZ6Sb^
z6LKZUU7!T3qyuVXAg^(QB$8;*-D<H=13?>ZP#40XC{nNk?IkJJ1I>^pCYQuR3ct*}
z^my=rFc3c|D-`D|q=Akr0c|S+?WO@OYRJrkp3sm95-li7Eh(u~D9%p<ojjHgNe#-N
zyarkb0a}%=3fdW*Sp-@EtOMzPfT9Z2Fi}WK1ucICuM+?p2I+<*fwl<c<!2T{&p=RC
z@beFGwE{1rO-wFPNQWLg0Cp)TIV7SSQ2>s0P*8xj34oV2E7~d;=ox^;l@&@dN{d08
zQa~}MfYg$M1P^#SfCA*KvXsoCRPaeq3d#BTMJbtii6yC!brZ^<75^Y#Ae4ZPQY!*2
zd;s|<GY=LHpcNKrrA451f8f1DU~eWW6s0B>=jVYoJ}4v>7nkOOPX$o`2L#BVoE&f>
z1la&igkYyb9Ef`bHky2VdTI$|iMD5+t1En&c5yx^Zor-g_2!Dfr5<>r7kJAU$V#X|
z&;{Yepo1wO3&BD2{{jpQ3@bqWPXPu7*!lW9_!$@~_!$_q`572w_!$^P_!$_u`573v
z_!$`b`4|}5`4|`y`4|}N`4|{p@Iub|JH^Yuu!EO@VF51#!+c%_hAv(PhFo3-27g`#
z26^b(fcrcQ3<sd+0Cw^)Fl6&EFqrZ%FtG7J#t3@385p9t85o?o85p#<85lI7;{(UJ
z7#K>R>j6x-7#KLX7#MDIGBEUVGB6ZzGB8AOGB8+kGB9v)GB7-c?gdx|JsW_XgMr~b
zI|IWOb_Rw%b_RxOb_RwBb_NDFb_NCkb_Ry)Yzz#0*%%mhK=%Q-voSCjvN13)u`w_(
zvN15sXJufpVP#;@W@TXb$HKsHk%fWbIB2~;3j;$a3j>243j>2K3j@PfW(J10%nS@S
znHd<4GBYsLF*7ijF*7jmF*7i*F*7iHU}9jn2R$2LA`=5c4Rk!ffr){ElZk=hC?g~u
zmV@E~Z7cv(n1C=eBEfsBA>B|jNUe*jZ~(241s8C+nR%JHrMb{z0hCT5<w87YpDwI$
zC{KlM^2-PB_(}$y#|O@~;9UTzh+|>$K?gmhLpu0yH$jR)cp`-+P|{bylC@6&w52c}
zbf7^zXm1dxDFtaX!Kz?{2y&qe8N3IDEUbwE?nMrd9R;A80J?*~IVTgcq8#K+P<Inq
zE<d**Co?e*d`={Cdk3TIiEXO@4Vu)bQHeNm0g}>DWf9Q|ibP11gIFL;Xb=Wm27?rW
zFsQMPbHE1Y7z|P{f|k!hPQ27ePAn=(EzV5L1D#9*9}x@BEXfFfoNa@&w*#$Z4cc@T
zUjR|130dn+$S}N7hHRdKl>+J+Gq8>mW=j?|kP%S;4l=a;jnH%XU>R)yXM)O7b3n&s
zK+*}6qhPB5I!g`hggWrPOd}ly)V&mt&W18VC;U7*)J+#U3OPx(IY~MSrAfA>Nf7Hw
zlcEhlt$WahPeX7pl_tS!c1Y6@y2GQ=2ejoItRx4jNf)6j2c%2aPzzF+;?hX@&IVAz
z2zE7&)-{CgUWK*E@{`Iy=cnaVg12{rlp&qw2s*qZ6J8Azr-FA*<)s&cH)ey{NuV=M
zz+R(f_W^7mD3d}01-=WlC?0lV4yX&{3^@)>qgn~%XeBG~AyL7u#?Z~Qh;69QJ**(j
znFXaei6!|(unG`#@Sl}}YnZ2xf}@{{f`?<ULWsYDldD3ItFM2UtBZnLkiV~jhii~4
z*iGPb=O8_ff&xA8KwJ)Z@DP$FltF7?3kpC@OOTU-U5%mkK(!>6WEi=`gSxW0snF9r
zpz6Sd30M~S+$(5K2dRlK$j<>CQ2<s0H32kYSrHF9s3g7&>HtUsv?#Rzv^6#lcGPfc
z9_Xk{=pHywVFmS!p(bR^)64}NcA(v&pq--8(G><dl?JgodU|@X(8F;+n^hq;LAH4#
z_gx^dgeB=lTN%L<TX6<#w_QO|DzsOa0`8=N&q@OylT!jZYakPJx*9|*FEz0U+_z84
zOixb*@A%D6%7LE60}2&HXhK{C9zxJSmC;1>ymbwAjG&z>kQ!`Z3NAiM5+TzMpd^Ug
zdO$w;1awdYXy`&IB|k4!2@=mB^(2<9AT1P@q^L$f$DjxfC4h86A`X<70#iU|u|Qgw
z5K%~U=9NLi3#1%`At_lYJ-<XD545JdBm>mL%*$6u$p;k&iFuH7n4#TmkPcWrOi4{D
zO^*lbFNOqHW|{&l6#eq^QXzs65pa5h)r+75ja(8-5}^%DWl-Z3+zSJBb-<k)BS-><
zCTmDv6RZN<y30u|E(T4W6{RL8<|Km(08lZO3bq4sb~NZXgH&irholThjjRU>8#mA~
zOsT~s@bX5z05n`z04-+<K+9ynd&rQ=15nBbyN0yZFjxy}U5se(z>*0<1UYSDwro@L
zQgl<((!fV0XcTKgOLb6w$}9r=9VD2ToDA+5r-K3rw54AmQ2`p7y3l5cLU93TDSv(`
z<e(<-z#1+B>;X^-gSR;ZRt0aF5^C=tHJ+daA~=O&v{sQL4x{OV{e&0LNy2c)fD&sW
z=x7v$^nB<jK?&&K%)~s%p&77YDUd9vh);yfv4X}2K$ESBMLCrUkdqidC$Xd|AkEQ&
z?EsGrfRk(q=(I<WKG2~YunAYtQS0!7RdP|ZLxv(UL1%%2jxGRo|FQM_L7PUv2MB@&
z&%m~&=7C38iuJ)m6seH?|4s~$d1wX!1_s#ud8hdq7*e7Ae_?(GhUa{c`Tr?=3=ENc
z3=9r@3=AK585qv+GB7ORWni%8WnfU@Wnf_9Wng&D13B~WEDr<2Y#s)NOlTkf1vdl3
zN^S;*YHkJwH*N+7Zs@%KK`sV{16&LY^SKxpGPoER61W%`e7P7HoVgenn7J4jUU5Rs
z18nDHV5sM0U~uDPV36WuV0gp9z_6NwfgzEDfgyl{fkByrf#E+p1H(mj28PAZdH*JM
z28MWc28KX(28Qo!3=FT?7#J?FF)$osV_;at#=x+Wje%hW^j-jGHU<W3HU<U>HU@^T
ztPBi~Ss55Eu`)2MhOP&&U}azs1BtURFg#*mV7Si0z;K3zfnh2O1H%*+28K2k28MDL
z1_lon1_lQf28RF43=DgqX91)$Gcf2gL(T@=&BVYknTdg+fQf;@mx+NvlZk<WkqHt9
zSF!Z{(S`zu;DS;v{8$NiYYDl%qkz1b9<H<~6|^raGdDFJbPfvmC{V2G(h`eH;)^qL
zK}9DhZ6Tcj1~&_JKowLb!j=xglweHpg2pLe?UwRXh3wSS0)<5Axw4Qf2T}n#i6jwz
z$R1d{JhM1ep*%IS2y`rLCb&782^w$%AJYQKETFj)_}E5rQEFld=v3Rh)N+)Ap&)~q
ziO>Uwi^1Ey5dMTr@hdAp#+E^6zk+fvEQf)IAYo@QfJWZ*Kt*UuzCv+6IJ<)n#EFKS
z1*Z%??hJIcQc)`C@FGz240^m=NoH=U0{DPl&>8!nIuv}5LrFg5WZa^}N(ImvuF#_t
zl@*e6Qo$t+$SFynHW9eOgxo9vI`tUTAXm`H%quPd&6Z~tE97e!YAO_$Wai|+{8OF^
zy#@k&JOk+7hup+G@X@kGso<*>6p~VPK-D_9gv$mUa11(bC9@dPUjh}}pcJPIu0*Ur
zBe0O(FPN{8o>`Wf2Pz*+5{t7TTi3whfv7UTTzH^Enp(P$BYxt+NBn?R6@WG_<`-3h
z!W442JgDM=8V)MyA<+ym6l@@*;RP`bRJM95<bjXJN(bL{0ouq0J}VdGr?e8?(gIK^
z3OeBmx_$=bcwg|~YAGm&K*PSNkbv_qEdiY}1KPrvSOShv=pjg;VRg`vI606ra>_w=
zfuaX=kP_I=)QW=C<PylhGbAm7E~o&XI;8;iFlYlG%)_8QL}m%ZdyugNkm7=({IX2Y
z5f#p`>Jl8<5SKwh0YZYq0+vP-b23XH=aoSuagkt6peZD{>7bDe&%Ba+@Wc&>16Bfy
zdQkL(l_PE$QGgtXqXW8kBQG_+Aip?O(+V=60lJO^=4y~pDCQzp>5vW%=va10G=i*w
zL@W4c4^V*yI%5~?6woQlkQO6I6^>D<%rwxk*AQjMN=Q9g6}3r*7#@L?ULXg6FxqGo
zWSmqTX$g@TbZHN&aiI1b*h^T<NWr&Ks0?ZxNFNBRmn4>gTBg|NsW4PS-G@8~26G_D
zG!Q0lkPKAzf_Bb;4#|cbNCYZgKx4I#_C7TapG6+aL>XFxL=7mBB29}@X+08n7yx-D
z8#EOLS)&7<&IV0<BI?6|TaQ$dQ3N_aN+C_ZL;-eZ1vsTa=G9?g=bK-a>Z72mkO*0<
z0Xec3G))OUAr&-KoD4b@Fe5XyEHwqJA6$-rS)iUgNJR<Q5U>!akbn$1L&urHQmFYH
z5nwu?9BK>dS%a4WCZ>SSL@&tz^||tM5F4-|i4{^TK#K%;??(rGU>ssH0Fghz?xFRe
zy`WqN8VM{(EJX}8fV(L0)q<Ea{t*9w7tlkP2(4Jw2Y`wJkQES9iCk+BTT=;Yn<ymZ
zmt;U97IcggxGn`n9q9O5=$N(wXa{>@jzUgmb}FcDgb&z20}<(r#>5hZa@c52T4^4r
zjs_V9T2q&)kY1Wv45{jrLG!%O<0N56>sIQ3hZ#ZDBlJ3hRM5GE(0O857nk@I5n#tb
zf(6u9M_m@74Zg~uSPy*Nih`CxNn$Bjy>dxnDd<?m#8S{<e#jvYJEsw%z)LSg!9f8M
zKVXgE^LZemh%;(3^U5^56dV*l;@Y5j{nW}-4KF?L(aq5Yu{sI}wx)s>WYQjX??*gn
ztXfmS0qJOCutmy{D;^4RN+Bop7vv;XA}yByT^IpX2MTylEP_H5(xQM4J7>b~l%VFw
zET~cdVR+s{Zs=pqNg{@=LGzN3XacqAkxl}JRL%&6ppnqbJiQ#GBYbIpat|bm6>Jrv
zP~$raGOU=AnV1e5AA<}pMjPrG=qTVIV2d~4CW5`21iGb23!0J?v=uZALB%cTkVKFS
zC{^SY=p~k=7A2;qA}j*cnDL<IJz_bLrj9~lMP{)r^pYpkh6F+xMpp~s9LV~ADFz0H
zQ=opQ00ToRw08-*_Yc(n59eoK2;*m9FyLok5aWm3`^V1D!0>>Nf#EnG1H*Ja$a;WA
z=)QntJ_ZI`J_ZIYJ_d$=ptJvZ85lP6GBC{GWnie_Wni%3Wnj?cWnf?g$?-5SEa!pT
z4N$_vz~Ifpz`)7Fz;F$8?jJV;!!~XPhBeUh0E@X97*e<y7{a+37^Jxw818T}Fr4FJ
zU^ojsA8-X11H&XP28J3g1_n1S1_n+p28LCf3=Bz}3=I673=FS07#JRLFfgp)U|`7M
zU|_J|U|<mEU|@L1&cLvfoq=HzI|D-!I|G9QI|BnZ^xi<wdH~SbfLUw|44|<AJ~jr1
z=d272>sT2WnpqhbnxK0GK;r`DtPBihtPBiqSQr?dut4q(*u%oWP|Cu<pv%I*z{<kF
z@Ryl^;W;w{!&PPmh6BtD42zi=7|NL$82p(T7*v=U7+x|lFkE0_U}#`sV6bCiU=V|j
z3;bb(#Lr1k96{p;lLF;JDi@bn4p67n(PN+%613zZ^+@#5lf>ZDjG%@9bWj|Y6hNH`
zNIwL*h{hZo1`mvntYr^5nc0v9xfmnIXuSj-jMKoubuF~n3B8d;15|UP%`!teAbO~k
zJ6NwW?2aTz3l`jgfSL!Na}LhTOLr{G&rI=6%*!nBO{{?E%uE9>n=8uC$w@5&Utj^A
zcEYU<IvIuE4%hf>UlPp+X(85ZPtwiygqjU1*wBLpY#MAL2`#ozLj!6E7SG2+uB`LS
zD~r!a%t-?qsf<5x(X<d_HfhG<HZd>&o*Z1`vndEpNG||%ols6HXk!(4;0n~sf-VjO
zT_y!~G*Y<34t|8+)dyC9FZ?ldKz)W}C|UUipPg7TAk<b+4-{lGWOl#2D6=FLGPeyH
z%|l!v2<fFjW(&bn&&jY(4dli$@Rcpa`MIg!6`bIEy&zMMpzG5h$4(~}fUbZn$^;EB
zgNJEhAytx*3Oy}7wW6d5v@fqTEe&>HeIj`95gGwWsp*;EOJ^a2(a>N5n^O*5fd?8C
zhs@sRg4cN_CMTyBlz=;N;OS5JSSn=G9#{evBjET3bq*j$)IzSofQ&ABf>!?~DkNo=
zfDhx>0kcZ-OF-xR!f%rUUCW!7qTuhXP@JDr3fhkd(d-D?Y6u#xhFK0;#tRuT%~yb4
z+z6VGg18&D5EwjgfJ}h>3B6hjI?<N{J;WOni=c4?3|a6{0@_-pLdd`bxPu0}wo4-b
zyffNeM<GK;!BIyc3ABy}w1@{ZxDC4K%T}qdG%=;1C_f!KU#(24{?z3BVyIrw$;zN<
zY_N}%!Ha<qT<|I%1Q&YiLM7-vnWFrp#3ayF2Z?zl3gFuziow^gfz5FQ9n%VCftCp&
zdP!g*cLiGoxS62yMM0ei@bUZ62C)iS@P4OZtd2qo`081ul>E{p_+og_I34H?I8blD
zBwxV*Y%OGvpgdIp+W!X)=Yy^xM7rk>tPbn~WrgC@<b2T9jY<W`S%aBn;Omz`x*+#M
z7lIWAKsr*$J!4$10!2G0^P=FKBn2DLl}I46G)X}tAH3W;F$XfrhoaO=0UU`*AVCe#
zeTks;K9!mvt^#BN1Hy!o?jYqcIjMyT8Q6?1E(IT<s89l#)PZV(Q`!oJaUmcR;4Z=z
zA_!N4SLJ3_r79Te8z~f~#DypXfOLby2zrhq(t+3tfdP>2G2A{70}>V$ke~qT1c}12
zqe6v(tpeEla4`r2Q?0u~g#y&?5G8ODt`<uGpm?GJ?1u`_bxv_13KftL1|KSznGAP7
zJja8Lg*RQmJWzatvN>E7%i23oDhFj;TMz>>PMA}iuLHg#BQsASCkb52l_tUW5rZab
z5G6PGE`vhQ(k4(r7#IL)gh3k_$R!782nS^>2IMeUNeZtGQ5}e<Ac7c2_DB(GI}&w}
U2;{I7%$s;MQ1qa$ceG*v0Bt}U8~^|S

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 661f287..2c0434e 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -222,6 +222,9 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     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
+    # this works exactly the way you think it does:
+    # the velocities for the other arm are 0
+    # what happens to the base hasn't been investigated but it seems ok 
     qd_left = clik_controller(J_left, err_vector_left)
     qd_right = clik_controller(J_right, err_vector_right)
     qd = qd_left + qd_right
@@ -234,6 +237,27 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     # hence the empty dict
     return breakFlag, {}, log_item
 
+#def clikDualArm(args, robot, goal, goal_transform, run=True):
+#    """
+#    clikDualArm
+#    -----------
+#    """
+#    robot.Mgoal = copy.deepcopy(goal)
+#    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),
+#        }
+#    save_past_dict = {}
+#    loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+#    if run:
+#        loop_manager.run()
+#    else:
+#        return loop_manager
+
 def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
     breakFlag = False
     log_item = {}
@@ -338,7 +362,7 @@ 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):
+def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True):
     """
     moveL
     -----
@@ -357,7 +381,10 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform):
             'dqs_cmd' : np.zeros(robot.model.nv),
         }
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
-    loop_manager.run()
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
 
 # 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 3b5c8bf..4b01fb3 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1073,7 +1073,16 @@ class RobotManager:
 
     def set_publisher_vel_base(self, publisher_vel_base):
         self.publisher_vel_base = publisher_vel_base
-        print("set publisher into robotmanager")
+        print("set vel_base_publisher into robotmanager")
+
+    def set_publisher_vel_left(self, publisher_vel_left):
+        self.publisher_vel_left = publisher_vel_left
+        print("set vel_left_publisher into robotmanager")
+
+    def set_publisher_vel_right(self, publisher_vel_right):
+        self.publisher_vel_right = publisher_vel_right
+        print("set vel_right_publisher into robotmanager")
+
 
 class ProcessManager:
     """
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
deleted file mode 100644
index 007e01f6afc624b2a6efd9ea7c340e95ac661700..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 16384
zcmYc?2=nw+u+TGNU|?VnU|{f%j7Xm^RKsv%9RovpesMv5N@5X63Lnl*EY8l%%fqJx
zELR6JP(LF-H`PcV&dn*!OVKYVO038%)-Nc^&q_@$Db_D7iZ9O0EyziYPtMOPDay~$
zFQ_cZ$j`$fkzY`fnVXmcSD0LspPZkPQkfHvCZktS33kaSF&Y9RF$8!SjExKpKt5Mi
zQdAHY3I#Do@n{H)hQMeDjE2By2#kinXb6mkz-S1JhQJ63fsz7FhI$4D1}3O~y`VHB
z8vP#%+@a#$P#UK0A5=a9Dn1EH!{q-$<%6K&ub?zc{tr~%7b<Q8m4H$VzoGJBQ29=%
z1e9X<1(k>CzYmpwQVc(#@*z<DI#3V8<e8!3f>62vO&%&dN{xoVXb6mkz-S1JhQMeD
zjE2By2#kinXb6mkz-S1JhQN>vfy5LB1{MYe1`kk2Sb%{6*8hLY&%kh%pMl{BKLbNL
zKLbNJKLdjxKLf)hJ_d#zd<+b0_!t<b@i8!D@i8z2^D!_4@i8#)@G&sF<Yizu&C9?r
zk(YrXi<f~xmzRNolb3<vFAoF5bRGtVC>{m|eI5pe58Mn4`?whx=5sSJBylq^sB$wf
z+~8thSjNS`P{_r=AjHMMaF3IL;Vvfw!zNA!hEh%j1_w?C1};tphKC#s3=26J7*aSG
z81y(87~ZfmFsx^1U}$7#V2EL7VBlwGV7SP}z|h0Sz>vwtz@W&+z`(`Ez_6E<fuV+#
zfx(-VfkBUzfkA|ofngsD14A|o0|O5W1H&<928Q*_3=DP53=E;n3=G=L3=CJ97#RAP
z7#Q3@;{i+z4Br?T7}hW{Fid5Hh8Z;070`gPLWsYMzm-CAMrv}lLS~vmNk(R|LT0f-
zNk*zdT2W$dszQ0HLSk}BX<|-Jr9ydPUWtN6YN|p>W`QOHnpq0PC5a`eI&fjI_JEwk
z<kZ~Mypo{Q;>?uN#GLpX1zQF9$_G7|uy1}!YK|MoLbwhMut}*o#i>?c7Rb7i)S}$X
zyu=*m{NfU@7QMuj6c9&4sVp_e4J4(c195m*YEFJ~W=SPTR!6}w)fnMmJf@@<Wfl~q
z76s%Nrz+_vq~|B*gcKzvXJ_W6!^|;4m{U}mmj{(Wa|}twfCCEIxhNq7vKMSRET}Xf
z;f(4_m?Xk+px^@Gg3LU<pwzU~qSU<PRB#CEh5HBjxL{Z5n_rTdpQn*mpjVYzlwYi2
zrm2aDQIK&UT$G=bU!s=_iZZ?Q)Dp0vo+%niCHeU|21-~oKwJ!uMHCOiT#n*tm>}5W
zVDn+&14$JiS15xBFiuKMPAn}>RVc|wEKx63NK`0DEGkjR2c_mzXsT35%u4|!<&^w9
z^%8~by!>*7G*D_$$jZ;mD^V!OFU|ycyciKG$_mN(#U%<RnoxzQsS0J#Bn8SK1_~O<
zi6FC5GK*4^OL8g|((;RPOLG!SQlVzTR3pL<JvpIx8Gk&(a|JA95Do<gfr0|Wqk4H|
zm@*)H^^)@oDmAdg4at$I3@#ipQ$V4Rn5U4LSCX2ZS_BSYXatmJB$g;77G#3Ui(+`R
z!yHQgLI)+FVIhb}TkybzB(BUf1xTQP$@n}_DX3_xpj26!o2djTi<A{Ka`N-D6%tFp
ziB6#;U!gcPRUsq49K=k{&n?KzN!3xvP0UVJfUE~oNXpDh$;?YHR!A*LhC~rKAd6B#
zDK<|bwIUI;iVT!$Q!7%zN)g4RLS~*qa(+&3L9#+oYC%3YM3q4{rQ|CV=j$j`=9enu
zrKYBUgusi@Kqan1qCye0&;@5zPy&Ux7GxT1?U^3bFi1TBb}+aONXyU3$u9@zrIHMo
zB5;l{)>KHzEG|jROHKvZ2TEKDx^@b|t{TRg3aLd!`9)wOD}qwNWhP44<4oiguvmcv
z4V;Uj8>R*vbHSk6238hU80es-e`xkK(p0E`2ZMq}epzZ!Vor`iVs3tE9=M3e%`XGj
zPY`=cNpf^4%t3I?kU9>xqn#mZ1Yynwmy7T$3w5%gra~!_lfjj5Nq!NyID%M&QGcOW
z2oK}n(p-o|5MAiC6;Ya$71A<`ic1uX6%vb5At?x2A3_5hobRDa9~FvH(?KO*W_}(x
z2`Ga~-15wv9EGG*h2qlO+|0am1rvpo%rtOyR08%=F#}jF#G9G<c__|u1hqUs1v5Cx
z-AZ$EkbR^KPWZW*=@}*9q6U;45y>Cagh|aS$t(glU_d5AvO!6HzCvb7wgRY>QZH60
zN(DD<@=}vii;EMBDitzPi&DW(LgY_q7KY~0Vn{e-WESfvq*mxDBqoCk3Rorq)ik9A
zDWHk~<b>q>qN3E|f_zApPDw4wEK4j&Ee5&O0BTKIei67#&CO5B%t=*9N-R!Q&;?PM
z$zZo-l@^yM6s3ZaDp*~m0*DFD<C%G25s((BVUUcRT$BoOKq69LBSIJw4jTDI;4nxn
zOD%$=5O~3ypI4ljl3JvI7W_H+i6xnN=}w8osUZb9;84g&ErLgOa(-?u$n%+b1*Ihl
zp1}$w`9+1LsS03+fs;B)+=4T(Z)r|RW>S7ir3NGn!A?VU4Y*9oEKw-UOGzz)cp^0g
zR3ah;mwK^6r7ol<h9^FVfhmb4i3+yR0v&9+9wcmB5=#;_pf*92LS$3Z((;Q+d@^%0
zOVIQo%V;SW=$V2&rc91zjl`n#V!g!V<kXzhqC`*vSI~#L(bq8|-qG3F)yFl+F~rl~
z59-9!ip&z2a|?<x^Gc8dOb^LvQ1uFa{vobb;Dioq)`J4FSRpa5QlU6CuQ*jlA*r-P
zAtx2oWJ=Em$7W^;#7m%BC^xYpGq*HXArVv$q^2mKI2Ni55|XfZPRcLMODR?;&&W*9
zP(UPmP)aXIElN(!D@ja;<SbB2090?p=Rn%n@kI)@;KnxCTx|8PEu#KSNljCLSnLd{
z#5}$Ioddw(sRIc?1uKOhPy^LBF)uMawMa(+R5^fbQ?OD1HHL#-jWw+xg-&K}L4Hw*
zLT+Ymd~tqGS!z)+LuQ%+L^LNeNw2gdGe<8iGcP5+xF9uILmf$>x~3HaL?N1bm}p66
zZYqooEjZ%ANhKbXBpK3*@^ck17LV(dWfqqv=44i-B3MxK6$*0lOWca`b6ql%OK@pV
zN-WMyF4lukFpZF^+&eQbH8-&&GuaE&;teTE%nE=xFR>`SAhD=86_=^GkdQ2f8JM10
z;+vTVTFdTOlpX-mS)`)?t=fF@^9$f1j~ov$%iw)Km_R{hUVd_NMrOW3VlgO!VB&eD
zxdoLVvAhBXQ2&1#4+FzD9?1HC*!=e?eg=jK{0t1C{0s~x{0t1d{0t07`4|{N`4|}F
z`4|}3`4|{3^D;0j;bmYb=4D`r<z-;-;ALR2;ALRo<As>_6=Wu~e>_T!hQMeDjE2By
z2#kinXb6mkz-S1JhQMeDjE2C#gaD|U48owUq+W4OYHERop`HPxgAWo<$t*6&Nvza^
z(xA>^Ft{750qM;`RF-Fe29-jJN}=;;P}O>AMX7}fwhD&eJ~vFT2sCPBtDuV_s(?IW
z=bKubk(^lK0<l{owF1(sh1dldnNUhh&d5wHOHBa{W0vNWC_#JXpvj_;`11Hv4Na(W
zq%N&S5p0r5M*%!Kk{6$ymtRt>psipC5rOD|E7L2;FVIj=DN%=r<Lu=kj}7Q3z_mkk
zfr1g<Bh>?Sv7Pht%2JEcL9=BVIr-^&pfT@a9R(1xpeQvR%1l8p(~A;QprXYk`2}!h
zDu|huneLgFl3HPlG;N4#pTBc(eoiT92ncLl1yp}&v5o><v!RYcaYlZ*EhrKZ?#{_i
zSFlxpL^UXAofC6%k`j}%i#0T(4P(L4nwbU(ONb%`&?rh$ennbJN&##l10EjH>V?JX
zv7jL@SUf;QR24LgwDJn{3NkenbQQo1u-V|z-TXYzv~Ye=wnBMkNk(R#f^I>kg04bA
qrUGbq(LmRzAQL<Wm6%tVn_rX)aXj2k9jNOSY!$GN<v@mqG#LP({Dp=9

diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index d20301e..ace01fb 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -29,12 +29,14 @@ if importlib.util.find_spec('shapely'):
 # and probably better. 
 # i'm pretty sure that's how croco devs & mim_people do mpc
 
-def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): 
+def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data): 
     breakFlag = False
     log_item = {}
     save_past_dict = {}
 
     # check for goal
+    if robot.robot_name == "yumi":
+        goal_left, goal_right = goal
     SEerror = robot.getT_w_e().actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
     if np.linalg.norm(err_vector) < robot.args.goal_error:
-- 
GitLab