From 8fee179d9e22fda69f105713f0744d2db628fa69 Mon Sep 17 00:00:00 2001
From: Your Name <marko.guberina@control.lth.se>
Date: Thu, 12 Dec 2024 15:54:53 +0100
Subject: [PATCH] stuff

---
 python/examples/.smc_yumi_challenge.py.swp    | Bin 0 -> 20480 bytes
 python/examples/dummy_cmds_pub.py             | 145 +++++++++++++++---
 python/examples/smc_yumi_challenge.py         |   7 -
 python/ur_simple_control/.managers.py.swp     | Bin 81920 -> 90112 bytes
 .../__pycache__/managers.cpython-310.pyc      | Bin 35908 -> 36308 bytes
 python/ur_simple_control/managers.py          |  38 +++--
 .../urdf/__pycache__/__init__.cpython-310.pyc | Bin 198 -> 186 bytes
 7 files changed, 155 insertions(+), 35 deletions(-)
 create mode 100644 python/examples/.smc_yumi_challenge.py.swp

diff --git a/python/examples/.smc_yumi_challenge.py.swp b/python/examples/.smc_yumi_challenge.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..df6c5270b503b62a62ad1f626b8dd88eaca12a40
GIT binary patch
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|{(5GAe!L=@^FRnhXr-`NakKDTzfODSS9Lu{b+3FAtv*
zuv{I?K>gC9_~Ojmf}GU&<ovvnqWm2Fg36MN{5<{CibRlbv3_xGa(rcJZf1OPMq*A*
zYF>J(UO^?;lu=?d1V%%E<Paz=Nz=99WiU1}Gyv&UR#H?D777J1NAYL~jE2By2#kin
zXb6mkz-S1JhQMeDjE2An34xLVW`=qO1_mamf4!hIBO1*D<$FMBZzv5@#|-7`K<UX)
z8YU0rj#8r`Fd71*Aut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0s{~Ni75;W;tUK7
z(V&jH00RT8|NnxYf#Ehk1H*oP28LdK1_nQV1_pJ01_oAs28J_y3=F6F7#Mc&F)+;L
zV_>l3V_=xW%fJxA%fP_P%fRrJhk@Y>4+Fzz9tMW(JPZsmJPZutJPZuSxfvLyb2BhZ
z<7Qx(%FV#g%FV!##Ld8<%+0`XgNuP-I~N1PR4xVvDJ}+vubd1F*Eks%j&U+D9OYzS
zSi;G`u$Ys9!G)87L4=cmft!<o;WGyV!*&h^h6x-D3@IE84BQ+H3``sh3=7#A7-ZQQ
z7&fypFu1cZFu1WXFtD*PFdSoLU}#}wV5nwgU<hDkU{GgeVBlkAU|?lsU^vXez%Yx2
zfgzHGfkA|Yf#EhY1H%z!28P4T3=Bog3=Edc3=G?t7#M7r7#KD(GB9jlgoIfXDBQ5e
zn*tORr{<*T<t65(7RTqLrj;nzDpaQ_Rf9$2^NMRB%N&aGlXBv-@-y>F;;S>Y4QrLG
z6fzak@{1HQ6*BV_iW2kEQ#H&rYZ(}zhAM#d7ndf*Cl=)v#}}6*mZU1!LPV2`QWHy3
z<BLm^ij#{n3raHc^EA9bMhAmb=_nxEUXYVml9peTs}HdoY)-L~4%DE;oSdY@<ZL86
zbQB8ni{lH5^3ycoPEm%a20JD<IR~FJ0`iMf!!t`Vob$^Pi!u}Ql2e0A5_1buQ*;#6
z^%HZGbK(o~i&NF1_Ji2*$xugNvl$);Nr}a&@%btFx%e#iPsz_sEh(zR5dt9XAZZNW
z!wtvg1Z9Qdg4E>9v`mmYD-}xe74l0<3Q9{9(u(qP6^inc@=M}#@{<#DGOH3nF@Y3n
zzK+4co?)&EA^tA@RtlB*r3&SVc_kntl2R2wu~(Ftl$rwKCn^*a<rkzDDIlAqP?C|E
zm#%|I5}>4!l3Gxbp<t_Eh=?ALa7JcvNq$kKf~`VPa!x^|USWQ*o(DuMAU`KFxl+&D
z)iogA$1ylW2kt_U#+1^c#H7re%o0?+E^uL}_K+Y)zhF;SzmRwze`iM@R6~kVb26c(
z*dpu?%nuGiNI=aAa`o|abn<aU7ywIeC~gnT4-P2GPs_|n)qp!!Ss^{MEL9;lRiPv!
zRUs#_B(=B%5=IKCWvO`znQ0)2qEv<Q#Ns^l5{1M(h0=nQ#FA9F32+8DT_qN!7c1B*
zfGAXnoc#QP_}s+2#Prl61zQDVE~-LE>?2g7@Zn0~3>0a-;?$D(g3_d%%;JpHqWH4Z
zocN;5^o$Y>u-ghslMq6Ps3v3rsQf`S03<@Zeo#(8)ejQUL?l6FOxL01NVt~>7fE3M
zg36xw<ow*+#Jm(G9prS4Y8S{O<XZ*OO{`5IFXOZ+q&%~@1V;e_^+kM6W^QIlYEgV%
zeoCr7$Ta=r+!SzhX6D7`C6;BTgG&m0z6I$b%MBon*n<<E48c*Do(d|q($iCmG&J=x
z^V0G)(v*~yl@)?Z5{pVQ^U@VkN^^576`<?&6iV|_Qi~M8m0Uq$a;gHNdQ(EAF0jGz
zd9X~SSCn5IkI<s6pro$^cZ9M+alS%%szO0xaj`;Xi9%vAD8D8arI+TW=9M6nA(;ts
za#4P9W?E)y3fNpoN(BkSZPCy~I7e9_B|o)TAuqo~p*+7R8*V0?QCwO8_I`YPW?p7V
ze7uHwkbkgqer~QuVqQv4YLRbbX>O)E#K4r)GzGXikfU_K1&0o_27r|TRtiC&8o?J*
zc<7*%9ST+o&XCoRKKc0tP$imH3=GLRpg_Q4w}xMSN-9VNsH84REh#O^1KSGr0jS(k
zuvI8XEGkYd(gRcRATbSygA<F=lk-7qElX1M5=%?+VH^#x$|8s&2wN{PB?Y7v9P}FM
zy1GUA#k%kyRo77{sVqpfEiNh2QAkNmODxSPu~mYu302Zj$Vkm8uvG%pZoZ+xAqt?X
z5!9l|FGfl(mHDOWMX3s<#hH2O&;$dqTESKU6o3BC0U!qyV{sfrg>z13wqsE`s)GD9
z1yCy^zqD8(J2Nk(SWm$>F&k8~7Nvq~+w8pja)t7YL{Ke}s*s+an4^$ctdNwNmJd}>
zRGO!#4h~F+YxGi5;Vmi!TLtx?(maLaoc!X{6mUc;WagEn78R$0Y)j2eEXhnRR)|5;
z4soh)W?p7)Vvb``dH^_TG&C7Ni3j9ONc31SFfb^S0*s9rAZ1}Zs2nLMO~P3+;A#az
zT7ShL{g6^e8B}o=gMuR?zg(dtU!gcPFGT@bfE8OQ=tJXIKe4nTGbb~#s8SzN_aKE3
z1BM~#sU@H)SfL=lI1|(sDppV^$}cujD9JC#Ojbxu&dA4RFmlT@J~y=}JvD{2@Iehr
z1_p-A+=Bd~5`}`yy!_<kjLdvc=~9rH#{h1!VXRHo%P%Ma#Taa@vR-mgesX?FN@Y%b
zZb7mF)G%j|pr^NQfHSeifMPs9xd5E7VWxn@V2K@<ImtPh*?J%XNfRvn>wvNghyyK2
za#D+Q6rgLYL8S^vN=G3#zbw@UB%WK4lbM)T;tS)0*J6X!fr<u@6huR2UYU1hUT$Cj
zRIf`ZC;{i<av`*|D@Jl4daBpKQr7996ppZ1LMo`hWhyu;!X)!ba|<d#(U(`i0C7!X
zQc^r*F?oDx38>+knOdv|F07DT6OU3)Ks4qhmc{25rx)wx7N^6M!g>r46~(D}#rZ`T
ziV&F!qAWcX(#^n72yGkS>;plxLmJ4b6{*RkCHY10F!U|WDaj1UC`wICNlkHu$|CCk
zwI*Qh2Gt9&AOf4mpsWxO8RFsZ7w;J4?(FXy;Nu$N8t?DT0P6o2b22d4b3xYs!{+Y~
z^D{6^;b&k-<!4}s=4W8A<7Z%C<Y!>G#K*v}h>w9`CLaUC3_b>iW<CanOg;vN6g~z9
zKRyNqeLe;TPCf>PXS@syM|c?+X7DmF<nS^u*z+<laPTrPJm6tqSj@w~5X!^A5WvI0
zpvl9)@R*x{VK+Ae!*Xs0hDF>A3_aWo3>n-E3>MrB4CdSn45Hi&47a%$7#46bFw}4{
zFr;%aFr;xYFr;!ZFnDo6+-}Fkz+lV8z+l70z+lbAz+lD2z+lP6z+l0}z+evY8}>f>
zsOV@2jE2By2#kinXb6mkz-S1Jh5%_H02+A6P0Y;G&}7I24S;|e+VSxUwzdjN@$n#u
z_;@8ye-@+;+z{0(&L}NO$u9@#01M_9gNLKyGazFspe}YvQGO+Oa11(Bp{!7zk(vkY
zIi@ETB_*b(DkSIU<fJB-<QFNVq!xqJ7lWn2ZQ%T*tkmQZ$N+}2f(EGfmY<iHqoAvh
zUy_kpRGwL!3K~%=&&<hDNJ>>m$<Iqw0F8ivd+dohIhEipyRw1{#AJlQsTBn|naP<Y
zIhBY3PjIhSFDE}8HXy85kdt5Hn3DtTfPp$w#ZZ4h{F{_tf}*}Su`JamKOL$jGYvd~
zqX!a=&&f}>0y_pwz`6u_#RcF12N?$!0}auDe6Il-R0H+XVF3yW2Xs*dTLo<6NuU7;
z=#Ula*c8MRm~9HS3Rt@%P(OfOi##f>U<>JTLc5I`DAs_+U39>sFYzTsiFw7K*(e=_
zqS8EDx5S*{REW*WXvQIW+t>_4(yR;$B@hN#qhPC`0iwb4a8NtJgUk?1V9LPKpuw8_
z#2kd1^q`|V8k%~E$t7TOh|-2bG2E|uU{(%z$T!+BR>4-mK+g~ygmCwORe-_;G@h54
zrx)yMtmm1MnpcupQV9)nm|DG}{1UKH3bqP)1$wEKsT#%*XJ)2B#v=8=Nftan3eKmX
zUDisFWCjWw&@@XvD2=A1re)@(y5}e61c2raoqar^R)O_{5)eokbeIwx3lMuikqZjE
z)DrN>22>N+Ly!^xG?)rjZwpci8T|qk6%aAM{5<%OCdgA@jUWbemJvRG03I+0$DSTk
zOe42A9pVOv`rP7ly|UDt{A9@NRkVSQ0y>ICG9IKn1Ju1P(kli{O2S6bKw~n=Ir+)i
z8k%}}`Q;j#dL{XwzCF}uaJv+26%a!K$dX8QfJTO4&P|2P>VT#mU_$iw0jU3P&A`A=
z4w}anU|@i)@87`Bz);E0z~IEsz`)DTz_6W<fgzQTfkBs#f#EAJ1H*n^28M;a3=Ao}
z3=C?#3=IEy7#L3RFfjD;Ffau2Ffe#P`}TK0eRpmKh6&sZ48hzC3=-T740pL07$!mc
z>6}~)3_Ca(7-~2f82mwV{u~SpYd9Df>Nyw~f;bo$bU7FpbT}9oUa&JTY-49&sAOkg
zuwiFl_{he<FpG_WA%=~CL79z#ftihgfr*WQ;VdfyLoq7@g9|GIgA^+R!wVJ$hHWei
z3~ek73?VEG44Nzq4Bw#h`jeO$7|NL$7>t=27~U{3Ff3zYV2EX6V2EL2V6bOmVBiF8
zieO@3c)`fPaF&sQVICs`!xTnHSe1doi?q5DSv4rpfUvRxXtasEgaew&!IyAyGV@Xs
zi}Wf$B??j!gjfh3i$Y4Ia0N+4sfpQcIf?18<d0nP>J^pd#pmax#)IasGz=h_4K#WU
z8XgAM<B54G#o&1t*t7)5mmmyI`QV}hUQ?FE7e<?CXKI^+3O%qqc=-Wjo)eyPqcahi
z6u&`e#(*;xEdLW>3O>icMy!c72Afl`nSrsw15yx!{0YMt0h5}RmS0p7oe8odH7yMu
zb2v31g@J;tLRo4KVJ%2*1=TtE#TZ&LVd(?Z<ST~6Su}`^8e6G3nQ7pO9?&utuvb88
zpz8IqQ!9%#H1YZw9E&7)8LXZpAA@y}>|u~DEdIsrTaaQ5&w}C}6c{?7`4AmYyyz&T
zrlo-bsw%SpR26{7t#v>}gdWU&5FX5b5FR9Spuq^rji99{XzO;sc@nH#!3wFGLe8ZH
zMVWae8cIc}$*GxTsVNHjpan6|**H+z0>>U^4mJj*Ljwf`W$<#P<ovwi%;J*Nyktl-
z8jnUJkVa6YTLPLh*MpFWdFiD&iA8!<@C2u<piz>MTB(qnUz(EwZm@$JCMgQV`MIf(
zCcQ#=Vx>ZU5vZkJl88{4pO#jfTB4Aj2387QEdwdBK#o#|F1tz2NKMXGC;`m^Rw*PW
z=7DS|1<mLs7Aqv@7lZ6BNz_zG%u7)y2Te2-XXZi0;4Z`wFyOWvy8obsGQq$xz!o@A
z%@tsqQ6d^t7l7A4p~N$6`U09XlodQY-93CF6?{CsU40@ILOdKp6hb^agB3i375w}`
zY*&RaS08_8&yYw3e>a5?4_5^z$6!|lw;)GfS45`=Bc;GsZsAWZh0)-|l2@Py2}ML~
zU<ty)XmA3_E6{^P141!)fs>I|UV&a>QF2LQo{>gzW?o5#5okuMxFo{}p%NkvX$t3L
z<|XE&>*eJa<!VHOq6M_N43w`@_411{Q}ZCrX1xk<CV_~9RUk`N;*+e3MOd7i4{`{o
z-&359!>n>}EJBhZD0P7sFO_6uf@fVpE11d>b4pV|sTahlfTnMSN`=IVOhj=EOUCJ`
zd8tK-C8-LD3h*94ZemGMW(8y+8ra~%(!`>Y)S|r1{5*y7%#sYm3@Bo`m9m1rpQ}Ph
zkf)E2r@x;<fNPMms~;$7fRl-zdWeFve^8LCa|n|2K^bC*Wza%MR|_>AzzSJCyxFrb
z8lE}92?R!fr?mADnG+-qU%iOPmsScmCjjC36l)Yj`iCm`xw^V2g!n6X`h~c<2RVkg
z>W2oqD)_tj`zm<)={mWFguA->f%0uIQg#JrF3?)C_>#=rRM3(&un2USC|FFxP(Qh(
zC?_72grJKrQJQo}DI2Ct!B)Y@6kP6SR4Kp{1Dv6(fV|99A2N4ZtPh&M)X&LGNi5PY
zPEO2IQm|6+Ni0q+3QkVUBcwO2C_k@+RL#kWxv52o`URjqY_So@K1g>qC%-hs2w|f#
zr1~laEeHfHZvy4H{G{TN#N?b*<lKhhDfr@9a5ogZehd`p5KD?-OIs0^pauzCH>h<$
zShF%{NmOnsc)dz#acWL#aWNv;LB$Ae3~3<Pg3_&)f;l2Caq9rdz#17aeTXswB#Rrv
zauKFJaI!;c<D>SNL2V)1oXp}9jpF>GlGGG%mH{_`P@9XInh2MGO$2F#D#5w9x`>#?
e)wunHBtuqUk{Omn1tlo?5Fe5t)zGFWC?Emj6Q(i%

literal 0
HcmV?d00001

diff --git a/python/examples/dummy_cmds_pub.py b/python/examples/dummy_cmds_pub.py
index a3aa151..b77cb14 100755
--- a/python/examples/dummy_cmds_pub.py
+++ b/python/examples/dummy_cmds_pub.py
@@ -12,6 +12,24 @@
 # WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 # WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
+from sensor_msgs.msg import JointState
+from nav_msgs.msg import Odometry
+from abb_python_utilities.names import get_rosified_name
+
+import numpy as np
+import argparse
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+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
+import pinocchio as pin
+
 import numpy as np
 
 import rclpy
@@ -23,15 +41,33 @@ from rclpy.node import Node
 from abb_python_utilities.names import get_rosified_name
 
 
+# you will need to manually set everything here :(
+def get_args():
+    parser = getMinimalArgParser()
+    parser.description = 'Run closed loop inverse kinematics \
+    of various kinds. Make sure you know what the goal is before you run!'
+    parser = getClikArgs(parser)
+    parser = get_OCP_args(parser)
+    parser.add_argument('--ros-namespace', type=str, default="maskinn", help="you MUST put in ros namespace you're using")
+    #parser.add_argument('--ros-args', action='extend', nargs="+")
+    #parser.add_argument('-r',  action='extend', nargs="+")
+    parser.add_argument('--ros-args', action='append', nargs='*')
+    parser.add_argument('-r',  action='append', nargs='*')
+    parser.add_argument('-p',  action='append', nargs='*')
+    args = parser.parse_args()
+    args.robot = "yumi"
+    args.save_log = True
+    args.real_time_plotting = False
+    return args
+
+
 class DummyNode(Node):
-    def __init__(self):
+    def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
         super().__init__("dummy_cmds_pub_node")
 
         self._cb = ReentrantCallbackGroup()
 
         self._ns = get_rosified_name(self.get_namespace())
-        # test
-        self._ns = "maskinn"
 
         self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
 
@@ -48,26 +84,99 @@ class DummyNode(Node):
 
         self._pub_timer = self.create_timer(self._dt, self.send_cmd)
 
-    def send_cmd(self):
-        msg = self.empty_msg
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.velocity[0] = 1.0
-        msg.velocity[1] = 1.0
-        msg.velocity[2] = 1.0
-        msg.velocity[3] = 1.0
-        msg.velocity[4] = 1.0
-        msg.velocity[5] = 1.0
-        msg.velocity[21] = self._A * np.sin(2 * np.pi / self._T * self._t)
-        msg.velocity[28] = self._A * np.sin(2 * np.pi / self._T * self._t)
-        self._cmd_pub.publish(msg)
-        #print("published")
-        self._t += self._dt
+########################################################
+# connect to smc
+###########################################################
+
+        robot_manager.set_publisher_joints_cmd(self._cmd_pub)
+        self.robot_manager = robot_manager
+        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)
+
+#        self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}platform/odometry", self.callback_base_odom, qos_prof)
+        # self.sub_amcl = self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, qos_prof)
+
+        # this qos is incompatible
+        #self.sub_joint_states = self.create_subscription(JointState, f"{self._ns}/joint_states", self.callback_arms_state, qos_prof)
+
+        qos_prof2 = rclpy.qos.QoSProfile(
+        #    reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+        #    durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
+        #    history = rclpy.qos.HistoryPolicy.KEEP_LAST,
+            depth = 1)
+        self.sub_joint_states = self.create_subscription(JointState, f"{self._ns}/joint_states", self.callback_arms_state, qos_prof2)
+
 
 
+##########
+# base is a twist which is constructed from the first 3 msg.velocities
+# 0 -> twist.linear.x
+# 1 -> twist.linear.y 
+# 2 -> twist.angular.z
+# left arm indeces are 15-21 (last included)
+# right arm indeces are 22-28 (last included)
+
+    def send_cmd(self):
+        breakFlag = self.loop_manager.run_one_iter(0)
+        #msg = self.empty_msg
+        #msg.header.stamp = self.get_clock().now().to_msg()
+        #msg.velocity[0] = self._A * np.sin(2 * np.pi / self._T * self._t)
+        #msg.velocity[1] = self._A * np.sin(2 * np.pi / self._T * self._t)
+        #msg.velocity[2] = self._A * np.sin(2 * np.pi / self._T * self._t)
+        #self._cmd_pub.publish(msg)
+        #self._t += self._dt
+
+
+    def callback_base_odom(self, msg : Odometry):
+        self.robot_manager.v_q[0] = msg.twist.twist.linear.x
+        self.robot_manager.v_q[1] = msg.twist.twist.linear.y
+        # TODO: check that z can be used as cos(theta) and w as sin(theta)
+        # (they could be defined some other way or theta could be offset of something)
+        self.robot_manager.v_q[2] = msg.twist.twist.angular.z
+        self.robot_manager.v_q[3] = 0  # for consistency
+        print("received /odom")
+
+    def callback_arms_state(self, msg : JointState):
+        self.robot_manager.q[0] = 0.0
+        self.robot_manager.q[1] = 0.0
+        self.robot_manager.q[2] = 1.0
+        self.robot_manager.q[3] = 0.0
+        self.robot_manager.q[-14:-7] = msg.position[-14:-7]
+        self.robot_manager.q[-7:] = msg.position[-7:]
+        self.get_logger().info('jebem ti mamu')
+
+
+#def main(args=None):
 def main(args=None):
+    # evil and makes args unusable but what can you do
+    args_smc = get_args()
+    #print(args_smc)
     rclpy.init(args=args)
+
+    robot = RobotManager(args_smc)
+# you can't do terminal input with ros
+    #goal = robot.defineGoalPointCLI()
+    goal = pin.SE3.Identity()
+    goal.translation = np.array([0.3,0.0,0.5])
+    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_smc, robot, goal, goal_transform, run=False)
+
+
     executor = MultiThreadedExecutor()
