From 18661828e5676845f3f8d4e8e7e009a99b5bf58c Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 17 Nov 2023 18:45:21 +0100
Subject: [PATCH] need to fix package first, but i just got a mini heart attack
thinking i deleted something
---
python/UNKNOWN.egg-info/PKG-INFO | 11 -
python/UNKNOWN.egg-info/SOURCES.txt | 8 -
python/UNKNOWN.egg-info/dependency_links.txt | 1 -
python/UNKNOWN.egg-info/top_level.txt | 1 -
.../pyproject.toml | 0
.../setup.cfg | 0
.../.measuring_stick.py.swp | Bin 0 -> 12288 bytes
python/ur_simple_control.egg-info/PKG-INFO | 39 ---
python/ur_simple_control.egg-info/SOURCES.txt | 20 --
.../dependency_links.txt | 1 -
.../ur_simple_control.egg-info/requires.txt | 1 -
.../ur_simple_control.egg-info/top_level.txt | 1 -
python/ur_simple_control/clik/.clik.py.swp | Bin 0 -> 28672 bytes
python/ur_simple_control/clik/clik.py | 204 +++++++-------
.../util/.boilerplate_wrapper.py.swp | Bin 0 -> 28672 bytes
.../util/boilerplate_wrapper.py | 261 +++++++++++++++---
16 files changed, 316 insertions(+), 232 deletions(-)
delete mode 100644 python/UNKNOWN.egg-info/PKG-INFO
delete mode 100644 python/UNKNOWN.egg-info/SOURCES.txt
delete mode 100644 python/UNKNOWN.egg-info/dependency_links.txt
delete mode 100644 python/UNKNOWN.egg-info/top_level.txt
rename python/{ => alternative_packaging_BROKEN_atm}/pyproject.toml (100%)
create mode 100644 python/alternative_packaging_BROKEN_atm/setup.cfg
create mode 100644 python/convenience_tool_box/.measuring_stick.py.swp
delete mode 100644 python/ur_simple_control.egg-info/PKG-INFO
delete mode 100644 python/ur_simple_control.egg-info/SOURCES.txt
delete mode 100644 python/ur_simple_control.egg-info/dependency_links.txt
delete mode 100644 python/ur_simple_control.egg-info/requires.txt
delete mode 100644 python/ur_simple_control.egg-info/top_level.txt
create mode 100644 python/ur_simple_control/clik/.clik.py.swp
create mode 100644 python/ur_simple_control/util/.boilerplate_wrapper.py.swp
diff --git a/python/UNKNOWN.egg-info/PKG-INFO b/python/UNKNOWN.egg-info/PKG-INFO
deleted file mode 100644
index aed30b2..0000000
--- a/python/UNKNOWN.egg-info/PKG-INFO
+++ /dev/null
@@ -1,11 +0,0 @@
-Metadata-Version: 2.1
-Name: UNKNOWN
-Version: 0.0.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-License-File: LICENSE.txt
-
-UNKNOWN
-
diff --git a/python/UNKNOWN.egg-info/SOURCES.txt b/python/UNKNOWN.egg-info/SOURCES.txt
deleted file mode 100644
index 4130086..0000000
--- a/python/UNKNOWN.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-LICENSE.txt
-README.md
-pyproject.toml
-setup.cfg
-UNKNOWN.egg-info/PKG-INFO
-UNKNOWN.egg-info/SOURCES.txt
-UNKNOWN.egg-info/dependency_links.txt
-UNKNOWN.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/UNKNOWN.egg-info/dependency_links.txt b/python/UNKNOWN.egg-info/dependency_links.txt
deleted file mode 100644
index 8b13789..0000000
--- a/python/UNKNOWN.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/UNKNOWN.egg-info/top_level.txt b/python/UNKNOWN.egg-info/top_level.txt
deleted file mode 100644
index 8b13789..0000000
--- a/python/UNKNOWN.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/pyproject.toml b/python/alternative_packaging_BROKEN_atm/pyproject.toml
similarity index 100%
rename from python/pyproject.toml
rename to python/alternative_packaging_BROKEN_atm/pyproject.toml
diff --git a/python/alternative_packaging_BROKEN_atm/setup.cfg b/python/alternative_packaging_BROKEN_atm/setup.cfg
new file mode 100644
index 0000000..e69de29
diff --git a/python/convenience_tool_box/.measuring_stick.py.swp b/python/convenience_tool_box/.measuring_stick.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..565f0d5acdeb8db8100b7857fee177e575e0ce70
GIT binary patch
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
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control.egg-info/PKG-INFO b/python/ur_simple_control.egg-info/PKG-INFO
deleted file mode 100644
index 5f07aec..0000000
--- a/python/ur_simple_control.egg-info/PKG-INFO
+++ /dev/null
@@ -1,39 +0,0 @@
-Metadata-Version: 2.1
-Name: ur-simple-control
-Version: 0.0.1
-Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
-Author-email: Marko Guberina <marko.guberina@control.lth.se>
-Maintainer-email: Marko Guberina <marko.guberina@control.lth.se>
-License: Do whatever you want at your own risk.
-
-Project-URL: Homepage, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Documentation, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Repository, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Bug Tracker, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Project-URL: Changelog, https://gitlab.control.lth.se/marko-g/ur_simple_control
-Keywords: manipulator,control algorithm
-Description-Content-Type: text/markdown
-License-File: LICENSE.txt
-Requires-Dist: ur_rtde
-
-# installation
-------------
-first you MUST update pip, otherwise it won't work:
-python3 -m pip install --upgrade pip setuptools wheel build
-then install with
-pip install --user -e .
-from this directory. now the package is editable, which there's a chance you'd want
-
-# description
----------
-- organized as a library called ur_simple_control, made into a python package. the hope is that if this
- is done well enough, you could mix-and-match different components
- and just have them work as intended. on the other hand,
- the code should still be simple enough to afford the quickest possible prototyping,
- which is the point of having it all in python anyway
-- initial python solution is age-old code which needs to be remapped into the
- libraries used here. it will sit here until everything it offers has been translated.
-
-# runnable things
----------------
-are in the examples folder
diff --git a/python/ur_simple_control.egg-info/SOURCES.txt b/python/ur_simple_control.egg-info/SOURCES.txt
deleted file mode 100644
index c8df780..0000000
--- a/python/ur_simple_control.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-LICENSE.txt
-README.md
-pyproject.toml
-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/requires.txt
-ur_simple_control.egg-info/top_level.txt
-ur_simple_control/clik/__init__.py
-ur_simple_control/clik/clik.py
-ur_simple_control/dmp/__init__.py
-ur_simple_control/dmp/notes.md
-ur_simple_control/dmp/robotiq_gripper.py
-ur_simple_control/dmp/run_dmp.py
-ur_simple_control/util/__init__.py
-ur_simple_control/util/calib_board_hacks.py
-ur_simple_control/util/give_me_the_calibrated_model.py
-ur_simple_control/util/robotiq_gripper.py
\ 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
deleted file mode 100644
index 8b13789..0000000
--- a/python/ur_simple_control.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/ur_simple_control.egg-info/requires.txt b/python/ur_simple_control.egg-info/requires.txt
deleted file mode 100644
index e95a92e..0000000
--- a/python/ur_simple_control.egg-info/requires.txt
+++ /dev/null
@@ -1 +0,0 @@
-ur_rtde
diff --git a/python/ur_simple_control.egg-info/top_level.txt b/python/ur_simple_control.egg-info/top_level.txt
deleted file mode 100644
index 8b13789..0000000
--- a/python/ur_simple_control.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..7e2d03a88b0a8fd17bdc30aa40008b6dee547ff7
GIT binary patch
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|>+38=jgFTEk!=!N`!FUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8Fut+55
zWM=Dw2)%+zhz+CUXb6mk0M-yFElJb0;AJp2GBg0mD=R4~2n&URn4@?!1V%$(Gz3ON
zU^E0qLtr!nMnhmU1V%$(goHpz0V_j20|NsS)W3#Mnh}j=gYxa5v<Z}ksbhum`Jl8r
zl!nQ(K>09x0-=1EJTsKf1*QF=G)x}K9i>J?U^E0qLtr!nMnhmU1V%$(Gz3ONU^E0q
zLtr!nMnhmU1jq=1#1saGJZ1(4KTro<fPn$l|9`>Hz;KnHf#D=S1H(>!28P-E3=Gcv
z3=B^E3=DSs3=IGH7#ObcF)&QyV_<OQV_^8e%fPUZmw~~bmw`c_mx19E4+FzA9tMU<
zJPZs?JPZulJPZuixfvKHb2BhFb2Bh}=VD-3%*DXq$Hl<#pOb-M5+?(L3nv2u11AH+
zDGmmP8V&{q7Y+u7SL_T7o7fo`eAyWoUb8VUtYu?hC}U$_uwr9i_{Pe>u!og_p_!F|
z!JL(WL7tU?;WY~b!+I8oe;imC7(`hZ7(OyHFl=FFU?^v1V2EaBV9;b{V31{IU=U(v
zVBlb8VED(x!0>>Hfnh5X149!N1A{d)1A`Sa1A_=N1H&&S28Opx3=D6W7#Qv_F)$op
zVqjRs#K16>iGd-WiGjhGiGkr0BLl;6Mh1p?j0_CPj0_A*85tN_-~$&55KvH*nOCBr
z<eZ;ZmRgjanwOlakeHm2nOc^bqN7lhnwXrCnxc@BT3nKupO*+C6qPg?Kw2}?6!Hr6
zax(K0bJF$l@{4jcQj3b>%Tkj|@{2STY!p%pimkv35yA?#3I&;YdO7*&W*Wh+sYONk
zMVfjL9SUHjP(cM-g`)hV{1UyC#F9k4eBaDy2-nNs(=R06(<N3fF}cJuuS^3X?3<pS
zm;-T?vO-#BQE`bvaz<)$wnBMEYDq?FkwST@dQqxEVu?aYMydixG1xMYQ_}K_$`gxH
zyfgDsa}!H4lZ&B7=H{n>{0PG-scDI&IVHC0DT%oSsVM=)sii6TnV=9WPF2@YNY2R5
zOinGfg((No1hjyZz%Zs#m<)+Dra{=8iEJpk4t#PrJRJ=W6q;)$!2(pJs63+L65%b>
zxK__fOwLcrOw0=@O3W)R$S+P+kA<W?m?ashIR&;##i==|$t4Qq8JWo$3duQ{$=M2t
zIqCUDnI##y3YGb#3gwA;B}zI9F_5&FSX7)^q?eeI5}#O<UYeVlSE8Y=s|#Ifl#^Pd
zuA@*=S&(X5Tv7znqpT3(@8YkJn34ic*F~wtC1A5q-2qCmnR)37#mR{|iA4&Tc?u<&
z*?IXH`DF@4sp+LTiA9-J;C!v5qX0_{hN-$n5I+@QcUMVfc1C_)ewi-FbePl9a`F>P
zV2*=10!BNzDmXg^xw?h=D1>{4cqoK;cn0esS!tkWWD2norUOQo<SP{C=cblqfP9gZ
zT2h{xnx|l(keHXEV5p-2(x;GA33EC$IAMArQIVWj04Yz@ixr9sQd3hD$}>wcz<~zg
z7UyK9q!vN-;_xz9r7lDzs*fQt2(uDOgQCU?UJQYYp3Gu}+|<OpViaGSVD&WC&;mQS
zw5T|_D6^nMAtgUqp*XR!SfL~%vsfXsSV27}H8Dj2v{p?aF*!LkC$$J1t?GKH?t=sg
zxZqSTR>;cF%quBI(Vz!QDh2t)nI)NJsS3&YdBr7(c_lguxrr4DhI-~Y;LwMr76k)6
z6FvN4hvFV+LdKd}AdbsTtjNqQ%~eRu$;mHIOv*`B$Si^SMj@{>Hz~D9K_jz7p{O*k
z7*wd68W?y~X(ENHp#ca$0v1PX<|bC?!VN8k#b+kSkJy3>RBxn#YB~i-pycN%fNL*s
zMouhBRmeyzD5wM{D`a=1>Kfy91*j_01seo&0iIBSRc4j>rJ&lZv>22hKn_bU$}A{I
zEkd%`EitD!6~E08J?c6NiOHZ!-8Qi(9UN48PWkycsfl_11t8(X97nJ;daj1V6OxO-
zrI|uWzCu}MacN>sW>qRUFci{L3sOr;@^uuFN=p>{{6k!=6!P**6u=89L4{muib7>7
zs2GGMCR@Tzhuc8B<FOQr*xa5~T3n)#nOBmUUX)k@^*}*pUVd_NMrJ-q!3)==o1d3c
zNi~OpQd&`IULGvwLxU8GGjmIG5=-()i+-rC%=|nu-IkVLl$%&ml3Em>oRe5wj7UC?
z(Avud(qbz1NX;p5gDHk2NoWbFke{YdmROXTUs|k?otc+Xtf%0cn4PLnTw0V0&R5xa
z`Q-}b8Q`)L(rg12b4jUb`A`K#rFn|#I^f6!r`;k2TLpMJhB+hvT!U$(q(aKg%=|oC
z^`O!`h2)(4;?xv{oc#O(1!zkS<P2ytP$7mPB{fYUJ+%Z>9~WzAS}`aoDKTUgD-@I#
zgIdIiIXMc6Mfs(9h{_k_Ay9ry&QD2I&`3&6PAn}36<zAZ3MH8-m6|#V#rX;;`FZM~
z=4oPHdMc<u&n!~NOUz9z)?<L=4v+zfMd`%~`AJ!z_CtPBNn&PRYKlTyQGPDScg3Ja
zG}upi2-h*>m**)IBo-y+rk11@6)Pkb!z;x6Jdoj_OqY>ZmI}(EMWuNPi3-J~1*t^}
zIr(|%3d#Aoxruox#X1V$9FUw}P^nvxSX=^fU}mvGT4^4*5C9kY1*IjR$SGnd&q&P!
z6~W-dSeaj1q)<?lpI(%htEb?Z22uhq5|T5(o=sFpDM~CZ$xK#A%*`*&D}e@AF1Rvb
zaLma81&2a$VqPlD?Zpb91fP*wlnPb@HZ(agPXT6NUL~wL$WH@l&IS29F%J|<;1(1^
zNk)EYI;5fpM@mV)LRuyy#&Q)jKt)nfYC%q7a;k#5t~#W^Qjb>$wUKg5b4oIG%kzs;
z;HDL8>M@}C3?+pYr^1sntO-}FkdhA#)byg%0tJQ;P+eV~SP62i0;s_T4Fm<S^D>JS
zl0jv!9)oLHYEh*^W^Q77W*)c#1hJB#zSU7k%SlWJ$3}5#NeQSh0H;e(*`)vqEj@5r
zWdNrbjAiS3r6rj;dP(`2IjKbjIf*5y@#RH{p!x$;DCZZIC^$owxBKMh7x*UTC8nnq
z=_mw&dSg%?#9VNq$j{FyhG{EEEGo%N%wfpPEdZGU%ZD&NC{e-Kd8N4pm7uuFD}V{X
z3rvu3L1rEUsQ*8kkAdMnXyRUgfdMxE{vXuG=VxGe#LvKRhM$39Ek6T8CqDy23O@sb
z8b1TWAE=(kd<+a5`4|{B@G&q{@-Z-&^D!{!@-Z;T@G&q*@i8!P@i8zk^D!`7=4D`*
z!^^-B&C9?b!^^<%o`-?q8V>`*G9Cto89WRO@jMI+E<6kjay$$SEIbSh@3<Kl`nVYw
zlDHWdT(}t+-g7Z99N}VM=;mTzNabQ+aNuHK;NW6lIK#=n(9X%g5W~s9Aj`?XaF2t5
z;UEVCLlXxBLmCGIgD(dIgAoS<0~ZGa!$)=oh9m3@3{%+|7?Ri-7>w8%7+9d=3g_7v
z7#_1RFq~jxV5nzfVCZLKV2Fi|F}z`AU^vRkz_10j$bpP;mQgLEAut*Oqai?z5CApo
zz#U5kTS$$8AposHG(asJNMjJhLMTJ9peEqZV2ji;W5`TXh>r&~qT=HfY;6^k;^T7@
zGxOr(m8=vP7{IN=jKsW@oYW!>zx=#Z9R<I9kToET;#d@`@XLY?1-l%ZLs0ZWIZ!IU
z1Qs+%w&9e+DFLz?Dfl3cAWd~*5vl6ai#15pLXvWvig8Myg#sRRIHhn(Bo?LPlPksu
zeUM``G?CIGUhQ}#DKwj^+9`A>BtueS2)TNR(npjc@)H7a#)CR^<lBM2;8TE%dtxbc
ziZw}aT`DPRVLfJ~yZ}iCc*<q$=3^I4Ez(HDEmM?^$eVan;gLcz030>&Apo$4P&K0R
zlJZNCigkoL5XRtCk5d9vuAt{=k`zP7FTjq6Bo+#_$Ak3aNmsZlM+F?o5o92isu(1k
zoRbNnkSlNyUqJy`L<b}U5kRmIwS69JC=@c&2^q%)kGd-9fQE6QbC!A~;0YZvO#o>_
za}=nx0f{>394nH|Amz}>9i)!7UQubD21E;{qz00Z4ro*Yn!u2RAwxr;ZaA`H=bX%J
z9Y}=ffX4Dco7Zzvi%{&zNd*m_rKgrad;g%;5+dn=Cs+}avZ-mAd8zLCi8%qF*#oG9
zu|!}IdOHu&Pyv~XQdMFr9zgy7WBd#ZulOPR|Cyi-)Ynk4%lr%s%lH`>GWi)8^!OPV
zSos+kKJYOx9N}YN*u}@d(8|ZakjKZspvA|)z`@7B@QIg!;Q=oLLklkhLkuqigA*?U
z!$%$lhEqHY3{!a+7>aqI7K~D(Aut*OqaiRF0;3@?8UmvsFd71*Aut*O!!-mzll;gS
zbZQr538<zOWbC*owWPEtPa&x&H8C5c62A(!oWyi+2N=}BgLM0d)I_8*khvfXF<S}L
zf6WJV-7vedxDCWD1M?`v8^o#uxko`kfv}Un%Oy25K|N1QvoS?L=3*D6I8x984u>r`
zgppDNB%nYJg$Dpa6M_YrKF5}z5ZbY-#VV+P?0KjGL@R{26gj3S&{_mqr~z8rnvFZl
zKphCO7u6={5C)c92G#=V|Nr7)VDRIG^#4K206>M%OMV81`Ox`(2Yv<y34R8KuY3#)
z5BL}ura<TUmG~GKe)2Lf?Biu%Xyk>MQ4cZ)WWXpM4S~@R7!85Z5Eu=C(GVC7fzc2c
z4S~@R7!8489Ri^47YHMF{IHK2Aa&6}8sNS9#60j`LFDlU(EeG}t-Mf0p#D9I=A6vD
z)WjnEdLe2sv=-!p_shZd0mF9yLllDcAcM9ODX1!dHhn?%8!LmC#+PKI7N<hSBp~|;
zAbT*23sRFa(=wA4G!k>5+XFJybHSUE3=Irakv4LH*XSn}6@fM{fVL_@Y*bb#PgO|C
z2TOuO6SVvuwCG*|w&4S5J|4OnM4>n%zceRBAt_ZMJvA@2C^1J5JVpebGS@3k%}WVP
z(I`xTSWuXvV2d<Mui=HfMggKk8M05KAg2_v3!oq;u~H!=Gc7H(C>6A?A}_TVY`!vR
zXGcLPcppb9WUHS-S!zyxa%M>;Xj2F%j=Vr?1~T*XlJj#xD!o86`d-i-t{S=dDXBR+
z3ZPxsItqn43b4?JIRlgeuxSMC)5$GO&OmZhNk(EGIMAH6wH3-zi;6Sz^AvJYbHEN&
zfDEsJI{pf_3L!<Mso?ehysQiiil77j1Q;0Xp!pND{{I0#1H*QH28L9A1_myE28Lgt
z{yucQe<mLTgCZXT!z*3}hBjUXh6-K=hH_p8hCp5h26J8p22EZD1`S>Y1{LU806TdY
z7~*&s7^HX@7*2CDFs$NcU|7b@z|hOhz!1aDz#zlT!0;Bdo*#NHKpqzZgF6=kgE$uh
z!&goQh69`o3^O?y7z#NV7{WOj7?d~}7=CjwFdXAxV3-eG>u=4$z+lC}z@Wpy!0>^c
zf#E$n1H(IZ28Mg=3=Esu85riWGcaVZGcW|RGcd@qGcbH)V_?|8#=y|Z#=sE61_^_0
ztPBiOSs57m*r5S2N{xoVXb6mk01ZO`R6l@m0hojl;A#*1i6Iar*c9NFA@8seh<T9u
z9lKjb_UR>{dK=QdfY#oSI=c|QiZ4C2BoMlEPgw!Fkuepzu{II3<1$k(RS(qCfgX{9
zv^f@BXM-oipfxsl(>8d+EK=hKwCoTvFMz!2P)8wC(~3b^AvZq-eD*|1r9x#sbejfr
zVgtI9Py=+f1ln0EkVykQ@Cg=q`DO7SB@mA(gXSVYXS6_O?UX_318QDoUYU1hUT$DO
zaAsb*V_AM?if>|GW`S>F1=u6XnQ2JvE$|Xq@HP_M+LU1JOwfW>$Yc<vV%PX=UlPp+
zX(85ZPtwiy#Ah~f#^E+09&#p*XI@!+Mq*AHF~*^3A;xUdjKystQd)40&!#9WK?i++
z_Q2+(f;tJ{t_SEu7SK5>8L8kszF<cqrALedWWWkC)4*$b^`JQjydc+BK?y@K)Kf_2
zk(FO?SO>~bXn9V_3Y@dCtPO_ji3bIMl|p$@W=X0-erbt9KKRHb_*pl_@Qv-D?ai=$
z5M&E?3TQhz;&7ML9MFFGG=<C(&|dPg)MACqyu^ZnqWprQOwfT0pxw$vpoQ!u8L5>D
z;O*?G6(vQ93dJR*X=%`n*13rVAOoP`pOl)O2{|tTeB=u>s2IvqA-mH-yZE7d$H80U
zK?ju-lz{sw#fiD8pcnz~Brn!c02ha#-TAN>0Y^1R0KAl30dkC$9)o9zda**HLQ-al
zLTP$Vr4E=?l3$XTlT!)bN}rem*6r`DP@JDr3OZ>+kHJx)xTFZwn}qEi2lb*#L1z?z
zPQXb8pC*)_mkQl6UYrWu<&BgO3RA!-3EYG9(hE^=0QI<`tJ}eG2y;68=pwY<CF*1>
zrg4b#zB2R5G`tiX6hPK!gU$g<txVPM(ksqLEJ%$uh}BU*ur(F5AV&_t3v5h>rC=Vb
zrGZk&>nNc1gRK~psRJ0mH3cXTv0_ASEXFDi6I7;}d%#gx1gTX(E9Jp_m^C08l;jZS
zcYuyh0Uaz+R8*;8rLUl;5Sd@906HWAoIOCNk0=;|(sOYt_|$-6g`&*#j1q;s{Bm_j
z#R)R9C^54*RlzU6#1nME9jF{jO>qUC&ZeR6l9>V?97)Se2c63S%HSFK<)C~EItc>g
t7U=Oapi>hdN9Vwb7MKL+q$ZG0^boFwRG=V1(C`D~Gz;(%9<bdi3;_OQ4-)_Y
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 0f5dcca..7f40d33 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -1,25 +1,22 @@
import pinocchio as pin
import numpy as np
-import sys
-import os
-from os.path import dirname, join, abspath
-import time
-from pinocchio.visualize import GepettoVisualizer
-#import gepetto.corbaserver
-from rtde_control import RTDEControlInterface as RTDEControl
-from rtde_receive import RTDEReceiveInterface as RTDEReceive
-from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
-import os
import copy
-import signal
-sys.path.insert(0, '../util')
-from give_me_the_calibrated_model import get_model
import argparse
from functools import partial
-from ur_simple_control.util.boilerplate_wrapper import controlLoopTiming, RobotManager
-
+from ur_simple_control.util.boilerplate_wrapper import ControlLoopManager, RobotManager
+"""
+Every imaginable magic number, flag and setting is put in here.
+This way the rest of the code is clean.
+If you want to know what these various arguments do, just grep
+though the code to find them (but replace '-' with '_' in multi-word arguments).
+All the sane defaults are here, and you can change any number of them as an argument
+when running your program. If you want to change a drastic amount of them, and
+not have to run a super long commands, just copy-paste this function and put your
+own parameters as default ones.
+NOTE: the args object obtained from args = parser.get_args()
+is pushed all around the rest of the code (because it's tidy), so don't change their names.
+"""
def get_args():
parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!',
@@ -30,112 +27,111 @@ def get_args():
help="whether you want to just integrate with pinocchio", default=False)
parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
help="whether you want to visualize with gepetto, but NOTE: not implemented yet", default=False)
- parser.add_argument('--gripper', action=argparse.BooleanOptionalAction,
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
help="whether you're using the gripper", default=False)
- parser.add_argument('--goal-error', type=float,
+ parser.add_argument('--goal-error', type=float, \
help="the final position error you are happy with", default=1e-3)
- parser.add_argument('--max-iterations', type=int,
+ parser.add_argument('--max-iterations', type=int, \
help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
- parser.add_argument('--acceleration', type=float,
- help="robot's acceleration, positive constant, max 1.7, 0.2 by default. \
- BE CAREFUL WITH THIS.", default=0.4)
- parser.add_argument('--speed-slider', type=float,
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.4)
+ parser.add_argument('--speed-slider', type=float,\
help="cap robot's speed with the speed slider \
to something between 0 and 1, 0.25 by default \
BE CAREFUL WITH THIS.", default=0.25)
- parser.add_argument('--tikhonov-damp', type=float,
- help="damping scalar in tiknohov regularization", default=1e-6)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tiknohov regularization", default=1e-2)
+ # TODO add the rest
+ parser.add_argument('--controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
-# parser.add_argument(
-# '--device', type=str, default='cuda' if torch.cuda.is_available() else 'cpu'
-# )
args = parser.parse_args()
if args.gripper and args.simulation:
raise NotImplementedError('Did not figure out how to put the gripper in \
the simulation yet, sorry :/ . You can have only 1 these flags right now')
- if success:
- print("Convergence achieved!")
- else:
- print("Warning: the iterative algorithm has not reached convergence to the desired precision")
-
- print("final error", err_vector.transpose())
return args
-def clik(robot_manager):
- # define goal
- # TODO this should be handled in some better way, but one step at a time
- q = robot.rtde_receive.getActualQ()
- # urdf treats gripper as two prismatic joints,
- # but they do not affect the overall movement
- # of the robot, so we add or remove 2 items to the joint list.
- # also, the gripper is controlled separately so we'd need to do this somehow anyway
- q.append(0.0)
- q.append(0.0)
- pin.forwardKinematics(robot.model, robot.data, np.array(q))
- Mtool = robot.data.oMi[JOINT_ID]
- print("pos", Mtool)
- #SEerror = pin.SE3(np.zeros((3,3)), np.array([0.0, 0.0, 0.1])
- Mgoal = copy.deepcopy(Mtool)
- Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
- print("goal", Mgoal)
- success = False
- def controlLoop():
- q = rtde_receive.getActualQ()
- if not SIMULATION:
- gripper_pos = gripper.get_current_position()
- # all 3 are between 0 and 255
- #gripper.move(i % 255, 100, 100)
- # just padding to fill q, which is only needed for forward kinematics
- #q.append(gripper_pos)
- #q.append(gripper_pos)
- q.append(0.0)
- q.append(0.0)
- # pinocchio wants an ndarray
- q = np.array(q)
- pin.forwardKinematics(model, data, q)
- SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
- success = True
- print("reached destionation")
- break
- if i >= IT_MAX:
- success = false
- print("FAIL: did not succed in IT_MAX iterations")
- break
- # this does the whole thing unlike the C++ version lel
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- #J = J + np.eye(J.shape[0], J.shape[1]) * 10**-4
- # idk what i was thinking here lol
- #v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector)
- #v = J.T @ err_vector
- #v = np.linalg.pinv(J) @ err_vector
- v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
- v_cmd = v[:6]
- v_cmd = v_cmd * 5
- v_cmd = np.clip(v_cmd, -2, 2)
- if not SIMULATION:
- rtde_control.speedJ(v_cmd, acceleration, dt)
- else:
- q = pin.integrate(model, q, v * dt)
- if not i % 1000:
- print("pos", data.oMi[JOINT_ID])
- print("linear error = ", pin.log6(SEerror).linear)
- print("angular error = ", pin.log6(SEerror).angular)
- print(" error = ", err_vector.transpose())
+#######################################################################
+# controllers #
+#######################################################################
+def dampedPseudoinverse(tiknonov_damp, J, err_vector):
+ qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * tiknonov_damp) @ err_vector
+ return qd
+
+def jacobianTranspose(J, err_vector):
+ qd = J.T @ err_vector
+ return qd
+
+"""
+A string argument is used to select one of these.
+It's a bit ugly, bit totally functional and OK solution.
+we want all of theme to accept the same arguments, i.e. the jacobian and the error vector.
+if they have extra stuff, just map it in the beginning with partial
+NOTE: this could be changed to something else if it proves inappropriate later
+TODO: write out other algorithms
+"""
+def getController(args):
+ if args.controller == "dampedPseudoinverse":
+ return partial(dampedPseudoinverse, args.tiknonov_damp)
+ if args.controller == "jacobianTranspose":
+ return jacobianTranspose
+ # TODO implement and add in the rest
+ #if controller_name == "invKinmQPSingAvoidE_kI":
+ # return invKinmQPSingAvoidE_kI
+ #if controller_name == "invKinm_PseudoInv":
+ # return invKinm_PseudoInv
+ #if controller_name == "invKinm_PseudoInv_half":
+ # return invKinm_PseudoInv_half
+ #if controller_name == "invKinmQP":
+ # return invKinmQP
+ #if controller_name == "invKinmQPSingAvoidE_kI":
+ # return invKinmQPSingAvoidE_kI
+ #if controller_name == "invKinmQPSingAvoidE_kM":
+ # return invKinmQPSingAvoidE_kM
+ #if controller_name == "invKinmQPSingAvoidManipMax":
+ # return invKinmQPSingAvoidManipMax
+
+ # default
+ return partial(dampedPseudoinverse, args.tiknonov_damp)
- for i in range(robot.args.max_iterations):
- controlLoopTiming(partial(clik, robot))
- if success:
- print("Convergence achieved!")
- else:
- print("Warning: the iterative algorithm has not reached convergence to the desired precision")
+# modularity yo
+def controlLoopClik(robot, controller, i):
+ breakFlag = False
+ # know where you are, i.e. do forward kinematics
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, q)
+ # 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:
+ 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)
+ # compute the joint velocities.
+ # just plug and play different ones
+ qd = controller(J, err_vector)
+ robot.sendQd(qd)
+ # 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("linear error = ", pin.log6(SEerror).linear)
+ print("angular error = ", pin.log6(SEerror).angular)
+ print(" error = ", err_vector.transpose())
+ return breakFlag
- print("final error", err_vector.transpose())
- handler(None, None)
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
+ Mgoal = robot.defineGoalPoint()
+ controller = getController(args)
+ controlLoop = partial(controlLoopClik, robot, controller)
+ controlLoopManager(controlLoop, args)
+ controlLoopManager.run()
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..d0befeb34512b3259ffe76f42513039b4404548a
GIT binary patch
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|<lK8=h(&TEk!=!N`!FUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8Fut=1a
zWaj86<!9!k78T?qmZZj)7bO-Hq!#HFR6;BqB}YSGGz1_aP+F3vYr)H4Y-DHv5>QrB
zR1g*l1u;kQXb6mkz-S1JhQMeDjE2By2#kinXb6mkzz7L}k^)wSdIkmtCa8aXpfn>I
z%?9NMKxuI(4O7Pq<=a8&NGJ`HXNB^?oeu^Eh6X4fCeH%pb3^G_P#Pu=<&ILLAut*O
zqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3@?8Ukd5Kw=65!%1ca22oH4U4VfB*8jiF
z&%kh)pMhZmKLf)Oeg=jjeg=jxeg*~&eg=kLd<+cN_!t;g^D!{Y=VM@)$j87?!pFd1
z%g4a*o0oy%H7^6hD_#bMXS@symw6c&_VY3@RPZt|81XVNJm+Cx*v-Sh(8a^R5WvI0
z;KRefAkV|V@QRy(VFxz@!$fWdh6&sZ3=Z533<}%~3`e;b82Y#v7<#!F7*e?y7&y2X
z818d2FihfPU<lx3U{K{`V7SD=z_5mcfnhZV1H)nt28PKT3=GX23=F;;3=H4d85pjz
zGcasmXJA;t&cHB@oq?f<oq@raoq<7&oq^#C8w0~`HU@?@Yzz#m*%%lWvoSEtW@BKO
z%*Mb_!p6Yh#m2z!hn0chCMyHOMOFreeXI-&6ImG;3RxK#Tv-_yv{@lxXTZY1z{SGA
z@Qj&(;RZ7U!!%|Fh7e{3hG1p}24-dkhQ~|{3>{1i41r7x3_45<4A&SL7&bF9F!Ue|
za40}QacWMQUP_6At%9L~z5<w6T95)-;82uUlF9&8g(0C}t6*wiprGtgg{s8M-_tK7
z-qS_FR>4d`Ss^E}xI`f<KQpgHAt^OEv9vf<p&&CaKRG!gGhZPwC8by)DX}<JA+0De
zH&r3ASiztq1EL&ms<J|9kwS4nYI0^;X0k$VVtQt>LSAWZQfg7L0$fpXW_n&?jvj>4
zfKYnDp6;H0AvzGJ7nkH0cqHbf<fIm9!nG+Ylw_n9rK%SzB<2+7D<tMABqo=XCgv!B
zR4XLs=jJBnrRXRmm6j+Jr{<+(=A|nb6f2bH7iAYK6z3~ss^==?<(DWVXC&sOgCsLc
zP>qIozqlYZH6=?SCpR%)4@o{VO`$TsR3WRhxCCNfW(mkS3I#>^=|zdT3gww484AuN
zMLD|8ItrO33gwwOISQa<Dw%nusR~7<d3l+6=?riKK@kOxy`qwo)cB&*<kZZvR0Ugw
zpb!_=APCPhuOzi7EipM&L&?y{T+cwyK+jMK5gMo_fNUzt&w-lY3|SsSz=YJC;#4b4
z7o`_vf>+8UCYNNEC6=UWpxOzFh0J1w%wmN^g`}MP<ZMt-CMV|PpqK=A9n7@k{Jgx>
z<Pr@fLrWt)Lo*9KLp?(SBPAULGh<^@BUCFg(-abm(u?(=da<|>9BP^QQ15#BBl{G^
zd{l!;2yr~7fcyr>NkyrN*(hGmOj7_0y5%IMqdFZftY8aC=FnL4$<Hs)$V78$Nn#Nw
zo0nwfrs{zRq-aRXFH*=<$jnnHO3X`7)d2e{H?bl<vm~`Bu_QA;uUHdeaY||$DCvP!
zK!uc)l)$bn$$&&+N`79dLRn@aI5EQ=2v(9=qFxLZMslWlu|iR5aY24wab{9xPG(7^
zLP<Wvy9$|Upmd*@oROJYmYSjf)}5Z8m{SbNM&LY~0nXE4Lm{PtLOw_r%-Il?DftQ|
znYo~3Rgw>KzCvONsPyuvf=KJ?>VlnDRGJ5-K|TUAKyi{<ky(NiQXnC4T7h~&51hNb
zG|EzQ;*)bzbQBEq40RL?^$ZmBK~|tf0aTHKtwLUbUR7#QezArbLIqkR7#bKrLl|Ud
zK~ZL2iH16?7&0_402PX<DOtq|X{9+iAYX$GR7lKAQ2@C)GcR2oVm2twKm{7K_yZS@
zrMWtgLKkX<LvcxBNoI0xYDq?Z3fS+UKmb{+3@+(F5eHGC11g}Ba#C{@it}^IQXz_!
z6>?HRN|I6)lJkp-Qj<$SkyoCokerwYafGr0q*PXbL|BnRMq)82rZW=DQbDm-oT^|1
z%A=*Zsd**E3L2?psd)+|8TqB@86fFO^`cY-P+41AoC+zC!H!f0S)g8`05%1bYC&~K
zT4qsk3EV6lc*RqakqW6XbP)h-3R2*J`5>=@84&-Mr>f`VC={orDwJoW<|&k?Dx{~D
zfZUE!+-IhtxHB1)pi@$d6f*NNQj0Q6K#30&HQ<^oF(<PMBmmAb`9%uFB^e6EnYo!c
ziA6d{7MEnC7N;sC7NsgA=H!4(S11NmlN!1C#U(kFnvlc`abqzk7&A-23N!N*5*1Q1
zlR^15v8WQ1H1bjsi;5B}i@~X*D7B=t2o$)9$kkPTnnH4ZX;E=1Brw5FE6oG>02FQ@
z^WzJ0@=J;p(sB~h6+n*82c;H}H=s^Oa(SXcaW2@+nYpRD<(YXY`Q-|UMfs(9pj?dX
z?#vQU3@YU3<y0ypfvg7A&?yR_S{WtYpt(&UCqKUcqB=RTD79E2F)6<kR&Ayhr{<OD
zBe^2P-^CxA-V{<2OA-}|6U#vE1(~jgs5v1Cb3`{DKv_o*8pfcyL%|l@(kRN$0rB8L
z3RZ?%FDTf8a$2z-swh|osN9K<&&<m#iH`?Y7&^#?>L{RU(X?Vn&PgmTR)97&Ko<KZ
z<|U@57HJ^4Ob2Y8rWFGylR_#8P)U+llnyC5bQF>jv%w`wE~q{OH=@!?^O8&Q^K**z
z3KEM-G81$382tQ0T&=+EuXIo|37pzN>AWa4F{M&BH!%fNkbzeOgOi*dLr!LPDw>OR
z6v{IaOCTw$Bp;NxQWa85lJyuoK}BJr0>nG8W^ZPRLUw9uL9s$!ex7bhCa8?esnmt~
zOc#`{b&FC#3yV_}3KBuh;gZy%Vg}DVg?vyO71ZcVDF(Fzp*3hyetxzt$UsnnhGs30
z2cQjDsJT$b2j^s_XO!erDx@XmC1<7<fr@ZY8xvL~g37Z*1?YliSVjezhH4C=*o3nf
zpem4J3mh{HnYjh|Mc`7nAhD=8l_9MtKUV=`xwRf>RkdDGeo}r(W??+Eb*2E-8w3${
zhqNB?nvkAa5}%u&l9~h43m0HeRsa_RxzJRhP>`6MotT~q3eaqrD<LHSjF+5WPzkY(
z1c!m!1ey6T`$4U4q(&svIOLW&%#&D}=SZrd!HaAxG=IPws0_+bqta6gQcFtm^^)_8
zK%J_hveY7oN$?(*URh>wDX8*Eg&F7$(Gvy}FM<UEsDTJ$=auFbRDx>OyaJdI+(?jc
zL1rE-n3Ho-6N?l;WpXK~b)H&W461=34lHH>_5T%E7#K{T{eRf}{Uv?|hS~fK44M24
z4Ced{3@`W?80z>K7?SxI81(rV7)1FP7#{F4Fl^yvU|7w|z_6H?fngRe1H%Mf28K9Z
z1_nJ|28Oph3=GqF7#R9^7#ON}7#RF`7#IwA7#KcrGcX+BW?)#%&A<@D&A_0_&A{-E
zi-F+*7X!lvE(V5aTnr2~Tnr34Tnr4aIT;u(b22a-;ACJ}&&j~Bj+23587Bk7Y)%G-
ziJS}!R-6nBk2x3^x;Ypa@;De6(l{6xd^i{wzOpkgY-49&n8(h*kjc)#V9L(G@P>_n
zVILa<Lme9fLmV3egE$)l!(~<mhNG+u3?;0PFgV7_z%YxIfkBLwf#EF+1H*h41_pl?
z1_mJ(28LhE3=9*P85pXW85rW285s1K85n*sF)$opVqjRo#K6$V#K2&{#K0iT#K7>C
zk%3_+BLl;HMh1o)XdfTcE&yRrTad{1D@Zvy1~=fr#W}i+FQ_T5S5lOiSDXXxBS4z9
z;D&|<NG?7nv$zD(`a{;6nWm7cU~8+Ol%K5x=~N)gg4mF@ZYri$grrhPMyf()UO{OI
zsNV=`7-oW6G^q-y6$PouC8;S2Y57IDi6wdpAw`u6iRp=%d3tDFU65fQ4C*8oR~GA~
zRwTz~=B4GM1r}IIMRICE33kUSC@8=^8=V=eV5^Xplb=|E2!ZI#SnNJQYwzcz<{>m`
zYGStyZk2*9*jl~ff}G3}4Rsx8TOZ_FWDN2gD4;Q7u%xIG(*}^~3bv4N)ldq^Nd=9~
zq=H5bz^%`c#3E2lkqYWzq@|@Mm*f{I6yz6Yf*P8j$`({(ROnUcRO(ggROwYgqrgf5
zE#S*DGILTDLW)Y!#tM`bQt}lN6(G`}3Q?gnuOt)H@P%|N!4*jgtb>tLiQ*?^Y{?N*
zOdHhmf+W#s13d#!cg;XYLDxXf5G|6B2PKM96N~fn5|eUL70MIA{nM0uP|vg^KOfYJ
zsZ0VFr#Y#ht_w&URErm(xDM<PaHFcAQZFSnwE$GdX!wE}C8%jb8QfZj4sU^mhw_kP
z6WsnPRImk&q3LDjm87PF#^^M_H6L`Gt58RwFhxNNG8&MAmi?ff0(H<!60<?g=bRh`
z3x!mO(~CiN7p5o_XXYiRDwL;!x?II2pi!sHJdll$Q9HQ5Fb8|Ubs)6Hg7u3FQy|@9
zu+tNhlT&k`U2~{=F`WqRP-kfrrsyaXCnx447RBeJCZ@!L_G`r_CMWBFTJ-6q1@WL7
zDn36i9y&~i2*=Dca8D6zV{v9~DWr5lq;`mtAf;7uPG*4yL_kMD*ANzPpr$F<0ieNA
z2nS)lvVsO^L=Y4^$)F)a(9j{c6$$Ep<tczu7qaTS0zHs5pn@tt8#1hy1nLSED<q|Y
zT5k|vg4%WpDfy|zkXB`8i9&HkerXP*e+^0m5LYVLBE>H>taDO};=!R74>AfgDhN^!
z!paJ{iJ;^Oa%*{F9=O*EQUh)v!&>9eo;f^Xkt~X~GK+<aD<k*Fic-O?XNVi~)Jqid
zQd3hvo#O1g{Bi|wdpa?%vOE!yOO>HXDkBp-kP0d+QY%V|5*3UT3W_p|b3x+|kg;Ed
z;*8Y1#JtS(#Jpk!aKjO#80uMt@*>cv6xdau9tLPcBrpZqzbH(BmYbm70Vn_<ZSXu$
zw;9}=g!U*P(F5uSBKav3G&Yr)mtL%pl3D;N<nkd^3cPOtDw>irz#Z~jNY4b)QPs#u
z1?3o!Bf-w9%r6DyoqSL~yeKs>M*-Z9)Byz;NE|wJt5BSuo2rnPS`O;^gA!?ekwQ`q
zXb3b>4-$Zx#h|pFk^>!NNy$u0OD#$T#bO57nXn*FN(B#3L!AvuQu%qHu?ld<0qPi-
zrHL@xGV}8wV-4W8JlNfkpae5P;Rj}b0uef3PzV<QXDv`Ugb)Jp@(T38iC?1-622h4
z$_k(^RU&j~4?M;T@;A8WR9u>rl?oaYPE{xbHA_n>6%x}*Qj0*N7;w8_LrdUsa_~4$
zQhrH>LVf|L)+q+nF`(fX@c2q0C=aO@Lx$876)aL9Rj~r79RfEG<fuYD(ALeo6b%DC
z1B6rY%P2#PO#_#Qpwak}3<b!D8@Pj3o>`m<w*#|igxd&CsIYP9_=5am1$_l0Q&UX^
zEd>KT10z!$&LK@3sGKhaC7vAcpp-&-Vgac10b7s@9s?>*g^Z%4<SP_sDwO7BmK1~2
zGTaNGzyKMO3e#+woQp+L-^kPyWOPYBC>B7KUrJ_fYF=?>eqK&$aWRSxaH@g!@xVT}
zRe<vJK;5<E(jrje2FZY16iDr`%rr=ShJ6ell=Q$kOb=WEL8m@IW;%jra{{3zfl7<?
z)RI7OSfZ5^ps{2fa3crmB#@_|B~DIiib8QJs8e2&no|iH4lPerPk|SBpuuI(&^IVq
zXMo1Jz{MV9q%j{7{$SVXKy-p;y+FffMX9;@WuQUN%#zex@K7kI{D4fAff6ialnhkS
zlw_n<f|^F4R&FA=-34}NepzY}Xp9YH45;M+4(rmQlr)8sqSVBaVp!mUdT1r(`RIjg
zu@1zEi0lsv8?Ygu%nyox5C#i?7~t50hX`7Q1DZY5Gk~iltJ(ou0I6#gY!wXjOp%O(
z4A*2TWF)3QipCO92?!d-Hv%<0!8uSNIX?xF1(8fk0lUr|$+*mvY*4Y3lT!(q+yl=n
zB&C*s%AS&Zqyf+zaD1jHB$XBwWkNa?3dM<Osi2Z7F9lMrE9B(orKgsGW-cHNMMSE{
z77pMdtRy2fS0N)aJp-H>lM@TTF<O*b3@uo|wYfrZPG(AKkv)>bK|>i<AhqyuVbEBx
zLUL+RNn$2w3>9Q%Dx&QP4+T(_3GSp7;~B7nM>9AdW#;P@r<MeRCNG1*4$%N-Kd{r{
zAx=XSR$z;en|Cm4kj23L{~Npv3<`XZ{r}w1H3Xo!|E>HC3<>-U3|#yS3<vla7<%~_
z7=rj17|i(?7?k)J7!>&+`ULqH7(ixh<7Hr&$;-eH#mm58#>>E<!pp!Q!^^<%ori(p
zDh~rgJr4teCl3RIB@Y9GCJzIHA`jHEQED^<MnhmU1V%$(Gz3ONU^E0qLtr!nMnhn5
zgaD`qf{f88uL$uK6coUH7id2fHV6${cLg;X-4y(~Kyw86mUKZ41uwdSjx@l8pl*S2
z@!AVh1fww(V~QX=i(3~$O;LUhT7ba4hF3dY$rRAhX@pgPmr`Kbj;0Z@CI>p?2%a~A
z_kY2iOt6rGtpb>aODTgF>6U=jW8{L?I)D}vfyPq`ic-^3i;6%))1VOp9ncswWHA?#
zR?z4Id@(O<b_O(z3LeJJ&r>KUEh@-I8sCFC8$^Q^b-||9A$n55GjovTb$SZH`9(#Q
zdJ4|@;Kdfj`T2QzIOgh-QWerND?m=oR{$-P2HOr^%%-Q{2%2PojrYP<2t%hlA${>;
zg&4S(K<+9^%q&h-@XIgp1f6dO8X!tdaV;v!FVavCNG!=H292A5LI^x!gtD9qG<pdi
z-9i`&5=Km-fUJPb(<*`8nOO|+H7J}w1KkQKscD&csVP=5c}hBv$kGJ&|4*<oFf0LW
z@E2fUfbGx!&(FZ{kDr0z8$ScXBYp;k)BFq!b^Htr_WTSC;`|H@xA_<tX7DjE1n@C1
zF!C`lY~*ENDB)#b$l+yR@a1J-Fy&=nkmqG!xXZ)9uz`nxp@)Zo!JUVJ;XXG5!#!>W
zh8f%p3_9El3^%wK7?yD{FnDk=Fi3MTFnr=<U|7P*z!1X8z#z`a!0?}gfnh2K149D`
z14A+g1A{RK1H*rI28N^T3=C`885r82bNdqP3=F@Z^ZQ`G435%s)D@#4Fd71*Aut*O
z6ode%VnfHHi=Y$~6jDbQL4n#!ql=&hU=b8_ZAnRG0ch19N;?;_zSjyotqPig1<wpZ
zHmM{g<y0yZ6;y&33FH@Lrsjb*x#Z`;*L;BI{XmflTIp7z0NFr<W_nR-ZenI0WZ4c#
zDRi1z0m(cR9neN`Xpm{DLWpyKm6DEv7JOj@xI>6BK^@{85RhM-s-dZ=2i_y0VWNqc
zSwk+1v_QKU^zwZ(qs?NGS6;%k=_qIwWajA=6;$fwCYBUsR)pjS6;x^<Yb(l!1OhI*
zpza5)Ack~d6jJk2bW_3e&XDzfpzWIZxrh~EkS2Z%qVooFGSnCk$o9%|@KPI4@PVho
z!ISHd>HeHbJq6di;!@~(BJk=V@Zu1VVkAdo7AruyLTHN#q4q@Pmm)TjfcMy>Rf6V8
z6_D4b7J++)dHLnYOF@xWi-9LNVVyao)rsJRdax~a@W2FjM`8P25zC}N5s;Q&RGwIr
z;+>h73SQS#tN~VxyigBhj1FQoVj)tLD?>X^;2sq`ae<aOfK36-(?bUuKua1R9WD3#
z#GC-oK3lL&Xsa8b9mQnOa!%-aAeb*R^U^_U7(t5x^YTH<h>BAoI}YFrT0lz>K+7W_
zy}En_*g7)sL^kqT96g2nq_WKX(&C&-u-f!|aK-~gB(#eP_6s=EfmidCq*jzb7Mp?<
zf)<V;c1wWvIpmil=Hyf=loo^5Ug|&=;DA;^6qgpH79~MeNPwoq6+p{ULH0m=1zIAI
zmkxGAN@fbEqnlZl4BGPmT5SZ$$xsJ@_x(c50VP_HS3yhVz}^L$nXgcmnOY8>rBBRD
z%&Dw`tegX_69z3W&dkq)BulW*Kt_N&!=P27Aam04i;^K4K&29RUu#loT7D5kEH@vt
z8wt7$1Ee=MF|QQtVd%o<63`;9JkW+k&{kiBGc)rPz~LSmq)?Jv0L`VKaR~5k7o?^6
zc`2~945_)W)gE>VN#JD$kcF&}-FV=f0$%(JS_7A_P?8GWZ&q5IS_E<g)WwLkF<}3K
zmyVziU}3Zb2^Iq_DPaKj|CJaS80wfH^Z&5({Xl(w&>Dc1(7peW{0t24{0t0=(Ea}>
z_!t;=@i8#u^D!`Z@i8!X@-Z;DLe~bU^D!{I<Yizu%FDnok(YrXl9z!&mY0EnftP{d
zAP)n>OdbY?Fdha5X&wfKuiOj_JGmJcYPcB~D!3UKQlR?*esD1`?BQZyn8(Gy(8|TY
zP{75&pvJ|(aGR5XVGAb%!%R*F1{Y2ShJPFk3~xCY7`AXQFtl(mFl2HtFvxK*Ffed1
zFx+KlU^u|ez_6U1fuWk6fx(-dfkBa-fq{`7vgf~)je)_Eje$Xije+4lD+9w$RtAQt
ztPBk8(7pZ4tPBiHtPBiySr`}&voJ8!u`n>0voJ8cVP;^s#LU33o|%E6gPDOLnwfz?
zotc5*2@?auOeSbpQqbNVD6OCb55lneJ2@jYIa>iVf(K485GlwaWN?;GtOSj&feVkq
zloX^YTp_a<wEh&N0fh0G2a;A$P{5-CwnmjG8)2Kc6f_bG3KBtU*K#Tm^>$)$aVlg3
zRIwiNA{Yf51w%b^NQna4l8z#0r(mFmyo?Hzso|##<YX2UK*9jNlr$}~0wRJGW}t!x
zx|Rv#4e-k3+*F0+qQt67h2oOZv^2P4aCJhPvmfAgfcFoBq7_`oKnLa(Qc^*Cu2PG@
zE1p5o2OHZ27nIO#oyDce845|r%c(%&0a>12k^$KiP@0#SS5lf;l3A7t8r%gp13@(b
zWJy?AVo@fj?ocSn&qs0**{4z<%3yHN<;8>7>w(r(nIRdE=fDcMDo|Pkn}OUUR2V(3
z0TeEvz2o4f7_`1Y9d$;8+CUxGfN%jUIYZ(NeP^Bq+R+q<!U5Xo1bY(O76q^k2npC?
zI)n&39Vsh-HrQrDHuZtB2lQABaBc%_5h(&4_<_hnpzsBo4h>sSl0(^N32j+|)Zslf
zL%A%oN-rg|xF9F7Qlk)U88uikCqFSIEHkw{wa6E=H518Tkl|n@Ah$!5V3Ad@RlwMu
z2;1wDoS&1E30h?g+4iZU0BQL_c!+QYTLjuDqF0=gnp&V?j3$L;%cNd0=$wrB;#AQ7
zNpx3%+G0?vGOJRHAe$3F>rNFkKxHlHWEfC+oLG{JYEeNV+FEFk!$26{oKG;pnIo#t
z@xWpn5}KJ+NPQrXYeDU7NDxETg2I&{d<#xn$o(9Qm;$SS=!BRB+X)2Pp>K;ci>0gp
z*9)#LA$k<Rh8Kf(VdQ~!GGVwGw7JkNF$ZbBPFVqz79l#}IS1?xP!5FVF0cfsLxr|y
z9i#->pHfzUPcDMb{7Fqw0Ppnz6<FXZt~eF6mOB^J!cQzHD9SG=%7h)F1g-}mXI|uH
z=4Iw)Lc0=)x%s7eC7`WWIhlE>(4$SDn+-t82DBLhBnzt45Ia%8{b^8kFD?OD23}8`
z2s%Rtx``fnvkPReyaITJ6x@D-R=(gpk_u^w#U-E)L1s#7B53~(_&O}mbRl@TumWg<
znI3~Ks7nPh9@L9U0*|YJ(mTjax%tJQ%|GS3;J!i{Xp|)dyh=W&6tqJFw8sRpCml2v
zQ<?`_vu$c%;86vRQjj5_&;r$~NSoMU#VU9cV0lJnat3rGOfl>nm*RYIQxVk1V^9Da
zRRTJjBc&+60JI4qGp_`sAhTEj<nq*_RM3b6VuyPg=s<wX{5+6$g@V!&23;_#9JI3t
z#43Vqjmk+aLR$I{@=JaZq#IhCkzbw)YHfh`DkOq;DT1~RW#)ml5kT4|kh{cC_OyU|
zXt{}bpq5x_L9vbk_*{XM+yYPtfV!WcAO%MSq$dVFJp|+@&~{N!Sb@%^D#*!9P6X|8
z1vQt_iy3r5!3u5?6_<cEw}Yb@+{aE-NJ89MrcjicQVKp71ade5g90R8!ADCafi};U
zq~@ihrYPh?_cKAx#)3=+gY3^N0l5?P5H8U9Bw(vRn{i7(8zVvK1wPgSayn?+PjPW+
zZfXiBbdmQ!f!&;+rvM%<E6xY+iDE$MQtIk56oD!z=#DE$Vg~0`P>BT2#7O72X!zyl
zrRpevNJN<gQV1O<a&z?bu~Gny3V@PcacOcg=wP8d9VLvj9h4AD=8=Y+a881QjVcDs
zsw?RzWa=m&X;(zjpP8nRsbB-y%>vr%fUrbC*ATUx19CM|)s<4BpsN7d0*2}@Ob3DV
zfih5LUMXTb1V|8sp}~-wSzMf&qL7lBm;x$w6p|{Hbijs#>_FQznwh4MQlel3mPU0U
f$TbSKpz%QkT?KHp36B?$U7#&=wxA3PKW+*DE>7pG
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 9441085..99e0492 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -15,33 +15,95 @@ from ur_simple_control.util.get_model import get_model
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import argparse
-# slightly fancier programming to get a timing wrapper around the control loop
-def controlLoopTiming(controlLoop):
- start = time.time()
- controlLoop()
- end = time.time()
- diff = end - start
- if dt < diff:
- print("missed deadline by", diff - dt)
- continue
- else:
- time.sleep(dt - diff)
+"""
+controlLoopManager
+-------------------
+Slightly fancier programming to get a wrapper around the control loop.
+In other words, it's the book-keeping around the actual control loop.
+It's a class because it keeps non-directly-control-loop-related parameters
+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):
+ self.max_iterations = args.max_iterations
+ self.controlLoop = controlLoop
-# robotmanager:
-# ---------------
-# - right now it is assumed you're running this on UR5e so some
-# magic numbers are just put to it.
-# this will be extended once there's a need for it.
-# - at this stage it's just a boilerplate reduction class
-# but the idea is to inherit it for more complicated things
-# with many steps, like dmp.
-# or just showe additional things in, this is python after all
-# - you write your controller separately,
-# and then drop it into this - there is a wrapper function you put
-# around the control loop which handles timing so that you
-# actually run at 500Hz and not more.
-# - this is probably not the most new-user friendly solution,
-# but it's designed for fastest idea to implementation rate.
+ # if you just stop it the program with Ctrl-C, it will continue running
+ # the last speedj lmao.
+ # there's also an actual stop command, but sending 0s works so i'm not changing it
+ signal.signal(signal.SIGINT, self.stopHandler)
+
+ # TODO handle data saving here.
+ # it can only be handled here because the control loop only cares about the present/
+ # a small time-window around it.
+ # of course have this under the save_plots flag or something similar
+ # save it all in a dictionary of ndarrays and return that
+ # these are all the same (mostly).
+ # if they're not consider inheriting and specializing this or sth similar,
+ # we'll see when we get to it
+
+ """
+ stopHandler
+ -----------
+ # can't have self as first argument, because the
+ # signal handler has to have these 2 arguments (even though they're not used, but
+ # let's be correct if we can)
+ # so it's static, problem solved
+ """
+ @staticmethod
+ def stopHandler(signum, frame):
+ print('sending 100 speedjs full of zeros and exiting')
+ for i in range(100):
+ vel_cmd = np.zeros(6)
+ rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+ exit()
+
+ """
+ run
+ ---
+ do timing to run at 500Hz.
+ also handle the number of iterations.
+ it's the controlLoop's responsibility to break if it achieved it's goals.
+ this is done via the breakFlag
+ """
+ def run(self):
+ for i in range(self.max_iterations):
+ start = time.time()
+ breakFlag = self.controlLoop(i)
+ if breakFlag:
+ break
+ end = time.time()
+ diff = end - start
+ if dt < diff:
+ print("missed deadline by", diff - dt)
+ continue
+ else:
+ time.sleep(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)
+
+"""
+robotmanager:
+---------------
+- right now it is assumed you're running this on UR5e so some
+ magic numbers are just put to it.
+ this will be extended once there's a need for it.
+- at this stage it's just a boilerplate reduction class
+ but the idea is to inherit it for more complicated things
+ with many steps, like dmp.
+ or just showe additional things in, this is python after all
+- you write your controller separately,
+ and then drop it into this - there is a wrapper function you put
+ around the control loop which handles timing so that you
+ actually run at 500Hz and not more.
+- this is probably not the most new-user friendly solution,
+ but it's designed for fastest idea to implementation rate.
+"""
class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
@@ -66,6 +128,7 @@ class RobotManager:
if args.gripper:
self.gripper = RobotiqGripper()
# initialize and connect the interfaces
+ self.simulation = args.simulation
if not args.simulation:
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
@@ -78,29 +141,137 @@ class RobotManager:
self.rtde_control = RTDEControlInterface("127.0.0.1")
self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
- # if you just stop it the program with Ctrl-C, it will continue running
- # the last speedj lmao.
- # there's also an actual stop command, but sending 0s works so i'm not changing it
- signal.signal(signal.SIGINT, self.stopHandler)
# ur specific magic numbers
- self.JOINT_ID = 6 # last joint because pinocchio adds base frame as 0th joint
+ self.n_joints = 6
+ # last joint because pinocchio adds base frame as 0th joint.
+ # and since this is unintuitive, we add the other variable too
+ # so that the control designer doesn't need to think about such bs
+ self.JOINT_ID = 6
self.update_rate = 500 #Hz
self.dt = 1 / self.update_rate
+ # you better not give me crazy stuff
+ # and i'm not clipping it, you're fixing it
+ assert args.acceleration < 1.7 and args.acceleration > 0.0
+ # we're not saying it's qdd because it isn't directly (apparently)
+ # TODO: check that
self.acceleration = args.acceleration
- rtde_io.setSpeedSlider(args.speed_slider)
+ 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?
+ # idk really, but it's better to have this low and burried for safety and robot longevity reasons
+ self.max_qdd = 1.7
+ # NOTE: i had this thing at 2 in other code
+ self.max_qd = 0.5
+ # maybe you want to scale the control signal
+ self.controller_speed_scaling = 1.0
+
+ """
+ getQ
+ -----
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ 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)
+ # 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
+ return q
+ """
+ sendQd
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
+ def sendQd(self, qd):
+ # we're hiding the extra 2 prismatic joint shenanigans from the control writer
+ # because there you shouldn't need to know this anyway
+ qd_cmd = qd[:6]
+ # maybe you want to scale the control signal
+ qd_cmd = qd * self.controller_speed_scaling
+ # 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:
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ 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)
+
+
+ """
+ defineGoalPoint
+ ------------------
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise
+ """
+ def defineGoalPoint(self):
+ q = self.getQ()
+ # define goal
+ pin.forwardKinematics(self.model, self.data, np.array(q))
+ 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))
+ # 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
+ Mgoal = copy.deepcopy(Mtool)
+ # this is a reasonable way to do it too, maybe implement it later
+ #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: ")
+ try:
+ e = "ok"
+ goal_list = goal.split(',')
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = sys.exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+ else:
+ raise NotImplementedError('Paths in the urdf or something else need to \
+ be fixed to use this first. Also need to program in the sliders \
+ and visualizing the goal frame. Sorry. Coming soon.')
+ # NOTE i'm not deepcopying this on purpose
+ # but that might be the preferred thing, we'll see
+ self.Mgoal = Mgoal
+ return Mgoal
- # this does not need to be kept here
- self.q = rtde_receive.getActualQ()
- # can't have self as first argument, because the
- # signal handler has to have these 2 arguments (even though they're not used, but
- # let's be correct if we can)
- # so it's static, problem solved
- @staticmethod
- def stopHandler(signum, frame):
- print('sending 100 speedjs full of zeros and exiting')
- for i in range(100):
- vel_cmd = np.zeros(6)
- rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
- exit()
--
GitLab