From 0b4cb89b1c01736a6945a8c17d5081922fe08040 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 20 Nov 2023 19:35:05 +0100
Subject: [PATCH] almost done with the dmp, just need to fix the calibration
script and add marker pick-up
---
README.md | 25 +-
.../{ => UR}/ClientInterfaces_Primary.pdf | Bin
.../{ => UR}/ClientInterfaces_Realtime.pdf | Bin
.../{ => UR}/Client_Interfaces_1.1.3.xlsx | Bin
...-E_Manual_UniversalRobots_PDF_20220114.pdf | Bin
.../{ => UR}/scriptManual_SW5.11.pdf | Bin
.../.drawing_from_input_drawing.py.swp | Bin 36864 -> 45056 bytes
python/examples/drawing_from_input_drawing.py | 101 ++++--
python/examples/path_in_pixels.csv | 293 ++++++++----------
.../__pycache__/managers.cpython-310.pyc | Bin 6514 -> 6516 bytes
python/ur_simple_control/managers.py | 2 +-
.../util/.calib_board_hacks.py.swp | Bin 0 -> 32768 bytes
.../calib_board_hacks.cpython-310.pyc | Bin 5604 -> 6463 bytes
.../__pycache__/freedrive.cpython-310.pyc | Bin 717 -> 1273 bytes
.../__pycache__/get_model.cpython-310.pyc | Bin 1996 -> 1996 bytes
.../util/calib_board_hacks.py | 138 ++++++---
python/ur_simple_control/util/freedrive.py | 33 +-
17 files changed, 355 insertions(+), 237 deletions(-)
rename docs/hardware_docs/{ => UR}/ClientInterfaces_Primary.pdf (100%)
rename docs/hardware_docs/{ => UR}/ClientInterfaces_Realtime.pdf (100%)
rename docs/hardware_docs/{ => UR}/Client_Interfaces_1.1.3.xlsx (100%)
rename docs/hardware_docs/{ => UR}/Hand-E_Manual_UniversalRobots_PDF_20220114.pdf (100%)
rename docs/hardware_docs/{ => UR}/scriptManual_SW5.11.pdf (100%)
create mode 100644 python/ur_simple_control/util/.calib_board_hacks.py.swp
diff --git a/README.md b/README.md
index cfc8215..c72d8e0 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,11 @@
# do this everytime you use this repo
+-------------
- do git stash
- and then git pull
# general info
+-------------
- all of the code has been used and tested on Ubuntu 22 LTS
- both Python and C++ implementations are available, there's examples of both (both for the libraries
used and for their combination)
@@ -12,12 +14,14 @@
and so c++ implementations can be looked at to analyse python functionality as well
- matrix operations are done with Eigen in C++, and NumPy in Python
# libraries used and their purpose
+-------------
- ur_rtde for the client-side real-time interface for ur robots (we have a UR5e)
- pinocchio for dynamics intergration, and other robotics-related math on the client side
- gepetto for visualization (has nice integration with pinocchio)
- Dockerized ur simulator as a simulator (it's a bad simulator, but it uses the same robot-communication API
as the real robot, making coding and testing easier)
# contents of this repository
+-------------
- basic documentation can be found in the docs directory: official UR documentation on the robot's capabilities
and its interface, short instructions on installing Ubuntu, ur_rtde, pinocchio, gepetto and the simulator
- basic closed-loop inverse kinematics control is implemented in both C++ and python. the C++ version
@@ -29,6 +33,7 @@
- some basic tests will be here as well, while full projects built on this repo will be in separate repos
# installation and operating instructions
+-------------
For all of the listed items, consult their specific documentation in the docs folder, and do your own googling.
Note that for most items you can install through the ubuntu package manager apt, or through the python
package manager pip. Likewise, you can compile the libraries and install from local files. Both options
@@ -61,6 +66,14 @@ Here is a checklist of what to do together with the key remarks.
skip using it when simulating.
#TODO re-organization of the repo
+-------------
+- RIGHT NOW: get rid of all ur-specific calls outside of the manager.
+ make all other functions depend only on pinocchio.
+ this is the only way you actually get transferability.
+ this includes the tucking of all rtde_control/rtde_receive calls
+ behind RobotManager wrappers (because other robot don't have rtde_control lel).
+ i'm also pretty sure there is not a single instant in which i call robot_manager.getQ() without
+ calling pin.forwardKinematics immediatelly afterward. so think about doing that in robot_manager.getQ().
- long term: write all code in c++, make python bindings with pybind. have common documentation
and examples
- short term: the long term goal is too big of a time sink right now both lack of experience
@@ -73,15 +86,25 @@ Here is a checklist of what to do together with the key remarks.
- the pyproject.toml actually brakes local install so i'm just keeping it out for now. sweet jesus
what a mess
+#TODO: write instruction for extending the library to new robots
+-------------
+- make sure the base frame of the robot aligns with base frame of the pinocchio model
+ you get from the urdf. even better if you make sure the urdf is correct (change it to
+ be correct), because then it will be transferable to simulators etc
+- todo the rest (will probably happen while i make this work on panda let's be real)
+
#TODO for current students
+-------------
- explain hom transf mat
- make this documentation prettier
# TODO for profit
+-------------
- writing documentation, making text and video tutorials to the point where this can be used as teaching material
- do the documentation with doxygen so that you get webpages from markdown as well
-#TODO for fun. general point: get a collection of control algorithms for the arm
+#TODO for fun and profict. general point: get a collection of control algorithms for the arm
+-------------
- implement and test other inverse kinematics algorithms
- rewrite finalized dmp (and other) code into C++
- learn pybind, create python bindings for that code
diff --git a/docs/hardware_docs/ClientInterfaces_Primary.pdf b/docs/hardware_docs/UR/ClientInterfaces_Primary.pdf
similarity index 100%
rename from docs/hardware_docs/ClientInterfaces_Primary.pdf
rename to docs/hardware_docs/UR/ClientInterfaces_Primary.pdf
diff --git a/docs/hardware_docs/ClientInterfaces_Realtime.pdf b/docs/hardware_docs/UR/ClientInterfaces_Realtime.pdf
similarity index 100%
rename from docs/hardware_docs/ClientInterfaces_Realtime.pdf
rename to docs/hardware_docs/UR/ClientInterfaces_Realtime.pdf
diff --git a/docs/hardware_docs/Client_Interfaces_1.1.3.xlsx b/docs/hardware_docs/UR/Client_Interfaces_1.1.3.xlsx
similarity index 100%
rename from docs/hardware_docs/Client_Interfaces_1.1.3.xlsx
rename to docs/hardware_docs/UR/Client_Interfaces_1.1.3.xlsx
diff --git a/docs/hardware_docs/Hand-E_Manual_UniversalRobots_PDF_20220114.pdf b/docs/hardware_docs/UR/Hand-E_Manual_UniversalRobots_PDF_20220114.pdf
similarity index 100%
rename from docs/hardware_docs/Hand-E_Manual_UniversalRobots_PDF_20220114.pdf
rename to docs/hardware_docs/UR/Hand-E_Manual_UniversalRobots_PDF_20220114.pdf
diff --git a/docs/hardware_docs/scriptManual_SW5.11.pdf b/docs/hardware_docs/UR/scriptManual_SW5.11.pdf
similarity index 100%
rename from docs/hardware_docs/scriptManual_SW5.11.pdf
rename to docs/hardware_docs/UR/scriptManual_SW5.11.pdf
diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index 6cf925d12f431010f4c30a9c95291912b3547236..f16debe88377c08adde32dd68a6a0aafb6b8ad24 100644
GIT binary patch
delta 5471
zcmZozz|`=7Nh--8%+puFLeGeSfq_AQf#G#WbZTj64Z~frjZ!c8896623-~fhO%4>0
zcN7OHVPIgG45b;NGzXOChtdn7G)$fg%D10fD4_0m8p?yoazgpSP}%}20i_t2q2dlu
zT5|JEf#-aa6j)g885kI%Cvz%P*C%l?Feq~|Feq^`Fx=u~V3@?oz~IHnz+lbE!0?`f
zfnhNR14B3m0|OTa1H)x@28Moi1_mc~28K^;3=CV?7#O127#J+r7#N<gGBC_zWnge+
zWnfTZWnj3>!obka!oU!~!oa}9!oaYfnSr65nSsHEnSnusnSo&^69YpN6N5N|HxmN`
zKNADP8Ab+%$&3sP(TofXMT`?KI!<<oW7%A;=*B6bqmZ6j5?qj)n&O$~l3A3RT#}ie
z=bz@cd9Ri`YrQBl1A`$8Bnn_YJ;Tqy(96%jV8_qE@Rg5&;WZxv!&*KDhDbgJh7djm
z25mkD25vqEh8MgH3|Dv=7*6prFm&-UFnIGaFvx-UJPZuoJPZszJPZt<xfvK*xfvK*
zxEUDqxfvL)b1^Wi;9_8i<6>ao;bN#~*ulxbP|C@`5Y5TJAi&AM@RNgq;VK6M!)Xo%
zhJ_pq3==pQ7$P|s7-TpY7@n{*Ff3<hV3^C!z!1#N!0?lefnhBh14AMk1A{0V1H(5~
z28LU#3=9id85l}f85lfS85s0f85sCk85nM}Ffi<7VPIIq!oaYQg@K`ug@GZLMS_9B
zl7)f6f`x&>n1zAChy@ZF1}vKyStOakf%wyeoqKY=y~O04!d3N&DGUtBTnr4)L1{~X
zfk6tII9BsBFihiTU`XX>U@+%rU{K&^U^vdlz_5vrfuWd>fx(lHfq{pQf#Coz14An>
z1A`+k1H*qF28Ja(3=Hu+3=G^n3=CX63=FTi85j<6GcXizGcbs9Gcd4nGcd4lGcYi7
zgYB$m$lzvRNato?i05WtFy>}pxW&c5(80yPV9v$Bz{17Au$z;CA)AwdK^B?<XK*ku
zIB_sA2y!qmY++|$NMvVV@MULU@L^|QkYZ<GxWUH2aEOh8p_h$;p_Gk*A(@SV!I6!D
zL4}Qh;SDPT!*x~$hC{3j44teD3`wk!3=HzD3=F?m7#J3_Ffg>RFfh2XFfa(PFfjaL
zW?(qQ%)n5`%)nsB%)lVX%)qdliGg7?69Ypg69a=W69dCDMh1pGj0_B`85tO+FfuR{
zGcqtlK=Rw>cX2|Do7<dBIAoL+((@G(^HLNFGLy3vN(&T9GEx<C6N|D_ix?*F@{yZd
zQX?phN6qG4K5opC3JMAeNr}a&DGK>{NG2#LO<s^FYE_<*T9T1kq)?e(s!*PoSE5jo
zuaJ_jkd~R30<}sZKP{~|wM0RqC^b1TC%H5yu_RR?#5rK{L0@4eC8fz55@m!y%5@<w
z(}gIX%&)37`ClR%NX3Fg(I`Lv5LYXO5D(8_1<zmww;+FC1xJN|AXgVxH%~uT7X=@G
zXU7mve?L72kb7aS0(rP7KPkUNp*SPIGzVKq=png-4-`=Gr3LX2r-0m0AHx7js#-h@
z42yUmxf@oBzT{_Mc)-uVaG#%n;Wj@5!)bm7hTZ%O4E6jB4DS3441&<4+senl(8$NY
zV9&?EAj`+V@R^r^;VLf!!)#s#h8kW51{+=m1}$C&1~y&>hV4*$#CRAO&T%s^oUP|(
zVCd#%V5o$q<$P$ewdQ7Ec*MoPFoBDK!5&fsFdXJ&U?}EfU{K{`VEE3#z_5UWfx(M|
zfkBdkfng6j14AY|14Ae~149Tq1A`Jf1H%J028L5?3=C7*7#M2V7#OnI7#KX+7#MWe
z7#O~=GBDg{Wnf4?#mc}ik(GfVi<N;vot1%siIst2H46hn4+{f>FAD>M1hizm#LU3Z
z#>~Lr#>~JV$;`lTn2CX5GZO<tArk|GH4_8FJ4ObEBa93Tn;97x<}flaR5LO#B%mf)
zL#}#I<S8qJ_`CQkl%y7yD3oMm7K39{Ss}ATy;vbVKQk{~A-}XlAwLbA7?bi7i&AvJ
zsk}T@0hR*O^A$4lO7f9a6lJDol-S$b+k=g(iqB7rFUl`buvI9^FG&QI<ng(QCG|y_
z7135YMzLT;%3xzlGE(yt((|Dr#ra^@St+2TWrbo;wSbyUVCE|1CYBUsRzUUV7nNk>
zr|0J-=2U{R3Mhesy#SG(yw{9f0OV(d%;G%t5{2~qyu8H8bJfJTAsz>-nw-!lI{8dL
z=j2=cYLgX8ltfZe(?C^|FF0rWgL9k4q)P32O)CZ^B_#&`q_WKX(&C&-NDyX~7MCa_
zDr6)kXQ!qp6zAuZf_$%|kW^ZtkXfQoo?nz*tWcb<P@a*mkepbQTCB%Vl3$vfp^#Yu
z3FqR}RB*tiWTvI17NzDTrz#|+mXxQaf{KU45?G)q)Rz<`<`w6F16QFeHMu0eNRPoY
zB{fkYvlv`nfN~pZv7`ekjiB*Xl3$>ZoL`ieTBO4OD#SoRm7HG$s&^GIO;$+DFH$JU
zFVM|NO)G)ut1r@H2+7DSRwzo%$xJN<6<-;NWuWwfY*c<xW@;WJ1;El&K~7>`sse*9
zDqu)YEkO=qaM*$)q#&`lm;ogcGz#N0^H?%VG_5xOk6*{cWTXiyWhNg=>Vh&iCnhiE
zsdr^&U`U0w862S%;7NW4hE@Cw41xR%3}5&d7|!!CFjVp}FvRmQFvRgOFzE3yFnr-<
zV7Sf8z%YTAfuWd}fgy^QfgzHYfx(=Yfq|Qsf#DGk1H(oh28J#k28Ku;28IY81_n(Y
z1_lit1_oXp28M&&3=E~*4D}3}+zbpNkQ$NU1Q!EC4HpB00T%<q6HW$(U7QRITR9mR
zCUY_{)N(Q~#Bwq)cyKZ>*l{v2aBwm(9N=JJn8(4u(8|HU;LE|lz{0`6aGsrk;V3%;
z!*q5AhHPjP?FAbH!yGmSh9EWu23a-+hUcsd49{2@7|yfCF)%c+GBEhDGBB{QGB8|V
zVPI%xfrQ9!76yiuEDQ|k&`R+uGXukEW(I~0%nS^%%nS_b%nS^-nHU%rGBGd&FflOb
zF)=WxGBGe5Wn^HO&d9(p4b+U2E>d&`6(}GKDO9W!k~0$X(m?`X3=vhxP0R)->Ecv{
z%(P+%H#fg1RiQXFuQ+w`+uq{M-X*F`oFF~Q3c9+wlif;`Cg+s+PTpH$Hu-yr$7JVH
zqsf(}{zA$S&7cHUkXVwTVC=G)uly6^<eQqps)<GE#d>-1$%#3cNk!mX5f3WkiZ#KB
z2xPW`g2H6oN}0(^O68b!6k;ZSuP|qf*&JJG%}9(xCm+m<-Tbafh;_0<+<I^WYc{Ce
z2(7hXZTn~Z3=I4D85p|w85pAZ85m^w85r*IF)*CuV_@jyV_*p6V_*>GV_-PV%fK+5
zmw_RXmw`c(mx19k4+Fy=9tMU>JPZtLco-N`c^DYx@h~ui@h~v(@h~tP<7Qwu%FV#g
z%+0`12ufSSy@B<I6I@$C;|;k;iiZ?-3c3o(rA0-lc_r~k<rO5h3yL!HN;H%p;xHpX
zG(-fFI#7)P$-*#1RXv(Am@J5fs6bOdoO1|kQqoa~VNg)0S5ScZ1B<~*Iz;*&**Ya1
zEa3<VF=bE%rK6CO3Tg%<CMV~Y=9!j2O8tyfg@XLz;>@I+RE6BkoXp(JlGGwtvr|1e
zzgVF>qf#L+zXY7oCSUBcGsfaza8n>DzeF!3u_RG1-#0VbELIOG7$={MH{r=E&`T^T
zO03i<)YP1O&sC~k8C(^@>!l1(^^%jHm#$EfT9m7hn4DacUtFvJt13$ria@Q=VujR_
zWUynD70Oc;N{hjzk3vCao<eedeo;zh9;l66Tv=R_nyZkMnw$txkqGj7c?OsQ>n_Tl
zyx3H9^6X-XdPoDr7Ss;F(nZyPHAX;<oTAJMNRUD!uSy|PJy#(uKPM-@JToudN=Zin
z);xl!Q-*OBY~eBr;AZhY9tH+AUIqq+dI1IoSpWYCsNKuYz_62_fng0l149o#14A}H
z1A`bp1H%hG28NY<3=H%67#Ono7#OmkH5)Iq#3_N+Wcs`igE#RoFeLIYFsSn|Fx=*5
zU|7n{z!1dEz+lb|tuCR|W`35{T%ca&WP@H72P~x}C{KVe7AdSHzFIO!H3$=>0~{S7
z)i6wyuE}qE<>8ePQ99uEfK-!emkPYV0_k?~hu2le^&ZqvoGPI53OE&{Dj=#(m|_sk
zprEk%VW}P?H;9v3ky)al$uRk0p(Sf-PI0Q$<cEbiS%wN)&`x7Kq!$<qNq`_7ki@DE
z>6t;L)pbynBUGa(QvhcR<hC26d5jd8pbjWFCqpzs3lFe@%rpgX%?(ll?HEF>w*t$9
z9S3Rwf*2sI4C=nXrIfMtzCp_BVFrVm!H7<$6*yxFFf%Y{KwE~OQ8G|6-^|az(815Z
zkk8M+kjKx!pa!kEr|~f`Oyy%>2<Bs8aO7iPu;620Fy~`n(C1@dVCG|BSjEf05W&m9
zzz!N^f{rLz^Dxviyya$K*w4)X9tsiSW?*>8#lX<Q#lT?5#lRrT#lRrL#lY~2lYwCc
zbU+T&0(9kMU@+!nU=ZYFVE6*+S8^~gEazZgm;@bNGU8xhc+bwjaEhIQp%~iN6h-Q5
z&SGO=@MB|O;A7KaV0g&Nz|hUgz);P~z+lPBz`)GPz_1P4x2$GiU`SzMU~pyug~Y~-
zVxU1Jw#{GTxIv>Ej$A^Jn1V(HIJhS#^l5vd2rJks<Q3@UWacI2r0eD77v*Xo73c)2
FX#k#~9?$>)
delta 1969
zcmZp8z|^pSNh--8%+puFLeGeSfq_AQfnnN|sMO7&H4N9QHcGwVXJntuEa1!NJULK6
zo{?+vMgeulQy_&53=E7=ngdGnLTLf01e9W6hKk!lY4OdHg3tLTK5&?%z``2Nz`#&E
znNy*<zKV;1!J3PK!HSE4;S(nV!zxY&h9ph~hG0$x26j#chTR+t4EY=k3`!gf46oT4
z7?!g$FhsF4FmSOmFq~myU?^r|U<hDiVEDtzz_5*#fgzTafx(KEf#Ed^1H*C_28Ik4
z1_mh>28Qd*3=H#`85lyC85ne!85k}yF)++xVi0FYW@2DaXJTM@!pOj|nvsE_n2~{@
zh;ib@$j#A;UYwH?wInuQ)zW9Jf6UCl@R<b?STH|r=VxG8%g?}&&(FXh&CkFf#?Qd;
zmXCqqEFS~IDn15=X?zR}J$wudK70%eynGA{4|o|EdUzQae0dod_;?u@w(>AAxbrYD
zxbZMBeB@?en90q+kj2fwpuo+*aE^<C0TejqTnr4dTnzOLmq4Mx$-pq3lYya^lYt?b
zlYt?KlYv2+lY!wL2Lr=04hDuy4hDvJ4h9BM4hDwZ><kR~><kRb><kRo*%%mhurV;q
zVPjxOW@BJ5V`E_8W@BLZz{<dIk(GgAE-M2=H7f%{3M&IcGAjc^04oE7J}U#mZx#lI
zUn~p^Us<>r7{0JTLg3S8!G+??6CYgMY;3~KU7wi3z!1mCz|h772^?nz2sq5oz);T5
zz+l7A!0>~QfnggT14A?)1A`bJ1H&O+28M&Y3=E5S85k;g85q=g85qv+Ffi=lVPM$J
z!@y9?!@!`;!@%&En}OjGHv_{0ZU%-vZU%-{ZU%-HZU%-LZU%+`ZU%<$pnzG*#lR54
z#lWD*#lUcblYyawlYzmClYxPUlYwC?G(hw?7#J?HGceS#GcaVaGcaVbGccI5GcbH*
zV_>+?#=x+Oje%hj8v{cF8v{ce8v}zQ8v_F;8w0}^RtARqtPBjxSQ!}VSs55?SQ!|^
zSs54(urM$zWMN=PV)0~P&}Cs@5NBavc+SkgFq@fyA)1+iL64b%;RX`}!wDux$V4+S
zFnBRBFc>p2FbFX*FnniZU^u|Yz_5^!fx!ckXC?>KZ<w4AIB9c9kRI1$gDjTGdFy#5
zAM+C2tmv)5YzGSGKfDYK4t$Vs=7Iz>!zq3ShFX3GhG2dM1}A<725o)@1}=UEhWC67
z3_JK37-sT;^lVlXWa66~SigRALZBeyri8Ujlea1HPu}OpHu+Fe7mU3*F?lgh{U0U<
z1{r8lcZAxqn4f{6j-P=+gP(zckDq~om!E;*E*}HKVm=0jE<OeZdp-sRB|ZiQPCf<(
z4n78kXS@syyLcHGCh{^cB=9mYSn@J3aPTrPu=6r7+~8qgxDJi&0v-ki6&?nL8{7;G
zhq)OTrg1aWGn8{PFlci#Fx=;2V3^Fsz|hCVz>vemz!1j8z+eWAX>l$FhKHOC3^O?y
z7|J;r7~(h?7=CatFudbnU^u|Rz%Ye_fgziNfkBXif#E1S14A`C1A`ko1A_oN0|P%h
z1H)c628L=j1_l*qOoL+i2rC0aA!{H5gEcDy!+RD6hOI0N3@I!O3{EVN5aDHEV7SK2
zz_6Z~fuWk2fx(!Wf#Cxa1H*bI28Mbj28J>w1_m)E28KtB3=E4I85lyKF>7mocyeQ*
z=;VSz!O5=+eJ1y;*LG7-P*70LO)SbzEsD=iODj$-QLt4gDN4*M&PgoE%+HH2OHD4x
zFN!vbRnS#PE-flb%`1s7$S+QfHi~5cX$E2C$>M!>vS1dn5(Qg@yaK(%qN2n~jmcX~
zRk<~hbj0`T>wt9>7o?`9DA+1Q8|Ww)=qTtK=ox|;ATF{MH)G|L!5Z>&QsTjy;)_9i
z5LQ+wC@oPa0Wl}b#wl&qE7D_{d|>YU$uebfn_0>(Gfs{w<Dcw1UxrD+b~1m3%;fjw
zlAAj#%orz&XUR+!(A3^+P^HM~1xnm^co-OTK-Gx=0|STxrM!#$3=He|85r8337ngs
zf#C!n14BC>14B3;1A`wQ1H(ILQmf_#>EEm<_=sn+o&BMq9pJ2>5LKT1Jys0>{epIH
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index ac6c1b2..bfa0b35 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -25,7 +25,7 @@ from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
from ur_simple_control.clik.clik_point_to_point import getClikController
from ur_simple_control.clik.clik_trajectory_following import map2DPathTo3DPlane, clikCartesianPathIntoJointPath
from ur_simple_control.managers import ControlLoopManager, RobotManager
-from ur_simple_control.util.calib_board_hacks import calibratePlane
+from ur_simple_control.util.calib_board_hacks import calibratePlane, getSpeedInDirectionOfN
from ur_simple_control.visualize.visualize import plotFromDict
#######################################################################
@@ -118,14 +118,23 @@ def getArgs():
# TODO measure this for the new board
parser.add_argument('--board-width', type=float, \
help="width of the board (in meters) the robot will write on", \
- default=0.3)
+ default=0.2)
parser.add_argument('--board-height', type=float, \
help="height of the board (in meters) the robot will write on", \
- default=0.3)
+ default=0.2)
parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
help="whether you want to do calibration", default=True)
parser.add_argument('--draw-new', action=argparse.BooleanOptionalAction, \
help="whether draw a new picture, or use the saved path path_in_pixels.csv", default=True)
+ parser.add_argument('--pick_up_marker', action=argparse.BooleanOptionalAction, \
+ help="""
+ whether the robot should pick up the marker.
+ NOTE: THIS IS FROM A PREDEFINED LOCATION.
+ """, default=True)
+ parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
+ help="""
+ whether you want to do find marker offset (recalculate TCP
+ based on the marker""", default=True)
parser.add_argument('--n-calibration-tests', type=int, \
help="number of calibration tests you want to run", default=10)
parser.add_argument('--clik-goal-error', type=float, \
@@ -141,6 +150,50 @@ def getArgs():
the simulation yet, sorry :/ . You can have only 1 these flags right now')
return args
+
+# go and pick up the marker
+def getMarker(q_init):
+ pass
+
+"""
+getMarkerOffset
+---------------
+This relies on having the correct orientation of the plane
+and the correct translation vector for top-left corner.
+Idea is you pick up the marker, go to the top corner,
+touch it, and see the difference between that and the translation vector.
+Obviously it's just a hacked solution, but it works so who cares.
+"""
+def getMarkerOffset(rotation_matrix, translation_vector, q_init):
+ # put this in a correct place
+ old_speed_slider = robot.speed_slider
+ robot.setSpeedSlider(0.3)
+ # TODO: this isn't gonna work because it's not going
+ # orthogonaly to the board
+ # so TODO: calculate TCP speed based on the rotation matrix
+ # and then go
+ z_of_rot = rotation_matrix[:,2]
+ # it's going out of the board, and we want to go into the board, right????
+ # TODO test this
+ #z_of_rot = z_of_rot
+ print("z i'm following:", z_of_rot)
+ speed = getSpeedInDirectionOfN(rotation_matrix)
+ robot.rtde_control.moveUntilContact(speed)
+ # we use the pin coordinate system because that's what's
+ # the correct thing long term accross different robots etc
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
+ current_translation = robot.data.oMi[6].translation
+ # i only care about the z because i'm fixing the path atm
+ # but, let's account for the possible milimiter offset 'cos why not
+ print("translation_vector", translation_vector)
+ print("current_translation", current_translation)
+ print("translation_vector - current_translation", \
+ translation_vector - current_translation)
+ marker_offset = np.linalg.norm(translation_vector - current_translation)
+ robot.setSpeedSlider(old_speed_slider)
+ return marker_offset
+
#######################################################################
# control loop #
#######################################################################
@@ -234,6 +287,7 @@ if __name__ == "__main__":
#######################################################################
# drawing a path, making a joint trajectory for it #
#######################################################################
+ # TODO make these ifs make more sense
# draw the path on the screen
if args.draw_new:
@@ -244,7 +298,9 @@ if __name__ == "__main__":
pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=',')
# do calibration if specified
if args.calibration:
- rotation_matrix, translation_vector, q_init = calibratePlane(robot, args.n_calibration_tests)
+ rotation_matrix, translation_vector, q_init = \
+ calibratePlane(robot, args.board_width, args.board_height, \
+ args.n_calibration_tests)
else:
# TODO: save this somewhere obviously
# also make it prettier if possible
@@ -255,29 +311,32 @@ if __name__ == "__main__":
[ 0. , -0.83234077, 0.55411201],
[ 0. , -0.55411201, -0.83234077]])
+ # make the path 3D
+ path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
+ # TODO: fix and trust z axis in 2D to 3D path
+ # TODO: add an offset of the marker (this is of course approximate)
+ # TODO: make this an argument once the rest is OK
+ # ---> just go to the board while you have the marker ready to find this
+ # ---> do that right here
+ if args.pick_up_marker:
+ # pick up the marker
+ #TODO
+ pass
+ if args.find_marker_offset:
+ # find the marker offset
+ marker_offset = getMarkerOffset(rotation_matrix, translation_vector, q_init)
+ print('marker_offset', marker_offset)
+ path = path + np.array([0.0, 0.0, -1 * marker_offset])
+ else:
+ path = path + np.array([0.0, 0.0, -0.0938])
+
+ exit()
# and if you don't want to draw new nor calibrate, but you want the same path
# with a different clik, i'm sorry, i can't put that if here.
# atm running the same joint trajectory on the same thing makes for easier testing
# of the final system.
if args.draw_new or args.calibration:
- # make the path 3D
- path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
- # TODO: fix and trust z axis in 2D to 3D path
- # TODO: add an offset of the marker (this is of course approximate)
- # TODO: make this an argument once the rest is OK
- # ---> just go to the board while you have the marker ready to find this
- # ---> do that right here
- # put this in a correct place
- #old_speed_slider = robot.speed_slider
- #robot.setSpeedSlider(0.3)
- #speed = [0, 0, -0.1, 0, 0, 0]
- #robot.rtde_control.moveUntilContact(speed)
- #current_pose = np.array(robot.rtde_receive.getActualTCPPose())
- ## i only care about the z because i'm fixing the path atm
- #marker_offset = translation_vector[2] - current_pose[2]
- #robot.setSpeedSlider(old_speed_slider)
- # TODO: change magic -0.0938 with market_offset (you first need to check that it works)
path = path + np.array([0.0, 0.0, -0.0938])
# create a joint space trajectory based on the 3D path
joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 9f8bdff..7714927 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,169 +1,124 @@
-0.40276,0.80752
-0.41318,0.81052
-0.45336,0.82404
-0.51140,0.83305
-0.52777,0.83605
-0.53522,0.83605
-0.57540,0.83605
-0.58879,0.83605
-0.59772,0.83605
-0.63493,0.83154
-0.64535,0.83154
-0.65874,0.82854
-0.69297,0.81653
-0.70190,0.81353
-0.70637,0.81203
-0.73613,0.80152
-0.74208,0.79551
-0.74804,0.78951
-0.77334,0.77449
-0.77780,0.76849
-0.78376,0.76248
-0.79566,0.75497
-0.80757,0.74597
-0.80906,0.74146
-0.81650,0.73546
-0.83733,0.71444
-0.85222,0.68891
-0.85817,0.68291
-0.86115,0.67690
-0.86710,0.66639
-0.87007,0.65888
-0.88049,0.63336
-0.88198,0.63186
-0.88198,0.62285
-0.88347,0.62135
-0.88347,0.61985
-0.88347,0.61835
-0.88347,0.61685
-0.88347,0.61534
-0.88198,0.61384
-0.88049,0.61234
-0.86710,0.59883
-0.86115,0.59432
-0.85668,0.59132
-0.83882,0.57481
-0.83287,0.56880
-0.82543,0.56580
-0.80608,0.55078
-0.79566,0.54328
-0.77631,0.52977
-0.76738,0.52676
-0.76143,0.52076
-0.75101,0.51775
-0.74357,0.51475
-0.72125,0.50274
-0.71678,0.49974
-0.70785,0.49673
-0.68404,0.48172
-0.67362,0.47872
-0.67214,0.47722
-0.66172,0.47421
-0.65725,0.47121
-0.64832,0.47121
-0.64684,0.46971
-0.64237,0.46971
-0.64088,0.46971
-0.63642,0.46971
-0.63493,0.46971
-0.61856,0.46971
-0.61707,0.46971
-0.61261,0.46971
-0.60814,0.47271
-0.59475,0.47271
-0.58731,0.47421
-0.58284,0.47421
-0.55159,0.48022
-0.54415,0.48022
-0.51140,0.48472
-0.50099,0.48923
-0.49950,0.48923
-0.48313,0.49373
-0.48164,0.49523
-0.47717,0.49523
-0.47420,0.49673
-0.46824,0.49824
-0.46676,0.50124
-0.46527,0.50124
-0.45634,0.50724
-0.45336,0.50875
-0.45336,0.51025
-0.45187,0.51175
-0.44741,0.51775
-0.44592,0.51926
-0.44443,0.52076
-0.44146,0.52526
-0.43848,0.53127
-0.43699,0.53277
-0.43550,0.53727
-0.43253,0.54178
-0.43104,0.54328
-0.42955,0.54478
-0.42211,0.55529
-0.42062,0.55829
-0.41318,0.56880
-0.41020,0.57331
-0.40871,0.57631
-0.40574,0.58382
-0.40425,0.58682
-0.40276,0.58682
-0.40276,0.58832
-0.39532,0.60033
-0.39532,0.60183
-0.39234,0.60634
-0.38937,0.61384
-0.38937,0.61534
-0.38788,0.61985
-0.38788,0.62886
-0.38639,0.63036
-0.38639,0.63486
-0.38341,0.64387
-0.38341,0.64537
-0.38341,0.64687
-0.38193,0.64837
-0.38193,0.65288
-0.38193,0.65888
-0.37895,0.66789
-0.37895,0.66939
-0.37895,0.67240
-0.37895,0.68291
-0.37895,0.68741
-0.37597,0.69192
-0.37597,0.69642
-0.37597,0.70393
-0.37597,0.70843
-0.37597,0.70993
-0.37597,0.72044
-0.37597,0.72344
-0.37597,0.72495
-0.37597,0.73546
-0.37597,0.73996
-0.37597,0.74146
-0.38044,0.75798
-0.38044,0.76698
-0.38044,0.77149
-0.38193,0.77299
-0.38193,0.77749
-0.38193,0.77900
-0.38490,0.78951
-0.38490,0.79101
-0.38788,0.79551
-0.38937,0.80002
-0.38937,0.80302
-0.38937,0.80452
-0.39234,0.80902
-0.39383,0.81353
-0.39383,0.81503
-0.39830,0.82404
-0.39978,0.83004
-0.39978,0.83154
-0.40574,0.83605
-0.40723,0.83755
-0.40871,0.83905
-0.41020,0.83905
-0.41169,0.84205
-0.41318,0.84205
-0.41467,0.84205
-0.41616,0.84205
-0.41764,0.84205
-0.41764,0.84205
+0.27578,0.76759
+0.27727,0.76608
+0.28025,0.76458
+0.31307,0.73749
+0.33843,0.71793
+0.36976,0.69686
+0.38169,0.68933
+0.39064,0.68332
+0.41749,0.66676
+0.42346,0.66074
+0.42793,0.65623
+0.45478,0.63967
+0.45926,0.63817
+0.46522,0.63215
+0.49208,0.61560
+0.49655,0.61259
+0.50401,0.60657
+0.52191,0.59754
+0.52638,0.59603
+0.52788,0.59453
+0.53533,0.59152
+0.55920,0.58249
+0.56368,0.57948
+0.59053,0.56293
+0.59948,0.55992
+0.60992,0.55691
+0.62782,0.54938
+0.63826,0.54637
+0.63975,0.54487
+0.64572,0.53885
+0.66213,0.52982
+0.66362,0.52982
+0.66810,0.52681
+0.67257,0.52380
+0.67406,0.52229
+0.67555,0.52229
+0.68003,0.51628
+0.68152,0.51477
+0.68301,0.51327
+0.68749,0.50725
+0.68749,0.50574
+0.68898,0.50424
+0.68898,0.50273
+0.68898,0.50123
+0.68749,0.50123
+0.68301,0.49822
+0.65616,0.48919
+0.64721,0.48919
+0.61290,0.47865
+0.60544,0.47564
+0.59649,0.47564
+0.56219,0.46511
+0.52489,0.45909
+0.48313,0.45909
+0.47417,0.45608
+0.45777,0.45608
+0.44882,0.45307
+0.41152,0.44856
+0.39511,0.44856
+0.38169,0.44856
+0.34738,0.44254
+0.33694,0.44254
+0.32053,0.44254
+0.29219,0.44254
+0.27578,0.44254
+0.26534,0.44254
+0.21611,0.44254
+0.20567,0.44254
+0.19821,0.44254
+0.16390,0.44856
+0.16241,0.44856
+0.15943,0.45006
+0.14898,0.45006
+0.14749,0.45006
+0.14600,0.45157
+0.14451,0.45157
+0.14451,0.45307
+0.14451,0.45458
+0.14451,0.45608
+0.15048,0.45608
+0.15197,0.45759
+0.15793,0.46360
+0.16241,0.46511
+0.17434,0.47263
+0.18180,0.47564
+0.20269,0.48919
+0.21313,0.49220
+0.22208,0.49521
+0.26832,0.52229
+0.27876,0.52530
+0.28771,0.52831
+0.29965,0.53584
+0.32202,0.54637
+0.32948,0.54788
+0.33694,0.55390
+0.34589,0.55691
+0.37125,0.57196
+0.37572,0.57497
+0.38467,0.57647
+0.41003,0.59302
+0.41451,0.59453
+0.42495,0.60205
+0.44136,0.60958
+0.45180,0.61710
+0.45627,0.62011
+0.46522,0.62162
+0.49208,0.63817
+0.49804,0.64118
+0.50998,0.64870
+0.55025,0.67278
+0.56815,0.68181
+0.57710,0.68482
+0.61290,0.70137
+0.61738,0.70739
+0.62633,0.71040
+0.65318,0.72696
+0.66064,0.72846
+0.66511,0.73147
+0.67555,0.73448
+0.67705,0.73599
+0.67854,0.73599
+0.67854,0.73749
+0.68003,0.73749
+0.68003,0.73749
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index c10725690a79e90b18d13c2a9bb6da1853d04ae0..ccb51da838093a6f4a35c6013d4b2b91b781502e 100644
GIT binary patch
delta 66
zcmexl^u>rbpO=@5fq{YHT3~dl>PFrzyz(^+3=B04Sq#P2H4Isd3z!x%)H2pEWHD#4
W6sgxRWNp63%f!gYv-u(4L@oeXArcM%
delta 64
zcmexj^vQ@fpO=@5fq{WxX-8D5!baXLys}jc3=B04Sq#NiH4Isd3z!x%)H2pEWHD#4
U6seVLe!$Db$jG(%G2cWk05=8^m;e9(
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index b786893..0e39dc8 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -306,7 +306,7 @@ class RobotManager:
# TODO: make it work or remove it
#self.gripper_past_pos = self.gripper_pos
# this is pointless by itself
- self.gripper_pos = gripper.get_current_position()
+ self.gripper_pos = self.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).
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..0c65b52814ff442ca7d522cc8b831f8b20b5c347
GIT binary patch
literal 32768
zcmYc?2=nw+u+TGNU|?VnU|=}2Bs#Syw1(lX7z0CkesMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y-{y
zl9{8QoS2iD6rYrzSd<c<k(iuatXEJ8v2K(c4S~@RfQ3M5Nt&(&FN3j>p#expSxHer
zSSS?49L1v{Fd71*Aut*OqaiRF0;3@?8UmvsFd70QBm_zd*cj>=7#NtK{?&)ljA%3)
zly3>8b)Ym%9Sf8n4yBW!G)$fq$`69lyPz~oo*l~Pgwldg2`I(D3>7zq(!x*)C<PTA
zrA9+wGz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtr!nu!TTk3IoGKb_Rx>9FX~c
zSpWYxKLf)eeg=kx{0t18{0t0b{0t0P{0t0n{0t0Q{0t1A`4|}X^D!{2<6~f0%g4a5
zl8=F5As++7R6YiVI6ejjJw678N4yLS3waqBGI$vnoOu}-e(^9ceB)tY_{zh;@R5gs
z;UNzL!v!7&hOImd3^_au46-~742(Ps42QTG7@D{l80xqg7~;4Y7_7J%805Ga7%p)!
zFf?*8FgS8CFnr-;U^v9dz_6Z^fuV<!fgzQXfkBQF;tx$u28R0_3=GFP7#LP@Ffh#J
zU|^WT!N4$sgMp!vgMlH8gMmSfgMr}%I|IW$b_RwXb_NC;b_NCub_RyOYzz$V*%%m3
zvN13$V`E_GWMg3PWn*BFVPjyp&C0;Al$C*@g_VIJj+KGIf|Y?moRxv$8w&%&1r`Q|
zc`OVJaV!iBhAa#WUzr&g)-W?L)G#wJR5LR$L@+Zj*fTRQ7%?+27&0?3=rS`f2s1M<
zd}U%_SkA=2P{hQ*5YNQGpvA<%@Pv_p;Q=E9!!AY!hBb%*4h0Y>D9X$$(NI!SVn|6%
zQ^-inOUX$MElw>8$@c)WL!1Kw@{3b7it>~4OEj$*Kq^i26pB)FQWJ|)6-qKv71D}Q
zQ&WmE%Tg5*^HM+}c?!iPiA5y}DfyXs=^z0G@S=*M#FEVXJcWXy{N&W)Vg_S9h0@|w
zh2s3&RE6x+O7Mych0MHy(h`N}Nd3H6g~VcoM1`cB{N!wqG0BNJIXVnRdQh|BmNF<P
zl;kTU<(FhA<mHzrBo(D5W-F9rWagz8gWZ#uQ=G4mm|Rktn3GefP+XdvoSK@#V5p~1
zlA4nPc3f$3YLP;DMq&xbpp<-t^30M91#q|&>nP+U=BDOUDwJoW7NvsZLCyntIyEmv
zH#IFSHMu0eh(Q-0V89uu;J{=6#X?bPNoi4@LJ*h(wpv3gNJpVKwZK+gCnir_6G=k7
zD8ED@7vkEy)N+NiqQu-(D|Jl<u$mwRTZOy=y~LzqjUY`0EfBjnGd)iuC_XbUvm`Sy
zCqA{fBr`X$Bo$(&vO;-AW^#r?d8$HsYKcNxQGPBsP@t+6V5$`~AnKA6^C002iu=S8
zh5WQMm_d2@<qD~J#id253W?C51zBCJkO&I8j8p{(uLKm*mHDL#<%xMEI>?UBC@x7%
z&eqTf(ov|2&r2=WD@rZSNGwRz&@k39)YR03SOoPUOmPJkl{yNQSVW=vl@%%#GK&?`
z^Yc>_)RXgz6*5Z{$`gwf@{>vuGxJhYAn~85kX)2sTnu)eGT1fIMzM&ng^7TISl19)
zl_7>I!&r#Jp~e_sC@_F20J|E+9+(KSJrFTudtf4%s*o*%sDfLjke`#Hker{A3d&KH
z#R}jQUYb{unWK=FSpf>7f};Gg)I3nCFV8Q^hIkT`TA>(}+p|iGOBB-bi;`0jc?_Op
zf<Rg`63akICAByeYJje;okC_xYN8b+`)8)-6+@C8D9I(}C<MXN98^Pyzl*<>LRw~C
zibA47QEFmweqLfy4yfeFDFv4(CHbHXUkpxPpyDMdCp8zU7vjFm3UHc+q`%CRWUwSS
zI3O_!j%p2XN`fXmNQ{9+AW9O8iV`a|q7C&7bQBDr1T@;fX#u1PT!bd)=M|R}l_r<C
zf%2tWQGTvpeo<~>P8hhTz>*Jj6!M_uYJQPIc~NFbszOSBd7eUfMx_EMo22CDsh23^
zrKYBUQmRI#dago7eqL&ENlqm=Cl-S<TuH8uLQ-i7NJ(a~LJl~8g8~j5m!JYGEit)7
z0iO53;gVQfT$&4x9EA!6&|*1;l8nq^P|WA2DJ18Y78R!|B<F+H&VlSoEK15ODM~D=
z)KMtTXDClqz-|UedqHARVopwK4kRmqY%c;;AL_+0d%%HInp~og2r0f45{vRnA(e+h
z8km&_4m5=_aH!!gV8Mki1K3EM;f_(%=7G}yC?!LRTP0;Ac3y#APG(+WPP$%ZUYSNg
zesLx!ofYeaC^#sDD>x{?MKqOkKqV}ZiZvOKErA<?X{?eC!U{}ToW`JP!=VgW5vr1K
zB^`xuNWu>XCFcUYl+46*4Nz@Tl$e*Es$r<3kdvB+2oFsKZ3RPksj6)V%I=VwA6$hR
zDrgu$D&~~@)M8K~&P~itRVYr)D~2=`GK&>JBE<^f3dNau>7_Y|MG)(i6*LlaO7ct7
zGeGG%IlnX~MZY)$L@6YtDx`u6rQCc_HIrVHT3ifjc@z}oCnYB3RKoNp6)TjdW~OJ9
z6zf3JX-Pg*09+D)>P=`H0b)>Iib76iUTR_yIQc>i1J@CdILm_s2sEN|ic5+M5#az%
z1E4Iaff_uTItoR}`FSa}e))N+n$ZTa49W_5`FXk!m!+mCfZ9+c3dMz`iA9jo5uPR>
z)<Fs#xS%GufJw_NK`BuX(va4fE~tP7<+;4n^3)=Qg8a<95=b4KnWq3Q1;AcT&MzuT
zg%k;S;Hm(kP>}&t=0F2GC0`*C8f>5@SbiEP@QPDmW@hGr_$a<8W&kz962bAA11df8
zN<iff*lZmJP>lgDf|4^p391;Z9keVLu0}y4H@~<<A-yQGpdhsf;_O678xh<TPyn@i
zGEx;%^HOveK;@ei$g1R2P)Vr{N)e#eURq`a$hn{}$S=xF%`1U4JR#19nGI@Qg1nlZ
zufvd>lUQ7wnT+Zag|y5PJ&clv0WJ6;1q-NE90YE|6)OZOB&8-Nf||A<uc{Y=o0jUu
z;1WDF1(ZkNMHSfp8K4$dQDR<knjWZGr^k?~m#PPrK`8@48bLlP$Vtpg)nf?BD1`)H
zep*^_DyXfUS`4lZauZ8RQj2sz4Z8e1P!$2HI6xW`^U`xtVS!eXT3iBd9)mKV0=NnR
zt4Yc)1GfiCQgc)DN<f(bmS~{qMGqW_pnxj@xd5qhD#?e|(RrXoA;?V{AZ3vHKVKmm
z)I7{AQOL|wNK7e9%u7zy)MJRuF9nAS$a6>~N1{SWet|+xYFddxa(+=>YLP;InnGrY
z4udPS?W&Nd01gL*q)Jc`0*dY2(wvga0z{fChEzV8C3*_Tk;ssqnwMIXm;;Ho%-n+f
zq7sFYjH1-Ul+3(z24zsA1RO~zsfp0~3*OaGFILFRO9M4iQbDceoK%IJ{QLq2aGM!p
znYkWlHMt&KuL9I6xBxC~xrup+>7b+!)er=#Iej653{bJ;{DMjtyCgF=6~@jh%`K<|
zCG5Ncm{37xUVd_NMrJ-pxB#@OUKtc(si1;2CqF+MT*iSS1r+@ysYSV<dJj}DKr$CN
zd4OxkVugaD)RL0S)FK6qqSRbaI*9@41GOM^z)1<z(_#Si|2x?k7z&~Nf7tx`Lw*K^
zUHl9Tv-ueqYWNu#V)+>u?D!cN6!{q#82A|&p7Aj-oaJL+*uuxaFq@Bop@fftL6?t#
z;Ri1R!y#S<hB>?p47t1v3^}|E4B@;C4C1^D45xV*7$);DFihfMV2I*jU{K~^VBq3m
zV7SQ5z_5&)fuWt7fgypLfx(-bfkBs>f#C-i1H)%728O*{3=EUF7#PC27#K{s7#K{r
z7#JkE7#RL>GBDibWMDYM$-uCmlYt?JlYzmIlYv1J+W()z0r6J>$ZyyNKSsqyLtr!n
zMnhmU1V%$(Gz91q0-$a?Y<M{}0Ms~!j8*F><i&$pxy7(9p#n7qtwFXaE9in+iI83i
z(IeL|osecOq8SJgCu<NJqE(lgBiRrmA#FeK2njS06>Jp@4Zs4*3LsfX8?pp6T$Grn
zkXV#n3hFO_6@Z6&6l@ic8?G9f44G*P@$sP1^!RuMTU!OC`1st!%)Iz`B`e6lHEal6
zN1+7N8Oj0o*>n^N!Q<9ogTc*E4Rug6QwQ8XLQ)7FC{oBQwo*qi0TTOQH|Z6pmIN21
zrlthvWTvDRY2@dm#2173@x@>Ph*D(**bqNhP5~mPkd&C54eF(2mVg_=WtsV<#R_GK
zIi-+}JCXyH%0WFRu=>&h9fjh=GElD%6jFttHaMvJkeQgHP^bj455$izRIpV5HR|<J
zQd0{+42?pFEJy<4`1I70Kn;i>M4+f71-_?M57Zv_(a1|Jk1xnCh7Rq+c#r{QustBj
zXroxLYhltDRwtJh6{Y5tAen?LqF}3lWPed=a%yH-s$P0(i6eN91vXy<aj7!6V*u*;
zrRPI}u(SYdxw1lj5vVT@9RbMzbtg(PQgam&%Mvre6EUFv2yzmFY6B%cEAUiBQDzFL
zTL_wJ0Qo)#Hquj>Us|L98bB>BDNRe$fut@-+EGf+FF{UT&{U(NkXfQ$Y^8)H;UFh5
z^eBP^LrP*vqF%mlW;BHB<?rbi67T5}tB0f&5*P)Ud3tI2MdgV_Dc+fRskw<InaRbF
z!b&eUKP5Fs2O6><Bf(=zknx{F=nxx5S^x(L#9EBR5DFT|cg_b7zi5C{GQ<&Jrh+Xf
z1i^X1GtVUxG!B%RpXZ<Erx65E1r4rD^_(2|XgsbgmzQ4-P0Gp&Y57Hvq4@Gt@bnHS
z$E4>gq=2o+sZ_|!1CQ0GDu8GE!0uO80F76Hl1OEKDQE}>)D^D;Y0OPcgbz$01tv7o
zKqCVnhv!s+jlmHNIP-)lXcnfZpb|YdM4P}Q3{kU5v@uM=03uOTP^n-G8eR{`%qtEG
zh}0-5fDS)GWI)kfR8Xmxn^;nmSrL*SR8R>EUS-HoeVzhn91>}W3hWbDz9f)sqrnqE
zh+K{CT=2{fLK^C5Wd&G9ghoVhVs5HJaY23w*ltoo5j-V^u*4`9-2Vqv{1u>iW&s8U
z*!uld{0s~M{0t0g{0s~a`4|{hL+AB5_!t;=@-i??;ALQN=Vf3J;bmaB$HTyIm4|`h
zC=UZeF%JWS2M+^-5f1~yb8ZHP!`uuEo46Sm=5sSJ6mc^!#Beh(2yrtoT!Z%OS938i
z%;jQWu;F50Q0Id5>lwHh7*=pHFtl<qFw}4|Fl2EuFlcZxFnr@+VA#aLz%ZGEfgzrQ
zfx(1>fq|EUf#EJY1H)l<28NaF3=E0Tx%}^J3=AvS7#M=t7#LL87#P^t7#LXD7#Lo&
zGBBKAWnehY%D^y(m4U&Mm4QKqm4QK;m4V?q3j@O=76yigEDQ`=Sr`~*vM?~rU}0eB
zU}0d$Wno~@Vqsu-%FMuUhnaz4BQpa-88ZVzGBX2%88ZXJEhYwr156AI>zEiA7BDd|
z*hA;?K^rUnGcqvTXJlYl&&a?44rc}i24#gr$WRWXeNhe@EC-L=7b}z&ftoDE3ZM)M
zD(pbHG%sBbqzyEv1*y(6!Sl))kdg^JT@D@e12=Ut^2<T$K?Sgan`eYVh=*seLa4h>
zq=JW|v$rA`KnhzdsVFrs#SOMhz&Af76<U*lnj0Vt8UxPED}~ghAOR4D7DC|}i6yCJ
zsYRe6UWJm%0?<5I9%y7TFEz6yBeh6DJyKmEFTY4ZJx^Uv!5uUk0I5Mh3A>~y5i|>+
zkerd4nWhLWr$C0M<`kz|fzv#Q2g2Z$11KUTnYpQY#W|^|1sX<>auK8sgrQDz2AvfF
zUDlA8lwS(2xWEyhr{Dpa_XO8Pp!t7=e8^aNQf9hBQEG}pF=&7@skEddKM&PanQ01%
zdBvcCds|xt^-6VcJ&6#AR7Z7mYDH!VT3ABe7LoxPq|DDr1<i=27bWH@lxKoU^9s<2
zsC#~XN>XJivel>-=b>Bd2env92^xrwMc_eq(1dIXxVQ%U8?n+OFCFA`&}@K4UcN$Z
zYGPio0?4(R_K@Wr;E?uINX%6LPeX!+a6!Yx;2Ih-zz<(v0UBlmH3wj+5!8y&Q;5j}
z8v%+AScpNEkAPRRfP~94GILTDLW)XLp>0fft2-Vt;;WaHpP2^@K4k?EM<F>QGY7PY
z1yl<uXcU(u=A|SSr6?2>B_@|-f?8H!OF_{W51NN60W~V2CV<zjXeb4M=A;x{^GZ^S
zK(!c1sh*ym5_EkAh7-WS1~ma)Qo$B6G_4l`9;DVNNi0fFEwP0wg$TQq<|TvXD0M(>
z@nTy@t3n5y%pmU6fi@zP6@tNQbKv?xftv_gX_8o!SOS_12KgM4k3mj{=tgmfYguX@
zEYT?|I2WaY^d*Ar0?*BrKy-s*0ECfpk%69pfhpFUR0wK)X@RF2LG$c|ntI>~VGR?g
zvq1)dFf?(O7MY@IKq_)@H%W0B3NjdkwXjv$5R*YoGH@%;ELN{5A2OkY%Q&bT3o`S-
zZp1VIsWAihrj9}kIAwv{0m2YNF!Z7Y9)&GRkVPO|sFzq!keZjGVW4M#Y6Bh_0v!QZ
zX9Z+FBqM^#0z{J1%PfvBPA$>UL=<Qc-=Z{S;mH)K(f}<~gNvgkf6R0bHC`Dq3k;dA
z0#8Y0mVj5hK)eg-)LKERL2$1YJlRnRn(9bNP0P$fUgefw1g;Ok^T#?0nJLMTMNE*s
zPD#E(QE47%b_F!im6s2$p&+FVtTa#n%`1X}MH#FNG&2ZV5SLg~37IV~$yWfo5bj4%
zw=ppfG4}+Tb%NN5l((QY6uf-KQVbzh>gj>gS^%iRLN9Bf?Ltufgf;;OuFN3QZlH<W
zT+o6XP_c~EfQ6U~F8e?QI@||(3SbK%i5fIn3i3V-56;RT<~n$QQW7Gd${3uwA(b$w
zx04F4AmAaS0O|qiDd39|a1g?rg&CBoIjB`QIc*2HXCV{xkmX^ZX>8EccnY`$23jTt
ziDr<e$}@1ZFr2|N{NO$#Y;*{e{&jWp@^#_Eb)Xgk0bil|31_ne6fEeifS}YwNXrDp
zzPx+|)CLKxeFSTmK-)KvG7Hjp0GSRCn(&O&Ja9;)WP(;?>44@%i;F?6P#ute3c%IB
zdY(GSyPyUSB(Ojk1$oUVbVVt+EnZw&0BX{Jr=62h^<wfMxdjxyL?jx8V$kxp#1gQZ
zK+7|86Z0U8T?<Mfy*gL~6obY~Kn^J=$_KXyK@F~=#5}l-Ai2EK+$6|^JE(9e2Af)3
zkeZyCmYJHOpplsd76C2k1eNEB<scS#WkqJO4yc(88YattG~_`ZK=m`I1qvDu01eM*
zfc+lg9H3C8P>^4hSpsUgLq-ZR^Yb*pBS$6qrO6rKL<BBv!D$7u4mA_9t^({BaKjd~
zdKI){I8PzJv;;JS2cDJ(m*uedhp#?PDJ@EcL`!LLY6_@$1^FD*j|Gj)!PhT>y_Kqf
zRKDpzV#XJ|jtR8x4x|rB9@H&?uTZRn4N*WWNGyS5{-jihA&@1-ph1tE9MG~R$ZF~0
z#7t206SRC8-oynLyx;&R#!*5*SH++gQNjMcuHhbzAqtL8{$Z~0*ar=gL)-xGTY+Zr
z!C_gHm<JxYfkb>sKD<E>Y8s|OS6$?TRwrgAXMlBpLq7?$Xc4r40yO_G#mK-=#{^mb
z58I!AmY;!P5<de&IX?q~BR>NJ6F&pP3qA&hi+l_WYxo!#rtmQ^<nS>tMDZ~&MDj5(
zSn)A1sPHi`+~s9p*v`wq(96rfP|3@{ki^Tt5Xj5G5Wvg8;K|Ftz{bnK@BzB!e<lwD
zLo#$7zyoduhK1Y=429eb4EEd%45Hi&4BxpRYyMY)#sjz*7?Qad7_7M%7_7J$7*x3!
z7~VtI|HpGOFvM{(FxYW2FtBqnFkIkZU^vggz_5pdfnh!e149l614A$e0|P$?1H(CX
z28L#K1_mQ`1_ncR28LH`3=HSl7#RB47#O127#O747#J?FGBB)UWnf5$uJQlH!oYBr
zg@K`&g@M6>g@J*Sg@NHUbPS*$x~^Z2nStRi69dB?CI*JJObiSYm>^*)#l*lM$;80$
zjgf)jD<cEL9!3U+iHr;k4WRKB21v^rF~|cQZG{af(Q2p{oG+m@5@>x)aYkwiY}^pk
z4hVwven15&aU+l*m7ta0@U>EI#4M);=>p-*G=)qBI|Tzo+frGfI6JcdT$H3`78RE$
z<mW*ODUfmy2A8yYD2-3hurm4pB1kC+!-6y#eIOAe3Bqs%=mUx%Nf3rBKp$8HNrEt}
zQ3_pCUy_kpoT^Y%P^k`CHmLwvNC~RU%2P9oQWQ#xGSd}uK?@@D(m^fD(gH~17GxF(
z7vUVR1j&LhL<P295U%Y5AYCA=3>pYUE-ArD0we>%<c2NA5T}AJXt`}sVqQvqt{#Na
z04;h4w@TwPz$-pcY=#Fi#&D>DHa?@uGgC@3P)t$=4Vi%#M}sDy6cY0kO4D;HAq420
zP&CD$VKU_Y1gNr1N-R#*MXoJUQbE10RM<8MP*8y|iNOU)C$PQ?D3O2@Zm~jfPJTIP
zDPT!PY9(l7y*v>#P?N8amRJmF7=SDTVR$HEj^~1;VHjc&vai5HHwB=92T*{(+8NMx
zP$p;rVF`Se2Q7eL<ItrA3L0QzKm|ofet|By*O6JQkdc|50a~S)nU@S&nv0s2VJ;@3
zM1#4W>`5$;0pJ3~1~#RvplgUIS>Wq$VV!8u{2gK<4-%Qk;|ECHe@*D95XcH>`APku
z4Jr(4K;1_EkOrtsp=9n06hIjBUm!sch6FK2(T!^g3_J!78ukK@jcY(>xS<0D@Yqza
zRfvv-*Qco^3WYifDfyrQbkGEzLN<H`Pe%dN^n|7^1yFl72-3JBRMCNFwz2KVO3w$m
z4%D;+nN|gzKFiF{gE$AXEXK$MkPv|<ItUjrH3=WV0_RC+$(C49oS#!#0^04Ckys4s
zy?`2Zkojm((?2mg5j;n&3$Kz>Q^5L3j9XkY+hDVB=4(*n9BD=x&c)$%aALqS5(o)N
z$l?MVMnERqA;k`82p`|RNU+->p^Uc3z(CIgVg&kX1NiV6IGI3~Ab`zLhA$vD&@%z`
zC_pIzyp|i@wMYduU%=y6P#u{HkfE()P<IT{Ljs!%TfPA5>4657bii8(U>zdV^)ir3
z6kPM^KpOhc8WpjW#)<(}2Nb0i<RpT2edXjRfT|nh%m!V04eqvpdV1jFw?XSez-u}f
zV6&gaC8>!i3Pt(J*$NQfLB~}=tKf?=lT#t<+d-99W{CoHM?z96XbuWAV_X97hJ*J<
zfd=E@OUWUN??DUMOA9hnKy4-+24zt92fR%b<lG!^_YxdLSs)Writ-D<yB3Q}z*<U+
zz+)efh0P%2aC-(c76y)b-Nf{~{Nj?#WJQn{Q$ds6sYQ9vd9=LZ%;J*Nyb?u7GXmUc
z0yiYUE36^iRM1|$-24>KJ{XXDK}`rqI)$!{0Xr8Q;b4}6tpbGBN36}s%S=vH01ZBX
z>VB+B5qq0J+Y6AF20#~vL(DAB1ud)q&D?<Z6M#1{pqK|AtB<aV&rgdl$}fpFhz0GQ
zgK`X!IYzNM3ZRX1ItsAuhcHbFwhBSfRyszp;DnhDU7>@qDgioy0}Y7MqWGc$$h<$O
z{D*L(!A(2_TUWsdG@($C33hg7ngT+~4zz_2QiDSTF^YhK%)DZd9gw`M46Xl)3M!R!
zK*<TV6Q=?;_YYSO$|2x=czNU{ZwAPE2<oM724w|s;szBWq$lqDA|1$juf&`ZP+|rR
z(&SWvYeJBLuo*dU&5@a(rvsk<U%<w|umE%bp8x{`?0i4azW)|}28I%T1_m#F1_o<>
z1_oJv28PRg3=F&Z7#Qa9F)&QzV_=xT$H36T$H0)w$H3sl$G{-P$G~uhmw{muF9SmZ
zF9U-CF9U-PFJ!;}V;%;Ey*vyIvv?R7YIztK9C;WR1b7%2&T}&`%;aWZsNiN`2;yd7
z(BWoa;O1svc*VuQa0I&cKM30Af62+fuz-_+p@5TtA)k|h!H1K9!I+bQL5-7vL6wt%
zK@mCz@REapVKxT?LmIU2FUP^aaD$zJ;R-tg!#Z}Te?bc$M)7C}jE2By2#kinXb23#
z5CFB8psktFGhaX_k(A_vy7k}<>)>!1J@W-Na-TYS<_lyEE~u-MRIC9$=OfxE7Bt{I
zdgjaMnJ?v_LmI%{R``e*QIk}OC8?ulzT~D3f-_%|Q}Rnd=b<P#DC9wU`k=Al0?_Cy
zd>j}uOaQJLG?X&ICtxU~<>%z&mxCrO(m{(BK!a;VpwminD!~(s;5GGm;B^$BttASe
z)00wDpzBJ&Cy6NNgC?J#r-^{aVW9RaL!@;Tpl3=!I^Uq9fD&^RGQkHtK-LL>M~o8F
z6EpMd!Q&gClSM$s3}se9rZr(h7I{iK3R-!vqr{*GiGeL4`ivXo18?A`(c~5AVLR~z
z-2ZRqV_-PW2buqeoxgvTpMjyDpMfEfpMfEPpMk-apMk-KpMimypMl{9O#L8iFO52B
zGz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtt2k0BD$ea4(|<xe&5cgV13PAeA6Y
c`f_UMNELX10b~LQgT@Zf&cH;RgsRB^09k)~C;$Ke
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc
index 984653c2a30827fec2c4c0d25253583930002d57..18ef112aef1717a31bc5af78a2f9a5da0aa64059 100644
GIT binary patch
delta 2889
zcmaE&z2As0pO=@5fq{YH%#!HTN|T9vGK_O3YG<3J@TBmzFhudE@TKs#FhmKY3Z@8T
zGZp!yvNtnC38e@|38xCA3Z)7+GchuxFb0FMrqIMa2K6iq3=HfH3=GZ;3=G9>3=9k<
z3=0@*7#1?tf>=x?3=5c37)w}c7@8SV7{wVDGA(4RWh`OHVqL&i!;r;Z!zjQ|!yv%0
zkP*ZaU;yz;7#46qbSz{PV5nuPVFIh?tYKQnD9upIRKk_Q)Xd1pP{ZKDAQQ`3%Ur^h
z!d$~#!z91}a$li&4O1|KCX3(8$r+5EG2FM9@(OOTCKeSXR^DPs$xKYY#g<r<n3tY<
zi#aDX?-pB5W?o`W`Yq<nys}%2A-7oa^HPg%vE=0!<=$c~D9X$$(PX*BS&(0xS(2Hb
zSA2^x{1#*0EyjYC3`L@oe>1j7D}#J4z`(%3#VEz5$0oqa!N|cVz{tW_BtCft(`H@?
z1_lOArkBhN3=BmqlOvde83iV<VV2SmW?*2r#ab1gms);{wE{v_f+@zJB2kcPFd;Vi
z9ka5m3&;{71_lNeMm|O!Mk!tqMiFKXCO(igs+;v#l3A?GgHjVyDiuod6_OKkGLwoD
zOHvi`ixl$m6*Thl6>?J(^NJNxD>6$I3X1a6ixP7+?G>UU_48tjgg0;HP-143n0$kK
zKBK_o3Z4QDu+3mX4CG4jw4&71l%mYC)R2s#)Wj6G(!69)XiS_e!24Y|FTVuM3eCw<
ze036_FuBE=nO9I+a*G9IbCEsBR}Pam@EI`bO@7EHVd4lfoh>o1xIDF}2;|2iCy<B$
z+`jk{u!Sh$q02AFm_FHu-?u)9fq@~J5tN>xn2CXb0Tj^S)M$X58X0RCQy3)~Qkb$?
zio~F7<`SkXW)TJn1`ywdp@tEhY>Gez7J*`|ND~xwEVo#a^9w3(aipZC7JyhqY#=u>
z6=c?HGTq`R$jmE_FDj_K#a3DbqLG~nQpc5+SrL$#R~!@&c@z|h9H5lK#KR~Cin<7N
z4}csGQUUgW9|HqJ4MPe;3PUSX3S%0RBm*exEC6|hNs^(4u|9<vL_z$O!j!_4&bE*d
znqQa}FvIdo4O0pWDBpmy1*;#}R}fEvGVe=Jd7#Mx_EZrlCl_&pVu>H*b><=!kQX?r
z;`7tui}Fiuu@)DA5&}47h%!VoFcg8)hgf=QNiaypGtVWnC>5MG{L}o-fcy(GUVu@F
zQGkhqiG{H!2IOOqz0eqA0A&NXkF%ITKF(sOWtn`CU%sBTgn0o=4MP^&0``TVB83gi
z2YHgchG7BgLWWw75{?C&H4Ir?H4ItYH5?0>85zK87_zvtcxo83cqJLK_!ctOa+dJd
za5gj6a-}oWa+e6yFgG*0FvK3I1*s8SAXLM(5L}?3npqDrv_yD;NDaqAMzGPMDNO00
zl8QTv2Nd>MVhhAmSQauaVl3gvl2{-KQV*5kTgV8L3u5SFU}Pu}O<_%E1e*r;5UPqA
z?gdf^$EPq$)-z;@Eo20{jHiY<g-wDXo1y3nEIev?!HRhoGS;%!u%)m|GSslAu-h=y
zFk}fXkgj0|>tjpdkYp$k&XQRmvyh>NQG`Jpq>96ap@uDm4M~OxoDJAh*pT(tGcrK^
z#kr8Nmajy%hOe2imam2nWM~b03a1T24Ijvu8uk<xFdO0sE@VR(8ESY^II>wLFcz_u
z$klK+GuHB#$k%Yz@HaEn3Y75I2!Nuc(5XZ;g&R_;L!!Hux1=5v`$YJ!hCK!DqZ)`G
zkirA%1p(5%P>%@6644YMkn3RX1qU*YWCKbC;CP0m8jv@T6QtNe##+G=(G=bqK~RN-
zGeYY@afKeEuyhYHlw%=dtx$<*3LnU6HK2-@1r+NgqAC1PqXlXNYgp5mgBdgh{L*L5
z1c96OMcSYot_31=K!h%c0Od)fqCt{@fkFEvD+2>Vm1tUKUS@GdYKlT?aY<%wVo7FR
zx<XKuMy7gB4ooO@@*5!u@4Wo-D%te>5`~hY#Ju91L{J@}P?nlpl3%2wkXfQ$Y*nRR
zo|;#pkd&C5tx#H^qfne!mYSkal95@gP^gettdN<PS(2HUqfj{csgN_cd@l+Hxh@1$
z4zZ-CmIM|Vg7|hI!URN^f(SDZVGbfJKngj${5}0b;yqo8Y(Zk6GUpa6xXv%q1IY{I
z=9i_0=9OgTIOpe;Bqo<2m42XFtH=_h%?eb#6j_6qHXy<nq?Hw<)#nylQDR<7er^${
z051{;$*O`1OyMGMjmBM&lbDwpU!Iv#l5vX%!pTU@OwTBp{8-qcz6eyh7b$>rMS}=I
zkim?Zw|J6Ei;7b7O5zLhi&G&MgGxtmAq;lXEjCd2#22F1F?{eEKLBL$T~N8(!@$76
z#>~aY!N|ubz{JJK!6d{e#>BzQ!d%b8$iv9ND8VSjD8<O}Ux`VIiHDJkk&BUsnTJV=
zS&ET|k%x(ck%tMSj)#$piGvYj3{wLK6BiQ~L>wXqQUy{CGLM-9L^4S*ih;~x<pPUC
zbaF9+OfM<`d6OeP-XJ$IGcP{AC=wK6BDXjTGV}72lQS~&i$D>2i#4w_x1bUdBj5%@
z@MKw0pL$PFNWnw82pl)y&<DjX-0Zx>+|>AZa4ZyMfOJ5sDh38fn1QP+4x8Nkl+v73
zJ5Wtu49Wr=%%Daa$Ok-(98641OgtQ1OcETNOi~=29B~}%9D-aNX>5!vOnmGt-vGKZ
BmskJ*
delta 2007
zcmdmQ^hBF4pO=@5fq{V`u{A1njpjr?8OEN8+SzKU{3$%yOhqoK?9B{O0x7&vf~kC|
z0;z({OpFXEjKN^6$v5$gK|M1A0|Ofa1A{XI14D5f0|P?|!ve+{hJ}o^3?&R%Oj*n|
zAQ6@l)*6Op#)XV23@MDg%qdKg47H3kj35;Y*lHLTGD<VlGM2EXFgG(YGSo1*FvN1!
zGL^8Wu+%WsFbXh$tSwZoVGL%_WcAZzyv16SoS&Cc#K^!fc^0Fa7soB8yn<V-iA6<;
zmABY(GV>C1(r>Zm6qghi-eSqmOD(>|oRgY&izP3=DEAg?K~ZL2i6-+c&Vu~n%#zIf
zyy9DodAAq~Rx%U`PPSock(2{@g^z)Ofs0XrQGk_$k%Liyk%h5Hc=Bzg&AcKE3=En~
zFG2o($vAm7b1)<C<oC={jQo?iSY)^b85kIfz=Y6bLl$LO6OdU#3=9k`jC_ndj8eQJ
zj3Ue&One~olr|T#B(ta}2BjvZR4SC@D<mi8WF{3QmZU1=7b&DxWR}<~L`Ul9#TM~z
z=HpaiW)zuh$TOdjck)G^0u+mUcx4zHCuj107f#7nsLU@_C{N5Q0T~D~V{$QHodhof
z1H&!W%)Elql3Oew(IOj=Lv1H>@*4zcgCqq~^HSW3Qd3ikGRsnZ^HWkaS&QsI5^RZi
z#pS6*MUEg1_8<{~G?=>hl8mC%#1x2!A-)DFkeuAlE6Er>c`3iIhCc%XLoyR6i9xXd
z0|NsGEWxZ{VqmCY$YQ8vntV!7zMi>;A&YGR`$C3VmJ;R#EHx|(8EaY78B&-QG1juB
zFxN8HFf3qQ$WY5(!m)s}h9Qfqh9Qf)hJ7J3BLi4HLl$=yPYpvBuOvej-$KS(j&z1v
z&Jz9_#%4wrhS-<2AoT(Z1Z&t9f|FSm+X5kwX*KK%gi}}+GS)LPz?h5-$X1kyED)_>
zU&siyNGydloe^Yl7EcPBBtw??0*Ms%g^Y_BOE|J57f2z=@GWG7$pta=F)%Weh^26(
zGlETndK5)PJp&^{4Oa?hHdE0e4F8BPWUS?`;anh$kbzjgkP++~t{TP^E(wNgh6#*C
zitupc0juT#g<}m%3b!Of4QmRw4Jgb77D(5yf_1W_a7r?ih-AssFOXTtkisLvAP!Q-
zX~R&%lEMX%0?Y7<Fi3!8xKIs%`kP}RV=ZrqSPEYaFDSp(@Pdp@VaaBhz*xiy<#CsY
zWXXa};Y|UV4^~sdn!<0xP{W&2&jofLJD3A;s{oo+ay6XIjJ13v@--Yae9cU?{DqDr
z`IR9H<Zl+Rzd;2R6NptJmLgcg-OL1vfBqWo8s;?SU<OSgzw}u%LExsnCL5@%VE_|Q
z!JYfTJOuF)RE`#bN{1p1P*za~5t<-E3q&x22r&?$#=yXkZVU#O?V+Zfv@fz?VBlsb
z5@lduc*)AZz%coPsB=9i^W0)d&M&CE#gUSlS^#FTq^Fhy7U_YEvIY@`Ai@Yl7=s8<
ziBn_>%7z?X{+@mz@t!V4Rv<2GQDR<tY7r#2=jNBChUS%I<~Zl)l_VyY6zPNX8h{8;
zX<oz*V%dO-jUqD;(;P%tfCx*lSs?vBx7dmj^HTD2Z!r~Q772qSl|Y4~0Jz|o%pz`4
ze~Y!a092+pfFxx>1gKoS#h3{;^%hTZX;D#XUP*jGesL-&1Q?3=L5dVX1h_l_6QJ_*
z79YHH49H2$OI-&lSkpk24>K1d2O}RN7ZVpF2MY@$2bU0|7!wN<4<ip_Jr^SgN-#<>
zN-=W$S7PE|5o45M;scBFF!L~h#CaHbn7A1E7{wTQz?yg%IT(4s#(|9BVdP>4nNgGq
z@;66(d~RZ9UVMB}0LZ68w>S$j^YW9EGcxmwK=E*kHLo<cpt1-YhBgc=3=Bmfld~jz
z0^C48f(KC%C}QCz<|XE)#>ayLttb|x4(w?J0rozJO>TZlX-=vgC?OSVOcsz76J=%M
X<=|wJ;^5>6=LlqDWMSfCXZZ#Ihu+FD
diff --git a/python/ur_simple_control/util/__pycache__/freedrive.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/freedrive.cpython-310.pyc
index c2ca7a5fb770db91952c5fc6e1ccd4911a035ba4..cee8b4de6ae0da7de2eac77dc079fcd1ff3c6da2 100644
GIT binary patch
literal 1273
zcmd1j<>g{vU|`VgicbB(!octt#6iX^3=9ko3=9m#Z43+yDGVu$ISf&ZV45k48BDW8
zv4CmTDAp8)6s8=uT=pn-Mvz+O9FAPhC{9L(6qXd$7KSLU6t)!h7KSM9RGt)$Y^EZ|
zRL*9GDBe`|RNiLBD85wI1+1y8S^TM73z--hQn^$4ni&}xQn^wXQy80>7#UI+gTYvn
z^CifIewvK8c!Khi@=JUZ^Agiji!>Q;aiph~#3vS|7bi1;lt3|v4Z_YKf5<Q}FqAN4
zF)m<Q$iT=@!koegvZaKjnE~YHU<OSlzbc88{Jd0!^30M9g|wp7)RdykvQ!;~<jO=%
z)?2JaAS-Y26qTf;#wX|Jl@#UY+!9F5OL2p#^vzF6y~SEkl$lp@izT%pvqY2W7F%&<
zdR}SnE!MQ6#N5=C3`N`w3=F^0^fU5vQ}xsHiwp8o5{tk*BLp|6G%rQJpeV5-vsk~h
zD84u|w;%^@r+z_YNk)Di7Kzf5%p85VyY&hxZ?R`2=B4DM7V&}tfQ^BHfs2uYk&6+`
zPiBOM5{LqY5<3F}1307(F)%RHFk~?-U<8E}CqoSbD9maZOPCk1)G%bR)-YtTm9Q`1
zs9{{lxR9BVp@b78lEsz6B*_2{Q_d9TbVh_&7WV?46qbdIix^9ovv?QqA<6JBWQ55D
zG4z4rg)@aUoe^w0Lzch-!G#Q1RIou+Fk;9FEf7v&U&zSFP{^jq;TQ4}<mW1ug3LUt
zm!Je)#Zp>iYE{KlXq8?qvd;ZsRf)Z(ND(Uo1H&y|c))?9ktIE~B=8n<L1x}9!L<CM
z^2DMP@65c^+{BX1<l<Ybx%nxnIk#9+5=#<qG36E9VofY6N~|mb1>r5`eBaDl+$BYc
zdBr)2C7JnoprlipmvW1_sG#x|cWz=yQD#L*eo#T>Esmo65{ME=vMNeVPR%S!y(N&I
zTH=^oQks|(;v5i=Uz~c2r6e;q^%iS!PHJj_CQ}i}Gq)HE!Kn#C@PItQ3HO8$D8?ng
zG0w!q!6?Bfz$nEi#mMnriAjc$i;?L^kupdFM|^y4VrE`^yr#e{&VtOm{N&_}%={vd
z?pv&RrMU%_MPdvL3`Ghc#o)A@m~%@CBTee%LULkp5hx+u(!x}boRgWY2O{DN@-y>F
z;!E-&j3Q=`@u2hxcTiqpZfbn|Ef!FAhIo`M*wfw9F9hOIPzHjqz+UFC$<0qG%}KQb
jCFf!}1_lNW1`b9L<YDAt<zVJu<=|yw<Y43jW0v0lWhYng
delta 524
zcmey#d6rc>pO=@5fq{XcE+Q&5nwf#&F^GeVnHU%t92giFiajQ3%hfaGu;#Kwu`x2F
zFsHDzFhsGZa-^_kGZiVNvNbbAai+32GcYowFb0FMCfiGpNq(A)w|Ii`lk!V^6Y~<&
zQ;U)rLBdcBGM}A+fx#JMhaUq2Lk&X~!ve;I42%q%AU4wi=7kKkj5Ul4SW=kk7cw$3
zK$sv_3F`v38ip+P8ip*663zu&HH-@x7cw(4lyEQLNnr-5C}h)Q@e4_Z+QpDwEwax2
zVO5E}ChsktqLP%<_~iV&lA`>aTfAvSsi`SNnPsWI`6;PT$)eQc)XcKfTU_a>C637@
zrHNcQfw$Na3kp*6QYNc18_3>bDap)Dy~SFblbTwf$#jdg2;|LMjD;&1ii9RtG3JDF
z!p&s^c~F3Xfq{#WiHVC51SJ@`*qFW*i7+rQ+~SCj&rQtCi;vgjy2V+LnU|lOoROJd
z!~v3H%`43<s4U`PU|=ZXV_;yIY|WzR3)hyHn420OUj#C`NC>0^K?s9%bJ*nOr<CTT
d+JS<-7$nKTz`@AD$iv9N#KF(T$i>L=4*-6$dS3tl
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
index 184950c3ec3f73f88e80199f281657e18a3678c9..565cc85c74e5580a485052c17069fe777cd20f1a 100644
GIT binary patch
delta 20
bcmX@Ze}<nspO=@5fq{WRFfMu{_d#|5GGGLw
delta 20
bcmX@Ze}<nspO=@5fq{YH?3VD2+y~hKIdlcF
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 286ed50..c1db22c 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -25,11 +25,27 @@ Returns R because that's what's needed to construct the hom transf. mat.
"""
fitNormalVector
----------------
-classic least squares fit
+classic least squares fit.
+TODO: since you're also fixing the orientation of the gripper as you go,
+and that changes the contact point (most grippers are a some shape at the end,
+not a single contact point,
+you should do a weighting of these points in the least squares
+---> the newer points are in fact more correct than old points!
"""
def fitNormalVector(positions):
positions = np.array(positions)
- n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+# non-weighted least square
+# n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+ # bs weights, just to weight the new things more and linear is probably good
+ # (altough it could/should be even more agressive probably)
+ # +1 to start with 1 (0 both doesn't make sense and is makes W singular)
+ W = np.diag(np.arange(1, len(positions) + 1))
+# print("W", W)
+# print("positions", positions)
+# print("positions.T", positions.T)
+# print("positions.T @ W @ positions", positions.T @ W @ positions)
+# print("np.linalg.inv(positions.T @ W @ positions)", np.linalg.inv(positions.T @ W @ positions))
+ n = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
#print("n", *n, sep=',')
# normalize
# TODO why am i not doing this again?
@@ -100,7 +116,7 @@ def handleUserToHandleTCPPose(robot):
When you did it, press 'Y', or press 'n' to exit.
""")
while True:
- answer = input("Ready to calibrate or exit? [Y/n]")
+ answer = input("Ready to calibrate or no (no means exit program)? [Y/n]")
if answer == 'n' or answer == 'N':
print("""
The whole program will exit. Change the argument to --no-calibrate or
@@ -156,7 +172,7 @@ def handleUserToHandleTCPPose(robot):
while True:
answer = input("""
I am assuming you got the end-effector in the correct pose. \n
- Are you ready to start calibrating or do you want to exit? [Y/n]
+ Are you ready to start calibrating or not (no means exit)? [Y/n]
""")
if answer == 'n' or answer == 'N':
print("The whole program will exit. Goodbye!")
@@ -170,8 +186,39 @@ def handleUserToHandleTCPPose(robot):
continue
robot.rtde_control.endFreedriveMode()
+# TODO FIX THIS UGLY HACK!!!!!!
+# as in make it mathematically correct somehow.
+# a good start would be ranges ur uses for it's thing.
+# or, TODO alternatively, do the correct solution,
+# drop the stupid ur code, and use your own implementation
+# of both movel and speedj
+# --> then you don't even need to use the stupid rpy,
+# but instead rock only rotation matrices the way it should be
+def fixPinsRPY(pins_rpy):
+ #print("rpy", rpy)
+ ur_rpy = copy.deepcopy(pins_rpy)
+ if ur_rpy[0] > 0.0:
+ ur_rpy[0] = ur_rpy[0] - 2*np.pi
+ return ur_rpy
-def calibratePlane(robot, n_tests):
+def getSpeedInDirectionOfN(R):
+ z_of_rot = R[:,2]
+ speed = np.array([z_of_rot[0], z_of_rot[1], z_of_rot[2], 0, 0, 0])
+ # make speed small no matter what
+ speed = speed / np.linalg.norm(speed)
+ # nice 'n' slow
+ speed = speed / 10
+ return speed
+
+# TODO
+# be internally consistent! this will also make this code modular,
+# and robot-agnostic!
+# drop all stupid ur code, and use your own implementation
+# of both movel and speedj.
+# --> then you don't even need to use the stupid rpy,
+# but instead rock only rotation matrices the way it should be.
+# TODO: replace all moveUntilContacts with your own implementation
+def calibratePlane(robot, plane_width, plane_height, n_tests):
# i don't care which speed slider you have,
# because 0.4 is the only reasonable one here
old_speed_slider = robot.speed_slider
@@ -179,43 +226,27 @@ def calibratePlane(robot, n_tests):
handleUserToHandleTCPPose(robot)
q_init = copy.deepcopy(robot.getQ())
- init_pose = robot.rtde_receive.getActualTCPPose()
+ init_pose = copy.deepcopy(robot.rtde_receive.getActualTCPPose())
new_pose = copy.deepcopy(init_pose)
- # TODO change this, i just shoved the pen through the board
- # the point is go up, but the frame is the TCP frame
- speed = [0, 0, -0.1, 0, 0, 0]
- n_tests = 10
- # TODO: for the love of god please actually read this when you begin
- # instead of having this horror here
- # NOTE: old code remains here until fix is tested
- #R = np.array([[1., 0., 0.03236534],
- #[ 0., -0.82404727, 0.56559577],
- #[ 0. , -0.56559577, -0.82404727]])
- # get q, do forward kinematics, get current TCP R
- # NOTE: this is supposed to be run only after the initial TCP has been set
- # TODO: move [above_sentence_code] into this function to ensure that's the case
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
# this apsolutely has to be deepcopied aka copy-constructed
R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ # go in the TCP z direction
+ speed = getSpeedInDirectionOfN(R_initial_estimate)
+ # get q, do forward kinematics, get current TCP R
+
positions = []
for i in range(n_tests):
robot.rtde_control.moveUntilContact(speed)
- q = robot.rtde_receive.getActualQ()
- q.append(0.0)
- q.append(0.0)
+ q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, np.array(q))
print("pin:", *robot.data.oMi[6].translation.round(4), \
*pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
- # the first contact point is the translation vector
- # --> this is how the whole program is set up
- if i == 0:
- translation = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation)
-
positions.append(copy.deepcopy(robot.data.oMi[6].translation))
if i < n_tests -1:
current_pose = robot.rtde_receive.getActualTCPPose()
@@ -225,27 +256,64 @@ def calibratePlane(robot, n_tests):
new_pose[2] = init_pose[2]
# TODO make these moveLs slower, they're way too fast
robot.rtde_control.moveL(new_pose)
- new_pose[0] = init_pose[0] + np.random.random() * 0.3
- new_pose[1] = init_pose[1] - np.random.random() * 0.2
+ # TODO: make this not base-orientation dependent,
+ # this is also an ugly ugly hack
+ new_pose[0] = init_pose[0] + np.random.random() * plane_width
+ new_pose[1] = init_pose[1] - np.random.random() * plane_height
robot.rtde_control.moveL(new_pose)
# fix orientation
rpy = pin.rpy.matrixToRpy(R_initial_estimate)
- print("rpy", rpy)
- if rpy[0] > 0.0:
- rpy[0] = rpy[0] - 2*np.pi
+ rpy = fixPinsRPY(rpy)
# TODO: ensure these rpy's make sense (no weird trig messing it up)
new_pose[3] = rpy[0]
new_pose[4] = rpy[1]
new_pose[5] = rpy[2]
robot.rtde_control.moveL(new_pose)
- n = fitNormalVector(positions)
- R = constructFrameFromNormalVector(R_initial_estimate, n)
+ # skip the first one
+ if i > 0:
+ n = fitNormalVector(positions)
+ R = constructFrameFromNormalVector(R_initial_estimate, n)
+ speed = getSpeedInDirectionOfN(R)
+
+ print("finished estimating R")
current_pose = robot.rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(current_pose)
+ # go back up
+ new_pose[2] = init_pose[2]
+ robot.rtde_control.moveL(new_pose)
+ # go back to the same spot
+ new_pose[0] = init_pose[0]
+ new_pose[1] = init_pose[1]
new_pose[2] = init_pose[2]
+ # but in new orientation
+ rpy = pin.rpy.matrixToRpy(R)
+ rpy = fixPinsRPY(rpy)
+ new_pose[3] = rpy[0]
+ new_pose[4] = rpy[1]
+ new_pose[5] = rpy[2]
robot.rtde_control.moveL(new_pose)
- robot.rtde_control.moveL(init_pose)
+ # --> now you're ready to measure the translation vector correctly
+ # for this we want to go directly into the board
+ print("i'll estimate the translation vector now")
+ speed = getSpeedInDirectionOfN(R)
+ robot.rtde_control.moveUntilContact(speed)
+
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
+ translation = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation)
+ print("got translation vector, it's:", translation)
+
+ # TODO: get rid of all movels, just your own stuff,
+ # or at least shove them avait to the RobotManager
+ # and now go back up
+ current_pose = robot.rtde_receive.getActualTCPPose()
+ new_pose = copy.deepcopy(current_pose)
+ new_pose[2] = init_pose[2]
+ robot.rtde_control.moveL(new_pose)
+ q = robot.getQ()
+ init_q = copy.deepcopy(q)
+ print("went back up, saved this q as initial q")
# put the speed slider back to its previous value
robot.setSpeedSlider(old_speed_slider)
diff --git a/python/ur_simple_control/util/freedrive.py b/python/ur_simple_control/util/freedrive.py
index 552333a..e5a55d8 100644
--- a/python/ur_simple_control/util/freedrive.py
+++ b/python/ur_simple_control/util/freedrive.py
@@ -1,25 +1,38 @@
import pinocchio as pin
import numpy as np
import time
+import signal
from ur_simple_control.managers import RobotManager
+# TODO: put sane default arguments needed just for managers
+# into a separate file, call it default arguments.
+# ideally you also only need to add your additional ones
+# to this list.
+from ur_simple_control.clik.clik_point_to_point import get_args
+
+
+def handler(signum, frame):
+ robot.rtde_control.endFreedriveMode()
+ print("done with freedrive, cya")
+ exit()
def freedrive(robot):
robot.rtde_control.freedriveMode()
while True:
- q = robot.rtde_receive.getActualQ()
- q.append(0.0)
- q.append(0.0)
+ q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, np.array(q))
- # print(robot.data.oMi[6])
- # print("pin:", *robot.data.oMi[6].translation.round(4), \
- # *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
- # print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
- # print("q:", *np.array(q).round(4))
+ print(robot.data.oMi[6])
+ print("pin:", *robot.data.oMi[6].translation.round(4), \
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
+ print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ print("q:", *np.array(q).round(4))
time.sleep(0.005)
if __name__ == "__main__":
- robot = RobotManager()
- freedrive()
+ args = get_args()
+ robot = RobotManager(args)
+ signal.signal(signal.SIGINT, handler)
+ freedrive(robot)
+ handler(None, None)
# TODO possibly you want to end freedrive here as well.
# or end everything imaginable in the signal handler
--
GitLab