-    node = DummyNode()
+    node = DummyNode(args, robot, loop_manager)
     executor.add_node(node)
     executor.spin()
 
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py
index 2af6a7c..d317f05 100644
--- a/python/examples/smc_yumi_challenge.py
+++ b/python/examples/smc_yumi_challenge.py
@@ -188,10 +188,3 @@ def main(args=None):
 
 if __name__ == "__main__":
     main()
-
-
-
-
-
-
-
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index d0587e1010aa996ade5bcbe92c010aa75a4c6838..e7d74c6d65fcc80968d7fb50b836b82eb7e3379c 100644
GIT binary patch
delta 6794
zcmZo@U~M?SDwSjq=IN_op=ZRvz`!8D!0_=$RQj?rF$~W&H%h(W=NDn9XJBAp0!cD3
zFsMux6c85_f%43tw8~^jfqq7z$twlq8M!8(6p&}+p8Qfko{?uVr=UC|?_^Cuc}Bj;
zo`UX-+><8?ir0%m^{GSY*~}0IBZOjLh0;1udMlbdKU6#rN<To87l4ZULuo@62m@-q
zC4(S@&)@~68&DN6K+R(?fXbhO%1>r2blw~wWXn5AfrU4olYxPo3leB5lLZy}IkmYU
zA_ALNDkO7yDkvzV73JqD6eZ^rRO*#v=B6rS<`(1^l_-Q{=B6?zz~qZl^NRC};&Y4B
zi}iAg(_xCd@-y>Ff=d!hQa6Wct!J%&!^XgHf*oQX%wbpg85maZGcc6$Gcc6!GcaiI
zGcbJRV_<l}$G~uukAb0xkAXplkAdL}F9XARUIvCNUIqqLUIqp(UIvDLJPZt%c^DXG
z@-Q&u@h~tr@h~um^Dr=o@h~v_=VoBI#m&I5j+=oYkDGzPlba!&L5iD!;Wifo!x}CI
zhFUHLh8ivgh8Qje26HY31~D!MhA*594C^=<7}_}*7?d~}7~XL(Fg)a7VA#&Vz%YS>
zfgy;4fx(!Afq{pEf#D=O#52d)H#4%lXPZ93kdbGypC{Mm&6d7w+mjR-uP_QGrZ6y^
z<z-+H<%4*dd$OQ|as2@v28L6-3=Fe)85mM|85q=g85mY`GccrbGccrZGcXu)GcbJS
zVqloh$-p4b!N9=F!NBmEoq-{jje#MDje$Xim4V?f3j@Pa76yiD76t}g76t|>76yhN
z%nS@ym>C#0Ff%aJF*7juFf%YnFf%ZGW@2Et%*4R3go%NngNdP@A(DxK!HS82ftQJa
z;Uyyj!*)gnhHZ=t49$!T3{%+|7<||n7`)jT81&g082H#37@o5+FqE<}FeI@tFi5a5
zFnniaV0g*Oz_5Uofnh!?14B3~0|P%R1H*9^28LrS3=B(H7#L=-Ffi1zFfb&sFfg#O
zFff2}KoK(ogB!CN0|Of~1H*nM28LoL1_lKt28J_?3=Cb23=DOQ3=El!3=D;g3=ETf
z-^)#wxx+EJVJ7=z`FKvREKq7Hj!(`_(JLrT%E>Iw(8w)L*JJ=ELr_wL;oRbMy|UDt
z{N&7%%IM5k1zUx}6p*&)Oa)y9L!(%Q$>%#|ObtzS6pSo1tw_)g(OGYdrU_(UMrvY8
zYLQ-XNn&n+f-N|EXlUw{<b(2rh9-&~pbTQGkeXXiQVEhpkpp=FE(7<3USdH(YF>(l
zfu4aTgTiF_-`tb$bn+V;S)w@vu2aEQ0g?BRtXG~a7%x5jy(}Z^<g;<Yj9~8M-*IAK
zmLa3^WT$v#M&-$=V6r1#QA8Q+pq%1VL@+5&_cvnnWmKMA=#|K%pf&lum*nJkUgq`o
ziuPHlDVes$ItsS73QCowxtU5<_KNlj8i`g4@xjg^Itp6xT8Z(wnR!+Ue))N+3N;Fj
zMMa5~KAG96TJc&s3W@Q#i4~Y~ItuxvB@kg-Jv}`gg_M%Yf>dOI@{H7?dZ>s(jY2%!
zSf~8_ocQE?h_?Kql++?Cg?N7u?d|C71`#eUP0G)<Qb@|r&w&bLrspM=loq90DHN9!
zDby&GlosTqMnj!kTvDW?pr@x73sIa}QIemOWu=f~o>^QH?U+{y6;DnqF3HSGw^E3A
zhOoRd^HM-sG!>u$Wv^(jq@ctw+0j-^uu#EPp&%0~VQZkHFnObo(&P{E{Gd>U24Hex
zPGS)#tm2dNX&aPCAz42VL5UQS8hSbkL<J=%7^mMcX5^T@)`(GX^4fUm=?_>K*|?Fz
z^sKibB0S+9iw335SUQDh{lFwnc!+9&GZ0ZBI{l(4qYNm#z`5E$M<G8sIU_SaK0hy~
z5}vQ&p-7|ftJl*}0Of9^Bnh{Y9+}$;n!DivNPPHBUhkzlUCWG7W^%qyH#{90>L^V9
z<)Z@%S8%?D`-Z{*gp_$9l?ADwLIz$&x`0GLg$7zMQY9UN>IN82&P@STW(u|npxQ75
zn(bh+AR0wQy&g;nsI)83EH2T|gw++)4-(AasE35ofF&&%1$f1QnY1>G`n_kYpTW++
zkix+L?$N-y5&!ua7(VecFnr`^VEDk#zyNAIZ{cTP=;dc%kmhG#;NWLq*vH4fFprOc
zVFDimgF7DsgBu?MgBBkH11BE?!y{e>h5}v&hFD$(24P+XhEF^U3{Q9%7-sV@FwEj%
zU<l%2VBq3mU^v9h;LLE4n}K0IHv_|DZU%-5ZU%-}ZUzQMZU%-8Tnr4kTnr43Tnr41
zTnr4mI2jo7I2jnEIT;v^b1*QpaxgGda)2DanUQ54`{ag)9FxP|D{kK6RK&jde>|(h
zVwMd7^%4vW3>Bd6i~s`ztQYixpMl{LKLf)Aeg+0Neg+0HkQezF7&h`TFnI7WFg$=Z
znpJoi7*6vrFwEs)U~uMPV36WrVED_;z_6K{fgzonfkBU(fdSOi-@?Vfu$YU1p^J-w
zA&!fIL63`p;Rh!J!xK)1dWNH%3=9>V3=E)7kSiwx11~27!*>n_hJ73i40|~k7`iwZ
z82mXH7)&@A7&tf>81Aw&Fsxx`U}$A$U?^Z`V8~}@U~pq+V6b9mV31&EVEDquz_5Ug
zfx(ZBf#DY`1H)2Q28IAu1_nu128N3)3=BtE7#Jq8FfdGHVPL3WiDqE%XJKHFXJKIY
z$jrcSm6?HI2{Qx3VrB-0c4*UGj+uc$fSG||9}@$^UM2>H`AiH9DNGCu#!L(hs-Px4
zBLl-FMh1ozj0_BOpbcnHh?Y-gER+U?`u5dKjG?@fFRbL)oZ+a<SP$xrNHH)l6hVU>
z*10^u&%n^Y&%j{A&%j{7&%nUK&%khskAYzi9|OZwJ_d#~J_d$VJ_ZJDJ_d$wybKI`
zco`UW^D;0r@-i^^@G>w6@iH(7@-i@7<zZl$$HTyo%)`JC$HTy&z{9}soEy@&S;fsz
z&#;o4fnfzV1H&Y428J$f28MEIhern5;knMmz_6T)fuWR(fx!V9SKB!m7-~5g7;HEh
z82C9E7;bSeFiZ!hE(Qh_4hDul><kPW*%=s0*cliM*cljJu`w`gVq;)f#m2ypz{bF!
z$i~2Ml$C*D4Qn_9Lp>`4LmevvLmVpugDEQm!#8Mr6tXZdc(X7t@USp2tYT(h=wfDI
zNMvSUux4gp_`}4&&;|{92POsv5hezP6VPA>g=FDm#zI+e?{9mqFryCF^pA3kA_~wR
zN@`I&qT8a7nOBmp5R{*kU*bFcv@D~7B9fdfRxQ&L<Qc_v5Xv-)Q*+XEuquW21whtI
zY~LWu7{D|=j+s$#@`PjZ+vhSevO(e>l<i`m@edn2xX;hPPyvni^Lz{p>-ZQL8u%C(
zJoy+H)c6<}j`1=ur1LT`ctE?}zjzoJKz-Oo9tH+Z9tMVc+zbqJxfvMxxfvMTxEUA}
zxEUDaxfvM#axpO6;$mRv;bLIu=3=M^=fgrS1_ldgSNj4d1H)2I28NlO3=FND3=9RF
z3=HO+3=D#t3=AhY7#O-a7#Q3+7#OTM7#IYgS?~@!1H(#o28Oxp3=ECz3=BE!3=GEX
z3=F*N3=Fr~7#QZUF)+lkF)&E7F)&<YWnkFI%D@o9%D^DU>de4!gN1=%DhmTcC<_Av
zXh`K2GXujsW(J0MW(Ec&W(J1aObiT@nHU&?nHU&EnIPF~A|nGs1|tIlDAb~1S!?nw
zJD%-c%8c81wzms1-eTOefJI<Civi;wesC_cWn*A4gbsAThL}KQ_iug%hPV6-40rh%
z7$);GF!=B@FmUlRFdXD#V5sC{U@+ukVED|-z;K?IfuWw4fgy^QfgzHYfkBFwf#CxW
z1H*D228IymAjd^$sL$bMV3^I#z|hOhz);W4kk4Sp&A`CL&A`CP&A{-3i-F+@G*^{#
zF)(;?F)&DTF))Zj2RwdrGBAARWMH_?2`R@xF_Fy4z+ef@ZS0&33<o$E7^ZSCFa&Zi
zFi3MSFg#^vU|7%2z|g=B^5kYl7H8<-2VWA$=Glt7Ilw_Mhl7D3gcDMI!A8k$^D{7P
z<!4}M<!4|B;AdbEf)+F_d<+cDd<+ahd<+aid<+b)co`Vx@-i@V^D;1G@-i^U@G>w+
z^D;1e<zZl$!o$E&#>2qi!^6PvkDGzvH8%spNp1#)rQ8e*job_j{@e^k46@t|3@^DD
z7>;l;Ff4%<D3M$Y3|i3Pt$R@Kc0k8^LN;$S*u@0S#FHiC`3KEVLlW(W8Yc6`OAi|V
z`cc=vlLO~UOcv;58&tzbiPZHYmf@pe9;LE_E=sALY#nXICd<Wh(IQ(K8$jC$pe`8<
lgSu>GSO-i;M?4i26p%+eSq=0IA%mXmItpO+<_}(~%mCDdS~may

delta 2375
zcmZoTz}nEjDwSjq=IN_op=ZRvz`!8Dz!0}GGQHR?iJ^jHqtpw2ej$c>1_lNukR$^G
zL&Ri30rANm0u78@lTQlBGjdOUDIm|tGnrFRo{@L5rl33{-(*igc}D)pnS$<&@{>0T
zir2G3^;$vc)yxnEBZLCk31$U?Nd|_yX!3$kaUUqH#{yx%^b0|_3^4gBRC!4TsCf)(
zP=&{!3MMlaI&YpJRKPp&0PiFP7G7=!28MQ!sRCe+D)e)PaxpLjaxpMCZ(ga8%sG9V
zKcm;?Fs*g0^)EOW80K(70t{yC34R8KW_|_+JAMX+pL`4q_xKnXF7YuiEazij$mL^T
zaN=WN5aVNDc+Jbe@QRm#;W94+!#Z9Dh9+JH1}9zy23cMPhIc#+4C{Fq7#esO7y@}1
z7y@`07z}tA7`S;D7+!HRFr46KU})lI&|`?^W?=Zn#lUcgi-BP)7Xw2#7Xw2I7XyPD
z7Xt%37X!lsP6md#oDfIO-o7z_aTn8O0Tv^+=@SeYc_znM@N9l)?#(8cn8Lttk&}TT
zlMCV(`N@I~#`Pa~85mCSGBC{IWnf6<WnfU}Wnj3^!@w|;hk;=R4+BFw4+Dcb4+F!0
zZU%-;+zbp0xEUDgxEUA>xfvJ?xEUC@xEUBWa4|4U;)0kAa+D+&1A_z?1A{mh1H&Iq
z28P?53=CU185o>685s0A85o{&FfbhDU|`tJ!BEeT$HBmm%fY}9$icv1z`?-4$-%%d
zot=T9g`I)Hik*Q$lAVEpo1KB-5E}!-K{f`4N;U=tLpBD6m#hp7FIX8Ej<GT@>|$kL
zn9s_<(89{Vpux((aGiyLVG0WaLk0^2g9Zx&!$W2UhN;X93{K1p4DXm27}hW`Ff3qV
zVCZI&Wnd^~Vql16VqlPFVqmz)$iT3Wk%1urlr$I^HedOt!#LURzUJoMh$8mME7k~3
zk1=A@pZw^u_+$kO*2zAxeA5Nu82KjKe`1~FAvt|P45Qfe*G7yE+yBNeGBZ!#YRK5N
zdA?sNWBn}_28Mf}+#$fg0LvVk`571%@-r|r@G~&P@-r}4@iQ=R@-r~p;A3D|%E!R4
zgpYw?Iv)eWG(HA~seB9!ReTH#k$emcMtlqm2Y4A6*77niB=a&bIP)?vnDR0(yy0PB
zc+JDWaGr;OVI2<xLpu)xgD4LJ0}~HJJ;M%e28QX}3=Eyz3=9t33=H<%3=H477#P-b
zF)*ZaF)*ZYF)+ArF)&zgF)+w-F)%Q3F)%FSWMBy8WMB~DWMFv4!N9PLgMlHQgMmSq
zgMr}yI|D-wI|G9jI|G9pI|BnJI|IW<HU@^<Yzz!D*%%o7*%%n)*`S_g*v!guhh=iZ
zMUKsP|0%;lw3vNz(jw968$uYlCQtBZnf{)Kk!`wgG^5butVO(V;mHNgV#op<)61h7
zg{EhlFmg?w63r+%S?dwk^c_&fWNsJn>6zAy@}PLK*p$HHu$)DKQD8C4f`EEn1_p*)
zP`nB-Fu=;6JNyg`#rzBm-24m-NBI~S{P`FdzVR|JoZw|($m3;TaOGuS(BNfYxW~i5
zFo}nOA(DrI!Ig)B;XXG5!#QpShON*LjpSxvQ08V};OAyw_`t=$u$YU1p^J-wp#U19
zN?Z)}3@<qu7+!EPFl^;yV5s6`V2I~rU@+!nVBp|nV7Sh~z_5XXfnhcW1H&v128Kco
z28L7)1_oyi1_pT!28Pq@3=Ea*3=FF53=9|87#OPA7#LjG7#RMrGBA8#WnkFL%D}LP
zm4RU~D+5CnD+7ZUD+7ZJD7~>VFq~&$U^vGT!@#hKg@K`*g@M77g@M7Eg@NHEGXuj5
zW(I~+%nS_u%nS^1%nS?x%nS_wm>3xTGBGe*W@2DC&cwj5m5G62F%ttrG7|%XG7|&C
zeMSa`%Zv;R%NZddn!`A~(U4JkvhN=8$?xa#ZvVx{$i}$2;$tjtJ*d#JMk;Gz1@|X@
z28Pr83=DJl85m~sGcY9bGcf4zGcbJOV_^8m$H1_ikAb0ykAcC1kAcCRkAZ=kkAdL|
zF9X9&UIvC4ybKJ}c^McQc^Mcgc^Meeco`Uic^MeEc^McU@h~tP<6&T!#>2pn0!sJ#
zJPZuaxfvLiaWgRFa5FGyb2Bjf<6>Yq$;H6X#>K!8%*DVU%*DX)nv;QH0Ve}PGA9Fr
zJSPLgO%4Wzg&YhFGdUO-{5cpH1UMKNwy-lW%w}g`$YW<<$Yp0>@MULUP+@0ac*@4W
z(7?vPkif>kV8zD3@SBx^VJ0gBLnW&-1495S1A`VT1H)?;28L1=28Li31_pf=1_ovp
z28Qj-3=Gqm85k0o85kUx85nLdF)*YuF)$b~F);8mF)%!2WMEh|xlmDTGJl}~xH#W#
zB*%D;aeEOXqXpM=AznuD?NvOC0Zia<7Gq#wNP;GCSU4}>XJByTXJAm`XJGgb3PU~y
zhFN?J4AFcH43Bvk7<TY7FqHE$Fl6#FFevacF#O_SVED<yz;K<1fnf^|149}Q14Ak#
zEE&9c7#M_jAPIjJHv>aGHv>a4Hv>aFH$y#x7dHcgAU6ZUYc2+cMO+LFsay;Uid+l~
zqFf9NuQ(YPwsSHtY~y5LXy#;KDCcBgh~{KqaN}fP;N@gsc+SDVu$6;>p@M^f!JLDE
z;WIk}!)kU0hHiES20eBLh97JU44c>(81mT|81&f~7`WLO7`Cu7FjTNIFj%rOF#KWh
zW?<OL!oZNr!oZ-*!ocvJnSo&qGXq05w9GeSW?<lEW?;C-#K5qciGg7i69YpW69a=b
u69a=B69dC<Mh1ozjF8YvoXl7#2`;Kk?9MPwe)LXcdx0S1HlAe$i~<0>iiRuz

diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 8344427e05627676833671090b0d83c03dc1c605..1600e6db912b8abfba2f23fb2dcc1c45018cf938 100644
GIT binary patch
delta 5862
zcmX>ygXzj_Ccb=LUM>a(28JWwqS94APvnzfY}lx+#u&dOg&{>S$0XM@%9N2Ig&{>K
z$1K-8${frV&aud~jIv~8;ABV<NfB*fh_Z5LND)gBZ(&FgZ)S?Jc4tVDNRez|NRe!2
zin4KMNRdjBZed7~Zf1(IO<@dX(3IIcgRzxSOq1~zmsfsfUP*9CVo9nd<1Lnu%-qz?
zvdl%yj0%${uuW%EXJB9`(wMBvp1`dJVnX<vXRv=`WYpf=&&kIQ5!V1qZeAg<g^|&E
za;#81qu1oULK_)vH|GoQ;O2G(NxOjvkI6>zYLeC<CYZ2cU|_h#5g(tKmst`Ye|dAE
zd@WOmDgy(9Cetm(!dt8%@#XQUMT!gz3@;f$O4&l<lM;(lHHC{nX5M0rFD^+fC;}Vp
z1v1nfq=>yBzaS?yt)$2oBoqK5JSICSc`!y#?pBf%3IquRgY4ie3IVYrCT~+xVG9E>
z!zVvbl2wWWF~RNw6JSU36qV-1=jWxyXO^TEec3FpJb{rhcJc-l7Zs45McyDSE+E@k
zN-}d(i*i7Gup60DN{T!tOQ<@sK`fpeuj&*Cb`;3jw^-6j^O8Y9ke8U78XvF826Yq2
zDAwYV#G;a-3=o?+H815BOG;*1S`pY$U;^wi=AzO(<ISH`KQb}KPJW^hC+!TfyAVWx
zTy={nuizGIRccXwagpofSWR&@50F&R<SI?$`Z5r+97I%r2)MI~DnZ;T5CQVuE%xHX
zvQ(e^^dg8KK;oq!A`wI+gWM;OnwR2Ml$x4Slv$SQo1c<;izT%pvqX~>?4Mg~#hK}O
zrMb6Q(~1&vQ;Q-%TG-2^Q*+{zb5qcKROH0Kzz{XrT`P*QYVsm2b@xb+E#MHX1F=BT
zNLt__QDn!!z_5aWfuWd(fq{X8frXJni$jsao<|)llRQySlJU#rUSSPJ>&d~|hK$ye
zTeTG#Z6+_*&gZpEv1(z6l273XX3(_VEUgpEA*BGa3zYPVlt3&M5TOhrR3}d|(P508
zyx&ADSdW2$Azi-pb>I#;PKP3GkhBg114CxTcLs(cT@cqElz9LB|Np-z8pH)9<0@fi
ze?LE0=MYancZCpth0q|=R7FjfB51^Dm@0B7fz+~th{nkwrphX5ASMTh;06((LZb-e
z%OY?L3WCJSC(kxbWXzxZ*R(0H9u&ST`FW|ux0vz@AvQOF6g7b)n9Je|Z!ssv7Z!oz
zPZA^{03w1wgdd0i#c2^Z4KabZEt4;pm3e{k(=FD-qN2pgq7;yDJSZ2jfMXc!_*=Zi
znJKC9pd?+AnV$!-7aTue0+d{fK#{p@@&xnOYT)ny*|w4qVgy(bm;kA~#gd*{5@<Gg
zp@q5GZjfzzK(;XzZ2+-B+KTpqSYY*F0<4J@q$$OciGiVL|7JeR1jaycaUxs<4$&Bp
z4zONuc?J$9_Pi8`m)3y7MldbEs64SK#XB=EH8-&&Gr72^5u^zm0gxb^yx&U7X+B6X
zbG~m{Q5Hx#8$>Js5ep%%1eGke*bC#+Gs{x*kWwqikwvKt3=B4trLALakvw*bt-L5T
zFFB(K>^^LM<4jL22?wi5fcS5#^)V)8a4fZeoWldk<!&Xxsd>fuMUWyX59Fjxljqrq
z=YV3bC?CWx0TCd-7wrUTV^1tgElNyJE!qYW2idO44vvsp+-W88MX8A?nR)5O;5=JY
z4bs4rR#H?0Qd<imz+n$2z<%4xz`&5aS<$|Xl~Hx_3>UR}P_Pyq0;xm<Bsl895ebf#
z!ys8uh!!0Iu|Q5J0!I}nFpI#!3J>L5eDUyL@XT{{y~Ui8QdqPRqz_A&6~(hKFfg=F
zc67bx0m`dI(-;^S{4`m?$+f5mq-!&X*a9M!Aw?Z)Sz=CUDtg3$lx^Ld?sk*O6qL<h
z{%2rdcs^?;7}ytqOALqwJP@-tfsEY&BEWfj<>XRNbxUwWfc*`R18`2!WCDjen2m1n
zc?JfC1DmgUK4Y#2SzL4lWad>60kW?MoW^BuF(>BafOG9FKF5-hqRgbyl2q5CqWmK4
zt^sMk1|qJ52(Yuj5d#i?e~=i$f$&(YN6HUiPoPB}u6WagmJ>%oDTt*cwYUVEM{jY0
zVi#QPKW1WJD7Io?U|`{4V&q}uVB`Uln|JypGS!28?Wf6w@YgNw^wbi^<dV|FoWK-F
z?5+ga#GPK0Sx}H#6knE_18EQ-`~*sX&_KV%RESXxU0`5fkYZ+FC>CR2U|{3sV&q_C
zW94DxW8`AqJTve!qbNA}AdCY?_NmDWgQYb<xxeTbC<MV715AK&$1Prv_aJ#VD6!mT
z@{{2DaB#WC3GoWLX&@uO*%D;fEw-Y>yp;UhqPZYpEUEn#7s7b2$-6?{)PoZSQ_&=l
z1~37te_k@fxSITsauKUfK#JgY7d-@V!4}^JvA{(kD6VgD6y=vBf{OcFj3JuLkR*H#
z6!h%5iBbgxnR)5xiTNS}14Gbc`>;^Pipf*M{-)J1FA!eHP|H%on8GQ^P{WcX5-(cA
zn8FQai^YrAFl0$AkgQ=@$jHc0!;r<0!jsKZv?+zNhB1Z9hJje!lYfL~*ISh^E?}x*
zOyRF#$P&mB5@ARYfUy=b*D}>G)iA{KmPj=-)UuXHrwBGPx-i6Y)UwsEX2~qzPGJMN
zpzwbUYb|q$Y%{|GxrGch%vtjB3MJAhLSVIAwQMy^HEfa~ahV!68-@vtg`y?WDZ(it
z%}g!~2C+O41u3Eu3@D0JYZ$T=7f9AHgRDql3T6OdO|i{CBH|hAK?RE+BoaZ*2~9=_
z8<Yvajblg)1r)VKv7qJ#6DWp|8Z4kj3Z#_Uxj!Asuzx8FQuP?5o*zWWfCx~VLQ@Hn
zC_riP7E5wYW_>{ss96O`q2QW^t+*gHHN~rFDaZ&=opOu4D782vu^{ypOHO7nwCN41
zT_DvpsPwqSnVDCTnqHJxk_st8KxNG>-m=u3{N&7%N}tT!%#vHIA?2CHCAZjeGV@Xs
zi*7Mi++t76n`{}aRu6I>xK6vp0n<_h^1&^(lKlAG;`Cc=8L5dWsYSOyt>)Z<TOtLe
zNjaIt8L36_S)fi%F{q^pt=Nk0fzl^iVG2mBNE*apPtHvN8^f8JTToI7VnS*>aNf0M
z5Mp2`0_EVMW(Ed^6i}&<H2HqCx0D<s8zUPR4<jEV3nL#R52FwxAEOwf029lr%?>dF
zjIN+U2vQssfWit~S<e8mKw0h<OMXFW9@cgYH%P`EQg$7kJT=xUl97>tp@@l*fkBhK
z2*0VU$vOGOsaTEW0ZT#6J<rU*P{hi(*&^;TE90ceJSoSVK=n+~K9KuBQHEY^gHi;z
z%}|6^=!3ll@=;OxWRX;@`X?Y0K;_{r0eG<-;v5hFau%p|xy729S5R7Vi?z5QC$j_+
zt;{*8c}12WjiB<jh#TZxaIFb$q=Flyphl-A2c(Drr5;El0i^O4OL~4{&Mn5&Tb$|n
ziIexHvTK0)7-%KWEuoauw9LFz_x!}108pZE_VK*C`Aw<<vpm>*aGwTSxhgq%ewrks
z<Yd>(Wq$ggRsshkPQhLVdmfZ6irPW02RFIEO{`837n^aM#U=R#LHSAfCF+wcvkKTi
z`MXGY^7<?TGjNrPFzh}qBY28aOW^%2y~(24&Yn`B5R(Qa5Ee)V1{(u*1Wsf4O7nt|
z3^HM1U?`H^yfC|i(FIG`7s1mowAw6^2iXbDBG4Rii?g&KC9xzmEVIgLa&B&y+INs2
zK!F6)1QtNkBsSSUPmIxHvPqsdqvGU-JfqNCLWmq(mYNd}?$RS0qk_e5vEtN{cuajh
zAb%+{GB8Yb$WxpwnJ>qvIN2iK*yxrps@kH=^o$a43kv2|H8e{m_vXv61%Qp4ygpxZ
zvTVLoy&@>gnf$7>ic?Dz5C$TO9fi!il6(blg!?AuftuQyOhtdug9R*r7Az8|{y`WS
z0(Jr;14EVS<X?Hx^$3|FaC?H$O;e!=+=1@{+0zdqCV&V~{|l1C4}iF!u1L{w5DV0B
zF9P-Gia@R0B5+9nuIfSUprQvLC67P^sP+bzf<-K#uwe%g;-Mf?0z^oH2pJF|$H>3{
z8nQ?(DN+FOl|Y0lh)@R+8XyAUT`&t=!cJshVCZ6GU?{ekJhwnYHj#sigO9_8gO7uq
zgN=`kgO7uSgN28s2&4?ur=R?@Ae1p^vwz`YHUV&&1yzAX&p}RoJejFNpD}H+QH2?!
z$K=e4Y$0$7b`VtHvx2LuBE8AaDtw}aL2dze3BjFV_~^$i7SOQFE$*B|P=hr-C9xzC
z+yz3)2+%<!u+6(b2J?Uivy$_3a})DYiVP?BR_cg=JqIShJ_N-<+2pg81&rF8?W&eB
zavFkqB`cYWj3&RWmXrgx9<@PE0&4*iV11zSZ_Q+t8W%>l&6zcxY|<dF`W3l@lzM>(
zuu?E#Gx=<j1mn)l&zhz(Ie>iz_j-{9NU0Twum(jDdt!27X=YI>s1DCbO)O3=!Z2%h
zt32bK$+uf~@;(NYetboSAl)XD7q*E9I)IoUgNl4WEL)I~tU39KDaA!LAOTAd;R_-_
zC4UjPUxBSqWKT~maY-yme7l*mU5JqlWKmJ<WW|o;WUyuCAQRX?hO?FC<z(h%LrP$H
ziBx0^QeY1vj6j4ZNGWGYYEf=x9;nH26O<sZ*jofD^uJGj-r-XZ&N$q-bB!j*Tu>Yo
zd4pJfAR-7vfQ&$@Y9J8+vJW(%62-#6Pz)Mg0gXs;N%NWU@p3402=Z_hfn+u9ZgCc5
z=H(|RXXs_-7lCTsTdaAdxdoL);0OcPCg6e`+^7T9sYT%C`{v0HI%~MWLFxcXb_$c<
zcZpA~>pIJ5G}))yn9+7}L$`DME&k$E&}aatJyWcgTby15YDeATDoV~NsMG@wRe~B(
zMYbT<g2vCl*$V73Y#zGBVUwGmQks)$2Wo5<drkh`Z7c=qmoYJMsPM%x)bk}X)We`q
FC;;d4l=lDt

delta 5455
zcmcaIo9W05Ccb=LUM>a(28Q*UBGYR=OyrYc%-E={#^^pHg&{>S$0XM@%9N3TlOaVY
zMYx3_%FLZ1MI=SEg&{?>nJLQLogqamMZAR}MZB3Q%EFx?MIuGAg&{?<nJLOLg)x{x
zQ)=@D##YA7-pobJjB=Cru}x=FWnf?^Qkxvjp1`dFVnX<vkFkGaWYpZemy?ejBCZCO
z+<Z-73nQcD<ZhvOMvuv_g*Gx;Pj)O4*}PGB2{*ShNQn!GaGRVUuO?{;VuA@P1_p*(
z9P#m)d6^~g@fSDGm#<|CQD$IZ&}6#BSa^#yB)&X8wMd?Uf#D@1NGV%Ld{Sa@s-|!e
z$jn=;@x>*n1w~+^JwS%Kf)ueA<QL?mrj-<VgM|D*gxll-B@dlQkSs@fY6;lbTP!Jw
zC5g9~^L;am{6UHWLH2SM1%cRMlb<N5u!Vq_p_BQPWtCz;Ot7251lZv`MWuQ1`FW}F
znI)-3pEmm{Phez>o_tTmMFr%JB2SPOCy<>iC7HRYMOh#|*xgJiB}HzN-BcafAQn&V
zRdoskI~3%VTP$g%dC8z)$xF;ljgQx4gSrW16l-xwVo^y^8i>uDnwN5mB_%U0tqANW
zFadTMb5UuY;bsN3k4%iwlZ7<nEF3|0=Yt54t8Ovn72IO2N-fGSh6ZGAeoAW2EvCG(
zTWrOtc`1P@MQ$L)1(R238oQN%n57`13`D>kT2v0=R)7eQhi|bLCzhr9<fkKov#1y(
z84n^7Co5{nnSlcL7F%&<dR}SnE!MQ6#N5=PFpyu^%TjaVlXFwhd{X4Vz`zhOxl}8P
zv10NCEp_K`kXhi6tO2n=(nwn1Ay8z)z`(GKfq|izhk=2CgMo#SLyJR^!=6XI$Y!!*
zku2kz$-9L$7%e9^Y8x_IPTs7o$Y?eBs&+mrM=*n?_2xjGSPm&UkOiQ$SEK-9DS-$@
z5TQKzgo%zzGy?-ex-l61wto)Fnjl=H0}_^ReI2+%j?<w?6Qo{?fq@~j;yVLFkv52H
z3rf!a{{R196gk<#RDlf|(s8DW+zB9gb`ViNxyw|UF=g^PQweTx_zQwWODA76O=QfS
zY;4vPSPKd^mi)Zb;#*94g+-vmT~r5B)BuuTE{iX`#he&lSOgAzNsxp9hzI}?J|H3y
zM1a!-6NuF`na#Y+3zTPWu_hK3B~}(CfrMj0xq=0f0zodl#ao=2k{S<6q9vL6c@TTS
z;SMH1$*%|$NlPXlGk>iHb`{9Bm5dN0z>2^GNaZb-^wg3-lgZaD%*}R!Y}*C0jj3oI
zhz-(Kv>U_%s|OQcO{^eIDdtQJ3`Ki4yI3YLYJrOk;UaMGMS*mH^@7VSa4@mwr9iy2
zYVtWN@tk^)k`xeu2r!Y-f|SIPR5wtP49H1LPR&itD=C@_4$FMsw4w}<@=Oph4@As|
zI24?o*$d;-Gs{x*kP;%uokhtE3=CG21Fd6ik^FXxt-L5TFFB(K>_lwd<4jL22?wi*
zgLv_^^)aS;a6~nMoWldkxNarEsd>fuMUYY^2jrv;AW6aW)Dp+!lG4PS5a$55{G#Mk
zNHLfTQc(mVKmk&;9i)~$u`IPHF+H_t3rHN~9!+*|EZyQxD~T^kO-#wmOD_iJ&m#3o
zkPfD_lA<b*;%X29ju9{c_U>i|28P7V;r3;$jLMVGxv2T90x3EGA`k%%P7vVe1xMCF
zkSr*Wiw=QUAU70&BM=n6Mc|Nz1pH(NS4p|`AT?NmtO%4yTPK&h-tz!uzoID&3=Dpn
ztk5zHq-i6F*aRY$AVnN&Sz=CUDtfemlx^NT*X<^gDJbW@{LjF^@O;)xFt9HImkJOI
zc!~-@CW6AfXd8$H${$6`C$IKYw*=)QNP2{aH#n<kGJ!)F%tp8PECU0>zRlcT&zS2$
z78hLtnRyvRfb1&*Cvw?a%!xTUMWEDhi_fv7q$o3~v?SHFs3^Y(yK6w&uYicFAOh?x
za2$Yx+7~2-a3DOw>WdIDhAp13MiD4rp#{ZZP-0;zNi8nH=FwZ6@vutxArk{bF(~!0
z@Gvp*Fmf>RfXU78{Suk#LB96W<U;uC7B@U~!)mDIAe*?;i!uueQj6ltQga~59pNWX
zT7w4qEv7<@`sEx01A_!J14FSG0|NsaHy0xZBO5CZD<2~l^XBt`pBW>-QH(GS9N8y8
zmJ0jjmw4tD<bVr`)D&<OKrJdd0t#1f&Hxi258mPhc^Z<hgA&WFCQF6Xhl7hdPKXDw
z*a69yAj5946(#1S<mVR62APZ{3*6#D81FIpL&%$YaB^WPngG%OCO|Eym&`D(CO;&3
z;_@HD?xOo3Jz$G(f>_`(6O;mOaTMj3B!X&zTZ|!^%#ehA1{5~zxrtH*1(|v2=t=xM
z0|P_A<kGNE#<<C+!~SZN2rm#>$WY5%!<fP;$xy?bB^obQ!<fPiW{by5)G%a8E|98W
zp8O$PjCjR*5!sBolWW2ixM~=(q_bEiFcxu5o)az=kirzqP{WwQl)~T2B*{?AQp1?S
zpCSNev(_-C2&6E9*=#io3nXh;7BVt2)G#cNsbPh(WNX+!ERZQ6tSPwJBQl<`9#kaw
zK>`|7w`(#&*r4PKZo5L-51`;L0yP?5GJ!%7sW}1SJ_I!iKn;9NIY_*KlEW>Q<ebcc
zB2YU9l19MgKU;A@YHEsC(PEIkTm}YThFk1Ksl^$I1*x}Kax#md4PQ_Z52;8&+4>e|
zW?o5ZdQoCYDkN`%3V~a^WvMy&$(bdUKAE|hB}LK<3=EU|qiyOz1=%g3g3_d%%;F44
zzKRDmsc#9RiWOz1XOuuIjiNiCm}e_Y0X5@pu_x!I#OD^L-(oLJ0k_$~46qp-g(>l1
zjp&J^k%56B36vKZ<0i+%c#BCfvN3Wo$}#dVfl37~MlMD^MwYLe55@>Ex`OgOq<qf<
z1p}x&ESd&lfztCWmi&U$JgjX8Zjg*Sq`25ISuD;g@;6A=A5h`NUWDIN*5sW0;#920
z@_?nF<{n~ZU?}>}uz6?PWmd)slY3K+Ih_SrvIj(fBBKbcQUt{{xJoWUs}I560{N&Y
zb@J3yt@=kG6F^1KEdh8*7~&id0CE<nK)=PBnO9I+a*MUNASbf~+%~wyoRgYYWDe2@
zD&mS*85tNL0S9gvf*XOLhMpz|B>#fa1*8fFsl3IKo}ZX=i!t>UXL^3(WV<wW4N!9(
zE#__srKF~1=B2vlC*}m?XXceS`*>d1oSUY=EDtsx+zY@~Xh}|1NS9>fnS3mBnV&8w
z3OFE61A7_lc~FWkY6ZC-+{OX7VA?@kY{qdGm*f`&<tOErNKM|ARlo+ym_>?{4YCc)
zz_l2{u)Da7;3-ZmaVttqO)1JOOI4gaE!){s1QcSTj0_B#ERd`THU{hnoW}5#<^>}e
zq|U;?P$a%tDW`+c1(dfSjdF0<fWp2Ao`zu+h$P5PaNPkWz!eB5q=g!mS*1JqU2d1!
zCy*aNfdtb8767XU6JSkZlP~0nF<MXFn&-_ZJ()4z$gSuLx<0S~SPPhd=qpYwiAT-s
zpv-Fz@`x-W1H<Hld5V))=F2fkPd<@vtW@+B-B7Rq*gP;{HCeYnhRqqQeX@UnW_=O3
z?qhV*lrIAJAbUUt^n!>!5CQ5!6oE6$J`fkwlrB07Vu4zj;Pze-sGU{>&RF1T2~?98
z-2*9k03tv&O%bR>Df$QEGJ*nB5JU)p2w^u6DFz}W7#SEqz3AkUA}J7G21Lk#2zd|z
zc0Gas1x`^v0|P?`BdFyvsYpXSjDw4VkHdz8kAt0qjgO6kkAsDWrKo?hV^I-fz~(JQ
zi`fLgaSkf`i=KiU^Kf!{r9NZI<mr`WjBb<9Rb~r;OMv~L0+bb8kQb>=_O9}Y<^{O`
z+>HTu;Nbo3TP&cVfm_@;iJ&%CJZK=Js1jr{A|XKsGQc+P02#~!9%M<*&&^HDODWQr
z{HaPu1kGQdKu?)$R9(QL1d3vx9Ldf5tGgH-^g-?EmCQw&prB$cEY7S-y~R~fS`wd^
zU!Is*vJ%>+1!+Pn8NhwWS(7_!T^MaQpR4s`Qvij6Uy&WiV~!vK)d{zRCqJweW?a76
zyJaeq1K8*ApefP^sW$`>Mj(f<Cngt`W)`J_YQLP+#NyN<bOR?Vw97M|n(WZNvmR8x
zYVs9nfOKnvbhD=B7L-)p;!012d85c2BndLQ$Qi^k0V~VNPfRH;G6o44fCv{50V->Y
zz>Q~Y#SnXXYKco?N#bo528Lo#r<;p$b43RSBOAyOMbVSnI+K&Z4$%Xd#Q?H~tu!wu
zGcOxb`oSZyNDHLE3`Br}zQ_Tjl(QtYC^s_?RFPi?ML8B1fQMb4PWI{YfwVBWac40F
zkm2Am$O*)91reSg0$gSzA_vR@Cw|ZXg2&|dU6QsSmZsG$&VtOm{N&_}%={uy<#vlT
zuQa!yvIrb+;6l3z<V<j*2UK+xft#QkC%bmnaD&6q4wO0-Ci{0wOn%aRhS6a1#vWrv
zlgUqeoGps1L0Um$Ip9231hNq+g~1(li^C>2KczG$6_l<Rirpr+_Zo{aF>$Ey#WB?L
LB{S5+ppYv7Q&1>T

diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 48467bb..00c2447 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -32,6 +32,8 @@ import pickle
 import importlib.util
 if importlib.util.find_spec('rclpy'):
     from geometry_msgs import msg 
+    from sensor_msgs.msg import JointState
+    from rclpy.time import Time
 
 """
 general notes
@@ -934,17 +936,28 @@ class RobotManager:
                 self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
 
         if self.robot_name == "yumi":
-            qd_cmd = np.clip(qd, -1 * self.model.velocityLimit, self.model.velocityLimit)
+            qd_cmd = np.clip(qd, -0.1 * self.model.velocityLimit, 0.1 *self.model.velocityLimit)
             self.v_q = qd_cmd
-            if self.args.pinocchio_only:
-                self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
-            else:
-                qd_base = qd[:3]
-                qd_left = qd[3:10]
-                qd_right = qd[10:]
-                self.publisher_vel_base(qd_base)
-                self.publisher_vel_left(qd_left)
-                self.publisher_vel_right(qd_right)
+        #    if self.args.pinocchio_only:
+        #        self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+        #    else:
+        #        qd_base = qd[:3]
+        #        qd_left = qd[3:10]
+        #        qd_right = qd[10:]
+        #        self.publisher_vel_base(qd_base)
+        #        self.publisher_vel_left(qd_left)
+        #        self.publisher_vel_right(qd_right)
+            empty_msg = JointState()
+            for i in range(29):
+                empty_msg.velocity.append(0.0)
+            msg = empty_msg
+            msg.header.stamp = Time().to_msg()
+            for i in range(3):
+                msg.velocity[i] = qd_cmd[i] 
+            for i in range(15, 30):
+                msg.velocity[i] = qd_cmd[i - 12] 
+
+            self.publisher_joints_cmd.publish(msg)
 
             
 
@@ -1091,6 +1104,11 @@ class RobotManager:
     def set_publisher_vel_right(self, publisher_vel_right):
         self.publisher_vel_right = publisher_vel_right
         print("set vel_right_publisher into robotmanager")
+    
+    def set_publisher_joints_cmd(self, publisher_joints_cmd):
+        self.publisher_joints_cmd = publisher_joints_cmd
+        print("set publisher_joints_cmd into RobotManager")
+
 
 
 class ProcessManager:
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
index b21ae860d8993fbb7c2b56d3cb77e67b296b9dff..90610d17abec485ff327a863a03e55b8bc09ef3f 100644
GIT binary patch
delta 38
tcmX@cxQmfHpO=@5fq{Wx{ieu?+&28N`WgATsru>p#Rd5(iA58WQUJ+13)}zz

delta 50
zcmdnRc#M%dpO=@5fq{YH?t;LH+&0pg`WgATsru>p#Rd5(iADO!`8mn?dGTebd1VvB
FQUG}v5XS%j

-- 
GitLab