From bf9d36f2d9a1cc601171640f899665133aea9b95 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 17 Nov 2023 21:54:57 +0100
Subject: [PATCH] click works in pinocchio
---
.../.measuring_stick.py.swp => .setup.py.swp} | Bin 12288 -> 12288 bytes
python/README.md | 4 +
python/setup.py | 28 ++++--
python/ur_simple_control.egg-info/PKG-INFO | 7 ++
python/ur_simple_control.egg-info/SOURCES.txt | 9 ++
.../dependency_links.txt | 1 +
.../ur_simple_control.egg-info/not-zip-safe | 1 +
.../ur_simple_control.egg-info/top_level.txt | 1 +
python/ur_simple_control/__init__.py | 4 +
.../__pycache__/__init__.cpython-310.pyc | Bin 189 -> 312 bytes
python/ur_simple_control/clik/.clik.py.swp | Bin 28672 -> 28672 bytes
.../clik/__pycache__/__init__.cpython-310.pyc | Bin 0 -> 194 bytes
python/ur_simple_control/clik/clik.py | 12 +--
.../dmp/__pycache__/__init__.cpython-310.pyc | Bin 0 -> 193 bytes
.../robot_descriptions/__init__.py | 0
.../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 208 bytes
.../robot_descriptions/config/__init__.py | 0
.../config/ur5e/__init__.py | 0
.../robot_descriptions/meshes/__init__.py | 0
.../meshes/ur5e/__init__.py | 0
.../meshes/ur5e/collision/__init__.py | 0
.../meshes/ur5e/visual/__init__.py | 0
.../robot_descriptions/urdf/__init__.py | 0
.../urdf/__pycache__/__init__.cpython-310.pyc | Bin 0 -> 213 bytes
.../util/.boilerplate_wrapper.py.swp | Bin 28672 -> 32768 bytes
.../ur_simple_control/util/.get_model.py.swp | Bin 0 -> 12288 bytes
.../util/__pycache__/__init__.cpython-310.pyc | Bin 0 -> 194 bytes
.../boilerplate_wrapper.cpython-310.pyc | Bin 0 -> 5360 bytes
.../__pycache__/get_model.cpython-310.pyc | Bin 0 -> 1996 bytes
.../robotiq_gripper.cpython-310.pyc | Bin 0 -> 12248 bytes
.../util/boilerplate_wrapper.py | 88 +++++++++++-------
python/ur_simple_control/util/get_model.py | 8 +-
32 files changed, 114 insertions(+), 49 deletions(-)
rename python/{convenience_tool_box/.measuring_stick.py.swp => .setup.py.swp} (84%)
create mode 100644 python/ur_simple_control.egg-info/PKG-INFO
create mode 100644 python/ur_simple_control.egg-info/SOURCES.txt
create mode 100644 python/ur_simple_control.egg-info/dependency_links.txt
create mode 100644 python/ur_simple_control.egg-info/not-zip-safe
create mode 100644 python/ur_simple_control.egg-info/top_level.txt
create mode 100644 python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/robot_descriptions/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/robot_descriptions/config/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/config/ur5e/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/meshes/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/urdf/__init__.py
create mode 100644 python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/util/.get_model.py.swp
create mode 100644 python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
create mode 100644 python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
create mode 100644 python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc
diff --git a/python/convenience_tool_box/.measuring_stick.py.swp b/python/.setup.py.swp
similarity index 84%
rename from python/convenience_tool_box/.measuring_stick.py.swp
rename to python/.setup.py.swp
index 565f0d5acdeb8db8100b7857fee177e575e0ce70..fc1c48cd886b09bab7b4826d832c3761456cf528 100644
GIT binary patch
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|?9cIXu-hw1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7T=@+M#
zlosd}R6@)eB}YSGGz3TpfzpyRT?<|YV<STYkS=8<MFnA@P!MwzkA}c#2#kinXb6mk
zz-S1JhQMeDjE2By2#k;rC@ElMsAphcV1oKr0!lNY(NOLvH5vk=Aut*OqaiRF0;3@?
z8UmvsFd71*Aut*OqaiRF0;3@?1VbP(g@Hkon}J~>H)Q@F*8ji4&%m&SpMha6KLZ0Z
zKLf)fJ_d%_d<+c5d<+cMd<+aKd<+aPc^MdP@iH(R;$>i%$jiVG%*()F#LK|&m4|_0
z7Y_qN4G#lDI1dAZ7!L!(3vLF6x!ep4lc07j1lczP!)w&ZqaiRF0;3@?8UmvsFd71*
zAut*Oqai@u5Kw>uWlaVsOF_9HF*!RiJvBZhu_V#9S{-AJth$v#w0d$*X12bTx{iW+
zN^Sv!QCgCjqpzhNTZ>^-v29vrUP?UF;9`yPjMSo3TlM0iWOYp)gldJPqWtXCJROCk
z(h`NtG=<9iQuUl1h4RF_5`~g{h2rwelH?49oWzpUA_b5MAO#BP`3M6t^NLFnb8_N~
zQVUBni&Beiqt)|Da|<ffV^M5LO-?K=P6g{u$<I?Sf!hGpuTYSgm!F)Rk(sYhmYJwf
zkXeARLLnu!AT=)~H7_|cwOAo9H8lm~w4_vplEm!PJcZ=MqEv<aG=<{Sg2bZ4lGL0^
zB&~@lDGK=|8L34I`FW|u3W<3s3MHw<B{~Yp8L7$H3VJ2^xj71HnK`KnY57G8x%ow@
z49Z|tnF<hhf=y2>Q7FmCELJEkNlXVRKne*Rh3vfi<ZO`7ii%Tn(iHMbOLP=cGE+dF
zEJ{tx$*EK*&o9b`n4hY{pse8MAL42S4QzNgKtsADU!f!;RRQd*l8nr}bVMjtWfsI2
zC#I#^x+Ufmry>#$C=t~nl!3UI2}rLfKPkT?J|(p{xhS)sBr`v+7@VfGkmCuOSfHs8
zDHK6wDkvzx<ui+HtCf_j6qGO$suGe*pvpl>0z)0LwWUQlw(1!rB?ZM+`ugdaB{_*n
zdeF7udO0N-dc~>wxrs&D`MT+ti^tUw9#1SS$;dCVRrdv{Rd6p&N-fIFOGHwD<at|l
z=lq<U)MQXVfYJ`sI)%iX^!%dCl8oG9aD0KHA~eV}RUxq`S0O2}I5kBfKMx#1rA6^Y
zB`K*2nRz9tMQMr2ko1$BpPO5nmzkUhHW!rG5Xl;pzLFDjl1p>Ivc-BxE-OndD$dN$
zvsE|HGgQ}M0I!7C$V<#kwZ)7{bsdJZqWoM1u%eRu{G4Ki%-n+fq7sORjsi-)X8-^o
CxToC!
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|{Hc<C9vZS;Jr<!N`!FUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8F^po@R
z%2M+(Q}dEj<4f}MbK;ZoEA(?y6N^iWGV{{oi%T+-v-JupAy$o&qaiRF0vI8{%V2C|
zXaEvcR#H?D777J1NAYL~jE2By2#kinXb6mkz-S1JhQMeDjE2An34xLVMuvI@1_mam
ze|4cWBN`3mj#8r`Fd71*Aut*OqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0z)ta5>pr$
z4stRuOyq*h|HJzKFZdZ4uJSW5oZ@F-IKa=qFqNNyp_8A1A)KFq!G@oK;XfY(!y!Hf
zh8=ti3~Trp80PUYFl6&FFl6yDF!=E?FevjeF#P9bVEDqz!0?%uf#DM`1H&<128LO@
z3=Ac_3=HYK3=AQ>3=F}%3=C$x3=9Ii3=I6d3=9W&7#P}l7#Nay7#K`>7#O~BGcbJR
zW?*>9&A@P+n}J~~Hv_|BZU%;YZU%-3ZU%;TTnr4$xghSJ0P@2SjDt}pkA}c#2#kin
zXb6mkz-S1JhQNpp0R;sGg_6wNRK4Py)YJkE13d!+Q%wetXhBhCUWtZMX_2Xwl8%B_
zUV&a>QBh*0Mo~#hYJ5>@a%yH-s$P0(iDPm}X<|-@b3j0TajJ%<re0BgX<mwki6)9M
z1(|tZV^R`J67}+ZGo#I7^-78o^NMp4OEUBG;F@$4v<fow^oj~9^>PzSiZUxg@`DO0
zHITIx<(Gi<V7CcLCDZ|#d3tI2MdgV_Dc+fRskw<InaRZ(x%nxnIXVg;)jA4rzZF6q
zU8t8>P>`CJ0tyd^DOf}lY!$GFMWBWzLwQDKPO3skQE93b0|Sy}3bqRAsU`7X5slKK
zl(hJQ#FC8o#H8Z<oYIn19fjP~;*9u|%p$muCIg0$f~`V+u^vdHUSd))h}M9qL8w-+
zRZ!B?(=W<T$}fpeNi9w;$}9i{b+Nt@1E$4Tj6^6ZN(IGIS*j+IMwl3GYe4$-ON&fX
z<I6KkGUCAw$t;Y|NX$z~)dNW?F)$QorspN*=s_qA2&EV7>F((lqN4y(nUh+i$p8*!
z==x;6w4&71l%mYCRNwrRR1HlA253S}&d<wBO)g1IQG&!6NPT8rX(}`-^72a{(FGDO
z%FofuEOv(J(9pDk*a1z}3bqPCAug^#kVLMbWN2xmXJ}@jXQ*drV5EfV9;jMp$m(fK
zwG80Qlv<Hlq5*Liif2;uQryt|gUEa8nF{5ZIXMccc_|8TUn?Z$r6_=mQ`cljNlk+V
z5;*Kib9EHbiV|~EHLVyJ(u(qP71A@yQsZ+|<4ZD9<C7C}GLwoDOHxxH$xk6Ow;;c$
z1d$#YpaPI+fbo*^3o2pk{9=f)U_ZbD3#tn##60s#Qj5|OlT#HEi;+VTNi#GSU~T|u
zh6W*qW~eyK4e6-`sU;=(ddc}kNr}a&MP;c)5OWJM^YW9EGcxn_$})>f6LT`FQenos
zL-d5f#EW2tfr>teZfJ&r=}5^e%1g{m)ltaG&&<<NfMzq8w)|oktGE)*%qz_;s08^q
QuK*?scM?drATy5v09^+*ivR!s
diff --git a/python/README.md b/python/README.md
index ca8d709..0368c08 100644
--- a/python/README.md
+++ b/python/README.md
@@ -1,6 +1,10 @@
# TODO:
---------
- go to a previous version, save basic version of click where everything is in one file
+- write test for every algorithm - just run it on sample data on simulation and pinocchio.
+ using things on the real robot needs to be tested manually, but simulation in URsimulator should
+ take care of that because the only difference is the IP address in that case.
+ point is test what you can.
# installation
------------
diff --git a/python/setup.py b/python/setup.py
index cbd9fa9..9daa358 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,10 +1,24 @@
from setuptools import setup, find_packages
setup(name='ur_simple_control',
- version='0.0.1',
- # TODO add other ones and test, check .toml file for more
- install_requires=['numpy'],
- packages=find_packages(where='src'),
-
- package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
- )
+ version='0.1',
+ description='Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.',
+ author='Marko Guberina',
+ url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
+ packages=['ur_simple_control'],
+ # package_dir={"": "ur_simple_control"},
+ package_data={
+ 'ur_simple_control.robot_descriptions': ['*'],
+ },
+ zip_safe=False)
+# NOTE: if you want to switch to the toml thing,
+# here you go, knock yourself out, didn't really work for me,
+# and i don't care at this stage
+ # add other ones and test, check .toml file for more
+ # dependencies need to be taken care of separately
+ # because you don't want to install pinocchio via pip
+ #install_requires=['numpy'],
+ # broken, but if you'll want to switch later here you go
+ #packages=find_packages(where='src'),
+ #package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
+ #)
diff --git a/python/ur_simple_control.egg-info/PKG-INFO b/python/ur_simple_control.egg-info/PKG-INFO
new file mode 100644
index 0000000..b15b402
--- /dev/null
+++ b/python/ur_simple_control.egg-info/PKG-INFO
@@ -0,0 +1,7 @@
+Metadata-Version: 2.1
+Name: ur-simple-control
+Version: 0.1
+Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
+Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
+Author: Marko Guberina
+License-File: LICENSE.txt
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
new file mode 100644
index 0000000..77d6984
--- /dev/null
+++ b/python/ur_simple_control.egg-info/SOURCES.txt
@@ -0,0 +1,9 @@
+LICENSE.txt
+README.md
+setup.py
+ur_simple_control/__init__.py
+ur_simple_control.egg-info/PKG-INFO
+ur_simple_control.egg-info/SOURCES.txt
+ur_simple_control.egg-info/dependency_links.txt
+ur_simple_control.egg-info/not-zip-safe
+ur_simple_control.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/ur_simple_control.egg-info/dependency_links.txt b/python/ur_simple_control.egg-info/dependency_links.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control.egg-info/not-zip-safe b/python/ur_simple_control.egg-info/not-zip-safe
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control.egg-info/not-zip-safe
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control.egg-info/top_level.txt b/python/ur_simple_control.egg-info/top_level.txt
new file mode 100644
index 0000000..9a3fac6
--- /dev/null
+++ b/python/ur_simple_control.egg-info/top_level.txt
@@ -0,0 +1 @@
+ur_simple_control
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index e69de29..38c21a5 100644
--- a/python/ur_simple_control/__init__.py
+++ b/python/ur_simple_control/__init__.py
@@ -0,0 +1,4 @@
+from ur_simple_control import clik, dmp, robot_descriptions, util
+__all__ = [
+"clik", "dmp", "robot_descriptions", "util",
+]
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index 0564b9f600679f04e19e0738fa67cd4f9fecb063..f0e52da6259b072f7fdf395595c1bd877a6aea0b 100644
GIT binary patch
delta 206
zcmdnXxPz%apO=@5fq{X+Zbf)%Edv9?V-N=!GchnQI503U6ss^WFr+Y~Fy=7iGDb1x
zGDR`vGDk7zvP7{kGNd!4FfL+@VohNRX3%7Q3DU30a*HK7Co}sNb4qT(Euo_Pr2LZj
zl+@znqRfJl%>2CKTP&p|nYKB8nrycOON-)*Gjj`aQsa~J^Gb^HbBaLL7BMq0Fch(X
j2v!CLhFk3M@rgM(@$oAeia0=m921>OI6%@83_OegwN)>#
delta 82
zcmdnNw3pF2pO=@5fq{X+GAt|=L_Y=*$e59Vfx&@+fuWd%fq@}~A(%mv(QhR~5l9$H
POx#n##>BwDz`_6kx7rFa
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 7e2d03a88b0a8fd17bdc30aa40008b6dee547ff7..9fc488300be91c4a2b8a7a31043e23c54fcc35ba 100644
GIT binary patch
delta 296
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQfx&BAcxvKC(f9hSllT}Iu1)4N&}V%MVr{lH
zn9OIWprD|ToDU*%^79Mga})Ct(^HG|ic0e|G#Su^6>JrpH#6EZ3wqw+VPG)eh1kFc
zvWbC#;Uzx<!+d@Qh9G_h20eZT1~z^MhR1vi4CnY57@GJP7*zQf7zFqj7;f`2Ff8C@
zU`Xa=U@+M18}OKCvR~|hNlJ2?72;AEm5TC{@=Nq`^HWlDbQB<*l*E!m9fd*|ck<Nu
zd{zcl1_qJIoC*I~{n;28L^ks#va?QBOp6zQXi3jc%!yAeD#|aKd{lAoW`}eE7RHRt
If!XU>0CTHOpa1{>
delta 272
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQfkAO@cxu8%(f9hSv-ubp?oZ}4&}aPvVr{lH
zn9MhMh8@3>f`WoVa(-S(QGSk3etv;(VqRi;YLQ-1X`Y5A16Xo1qdl{r=Pw=x20vbi
zv3wxo85kH|@-r~Z=VxF@;b&lQ;Adcv;Add?%E!R)fRBM;3LgW5IUfUq5+4J@PhJLw
zeY^||jl2vDew%#*9`j6mAUD|{_5f!}Vo9Q2zHesqX4beoMuFV?l++v@1(2wYLZObr
z<f-xbth}rY42qLE6aKTturV+wZstv7XPq38#>1#E`9iA8<cKus$%hmVY<5T&V419t
LA+R|idjSgo?2SjJ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..22ff463a3a86e96cac507291831172989a59a7cc
GIT binary patch
literal 194
zcmd1j<>g{vU|@(?5uOU7AA<;F%*epN;K0DZP|U)>z>vZa%%I8Wx00a<B#a<_rRrzo
z=cekX=NA{`rz94Ec}56sPHA3>enC-UMP{*nX;FM}W^O@FYJ75jUP)1Yj($O9Nk)Di
p7K!AX%xwMm_{_Y_lK6PNg34PQHo5sJr8%i~AnS{n7#J8>7yxp~G*kcp
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 7f40d33..6f10066 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -75,7 +75,7 @@ TODO: write out other algorithms
"""
def getController(args):
if args.controller == "dampedPseudoinverse":
- return partial(dampedPseudoinverse, args.tiknonov_damp)
+ return partial(dampedPseudoinverse, args.tikhonov_damp)
if args.controller == "jacobianTranspose":
return jacobianTranspose
# TODO implement and add in the rest
@@ -107,11 +107,11 @@ def controlLoopClik(robot, controller, i):
# first check whether we're at the goal
SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
+ if np.linalg.norm(err_vector) < robot.goal_error:
print("Convergence achieved, reached destionation!")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
# compute the joint velocities.
# just plug and play different ones
qd = controller(J, err_vector)
@@ -119,7 +119,7 @@ def controlLoopClik(robot, controller, i):
# we do the printing here because controlLoopManager should be general.
# and these prints are click specific (although i'm not 100% happy with the arrangement)
if not i % 1000:
- print("pos", data.oMi[JOINT_ID])
+ print("pos", robot.data.oMi[robot.JOINT_ID])
print("linear error = ", pin.log6(SEerror).linear)
print("angular error = ", pin.log6(SEerror).angular)
print(" error = ", err_vector.transpose())
@@ -133,5 +133,5 @@ if __name__ == "__main__":
Mgoal = robot.defineGoalPoint()
controller = getController(args)
controlLoop = partial(controlLoopClik, robot, controller)
- controlLoopManager(controlLoop, args)
- controlLoopManager.run()
+ loop_manager = ControlLoopManager(controlLoop, args)
+ loop_manager.run()
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1935145d6fced2a08057f336744c1f3174952a6e
GIT binary patch
literal 193
zcmd1j<>g{vU|@(@5uOU7AA<;F%*epN;K0DZP|U)>z>vZa%%I8Wx00a<B#a<_rRZnm
z=cekX=NA{`rz94Ec}56sPHA3>enC-UMP{*nX;FM}W^O@FYJ75jUP)1Yj($O9Nk)Di
o7KxPH0{!^-%)HE!_;|g7%3B;Zx%nxjIjMFa+l!eP7#LU>06iNs^#A|>
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/robot_descriptions/__init__.py b/python/ur_simple_control/robot_descriptions/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3c8fc8dc0aef0b7824804dd26c62c026082113cd
GIT binary patch
literal 208
zcmd1j<>g{vU|@K;B0LpDKL!!Vn2~{j!GVE+p_qk%fgyz<m_d`#ZzV$!NEku<D%Q`)
z&rQ`&&o3^>Pf08S^NbMOoYK4${eq&zip*mD(xUj{%-n*U)cEB5ypp2)9Q}gIl8pR3
zED}ZeN%<x5DXGQDMVSR9nfZCe`tk9Zd6^~g@p=W7w>WHa^HWN5Qtd#FDQ03|U|?YY
E06xn)>;M1&
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/robot_descriptions/config/__init__.py b/python/ur_simple_control/robot_descriptions/config/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/config/ur5e/__init__.py b/python/ur_simple_control/robot_descriptions/config/ur5e/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/meshes/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/collision/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py b/python/ur_simple_control/robot_descriptions/meshes/ur5e/visual/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__init__.py b/python/ur_simple_control/robot_descriptions/urdf/__init__.py
new file mode 100644
index 0000000..e69de29
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
new file mode 100644
index 0000000000000000000000000000000000000000..604494c47ace559fed14c143479ef69e2f44abc8
GIT binary patch
literal 213
zcmd1j<>g{vU|_hhE<6=PKL!!Vn2~{j!GVE+p_qk%fgyz<m_d`#ZzV$!NEku<s?g8K
z&rQ`&&o3^>Pf08S^NbMOoYK4${eq&zip*mD(xUj{%-n*U)cEB5ypp2)9Q}gIl8pR3
zED}ZeN%<x5DXGQDMVSR9nfZAj-6?7M@$s2?nI-Y@dIgoYIBatBQ%ZAE?LaOnW@2Dq
HU||3Nc5pn#
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
index d0befeb34512b3259ffe76f42513039b4404548a..30b86a09891ea955ae68c488d3e981f4f26b79da 100644
GIT binary patch
delta 2671
zcmZp8z}V2hB${Lp=IN_op=ZRvz`!8Dz+ikSJk@KX=v#h%Himiz1_maOI0FMiz+^!I
zc}CUAh63W6>`*adC|w4n8KE>Yly;pQC?L;hxVcaupMR193$GP31H&{Hh&=(51r@aG
zr|>Z_bnr1SRPiw|#PTsPsPQo{JmF<vxWdc8aG95Z;XE$`!yaA+h84UF42ir94AQ&|
z3>SD97?$ubFckAJFj(?1FqrZ%Fi7(-Fnr)<VA#jaz%ZSgfuW9@fgysMfx(iSfkB9y
zfkBX)f#Dz*1H(ct1_n1S1_pjE1_nMZhI)o)oD2+|oD2*JoD2-YoD2*XIT#q0aWF6h
zb1*RIaxgIHa4;~aa4;~4a4;~ia4;~OV`pHfU}s<mV`pG6WM^Oig@rgf1H)%F28Jzc
z3=GrR7#K>~7#M8W7#MWf7#MWe7#LL87#L*O7#Kv@7#O~?GBBKGWnidfWnhS6m0@5A
zVr5{kXJudzVr5|X#KOREjD>+=4GSooHis!XFi(~@;+TBFh<)<*FyYPSs_AT#XDV9Q
zGbliTvO+;oYD#KaW?pKFLTQmgZen_7vO->IZc=JdF<ez~YEGJ7Vo`drf~^9Gnmj?f
zjybU?eX_KU%w{j0G^WW;dg7DC^|UwN*3)FHKh4Cz@Ea6|0t^hU(5Sq`&%m&bpMjy6
zpMk-VpMim&pMl{39|J=e9|MCm9|HqB9|OZ%UIvC8ybKIIybKJbybKIxybKIHybKJ?
zybKJ#c^DWz@h~vF;9+3c#>2qS#>2qi%EQ3$j+=qu88-vNC2j_Wb=(XL_1p~g46)n{
z4AR^T3{SZj7>;l;FjR6eFxYW1Ffem5Fnr`>VA#vaz%ZYafuWI;fx(cIf#CxO1H%;#
z28O*H3=Hcz7#OB<Ffg=mFfcT8FfdeeFfbHwFfgQXFfj0QFfi<3XJANRXJBw<XJD{q
zXJAldXJ9zR#=y|c#=ua_#=v05roq6##m2yJkd=X9GAjc^Fe?LtJ}U#m3l;{3`78_!
z6ImD-99Tdhv{}pW4%1|NQwdgZ08b7#?U;N#O>pvS({{ds%)I>M<c!Sx`24(_%FQ)q
zKForkI4epmDJ{xV0Bb0meBVlX@(D}f$=5A6PHwT11LGZ529uvyiB0}yWevtD*4~p>
zTPsdJVQn_qMnzyUmyO<J6B{L7kZFYqwh*HxGe(Q42Wn`-qdYTBAuqoKB8lo2tId@*
z9*p(r`H4C4sYONkMbJc{hY*3AqO1_&@8Yjel95@gkXfvdoRgWYP+X9joSBxHtfNp+
zTB49yqL7)Vkf@N9T2hi)q)?EPn4F5F9b!RFW^QJQ0$dp6`N?y&IX0iLz0EjzzP<Hi
z&U7ArbXQ)qHx}R(WMyFZ$O<Vj3?~Z)#AnI#FfeT5W?*pRW?=Zk#lX<S#lWD-#lY~H
zlYwC>Cj&zUCj)~$Cj-Mr4hDvW91IL891ILT91IMP*clkkvNJGjU}s=xU}s?PVP{|v
zVrO9Z#m2yJnT>&=iH(6Fm5qVHosEHkn+;^^W=0lq*2z3c9GlmKDKRnXZayC&$~3ty
zpL_D_C^2Z1Ge+A_R#0VO(*pTPZ?a>wIaHz|S_iCjb+qy1qI~hmpP~~%{Cdwkg_4X^
zg|z&foc!|4ymTFfw9KO75(Q&erd3ERN>!-Psnn@bD9A6)EXmBz(@{vwOHn9JP0r6t
zQ7{H8DAFm=sZ>bJOV3FyRzk!&C{5src?N~aNBo60N5!mSuAj-tz~IRR30YNWCV#}w
zz;KwKfnf<h149Ns1A{+51A{F;1A`Dh1H)@R28Nw{3=FIJ7#I@x7#Pg?7#R5Y7#Mi@
z7#KMD7#M!@GBCX6Wnfs#%fQgd%fJxI%fKMZ%fRrEhk@Y;4+BFh4+BFi4+Db~4+Fz}
zP$u8W&A?F0&A{Nm&A?#8&A_0+&A{-Ii-BPe7Xw2B7Xw2Q7sP?uTnr3cTnr2cI2jm9
zIT;xII2jm>IT;x4aWF8<;9y{A=U`y)<6vNr<6vN5hGy_t><kR$><kQ1><kR5><kP)
z*%%m3vN15sXJcTfXJcRpVykChFkoX~c*V-Vu!NO?A%m5H!JCzVfe)I|kFYQ>Y-3?y
zC}Ck>Fk)d~_{Ge?@RON=;RiDV!)0a$hMmj|3?<AA47$t=43C%?7)~%TFf3(aV5nna
zU<hPlV31{EV7SG|z|hagz)%9M7D15%!r)A`xjU(XX);r)_~ZpD;*-xR3s0V>%s2Ut
ziXw=;d7g3=<K*LMPE4selR48BoI%>aDGZe13o`Tc@={AniV|})z=FB?DXBS#M39*V
zF68x4Gt^}H^!YO2gaj%VK(zpxg3bHVFEh!3Yyx3rP%<q}1t+%jocyH39FXYd)~vJ4
zletqlCx_=LOr8=Wh*r)`KIktxxhYSaNl9n&?7W50ykL``DGPRJN(sW1AmcLAAO+Oq
z9r+&hi75;Wk3qE~8zfP}8W-oF39_1>fgz2bfkB3!f#D(_1H&pl28Mn<1_moW28J)Z
z3=EHX85maaGB8x}GBCvRGBAknGBAAOVPLont#N927#IS07#I|I7#O~BGca7^W?)#!
z&A`yk&A{Nu&5+FSn~Q;AKNkbTA}$7oR4xVvdoBhBX)Xo^UM>cP)6ix@ASVMu04D>3
zKPLl&7AFIP04D>(X=rU@$-%%N&%wa(hn<1pDmw$i3U&sD5@<8w85;w`X|~ObEH5A>
zN^4Rb?P4Ds))=urSr;sxUX)o-kXjU<mXnwcPYz%iR2`f5rC(u^Mo|rEJm^8iCM%>H
PLyA3Ajj-wjRg?h$;S6Ja
delta 1572
zcmZo@U}|{4D4Jvt=IN_op=ZRvz`!8Dz#uR;Jk@-o=v#h%R)%^81_maOI0FNN&tyRX
zdB%Xrh63W8%uq4A$%O*)j0T%03gq)o{Gc#Nfra-ZGXsMtE5s<D$$|>n^{e?980Paa
zFihlQU?|~ZV6f$5VEE0;!0?)vf#DS|1H&_328PSL3=I2u85k;f85oRs85o}PFfi=q
zVPNRuVPFX0VPNp#VPKHwVPJU0&A_mOn}J~>Hv_{2ZUzPiZUzPgZU%;<Tnr3-Tnr4o
zTnr4UTnr2xTnr5NIT;uxaWd311aLAisB$teT;gD0Si`}<u$qH`VKD~-!(<KyhGq^1
z244;ahVSeQ3|HA17&fpoFsxu_V3@|vz)-}_z+lYIz#zuX!0?5Qfnhfr1H&3N28Pva
z3=E6e7#L==F)&PKV_+y@V_@)NV_^8h%D`}wm4V?RD+9wmRtAQNtPBi=tTGG?uB;3U
z+N_XpF<@a};9_B5c*e}YaD#bsn4$yoWV;lu$pR_jliyS&PQD+;F)2)Z^A44Cw#iGh
z3v6-|E8;UtQi~ExGV}9_HLVzua}tY-6`b?)N{aGxeDd=Pd=v8$(^HEyk`Y2W3W-JO
z#hROqb<&tNZ_o>2tXE)RU@(D3iybufF7Y!k%;slc$mC~WFz07rc)`cOP{+r>kj%%x
zpwGv^Aj-$U@PL<rVGAz<!)jgzhQ+)L46}F{7$)#CFvRgPFzE3zFudhqV3@|kz|hCT
zz);1*z~INjz+k|`!0?Hif#Co*1H)o&28I}JhI$57ZU%;TTnr2kxEL5Va4|4U<6>Z_
z;bLIW;bLHT&B?%UnUjIx04D>(dQJw0b({<g%QzVrW^*zyOyp!>u;OH3c+A1T(9OZX
zkjKHmkjBBl;KRYd@Rgl`VH-OG!#s8dhD>$_22*whhBs^s4Exv^80y#<7~<F%7{u8a
z7%sDFFfbftWnd^_g@nQ}RtAPytPBidtPBiqSr{1RvoJ9DvoJ6Sv24~dyu-9PB0`UG
z@^541$;u`jlV|yeOg>^#&sCh6TbcuohRuPdKFpI(S_n^mV6k@cL`x|!K5MBnd8Ucb
zWMM0FFz&Q+pM1qicJfy%qsjI5tdk9_O($ns%PAKs*eVp|C*_yu6_uo<#uud~r)HL=
z>ZPZaI3|~rCguccXl}k}?ZLQtrtKrf$@TW;n{U}`3Gkj^Wnfss2FX|klLZ6fQ}1&#
zFx=y2V3@(pz@Wp;z;J_$fngaJ1A_+_1A{ad1H&gy28Jb^3=AQh3=HC&3=ID{7#OB<
zFfcT5Ffb%@FfbT%FfjaQXJ9zW&cLvioq?eZ8iW$;3=F@Z!MJ4e!-DgylbgasCNEPF
zn%o!0u~|Os598#M#<rUyB5pEHHjR><93ExQs5N;(l=<X4Q96@(qOBMiCVNGfYz~gu
z$Xu_)$iPqsDntYr7y_Uv?-4%(!(n~~hL!vb3`P7543Yc{4DS3442t{=48Qmo7*6mp
zFzn)EV94iVVDRE&VDRK)U~uJQU@+%nU{L2{V0g*Pz;Kk8fng#q14AS)1A{Ct0|Nsu
z1H(Zc28NkD3=CmB3=Gmd3=CgEDQ_n?149ir149Kj149Zo12`rA;9_9d!^OZbkBfn!
zm5YI)fQx}ajf;WdHYWqa7ET6+nVbv^E}RSu|2P;J-f}Q7Y~f&FXyIUB$mC#PkmF!r
zVBlb2xXaGKaDbhGVL3YkLp3`CgEu<^gCaWv10y@6s3>J)V6bEhVPKG9V_>+?%D}Lb
zm4RU@D+5D2E2KzZW@TVtVr5{s%fi5Ln1z9%j)j52oP~km4KoA7C1wVO^~?+m9n1_2
z(aa1C>dXubPnZ}OW-@_7bK=8olS>i<CZ8@--h8I4g=w=<3LDepiPZ{|m#0mgTx-p@
zIW7Gt)8sB~!Odw|8<;0c<*822$Wvm{QJUP7w~#BPL_t>}B{MBebMgmE<;n5+Ho_?-
QnhXj^ATv#2@{W8D0BTuD`Tzg`
diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/util/.get_model.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..bd148d33717e53ec67b40270ae0735a98b999d35
GIT binary patch
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|=}AB|Nn#w1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUZ{;
z$;{DDPc4bh%}+_q(JQEgSTjnFhQMeDpoc(dNt&(&FN3j>p#exrSxHerSSS?49L1v{
zFd71*Aut*OqaiRF0;3@?8UmvsFd70QBm_zd7#Zpr7#NtK{&j)UjA%5JJ4%g)z-S1J
zhQMeDjE2By2#kinXb6mkz-S1JhQMeDjE2By2n@jxNK9d1aAIO$$YX}g|HJzKFZdZ4
zuJSW5oZ@F-IKa=qFpHmoVJ1HVgBw2sgDXD+gFHV211moR0|P$;!x}yY26H|JhF`o4
z3>SGB7^d(tFeLIaFzE9#FzE3zFnr-*VA#RKz>v+uz>vkmz@W>+!0?cpfng#y1H%Mv
z28J+h1_lOh28L~13=CVj7#PC17#O~CGBB*>WMEju$-v;w$-wZ3gMnca2Lr=K4h9B)
z4hDvw><kR+*%=ttu`@6fu`@6vurn~+XJcSE$;QC2ij9GxkBx!Bh>d~a0xJVU1}g)D
z2P*@E9V-I^Gb;nb6BY)B6D$l2$5|K{X0tFb<gqX?II}P?NU$(4yk%x!ILOSvu#B03
zVJR~MLq0P!Y=&UmjyjnRA)uh3puhkkic(8Ti}Dn}Yf*F*lJj$NGK(|w^B{{;bQH=m
zi%S!UR+t0RmXcVK$N<&`Vky`v6lCV<xg?e(YJe@$1WSRLdRh6Ic_jfkiOH$Csd**E
z(Pps<whHA%nZ+gX#_`3e#t>&>S8Iw;YXnk@$5<1DQbPhtjX_G&@{3Xvi*gAlH3BIu
zEhtDWiYKht5Tv*`Bfm5!CAA1_Fxb7ISO5nzH1rf~75wt^QsHuFae^)l4jE_&CzWRA
zr1*lC6}c7V=Y|%gq-m5ErKH6dB$i~vCngo==aiPDLIOWEr#RILZih0SXac!U8PgG<
z7zNwso|>Nvw+`$Y9R*BVb-+mgq&Bsrs4}FoAXU%V-^a%@*wfz+;v%p;XaPaFC1IYy
zp^iRCHb7m7<{@ZGM??YKW^iy8!##-UGlbb_jwdk|OLJ1u<F+U<vp7}3FTcbSw6qMA
zKT=a%i;D7#G}HqUOEQWTGV>HlGE%|8t&m@&P@JEeT9T2Om#zScUWL5W)D(r1e1)V`
zg|y5H2)nd6RiPv!vsfW5v#7X4Pa!zJsHjp;!8tz{q^&qVKTl5`;-kzoNbb+ctb(Q#
zWrYxb7k`jh3ZN8KtWcJjr~p~#rcjWWoSm4SnU}7kke*rsv7#hjp*+7R8&VJ;XK6@z
z5bSEK0Se}#f=a!jg36G5-^7xl%nA*d%A$fwO&u7gq$n}37~(LPP*Fjpf~`VcfnH)!
zQDUV=w4t7<xsj2fsi~ofsfnq%fw7K)v7Vucp{b>jnW?d<g{8SAvXHT{nT5HTv0*H#
zsbGsx%(c`rFf%eRGc-0Zurx3*Gc!rmH89grFwirwv@|g{w=g$0u{1L>Ff=y>3mO<0
zniv{_#7&Hij4jO!jbb&Cg8*DoK{G7=ATWwY3Id~eu*;Ag3zkQ5s-d2#nT4r^v8l0v
zg^{s^Dab`S3cAL6riR7_CYGiqmKNqF<|gK;x(4Pb{)U>4VwkBO$X8}&mKNq_=EkO$
zAYG<93c3)FTUZ(zSXvmGTbdb|7(zX6WMp7yVrgP#U}j-zX=aS(ad5dyl*bK`J#L8U
zaj-m!TMhIK3@pq|O-)Qp&5TS;Ei6opAyH&tU}RxtU}|J+Vr*z&Xl`U^qN8B0X9P+V
zhQ_AGmgXj=rpBNMMF|e5ohVl58t55Y8W|az8e3SJn3$WJ85%<Z!@$tc+|bm(%*fcx
z(#XKT0vsBK#>VEBmgdGLW|rnghUTWQG=NmE5C{sGN^ly0agfpgOb9(2fYQ8yg{g&^
zsgZ%1xrw<EG#eP0nHpOdTUc5c85=@{Of3v8Obrb!jg5>fq5g-N3bqK{|0YHzmZrvL
zh6V;Eh8E_gMmh=xItm6buc6d~1U!dO3-%s@3$_=M$Pt3*UIgWILo-uLOAB*zLr`3R
z9b%|wXliO~Vq{=xU}0=zYHEQLtq8Ng7NB?xly(e2ncL9R(A2=vz}&(V6op7WL$0(5
zcnq!->@PSEY%9cDa4~dW>4E}NN5NFj*wED6(9qD>!py|n%*ZlT*T4cAJ8->V^H6;O
z4g&){Lo*{&Lt_&YBU4i&V`BpoSnTAc7H7n#WER2O0SdMX`Nevmf?O{#sTf3Sz|=q-
zstn^2uLY(<!B!zHGbgoJLmgvbv|dquQhrH%N@{U(QD#9&W`15Vw8DYgr(mm~q^GBk
zsZd`D++xA3gR%Gpp{OV|C$S{64BGI)6erEGdLRSTHNovxaG<G|7MZ5TmuHq_#DiU&
zSs0&@n3s|YRS0&mGDs7o83uC=Zcl>L>f<vM5(*3{scE3P7SiU{fHXcbt5P+s7>KH<
z85oomk`wdPOF)Jonh`~*i7B9_0LVii(c)Bvw9>p}a72L`6UB*TsS3H33OSi+sX7Xo
zDcK6;8HpvJ)<=0pB?CiRQGTvMW^O@#QAti_l3r12aeir0a%wSD43aDuAPN-ni(#zd
jN;osGG`FBqA+cB?uK*@nkeQdCoSc!F4-ziO%wqrm^^suS
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..41c97b7686410f2feed21cab462d3e8c926d6c9c
GIT binary patch
literal 194
zcmd1j<>g{vU|>jE5uOU7AA<;F%*epN;K0DZP|U)>z>vZa%%I8Wx00a<B#a<_rRrzo
z=cekX=NA{`rz94Ec}56sPHA3>enC-UMP{*nX;FM}W^O@FYJ75jUP)1Yj($O9Nk)Di
p7Kzf5%pCpr_{_Y_lK6PNg34PQHo5sJr8%i~AnS{n7#J8>7yy0gG<g63
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8b3e26aeb2ede980f7ca8364b51c2f398c05e7c0
GIT binary patch
literal 5360
zcmd1j<>g{vU|=x56rSp3#K7<v#6iZ)3=9ko3=9m#D;O9UQW#Pga~Pr+!8B78Gni(H
zVo700Vaj36Ws72C1gT-pVbA4=;sCQ*ayWCjqPW0p)*S9!o+utLn=OYomoJJB%x28t
zkKzZLB@iW$!jQtABbX}`B?Oj3RVN%J%*o)+kiwC|*}{;**~}Cr0_Jn2aJMj|a5pnW
ziKZ|HGidU>1o^>Flkt|Idul;yNlAWKW^rj^PG(hVktX9Uk)RM4SLgh^lA`<^&%Bb<
zqO`>1RG4^BYI163St_zPKS;f&Ke7;CP<~Q=NoJvYQD#8_*ig>&)ROqz{FKz3WRT^^
z7#@f^3=9mZ-~fzbN?}Z4YGH_C=441^NnuW5X<=w)jAG?vNMTK3Yhj3DOJNUY(B!x!
z1a+rRetv;(VqRi;YEd%CJec_)HVXp-gEJ_StQZ&=YM8PZY8c`fY8Y!6;u&k0Y8c{~
zQb26x6s8)6EM{qtEK3RN0@f1NEVdekEcS&=pr8(B&}8;|`Gt{z;Uyyj1ITK=+{B9b
z%#zfi#FEVXyy9Ej$&ereIpr2hVo`eWE$-5Sl*E$M_@cy;)LTp`CAZj$Gt=`Db8fK(
zd%Ao2h1}vUF3B(ONX$#gNiEW3zQt0Unv+(<!@$5$#LK|Iu#%xjgn@zKSAl*;er~FM
zdVX<1eoA5ym}i9G=9K28=ob_vR%90ImlnkrXXX~<q{c&CqhC;2l98W>MWVDMGe<uu
zKQkw_s2~UA%JQN_P-N*9RNmr<kI&4@EQybo1%(^PK^%;1AjrhSD8gDLjT!2CFdfNE
zu&`%fU;qU@8!Y%O7#J8z7*ZIU85tQ$7*m*<8C@7+`D&R;m=-XnFfU}RWiDY^z*@tc
z!ji(ekeQL8kfVgHnE@L3Y<^X`#i@BInR)37h6V--#RaLUDOtq|X{9+i3i)XYRjEb!
z#R`deDGI3-nI)Nd=`T|l7#LoHEKi>`69jhJr~g^`@se*+g1sjDE!Kjf%)F9YtVM}=
z>8ZDv@(OOTf{nPvQ&f@y54~G#V0*l7v4Bj~WPwCrY3?o7w4%h^)LV?1x7f>4bK;Y8
zQ@~LLAw)qDD8#_PU=0dAPzl1p#KXwND8R_YSS5!g?2uAHGAMPyLJ;J7R#@<@0ta6W
zLlz@A;B;#lOPCfgEo7)=s$p2bT*C+oz#8Tv5k`hQd8iCa4NDD^1Vb%r4MP@74Qn<-
zkyZ(73S$kJFTv2v#K=&{TEesdl+IadSS1)1GJ-@vc_WK4g-L=To1w_Dgf)e^hB1Yu
znVFFxkFA6?g|&n+g$=|5<&G?l;@}#F1)M1iDGacjlEM(opvmr6C6t?4T%4Mskdm61
zl9QR2s*qG!1PYBRuHw?<<kaF~h0MGvZbTMRtP*!~^z^Y(NXbl5$jdKL04qyT$jrON
zg;1r*RU`!pU~UEmhFdHpnYpQu9K#P1lmrQ~7U!g<7J#$RExv-xy!_<kjLiJ_{JfmX
zB2ctwvK0w~6e~d#mn0UI+~Q0sN=?jm%SlYX#hjX#a*HJ;Gc64gouE8e1d7vJ%tfVn
z-k>-I>EvVNVB}(CVPs+C0J8)bnHYtb_!#*Zd6+mD1sGZWSBYVX>!Q*;KTUQ}4$Di-
zO^uJg#T5@K7fW+MY@Yb|!qUVXh)j_f$bCG;C5a`O$+@W|8Tlz-=NEy>NhGJi-B-lH
zz`!5_avvW90|N&G2P2ah69*R~6FUzFSPrA;1?5$6{bhu;=!F!#Q7rIc7gPYVr*MD^
zV9sC$O|Dx!;F2Cz^n%QTC0-C4o_K>886ZV2Lk(jVsJLaUVax&-zBP<l%ry-0EG4Wp
zj9F~Wj4>=V4DswW4DlQ_4Dp;b4Dnnb@gn6CZcxe5#0W~tMY^DDP^??R(+n!Enc-$J
zWHA=ml<+R#10~IcAba@p3_-HRaU}wv5~qeCUa&+6$`URSfwDwFt|*ECRR9abQkYVh
zK}i|R6JN-{$dJchB9Ow;3^EU-8^QwVhOk6aSZf&KCBUR)3R?|Byi^K%4MUa`xH6Hh
zVa$?A;mBqvY6G)4vssEeycn85As|!35HAA?k>Y7J3|XQJWNR3+<UpCQhA~UNh9O=)
z1r!PjDO@!S@ro(jH4O1eHH=xxH4O2};7rEjSCt=-SdszCNeU$ysS2e<DQOD%MGD3F
zpacUdEK_reQx)=3Q&SX5@)fXwq*R5p%nFEnX>qDTNk(R|LRw}~afzNnaDGuyrJjOw
zelEzU;{5zPy((TqOCvo)GYdUKJwpSd%!=;}3{{+lM&^13dIoxiMG~N##R(!nSv}nt
z41U``hb4=22;+f0l<~n{Qw5w=i_}5MovS!Aw=@S_rQTvshcq8<@jw~zX*r4MMeHDT
zoMo^k%Pm%Lv*H$ia(+%uW^rbI9w<Mh=G@|eD1q`=QW8rNZwdS5mw1Be$K2GslGGH}
zqN4nwB2Y1Vi@6{(?-qMrYH3MPV$Lnb!Xi-9p$Js}6@h9oXuVJbY5o*}680^2a8;3+
ze~UdiKQAvex#Si{Vsc4l8K}1B$cxX)&&(?+zQy6?@97s3@99zm%9TZ+V(J!8Vsdh7
z4z%fTi?2AfBp6hm1?Oa@q!!)c0arTl#b90$xSV4LH8u)UQf{$97`G&#^?OcgQ9M+6
za$-(qUivMr^!&t}_*9V3G?^d;9jGyZRA7M0GH~Hi0xBy!7#JAXz-1T*BL|Dne>Nr#
zMjkE>Mh+%6Mgc}PMlLX91D9)DOduW`3mXd?BM+klGY2zBoP&{#g^h`giHEsL6uCG+
zv<Z+aHBh+#3Q=&AtP0d5W5{ACwgTm7aCHXC;mlbqMQWhdz(P>|W~*Ub0Lo8nDU8w#
zDNG{ZY6_cJ9v4Uy)RtgmK(I<U7I1>AM@CS^1}aa28EP1^xPlopS^Qr9XJBA}wh(68
z7lFz_O>S^fEV2isGp_X16366{(!`uVNGde}iHd+)v&p4JMX7lu@df$CnV=@%EpAvc
zEyypv#Rh6j<)su!gVeJo78NB{7TJMXj?g#(vC-n>7E5|+N#H?Hq=0e}DCj{|j0lSu
zBNsClBNsCpBMURz|0+S0KnCeSX>@@SE+|QZ12_W|z~C-PE0ZLstpEyM1`&o@rV_>l
zOf^i9R<8&sK$sUYgW3{VEXArd3|XuT*g&o0EKtC6fSO~V0;hy?0apz}7IzIp77r*e
zvp7T;7J`az9+0divmZG7zx@CI|9?&1A}>%W0QNsia!zJJktc``%H@!3B?scVf(THa
zD6#<s6lZ2$NoqQ%y<B7tk^zO1CNnq{-(o6Exy4qPf~B!_iw)E?3{1HX3OZ10m4}&&
zNsO6|QG!X1xk?x%xF9M~npqqS3=E(`1sq`Dnu=io!$O8yMsN@@WHE#4oDxugFfL>S
z1qFx?2^esYGGuXPaTPCs)KH9|pyVlzEa3*#EsP8$+$qf97B6=SOFAg<S+dwuSS1;<
zco*=cuq|X<#8|?Z#lJuRVkTHda3Ld1E{LIz0c-|)IwRNx3|T@8gcmYks1R8oTEhft
z0&+5xh^25gGuE<HNu_YrvevLH5KrM=$XLr(B2gk)!`2LGK62Hvmq^vH)v!x4G&6!|
z8-~KdaE3eqMg|ZpkzOFPkfD~fggb=?;`$m^5FN=-!<xdC%~UiK>>9R(j5W;hym^d;
z@g>|Tyfw_ACS@K=iEIjAGh-8D4Jc{I)i4J$X!842=||?5DkLZ7DS#Scpay($W?Ch<
z)+#AV%qxZz`U*vv=@}&odHLmfRl%NlVCl5{oSgjf%)E3RNWG+BtdLilo0M8qtdLlg
zs!*X*sZ*r@%Y{1NPC#*La(-Tlf-zV@kxqe5r9xs}dQNI_Rggz&QL1{eLU~4N5va0D
zRVV`WRTVOe6`&b9r&3SBHLtj|C{>{{zf>VRFTY%&JR`9L?3Sd&;#382;~%60QcYIr
zB8jHvrRb)nrKKj9<QIV)1yY@#s{n6?T5&=02&f3G;tUNkO;reS4zLQT@(jpH1sM*i
zn2Nx5l_VCWr<N$7n+Ps!6fz-UQ=wO(Q>j;}Q>9m>kd|MRn^<C{aEmEF`xm25m2pT$
zszPR7L1_udt)K=qR8wk2L27bIY6?_`o<c}br9xtQVrHIRl{~oq&n#8|RU;4&fI=Z9
zH7zqQHN}dn>NqVM9F94~`S8|CK~a8sQDQF4tC0Gz7+V_#9OsbgKC=qcQ~|Yxz#afc
z71oA~rhHKlsE7fVJA!HXMdgV_Dc+fRskxx`b}^*<0Sy^|i}0dAQ1Qf^@0(fV0@4Mp
zQq4dtNe}@l)oyVkrQ=(yMfs(9DYux53My}L=O&gEWmbgb2Z8$5Mfso>eSRLKf)RjM
zFd@zX0r|zLw^)+%LCR86QwzW>R&W&FVl6Jn$t)=X^?hzJ=cMM{Vol4*Pb|5`TwGav
zizBrnIX*KlEx!oV=e))03-Y`s2e{w_S39>@eM|E5bBc06-e3XA-eOF>#R+1>=VTU_
z6oI-l==BjlB-Y*Y6LSJUjhVlo^3@I0w_ucD0`-4DqXMAb4;!NpBh!B=MvnhVOk6BX
zEF6qXj4a?DuMi^(xJ3b~$T%2z7?~KEKJ)N#FtM?+FjmQ+l+0+B`DwBj>4L&m4@BsL
z2vCOz5@``2ZXt*O_kIurxNT#|z`#%da;pHytqdG2Os*UpOkx}yOoBYTMIc#Cv0I$*
zED5Q4So2DA3o0RztqKYyVYmW4Skg)@0<~Ts)fcFm1xF#ceOLtQMnRf|Y8ZnsdZ4iu
zJ#aabSr`v(C4ifqMW9iNB9Jq0Dd5lsYHEO6xJ95k>J~?0QF=jQQE@7`M+f!{QnY~s
v0u%<fIBatBQ%ZAE?Lcj>Vo>aYA_^1*JWM={APC}v*c^;ZOnh8S&O(9!!E|mE
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..184950c3ec3f73f88e80199f281657e18a3678c9
GIT binary patch
literal 1996
zcmd1j<>g{vU|=}AB|LQ!4+Fzv5C<7EF)%PVFfcF_TQD#%q%fo~<}gGtf@!8GW-!eX
z#R8^TqgYcIQkZhsa@nKU85vTTQ&?IUqBv3*gBdhgUxLi^(`3BGnwFW9TAa)X;zO|v
z0|Nsq0|SFI$eMkO3=Aa<DU8jG3m8+F7Bbc{mM~>8FJP%*T*z3<RKk$L+{{?Z3>9Nu
z$XLr#!n%Mhogsx~5l96ql*bC@v6Zkd;3#3w;#|O0!?uvIhIJuREjv^N8(0Mgl*bO{
zaU!YbT*z3%v5={j3#x(xtb!ZL;{@|~kks=mWUS#{$W+S<Rlx;T!3X7WgL(W&>iHKk
z*6=N4suh5$-~p=;gz|X7JRv0YLJJvd1Q#;Z3PV-!fmMh=dHi6WD3W^7g^V>K3z=%g
zY8Z;vmT;#CG&43amas42sR4zmR0>0_L<&Q#WDR?bR2FZFpbbL}7nmhv!%)KuW(nIc
z)ChoCA~p;)!eExD4MU9>m?dVzP{O`|uSRMiW36<JRE<Q9WQ}whOE80`xL=iKX;FM}
zW^O@FYJ75jUP)1Yj$ToIQhrH%N@{U(QD#9&W`16=UTINET9r&`k!fmtd1gsQJXlF)
zVSGknUP>xNR)rXYG?|M)=?P4v-;*etV0-q6J&GVhgTS=gojG=z%;^VKr|xdqVQddp
z4JI_1)0Zx-`q-lS6C#+tQAI{TBWW{8b>r-q1qDX8_k$IJ2~Fnoz!&xnRSKLA>A`Km
zkDq}Ud@E8`eO2Ui&}2@3d$^%2N$TeQ^sliAcFPTe_opu?k86umIB2iQoPP4~7nX!~
zjrQrUr>2x@?l-be7oKuBn$zVT$gca=wriZqdF|8v-u?8DtJt|e{qo~|<$rJKfOHzY
ziQ|4P0dpY(gG5LC<mvlB{&~FFLWA+KUHZ?4@S}<4JMGiWPM_PN(x?Q|WqWpV$!YbU
z_URJ3E+M}aIUUk3htCKEF{+{i5=%0Q6*BV_N-|OvKvAKPU!+i+pPO2ek(rmSkeXAR
zs*snOnxasWuaK0gkd|2iVV4%CDwJeo7AvG>78RH1DFo*i6;<jfIOperv=!&)=jpu!
zMaN4}(eaX*fq~&A3y1(^@t1523=Eq5MIfnL99j99c?F3j8Mm17i*K=jS?q~P#b5?g
zUcoKa#G<0a%3I6@nR&OEgI$epF&7n7-r_DQs0_*XO)M$OthgoYmtW!uN@uyLc_pbS
zu0=)pMYn{KN;7j(eDhOMbKHvZb3=<#(r)pCEGr4fNlZ=!DJ{Om;*waBs3~)cvn;c?
zG%+W$D)p8SC=}vB*2fp6<|LM6mZ69xCKc!Bl$NC4;>b-c&WKOREV?BKW5vTI`HD00
zOLJ0Ei{eX)67!00@xb{Y$0K;fsm8bXOA888i{cZDa-oWO5dt99NL-L|-n9IpRD@z~
z7#E}z&H*bdFUl+~i8q9r$_?X!6~Z`33XM<|8pR_iG>S)3XpE}R7+Ik)SRq?(eoARh
z>Mj1{{G6Q3;>`TK_*_s#+~R?RGnB^)5oAe8EJ<9+P-M)&!0;<gKO;XkRX;tyxFA0z
zu?WmFLU40R^HTH+iV`a_i}f)|1^t4`l8pR3EE1(9nK}CDsU;AL^$IF)al(0^s;}6M
zfq{XKnTL^siHC`giH}i)5kJYo%)!XT$->CO$i>9Ph=4pyMW&$C&3TKnATuvNIXNRU
zzlfiKf#DWwUTJPYWf3ok&0JhrTm-WBmT+cnL4Hw5PG*u`QEG91X;E@&F*sv@jR6M<
f5@7<;%wYowT{}?zD&}EeVBlcjU<5%PMjloG<`a9S
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/robotiq_gripper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9310cab2c4819c34952aa62d53df3b15536a6460
GIT binary patch
literal 12248
zcmd1j<>g{vU|=|K+&gu*F$2S65C<7EGcYhXFfcF_`!O&uq%cG=q%fv1<uFDuf@!8G
zCNRw$#hk*J!kojB%NoVX2vWn6!<Ne)#h%L%#gWSy#mUIv&XB^I!q&o&!q&_b#g)Po
z%%I6$W$BxrQks*hP?E2ZoS#=xl%Jyzl%JGel3A!;tdL%mSx}H#RIH$@P?B0)lA5AW
zo>`Ki;Nj@!qU-t+<VZhF##=0|d8N6U%(qxW^D^`EZn1`x7UZPf;`T2}Ni9lEamh?B
zNd_rJ#vmVq{j`OVfgzP4iZO)&<c}!k6s9Pa6y_+_6qYEq6xJyA6t*ai6!s|26pkpa
z6wWB_6s{<q6z(YA6rL!)6y7L)cZL+c6#f>56#i6!X67hCcZL*!6u}mT6hSawC`Bkm
zxP>7~IK?YPG*u);ELAi`JXI`3B1JNdBSk7jx`j1LJjFXjCPlV|B}yViE>$vBBvmR^
zG*v8(Ns<AAQ{+<=T3Di_QxsE0Qk0NnSyGf!R9aY~WI!@$Oev}<YAq~LvZ-<@>Zu}W
zOeq>Enk_8Nj8XC_S}EEs3{eUxJ}J7XifK$KdMWxXtWioS1}TOu3{lD{5~-qTOesbw
z#w{#SDo~zDifIc=lq!^GmSWz*5~T*^S)^FDutce+SfyCEFhpsj*reFDFhprWW$jYz
zTUesBQhZVzQWe2|Wl3>NacW_W(oS(s6-{wT6-{wX6-|*y)k#$Z*(}MB#*yNd;@-j%
zrHi3LG*t*34jd^SDV{AXQF<wo!3>(dxA>sx#2u1Isw%k@6ciMk^K)}c^D>hYOHzv=
zsih<%6_$_`QZkEDlS^_cbri}n6BUZ{le1GxAgYQ>iZb)k6_WFFa})DYiggrnQp-|{
z64OC~P`SLs+|*))wEQB4vc#gy#H5_mVm+{(x0oHBLvAs<hxp%Ob_@x+#q8)3bc@-|
zKj;>7aDeMA<^ccTTg<^Bj<=Wtf?RJg`#X8vVs`ThsbUQcanrTHNSutY3@5{YFL4Gl
zXtGtwpay~-G%$io5=%;ps}i7Gg<>#Yp(wQ=zX+6#k}4IDf<;duBqLQJGp{5yJr(4+
zoYK@{g^a{9aIVZvEJ@B#D9=bNLDr&BoSK(XtXBkb>q{^JDkl6i*>7>g$AbbYKK>S0
ze0(lAbH~Tu;)#zhEKSUT%CN`Br{pKc$KPTNat(G3xy9w^9O4<~7~<*Yev1vvc3sI(
zB+bA8A%5lPXXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y$d~s%OK~8Esv^dc(
zs4U6I&%+{7T9TQgUj!*^;-N*2UP0w8UTCO60#O!J^nuC?4h9xR5aeJg0*T>{HU+%V
z1}am)C7(1})cGf6r6!j^EUk)%@Ms%xMM4Y=3@@2L!7R+cz~HCJQ6vK5h=K?)5FyUM
zz;KJr*FOvtGa|tu{s955F7f`MA+AC3{!U)5&LOwpqMm+!2vK&&ka!o@;1EP4LRg?A
zeTxU;Ifz$PK;g^7z`y_tUlv9V#v)|~28Lu%x`LJ3AU4RQ&LHI$pqhxWh9RDzgmD4W
zLWUZKc;*zQ8ish56y_R+cvdi*4a{Z-vpK+Q&J@OA22GYK9_ReL;*z4$<dXa%JwHeq
z{?EX`pviTM4N@lD;w;H1N=;13%uBz;;*+18eTxTLGQ{VAc)ansnR)RA`Nf$fpelzq
zJ~y!f!Q}$W78j(ZrrhEJOTpQhdGTraMaij9aWH!&<1Lor)SNU(2!T^QIG8x%<1_Oz
zOXB0TL4FosU|?Wl;$mcjLoUWDakMl9)A1Y{Y!0_LGV+T{K-umV3n=g3;)8fFz9cg@
zHNUjv7F$tjNoi4@Uos0+3y1=-K^Pp^MhpxLB@7D~N*J;jOBk}47J>qrp@t!gVF7au
zV+~Up(?Ui@2%n{f8N>z$GgH-m=ls09)Z~(4Q01NoFN0wPNoHAUo<d?uN>OStv>XMM
z-&O^QMTxlzFjreCctG8+kYA*bSpX?;kraYFWu*|1UsQr2h3Zo)g%F6B!6kSSC>p_K
zJERWCFGwv)1jQwiQ~Wepi&Q}wfZfq8-qX)D<Q8|Zzq5CIa7d7=qwg*DWQg}}aTTYQ
zK!a10rN{u3Z;U~N35YNS5#Trl6QGDKvSeUjFayOY8v_G_5Ssv_0ArOHT1-L>$DIfG
z7#JXd&bW{P)ZPka&}6Doch1Q#PAvwP)KC*L^Yb7z9CCs0r^y8I8f!90Ly<nFD{pb7
zWEMls05_FDxsQ*jN)pYX2vxT@$`Xs>Q$X!4kXlee0%4FRIT#oiz=^31oO5fLY8V$V
zEM#zDh!u!os%5TasbNZCs$rhZkit~LSi{oHIGbTEi#bCrQz2UoQwlSPRl|_Q)Gq-}
zXv_;(YM2(V)-YtTEo1~GxEh8m<^}92EDITHS;HApSXmfY7@C<G8S*#`8Hz2-85tQs
zFp>eJdjUrc>q1aDz?K-m$iNT`s>i_Ll~_<vlwVMknOKqvYX%^<5=!zFic=wVU|C{L
zX{thgnnE6;C7`2_n3tkZo|stzX(E7V^~B_Cb%mnT;)49V;#7?E2@5SN1yCC`Ge0k}
zs1jr<ymbM}Hy}GTU?Sj}RYw7Azb2A-kUVUq5K;tkSDr#~X>xLEadBE{jzUpta%ur6
zVS`LgOwQI(NK4EqPF2WEQ^?C#NKDRFC{HW~tI8}(P0>-v%u4|^o<Laxlwd&C=O$Jv
z<mH!un>wJJ2W}Z8rKaX7q^6~%CYNNEA^GhVbFgd3FGhu1j9f1nSU{Bu=PlOElGNPd
zTg=5JMMWB*lm^LFpe96OPR=d1)V$>Ul+;@su71w`E})u*r6@JI>=t``W^sICa<(QL
zBvaXf@*;C`ZVEV|fg51ASixa&izOwoBoV#*<cu#)Er~BnEGl*ZWj0V{$HFeaD8<OZ
z$iXDS$j2!2kBhlV0xdnmH5GY*oazm#h(K){P*MhA5Sx_&nw&w^WeuooWQ3H2>5R>c
z;Kaq47)NR1N`xma^3oPKfUFe4plwxXVnRwakdhW0fFNa{G!0GUpf)Zj_F?G`Nz))o
zYMM+%zMv=q#fBy`B#pr13_US$z~VI&lomkclL$Lz^$ATd$sl=9WP>m}xNQQB@OjWu
zw_gra=GHKRO02oewM;b(S&R#qYM2%<*Dz$Uz)K~@1*|E|3mI#f!x>UoP)a2glv0U#
z0UJc$0``RrF-)~AwX8KPHH_KJMV=)bDXid<k*x-<YBoa(JJ@cHx$Mo1O^hX+HLT5y
zwQMzP!3>(5iNXYvUP-<}QEEw1W-6$SpOFeFQNSexJkcqn73JrG3r1+uSVsX~X_n@d
zWafb6;AH}=kW<J+6ege=8<dJ5^{@h{iY!*hFU72=Vad!&!7nivT(E+)!9+m`7v@+j
z2{8;-7^CP(EQa-G5L&8O++9Nyia?cFkt3+RTO|^jms(Mfnhfd^!F^E0q@YnHtZAi?
zlAl@(D)<y2T_E*ejOv<Phytt#RH_uYf>NLxh;RoLPi!fv;IfRhxF9F9<Q5m$b*@E4
z`9-&wGxJI`*^!c|CrFnMNEf)`1UJ$+!Pzk$R8oRU9?Vpn4k^;&K&cp1`f#xGFoMgn
zzas1$j4F&Aj4W81g-~5ZAlq;^DL}P!4MPfJHfU&u$*&kx)FmcogB_vC2-XZ{LkMtF
z#slOB1_p-Dpd7=&gwc$F7U39n!dsW11_7u91!bTV#)VAaDjL+VVS@8OS&DHXBgkDv
zay6h<Cpe3N_{BEl>n)NiVO_wM0&cygFoQY<iK<Kt4Dci!lnSc+K&c9v0T3w()Ll-?
z&&kOz2i23H2nVJ6)QZ&PQczOL%md{Oh2;DqNTLFDBNU30i!uvJ6w*rbl0nrm+?tfs
zG=-wV_#%*P8c>_!Az8*&$xunt3a%1V_ZAhx$}tTkN9PbF9R&j&Sj!r!OB1(-kRa?D
zP;``MWagx5<mH!uoCQrdFv|=vEK{(xRWJbMbaV~DA&yEq3OF>N`HC!`VQ9kTJCH(T
zTwIx#tbrOzsd*_}D1lI%lbTwfVW4M<PzO>2GX~seOb68t3=9wK{WRH&K!c-2psJ#X
z6O<*wKm@3Xev1WE?%iSq8;G@?%?644GEj~N>EILOV^m<|W8`C0_{YawB^m@7p@0rb
z=s{G6+~P|tEy<5hPRz+nDoQL#MX9ntnGv4DE`X~nP;<LDu7+U&xVuopumDtLF++2e
z1eDJL;TP*7at2fehsq+5PIgf9JB1mPcftBtAWd?(4b0h0MV?S|KsK;uGZn{yRf6*#
zR0d=N7L`SwHH<}RH4F<lz}+0SV1^V>CC8pv!@|H21j;m!-cBN@9SZKjDWv5jrYk`6
zAT&ckT2RHQCB+JvB??KPW^YNpLVjMVjzV%yY9e^(ub?QkEHl5fSRpO3G^Ye?3Q}hf
z+Al>7bSs7MjMS2h)FMzF1&<1F=FiQ{%gim!1@{Va6Dz=M*myy)LQ-OJYKlUBo<d@B
zNoit^LT)~&OrU820}Y^5!evZmngT5QXBNW-#^Pc6Q_u|1#GaS2m1ml$c?^`92B@$B
zS&gI6MPzu8A{fR}W`LwXWdcgM0TRS0qcICX6g?nAK>9!!nGLNOU;!VWn3oa{>R_Tp
z0G3*UDlrJN7tK1%@BtTX8islXREb|hQY#Z<EC~=4ln`eb=ow<V7!=AVQGl!-smKQj
zgVcg(5JqN0s(@;db?y(VTI|6?sn81HgMATXIF<7jC%6X->LC|JfocSBwE(UqKqY-q
z1V}6rM1ZT0XwVoW6QqR*Vnf<l@NuJC9Iz<q2h|l)pni!g2crTb7o);|K4vZ`i-m=U
zk>x)|-v*`^r6~rgC_t?|@YswTw4wkt#TGKwGM0dPGmHxvOPI2lvsl25LdGh!An<r$
zu|hel<U^WX$t(sHXrS=`P1Yi?U%}o4jR1ki%!@#7)`a-*78_(z0?m^}F$@e0yFmT}
zRbV_!LW~$aba<Gecm*^-48kC<d<NMF8lenk&}6J~hB+D3mV=ecu(B9jSVL<Gh2+FM
zg`(8N<O~Ik{DRax1(+gDJx#_UO;9{x8z>V1HIk6WgZIN4#B5Ae@<FK8H@aG!ekh6u
zr3S_-R}|Ny)g-uF4<6M^L3O_te)l5{U>^plOk`kS!0vh^l^CuEB`9zVK>8A(RtGo^
zf?&P?Pb6g)gC={6Gr<$IU_YP)H+X~=7AG1QF$V4;fuke=<WOu;A_xj*keTp7`Qsp^
z;UGW3=9gg&Dh#DKJyisn^U-9i3L)Sbw9rTM4tf+KypsfSBR1~{gWL!)74D(aAjM!0
zDPc)sXo@ks1kd!KX$4Rp3N(oVY8z&QhDI5y+!0=cHgGah6_QJfic<4R;K>Tq=}FGd
zgT-TFu|iTRs6&)inv+wh01AFRO{QBSV8=tW!d+jK0&+e!zp`f*$Ah$<hh-uG%w8o_
z2?@Sp1*bw!%6$b1SUpXqqBM{XQb7bZZ-7dEh}z2_B^jWUhd$JpSsV{hg4-jYi7`<0
zK+0sWC!AoO02OB#Q34*!1<mNBrYJyXc?&Wsi!+lGa}+WXi&DxHi&8<Z2ba#gAg5t*
zJ=ky8L4`0VKl3nEDPv1wXi<8LIVV3Ir5ge&!9gW4s6Z%YU<A!qEMS0iK|pIp7Ba^$
z)iTvGgX<Y)cs;{hBwNCo!d${u!wjAWU;>SBLBaquI)+!LRS9bf3z|+)M~5SuvDndz
zp^32uG<n4VHH~p0Xq^fZPIn~YFpV>twKxNA8fOh-QC$gF3Tq9+0`7$jvl&v@Y8V#q
zfLZKwS(_Od8B#cc!B~^CYN2CENosBZWKs;Z%K%FC;Qj(6QNXiVu?{FEi*xeJb1D@;
za-cj0nr#4S0I@*{0a{!lx)9I~OHO_|N-w3LC^HW-i&2zXT$%%MM^1h^LgP#D02yes
zyh_y>+Pwr#BBdo}=72ga1*v&pOY=dK(?um#3RRj|lqIDmCzck22lEp16!O7y+g1uy
zGSE_20o0Mwh5J7R)PahwV$xCg#Te_S$$LvQH@_?u-r@z#nSzQANT~rzQs4>VA|=q^
z8BY*sI5IaCJjzxCN^3>nT&V?0+@MPP7Ax4(Mc~>3RK{vTXX&y)+BsmAd=Y3&4ZSr2
zYKGkgWdzWqz8I?zqY$GQGl=A4WJ6-HFe!oBT(W4{1msCX7rm$eWFISJE|?WO^@}p=
z3K}>7`3>ZS;ymy~Sqf7NLoG`U3#ctq!w4z_nwh~v!wgwWpro9|+{|3d29{^30gaQf
zHZ#|<mw*P7*=tzSz)8P`4NP;^u!Cu?6y`Lhbe3kuT2MCNuHjh72vP|i|6*W*O^88f
zyTMbp#U+VFB?`IuWsqnuPb^A-^tsc)Q@F73(@_8~&H^RI;)2xV%(TqZ6a}zn!HF8|
zUqmXig3La^G=rvaA(QE#lmLo+5FL%&s?t$Fo@9@Ox8I>Afo-x<2nNl%gY<&UNGwqR
zn*o-D7yxEs=mxvaO2G{@HxJScO1fa3U<rs$FcV29Y%0W2p#-$z4C4Jng{1ua9OP*<
z<c2b+5uTq18iN4Mx*?|Naw<U!YDyr(6p4ATv1_oAaF2pQ7CK%43pRxk(7GIuSw*RZ
zrQjuUItqztC8<TwsyHzvs}#I&t|TAa&dSZq)6Y$;P(aFgMTvRoNE3L;pr!$2WQ&0T
z#0E{T1cR~wsHsrGP{U9Hp6qI7Y6cAxGx}*V-D1v7thmLTo0+G{e2X0vXJv^wx0pc$
zJ1EY|5_8b<)h&+XoXmpwvc#N!Ad^A0i~wVmDq8jdMWLQePJVJ?PO+UHOb58DrO5+n
zkApKgXnjPH4k)|pf(Shj0djQ_xPAjwp+(#vF;JTnJY}TG3F%~kGGtL9NU8`#RDkj)
z#AOBf#ZWgE7o<X2Y57G(pgBl%zp#MZ`4!|3kX0O<0?eQpeGO(l#wuYfKElZJAg6-!
z{32Kf3{*`qgR3R*L{Kq*31bQ)v>F8EX=YHK&1NoADPdW_3K_vhjH1=BW-}LyL-}kA
z8Ee@}*cWiruq|XPVb9{o;>>0$a>6HDYz&p<TF6+-Uc*+)QNs>4fny<K8Ys${6Iao?
zoB(G8<b<DxG_?-O2cRGa<<sPR&|0q&r1T3d4q!QC_!KONQlTid05rUoR{}1OGg1|D
z5{r>q<&bg-(f)%r4WQ*hW(jCc3tT!Q1u%3`4memd5>r46APRC)OF%0E5<&HRNhxST
z4ZbuWBflI{oaC3KDx~J6q^2ln6sM*tATR7vNX;wF1^0bYQcDsubBZ<f6#Vi_Qc-Qq
zECKZ*3i6AKGm~;sA>|n;f?@FoUY-D+TLAUTLGH;+Q-HT9z_KZ+C6Fn+lv40OF8W|<
zm7*J@=>nbIhD|I&7HWXJte5_0<;P3DNdfkn+(k8@q5xF=LPjw7L0rT@4X6+-0@c!x
z7Kl7Z7F5h&FH&x?fo7ERvv08{=ca&KFt^x~ON-+3ld^7cre)?O=77bZ%>1M*w4$Xb
zmw|zSlaYa;7&LGq#06R!C&0|bDDa<$MTn7yQRqJp3kxG3W0eY)at5RIq{&zWTFa%$
zR0LXl<fq9PqN$H<!5?HA16){wnj}S_#gC8z6jZ)J3RUp94mfDQ0U`(r2JkE>s3?bw
z^?;V}6&ZpgK!sZos0z8onwFEFSOQ)ERb&H_0+oG5pjrl8E<<KfKwXntJfP8`%;enE
zl8pS6BJeaLXawyR3#fW6iUsKfjT9job&$3pxa9{LhbzhiDFOG<KyA1pP+tTRwV*IX
z3TOE68aP%#9xetgA^@*OWC5-BgkvTsi%Eb<fSrR~hMS8^jzvn4kCRD6fRTe)K$1h7
zkCl%F1UXnZSol~(Sd^sHM8M{1au<QtQi2y<6{&%o21-)5SU`EC2oz?w*h(r3GV{_Q
ufdO_cIBdX44eUk)0h)Qe#bE<kTx$m!^eP6$2MYrS69*#)BWOZchzS4;pY7fN
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/util/boilerplate_wrapper.py
index 99e0492..e93b62d 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -7,7 +7,7 @@ from pinocchio.visualize import GepettoVisualizer
from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import copy
import signal
# TODO make the package work
@@ -25,10 +25,14 @@ like max_iterations, what data to save etc.
NOTE: you give this the ready-made control loop.
if it has arguments, bake them in with functools.partial.
"""
-class ControlLoopManager(controlLoop, args):
- def __init__(self, controlLoop, max_iterations):
+class ControlLoopManager:
+ def __init__(self, controlLoop, args):
self.max_iterations = args.max_iterations
self.controlLoop = controlLoop
+ self.args = args
+ # predefined ur magic numbers
+ self.update_rate = 500
+ self.dt = 1 / self.update_rate
# if you just stop it the program with Ctrl-C, it will continue running
# the last speedj lmao.
@@ -76,16 +80,17 @@ class ControlLoopManager(controlLoop, args):
break
end = time.time()
diff = end - start
- if dt < diff:
- print("missed deadline by", diff - dt)
+ if self.dt < diff:
+ print("missed deadline by", diff - self.dt)
continue
else:
- time.sleep(dt - diff)
+ time.sleep(self.dt - diff)
if i < self.max_iterations -1:
print("success in", i, "iterations!")
else:
- print("FAIL: did not succed in," max_iterations, "iterations")
- self.stopHandler(None, None)
+ print("FAIL: did not succed in", max_iterations, "iterations")
+ if not self.args.pinocchio_only:
+ self.stopHandler(None, None)
"""
robotmanager:
@@ -110,6 +115,9 @@ class RobotManager:
# might be changed later if that seems more appropriate
def __init__(self, args):
self.args = args
+ self.pinocchio_only = args.pinocchio_only
+ self.simulation = args.simulation
+ self.gripper_flag = args.gripper
# load model
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
@@ -125,11 +133,13 @@ class RobotManager:
#viz.initViewer()
#viz.loadViewerModel()
#viz.display(q0)
- if args.gripper:
+ if self.gripper_flag and not self.pinocchio_only:
self.gripper = RobotiqGripper()
# initialize and connect the interfaces
self.simulation = args.simulation
- if not args.simulation:
+ if self.pinocchio_only:
+ self.q = pin.neutral(self.model)
+ elif not args.simulation:
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
self.rtde_io = RTDEIOInterface("192.168.1.102")
@@ -140,6 +150,8 @@ class RobotManager:
else:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
+ self.rtde_io = RTDEIOInterface("127.0.0.1")
+
# ur specific magic numbers
self.n_joints = 6
@@ -155,7 +167,8 @@ class RobotManager:
# we're not saying it's qdd because it isn't directly (apparently)
# TODO: check that
self.acceleration = args.acceleration
- self.rtde_io.setSpeedSlider(args.speed_slider)
+ if not args.pinocchio_only:
+ self.rtde_io.setSpeedSlider(args.speed_slider)
self.max_iterations = args.max_iterations
# TODO: these are almost certainly higher
# maybe you want to have them high and cap the rest with speed slider?
@@ -165,6 +178,9 @@ class RobotManager:
self.max_qd = 0.5
# maybe you want to scale the control signal
self.controller_speed_scaling = 1.0
+ # error limit
+ # TODO this is clik specific, put it in a better place
+ self.goal_error = args.goal_error
"""
getQ
@@ -175,22 +191,25 @@ class RobotManager:
also, the gripper is controlled separately so we'd need to do this somehow anyway
"""
def getQ(self):
- q = robot.rtde_receive.getActualQ()
- if self.args.gripper:
- self.gripper_pos = gripper.get_current_position()
- # the /255 is to get it dimensionless
- # the gap is 5cm
- # thus half the gap is 0.025m and we only do si units here
- q.append((self.gripper_pos / 255) * 0.025)
- q.append((self.gripper_pos / 255) * 0.025)
- else:
- # just fill it with zeros otherwise
- q.append(0.0)
- q.append(0.0)
+ if not self.pinocchio_only:
+ q = self.rtde_receive.getActualQ()
+ if self.args.gripper:
+ self.gripper_pos = gripper.get_current_position()
+ # the /255 is to get it dimensionless
+ # the gap is 5cm
+ # thus half the gap is 0.025m and we only do si units here
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ else:
+ # just fill it with zeros otherwise
+ q.append(0.0)
+ q.append(0.0)
# let's just have both options for getting q, it's just a 8d float list
# readability is a somewhat subjective quality after all
- q = np.array(q)
- self.q = q
+ q = np.array(q)
+ self.q = q
+ else:
+ return self.q
return q
"""
@@ -210,12 +229,12 @@ class RobotManager:
# np.clip is ok with bounds being scalar, it does what it should
# (but you can also give it an array)
qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
- if not self.simulation:
+ if not self.pinocchio_only:
# speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
- rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
else:
# this one takes all 8 elements of qd since we're still in pinocchio
- self.q = pin.integrate(model, self.q, qd * self.dt)
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
"""
@@ -238,11 +257,12 @@ class RobotManager:
Mtool = self.data.oMi[self.JOINT_ID]
if not self.args.visualize:
print("You can only specify the translation right now.")
- print("Here's where the robot is currently. Ensure you know what the base frame is first.")
-
- print("base frame end-effector pose from pinocchio:\n", \
- *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
- print("UR5e TCP:", *np.array(rtde_receive.getActualTCPPose()).round(4))
+ if not self.pinocchio_only:
+ print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
+ print("Here's where the robot is currently. Ensure you know what the base frame is first.")
+ print("base frame end-effector pose from pinocchio:\n", \
+ *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
+ print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
# remain with the current orientation
# TODO: add something, probably rpy for orientation because it's the least number
# of numbers you need to type in
@@ -251,7 +271,7 @@ class RobotManager:
#Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
# do a while loop until this is parsed correctly
while True:
- goal = input("Please enter the target end effector position in the x.x,y.y,z.z format: ")
+ goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
try:
e = "ok"
goal_list = goal.split(',')
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 34b958f..62537f7 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -2,15 +2,19 @@ import pinocchio as pin
import numpy as np
import sys
import os
+from importlib.resources import files
# can't get the urdf reading with these functions to save my life, idk what or why
# TODO fix paths via python packaging
def get_model(visualize):
- urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
+ #urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
+ urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande.urdf')
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- mesh_dir = "../robot_descriptions/"
+ #mesh_dir = "../robot_descriptions/"
+ mesh_dir = files('ur_simple_control.robot_descriptions')
+ #mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = os.path.abspath(mesh_dir)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
--
GitLab