From 7204dab482a62c75d7412ce150c912f67bef4e18 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 20 Dec 2024 18:23:48 +0100
Subject: [PATCH] defined elipses for arm, put obstacles back to casadi ocp
with obstacles. need to visualize ellipses as they go, and make a box
obstacle
---
.../robotiq_gripper.cpython-310.pyc | Bin 12234 -> 0 bytes
python/examples/drawing_from_input_drawing.py | 2 +-
python/examples/joint_trajectory.csv | 195 +--
python/examples/path_in_pixels.csv | 195 +--
.../__pycache__/__init__.cpython-312.pyc | Bin 392 -> 430 bytes
.../__pycache__/managers.cpython-311.pyc | Bin 57066 -> 61536 bytes
.../__pycache__/managers.cpython-312.pyc | Bin 60484 -> 61903 bytes
.../__pycache__/__init__.cpython-312.pyc | Bin 199 -> 237 bytes
.../basics/__pycache__/basics.cpython-312.pyc | Bin 12746 -> 12790 bytes
.../clik/__pycache__/__init__.cpython-312.pyc | Bin 195 -> 233 bytes
python/ur_simple_control/clik/clik.py | 113 +-
.../dmp/__pycache__/__init__.cpython-312.pyc | Bin 193 -> 231 bytes
.../dmp/__pycache__/dmp.cpython-312.pyc | Bin 19808 -> 19826 bytes
.../create_pinocchio_casadi_ocp.py | 17 +-
.../optimal_control/crocoddyl_mpc.py | 2 +-
.../crocoddyl_optimal_control.py | 3 +-
.../PKG-INFO | 8 +
.../SOURCES.txt | 7 +
.../dependency_links.txt | 1 +
.../requires.txt | 5 +
.../top_level.txt | 1 +
.../path_generation/setup.py | 2 +-
.../star_navigation/config/load_config.py | 3 +-
.../star_navigation.egg-info/PKG-INFO | 9 +-
.../star_navigation/starworlds/LICENSE | 0
.../star_navigation/starworlds/README.md | 0
.../starworlds/obstacles/__init__.py | 6 +
.../starworlds/obstacles/ellipse.py | 217 +++
.../starworlds/obstacles/motion_model.py | 119 ++
.../starworlds/obstacles/obstacle.py | 201 +++
.../starworlds/obstacles/polygon.py | 158 +++
.../obstacles/starshaped_obstacle.py | 85 ++
.../obstacles/starshaped_polygon.py | 866 ++++++++++++
.../starshaped_primitive_combination.py | 226 ++++
.../starworlds/requirements.txt | 4 +
.../star_navigation/starworlds/setup.py | 14 +
.../starworlds/starshaped_hull/__init__.py | 2 +
.../starshaped_hull/cluster_and_starify.py | 500 +++++++
.../starshaped_hull/starshaped_hull.py | 207 +++
.../starworlds/starworlds.egg-info/PKG-INFO | 9 +
.../starworlds.egg-info/SOURCES.txt | 28 +
.../starworlds.egg-info/dependency_links.txt | 1 +
.../starworlds.egg-info/requires.txt | 5 +
.../starworlds.egg-info/top_level.txt | 3 +
.../tests/test_cluster_and_starify.py | 129 ++
.../starworlds/tests/test_obstacles.py | 147 +++
.../starworlds/tests/test_starshaped_hull.py | 53 +
.../starworlds/tests/test_timing.py | 173 +++
.../starworlds/tests/test_utils.py | 106 ++
.../starworlds/utils/__init__.py | 2 +
.../star_navigation/starworlds/utils/cg.py | 593 +++++++++
.../star_navigation/starworlds/utils/misc.py | 51 +
.../starworld_tunnel_mpc.egg-info/PKG-INFO | 16 +-
.../starworld_tunnel_mpc.egg-info/SOURCES.txt | 2 +
.../requires.txt | 8 +-
.../starworlds/starworlds.egg-info/PKG-INFO | 12 +-
.../starworlds.egg-info/requires.txt | 4 +-
...UT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF | 0
.../__pycache__/__init__.cpython-312.pyc | Bin 183 -> 221 bytes
.../robot_descriptions/yumi_local.urdf | 1160 +++++++++++++++++
.../util/__pycache__/__init__.cpython-312.pyc | Bin 169 -> 207 bytes
.../__pycache__/get_model.cpython-311.pyc | Bin 16341 -> 18424 bytes
.../__pycache__/get_model.cpython-312.pyc | Bin 18507 -> 18521 bytes
.../__pycache__/logging_utils.cpython-312.pyc | Bin 6993 -> 7005 bytes
.../util/encapsulating_ellipses.py | 91 +-
python/ur_simple_control/util/get_model.py | 8 +-
.../__pycache__/__init__.cpython-312.pyc | Bin 174 -> 212 bytes
.../__pycache__/visualize.cpython-312.pyc | Bin 17238 -> 17520 bytes
.../meshcat_viewer_wrapper/visualizer.py | 11 +-
.../ur_simple_control/visualize/visualize.py | 11 +-
70 files changed, 5338 insertions(+), 453 deletions(-)
delete mode 100644 python/examples/__pycache__/robotiq_gripper.cpython-310.pyc
create mode 100644 python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
create mode 100644 python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
create mode 100644 python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
create mode 100644 python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
create mode 100644 python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/LICENSE
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/README.md
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py
create mode 100644 python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py
create mode 100644 python/ur_simple_control/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF
create mode 100644 python/ur_simple_control/robot_descriptions/yumi_local.urdf
diff --git a/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc b/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc
deleted file mode 100644
index 122384842b716013f5bd36e61282ee1c6cc548bf..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 12234
zcmd1j<>g{vU|^UrB`kHfF$2S65C<7EGcYhXFfcF_`!O&uq%cG=q%fv1<uFDuf@!8G
zCNRw$#hk*J!kojB%NoVX2vWn6!<Ne)#h%L%#gWSy#mUIv&XB^I!q&o&!q&_b#g)Po
z%%I6$W$BxrQks*hP?E2ZoS#=xl%Jyzl%JGel3A!;tdL%mSx}H#RIH$@P?B0)lA5AW
zo>`Ki;Nj@!qU-t+<VZhF##=0|d8N6U%(qxW^D^`EZn1`x7UZPf;`T2}Ni9lEamh?B
zNd_rJ#vmVq{j`OVfgzP4iZO)&<c}!k6s9Pa6y_+_6qYEq6xJyA6t*ai6!s|26pkpa
z6wWB_6s{<q6z(YA6rL!)6y7L)cZL+c6#f>56#i6!X67hCcZL*!6u}mT6hSawC`Bkm
zxP>7~IK?YPG*u);ELAi`JXI`3B1JNdBSk7jx`j1LJjFXjCPlV|B}yViE>$vBBvmR^
zG*v8(Ns<AAQ{+<=T3Di_QxsE0Qk0NnSyGf!R9aY~WI!@$Oev}<YAq~LvZ-<@>Zu}W
zOeq>Enk_8Nj8XC_S}EEs3{eUxJ}J7XifK$KdMWxXtWioS1}TOu3{lD{5~-qTOesbw
z#w{#SDo~zDifIc=lq!^GmSWz*5~T*^S)^FDutce+SfyCEFhpsj*reFDFhprWW$jYz
zTUesBQhZVzQWe2|Wl3>NacW_W(oS(s6-{wT6-{wX6-|*y)k#$Z*(}MB#*yNd;@-j%
zrHi3LG*t*34jd^SDV{AXQF<wo!3>(dxA>sx#2u1Isw%k@6ciMk^K)}c^D>hYOHzv=
zsih<%6_$_`QZkEDlS^_cbri}n6BUZ{le1GxAgYQ>iZb)k6_WFFa})DYiggrnQp-|{
z64OC~P`SLs+|*))wEQB4vc#gy#H5_mVm+{(x0oHBLvAs<hxp%Ob_@x+#q8)3bc@-|
zKj;>7aDeMA<^ccTTg<^Bj<=Wtf?RJg`#X8vVs`ThsbUQcanrTHNSutY3@5{YFL4Gl
zXtGtwpay~-G%$io5=%;ps}i7Gg<>#Yp(wQ=zX+6#k}4IDf<;duBqLQJGp{5yJr(4+
zoYK@{g^a{9aIVZvEJ@B#D9=bNLDr&BoSK(XtXBkb>q{^JDkl6i*>7>g$AbbYKK>S0
ze0(lAbH~Tu;)#zhEKSUT%CN`Br{pKc$KPTNat(G3xy9w^9O4<~7~<*Yev1vvc3sI(
zB+bA8A%4Z_XXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y$d~s%OK~8Esv^dc(
zs4U6I&(lw>NCXKN>lZ<amw0I5qE}FPix(PBkYJMq6?UK!f`fsD5d=Ayia=txBT4~p
zM1e{Xa9Jmf7FqsDS*gh-5KF7#Aw1efRgn+_1H(%uP{0Z^FfjOOaukVxIHDjz3`B@C
zFfiO=^Ysq{#f3<4h<`wUt4q9pXozc2yuXu|t8>ULxTvR}A3~JfF(lr_H8=zjZ4eeH
z5#Qp0cn;!K6;Sx{FfcH{!k2}SgRw}Nfq@|zl#*cOG>8pysWV8q1*q;}tYL^}C}CW{
zw2+~OA)YyfsfHn*C55?$A)Xb?W&^X?!E6pNn=^$mm_d`JipM!WuehYBG`S?dNY4+F
zYX37ZFlch!VuO?dw>V2Oic%9(GV{`JvH0XCXW!z1mId)SARcdgZf0J5L4I*&38<3c
zjn7T2KybOhvc(0dsVTR(z*2B_W?p<+eo=BNR2<A+$#{#UI5j5?5<=i~4h|-c`1s7c
z%#!$cZIGV@7#JAXn7A0(;E;>4N*pZ>!E`)_2Ajhzj*R@`5>Q6F#RAIhxA-6)j4#Q|
zP0cSYxy4qLT2fk+=a<X^)dHeGY!C(qwh;pZLkYtIh7yJ>#uA1sriGw@W~gDvVpzaj
z!&t+V#<Y-;5yEGwVFt0m!OT>(-#I@oFEzQO7*w4n!b@FP(UDn}nx~MMl2Vjf3@tf9
zCAL*TVo_qQ0?gG`3La4RE94g`WEMb*SR{oYPgyAh<QJ7-NTK@FN+AT|WpJ6D1d2v*
zX$~p%^9xdo5<zi^<P<+m)*@9<24Ht|i}&<%4Y|b~?C<Oy9~=_o>gaomJsIM?TU^Dd
zCD7p1WGONL<r`xVVFDsdK?FEX!2~E`i!2!!7|cL%%ErLJAjBrXD8N`Hh8B}h!*S;U
zJ_ZIzpffIH0JX4!88n%y)SYwki&KliWi!--%=|n^orYY*`)M*kyvCXg(om$2>B?JN
zDVfDkGr)}_Q10Vns**%=C_>dOj<Up}_!LkJ3#1m5kU$vZNe%`E25@3(1LxdYrW(ct
z3=0`t7-9utm};48S!$S4m};13Go&z;FxId%GtOq1%VN$@%T&l#!<51dV%0EYG4)G;
z6B_dZmKvr7tThZ-YzrAd39g1Ai+KTi3d=&qTGnue6jl}n7KUbKMut2NLxy6@az;i5
z5R7C1>0ZE5!@3Yu4zMK#FfuR%gK92tcqJAT6y+BbWhR!S!YY2`c0ozLLUAgj)+<ZQ
zDNR+#PgBT4)craNiFqjs<%yXkkVXNBR!>aMR#zxWEiTB<D^A5opRmxfQUJ9|GxPHj
ziz-2;!rK#|d;_vm1118lOLY{$_G==U2g$=$3L!-xcjYM*mnJ8t78j?L<|q`UCZ`sF
z5;n->#N=!pg|x(+;#7sqG=;o;g~a4+h4RE=u&T_m)D#_s%)As(a|x6+KnVt9eQsi<
zLSB9exN!r@dEmA{Qfg|RLTXxCYH~?t8Is>_F$cSb{9;tN#mM!Nfdy2laNc6gEJ@8R
zzQtTzQdFb?N@<W>1!^=T=H%RBOU+BpPf5MS;p*q??*gi6Sc+1U%Wkp9XBNjNCTDB1
zK{AysC@(T6=ca%Y8o0@IixnIew^&jVOA^t`PtN$_)ROqJ#G+ysP-X*Fb}Z}yj8cpo
zj2uiNjC_nT|G1c|B+$|`TvL%3$f@3-iU`!20VQP+2C-Qgpvf6jUDklgMn*_Un9kVD
z2u@s#iE)%Bu0(j^A}?)$1IS7t4B9${CMKj*11V|20SHnCO4HCp4r<kcVjq_7kTeaV
zq^8MK<O_--P;6*2L(&L5&d?JB2P|GgL1_V0K8dhnR-e!WlMIpvMK%btgWD$12%iTn
zb^GN&Wo`{4sKlDfT+39$kj1!wsfK9*a}7fl3%pcfT)>*bypXY$Ih-Md1*KF{K`E7(
z7qCI}Enr{B5W`f<Qp;MyQp1?dT;y58k-`ct8QE&!s%A5!u!HU9n9JVG*u+@ES;N}Q
zSj$$!7R;c@nJ7#!>6PRw6s49FWu}5!^%<#<5(QjBz!RN9T2X#3xL|}fjdc{@m1b#P
zNoEd64qhg}3OR*LL}3D|u|cT_QV%PDs>ou6{8G${8kWqg6#No%!38Tw8%z|GaAA(c
zk`TjSg)xer#9~+v2BD>j#oaYTp$Jr&6*+?1yHz5gd8rizsmY*D5Znh<ObQxR!kShJ
zDfy|zpn^{U(&<tE#i*{yg($#^K&48ND<}oJfe3d{@x+#r3NFi7iwklxOKx$2UFTX<
zlwWj<IWw<BlN~9UdV+NMfOLT?PH-cQ6Pz96K_w-q<iSkE>5w8V4wQ;Pr4I)?4<ooN
z`zylE!KlK>!N`K8SqRlt1hNfxlLAyr*D$0oW`hP}nEZ-CMO|WYHrNrGj9|@RHiQ5-
zWjsKBU|?YQ49YPaOc>1=Xc3NKC%knDY7l@*P*4U+VO+=ruA)H=8zwjpl%*IKGJ@Py
zBv%7!b%L`Ph+k|&zTP6a64nK5Dd5&?3Nxr<kf_SUzyMFuL8+k150t8)832)zKpo_?
z{G6Qpa!@@9if~Z6PpwEzE(Il}%sf!uP)N=%f+Q+XH$tH}xhS)sL?Nv-FBw!F!>vh4
zO;ac;j4uM&rUA7n9+G8jl?;_Mt>7v_b#GB2tQ^x&a&!(+(orzbfwiomx-@ZX2noWj
z0Yyi7MrKZ`MqYji$XU>Y1GCH!!!iY1TLlA9PDj@e9O9^?qkuyLny<+68HOfYz5^*l
z#>JI+$r`Akl$w{qg%SwGIjN}y8U}i%2z4MeFk`@d#&l5az`*dp-cOUg2s9j81ga{E
zI6+w=3`BsM=(kuv<=!n;uz^_H*=&%wF9YRhkPbdUK1KybK1M!9g@1g^RiZ(V5en#_
zgdRk7$SuCa(vtl6<iwoJq@u)<RFo<Ulo{bU>;kyT0yVdb<7yZdfV&Gd3=2S27Be(g
zNkI855Pq>PB4<ElaHuQ->0}2rzf+h&c^9mY1=1vk+rXU7ROAUY2V?_lHdApNSS2{`
zL1jQTU{P7*S;JVAR>QD>1KiDF3uZ_GRdVc!H7pDaL7+?n>Fp$f+M(bcoI+YoV!8q}
z4?;5(qy<%+T2ic#S)z~xYW9}oE9B>;>L?`Vq$Yv~`wEIu%QEvzixtumOLI!VrXY0&
zq5V?SK(|r|&qyuFNG$@@QShh$Xa3yGyv*FvTyU=-H?acDhK&~#D<mZrr=}?6=P4v6
zmy{;vDCFjY$^@DgFwg)>C0xd2rYXR(e`YakU@RV{KLyPIP3(CYTY095n#VwyX@CkF
zkkvQ}T||ZlDS}}vWd=wJR3@O58z4c9G8(fGM9~8>1f&myk=f9i0T%G_iFqmUpbjQl
z1YoHps1k!Pd(o`J3?FdOreUaOK$ZA4B(*Xz#*zR*K?!k|fu13zi$S4`5(UWWk&1kf
zFi0(k24Q42qzb4OS?B(+s>L2WlnSj7KG+vQhEq9jae{lWpdNBj6sSf3R}0`;0#wo$
zMS#R2K?Jz^hz5;8GC^91AU33(1s^xM#Q}?weo$Q@1?rc`axf||axp6W=VRuAvRGJn
z7+L;f^le~zQJP|)iUQQi1CPzfK`RPSQ*0q)En^9&H^aD)v4km$Ig16{C}gZs3j&W9
z7Aus)N<O3smCRyLfd(26&}1zF`xWd>&<GHC%)AKXW=)6>Z?Qq9A<#To6vM#4unXir
zPzA=rB*ci(Lx+bcidR4b#2^gv%4d*`pb^So22I8)XPA>gZ8=!E3@eMlg*CL6P)JV9
zQz%MJOwLfy$S+9EQ-CSb)YD`v(gej5wt+GMP$LO>Ja|8>LCnTfB_D)ZeWR<z>4&0t
zP-<YTaz$}HT1|q>_25yx6jb+X;dejM0QO;!%0vbR2JEg!Qi<VuP=W%-0HiMgYIT6)
zAPD9Q@YGReF=%?XI1@Z23-$v_aDzu^VR52?5o6#k5;#f{Kn}$gC4!(}2AK&Tls^tq
z8V>RkY(^Q@pu$j!(^EyDIUh~Nst^L6K?{8}@1RE^!aGSIH)8XSFvyJ%Q{f&u4N?sD
zkP?<8hNc+9OYlq&npObyp+J);ptfN)XlRtN${pcFXagrBRUx^ws3<kB1fHxwou1_U
zJXky?7AqvBf;vQLr8zm33ZUTE(`33O0(Lw^E8O)(DIn)#^DBF1aXd)tc~~YA!0c5*
zm5|^oR&Xlxq}*4KfYsAvDoO+SAQePl^9HEohp4>_Qj!5mdFVrpnZ@xCCAd8TnivB`
z52Q>6d%_9k2~cr{5hdWkT+ob8YKj7MUbi5lvN$t2F-IXIu_&cHu_zVfdT{B?3vwD3
z*Mt3b9aIQ|@-q)pl`^&@h8Cr_m~-;eQMw_Z5*$<#g9?OV21d|)#R3LM7X-9QWFd15
zQ!P_1Gq|2%hSxL9MY1KVDa<8oHO%0704C4~7bFZoqhok=T9vS-u%PJ#b#yqg8H*jg
z7@8PsK$BM-P}3L}f>x$5;dDnL4%0ZZS&K8^rg7FV7S)w-rLfj8EZ|<qFq<KTt%hL%
z517S1m$jLZks*a67>qSJs}?$zl%(bsKqkddy9}UI5AH8O5(PY)73+XvvN$KdJf~6t
zBnQf4pxFkH1`r#R5TL~+q6-1-u;k>Yqx4b=iZb&cvlvCG#icnAcjV-!BQ(AQ50HUI
z%d1qKq1{W+BvM*pW)7&+QjnSlwlp6!IbBp@rBJ1bMOjj6a$;#QcrY(9Paz*Xw{4|R
zB?B#W6+j&+UAX^KKpm*)DkdF;UyQMSn!L9}bMwnm;VoX!oGGa2fRq}bBn6%zE>Z#w
zp78{Mh9h%R!J}+NptM#5&XroA#0{#nZ?S?sT?DQzKxM2Zbe1j)q@4p+$rpjf)X-Za
zpk~-@P(}bv>Wi@oF$yt?F@s1xMm8iC3zHJ4%_WPLO+cPRbkU0nK=!dh=7L$lQ@<#)
zuAqSfkl#RFD9!^<l%+7WFx0Ztuz=b!HH@G_pqUvwG|Z621WL+T%+1WTY+!ko8qhcy
zYcq2#dkJVTnZ1TJ4V?6A*uXSr4Lg|TN?}f8N@r<itOaEQ?i!ATj3AZZ@h=7@*n}8#
zwi`TkTU?S@RHBfZUj~Wx^2DMPNS`|$JcSDjKOF_|ax73{EG|e*&P>ZpO;G@Q7M!TT
z{zarRE6D5vOfzT-7c!X+N(rFI2hq{UttuS_<Vp5ec>5h{64)jyg<#OEJ4i3sjKmTJ
zuo+-Uhyh?GhHkLytQ6cpbMqkGpri}d36_B91T&Fz!lptT6-q#B%pl%RR7lFt&q1C>
zLvAR88sYhQpfL#0tQ%sQE~gT-gr)>COp%xe8@mP@3HK-{WTE2)uwYXt0j<OVnN^fp
zSPEVgr=yUVR+3r-t%?&<vP!{A<x29w?X29)JpJ6n3I(K`SCp8Sjx>Rn3~CxcMz$Cj
zKy1+TN-!u3fSL*=3^fcT;K{CLre@GEF{7U*(=F!Q#EM(YxtV#I%(vJ<aaNX?bBh^7
zu!G{PEHMWyU)|zJ&dDr@FH6k%2QnE{%Lp)5siI{cP!#If<m4wO<`moM!E}JTTADnN
z_Bc3`gVskB>437kE{M<r5g=C=f$KL=6<Wj%5(BkK!Ba+>oRCfyC_@$%f~1N-L<J~+
zLR?mmUkr6)aX~7Sm6l&r1e$|H_X`WionJxz09nPsDZmVx(br(+W2_R!;v<Ya4{|Cv
z&o6>?z(Ca$Gq_p;PXrb7moTO<LaRYgo@NH+*=*(_l@gW(tdJ3G#3)(~Yc_MSIF!$}
zkg=Angna==4ckJ-680>PEY57EA}4&Z#l}!su7!-X>@{q)95w7<6F3$!rh%fIIdK)O
z%L#BsKu-91NK@;ed;kh^P(Dr02QA|&K}x^S;sBOIhEKtQC>4rQ3qZqbc_rWiIU`je
zC$Si*RSqea5bZx`(*Rm7WR`&Dw7{h^QUF62<$!}VBQXWE0HPo#wFI;xAQ4p0mz07g
z)Zj}4GV;qI#YuiyszPdBN@|LNMsaGY0`kHxh19&#TyWnfCAB0mGpATnPr)z0Bo)=x
z%o0#9q9DJxI5R0H6;hsoA{Z8b;N=P6xdl+a9ORzNGzEBz0xX-7S^}BEODP2p<f0Fz
zRw=qcnl8}UZP>&jWT6Jg%X;a5R(`zXn-pNL$z4<fDhfc=FJuIRAH+or)PM@XB2X<2
zX@SUtWI@FY_9EpL8)!y3Kl>JYa&8K!1#^o%xwI%gKPl@LXIf@nVh&gg%FIv7LMvK|
zav2yHI2jojia`S>LR_GwaRSU-i~|38ScDjP7=`}xu&^-lF;=NyDQ7TRPnwKHptW3@
zOhur@M}C@&A)5Nw7W_e`F~Ef-s7X=;TKotpKtbgjq)-Kq>wtp>93X<AU;xj8f{Jp;
zSPy6kUy&h50#vvafvS*OtZ6y<i6!6#P(?N%DNxy01gd4g<uYUz1=KaU#RD20%1q8p
zEy>7FDFRP3f=1A8v4E=AqF9h#&`1%YQ3q)of?Ix|ak!#PkP>ho4b+A!0`)~8Q40!F
zq;Q50uYqF~<l$n_A_DMgL>AC`PdH|RvX}&z1lT#)Ww^Px<XEHx`8b(G1Q<D(1tdAN
z`B?c_K#+rlgN2Vpghfe8O$2PNCU+5NEhTu-RgoIVX`m!^iv^TNia=p@i>;)xATuu=
u5*T3Dg2M)!)WB{;5TKdYTO2l!<+OI7L9b#^e6TQZFmW()FoGt8g_r;#wdcYB
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index e3213a2..612c145 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -253,8 +253,8 @@ if __name__ == "__main__":
assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
args.mm_into_board = args.mm_into_board / 1000
print(args)
- clikController = getClikController(args)
robot = RobotManager(args)
+ clikController = getClikController(args, robot)
if args.pinocchio_only:
rand_pertb = np.zeros(8)
rand_pertb[:6] = np.random.random(6) * 0.1
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 5b8f74a..6e4bc96 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,183 +1,12 @@
-0.00000,0.93845,-1.46316,-1.40613,-1.05605,2.05935,-0.42868
-0.05495,0.93841,-1.46317,-1.40614,-1.05604,2.05938,-0.42872
-0.10989,0.93837,-1.46317,-1.40615,-1.05604,2.05940,-0.42875
-0.16484,0.93834,-1.46318,-1.40616,-1.05603,2.05942,-0.42877
-0.21978,0.93831,-1.46319,-1.40616,-1.05602,2.05944,-0.42879
-0.27473,0.93828,-1.46319,-1.40616,-1.05602,2.05946,-0.42881
-0.32967,0.93827,-1.46319,-1.40617,-1.05601,2.05946,-0.42882
-0.38462,0.93827,-1.46318,-1.40617,-1.05601,2.05946,-0.42882
-0.43956,0.93829,-1.46317,-1.40616,-1.05602,2.05943,-0.42879
-0.49451,0.93833,-1.46316,-1.40615,-1.05602,2.05941,-0.42877
-0.54945,0.94162,-1.46188,-1.40514,-1.05663,2.05679,-0.42599
-0.60440,0.94633,-1.46034,-1.40345,-1.05762,2.05314,-0.42210
-0.65934,0.96110,-1.45514,-1.40004,-1.06010,2.04208,-0.41023
-0.71429,0.96696,-1.45285,-1.39930,-1.06083,2.03771,-0.40558
-0.76923,0.97236,-1.45106,-1.39815,-1.06173,2.03372,-0.40134
-0.82418,0.99662,-1.44066,-1.39826,-1.06358,2.01597,-0.38260
-0.87912,1.00312,-1.43783,-1.39852,-1.06399,2.01120,-0.37764
-0.93407,1.01069,-1.43436,-1.39923,-1.06431,2.00565,-0.37190
-0.98901,1.03109,-1.42384,-1.40357,-1.06416,1.99070,-0.35659
-1.04396,1.03416,-1.42220,-1.40436,-1.06408,1.98844,-0.35430
-1.09890,1.03624,-1.42106,-1.40496,-1.06400,1.98691,-0.35275
-1.15385,1.03726,-1.42050,-1.40527,-1.06395,1.98616,-0.35200
-1.20879,1.04878,-1.41339,-1.41007,-1.06289,1.97767,-0.34347
-1.26374,1.04883,-1.41336,-1.41009,-1.06289,1.97764,-0.34343
-1.31868,1.05067,-1.41208,-1.41112,-1.06260,1.97628,-0.34208
-1.37363,1.05375,-1.40981,-1.41308,-1.06203,1.97400,-0.33981
-1.42857,1.05685,-1.40730,-1.41547,-1.06128,1.97171,-0.33753
-1.48352,1.05723,-1.40694,-1.41586,-1.06114,1.97143,-0.33725
-1.53846,1.05836,-1.40581,-1.41709,-1.06071,1.97059,-0.33641
-1.59341,1.05948,-1.40467,-1.41837,-1.06026,1.96977,-0.33559
-1.64835,1.06029,-1.40369,-1.41956,-1.05982,1.96916,-0.33499
-1.70330,1.07022,-1.38772,-1.44116,-1.05144,1.96178,-0.32768
-1.75824,1.07330,-1.38128,-1.45046,-1.04773,1.95949,-0.32541
-1.81319,1.07521,-1.37752,-1.45581,-1.04561,1.95806,-0.32400
-1.86813,1.07766,-1.37149,-1.46478,-1.04201,1.95622,-0.32219
-1.92308,1.08496,-1.34982,-1.49779,-1.02870,1.95075,-0.31680
-1.97802,1.08687,-1.34034,-1.51297,-1.02248,1.94931,-0.31536
-2.03297,1.08765,-1.33677,-1.51863,-1.02018,1.94871,-0.31478
-2.08791,1.08782,-1.33590,-1.52001,-1.01961,1.94859,-0.31465
-2.14286,1.08816,-1.33331,-1.52427,-1.01785,1.94832,-0.31439
-2.19780,1.08826,-1.33242,-1.52573,-1.01725,1.94825,-0.31431
-2.25275,1.08839,-1.32982,-1.53012,-1.01542,1.94815,-0.31420
-2.30769,1.08815,-1.32783,-1.53370,-1.01390,1.94831,-0.31435
-2.36264,1.08743,-1.32478,-1.53937,-1.01145,1.94884,-0.31485
-2.41758,1.07796,-1.31761,-1.55757,-1.00287,1.95581,-0.32158
-2.47253,1.07419,-1.31575,-1.56313,-1.00017,1.95859,-0.32429
-2.52747,1.07179,-1.31457,-1.56667,-0.99845,1.96036,-0.32601
-2.58242,1.05474,-1.31213,-1.58178,-0.99044,1.97291,-0.33836
-2.63736,1.04854,-1.31152,-1.58687,-0.98771,1.97746,-0.34289
-2.69231,1.04435,-1.31127,-1.59003,-0.98599,1.98053,-0.34596
-2.74725,1.03792,-1.31135,-1.59415,-0.98365,1.98523,-0.35068
-2.80220,1.02013,-1.31400,-1.60160,-0.97883,1.99823,-0.36389
-2.85714,1.01458,-1.31499,-1.60372,-0.97743,2.00227,-0.36804
-2.91209,1.00724,-1.31672,-1.60583,-0.97585,2.00761,-0.37356
-2.96703,0.99540,-1.32115,-1.60660,-0.97442,2.01619,-0.38253
-3.02198,0.99124,-1.32270,-1.60690,-0.97391,2.01920,-0.38569
-3.07692,0.98823,-1.32397,-1.60688,-0.97364,2.02137,-0.38799
-3.13187,0.98100,-1.32724,-1.60650,-0.97314,2.02660,-0.39353
-3.18681,0.97790,-1.32877,-1.60613,-0.97301,2.02883,-0.39592
-3.24176,0.97470,-1.33047,-1.60554,-0.97297,2.03113,-0.39838
-3.29670,0.97003,-1.33304,-1.60458,-0.97295,2.03449,-0.40200
-3.35165,0.96503,-1.33572,-1.60368,-0.97288,2.03808,-0.40588
-3.40659,0.96402,-1.33627,-1.60348,-0.97287,2.03881,-0.40667
-3.46154,0.95349,-1.34301,-1.59982,-0.97347,2.04635,-0.41490
-3.51648,0.95241,-1.34372,-1.59942,-0.97355,2.04712,-0.41574
-3.57143,0.94928,-1.34585,-1.59813,-0.97381,2.04936,-0.41821
-3.62637,0.94405,-1.34960,-1.59569,-0.97439,2.05309,-0.42233
-3.68132,0.93994,-1.35266,-1.59359,-0.97492,2.05602,-0.42559
-3.73626,0.93789,-1.35422,-1.59249,-0.97521,2.05748,-0.42722
-3.79121,0.93697,-1.35495,-1.59193,-0.97537,2.05813,-0.42795
-3.84615,0.92745,-1.36381,-1.58412,-0.97784,2.06489,-0.43555
-3.90110,0.92454,-1.36693,-1.58103,-0.97890,2.06696,-0.43789
-3.95604,0.91913,-1.37356,-1.57390,-0.98146,2.07079,-0.44227
-4.01099,0.90474,-1.39758,-1.54395,-0.99298,2.08097,-0.45402
-4.06593,0.89803,-1.40912,-1.52935,-0.99871,2.08570,-0.45954
-4.12088,0.87738,-1.45433,-1.46737,-1.02396,2.10019,-0.47682
-4.17582,0.87143,-1.46740,-1.44931,-1.03148,2.10435,-0.48185
-4.23077,0.86544,-1.47993,-1.43221,-1.03861,2.10852,-0.48694
-4.28571,0.84436,-1.52459,-1.37096,-1.06446,2.12310,-0.50505
-4.34066,0.84005,-1.53462,-1.35674,-1.07060,2.12607,-0.50880
-4.39560,0.83705,-1.54126,-1.34746,-1.07461,2.12812,-0.51142
-4.45055,0.83540,-1.54508,-1.34206,-1.07696,2.12926,-0.51287
-4.50549,0.83178,-1.55368,-1.32977,-1.08230,2.13174,-0.51604
-4.56044,0.82471,-1.56996,-1.30674,-1.09233,2.13657,-0.52227
-4.61538,0.82109,-1.57923,-1.29322,-1.09829,2.13904,-0.52548
-4.67033,0.81921,-1.58407,-1.28615,-1.10142,2.14033,-0.52715
-4.72527,0.81762,-1.58858,-1.27940,-1.10442,2.14141,-0.52858
-4.78022,0.81728,-1.58950,-1.27803,-1.10503,2.14164,-0.52888
-4.83516,0.81623,-1.59254,-1.27345,-1.10706,2.14236,-0.52982
-4.89011,0.81621,-1.59259,-1.27338,-1.10710,2.14237,-0.52983
-4.94505,0.81604,-1.59312,-1.27257,-1.10746,2.14248,-0.52999
-5.00000,0.81603,-1.59317,-1.27249,-1.10749,2.14249,-0.53000
-5.05495,0.81585,-1.59380,-1.27152,-1.10793,2.14262,-0.53017
-5.10989,0.81583,-1.59384,-1.27144,-1.10796,2.14263,-0.53018
-5.16484,0.81582,-1.59389,-1.27137,-1.10800,2.14263,-0.53019
-5.21978,0.81582,-1.59393,-1.27130,-1.10803,2.14264,-0.53019
-5.27473,0.81735,-1.59459,-1.26858,-1.10937,2.14161,-0.52887
-5.32967,0.81875,-1.59493,-1.26660,-1.11037,2.14067,-0.52766
-5.38462,0.83542,-1.58930,-1.26107,-1.11395,2.12940,-0.51327
-5.43956,0.84030,-1.58724,-1.26031,-1.11460,2.12608,-0.50910
-5.49451,0.84382,-1.58530,-1.26059,-1.11469,2.12368,-0.50609
-5.54945,0.84977,-1.58167,-1.26178,-1.11452,2.11961,-0.50102
-5.60440,0.86461,-1.57253,-1.26512,-1.11391,2.10941,-0.48848
-5.65934,0.87012,-1.56857,-1.26748,-1.11318,2.10560,-0.48386
-5.71429,0.87556,-1.56470,-1.26980,-1.11246,2.10184,-0.47931
-5.76923,0.88942,-1.55405,-1.27724,-1.10996,2.09219,-0.46780
-5.82418,0.89581,-1.54928,-1.28052,-1.10888,2.08772,-0.46255
-5.87912,0.90221,-1.54459,-1.28369,-1.10786,2.08324,-0.45730
-5.93407,0.91325,-1.53539,-1.29127,-1.10516,2.07548,-0.44831
-5.98901,0.91669,-1.53246,-1.29377,-1.10426,2.07305,-0.44553
-6.04396,0.91965,-1.53016,-1.29553,-1.10367,2.07095,-0.44313
-6.09890,0.92487,-1.52590,-1.29904,-1.10243,2.06726,-0.43892
-6.15385,0.92751,-1.52371,-1.30090,-1.10178,2.06539,-0.43681
-6.20879,0.93014,-1.52149,-1.30283,-1.10109,2.06353,-0.43470
-6.26374,0.93093,-1.52078,-1.30348,-1.10085,2.06297,-0.43407
-6.31868,0.93670,-1.51474,-1.30983,-1.09840,2.05886,-0.42944
-6.37363,0.93759,-1.51377,-1.31088,-1.09799,2.05823,-0.42873
-6.42857,0.93846,-1.51277,-1.31200,-1.09755,2.05761,-0.42804
-6.48352,0.94343,-1.50626,-1.31987,-1.09437,2.05406,-0.42407
-6.53846,0.94797,-1.49828,-1.33073,-1.08984,2.05080,-0.42044
-6.59341,0.94800,-1.49823,-1.33079,-1.08981,2.05079,-0.42042
-6.64835,0.94995,-1.49424,-1.33648,-1.08741,2.04938,-0.41886
-6.70330,0.95449,-1.47425,-1.36889,-1.07332,2.04609,-0.41516
-6.75824,0.95498,-1.47198,-1.37258,-1.07172,2.04573,-0.41475
-6.81319,0.95592,-1.46576,-1.38300,-1.06717,2.04504,-0.41397
-6.86813,0.95629,-1.44199,-1.42504,-1.04869,2.04469,-0.41350
-6.92308,0.95622,-1.43615,-1.43544,-1.04413,2.04472,-0.41350
-6.97802,0.95462,-1.41562,-1.47284,-1.02775,2.04580,-0.41459
-7.03297,0.95413,-1.41007,-1.48298,-1.02332,2.04614,-0.41494
-7.08791,0.95343,-1.40608,-1.49049,-1.02002,2.04662,-0.41545
-7.14286,0.95139,-1.39595,-1.50973,-1.01159,2.04804,-0.41696
-7.19780,0.95019,-1.39000,-1.52101,-1.00668,2.04888,-0.41786
-7.25275,0.94968,-1.38744,-1.52585,-1.00457,2.04923,-0.41824
-7.30769,0.94911,-1.38511,-1.53033,-1.00261,2.04963,-0.41867
-7.36264,0.94845,-1.38288,-1.53470,-1.00070,2.05009,-0.41917
-7.41758,0.94822,-1.38214,-1.53615,-1.00006,2.05026,-0.41934
-7.47253,0.94737,-1.38014,-1.54026,-0.99824,2.05086,-0.41999
-7.52747,0.94705,-1.37942,-1.54175,-0.99759,2.05108,-0.42024
-7.58242,0.94673,-1.37892,-1.54286,-0.99709,2.05131,-0.42048
-7.63736,0.94630,-1.37835,-1.54419,-0.99649,2.05161,-0.42082
-7.69231,0.94374,-1.37639,-1.54954,-0.99400,2.05342,-0.42282
-7.74725,0.94302,-1.37590,-1.55093,-0.99335,2.05394,-0.42338
-7.80220,0.94115,-1.37508,-1.55379,-0.99199,2.05526,-0.42484
-7.85714,0.94092,-1.37500,-1.55411,-0.99183,2.05542,-0.42502
-7.91209,0.93810,-1.37500,-1.55629,-0.99067,2.05742,-0.42724
-7.96703,0.93756,-1.37503,-1.55666,-0.99046,2.05780,-0.42767
-8.02198,0.93577,-1.37530,-1.55760,-0.98992,2.05907,-0.42908
-8.07692,0.92872,-1.37787,-1.55868,-0.98886,2.06406,-0.43468
-8.13187,0.92613,-1.37898,-1.55880,-0.98859,2.06589,-0.43675
-8.18681,0.92339,-1.38030,-1.55868,-0.98841,2.06783,-0.43894
-8.24176,0.91389,-1.38615,-1.55616,-0.98869,2.07453,-0.44658
-8.29670,0.90999,-1.38875,-1.55479,-0.98895,2.07727,-0.44974
-8.35165,0.90710,-1.39077,-1.55362,-0.98922,2.07930,-0.45208
-8.40659,0.89358,-1.40279,-1.54386,-0.99231,2.08878,-0.46314
-8.46154,0.88810,-1.40785,-1.53961,-0.99371,2.09261,-0.46766
-8.51648,0.88376,-1.41230,-1.53548,-0.99515,2.09564,-0.47125
-8.57143,0.88119,-1.41495,-1.53303,-0.99601,2.09743,-0.47339
-8.62637,0.87199,-1.42516,-1.52302,-0.99964,2.10383,-0.48108
-8.68132,0.87114,-1.42612,-1.52208,-0.99998,2.10442,-0.48179
-8.73626,0.86863,-1.42917,-1.51889,-1.00118,2.10616,-0.48390
-8.79121,0.86626,-1.43215,-1.51570,-1.00239,2.10780,-0.48590
-8.84615,0.86526,-1.43354,-1.51413,-1.00300,2.10850,-0.48675
-8.90110,0.86346,-1.43639,-1.51070,-1.00435,2.10975,-0.48827
-8.95604,0.86343,-1.43643,-1.51065,-1.00437,2.10977,-0.48829
-9.01099,0.86113,-1.44156,-1.50364,-1.00724,2.11136,-0.49026
-9.06593,0.86046,-1.44344,-1.50094,-1.00836,2.11183,-0.49083
-9.12088,0.85960,-1.44764,-1.49431,-1.01117,2.11244,-0.49159
-9.17582,0.85930,-1.44994,-1.49053,-1.01279,2.11265,-0.49186
-9.23077,0.85915,-1.45229,-1.48650,-1.01453,2.11276,-0.49200
-9.28571,0.85928,-1.45381,-1.48371,-1.01576,2.11268,-0.49190
-9.34066,0.86640,-1.46518,-1.45740,-1.02771,2.10783,-0.48604
-9.39560,0.86798,-1.46768,-1.45160,-1.03037,2.10675,-0.48475
-9.45055,0.87141,-1.47135,-1.44211,-1.03476,2.10440,-0.48193
-9.50549,0.88524,-1.48141,-1.41237,-1.04880,2.09486,-0.47059
-9.56044,0.89056,-1.48635,-1.39904,-1.05507,2.09118,-0.46626
-9.61538,0.89406,-1.48922,-1.39096,-1.05889,2.08875,-0.46343
-9.67033,0.90127,-1.49475,-1.37501,-1.06647,2.08373,-0.45761
-9.72527,0.90334,-1.49610,-1.37087,-1.06846,2.08229,-0.45594
-9.78022,0.90393,-1.49651,-1.36964,-1.06905,2.08188,-0.45546
-9.83516,0.90469,-1.49697,-1.36818,-1.06976,2.08135,-0.45485
-9.89011,0.90472,-1.49699,-1.36813,-1.06978,2.08133,-0.45483
-9.94505,0.90486,-1.49706,-1.36787,-1.06990,2.08123,-0.45472
-10.00000,0.90489,-1.49708,-1.36782,-1.06993,2.08121,-0.45469
+0.00000,0.89672,-1.14483,-1.60108,-1.19515,2.08894,-0.46310
+0.90909,0.86014,-1.15888,-1.59376,-1.20231,2.11354,-0.49311
+1.81818,0.78343,-1.22114,-1.54642,-1.22208,2.16489,-0.55978
+2.72727,0.73242,-1.29426,-1.47359,-1.24837,2.19812,-0.60696
+3.63636,0.72427,-1.30838,-1.45836,-1.25398,2.20334,-0.61473
+4.54545,0.71705,-1.32065,-1.44531,-1.25883,2.20795,-0.62166
+5.45455,0.71180,-1.32990,-1.43534,-1.26254,2.21129,-0.62674
+6.36364,0.70894,-1.33521,-1.42951,-1.26470,2.21309,-0.62951
+7.27273,0.70580,-1.34125,-1.42281,-1.26719,2.21509,-0.63258
+8.18182,0.70304,-1.34727,-1.41584,-1.26974,2.21683,-0.63528
+9.09091,0.69707,-1.36317,-1.39643,-1.27676,2.22059,-0.64116
+10.00000,0.69705,-1.36322,-1.39637,-1.27678,2.22060,-0.64118
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index c1306d7..fd83f22 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,183 +1,12 @@
-0.48757,0.61206
-0.48757,0.61206
-0.48609,0.61358
-0.48461,0.61510
-0.48017,0.61662
-0.47869,0.62117
-0.46832,0.62573
-0.46239,0.63029
-0.44461,0.64548
-0.44017,0.64700
-0.43424,0.65307
-0.42832,0.65915
-0.40758,0.67130
-0.39869,0.67434
-0.39276,0.68041
-0.35869,0.69256
-0.34980,0.69560
-0.33943,0.69864
-0.31128,0.70472
-0.30684,0.70472
-0.30388,0.70472
-0.30240,0.70472
-0.28610,0.70472
-0.28610,0.70472
-0.28314,0.70168
-0.27869,0.70016
-0.27425,0.69712
-0.27425,0.69256
-0.27277,0.69105
-0.27129,0.68953
-0.27129,0.68497
-0.25647,0.66522
-0.25351,0.65459
-0.24906,0.65155
-0.24610,0.64244
-0.22981,0.61510
-0.22684,0.59991
-0.22388,0.59535
-0.22388,0.59383
-0.22388,0.58928
-0.22388,0.58776
-0.22388,0.58320
-0.22684,0.57864
-0.22832,0.57257
-0.24610,0.55130
-0.25203,0.54523
-0.25351,0.54067
-0.27425,0.52092
-0.28017,0.51333
-0.28462,0.50877
-0.29203,0.50270
-0.31277,0.48903
-0.31869,0.48447
-0.32758,0.47991
-0.34388,0.47687
-0.34832,0.47384
-0.35276,0.47384
-0.36165,0.47080
-0.36610,0.47080
-0.37054,0.47080
-0.37647,0.46928
-0.38239,0.46624
-0.38387,0.46624
-0.39869,0.46624
-0.40017,0.46624
-0.40461,0.46624
-0.41202,0.46624
-0.41795,0.46624
-0.42091,0.46624
-0.42239,0.46776
-0.43721,0.47232
-0.44165,0.47687
-0.45054,0.48295
-0.47869,0.50573
-0.49350,0.51333
-0.54535,0.55282
-0.56165,0.56194
-0.57794,0.56953
-0.63424,0.59839
-0.64609,0.60598
-0.65498,0.60902
-0.65942,0.61206
-0.66979,0.61814
-0.69053,0.62725
-0.70090,0.63484
-0.70683,0.63788
-0.71127,0.64244
-0.71275,0.64244
-0.71571,0.64548
-0.71571,0.64548
-0.71571,0.64700
-0.71571,0.64700
-0.71571,0.64852
-0.71423,0.64852
-0.71275,0.65003
-0.71127,0.65155
-0.67720,0.66370
-0.67275,0.66522
-0.63868,0.67738
-0.62979,0.68041
-0.62238,0.68041
-0.61201,0.68345
-0.58831,0.69408
-0.57794,0.69408
-0.56905,0.69712
-0.54535,0.70168
-0.53498,0.70472
-0.52461,0.70775
-0.50535,0.70775
-0.49943,0.70775
-0.49498,0.71079
-0.48609,0.71079
-0.48165,0.71079
-0.47720,0.71079
-0.47572,0.70927
-0.46535,0.70472
-0.46387,0.70320
-0.46239,0.70168
-0.45350,0.69560
-0.44609,0.68345
-0.44609,0.68345
-0.44313,0.67738
-0.43276,0.64700
-0.43128,0.64396
-0.42832,0.63484
-0.41647,0.59839
-0.41350,0.58928
-0.40313,0.55586
-0.40017,0.54675
-0.40017,0.53915
-0.39573,0.52092
-0.39276,0.51029
-0.39128,0.50573
-0.39128,0.50118
-0.39128,0.49662
-0.39128,0.49510
-0.39276,0.49054
-0.39276,0.48903
-0.39573,0.48751
-0.39721,0.48599
-0.40313,0.47991
-0.40461,0.47839
-0.40906,0.47536
-0.41054,0.47536
-0.42091,0.47536
-0.42239,0.47536
-0.42684,0.47536
-0.44017,0.47536
-0.44461,0.47536
-0.44906,0.47536
-0.46387,0.47536
-0.46980,0.47536
-0.47424,0.47536
-0.49646,0.47991
-0.50535,0.47991
-0.51276,0.48295
-0.51720,0.48295
-0.53350,0.48599
-0.53498,0.48599
-0.53942,0.48903
-0.54387,0.49054
-0.54535,0.49358
-0.54831,0.49814
-0.54831,0.49814
-0.55128,0.50877
-0.55128,0.51333
-0.55128,0.52244
-0.55128,0.52700
-0.55128,0.53156
-0.54831,0.53611
-0.53942,0.56649
-0.53942,0.57257
-0.53498,0.58320
-0.52461,0.61814
-0.52461,0.63333
-0.52165,0.64244
-0.51720,0.66067
-0.51424,0.66522
-0.51424,0.66674
-0.51276,0.66826
-0.51276,0.66826
-0.51128,0.66826
-0.51128,0.66826
+0.20381,0.36380
+0.25473,0.36700
+0.35332,0.38197
+0.44758,0.42152
+0.46491,0.43007
+0.48116,0.43434
+0.49308,0.43862
+0.49958,0.44183
+0.50716,0.44503
+0.51366,0.45038
+0.53100,0.46320
+0.53100,0.46320
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc
index 39cc8b4f8587f07e50e07745e3c0e25cbebd5262..962f7ffb2839b04d64de59bffc301462294b51dd 100644
GIT binary patch
delta 82
zcmeBRUdPORnwOW0fq{WxUzE>AZZ1aSO#O`f+*JMa{NjTAl*A%1&j`WIDa}jKFDOc^
a$Sl?`D9X=DO)e>(ti))}cx!SLqaFYl7aJh}
delta 48
zcmZ3-+`-I!nwOW0fq{XcLnUG(Hy0zXvwl&2eu;ireokp_>f}U5;mOgADvZA-H!$h}
E04Q(`WB>pF
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index a9cb9681827fd5bf80a51b5ad0c1febe78fd9923..f483a01458224dd004f83b838d10cbd14cccf706 100644
GIT binary patch
delta 11344
zcmaF0m-)d1X1?XTyj%<n3=D#N(dp`+C-O-!p4q4#!00ZN!jK}EW0GqcWy;8q!jK}A
zW0q?kWe#Qw=UC)gMp-g4a5AKbv@k?jF)_F^q=>dKq=+qJVqjR!1TqkWqpT+%V3bs}
z0n1CaFr-MK$=jwd1~X_%Z+^ho$|$DEc#F#`KQpf+xFoS8Rg>`+OGsvJ>gH7Db?l5v
zlSR0vvuQFgFcfJ`-pHN6tpQ>}_?u;TzA-ZDZWiX_V~2=qfh9NVh*~hQ+A%ONcx}!U
zuNLI?1PObA2%pLCHH;Z;Co5}eF<##6r)kN^Xg`^^M3QM8^JM-glg-z)JQ!IfFcyhT
zR@4#Ke^SGc1@aCUXMuwY#7tq%W(LtkE-3QclOxLIq)>!-LCQcFBsw|XQf~5rL_Vn$
zmN`tz7#SE=GlHZ*n2{mR16he!3=;!GEn6)+ND!m|gtNdFNTUcLEMQAvnKSv~Dv`-{
z`?$CfPGy~ZT}M-zO%h}P7}s*37yvetErq>?Ba?BmqowfVAYG-&iRGe`J9WhwIVLaE
z{a#<B&cMK+$#jdc@D^)Ge0h9okun1V!%L9kZ?T2MCnXlAY6=&D!uS?zd~r!?K@m8p
z{XpUE&A`BLi@hMfASX4gq$m(16apfAK;oihnZ>1vIhj?d@wth4nFXaei6!|(x44S(
zlk!XA^AdAYZwZxUPG-}W(@6lS=SWX20XyIpOG;u%;w|QU-^`*=ka6K47jhOwfY>pU
zWAs(bqd?4PkS;E;mUxf@ZgE4{MVaXtB}K^$3=GAb3=9kkFi>PWd5*pj`xlS|H`C;+
z`q%0^tsn3U_f%ft*ZaW0#T(6dLs<NZu<8|IgAHz%gsm<JTip<mxFVu<Ma1&Di0dU0
z*ApREeB-b9Cf;!IxZ)Ca-6j5#OZ-Kb#49d|AD9{48Iu`5Fo3C#42+^Fj9<Xy2V|0s
zLErd_p2KxL&r5oq7hUzdujqMS6w#jGwSXI^ReoTz10ZGxFoMhu1e+ZQ5)uuZ%xqY~
zXur9_(2JQdY4Twc7Zp&v6#0X~$O9BcEG3z_sYQh#J~$qkQc8+^CTp8IvO$7(a*?T%
zCph9jQG1Ietu!zB7Dqfdj^g7r*`Og0GK#ghB(bQZC>O+LPR&cX#gdYlmR8IL59cD=
z$$w217>zf}nQdfbwBLNed>Sib(&Tj8IB9o~1Ij@J$Z5Bj@(OOTR;3o@7Z-U>eqbxk
z21+$W6_fwk8oSkiRMmorIuHSOTTwlT+W;a!e!RtAoLH9Xlb?=ACq>mD$utm=F}ci6
z&IpuhZm|_-rstLB-eOHFO3Y0yiUB#Fy(~2+J~=m~m=)&uB3qC)x5>xslo_Kozp}f{
z$Z5~Oz~HCJQPeQ`oP)YnEJz+4jx8XT8v_HwN`@kk8i-a$kSK^)F<IWxh~Jh~@&f~$
zsGFSNsKW+U>^Qk+izMTL$!i={7!@X8bX1jANa1N=jN(n<ZDEMwOW|u_h~iJ-Z()cM
zNKu?D=;SFb+rkhfn<Cf35G9u)-@*_jpTZi<psBPu-f0&L(>m_W((b;@lXrRWOz!jK
zV2qut>7^{pg<1&JFvN>aR`k-ITrr2ArG_D148&pNn(XT(TQ3eSI~W)kY8c`rVC)p0
z8ip)5uZkNi3omI>cxxEqrC@4n7_+3|Y=$hjcs8gQVJPB8mQUf!W&x=!X7XZaVg$)>
zfVdz$S>IctK82sKt`vb9hIm<!77(ss%#s7qAY8+c1s6v$2QH6cJ6xuQAs)om2ART;
z0tyOwFcTasNEHQKG(~XoW^ZLt1Yal{Ttyc7lsHcQXf7oY50|Q8$bz#MC{DiUFFrZX
zpHm@47`5VLWI$1<G+FVS%4F+(yhwJ4K<yANaR=GKg|K7tYab<n6wwl2n5!8WveYLl
zdWkS)X-qz-CMHzH%D{kAc5-DWA1GO9PTuGtI=Rc2%apH%Aq!L!gN;iOwP9djNI?}7
z1Bs!Ss0Fc4d-6hmNpT&pP6jkfk(H?H!j#l7X6eD$MPViJFg622Ndf}{$kfTXojj9U
z{bc1q1v^x_hOvezMI70k3ydZo^iy?{SjNJ@uo|4qKuT*E;*CKx2-h%XffFT&T|BuY
z6GTIBiX>`|VVoT2pUPBHJUP){UEB(y29yc1;Oqs~pr~Y&0R`{m|Ng9uHj@wfDb?37
z#Dhu_u#P-Q)Bpktf`XC*)iPu~HH=v@Fm*Ky@d!4O(~B0?FlK=oL12YNvr6*83=lDS
zj=u?`J;)_&wahil3mhf~vWklGq1wmD0F#l&kU?>=<K)CZc~y8qK#3@s8ip)*z%FnC
z*~2Pc!<@-D`J=xuqw{3NbE@?W!3>&GepQ@?M&^13dIoxiMM|Jj64b~jQUS5lK!hrY
z0JTJ3f~t~RtR?yRIR>|Qit>|+;<G@F$9QuvF9*geN@8GONH+$9-}cWz{s!S91CX$M
z>+8TBa-0rDx(u=m3`L-nm09tffuTqrWQ{YZee>`C|Nlh^lPiMcI9!ULHP5^tMecNv
z3Q&Pm)G_%~kTPTT<aa?5+~E312qaZISuj{zh_@&{H8mbwnZ#$N+~UoNpKK7U!dN;v
zG`K0S4OF|Z<maUp-(t!uECMxdirPVXJ3$i6W$}f#m=og*i@=o>sHiLw1j&Yhh#(M=
z0V2Q^C8!}<)IC`vq^us)y1K=hSX7i)Sp-V7MJb?G5euYl26^BXZ*gWyYCNd&F3HT#
z1KV2+$^xMF52Stq)n8SjL7)bhZ(?3zdTNnge0*kJW=VWJD0LJ&vobI=Fx(K4=wRvL
zydkG_Mb5B;<qC)N4Q~F;vY{o6;WxPXd+cYVUE)@`z^!saQFV#bbw#sFie?+?E-JcR
zQFQBYxxyodOZ)>DgM!MOs_XJ5m*h=0#9fqkxgzfZ(jl{XVptM0qy6MLQGwPg8T~Yw
zz>Y5h1;$E7un&ttc~$}Ji(4$|sU?9`f+*eq37Ji9h>`}I5+r*J6gW&pyFeicGN$M_
zsMQBGWHNWO9HZrAt!QZlP<8B=3@)}o$%lb~fs=uO0Tk??yI2?)rcd5*MU3@94P%4k
z<O6vIEWr#V?34Y^N`u<YEFv7G43J7}a^Xzz$!lZyBte?N=F~8jz}XB8SzMC~{Y03u
zxIqO4A5NLcRWX8$yp#VMiBGQa6J@Dk$l{$`*eL<(bifVE;scuxsg!spA50UT9CbyR
zE102%u>sU!;h!9MMF*r+0K}Lac}0aSjfvjQ76dt)sRZs7#+u0r-I9~v&tw(H?hzrF
zM<#Dp69c(k5GGI$a=j*#AEcWiTm;UaiSTFz_ZPt#jy*30k~y}4a+zRSeo=X1QHpnF
zUTSV)NoI0!Q3ps<Hi$rEMv>Bjl*E!$H&ClEASW?7H8(Y{q-Z6`1m=9-w4!{5dIpA~
z0+6OvAYwHnS%KS>?1k~^nPsVY#h}!q07;ugpmqXU4uf@TY?v4riq9~Da~MiC;uq_v
zzacI?rD}Tpr232E8dt<ME(mBm6_=jjKC!lbO6^*vHCz|Ojo|1j7lWY46#>-+DNC|8
zsBF>P5q?)qV@bw}k~LLpYA>qUUQx5%TePR*1j`w&GrSk=!>-teePm`7i~#i+1S3Fw
z2Eho%uN+`QG**bLk=hWlCGr5v6^DR}0)bZq0xt*zf}B%1rFKEcm3mdvE8=F?#jP)i
zTVE8ny&`UVLBRF_%*plF#kDVqYhM)Cy&|r=qU4Ic{dIkpOZqMs_1&)MyIt4!zNGJc
zQQ!B9zV8Kb-wOi1P*0KIr+P{~6?9P`_=-UA1%cqZ0>W1WP&|x~LwH!#{EE26b#dEE
z;<gvX?XRea+aC})B6&piM9CHZ#4C<T*B#R@Ii_E9%)H{5c|kn$f<R^wsNf1xfD}2m
z*vgAi^O7^bg%@h6^Dj<~F=4W5ygZ}*X4m)vmU?gz-wjG^JfMD%TS;(gUU7aAq~}%w
zN^g5Wl7i`}C637@rHMHq&H--uMaij<u68L%MHPquWq_i?Ahqm?WvNA=g8cwUJP4Fr
z*}(<qE$+0E_@dOrl+3*JVsHnjNVO59gDI_~s0pOF8B{{D!}>z71igQ9WQr_f^5*gs
z1x6WAX_*WvL}6tRsImqZmMqMZ-_Hn|%$gP<QNxf0sz1R>QW&9QDIq1SlOH6APmY-h
zF4wp~1;pg#X}rALV0Dn<j0>FU6U)_j;R7Fe%TP7)fi<$H@lJl7zymXlA1sJ&8XwFw
zMuEv2Jw*7jkgei}Nlu>C&CRqx2-I|8Vp=nKW4y@ZRcTz(!U$)@i@?|^EWr$#teeBr
zJy{^bCLx-Px41!LBJs(IIXS5*eww1USmQx;(JeMGl~M#Mjf&2K!WU8gfLjFMstH`0
zodd~&%ABI}AQmXniohk@Y7iG(?tmJ1;9}?&AIRMBA}3H9^UQN~y~Ui8QdqPbq!PJ`
z1Mxu(geoDFY7#tv(+(<#G#Dp4=Q!4*ROokk_^<G&EC^W=wLxTy<PMiBHa-`5e6R5M
zUf}V)At^gYa*phRk}K-wS0pX2OWIzNw7n>4e?`*1!==Zo!|N%AX1|L({#SVXFYx$d
z8Vr?ly1?UqLr!T<=A7IWE?0EyuE^P6mvgx!=W<cb?TVaRhf9zD4SD4`m2+xWgj~_J
zzasB&UEcMQyz51I_bc-5;I_l${W+S9_LDarm7L6!d&?2j{wrDl?zOW*+jXF@*b5@|
zfrt&DJi!FX6Rc&4Ii;y61^B+nvUv)OTPNG)g)rJrUeF-9c}?CWW>ZiT^5uU928QRe
zW`cn|q!|mbiKnOnWHczZ799eyK<%-j&6775se6IiZID_Eo=d^48%>BCz-$y3++|>3
zK#w+1k~%P1rT7JOJo5s}W{r|VOe!@DS%}oho(;;13`L<Oim(*Rz>p<6`A3;DqZlZQ
zfEp6;gq?-d>CX~_Wd~_^hd%|>YQQu|5@gWiS7p465QC*aeB1_0!VFeM9v{H&2q~Ca
zet3@u<OofU&C|+tnCd}MQ1k>8x=%p_DC8lH0@+*4i8(n%pwjdfpJPc$QD#zUNvdm6
zQGOBjcmZjD1|mS+21r>7&XVAq7Yvd>L=`;$Mj|yJuw`3ZIaU+eM85zkY*|WDi%TF;
zj%b-ruBlREe+<f$3QU_<Rz)&WF;|k6FTKE-mco2_fk);7kIZJr8eQfjP+a+Gav@^l
z7B{?33rvAz)6Jmp;Z85gEGS4ViZ4sefkX@<CO`!yG}GK-DojDiGxsK6sFz@pVxIh}
zek-H><^>JbOrqc{iO>$toYyCRXp$BJHQ$OZfvP(uaCKK?J6X0_kI`nbPqP;!(@);e
zeBT*dp>skUjcyJok$|gMkTJK|iW2iu@^g!pgM^Wb;mMa;G#I@m|87ZQw4WT;D#iMR
z5!$KXzg)wBG!S7qnLSNuva7brWUdxLSx~hK&can(paF+?P^}E+=V_L(OrE$$WU@)A
z*yNg0p2;iy1qI=<HH^g~H4F>5z{5uDX}pZwlMAOx2-GswFl6z7^)oPl`V@ECWOY$Q
zU~)A~DJ&_htxz(J2{gV_!<5Ab8h=r$VnKCF3PUhM325vTsymB&vO<9bXz&B>AdnJG
zHfY<Osb~%;c)$cG++Tu9E3lX*Kcs-f8e<?m@Q^P03gUu;`3;B#Zpnjk-Yt%z{F20y
z%zTZ!TZ|!^%;569=ng1NvF9ch6o6ViAo~<x?V9_OH?}L(2Qf1+6oZO_1_qRx5Ihf{
z_JM(g*NO42oWh(8P`zhyUES-Fy4OW@pDXG<7v+4f$oXCf2)PoFbWtv80?QQc3EW@V
z7~~b_WL}rkyCkOvY7RSGlykfy=LnL$AuRWSfst47hJeU)zDax+1r)9b)GJ&NQ25Hm
zATBv2V@B!3{3-bz?l%-vmT+BBFz#@l;5Z?1g7XZ)8HO{2CwN@sk;Nw^r8q}(LCTWs
zi&BOiE<N5I-W}dgrDW!4EHIp@Jx3cR1RkqKbayAO?ciaZ!4682?=OmNzS8l8NiZmd
zvxYH+3l#NC3^mMI!jms{i-)t+Fs5+A2Y(rASP=D2yeKpQFw`)naHFUZgUQ4rS2pk}
zW`X47KpoM^UAjV(59n}9rSL$<HV4cJn~i%lz!QXwlOJ|TGV)KJ*eAvY8a7Q405K<P
z_6be4>=tA+ogAnwp;60PVh-~l14D`+Xb1%{>F2@_D^tr>!<r>CSz1?NQkeipk8BNV
zEpv%ANHfy{InV%IeGM~WU^rd@W*UmQLKx->)w0zv)gaCK)i5Is7_xz)u16Tf4B-?(
z)RATvhFH;Bwi;8W8nzS>P$`2HZ<vOO)i7kin*s|YCo}e{O*UB1$tb#6vp=4(9yFlm
z2hPbwps_VgMhF{J>Vqc}A>(|YJP#hVdkJQt4*R87i>z~hSk<y0#;|`08pbdB0UE30
z2N9qGOj8L`P=HF~TP(>rnFU3g7#LC*AZ0YTC&gA=keZs}RkR*dJb-#gx7dqPi!%}n
zQg5;3WEMkbDM5WD$lwsDe!s<;nOBmUUX)mp3aR8lt$|y-WvMy&$(bdUKAE|hC6m1-
zTGtnW-2|S!;(*B(fhG-Zv6bY<=N6~mV#`QPOi3-e1)3+zEx098P@0sJS)7py9mOmL
zO*m;ndSpc(K&2B~VG2mB2-M`c#h#p-0yc&-HMgLo62t^|)rvtSs)B+xXe19h(9p%e
zz)&TO(zq{9%}WVPNdXmUCCr;IP7Gyay&)z)A$fBBWT(m5lLg_t<MkK0Rql$4PYIt9
zv_KRz0&qps@uH&R0h1FVXQZxpCSG(-yeOJ<MKr0y?S{B)hf9ygR}ltD<(WKlcse}q
zib~Benix4H5>%Y&gNX@@Hv}ZF2q;|_(7Ys|xkCA(fZi1Wy$fJ;LrA*A`L4Y3%(6LU
z^J{0-&a9tPU*F-}<8_5c24s}s#E2;oAX9X}#0170N*XH!muOC4p29ODa3cQ%evm4o
z8ATIgr^JGk7=j6?x)nhy#g^zH)IQ)5xWc1)ok#Z)kM4@(i#$eGc#JN<&<9-x9@Pun
zDn+0s!ju+915ld-()uU^B^OW&vS<;A1*(Z|vE&z|=Am}sL#Aml9-Ul2tsc}&nDXAT
zNSu*@L6f}*zd@|YIr+t@s3xh;&|y43xqe#M<W(~?8SOV;nNiNkIA^lYqGL{=4nWZf
zkTXDu6TR~YO8elxe$hFQ5=6fd?0S&Hi*hE1EY_<336ci2dTt588(SgH0RbSVfLgk@
zSTpkqN=t6B78m4XmVgHxZZYSi<`r3kG=d83B1J|921xjT$Ev~O(x9<tO%6!?3d$#t
zt}007Etd5B#GG4<skb=O^AjikUCb^FnrJNsjRt^IF1YfYEWX5o@$Tk;C9$m1U<1Ju
zw#bbpP_~eq?66#N^23!HOp+{<xmT%!!&`E)&#DjBhM*1{2gF%m?}9xJ%KSxrAeVy&
zPQb$=6F^*KlP8z2R%23UnLK}WF4Vw^wK+U1nL#7YphB!jb@IG51`6QjJ;K0G7{<I?
zqr;@fGTCmO{ABC3%5bA{)r~>Pfdx`XfeipV0n-4Ubp}i(AOoBj?Kd~BJI-hX?lGc<
zauGaHLYuus=8Oys@OJOystsyPRxFbjZ2&bcLH%F3jaGab#i=C<pqXvZ!j1UJu^Z*&
zctEiT3M7~@AOVCiGdDVDd9W}rd<M->HZa^!G69YG%irbTy})O2k;C!|hvfwrDuQ&h
zQ7i&2y#QGx>v>BEQQU!?0FnWB%U~AUA-f+G&0@u=CGnV=CU4oO$Pd!(!@|H&WCL<P
z++u!Bm=D40Jis>l-V#PN1T2MOwIjOK;+R&0bycZM-nLO*AIuJ5VPGhR28*&8C|DH0
z!D4xl!|Do$)dd(r1WUaZ$aM(Efak8jrM^ODUP-<Ja)GVMR3rrQ8gkTt1mIBv3Lyzp
zXCln2QbjgP52~ylqqYe_@`H*6$PbEOKUiJlu)e}!eF28RO~WE^pNG*+Q=tgF$YLre
z=}!X@(?JAi{s>YsodR(|gWW}!L9G2C0yN%KUj!ax0T*H57WzGq%x4hs4Mf}r5um!O
zND}0A84#fhBJ@FoA&4*m5oU}G44}0o$t6XWAigz-um=%NAi@Pi%mA4M8j=K03l+@*
zak)Tb7bvd3Og^|tQpk&mRrCV`5}7dh%_ezCdDbk(4-6Qj2jgV<&GP(Otco8P;6%)1
z-^~gV#;hKU9~dymyvdE5jnoBL)jlww5-Kr_ti~T0kVq%y$#*wT_O$xIz{CnAIN&U>
z7zY!pHAogpa4@mjfVk)cB&KFf?%9&U7`FN2maUuu;L-y$BU<zq6be5kZ`-HOm_7N~
zJ~Kw2$+G*ig}}AO83qQ1Tdd&5P?68%S^IsWH9_GBo<jl8L&8_r++qQ*&*IKW1Pxfk
zgVtjefkumwiX`ZIEU?W-KnC-GClZtMb8{2(Qi}X1J07rN15JY$#ZGQKAj4R;dESA1
zCh&rqB2PvJhRJ>F#5U_3zRzd}nnCtk$y^iwigVV&;>@blTU-UDCGmOr<%yXkE1?5r
zAWg_w13U{_B{g~01u4cglT(iRFxqe4c=R@#1SlN*ic&#d%K)VWP;(R1?VG&hj1A+?
z$q&zfRO+4OW3~nR93C`9VIVCLAR-dv6!ygA!qUv5R8R{tCpED+wFuR?$_rkMcP1aX
z&;h9cHm6-|U`zouST*^I{6VG!flOfqH|1_|rKiF?R1^o21X)p(17bykmF46orW6-N
zfds-qL@tN`)zd}b5h;{v%<HlW<J-ykmsc>_Z<e?c%)|z=wJ2$F;kD#wu+5<$&C(!q
z*-G<rGV`(_)g`<_DGCHBhy@X#P%lacDdj9lEy~Tz1GR!*fl?8&l^}lgWc}+ZjNdnh
zTrXw}2Nx9FxJwCFklCQNLJ??`qbLu=1x?~Y)@Pw*-7t{xAR=nA+|89v;5-fDYTDi6
zEXd5uPfpIr%m)vp-(t-x%`K=b0!JFSrUzHEpssxpsI5~38dfXXJDKxV4L3OOTtG=v
zVRGOtiODN&onbVYTzcD>F?#aK+s^T~_=~~wEudz0v0iR*I=Fd%i>oL(r=U^~yk-v6
zwl4y$vM2(r>Vgz8Acqy9df*p_O>TZlX-=wLQ7<C{189b%_|aySJFgk*nHd#suqa+Y
zMK@TqE})_tEb<pn(FYbKMwSmAjM0pYLLV5A$Qq^uW=8%G4A{w!VDT?t@&iMHIY_As
fyAL;`%?AeT<VUdh7cdD`lOQ&E;XPLtP&@ztY*==!
delta 8086
zcmaFxfce#4X1?XTyj%<n3=Fo05$VexP2`hc+_F(UfYFmJg&{>S$0XM@%9N3TlOaW@
zg(1p}iNT#AMYx3_MPwNh1H)=2kVX)WG6&0uwlJiKp~+ZG-p44(ZkfUu%%CZ``3GYw
z<L1fC>)06;COdIYXH#cjU?|d<e4jgkTMfj7@Hcz$d}CzP-t5T7#|{zK084I;618Ar
zwP9dj@Yp<Eyjqak9VF}lBD^NcY8o?IPY%@7V!XJyM$?i}#+HGBAsOyO1_lOJ1_lOZ
z1_p-DznC^()T&`*D%v|aP)D4xd-6hEvB_H>i%rfn<(aIN!96+4lzVcemDps1T7E|6
z$^E+W+<YK!f^Z7U<ej>jysVNS4j9+6!Su7Fu+^|lme(_%yzQ~*<ZL~0M)t|gdfx*y
znQk!_-eL`jFON?xQe<FYc*)4Xz;KH#Bt9vzI8{@)2;`4jtntMqsRcz~@A!cH<Ox#5
zUXWjqlbTji<PQ=G0uf%5&+AK1{-&>J5eHJik)B!tHu@G<QGQZ>Nqk;nZt5+Tl*E$6
zTg>^snMJ`MO`!}744UjkVIX$YWM=~vnFtUw5=102FfbIeGcYhHKtPf8<aq{0?4Llw
z^BE^!H@FtjVf}z#xTo?8zupH1R^DjF8v-I%1eC4_m|hofyd>awLgb2P^cBz88xAg4
z9D=SpL|<}<zUUBp#Ub_sGov$OJmUujF!hmvQ80n=3z+->A}1Fbm9T+higB{w6p_vU
zjSQI?6DAv)xu}4=U*rpNpBu=VEG3z_sYSUUJ~+6TQc8-vCeJi;WP|u`@&hv`J#hGd
zLhu$#T4`SLEsl6_XvD{BvO&ELGK#ghB(bQZD0^~%xfG-I<ZN>VMxD()<{KFqZ8zIl
zPGe<En0(bPPTCb@S22hHx#AX6UcoKas??(V;v)CSe)i&QULdKG$=UYC^%WpyC5Wg3
z5pZV|RfD)SAOhr_TkOS&WvM>->4-QfDhElXfQVF(+XPbcQrwDCQ&WmE%Tj&wQ&MlS
zq*i2>XtILce~Ya+Gd-^~_ZDkfQDSatQ4~lEdznmXPJD81N--#fgQKv>8f2gg0|P^q
z+~kc0!jpX*xEX^tM>yPO<OGGbpC(69&14%Vb**TSLU5=yfLI_cD;bJFY9MCVgG52Z
zvdO(pM*J46k{=l0M9JiHPCDS|E3%*DEXlZTvXrw5qwHjRXH{w06rL8wDBcv_7KSLk
z6uuUQDE<`w7KSK+6uHUu&YrBS!3>)6o9{dCVqv<+wRyIOFLOO(mMAz!F)%RHFvN?&
z*eP5!3|Vkq6*pK`94^aU!w@e4<JB-`!E<H}Ll#^-8<gW1inx*GQ+TpjK>CWAycn7o
zYZyI{wZ<ciVa$>ODFfjYUcv^Y@J)8~mGG%y$dU$W0bwLfG9W$(quZVZm#JZhhqF^a
zAt5_C(Nu&dOBSRJgj4t@Gy5rvBKQK?;Bv0Wr^I%$pqjKeT&9Krp=*KsWW^xy$@>C1
z#Zm-OOKC<1kiyApSIbRKxGymI&Ql&SguWCZsOiEb&ZwrBfN}`PK@1EGS;~_adW$fn
z2u~LGS7ORioBT0QjH!xsvZ%ibh*y{>K3QufFG~$Wy!vEAFVV@j{JBhdY8bLWB|g}Q
z6k$-oo`NbS0un0$WgV~@28JvRur*nllRpMZGHOlc_ZMYE7E;%SDrBf(%+i6ei^57k
z=^V-}iD6&>nKD^+CeP&eaXbdgSQr>qgOdVCMJ-cFJcx$i8pax?6j5Y{E-;uZ7^vzg
zhN`QEA>I(8g8^hUIJtrN#gj`w`5MGzU|>iQU&hG5uo^DQ$WW3#d1IhDw+U1MLrFG>
zKY3Z8^<>##UPen$oH0s)LUXcz5G$kAWWhkC`Wl9KPzDF<$&*A4AFv?EDh^bukoD9c
zCFvT5czE)zVa$R%t!QBlV-_g*!3Gx1D#->jK*Z!-fpX$zAQlMMFl52m3v4GhwkxwT
z*Dx=zo7~7LI{C#aK^7#A5HE6E+E3mXEUyet0VpvhQ^Sx2575bh!Q!l<HO!felO2PE
z8677d%u%sRVF+f>l<=$KG&C~TGte{8Gb~a76;_}Mwnz!YQUMXlAVL*X>9T<eOV*P7
z{2YU#gvkp-#Deu07#Py!TVDt6kmGbH0wtXyP@2iC_|CvkqzlsM2-5iP|Ns9*pt`mQ
zl+CJyo&Ej%T%AKa{oEBo{1rljOj8v#or<8<0avIZcLqo+2Z(5yY!<4lk_lpRf(RZE
z!3`pKK?Jzo5CX9(C)b8*Pd*kZ!B{x?erQu*6UebF`FW|ux0vz@A+|Sz6t;pSn9Je|
zZ!ssv7Z!nQ4p0OZ34&xpKtuqD0HwMjaP0+Z^AxpD-V|2m3Ti~$VofY6N~|nO1F1>^
zH49k4bqm-7w|I*)Q&QtWm1jw2ejeD~Vo=rqRV$DhA$oFdxD@+*Mh1rBH!Pd`!h0Dv
z7eyv9GulqBjSaM3$>^ua1lCpra>hzVu-V0+WTgPse~TqOwIr}gFbLFE@lDK2OiwM+
z0|}{5PK=cXn-V0u59BPSqD>$tf{ZEJ4{D1*4VnBVR*q3?GIyM`DyW|FLuurJLK)=Y
z&xaTp7^XASFg8d|j=f^Q7R*q>%D^z$ak+#}2`C|eQZA_20{IQhE@gn!10|r?0gEs&
zWU;}@36>hh686da(#0pQ@#dV|`dESql{>jTQIL^)vcV*g$t%5iKqU)H4MP^sWW|{h
zldW@kCO?17%LR&iu)&g(6RzlL1Tz$Yy~$SuDsI4`RRnGXgQK23F9i}!>n8tA60dIo
zg#<VmArgT|X+cV2Nva#DB@mF4n4FrMnpaY^7@R)xebY3Vz(IeDy)ZsKvn(~Q7?gMw
zAdyg%4r&^RqQ?Qa8L7*}z)-xIfq|ic;Rd%*hy4v<@hSCI)9okOUldlkBCK+ON9C!o
z_zd=mwo`1EIxX?KAgl*RU%41~1+MTYE>KybwIXFr<___@N-9e<R+y}@T4Q@r$?}Sl
z<zAyb76)98cpdS*XdQILI_M)aBX0<(Nyr-lY9;c9Fn;9#8=|rxWJ%Npku8!tT&_&+
zYU7a?7N24{#dd+n6-C1<!baDH%`XX?Ulg{yB5Zkq$Fc~N{DKs~A<A@%t-L5TFF6Ae
zs3>XYPO6%s1IUM<21NtI4SwOC>g)XSm-yu`@+)59SG>TXSY*3dIjw-D9-MjFLBYub
zYWBI61gGW|=NCcRqy?Y|*#eRjOiwLwOfD%+%n5N0aLX@BPKC793qdN%Km;g4i*|$5
zvL}|M7J*Xy4v;t~WkcKKx46?v;)_xfQ!?|?i^0vhBGp=u4yLq{qB@Y`dQc8PX{3Wh
zw@)_DmSuF_9Gb1b2yTcagWJ(e3m7L)%vm)#b9ve1t&dd%Y8c|-sjr4%0WYX*W@cQ&
zG&wO}RvO;7sbPrc2PaTa^}!s>pvkg%X1*s&eTXLGEpAZfBtAJYCnq(<PgCF)Ydk2=
z-eLn&DMjl*jyVb<5GfU0VuG{qF^~i(VHO<+u|WO?C;D4_AnoDcxbn<%b-l%$l2TX%
zN_)r|UIkRw6@f~sDj}2<3+^zbgCcVg!{o&!j*}xd$;ydJPKlfnyTIg%vhfvBlk1|E
zmqaZuidtV0weDc);hMa(RFlzm^7+!wNua{NXchwlgP$fVxa=z`0Xc3fh}Z@qRzM0N
zP?zZzYguAWX(~z<0;#EzMsZMaYDsWGYHCVwPG(AKQU7F>@(@Pb$=!XDn<tfDVm1ZU
zEieBwFfcryH4_Z%AvG4n5j+t4L20UJ7s!5ag|d3`%xZNnP`O<Mu2tY^30%fP^EjA|
z;<&4z{EZe`pa`2anXBdnp005g^Jc!<LrjxzHSjWuOrF=E%qR*j!wedDc|kb_oIGk6
zvP37_HCpl`8sSXA44UklA2sSQ)q{LlbO+>tyC4GOIZcUM%!xTU;OgiWpJPc$QD#zU
zNvdm6QGOA2UxRes0}=N@1larF<OEJSfgmwNK%k~Q>mqP5iI(I*MXV;Yo;v}`S1cu|
z#U+q%162W#>TU9~7B%+0paeCIakFr1BqJzADdA619b6q;n@!twnd?D5_0!}+c<2^4
zyl4nafh4ijAiKHKi!uueQj6ltQga|>1;R6+`~r>DTTF#1D3N*%6sf`}DGro>0#n|A
zoILqq*CuFcTidO~<OEL52!p}t>O9DLVZZzm&)k9>aD9=Q0*(c!l|`pOr3@2zD4@uC
zvS6PcBj03)J}<DjKb*m(D<{N*SWGSgmues*Zm|_5=B4E47A*ut6-ohotY3psX7c;~
zBz;ij?S@i$gZi)F%6kQ*@@B{ar#BE$Y&rR0cgSR(ei^1b&B;3bL98{5#UeG66|BU;
zeU{}@c^SDT8|;v<s%5HS$bwhmS%`9vDTOJ8xfM#LF@f5)s46G#nJ8P&lm%*;fvf~|
zir`I!Di(0J6V3;>K3PF+of#l6f(cN#y#%?x2*lOoBO;)R-hy<4L+J^K)hq`hL8<i?
zM^S!BBB&8|i!nr#8C;haU77q~k|-OvJiI>n_ap^26=nv8;v<v4^jl3fm>^SsgJ0?c
z10$#G4IY7hzb?OvJTg~!WG?W?d}U)07M-Fo!*rtl6#WkN8!`%WyspUTcd$=joS-;?
zc|zcX!U@3>I4*KX;*%1SoszjgWr_AhG2IT99_|kA4sP&B0Xz;TOHAQq%3_-w_f>
z!;~jXl3^*FHH;}-pr9^+xAPeovP32~YKukGGS@Jsa7u#18bs7EBL)Bv-TxZK6mAqX
z@KOh<+s}aLyDyNMyiiAU@~&Y1$@6vjlu~%0-PeJ4%jW(W8jMUu$&(Lui%;G#gKcuw
zw541rOu-BwtjWLm@a%ZTdQcz651fFCK%EawMhF{JxPZqFAzd3#QY%UTbxy!6)Six}
zG@`D!#gd$pSx~eJl&rud0=PlLRt&19yo#2CgbP810(((vaYkZ6s`D+DoXleAa3iSA
z0%?SU%Gg_+nRz9t=|zbpsYT#g9#kUV;w?+f$xqHKsr1Rr%`7Pbja*D-ooDR>D{WtZ
zOlK=h0S!>yVo%OZiO(%gN9$6wVHRw~sd*`ZDaN4WeVS?Wt9hY}%y-2kC(l{nG`Veo
zAe4Qa<pQ_L-N_H<D>B}g%)CI2ufwgw?E^alkLm?(l_J~82ba8`9JqL@0jSi4l)ptF
z4}%+ab3iOmh}>eyFG$TpZg5yn7F(jl*f7~~Nj)RTkPjAYjNs8z{N}JG=j0csq8jva
zsSab;WXC08lXI47GTLsQwyd0!amHl64ab~7Rdmq-kSjoeTZGnlLKLrPH5u6PAa@sK
zO%~gzRsRuW0;q7mB>=B&LYxBvKu!VGmbX|l^9o8!Zm|{@<YbnB8?U#Rb5iq)EI}GU
zd8tT%k%0jcJmB6WxaSD!9cpqwN*z!<gWGFhmA6>Z^AmG!F{a+)OwUi8yniFRFlZRE
z7}VbdRihwWWIg%jMhnInn}s&TvWkHX1dnr~)C8^D-I%U0Pd>X{9URJ%llgXhu+|4P
zx;P-N0s9i{Ur?eg>H;|!-0lXqs(V0O6ay`Hsxduco}9Qdm(g~z;_g<SmCT?58kD1o
zlqV<bGEe|Fp%8|>!Z2dPE*++q%#-%WPyV}08Ei_Mx)LbzSs+;otQTxErrvYA4Vd0D
zPuk<e2ddd_ag=3N#iwK@mrTC6Uu<*Qo+XS%;1(Tf7#G1)B&-Tn1NE#yxf;}0n{2aB
zjp-}%<dl6$jJA{S9}H%dFHS8{n5=(5R;7p$6mOvLfoTN^Ah#H9iB0a?FE+V;ziK@*
z3j;&3Jfv5rWYWQMg+u->2k!+w{fit1S2zqVz!11VhjdIC-85y3z+=}FLE$tBL`((|
zps_Sa$~^?)g8Gt0r$MakAOh4-F9J1=i@+%yT&-OL^<IizgQVYqi0dE%l<SK)Kz`r`
z5i%e`4n!z`2xSnV%E-WQiv<*jMH(Q!7KoSv(hLe>@OVPeG!Pd=G=l=}AS0;b+Q0yT
zADF~gIX*Cmv3hWPFmqyJ75%_~L<X=hvdVs7fD>$ttnwf(lwjjywfew-N~leiIU>)m
z!m9Xz0Zs%=_Bx^<qRVRYfdQ3>o7{B7NQ;+M?*juWA?L}+>dE+l0fmZUntc1nWEsN`
z3{0%(1SD3bO>&l;eBr1!W5{NiV_P`|z{v?z2^4(=g~5l(56|l}W=<BoV8-Y*+2caC
z5V#OI!oa|AixphB7a2`Hdch}J0u+kip$7245qyr~77KXlh&v|{)Hsd@%^($lI-y8u
zloL`SfNkCbGMFbbFS8^*IX^cyF)yXabaKu`D@gk!dh(i!GK|WbPhQMtVh6{TKI7yI
zr4pONuiR(k1P$%^tz<4Tn{0ngQXJI9D$)l<A1vL2{F630|C&5w<mCQqKHx5!<mB)x
z^_xqsJF`iEJndKH4f2N{vbM?R@7ge?P3F1h1L+JiO`gyvvbpHqS0*X2f8haAWCPM`
z2O{hzH$D{OMKxy0Lode8$?qR_KuV^~6CX7&@_qmnO?*YBAWasN<(`N;xPX}UAR-7v
zID%3uYfgS*N^y|`NWd0E1cL}rNn8Z(x1f|*rB75CXHA~<WCf$`X6L8DOl%-KiV`Nz
zd7c~%w#OQznG0ktTWMZSW?nX=WP}$QMdly{&LF}JMEHV~a+ahP<!0uA>gh+IfJC+u
z#4n#5^+JVl$>#bO#f;(LHa$1)e69^L7E~J)1%Ox~AR-b(fD3fAya*nC1QG0$JzuSK
z0!KTDt7&zMvmi4sKRG!gGao!yaEmpsG`FC#2pl8eLLFSRf?Lm^+N=oFD=XSM+2nN%
zcL2!IPN3ALFxl?4#N^Ab&oCNIUhu}4(Q)#XH_jGCp!Q`EXp#z27=UanLUq|M4x8Nk
zl+v73yP`ry1_sbbV)4GsWp7_IiZU`P-C$9?fQly1dM}+6&CJOEfdM=D5iI@%OhVOE
nu{m)u8h&8FPJRT7e*u$FH69|99Y07-_Wi&<+2Vr=3n;Vz)heiu
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 3313d6f721c2d15fc7500a8604808aa9f66a3c50..3cb97171b2bb96a925d27897af75a8930c1df8fe 100644
GIT binary patch
delta 7112
zcmX?dgZcbpX1>$Byj%<n3=D#N(dp`+C-O-!?%Jpx#8@ws!jK}EW0GqcWy;8q!jK}A
zW0q?kWe#Qw=UC)gMp-g4a5AKbv@k?jF)_F^q=>dKq=>C%0vX1@5M>P(6K`Qik${WY
zfW;(R7*eF*Vz!lxn$j;pn*1~wZ;1qjxVSp!=ap=Jz*x*Erpb7V%PT)KuOzr6u_RTK
z@fJ%+W^U@{U}hb5My1IOT+`V!85kIfv?j}PCva<km=ONv4(@M^jJliadHL8O;#y$I
z%`-%HFdAp-XXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3@~OepYI7N%7=ZF<(|Y
z1_lPN&FjT>32=LYjPn8!K9ft-jTvnxuTa-wyUf7AP|Q5}hI%6FXATAtrO7TDip<wU
z%_iq*XlpWvnSWqs5K;Od!5}H!!P3KfTUffo`G%r$hfBY2r|%5qOFVKPm>EQ5CZE*M
zWV2^rU`S?~eDJZzW**Hj)(~|D1_n*0Ta1ObSVQ8=<5P>285kH|GBPkQ++qufPf9FK
z)f6rQx#|{cd~r!?K@r$-ejpcmgA}nB<QL?mrj-;0f`mdqgwNz{dLE1klmF{U3Wb6M
z!a;U$7Da&AF_X3QRm`J6%xI8)F0iffAR}&ZL)b-`=@}(O$sj*-GB7YGz(A4h<W_wn
zwl5%wdz1I-JF<S}Vh~ZE%xPf8!62gkSp?)qUjs#uUpE&TSTQo%PyUc6x%sG}EDK}O
z<j1BiDj+8p`GYL?09npbl9`)YR0!gOy~vbOQsgt)+su&-;_k_l%$z*IeggUW7E4-b
zUh*xD`1riU+|>AZO*W{PKt{0^mn0UI6y<{0%&B=Pw^&j#)6$CB;2tQloh)mvplHm<
zz);-9$iUFRa6?|9gXIc`%pF0o2_@IXwJwTlT@%!voNT_8nL$Wrvb4o+NNh2KW9x%O
zG#g{mWFPxD3wMx%%RvOlE4P^P3U0Ahr55EELnA6TKP5Hi7E@l?Ew<v+yp+I{A|H_A
ziph`cjooTM%vump2O{9UE2;-^8$bjoly0#XCzhr9<fj)wf&wI74I<J&M8@Q32RTtt
zgx+E+&P>lM&Ar8%R+N~VS`;(6&q18c7Np2+@@fZVwkVK?H%z|bpvE*!YBHmv1v7(?
z%w`+MKt@hbTJ+Q8C~BBI%}HG=7NiFp7cC%`8v_HwN`@kknj)}qjv!GGv10ObCoOQ$
z6gf_Qe^`=n-(*8)6-N2VVa}?I@{?<vB^ebaPjmJVOOa_|h>}f_ZDEL#OOb0~h?1{l
z)l}U4$XQiLN(p2dD6JQ%fLLlELKQ@)PgV@mVN9Cr7bd1_z`($eF5mh(aEBbHLy;~>
zT91K&A+zE;14EHMi0cf>E&u-i|6i0ad19CX8#JJ|hbeNWgXB3tM91XEVah7mASNh5
z7x92t+#rG%M1W&N2*j$LtQDTfSUR~UyeY5^l#*ET^HPg%G36B&C4l7HK}07=g1Icd
z@D_7od|?qdKBPbrf*>LcL<E6|3=jcMCCngJ_vHA9GFMP8y~UbXRFqg*lnD|}0p(2=
zNJ0iV{uXa>W=d*2D5aNV=I4R!EoOnHVn{^9PL_(4Vp|4^SCP$Dkz!1&7dS#M@J|kk
z)?vNC;e3IAaznH#>je(`3;dgxM=xSxw4dx17byYu0mx-58Nt>Sg9;giBHPKk<78yb
zm>3v}L1ECqaD!jCzpS&Yzq+%!yY>QyOc6+XaAeUjkV~10c7a?CGN$M_s33qE0tyq>
z^wg5Tlq#X1{G|L6-^9Gc^wc6fu$U#P=@p$7{k5I7-SrnZ<cjP!FNi<ES`RJ<g^R$E
zmI${3T=auuf;}$<5^CE(Rtcu%7nLU#rFduNrRFA<WF{9Eb$~R1;}{W3BBcc>i6yCS
zpqvqqlbD>Eo0?Zrv=U?jbG~m{Q9ejd0f<mv1tM2Nya>)$?1k~^nPsVY#h{8$K>-r<
zS(EwFq}Xgg-nlhdFU_3wf`HKl@yV9yNs~{e<-vJp!9gCZ2o1AaY~@9%dC3_?7{NBV
zIzvq%0c4yg1Gq@y7Y2p<b$<Da{PLIi6)$io7TIq;m{G)14^BzlAgA+y${M$l;MBb0
z{31wcS^{$Y9+0G9dTNPda!F}oPKa}WTYgb;Dx_v81*xb45uo5KIt)_Fo>-Pz1d62t
zAaRi6G}*yP;}&;XNqkXiVoGLSdT~(<NLIBGq=PA~q^JqRZ3e|XJ2Y7;z=CK0<m<Vz
zY{?-1yqWwf*MVu8^km1p6lMk?*~u&N+!^gRf5_A1U{s$hRj%d(ipZj~Aj1$*2Tr5l
z6b4QR=RmTc=qfr7Vu4&(1Wp{Fh$;d{86*lP_moTO?FOkqOO!>R{Dqbz!S!hS=F8=_
zjEwf11uK~u9YF<6(E<hr20u+!XkiU9Y%hq|2O>5gr3}`x#GKMplytFga%h!;{8o_1
zL5+z9h7UXp0-`gRyJ{A&u3%ooeSt&!0{`TRRho?Un|D{SvY3L3v6ufD7#N<<nh6H>
zMc@(<;s&0g3Xr9s2rW7UVu4DAqRo@P)~OqTN)Sjkfkz~`kkDiTM;(}r;<me!{p!UT
z4@^$0KLakbp{);}hE44CAZHXk0om{rM1X8B0_Q~8Tg-_$IYppEcZ<)lq@*Y_sk9{3
zwWuh+2)o-r+Mj`l=O6;?NN{9=<182?hHx%C<|C1EEw%{98rPthfEJh+KxvYtB(=B%
z;%!7ZIr&<L8sp>5e>+@Q>p{Nq)8s<9`xZAmlfdfc%^;Jw(~B|-3Q~*W%TjY7xd!28
zP@;qe@hzsp6qFFY$H2f)C5%$+#e>T|DP{(S;>id4rZL)2-d!cRxu9Q-NfaEl2!p|q
ze0}ojiPA!#a;NALC}%N&b5@b<<Qo(97;PrAPqK%YFgan;UuSSZ%n9)hx;dbL2Ippw
zF}K)?67y2>bBmUPgpqT`<OP#87`-N+oE(FkB<!a|F+mDqrlL6@EnouVmY3iv48+ys
zha^6%z5wZg+g|h)#06XZ2E+mv%b*y&#Zi=Bl30?NpLdHfM3WhkZ0~>~fIT;Hvcz;g
zNb}|XWS!{>j6suqr@w$Xa-*RL(;<${fiqf}`6E*}YZz0wY#10Am>BArCp)x?O#aZo
z$<0#3n8GQ^z`#($GWp;f5wmzu$qMR*Fw`)na6=@rbie{AM7%hRTf>lLFj;VoFr)G0
zg(kw2BXxNu@AMZHOX11ZU|=Ysx4oOI=e97iOkgbHo6I<0k&%CL;CwB%Dpm%D6af%(
z^67azlbh!YGMY|iTrVtZ4)a8c;A&9S%)r3l!VoLVz);Ip!#epwu*hViM<SEg&*TyA
zk!NA3VXb8@v4&|`J-ORf*{y~-3si)`1fg`i0+hi}g0Mme$qEs$6-+g3;1Fh>91tRE
z&IXE{9x)bVJ;Eu1ur8ttL##MVSBgj#7Xt%AFE2!|Bnv|gLzWX#Y-ERWaxgMfGF38Z
zif$HO5YJc-DmVQg=@rx()MSLPLAf5>nSwMhL8-VX3DjByvyj@Gpf+Z;$U66jRW19|
zp$z+%vY;gU1EiiGMDT!09VJN82j#h2EXg^U1x1?}80r}yIT2hnvlSPlrlxomtq17_
zRk^p=i&Bd-5(`pqvE*bHLwjkU$`{g(0+ocfI5YD~QqzkPOHv`lA*kHE#aot|lb@Vf
zQt6YKn^|&;HKaVVxa1aFPG(+eV$m(eid*c7dFiD&iA9tB7b(;ift?HPjc~v;7RiCK
z6kADtd~R|2Ew+r*#FW&cTcCbYZow^)g3_d%%;JpHqWCOOKfE|TIX4Ab`xkuxYb;Cw
zi51C!IPA%}DPUtbQ*#SSDnU$ev!EE#1mFR85g|>auE}#2NwBAYO1@C$&AS%`GqT<g
zlb?`0S#U`(oaH)s&63vo%iJn=M8&6vPYj<Cv_N!)$a<-jQr9&dFKRkoRCGLGazf<1
z)Jdu9o{1Md6E8X^UKUO2aJwNc+u_pV@kN9|Qh7E{hvywpsToGoA}2835Rkkspmb3{
z>AHaCMFGtf%2x&SJ}@(g$v+U1?r^>%uRObKe(lWK+4UXH{a&43S9oOZB#27QFq{@K
zf$@ft#tOlOniH6(^GxKK5jcq-q*3k($mkhG(_*0tRs^jSTd0SsNcjUB1Fzt99@UFH
zs@HjRFY@TFNWRKr^g)+_N43a)^8N>sn`KvUFizgQY^njMih;B(%0Q_DRJ9f@0<l0v
z=`EK0g48^eCiUcs<ywqKCwH&(7m~fmBYTxc{t1uZ1tIsVJRWePIT<a)85tNf*^BU-
z&YGN)U!00+eB4SM#`BZAS0*spZ+^3~m6LJK<f6?-o$i91bOJ<x5+`~C4V3o5jmM&M
zASH;l8Q41@pA_XxuH2%P_Y))ys@rY}z$@Dj=YRl^GeB*ETdbLR1*IjoSc?mCGE2a%
z*jvmwsd+`9;;0Bz78fZpGB7{_4BW>8_oqO8CQS}Vl>y2pkmjJ`WWlWx!k~6)F{r}_
zPL1G3>13O&7HoGxX;EZz)z)%GnQM}I6CCd-sjuMNptzFfvXbcp%Nc=_Y(6qGNa=4D
z+MdEJ2DS#=(?+Q<dv>`gO0qC86pMiy>K`N+IC-yg$Y11;zs_NBk;C9BhY=)JCiCq6
zXKe^7E;t~L2L}o`ctAx&Q6DH^!0mN#YkUHTi(-@O9yLbw$vJyM8Nmr?^6fn(Y@jl(
zNOf}PUIPViI|*UZCk*{d_v$d}O@6sgY4Vr7B5*VEG>Sk&ADS$XG7M}0*ltV%cI-1?
zG@1NzpBbb5W`+Ha8I8cLPt-Ulf~R3<qo~Ln)E@<vt)R}pWYdFcj8>Cl4tg@$PZq3{
zoV@>_ftN;cYKcNwY7Tf*D;{0|DP-oA<SP_`I*qxIPNODM5f3QtL4gJ{9wY#7bWJum
z<RIt4!ocuZ7#tfnluSS)Ir4WncrWl-T;;GVg4-Giw-q$Rh2Pd&LU2ccnt<RzF>n(Y
z<_J3!KN=hoX8{>H+53<pKZxPO!oW~u19Cas`g%>6r@^CY1nd_^wI8en#Q~1U4geL*
z;>D>Y2t&cH2J5O)fiy=a2OpAD28#rM5-&8kl+8fFr2q~t%c~q#h~UcB0=XCA3`FGs
zi6)4t@MsbOc@{aEKmzb+0y$6u)qx1}s#GWY9+Iv{2!tRxP{jh|Kt-?vt*&xdLx!Qi
z4PQn#O@$)xFxga4(w_z*rh^FZ_!YR|ItAipfr!f>Vn2ug^#hB*T{&<82X2wx1J~Z4
zLDJtq#C;Ia1tKIt{*nO^x*$RyL>Phy6A)p>$iM&^Ura73vIOz1L4-YsZ~_r7AYul{
z7{uV&ED#q&fQBB6L5<=ThL22Qtdm0zhp@*mvKoJKVxGM5u#xm9875Y%PaIIf`ZEU;
zs}02WvnIbkoWvNmIp~NgmjF0Rf|@f$e?cbynB0C&pD}y#_H$;8K9k>_%N7C`)Mr33
zzzS}E6!}cfKJOE)333~FR0TX_1s`*~#R3{ry~UlA2<lD6rzDmn7J+*FNckK(fDE?z
z2*_Zb%)HE!_~iWD+{C<;BLB%k7p#=QBZZ)ts1gq-%1=%$E`|*R#m9q{$Cqu+zo5?q
zZqXNcZeDWf8>5{usN=noxhMb>uB?T{nN_K`xC%;3;`8#$6EjOzLVLEJXo(*@fL0|n
zS@D4s<C@8zuh=r$Z#KMolT88?E`CL+Ag^X1>zS-{$A)p|<oG)vm2>Z?G24Pe0v=RF
zVIVCLAR-dv6!ygA!qUv5R8S)>CpED+wFt$yNe{f(?tl_)!Q_Ak;>s5|v@Y;#-QX7k
zjcqMpxvpS*QNj4Sg5^a8%S-&07dR|0@K0`eV8v*^dH;jUjJ%+>r6ymIKghPA$@3qH
zJH&yQAd8D~K&)tx6IgTd6H|(dK!dnN;ULjm5CN({i@=>~l$zt)BNfKClcgUoV6>ln
zFhg?l)yEr{*g*CaB~9*rmK+VXD->jaG{|hW(!89^ylhA<1uxHw0znF5K?Ep1iqb(!
zIZINDax?QlP1IMQ^oDFLh+jR~^SKJ+_sx0FO&G($*_0c19(4to4XVhCvOugn5CIyD
zECScMXc-4Qo(dwOCjWfl-~`T5Ag-p}EzW|>y!_<kjLiHZP*dd=YhGz?L1hs*I>8kz
zxRL|+TtN+yBGBkS(ca0`FKgJq0q8RM@5`5rCX=7PGG>gPEdAO!{uX~Rcu)=0zAe_v
zElvlwt8Z}?CFc}W>VYRKK#l4mPzR|9G!$M0%F;z3_Y|SJ`xb{yZhlH>PO4o|FCzm3
ws4G^yX7aSxC2V(DwC=LVPv&|fBj>}-X!DT)M1D^Y%VuJ9X8gnerob5<0I)N4LjV8(
delta 5890
zcmX^AnEA*JX1>$Byj%<n3=H%9Bhv4_oyaG_xM-t#5MwA?3PXxuj!CX*lqn+vCqs%*
z3qzC{6N5WLif{`<ipXjvkR}F(D08rwXbVG%7+lN(EGFK<kRkyWv#ey)l-&G`v6XRi
zBeM=Wqr&8MT+`Xq85kIfG$wm<CvdBQm=ONvZQS1&8MQaB<>h0Ch--i)Hy;z(!N}{Z
zUzDF;qF<JuQ<|GPd83#ws|^DKgU4oO@m&Jk?jVI8Ai`_%33X#e>&buAwb(8)FfbI0
zP1ey!H2uKAAfoh%fsNCjvBUX}lKNux4wrtvPQMw(7kT8Lh)T^cnQuAM^18I?MQPK^
zqGq2Mn0fsfCvVfxHUKd`u!A&ykYJFM?qKQRy&)|9iGh(*rNjA#qH>2zzi+4S4CPBa
zavzu(IaMZeX=*asPA-@tx!G4Uj5S1+fq_Aj=@w(*E!L3u^7zyuMFs|jmy8Sy47b=q
z;*%1KQ#FN)K<>ZA8ed$JT2KV`h7SV+Ly;#)5qm*?K~8E~Ns&KDC<sJ&P5!Ru!5BB$
zL0?iR7$gu1vV*fI48)F_oUN~79sy!Tg7kBNZH)&Raf=(mF3L>LC@D$=g$^eJ1A_t#
z6j@I`rf<ad2_*4q@;`k??hjlHBI=(QSU4RSC%YJ!8G#rdL>NS5Kw_pH&L21!cm*N;
zzsMtZok#T|kLrSq6~zm4ukx6D;AG)6-Mq}eijmQF^Fu>37RH3h5@s$cAa51<f*j}u
zav)1dW^QUxK8Ozv2BwsfBCp8}W{zwSzfC@F=EMf}x5s3DbFq4eFF`t4i%SxVN{X^U
zZ06Lwlv^w*nQ3XoY;ZpnS%amDO7p73ob&Tait=-O^79LP6Y~<&Q;YP9O7jdE85oND
z85tNF7;eZbbg*3Ekhvo$HlgIYxYk8+t;>SipBO|rZ95$Ah)7KLoai~(yJPZv^Oek^
zoVJs_Ep{{7Zf3VEVPi~~Tw@<+;R^CXDTn}h?iN#C!7bLR)S~=iX!Pdhr=;fGV#+JK
z#a5h}mlBv#<ONb(Hd)BQ*sTi0tOgM^AOhlF5UUnM)PV?4xZPqePAp6H$xknW1P(~N
z5=5kch_uPw4sxQP0J_CioSB|intO{ittc@!wJ2)xJ_m6&P;eBvOup@)%oYLi_}0lR
zj%w_ar9Lq*aq3UjakOA%;?)1ZHM!SGZgab%AtR?P0|SGfCPz`-<bzJ?TG1doz!B91
zV!1FdFsx)K0;vH9bdfzs6hthWEaa@E0G0r8lOcr$goKtL44>Cbj&W9Dl$qS<tST*&
z!qdVS#hb$0!Vtxm!q>tO#h=38!Vo2pB0Kq@vj<BhtESv$c^6e7DFu)npe$6R1Y)Ux
z2xSnVIyo*($0dP*fg#-(41U``huFx#P^1SEmT!F>xI>Q9p-3B~UWb8!A+zE;14EH6
zi0cT-mH+<#|6de0`COO+8#HRZgeh{Tg5)_sMB8MAaAlQD5EGPfig-XQZV<r>BEWGd
z1Y%WBP7hCHES`KayeY5+lm=Py^HPg%G36B&#ew8oK}0)9g1Icd@D_7od|?qd2Bkm}
zf*>LUL<E3{G!Ow!SIi(*=j1sNWvQUj;1+9QQBh)LQ94LC2~-BKK*|7+<8Sd6XQrga
zr<LX<mt^MWf$c43fo2Uze1YP#N;D`xDZd1kCF0{V^D;}~<ClPv2lwXkNHHd+iyV_D
zMeBg5!_lft7dbY+jb6mWXghgAT%-ipHy~H8WCUAX3@X?ZimWFy#>+68OqPq61}hAf
zD>?#lA5+l|kXu0-ijIPc8JP0b@p6pjlTU$^Z+;ekf;A9aYzh~F!#*Ca2V7f#1C%{4
z1>*TFpimS{%P%TVEK2dt%uCHpEXhnRE@}g50!I}juqXddm5N>tQp}w1n^u$ylFkDW
zD?r3bi0i;vhrKX9J+mw|uNah66%-(0mBGNkfEHHisU_g@)QX9Lq4>(=k~DLsivp83
zq+NheNt2(aXG46i2=)Cfw(_FXyyT1`%m6r^p~e_D`CWz_qwQwi%p#V0aD;b)+{*(h
zMBGY(Q}c@Riy#GO5y;KEK$3##sU?odC8dcuA<hAA`9;a8kcy%hq@n^ufP$;&AV@8H
zVp(btsN~xR5(l|KlN}sux46?v;)_xfQ!?|?i;JQ_vL5vy9ZYE@MGYWsBPg=ip{Yj!
z7ASi`fg*vDfRYn)GLwoDOH$oJ5<%heak5*U1N&s@PYmpw+LJr-QkX&X`#g6>+sO|N
zB{v7;3ve*1PEIUWs|Uqo(P@xvh`0l%FL0Uxr-U;gSx_VvodvN#UMK>m4p96RfnyRL
zvA6i*(^E^r!2#}>=jwWkIVGjAXeUS?a?%6wL3yW2L<q$r;L5mlvs8sGBgm;oBqtYE
zzH$T=Bt`SUr6Mb|_y!rU8$|2@5$li=5^GsvPH8GiGTJkFPL+bpW>Cn03gZTb4?GM4
zqBEGgY8J4rU|z(1kz?}hDp31jVZY>Nx#~B}rl4}_<$neShUc?pf`NSzxD13if~Tkq
zWGg5s6deGuKv}<N<7E4KbuUm60m(q{Bmgc2G?~DW4rZe`?lveo(Ly0TwIp!g<PG&_
zplzCI4V&2OL2fE~1hVcihyXdI2%KSMZ!st4<P?F@?kzsYl9Hm#q|%a9*P^2QBJ2(Y
zX@3GDo`MLli@{M2j@dwv7{cxF^bv`axv?b+Tq!~mT0EWuC2f|H)Z!9I7$8c;$qJon
zj1M=vce=19fqdns$%SzDEpB*TgVo&|K_+ph7iAU{q!z`OrRG2~AHvO`qzeu2TTF#1
zD8YSa^4mTMMv2L6{j(TtH!tkBXL15ZCPFtjTCala5cbP2@yspANzF~oD@jcOhbYwC
zq6?tB$pp@uMb?v*Ch0L+O%9x7&uF`O;v_LfXK=yG3GoINql>_W0mzJ7Y(<HADfziY
zOF`j|oJS_VnykU-F<E#@45RJlf+@vJ_2AfMDw+jS2_``HzXaD$Ag(4qB;n!m55m%-
z&mbefHogL}z=bs^0&j5?<(DLas;*m%A)3sPBzp@K7VNo+A_WDYq6%amyfC^uxoo-u
zW5DF;(_b*!PHvbX$+VAsvO=}gWdE0Jn|IGhWfqA^;jCdy;R5v=N<du>Q1f8&e-qKk
z4>Gwq8B;hV85kI9m?tl|C?XOs29gBf8pafEsKke65vF*F$#$)BOj)Lr4RwSk2Q1{C
z>~UF4I)x`&gMp!lW)^JTIk$z8si=MO!MUoFWoAETt7NKV(&XQqxG<iv9#kCoL81cG
ziqT|*utDh;+}VJ%lt2+w1Zp0=1hbGDPM{J?8WLZi^l^(NIVZEAXaguV!131!3Us#O
zg4EO$ucEaeVNm6Ai@hkdI3uwj)%g}nPG&K*e*&s!Aay(_x8CB+%qvMv2UQ1<EDOrp
zw|L7^bMlijODcUbb2CedWEdD2CNExW?E}l6@4)sIro<=brrcsr&P|EWElx*kx^#ew
zDq)lY8`7puVrF0{zRa|_V@WV0^BpnC$q$wWLuuE^waZ#rE^({enOv|`i}A+f*-JG!
zd)+>;Gw`Su*=~Ngd>f+yC__Wa^%9UrK?PaS0uT!n61Q0L3sUn?8!pFJYB8>#d~3D8
zknBYs*{eMAPk00`2)SS7@c<jC$Y@!_!^ps($zFutXx8MM{Nhwpv)8WHVca?S*6IXC
z+s)Q%S~(eKO+L8ws1vA|E;<Hs6ex_*YfMlSf~)%?w8|3f7mzoKvL>I{rj_>{WCEy!
zxg`KE*Fu~F0zl3H6~DJwGxG{cOK!0i7vyA?fZH9nm~&F|iY!4I#X*D^BLf2@w7`v5
za6=W;7}ex}WLHphL+WR-$qCyfgh4HfVo*yOsmIy9-Gc2nDAFW1pWI%~sCZdY?-K)~
zpxFe+J4)&+I5#M+<hiV5I>B;A;3S)m%#4C&n`3sSFpGih0{47S3aV>++!&cB%kGs2
z)!EA>Cx`9*XRQxP;2aQlg8dH;2~eso>H+xz+?WD4mHI$j6!ZG`sWA#p-nK6kYF^HP
zJf4-zews{>N?3XF{QU+B;5rCl-Ukd*-tX68l$b1Yzzl9mzPbh|Rj@#^9at~ea!kEU
z2MrkICd(X9njCsigwb|$*1^Y&M&Mc=HO!0P2^Ll_=z!Yrpv(?xu1&5#tj4G^dD&r4
zkQ<J8vdR~ymMBcNI4Ubu^P@uqcf&}1I)#T<Q4)sPX3=GA>4B&pUk_o7vEPscC
z_X3~(RSpA44-j0<FuG~V7J)mflR<$n1w>2*5umOHB+Z@xaY3!=BGB+m5vU1S1Zsa3
zftyd@v<<EZ?ts*N1QA~t7-AWU?t+-0Y+u9&a;_kVPy!JuAVLj9Xo3iBMg|7ZKtOUy
zksgR|03xP=G=ux2pjKDW3=kJYbbx|r=HzK7VmUk+Sv?s)MKMkOb;5|}vkVifA;e!Z
zCcB?ZVhq{5>ZB@{065%1g=o=lkP+V|-@mNSm^qpMiW#HVWVb8XLg3tY3RHlxf(yqY
zi^+$t_(aQr90BgUfIE2bKK(70yu{qpTiiK`pw>uyN@7W35vbvf6q(Rr9k9)ZKnC-G
zduYk|xw(mXDMi+kv#we}I`+|%S6!814BCABsy-7t*t4dLlRqAo*erVEE+eNQsL{HT
zxyWYn@0*h1ppH(FDaiM*1OoC;`ef}}@{Apmy>HoqQ;Fnc(Hpg!<!?K)Nq{`<R}={H
zM+maE$yE<+7^hC&{?L}ucJtSVYRtA^Z^FZ&$O)vy1w^=lVun32xv(^|C>2zF<fJAR
zrxu|a7xC1~Y6mE>Wq}jh7cm9_kqPeIwM&?mb1me$#IJRcL+b{=SbuG2?E;qT3dR=|
zjIS$LUR1EW#BX_#WAdY?Rv?!zm)vajY!@T%H&E8+E3yVzZ#Vhxb8!bR5EJB*BG3p(
zkvqsutU39KDaA!@ASq`M5eXtdWndAw35rr8guGB;JUO}Y#R5j#$-J48n~h&?U}6K=
zQ<O0I<?G~Vuw9NI0|Y>3vz6xMWaec<N*{O@EwTkE@B|SyAR-u~l(QtYC^s`Nu_U$V
z1t>O<tp)KbCog}a!gzV}nKvek;o$7XjXSRyf{X=~#YJHtRuqVc2N9JZqtNmLc&q_L
zm`x6U=imfRJ0Py6)h*6~%)I>M<c!SxB2aB}i#4w_x1h2J9KYa#6I^zI8e&DDx}*ry
zmM+>o`R2PCZg2oPfs(AkWX1OqlPlhzVKkcT`oWmdeRAaoXNw|GLku!#1j>s=ARCKN
zUG|H^CO1E&G$+-rsGO030W?}rylk@c#}ca#EJ}<lpF9|&85xDX*Dxh8GxC3A0FmEa
c*mJlT4L>q~$nP<t*-VVijGq|56gWo%0KQKU0RR91
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc
index 43d425d803da5fdfb5ad1ba1bb8709ae74398a8b..90f7a9633aa4ec8f8144b86339c2c17b3825f0c6 100644
GIT binary patch
delta 79
zcmX@k_?D6TG%qg~0|NttMzi%qZXe?U{fzwFRQ>e);)48?#3C@y2*J%M%}dcQC`zoz
XEY>e5%FjwoE-9XvV#j!E;$js5@CzHq
delta 41
xcmaFMc$|^@G%qg~0|Ns?s7k~{ZXaGR{i6K*68*CLoYLIXiCuP#zb2kk0RZn!4NU+5
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 7d4c1b2825ca761c742f7999cc5dbd5cd7d21b2f..5f7f84bdf9b600f8d0c911ebd0919c211ea1c011 100644
GIT binary patch
delta 150
zcmX?={4JULG%qg~0|NuY`_6!k+>I>8dHNaoxvBc;`NakKDTzg3o)LnZQ<|5eUr>}-
zky)%?P?VpQnp{%6c_xb`vupwb149GD7d{3afeD=5_H&ZvXU)vI#I13GMdJeZ<~QuK
q#U-DBmE7SLoshXiXt~rvsVm$%7g%&IaBrR~&nU=vYx8^qZAJiybTmc)
delta 92
zcmeyCd@7myG%qg~0|Ns?hf2go?nV|~5B;M2{1W}L{G8I<)Xn=@ESaTZ85kHE7{2f^
y@CZ!c?6#kiG(T%*)+KI@i!7T(IcAG9KAgN!UYO}3%jT2vjDn26HlH@oW&{97A|4h1
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc
index 61a7479eba52df830deb8cbe9f5dc2f9142f0c8c..c00dbcf1872f6817c3bd5e64964ec57c2189cb2d 100644
GIT binary patch
delta 79
zcmX@i_>z(PG%qg~0|NuY^zP7!++N0c`WgATsru>p#Rd5(iA7+Z5rUgjnwO$qP?T7a
XS*%}Bl%JKFTv9wS$%gUP#Q7=!5xyKm
delta 44
zcmaFKc$ks<G%qg~0|Ns?hf2gmZZBRB{i6K*68*CLoYLIXi3^1%&eml7HSwql03BZr
Au>b%7
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 6d44814..1b140ae 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -8,6 +8,7 @@ import time
from qpsolvers import solve_qp
import argparse
import importlib
+import proxsuite
if importlib.util.find_spec('shapely'):
from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
@@ -40,7 +41,7 @@ def getClikArgs(parser):
# TODO add the rest
parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
- default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP'])
+ default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP', 'QPproxsuite'])
###########################################
# force sensing and feedback parameters #
@@ -90,7 +91,8 @@ def jacobianTranspose(J, err_vector):
# TODO: put something into q of the QP
# also, put in lb and ub
-def invKinmQP(J, err_vector, lb=None, ub=None):
+# this one is with qpsolvers
+def invKinmQP(J, err_vector, lb=None, ub=None, past_qd=None):
"""
invKinmQP
---------
@@ -118,12 +120,47 @@ def invKinmQP(J, err_vector, lb=None, ub=None):
# TODO: you probably want limits here
#lb = None
#ub = None
+ #lb *= 20
+ #ub *= 20
h = None
+ # (n_vars, n_eq_constraints, n_ineq_constraints)
+ #qp.init(H, g, A, b, C, l, u)
+ #print(J.shape)
+ #print(q.shape)
+ #print(A.shape)
+ #print(b.shape)
+# NOTE: you want to pass the previous solver, not recreate it every time
+######################
+# solve it
#qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
- qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=True, initvals=np.ones(len(lb)))
+ #if not (past_qd is None):
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=past_qd)
+ #else:
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=J.T@err_vector)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=True, initvals=0.01*J.T@err_vector)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False, initvals=0.01*J.T@err_vector)
+ qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False)
+ #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp")
return qd
+def QPproxsuite(qp, lb, ub, J, err_vector):
+ # proxsuite does lb <= Cx <= ub
+ qp.settings.initial_guess = (
+ proxsuite.proxqp.InitialGuess.WARM_START_WITH_PREVIOUS_RESULT
+)
+ #qp.update(g=q, A=A, b=h, l=lb, u=ub)
+ qp.update(A=J, b=err_vector)
+ qp.solve()
+ qd = qp.results.x
+
+ if qp.results.info.status == proxsuite.proxqp.PROXQP_PRIMAL_INFEASIBLE:
+ #if np.abs(qp.results.info.duality_gap) > 0.1:
+ print("didn't solve shit")
+ qd = None
+ return qd
+
# TODO: calculate nice q (in QP) as the secondary objective
# this requires getting the forward kinematics hessian,
# a.k.a jacobian derivative w.r.t. joint positions dJ/dq .
@@ -156,10 +193,8 @@ class CostManipulability:
res += JqJpk[k,:]
res *= self.calc(q)
return res
-"""
# use this as a starting point for finite differencing
-"""
def numdiff(func, x, eps=1e-6):
f0 = copy.copy(func(x))
xe = x.copy()
@@ -172,10 +207,8 @@ def numdiff(func, x, eps=1e-6):
return np.stack(fs,axis=1)
else:
return np.matrix(fs)
-"""
# and here's example usage
-"""
# Tdiffq is used to compute the tangent application in the configuration space.
Tdiffq = lambda f,q: Tdiff1(f,lambda q,v:pin.integrate(robot.model,q,v),robot.model.nv,q)
c=costManipulability
@@ -219,7 +252,7 @@ def QPManipMax(J, err_vector, lb=None, ub=None):
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
return qd
-def getClikController(args):
+def getClikController(args, robot):
"""
getClikController
-----------------
@@ -242,7 +275,30 @@ def getClikController(args):
#if controller_name == "invKinm_PseudoInv_half":
# return invKinm_PseudoInv_half
if args.clik_controller == "invKinmQP":
- return invKinmQP
+ lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ # we do additional clipping
+ lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
+ ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
+ return partial(invKinmQP, lb=lb, ub=ub)
+ if args.clik_controller == "QPproxsuite":
+ H = np.eye(robot.model.nv)
+ g = np.zeros(robot.model.nv)
+ G = np.eye(robot.model.nv)
+ A = np.eye(6, robot.model.nv)
+ b = np.ones(6) * 0.1
+ # proxsuite does lb <= Cx <= ub
+ C = np.eye(robot.model.nv)
+ lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ # we do additional clipping
+ lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
+ ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
+ qp = proxsuite.proxqp.dense.QP(robot.model.nv, 6, robot.model.nv)
+ qp.init(H, g, A, b, G, lb, ub)
+ qp.solve()
+ return partial(QPproxsuite, qp, lb, ub)
+
#if controller_name == "invKinmQPSingAvoidE_kI":
# return invKinmQPSingAvoidE_kI
#if controller_name == "invKinmQPSingAvoidE_kM":
@@ -264,6 +320,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
"""
breakFlag = False
log_item = {}
+ save_past_item = {}
q = robot.getQ()
T_w_e = robot.getT_w_e()
# first check whether we're at the goal
@@ -273,15 +330,23 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
breakFlag = True
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities based on controller you passed
+ #qd = clik_controller(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
qd = clik_controller(J, err_vector)
+ if qd is None:
+ print(i)
+ qd = dampedPseudoinverse(1e-2, J, err_vector)
+ else:
+ print(i, "actually worked")
robot.sendQd(qd)
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ log_item['err_norm'] = np.linalg.norm(err_vector).reshape((1,))
# we're not saving here, but need to respect the API,
# hence the empty dict
- return breakFlag, {}, log_item
+ save_past_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform, i, past_data):
@@ -339,7 +404,7 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
# -----------
# """
# robot.Mgoal = copy.deepcopy(goal)
-# clik_controller = getClikController(args)
+# clik_controller = getClikController(args, robot)
# controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
# # we're not using any past data or logging, hence the empty arguments
# log_item = {
@@ -428,7 +493,7 @@ def moveUntilContact(args, robot, speed):
does clik until it feels something with the f/t sensor
"""
assert type(speed) == np.ndarray
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {'wrench' : np.zeros(6)}
@@ -447,15 +512,19 @@ def moveL(args, robot : RobotManager, goal_point):
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {
'qs' : np.zeros(robot.model.nq),
'dqs' : np.zeros(robot.model.nv),
'dqs_cmd' : np.zeros(robot.model.nv),
+ 'err_norm' : np.zeros(1),
}
- loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ save_past_dict = {
+ 'dqs_cmd' : np.zeros(robot.model.nv),
+ }
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True):
@@ -468,7 +537,7 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=Tru
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -528,7 +597,7 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
path_pol, path2D_untimed = data
path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
# should be precomputed somewhere but this is nowhere near the biggest problem
- max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+ max_base_v = np.linalg.norm(robot.model.robot.model.velocityLimit[:2])
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
@@ -544,9 +613,9 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
# NOTE: obviously not path following but i want to see things working here
SEerror = T_w_e.actInv(pathSE3[-1])
err_vector = pin.log6(SEerror).vector
- lb = -1*robot.model.velocityLimit
+ lb = -1*robot.model.robot.model.velocityLimit
lb[1] = -0.001
- ub = robot.model.velocityLimit
+ ub = robot.model.robot.model.velocityLimit
ub[1] = 0.001
#vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
@@ -626,7 +695,7 @@ def compliantMoveL(args, robot, goal_point, run=True):
"""
# assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopCompliantClik, args, robot)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -746,7 +815,7 @@ def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_tran
# compute the joint velocities based on controller you passed
qd_left = clik_controller(J_left, err_vector_left)
qd_right = clik_controller(J_right, err_vector_right)
- #lb, ub = (-0.05 * robot.model.velocityLimit, 0.05 * robot.model.velocityLimit)
+ #lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
#qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
#qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
qd = qd_left + qd_right
@@ -771,7 +840,7 @@ def moveLDualArmsOnly(args, robot : RobotManager, goal_point, goal_transform, ru
"""
#assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClikDualArmsOnly, robot, clik_controller, goal_transform)
# we're not using any past data or logging, hence the empty arguments
log_item = {
@@ -789,7 +858,7 @@ if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
Mgoal = robot.defineGoalPointCLI()
- clik_controller = getClikController(args)
+ clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc
index 36d923a88e1d3b72459e16e37f40e8b62362d845..2f9b2824737ebed52a6982e9abb3a3a070851fdd 100644
GIT binary patch
delta 79
zcmX@e_?(gZG%qg~0|NttMzi%qZcpP}{fzwFRQ>e);)48?#3C@y2*J%M%}dcQC`zoz
XEY>e5%FjwoE-9XvXw7(Q;#?H~>c|_Z
delta 41
xcmaFPc#x6%G%qg~0|Ns?s7k~{Zcko!{i6K*68*CLoYLIXiS5>mza}150RZf_4KDxy
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index 7a66c5c8e1c46e94ce711f546a338bab792ad7b9..496b8c705ee1e32c4b4865057475df6ae5f3db9c 100644
GIT binary patch
delta 155
zcmaDbi}BMeM()$Ryj%<n3=E-~b{o07S&TFEGxBp&_0#i<3-VJEi@-c11UIKNFGatg
zD6t~5SihhsKPxr4q<HgUmhXy;X`5%O=`k@)mz{h;V>k0PQMJjFH8VIE#MD3VG6>0U
yX4X0>$QZu)taT<M$8AC34#(S~;*&jWn>iT7Bt9@R2#IXIY%9#qcx&==pGp7~w=|Lf
delta 137
zcmew~i}ArMM()$Ryj%<n3=ADA5gWO?S$LiFi}LeJ^vm*dN^?^;pJe&2$XKxXpqd^N
z`()YA3_P5glRs+g=C~}X_KAUwS99`u%?u7UUd<1@Je-=F<+V-<GA3?*X`RU^bVE@1
q69YS^Mu+1KQE`x3jmc5A%^YmJ8XuV1IW;yvwH4-P{5AQfPbC2AB`h2O
diff --git a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
index be9364a..f2af8fd 100644
--- a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
+++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
@@ -95,11 +95,12 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, T_goal: pin.S
# here you need to define other obstacles, namely the table (floor)
# it's obviously going to be a plane
# alternatively just forbid z-axis of end-effector to be negative
- obstacles = []
-# obstacles = [
-# SimpleNamespace(radius=0.01, pos=np.array([-0.4, 0.2 + s, 0.5]), name=f"obs_{i_s}")
-# for i_s, s in enumerate(np.arange(-0.5, 0.5, 0.25))
-# ]
+ obstacles_sphere = [
+ SimpleNamespace(radius=0.05, pos=np.array([0.0, 0.3, 0.0 + s]), name=f"obs_{i_s}")
+ for i_s, s in enumerate(np.arange(0.0, 0.5, 0.125))
+ ]
+ for obstacle in obstacles_sphere:
+ robot.visualizer_manager.sendCommand({"obstacle_sphere" : [obstacle.radius, obstacle.pos]})
# define the optimizer/solver
opti = casadi.Opti()
@@ -135,11 +136,15 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, T_goal: pin.S
ellipse.e_pos = casadi.Function(
f"e{ellipse.name}", [cq, cpos], [cdata.oMi[ellipse.id].inverse().act(casadi.SX(cpos))]
)
- for obstacle in obstacles:
+ for obstacle in obstacles_sphere:
for q in var_qs:
# obstacle position in ellipsoid (joint) frame
#e_pos = e.e_pos(var_q, o.pos)
e_pos = ellipse.e_pos(q, obstacle.pos)
+ # pretend obstacle is a point, and then
+ # do no intersect with the point
+ # TODO: make different equations for different obstacles,
+ # most importantly box
opti.subject_to((e_pos - ellipse.center).T @ ellipse.A @ (e_pos - ellipse.center) >= 1)
# now that the ocp has been transcribed as nlp,
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index f056d26..fa5f37a 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -119,7 +119,6 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
- path_planner.sendCommandViaSharedMemory(p)
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
@@ -127,6 +126,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
if type(path_planner) == types.FunctionType:
pathSE3 = path_planner(T_w_e, i)
else:
+ path_planner.sendCommandViaSharedMemory(p)
data = path_planner.getData()
if data == None:
if args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 16315d1..d5576b4 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -438,10 +438,11 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
"""
if robot.robot_name != "yumi":
T_w_e = robot.getT_w_e()
+ path_ee = [T_w_e] * args.n_knots
else:
T_w_e_left, T_w_e_right = robot.getT_w_e()
+ path_ee = [T_w_e_left] * args.n_knots
# TODO: have a different reference for each arm
- path_ee = [T_w_e_left] * args.n_knots
path_base = [np.append(x0[:2], 0.0)] * args.n_knots
# underactuation is done by setting max-torque to 0 in the robot model,
# and since we torques are constrained we're good
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
new file mode 100644
index 0000000..154e4bc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/PKG-INFO
@@ -0,0 +1,8 @@
+Metadata-Version: 2.1
+Name: mobile_manipulation_challenge
+Version: 1.0
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
+Requires-Dist: pyyaml
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
new file mode 100644
index 0000000..f8a90ef
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/SOURCES.txt
@@ -0,0 +1,7 @@
+README.md
+setup.py
+mobile_manipulation_challenge.egg-info/PKG-INFO
+mobile_manipulation_challenge.egg-info/SOURCES.txt
+mobile_manipulation_challenge.egg-info/dependency_links.txt
+mobile_manipulation_challenge.egg-info/requires.txt
+mobile_manipulation_challenge.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
new file mode 100644
index 0000000..e198600
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/requires.txt
@@ -0,0 +1,5 @@
+numpy
+scipy
+matplotlib
+shapely
+pyyaml
diff --git a/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control/path_generation/mobile_manipulation_challenge.egg-info/top_level.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/setup.py b/python/ur_simple_control/path_generation/setup.py
index df2f0c7..dad9986 100644
--- a/python/ur_simple_control/path_generation/setup.py
+++ b/python/ur_simple_control/path_generation/setup.py
@@ -1,6 +1,6 @@
from setuptools import setup, find_packages
-setup(name='mobile_manipulation_challenge',
+setup(name='starworlds',
version='1.0',
packages=find_packages(),
install_requires=[
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
index 0858ef1..28e0590 100644
--- a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
+++ b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
@@ -34,7 +34,8 @@ def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, verbosi
name=robot_type)
x0 = scene.p0
elif robot_params['model'] == 'Unicycle':
- robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+ #robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+ robot = Unicycle(width=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']],
name=robot_type)
try:
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
index 21a305d..f45c709 100644
--- a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
@@ -1,10 +1,3 @@
Metadata-Version: 2.1
-Name: star-navigation
+Name: star_navigation
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-
-UNKNOWN
-
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/LICENSE b/python/ur_simple_control/path_generation/star_navigation/starworlds/LICENSE
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/README.md b/python/ur_simple_control/path_generation/star_navigation/starworlds/README.md
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py
new file mode 100644
index 0000000..52cb093
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/__init__.py
@@ -0,0 +1,6 @@
+from .obstacle import Obstacle, Frame
+from .starshaped_obstacle import StarshapedObstacle
+from .ellipse import Ellipse
+from .polygon import Polygon
+from .starshaped_polygon import StarshapedPolygon
+from .starshaped_primitive_combination import StarshapedPrimitiveCombination
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py
new file mode 100644
index 0000000..17f216e
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/ellipse.py
@@ -0,0 +1,217 @@
+from obstacles import StarshapedObstacle, Frame
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from utils import is_ccw, tic, toc
+import shapely
+
+
+class Ellipse(StarshapedObstacle):
+ def __init__(self, a, xr=None, n_pol=20, **kwargs):
+ self._a = np.array(a, float)
+ self._a2 = np.square(self._a)
+ self._n_pol = n_pol
+ # self._area = np.pi * self._a[0] * self._a[1]
+ if xr is None:
+ xr = np.zeros(2)
+ super().__init__(xr=xr, **kwargs)
+ self.enclosing_ball_diameter = max(2*self._a[0], 2*self._a[1])
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return Ellipse(id=id, name=name, a=self._a, xr=self._xr, n_pol=self._n_pol, motion_model=self._motion_model)
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._a += padding
+ cp._a2 = np.square(cp._a)
+ cp.enclosing_ball_diameter = max(2 * cp._a[0], 2 * cp._a[1])
+ if self._polygon is not None:
+ cp._polygon = self._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._kernel = cp._polygon
+ return cp
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Default facecolor
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Ellipse(xy=[0, 0], width=2*self._a[0], height=2*self._a[1], angle=0, **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Reference point
+ line_handles += ax.plot(*self._xr, '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self._xr, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ pos, rot = self.pos(frame), self.rot(frame)
+ line_handles[0].center = pos
+ line_handles[0].angle = np.rad2deg(rot)
+ if line_handles[1] is not None:
+ line_handles[1].set_data(*self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_position(self.xr(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ b_obstacle = self.transform(x, input_frame, Frame.OBSTACLE) if x_is_boundary else self.boundary_mapping(x, input_frame, Frame.OBSTACLE)
+ if b_obstacle is None:
+ return None
+ n_obstacle = np.array([self._a2[1] * b_obstacle[0], self._a2[0] * b_obstacle[1]])
+ n_obstacle = n_obstacle / np.linalg.norm(n_obstacle)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ loc = (x_obstacle[0] / self._a[0]) ** 2 + (x_obstacle[1] / self._a[1]) ** 2 - 1
+ return loc
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # Transform line points to left/right points in obstacle ellipse coordinates
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ l_left_obstacle = l0_obstacle if l0_obstacle[0] < l1_obstacle[0] else l1_obstacle
+ l_right_obstacle = l1_obstacle if l0_obstacle[0] < l1_obstacle[0] else l0_obstacle
+
+
+ vertical_line = abs(l_right_obstacle[0]-l_left_obstacle[0]) < 1e-4
+ if not vertical_line:
+ # Line parameters
+ m = (l_right_obstacle[1] - l_left_obstacle[1]) / (l_right_obstacle[0] - l_left_obstacle[0])
+ c = l_left_obstacle[1] - m * l_left_obstacle[0]
+ vertical_line = abs(m) > 100
+
+ # Special case with vertical line
+ if vertical_line:
+ if l_right_obstacle[0] < -self._a[0] or l_right_obstacle[0] > self._a[0]:
+ return []
+
+ l_top_obstacle = l_right_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_left_obstacle
+ l_bottom_obstacle = l_left_obstacle if l_right_obstacle[1] > l_left_obstacle[1] else l_right_obstacle
+ x_intersect_top_obstacle, x_intersect_bottom_obstacle = np.array([0, self._a[1]]), np.array([0, -self._a[1]])
+ x_intersect_top = self.transform(x_intersect_top_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_bottom = self.transform(x_intersect_bottom_obstacle, Frame.OBSTACLE, output_frame)
+ if l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_top, x_intersect_bottom]
+ elif l_top_obstacle[1] >= self._a[1] and l_bottom_obstacle[1] <= self._a[1]:
+ return [x_intersect_top]
+ elif l_top_obstacle[1] >= -self._a[1] and l_bottom_obstacle[1] <= -self._a[1]:
+ return [x_intersect_bottom]
+ else:
+ return []
+
+ # obstacle ellipse coefficients at intersection with line m*x+c
+ kx2 = self._a2[0] * m**2 + self._a2[1]
+ kx = 2 * self._a2[0] * m * c
+ k1 = self._a2[0] * (c**2 - self._a2[1])
+
+ # TODO: Stable fix for finding intersection
+ discriminant = self._a2[0] * m**2 + self._a2[1] - c**2
+ if discriminant < 0:
+ return []
+ elif discriminant == 0:
+ tmp_x = -kx / (2 * kx2)
+ x_intersect_obstacle = np.array([tmp_x, m * tmp_x + c])
+ return [self.transform(x_intersect_obstacle, Frame.OBSTACLE, output_frame)]
+ else:
+ in_sqrt = kx ** 2 / (4 * kx2 ** 2) - k1 / kx2
+ if np.isclose(in_sqrt, 0):
+ in_sqrt = 0
+ tmp_x = -kx / (2 * kx2) - np.sqrt(in_sqrt)
+ x_intersect_left_obstacle = np.array([tmp_x, m * tmp_x + c])
+ tmp_x = -kx / (2 * kx2) + np.sqrt(in_sqrt)
+ x_intersect_right_obstacle = np.array([tmp_x, m * tmp_x + c])
+ x_intersect_left = self.transform(x_intersect_left_obstacle, Frame.OBSTACLE, output_frame)
+ x_intersect_right = self.transform(x_intersect_right_obstacle, Frame.OBSTACLE, output_frame)
+
+ if l_right_obstacle[0] < x_intersect_left_obstacle[0] or x_intersect_right_obstacle[0] < l_left_obstacle[0] or \
+ x_intersect_left_obstacle[0] < l_left_obstacle[0] < l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return []
+ elif x_intersect_left_obstacle[0] < l_left_obstacle[0]:
+ return [x_intersect_right]
+ elif l_right_obstacle[0] < x_intersect_right_obstacle[0]:
+ return [x_intersect_left]
+ else:
+ return [x_intersect_left, x_intersect_right]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ if not self.exterior_point(x_obstacle, Frame.OBSTACLE):
+ return []
+ px, py = x_obstacle
+
+ # Special case with vertical tangent
+ a2 = self._a**2
+ vertical_tangent = abs(a2[0] - px**2) < 1e-5
+ if vertical_tangent:
+ m2 = (py**2-a2[1]) / (2*px*py)
+ x2 = (px * m2 ** 2 - py * m2) / (a2[1] / a2[0] + m2 ** 2)
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([px, 0])
+ tp2_obstacle = np.array([x2, y2])
+ else:
+ tmp = px**2 - a2[0]
+ c1 = (px * py) / tmp
+ c2 = (a2[1] - py**2) / tmp
+ tmp = np.sqrt(c1**2+c2)
+ m1, m2 = c1 + tmp, c1 - tmp
+ tmp = a2[1] / a2[0]
+ m1_sqr = m1**2
+ m2_sqr = m2**2
+ x1 = (px * m1_sqr - py * m1) / (tmp + m1_sqr)
+ x2 = (px * m2_sqr - py * m2) / (tmp + m2_sqr)
+ y1 = m1 * (x1 - px) + py
+ y2 = m2 * (x2 - px) + py
+ tp1_obstacle = np.array([x1, y1])
+ tp2_obstacle = np.array([x2, y2])
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ if is_ccw(x, tp1, tp2):
+ tp1, tp2 = tp2, tp1
+
+ return [tp1, tp2]
+
+ # def area(self):
+ # return self._area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ self._is_convex = True
+
+ def _compute_kernel(self):
+ self._kernel = self._polygon
+
+ def _compute_polygon_representation(self):
+ # logprint(str(self) + ": " + str(self._polygon), 0)
+ t = np.linspace(0, 2 * np.pi, self._n_pol, endpoint=False)
+ a = self._a + 1e-3 # Add offset to adjust for polygon approximation
+ polygon = np.vstack((a[0] * np.cos(t), a[1] * np.sin(t))).T
+ self._polygon = shapely.geometry.Polygon(polygon)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py
new file mode 100644
index 0000000..64e4746
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/motion_model.py
@@ -0,0 +1,119 @@
+import numpy as np
+from abc import ABC, abstractmethod
+
+
+class MotionModel(ABC):
+
+ def __init__(self, pos=None, rot=0):
+ self._pos = np.array([0., 0.]) if pos is None else np.array(pos, dtype='float64')
+ self._rot = rot
+ self._t = 0.
+
+ def move(self, obs_self, dt):
+ xy_vel = np.array(self.lin_vel())
+ rot_vel = self.rot_vel()
+ prev_pos, prev_rot = self._pos.copy(), self._rot
+ self._pos += xy_vel * dt
+ self._rot += rot_vel * dt
+ self._t += dt
+
+ def set_pos(self, pos):
+ self._pos = pos
+
+ def set_rot(self, rot):
+ self._rot = rot
+
+ def pos(self):
+ return self._pos
+
+ def rot(self):
+ return self._rot
+
+ @abstractmethod
+ def lin_vel(self):
+ pass
+
+ @abstractmethod
+ def rot_vel(self):
+ pass
+
+
+class Static(MotionModel):
+
+ def lin_vel(self):
+ return np.zeros(2)
+
+ def rot_vel(self):
+ return 0.
+
+
+class SinusVelocity(MotionModel):
+
+ # If cartesian_coords: (x1,x2) vel are Cartesian (x,y) vel.
+ # Else: (x1,x2) vel are linear and rotational (lin,ang) vel.
+ def __init__(self, pos=None, rot=0, cartesian_coords=True, x1_mag=0., x1_period=0, x2_mag=0., x2_period=0):
+ super().__init__(pos, rot)
+ self.cartesian_coords = cartesian_coords
+ self.x1_vel_mag = x1_mag
+ self.x1_vel_freq = 2 * np.pi / x1_period if x1_period else 0
+ self.x2_vel_mag = x2_mag
+ self.x2_vel_freq = 2 * np.pi / x2_period if x2_period else 0
+
+ def lin_vel(self):
+ if self.cartesian_coords:
+ return np.array([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t),
+ self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t)])
+ else:
+ rot_mat = np.array([[np.cos(self._rot), -np.sin(self._rot)], [np.sin(self._rot), np.cos(self._rot)]])
+ return rot_mat.dot([self.x1_vel_mag * np.cos(self.x1_vel_freq * self._t), 0])
+
+ def rot_vel(self):
+ if self.cartesian_coords:
+ return 0.
+ else:
+ return np.array(self.x2_vel_mag * np.cos(self.x2_vel_freq * self._t))
+
+
+class Interval(MotionModel):
+
+ def __init__(self, init_pos, time_pos):
+ self.pos_point = np.array([init_pos] + [p for _, p in time_pos])
+ self.time_point = np.cumsum([0] + [t for t, _ in time_pos])
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if self._t > self.time_point[-1]:
+ vel_norm = np.linalg.norm((self.pos_point[-1] - self.pos_point[-2]) / (self.time_point[-1] - self.time_point[-2]))
+ dir = self.pos_point[-1] - self.pos()
+ if np.linalg.norm(dir) > vel_norm:
+ dir /= np.linalg.norm(dir)
+ return vel_norm * dir
+ idx = np.argmax(self._t < self.time_point)
+ return (self.pos_point[idx] - self.pos_point[idx-1]) / (self.time_point[idx] - self.time_point[idx-1])
+
+ def rot_vel(self):
+ return 0.
+
+
+class Waypoints(MotionModel):
+
+ def __init__(self, init_pos, waypoints, vel, wp_thresh=0.2):
+ self.waypoints = np.array([init_pos] + waypoints)
+ self.vel = vel
+ self._wp_idx = 0
+ self.wp_thresh = wp_thresh
+ super().__init__(init_pos, 0)
+
+ def lin_vel(self):
+ if not self._wp_idx < len(self.waypoints):
+ return np.zeros(2)
+ dir = self.waypoints[self._wp_idx] - self.pos()
+ wp_dist = np.linalg.norm(dir)
+ if wp_dist < self.wp_thresh:
+ self._wp_idx += 1
+ return self.lin_vel()
+ dir /= wp_dist
+ return self.vel * dir
+
+ def rot_vel(self):
+ return 0.
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py
new file mode 100644
index 0000000..7789611
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/obstacle.py
@@ -0,0 +1,201 @@
+from abc import ABC, abstractmethod
+import numpy as np
+import shapely
+from shapely import affinity as sh_affinity
+from utils import affine_transform
+from copy import deepcopy
+from enum import Enum
+
+
+class Frame(Enum):
+ GLOBAL = 1
+ OBSTACLE = 2
+
+ class InvalidFrameError(Exception):
+ pass
+
+
+class Obstacle(ABC):
+ """ Abstract base class of obstacles
+ """
+ id_counter = 0
+
+ # obs_id <0: temp object, obs_id=0: new object, obs_id>0: existing object with id #obs_id
+ def __init__(self, motion_model=None, is_convex=None, is_starshaped=None, id='new', name=None, compute_polygon=False):
+ self._id = None
+ self._is_convex = is_convex
+ self._is_starshaped = is_starshaped
+ # Pose of local frame in global frame
+ self._motion_model = motion_model # if motion_model is not None else mm.Static([0., 0.], 0.)
+ # Initialize id and name
+ self._set_id_name(id, name)
+ self._polygon = None # Polygon in obstacle frame
+ self._polygon_global = None # Polygon in global frame if static obstacle
+ self._polygon_global_pose = None # Obstacle pose corresponding to global polygon
+ if compute_polygon:
+ self._compute_global_polygon_representation()
+
+ def __str__(self): return self._name
+
+ def copy(self, id='temporary', name=None):
+ ob = deepcopy(self)
+ if not (id == 'duplicate' or id == 'd'):
+ ob._set_id_name(id, name)
+ return ob
+
+ def pos(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return np.zeros(2)
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.pos()
+
+ def rot(self, output_frame=Frame.GLOBAL):
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return 0.
+ if output_frame == Frame.GLOBAL:
+ return self._motion_model.rot()
+
+ def interior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) < 0 else False
+
+ def exterior_point(self, x, input_frame=Frame.GLOBAL):
+ return True if self.point_location(x, input_frame) > 0 else False
+
+ def boundary_point(self, x, input_frame=Frame.GLOBAL):
+ return np.isclose(self.point_location(x, input_frame), 0.)
+
+ def move(self, dt):
+ if self._motion_model is not None:
+ self._motion_model.move(self, dt)
+
+ def id(self): return self._id
+
+ def polygon(self, output_frame=Frame.GLOBAL):
+ if self._polygon is None:
+ self._compute_polygon_representation()
+ # self._compute_global_polygon_representation()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._polygon
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._polygon_global_pose:
+ self._polygon_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = sh_affinity.affine_transform(self._polygon, affinity_matrix)
+ return self._polygon_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def intersects(self, other):
+ return self.polygon().intersects(other.polygon())
+
+ def transform(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._transform_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._transform_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def rotate(self, x, input_frame, output_frame):
+ if input_frame == output_frame or self._motion_model is None:
+ return x
+ elif input_frame == Frame.OBSTACLE and output_frame == Frame.GLOBAL:
+ return self._rotate_obstacle2global(x)
+ elif input_frame == Frame.GLOBAL and output_frame == Frame.OBSTACLE:
+ return self._rotate_global2obstacle(x)
+ else:
+ raise Frame.InvalidFrameError
+
+ def is_convex(self):
+ # Check if convexity already has been computed
+ if self._is_convex is None:
+ self._check_convexity()
+ return self._is_convex
+
+ def is_starshaped(self):
+ if self._is_starshaped is None:
+ if self.is_convex():
+ self._is_starshaped = True
+ else:
+ # TODO: Add check for starshapedness. Currently default to not starshaped
+ self._is_starshaped = False
+ return self._is_starshaped
+
+ def set_motion_model(self, motion_model):
+ self._motion_model = motion_model
+
+ # ------------ Private methods ------------ #
+ def _set_id_name(self, id, name=None):
+ if id == 'new' or id == 'n':
+ Obstacle.id_counter += 1
+ self._id = Obstacle.id_counter
+ elif id == 'temporary' or id == 'temp' or id == 't':
+ self._id = None
+ elif isinstance(id, int) and 0 < id <= Obstacle.id_counter:
+ self._id = id
+ else:
+ print("Invalid id '" + str(id) + "' in set_id. Create temporary obstacle.")
+ self._id = None
+ self._name = name if name else str(self._id)
+
+ def _rotate_obstacle2global(self, x_obstacle):
+ rot = self._motion_model.rot()
+ return affine_transform(x_obstacle, rotation=rot, translation=[0, 0])
+
+ def _rotate_global2obstacle(self, x_global):
+ rot = self._motion_model.rot()
+ return affine_transform(x_global, rotation=rot, translation=[0, 0], inverse=True)
+
+ def _transform_obstacle2global(self, x_obstacle):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_obstacle, rotation=rot, translation=pos)
+
+ def _transform_global2obstacle(self, x_global):
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ return affine_transform(x_global, rotation=rot, translation=pos, inverse=True)
+
+ def _compute_global_polygon_representation(self):
+ self._compute_polygon_representation()
+ if self._motion_model is None:
+ self._polygon_global = self._polygon
+ if self._motion_model.__class__.__name__ == 'Static':
+ pos, rot = (self._motion_model.pos(), self._motion_model.rot())
+ c, s = np.cos(rot), np.sin(rot)
+ trans_matrix = np.array([[c, -s, pos[0]], [s, c, pos[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1],
+ trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._polygon_global = shapely.affinity.affine_transform(self._polygon, affinity_matrix)
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def draw(self):
+ pass
+
+ @abstractmethod
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ @abstractmethod
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ @abstractmethod
+ def _check_convexity(self):
+ pass
+
+ @abstractmethod
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py
new file mode 100644
index 0000000..d8ec63a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/polygon.py
@@ -0,0 +1,158 @@
+from obstacles import Obstacle, Frame
+from utils import is_cw, is_ccw, is_collinear, tic, toc
+import shapely
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+class Polygon(Obstacle):
+
+ def __init__(self, polygon, **kwargs):
+ super().__init__(**kwargs)
+ self._polygon = shapely.geometry.Polygon(polygon)
+ self._polygon = shapely.ops.orient(self._polygon)
+ self._pol_bounds = self._polygon.bounds
+ self._compute_global_polygon_representation()
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+
+ def init_plot(self, ax=None, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+ if 'show_reference' in kwargs:
+ del kwargs['show_reference']
+ line_handles = []
+ # Boundary
+ line_handles += [patches.Polygon(np.random.rand(3, 2), **kwargs)]
+ ax.add_patch(line_handles[-1])
+ # Name
+ line_handles += [ax.text(0, 0, self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def extreme_points(self, frame=Frame.GLOBAL):
+ vertices = np.asarray(self.polygon(frame).exterior.coords)[:-1, :]
+ return [vertices[i] for i in range(vertices.shape[0])]
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ polygon = self.polygon(frame)
+ boundary = np.vstack((polygon.exterior.xy[0], polygon.exterior.xy[1])).T
+ line_handles[0].set_xy(boundary)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.pos(frame))
+
+ def draw(self, frame=Frame.GLOBAL, **kwargs):
+ line_handles, ax = self.init_plot(**kwargs)
+ self.update_plot(line_handles, frame)
+ return line_handles, ax
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ return cp
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ xmin, ymin, xmax, ymax = self._pol_bounds
+ if not (xmin < x_obstacle[0] < xmax and ymin < x_obstacle[1] < ymax):
+ return 1
+ x_sh = shapely.geometry.Point(x_obstacle)
+ if self._polygon.contains(x_sh):
+ return -1
+ if self._polygon.exterior.contains(x_sh):
+ return 0
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ l0_obstacle = self.transform(line[0], input_frame, Frame.OBSTACLE)
+ l1_obstacle = self.transform(line[1], input_frame, Frame.OBSTACLE)
+ intersection_points_shapely = shapely.geometry.LineString([l0_obstacle, l1_obstacle]).intersection(self._polygon.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ intersection_points_obstacle = [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ intersection_points_obstacle = [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ intersection_points_obstacle = [np.array([ip[0], ip[1]]) for line in intersection_points_shapely.geoms for ip in line.coords]
+ else:
+ print(intersection_points_shapely)
+ return [self.transform(ip, Frame.OBSTACLE, output_frame) for ip in intersection_points_obstacle]
+
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ t0 = tic()
+ phi = np.arctan2(self.circular_vertices[:, 1] - x_obstacle[1], self.circular_vertices[:, 0] - x_obstacle[0])
+ phi[phi < 0] += 2 * np.pi
+ t1 = toc(t0)
+ t0 = tic()
+ phi_diff = np.diff(phi)
+ t2 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = phi_diff > np.pi
+ phi_increase_idcs = phi_diff < -np.pi
+ t3 = toc(t0)
+ t0 = tic()
+ phi_decrease_idcs = np.flatnonzero(phi_decrease_idcs)
+ phi_increase_idcs = np.flatnonzero(phi_increase_idcs)
+ for i in phi_decrease_idcs:
+ phi[i+1:] -= 2*np.pi
+ for i in phi_increase_idcs:
+ phi[i+1:] += 2*np.pi
+ t4 = toc(t0)
+
+ t0 = tic()
+
+ i_min, i_max = np.argmin(phi), np.argmax(phi)
+
+ if abs(phi[0] - phi[-1]) > 0.00001:
+ # Interior point
+ return []
+ if (phi[i_max] - phi[i_min]) >= 2*np.pi:
+ # Blocked exterior point
+ return []
+ t5 = toc(t0)
+
+ t0 = tic()
+ tp1_obstacle = self.circular_vertices[i_max]
+ tp2_obstacle = self.circular_vertices[i_min]
+
+ tp1 = self.transform(tp1_obstacle, Frame.OBSTACLE, output_frame)
+ tp2 = self.transform(tp2_obstacle, Frame.OBSTACLE, output_frame)
+
+ tend = toc(t0)
+ # print(sum([t1*100,t2*100,t3*100,t4*100,t5*100,tend*100*0]))
+ return [tp1, tp2]
+
+ def area(self):
+ return self._polygon.area
+
+ # ------------ Private methods ------------ #
+ def _check_convexity(self):
+ v = np.asarray(self._polygon.exterior.coords)[:-1, :]
+ i = 0
+ N = v.shape[0]
+ # Make sure first vertice is not collinear
+ while is_collinear(v[i-1, :], v[i, :], v[(i+1) % N, :]):
+ i += 1
+ if i > N:
+ raise RuntimeError("Bad polygon shape. All vertices collinear")
+ # All vertices must be either cw or ccw when iterating through for convexity
+ if is_cw(v[i-1, :], v[i, :], v[i+1, :]):
+ self._is_convex = not any([is_ccw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+ else:
+ self._is_convex = not any([is_cw(v[j-1, :], v[j, :], v[(j+1) % N, :]) for j in range(v.shape[0])])
+
+ # Not needed
+ def _compute_polygon_representation(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
new file mode 100644
index 0000000..122ab8f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_obstacle.py
@@ -0,0 +1,85 @@
+from abc import abstractmethod
+import numpy as np
+from obstacles import Obstacle, Frame
+from shapely import affinity as sh_affinity
+
+
+class StarshapedObstacle(Obstacle):
+
+ def __init__(self, xr, **kwargs):
+ super().__init__(is_starshaped=True, **kwargs)
+ self._xr = np.array(xr) # Reference point in obstacle frame
+ self._kernel = None
+ self._kernel_global = None # Kernel in global frame
+ self._kernel_global_pose = None # Obstacle pose corresponding to global kernel
+
+ def xr(self, output_frame=Frame.GLOBAL):
+ return self.transform(self._xr, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ new_xr = self.transform(xr, input_frame, Frame.OBSTACLE)
+ if safe_set:
+ k = self.kernel()
+ if not k.exterior_point(new_xr, Frame.OBSTACLE):
+ self._xr = new_xr
+ else:
+ self._xr = new_xr
+
+ def reference_direction(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ dir = self.transform(x, input_frame, output_frame) - self.transform(self._xr, Frame.OBSTACLE, output_frame)
+ if not np.any(dir):
+ print("reference_direction for xr is not defined")
+ return dir / np.linalg.norm(dir, axis=x.ndim - 1)
+
+ # interior point: <1. exterior point: >1. boundary point: 1.
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_func = (np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1) / (
+ np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1))) ** 2
+ return dist_func
+
+ # interior point: <0. exterior point: >0. boundary point: 0.
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ return np.sign(self.distance_function(x, input_frame)-1.)
+
+ def kernel(self, output_frame=Frame.GLOBAL):
+ if self._kernel is None:
+ self._compute_kernel()
+ if output_frame == Frame.OBSTACLE or self._motion_model is None:
+ return self._kernel
+ elif output_frame == Frame.GLOBAL:
+ current_pose = [*self._motion_model.pos(), self._motion_model.rot()]
+ if not current_pose == self._kernel_global_pose:
+ self._kernel_global_pose = current_pose
+ c, s = np.cos(current_pose[2]), np.sin(current_pose[2])
+ trans_matrix = np.array([[c, -s, current_pose[0]], [s, c, current_pose[1]], [0, 0, 1]])
+ affinity_matrix = [trans_matrix[0, 0], trans_matrix[0, 1], trans_matrix[1, 0], trans_matrix[1, 1], trans_matrix[0, 2], trans_matrix[1, 2]]
+ self._kernel_global = sh_affinity.affine_transform(self._kernel, affinity_matrix)
+ return self._kernel_global
+ else:
+ raise Frame.InvalidFrameError
+
+ def vel_intertial_frame(self, x):
+ if self._motion_model is None:
+ return np.zeros(2)
+ omega = self._motion_model.rot_vel()
+ lin_vel_omega = np.cross(np.hstack(([0, 0], omega)), np.hstack((self.reference_direction(x), 0)))[:2]
+ return self._motion_model.lin_vel() + lin_vel_omega
+
+ # ------------ Abstract methods ------------ #
+ @abstractmethod
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False):
+ pass
+
+ @abstractmethod
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ pass
+
+ # @abstractmethod
+ # def signed_boundary_distance(self, x, input_frame=Frame.GLOBAL):
+ # pass
+
+ @abstractmethod
+ def _compute_kernel(self):
+ pass
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
new file mode 100644
index 0000000..85eba3c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_polygon.py
@@ -0,0 +1,866 @@
+from obstacles import Frame, StarshapedObstacle, Polygon
+import matplotlib.pyplot as plt
+import numpy as np
+import shapely
+from utils import is_cw, is_ccw, is_collinear, orientation_val, get_intersection, is_between, tic, toc
+
+
+class StarshapedPolygon(Polygon, StarshapedObstacle):
+
+ def __init__(self, polygon, xr=None, **kwargs):
+ super().__init__(polygon, xr=xr, **kwargs)
+ if xr is None:
+ self._compute_kernel()
+ if self._kernel.contains(self._kernel.centroid):
+ self._xr = np.array(self._kernel.centroid.coords[0])
+ else:
+ self._xr = np.array(self._kernel.representative_point().coords[0])
+ else:
+ self._xr = np.array(xr)
+ self.vertex_angles = None
+ self._update_vertex_angles()
+ self.enclosing_ball_diameter = self._polygon.bounds[2]-self._polygon.bounds[0] + self._polygon.bounds[3]-self._polygon.bounds[1]
+
+ def copy(self, id, name):
+ if (id == 'duplicate' or id == 'd'):
+ id = self.id()
+ return StarshapedPolygon(id=id, name=name, polygon=self._polygon, xr=self._xr, motion_model=self._motion_model)
+
+ # Note: Does not recompute the kernel
+ def dilated_obstacle(self, padding, id="new", name=None):
+ cp = self.copy(id, name)
+ cp._polygon = cp._polygon.buffer(padding, cap_style=1, join_style=1)
+ cp._pol_bounds = cp._polygon.bounds
+ cp.enclosing_ball_diameter = max(cp._polygon.bounds[2]-cp._polygon.bounds[0], cp._polygon.bounds[3]-cp._polygon.bounds[1])
+ cp.vertices = np.array(cp._polygon.exterior.coords[:-1])
+ cp.circular_vertices = np.array(cp._polygon.exterior.coords)
+ cp._polygon_global_pose = None
+ cp._polygon_global = None
+ cp._update_vertex_angles()
+ return cp
+
+ def distance_function(self, x, input_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ dist_center = np.linalg.norm(x_obstacle - self._xr, axis=x.ndim - 1)
+ local_radius = np.linalg.norm(self.boundary_mapping(x_obstacle, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ - self._xr, axis=x.ndim - 1)
+ if dist_center < local_radius:
+ # Return proportional inside to have -> [0, 1]
+ return (dist_center / local_radius) ** 2
+ else:
+ return ((dist_center - local_radius) + 1) ** 2
+
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ intersect_obstacle = self.line_intersection([self._xr, self._xr+1.1*self.enclosing_ball_diameter*self.reference_direction(x_obstacle, Frame.OBSTACLE, Frame.OBSTACLE)],
+ input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ if not intersect_obstacle:
+ return None
+ if len(intersect_obstacle) == 1:
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ if np.linalg.norm(intersect_obstacle[0]-x_obstacle) < np.linalg.norm(intersect_obstacle[1]-x_obstacle):
+ return self.transform(intersect_obstacle[0], Frame.OBSTACLE, output_frame)
+ else:
+ return self.transform(intersect_obstacle[1], Frame.OBSTACLE, output_frame)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='edge'):
+ x_obstacle = self.transform(x, input_frame, Frame.OBSTACLE)
+ angle = np.arctan2(x_obstacle[1]-self._xr[1], x_obstacle[0]-self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'edge':
+ n_obstacle = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1], self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ n_obstacle /= np.linalg.norm(n_obstacle)
+ elif type == 'weighted_edges':
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]], self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x_obstacle))[0].coords[0] for e in edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e)-x_obstacle) for e in edge_closest]
+ w = np.array([1/(d+1e-6) for d in dist])
+ w /= sum(w)
+ n_obstacle = edge_neighbors_normal.T.dot(w)
+ return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+
+ #directional weighted mean (see Appendix A) of normal vectors of the surface tiles ni(ξ), the weights wi,
+ # and with respect to the reference direction r
+ # n_vert = self.vertices.shape[0]
+ # r = self.reference_direction(x, input_frame=Frame.OBSTACLE, output_frame=Frame.OBSTACLE)
+ # B = np.array((r, (-r[1], r[0])))
+ # ws = np.zeros(n_vert)
+ # kappa = np.zeros(n_vert)
+ # p = 3
+ # vi_min = np.inf
+ #
+ # edge_r = np.zeros((n_vert, 2))
+ # _, ax = self.draw(frame=Frame.OBSTACLE)
+ # ax.plot(*x_obstacle, 'ko')
+ # for i in range(n_vert):
+ # # surface_i = np.array([self.vertices[v_idcs[i] - 1, :], self.vertices[v_idcs[i], :]])
+ # # edge_normal_i = np.array([-(surface_i[0, 1] - surface_i[1, 1]), surface_i[0, 0] - surface_i[1, 0]])
+ # edge_normal_i = np.array([self.vertices[i, 1] - self.vertices[i - 1, 1], self.vertices[i - 1, 0] - self.vertices[i, 0]])
+ # edge_normal_i /= np.linalg.norm(edge_normal_i)
+ # # rotated_edge_normal_i = Rpn.T.dot(edge_normal_i)
+ #
+ # pi = np.array(
+ # shapely.ops.nearest_points(shapely.geometry.LineString([self.vertices[i-1, :], self.vertices[i, :]]),
+ # shapely.geometry.Point(x_obstacle))[0].coords[0])
+ # # ax.quiver(*pi, *edge_normals[i, :], color='k')
+ # edge_r[i, :] = [np.mean([self.vertices[i, 0], self.vertices[i - 1, 0]]),
+ # np.mean([self.vertices[i, 1], self.vertices[i - 1, 1]])]
+ # vi = x_obstacle - pi
+ # ei = (vi - edge_normal_i.dot(vi)*edge_normal_i) * np.sign(vi.dot(x_obstacle-edge_r[i, :]))
+ # phi = np.arccos(ei.dot(vi) / (np.linalg.norm(ei) * np.linalg.norm(vi))) * np.sign(edge_normal_i.dot(vi))
+ #
+ # if phi > 0:
+ # ws[i] = 0
+ # else:
+ # ws[i] = (np.pi / phi) ** p - 1
+ #
+ # # if np.linalg.norm(vi) < vi_min:
+ # # ws = np.zeros(n_vert)
+ # # ws[i] = 1
+ # # vi_min = np.linalg.norm(vi)
+ #
+ #
+ #
+ # # if np.all(np.isclose(pi, x_obstacle)):
+ # # ws[i] = -1
+ # # else:
+ # # ws[i] = 1 / np.linalg.norm(x_obstacle - pi) ** 2
+ # rotated_edge_normal_i = B.T.dot(edge_normal_i)
+ #
+ # kappa[i] = 0 if rotated_edge_normal_i[0] == 1 else np.arccos(rotated_edge_normal_i[0]) * np.sign(
+ # rotated_edge_normal_i[1])
+ #
+ # # v_idx += 1
+ # # if np.any(ws < 0):
+ # # ws[np.nonzero(ws > 0)] = 0
+ # # ws[np.nonzero(ws)] = 1
+ # #
+ # ws = ws / np.sum(ws)
+ # # n_obstacle2 =
+ #
+ # kappa_bar = ws.dot(kappa)
+ # tmp_vec = [np.cos(abs(kappa_bar)), np.sin(abs(kappa_bar)) * np.sign(kappa_bar)]
+ # n_obstacle = B.dot(tmp_vec)
+ #
+ # return self.rotate(n_obstacle, Frame.OBSTACLE, output_frame)
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = super().init_plot(ax=ax, show_name=show_name, **kwargs)
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ # Reference point
+ line_handles += ax.plot(0, 0, '+', color='k') if show_reference else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles, frame=Frame.GLOBAL):
+ super().update_plot(line_handles, frame)
+ if line_handles[1] is not None:
+ line_handles[1].set_position(self.xr(frame))
+ if line_handles[2] is not None:
+ line_handles[2].set_data(*self.xr(frame))
+
+ def _update_vertex_angles(self):
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_kernel(self, verbose=False, debug=False):
+ if self.is_convex():
+ self._kernel = self._polygon
+ return
+
+ # Returns true if vertex v[i, :] is reflex
+ def is_reflex(i):
+ return is_cw(v[i - 1, :], v[i, :], v[(i + 1) % v.shape[0], :])
+
+ # Show polygon
+ def draw(xk, F1_idx, L1_idx, i, xk_bounded):
+ v_ext = np.vstack((v, v[0, :]))
+ xk_ext = np.vstack((xk, xk[0, :])) if xk_bounded else xk
+ plt.plot(v_ext[:, 0], v_ext[:, 1], label='v', marker='.')
+ # plt.plot(v_ext[:, 0], v_ext[:, 1], label='v')
+ # plt.text(v_ext[0, 0], v_ext[0, 1], 'v0')
+ plt.text(xk[0, 0], xk[0, 1], 'xk0')
+ axes = plt.gca()
+ xlim, ylim = axes.get_xlim(), axes.get_ylim()
+ plt.plot(xk_ext[:, 0], xk_ext[:, 1], label='xk')
+ plt.plot(xk[F1_idx, 0], xk[F1_idx, 1], marker='o', c='c', label='F1')
+ end_point = v[i, :] + INF_VAL * (xk[F1_idx, :] - v[i, :]) / np.linalg.norm(xk[F1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'c--')
+ plt.plot(xk[L1_idx, 0], xk[L1_idx, 1], marker='o', c='b', label='L1')
+ end_point = v[i, :] + INF_VAL * (xk[L1_idx, :] - v[i, :]) / np.linalg.norm(xk[L1_idx, :] - v[i, :])
+ plt.plot([v[i, 0], end_point[0]], [v[i, 1], end_point[1]], 'b--')
+ plt.plot(v[i, 0], v[i, 1], marker='x', c='r', ms=12, label='v{}'.format(i))
+ axes.set_xlim(xlim)
+ axes.set_ylim(ylim)
+
+ def point_left_of_line(point, line_head, line_tail):
+ return is_ccw(line_head, line_tail, point)
+
+ def point_right_of_line(point, line_head, line_tail):
+ return is_cw(line_head, line_tail, point)
+
+ pol = shapely.ops.orient(self._polygon)
+ v = np.asarray(pol.exterior.coords)[:-1, :]
+
+ # Remove points in line
+ v_idcs = np.arange(v.shape[0])
+ for i in range(v.shape[0]):
+ if is_collinear(v[v_idcs[i - 2], :], v[v_idcs[i - 1], :], v[v_idcs[i], :]):
+ v_idcs[i - 1] = v_idcs[i - 2]
+ v_idcs = np.unique(v_idcs)
+ v = v[v_idcs, :]
+ if is_collinear(v[-2, :], v[-1, :], v[0, :]):
+ v = v[:-1]
+
+ N = v.shape[0]
+ for i in range(N):
+ # Order v to have v[:, 0] as reflex vertex
+ if is_reflex(i):
+ v = np.vstack((v[i:, :], v[:i, :]))
+ break
+
+ INF_VAL = 300.
+ # Initial step
+ F1 = v[0, :] + INF_VAL * (v[0, :] - v[1, :]) / np.linalg.norm(v[0, :] - v[1, :])
+ L1 = v[0, :] + INF_VAL * (v[0, :] - v[-1, :]) / np.linalg.norm(v[0, :] - v[-1, :])
+ xk = np.vstack((F1, v[0, :], L1))
+ F1_idx = 0 # Index of point F1 in xk
+ L1_idx = 2 # Index of point L1 in xk
+ xk_bounded = False
+
+ if verbose:
+ print("------")
+ print(0)
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+
+ for i in range(1, N):
+
+ if verbose:
+ print("------")
+ print(i)
+ print("F1 index: {}".format(F1_idx))
+ print("L1 index: {}".format(L1_idx))
+
+ L1 = xk[L1_idx, :]
+ F1 = xk[F1_idx, :]
+ vi = v[i, :]
+ vi_1 = v[(i + 1) % N, :]
+
+ # (1) vi is reflex
+ if is_reflex(i):
+ # (1.2) F1 lies to the left of line ->(vi->vi+1)->vi+1
+ start_point = vi - INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+ if point_left_of_line(F1, start_point, vi_1):
+ case = 2
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title("F1 lies to the left of line ->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.show()
+ if verbose:
+ print("(1.2) - F1 ({},{}) lies to the left of line ->(v{}->v{})->v{}".format(F1[0], F1[0], i,
+ i + 1, i + 1))
+ # (1.1) F1 lies on or to the right of line ->(vi->vi+1)->vi+1
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(1.1) - F1 ({},{}) lies on or to the right of line ->(v{}->v{})->v{}".format(F1[0], F1[0],
+ i, i + 1,
+ i + 1))
+ print("Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk ccw from F1 to L1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w1 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ if not xk_bounded and t == 0:
+ break
+ # Get intersection w1 of current edge and line ->(vi->vi+1)->vi+1
+ wt_prev = xk[t - 1, :]
+ wt = xk[t, :]
+ w1 = get_intersection(wt_prev, wt, start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([wt_prev[0], wt[0]], [wt_prev[1], wt[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk ccw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+
+ if t == L1_idx:
+ break
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found.. Polygon not starshaped.')
+ plt.legend()
+ plt.show()
+ return False
+ if verbose:
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w1[0], w1[1], i, i + 1,
+ i + 1))
+ print("Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+ # Scan xk cw from F1 until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ w2 = None
+ idcs_list = np.flip(np.roll(np.arange(xk.shape[0]), -F1_idx - 1)) if xk_bounded else range(F1_idx,
+ 0, -1)
+ # for s in range(F1_idx, 0, -1):
+ for s in idcs_list:
+ ws = xk[s, :]
+ ws_prev = xk[s - 1, :]
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(ws_prev, ws, start_point, vi_1)
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([ws_prev[0], ws[0]], [ws_prev[1], ws[1]], marker='*', color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from F1 until we reach edge intersecting line ->(v{}->v{})->v{}".format(i,
+ i + 1,
+ i + 1))
+
+ if w2 is not None:
+ if xk_bounded and s > t:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[t:s, :]
+ L1_idx = L1_idx - t + 2
+ else:
+ alpha = xk[:s, :]
+ beta = xk[t:, :]
+ L1_idx = L1_idx - t + s + 2
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ xk = np.vstack((alpha, w2, w1, beta))
+ w1_idx = alpha.shape[0] + 1
+ w2_idx = alpha.shape[0]
+ ####### F1 index reassignment should not be necessary for this case
+ F1_idx = w2_idx
+ # L1_idx = L1_idx - t + s + 2
+
+ if verbose:
+ print("Update 1")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope ->(vi->vi+1)->vi+1 is comprised between the slopes of initial and final half lines of xk,
+ if (orientation_val(xk[-2, :], xk[-1, :], start_point) * orientation_val(vi_1, start_point,
+ xk[0, :])) > 0:
+ beta = xk[t:, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*', color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "->(v{}->v{})->v{} between initial and finial half lines of xk".format(
+ i, i + 1, i + 1))
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= ->(vi->vi+1)->w1->beta is also unbounded.
+ xk = np.vstack((start_point, w1, beta))
+ w1_idx = 1
+ # xk_bounded = False
+ F1_idx = 0
+ L1_idx -= t - 1
+
+
+ else:
+ # otherwise scan xk cw from xk[-1,:] until we reach edge intersecting line ->(vi->vi+1)->vi+1
+ if verbose:
+ print(
+ "Scan xk cw from end until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ w2 = None
+ if xk_bounded:
+ r = xk.shape[0]
+ w2 = get_intersection(xk[0, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[0, 0]], [xk[r - 1, 1], xk[0, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y',
+ label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ if debug:
+ plt.legend()
+ plt.show()
+ if w2 is None:
+ for r in range(xk.shape[0] - 1, 0, -1):
+ # Get intersection w2 of edge and line ->(vi->vi+1)->vi+1
+ w2 = get_intersection(xk[r, :], xk[r - 1, :], start_point, vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r, 0]], [xk[r - 1, 1], xk[r, 1]], marker='*',
+ color=col, label="edge")
+ plt.plot([start_point[0], vi_1[0]], [start_point[1], vi_1[1]], marker='*',
+ color='y', label="->(v{}->v{})->v{}".format(i, i + 1, i + 1))
+ plt.title(
+ "Scan xk cw from xk[-1, :] until we reach edge intersecting line ->(v{}->v{})->v{}".format(
+ i, i + 1, i + 1))
+ if w2 is not None:
+ delta = xk[t:r, :]
+ if debug:
+ print(t, r)
+ plt.plot(delta[:, 0], delta[:, 1], 'k--', label="delta")
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line ->(v{}->v{})->v{}".format(w2[0], w2[1], i,
+ i + 1, i + 1))
+ # Set xk as delta-w2-w1
+ xk = np.vstack((delta, w2, w1))
+ w1_idx = delta.shape[0] + 1
+ w2_idx = delta.shape[0]
+ xk_bounded = True
+
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+ if case == 1:
+ # If ->(vi->vi+1)->vi+1 has just one intersection with xk F1 = startpoint
+ if w2 is None:
+ F1_idx = 0
+ # Otherwise F1 = w2
+ else:
+ F1_idx = w2_idx
+ if case == 2:
+ # Scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+
+ # Check update of previous L1 index for new xk
+ # if not np.isclose(np.linalg.norm(L1 - xk[L1_idx, :]), 0):
+ # print("BAD L1 update [{},{}] -> [{},{}]".format(xk[L1_idx, 0], xk[L1_idx, 1], L1[0], L1[1]))
+ # plt.figure(2)
+ # draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ # plt.show()
+
+ # L1 update
+ # scan xk ccw from L1 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (L1_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ else:
+ if verbose:
+ print("(2)")
+ # Endpoint for line vi->(vi->vi+1)->
+ end_point = vi + INF_VAL * (vi_1 - vi) / np.linalg.norm(vi_1 - vi)
+
+ # (2.2) L1 lies to the left of line vi->(vi->vi+1)->
+ if point_left_of_line(L1, vi, end_point):
+ case = 2
+ if verbose:
+ print("(2.2) - L1 ({},{}) lies to the left of line v{}->(v{}->v{})->".format(L1[0], L1[1], i, i,
+ i + 1))
+ # xk stays the same
+
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([end_point[0], vi[0]], [end_point[1], vi[1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title("L1 lies to the left of line v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.show()
+
+ # (2.1) L1 lies on or to the right of line vi->(vi->vi+1)->
+ else:
+ case = 1
+ if verbose:
+ print(
+ "(2.1) - L1 ({},{}) lies on or to the right of line v{}->(v{}->v{})->".format(L1[0], L1[1],
+ i, i, i + 1))
+ print(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i, i, i + 1))
+ # Scan xk cw from L1 until we reach F1 or an edge intersecting line vi->(vi->vi+1)->
+ idx_offsets = range(xk.shape[0])
+ w1 = None
+ for off in idx_offsets:
+ t = L1_idx - off
+ # If circular
+ if t < 0:
+ t += xk.shape[0]
+ if t == F1_idx:
+ break
+ # Get intersection w1 of edge and line vi->(vi->vi+1)->
+ w1 = get_intersection(xk[t, :], xk[t - 1, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w1 is None else 'g'
+ plt.plot([xk[t - 1, 0], xk[t, 0]], [xk[t - 1, 1], xk[t, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk cw from L1 until we reach F1 or an edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w1 is not None:
+ if debug:
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached no kernel exists
+ if w1 is None:
+ # if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.title('No kernel found. Polygon not starshaped.')
+ plt.show()
+ return False
+
+ if verbose:
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w1[0], w1[1], i, i, i + 1, xk[t - 1, 0], xk[t - 1, 1], xk[t, 0], xk[t, 1]))
+ print("Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ # Scan xk ccw from L1 until we reach edge w2 intersecting line vi->(vi->vi+1)->
+ w2 = None
+ idx_offsets = range(1, xk.shape[0] + 1)
+ for off in idx_offsets:
+ s = (L1_idx + off) % xk.shape[0]
+ if not xk_bounded and s == 0:
+ break
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[s - 1, :], xk[s, :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[s - 1, 0], xk[s, 0]], [xk[s - 1, 1], xk[s, 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from L1 until we reach edge intersecting line v{}->(v{}->v{})->".format(i,
+ i,
+ i + 1))
+ if w2 is not None:
+ if xk_bounded and t > s:
+ alpha = xk[xk.shape[0]:, :] # Empty array
+ beta = xk[s:t, :]
+ else:
+ alpha = xk[:t, :]
+ beta = xk[s:, :]
+ if debug:
+ print("xk: ", end=" ")
+ print(xk)
+ print("alpha: ", end=" ")
+ print(alpha)
+ print("beta: ", end=" ")
+ print(beta)
+ print("w1: ", end=" ")
+ print(w1)
+ print("w2: ", end=" ")
+ print(w2)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='k')
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.plot(beta[:, 0], beta[:, 1], 'k--', label="beta")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 1")
+ print(
+ "Found intersection ({},{}) at line v{}->(v{}->v{})-> for edge ({},{})-({},{})".format(
+ w2[0], w2[1], i, i, i + 1, xk[s, 0], xk[s, 1], xk[(s + 1) % xk.shape[0], 0],
+ xk[(s + 1) % xk.shape[0], 1]))
+
+ xk = np.vstack((alpha, w1, w2, beta))
+ w1_idx = alpha.shape[0]
+ w2_idx = alpha.shape[0] + 1
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ # If no intersecting line is reached
+ if w2 is None:
+ # Test if xk+1 is bounded
+ # If slope vi->(vi->vi+1)-> is comprised between the slopes of initial and final half lines of xk,
+ if not xk_bounded and ((orientation_val(xk[-2, :], xk[-1, :], end_point) * orientation_val(vi,
+ end_point,
+ xk[0,
+ :])) > 0):
+ alpha = xk[:t, :]
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot(w1[0], w1[1], 's', label="w1", color='g')
+ plt.plot(xk[:2, 0], xk[:2, 1], '--c', label="initial half line")
+ plt.plot(xk[-2:, 0], xk[-2:, 1], '--m', label="final half line")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "v{}->(v{}->v{})-> between initial and final half lines of xk".format(
+ i, i, i + 1))
+ plt.plot(alpha[:, 0], alpha[:, 1], 'r--', label="alpha")
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 2 - xk still unbounded")
+ # then xk+1= alpha->w1->(vi->vi+1)-> is also unbounded.
+ xk = np.vstack((alpha, w1, end_point))
+ w1_idx = alpha.shape[0]
+ L1_idx = xk.shape[0] - 1
+ F1_idx = 0
+ else:
+ if verbose:
+ print(
+ "Scan xk ccw from start until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ # otherwise scan xk ccw from xk[0,:] until we reach edge intersecting line vi->(vi->vi+1)->
+ w2 = None
+ for r in range(1, xk.shape[0] + 1):
+ # Get intersection w2 of edge and line vi->(vi->vi+1)->
+ w2 = get_intersection(xk[r - 1, :], xk[r % xk.shape[0], :], vi, end_point)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ col = 'r' if w2 is None else 'g'
+ plt.plot([xk[r - 1, 0], xk[r % xk.shape[0], 0]],
+ [xk[r - 1, 1], xk[r % xk.shape[0], 1]], marker='*', color=col,
+ label="edge")
+ plt.plot([end_point[0], v[i, 0]], [end_point[1], v[i, 1]], marker='*', color='y',
+ label="v{}->(v{}->v{})->".format(i, i, i + 1))
+ plt.title(
+ "Scan xk ccw from xk[0,:] until we reach edge intersecting line v{}->(v{}->v{})->".format(
+ i,
+ i,
+ i + 1))
+ if w2 is not None:
+ delta = xk[r:t, :]
+ if debug:
+ plt.plot(w2[0], w2[1], 's', label="w2", color='g')
+ plt.legend()
+ plt.show()
+ break
+ if debug:
+ plt.legend()
+ plt.show()
+ if verbose:
+ print("Update 3")
+ print("Found intersection ({},{}) at line v{}->(v{}->v{})->".format(w2[0], w2[1], i, i,
+ i + 1))
+ # Set xk as delta-w1-vi-vi+1-w2
+ xk = np.vstack((delta, w1, w2))
+ w1_idx = delta.shape[0]
+ w2_idx = delta.shape[0] + 1
+ xk_bounded = True
+ F1_idx = 0
+ L1_idx = min(L1_idx - t, xk.shape[0] - 1)
+
+ # F1 update
+
+ # If vi+1 in vi->(vi->vi+1)->w1,
+ if case == 2 or is_between(vi, vi_1, w1):
+ # scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the
+ # right of vi+1->(vi+1->wt)->. Let F1 = wt.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ t = (F1_idx + off) % xk.shape[0]
+ w_next = xk[(t + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[t, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_right_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update F1\n scan xk ccw from F1 until find vertex wt s.t. wt+1 lies to the right of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_right_of_line(w_next, vi_1, line_end_point):
+ F1_idx = t
+ break
+ else:
+ F1_idx = w1_idx
+
+ # L1 update
+ if case == 1:
+ if w2 is not None:
+ # If vi+1 in vi->(vi->vi+1)->w2
+ if is_between(vi, vi_1, w2):
+ L1_idx = w2_idx
+ else:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idx_offsets = range(xk.shape[0])
+ for off in idx_offsets:
+ u = (w2_idx + off) % xk.shape[0]
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ # (2.1.2) line vi->(vi->vi+1)-> intersects xk just in w1
+ else:
+ L1_idx = xk.shape[0] - 1
+ if case == 2:
+ # print("(2.1.2)")
+ if xk_bounded:
+ # scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the
+ # left of vi+1->(vi+1->wu)->. Let L1 = wu.
+ idcs_list = np.roll(np.arange(xk.shape[0]), -L1_idx) if xk_bounded else range(L1_idx,
+ xk.shape[0] - 1)
+ for u in idcs_list:
+ w_next = xk[(u + 1) % xk.shape[0], :]
+ line_end_point = vi_1 + INF_VAL * (xk[u, :] - vi_1)
+ if debug:
+ draw(xk, F1_idx, L1_idx, i, xk_bounded)
+ plt.plot([vi_1[0], line_end_point[0]], [vi_1[1], line_end_point[1]], '--',
+ label='v{}->(v{}->wt)'.format(i + 1, i + 1))
+ c = 'g' if point_left_of_line(w_next, vi_1, line_end_point) else 'r'
+ plt.plot(w_next[0], w_next[1], color=c, marker='s', label='w_t+1')
+ plt.title(
+ "Update L1\n scan xk ccw from w2 until find vertex wu s.t. wu+1 lies to the left of v{}->(v{}->wt)->.".format(
+ i + 1, i + 1))
+ plt.legend()
+ plt.show()
+ if point_left_of_line(w_next, vi_1, line_end_point):
+ L1_idx = u
+ break
+
+ if verbose:
+ print("F1:", end=" ")
+ print(F1)
+ print("L1:", end=" ")
+ print(L1)
+ print("xk:", end=" ")
+ print(xk)
+ if debug or verbose:
+ draw(xk, F1_idx, L1_idx, 0, xk_bounded)
+ plt.show()
+ _, idx = np.unique(xk, axis=0, return_index=True)
+ xk = xk[np.sort(idx), :]
+ self._kernel = shapely.geometry.Polygon(xk)
+
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
new file mode 100644
index 0000000..6fdf2cd
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/obstacles/starshaped_primitive_combination.py
@@ -0,0 +1,226 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedObstacle, StarshapedPolygon
+from utils import is_ccw, is_cw, draw_shapely_polygon
+import matplotlib.pyplot as plt
+
+# Note: Local == Global frame
+class StarshapedPrimitiveCombination(StarshapedObstacle):
+
+ def __init__(self, obstacle_cluster, hull_cluster, xr, **kwargs):
+ self._obstacle_cluster = obstacle_cluster
+ self._hull_cluster = hull_cluster
+ super().__init__(xr=xr, **kwargs)
+ self.vertices = None
+ self.circular_vertices = None
+ self.vertex_angles = None
+
+ def obstacle_cluster(self):
+ return self._obstacle_cluster
+
+ def hull_cluster(self):
+ return self._hull_cluster
+
+ def dilated_obstacle(self, padding, id="new", name=None):
+ pass
+
+ def point_location(self, x, input_frame=Frame.GLOBAL):
+ locs = [obs.point_location(x, input_frame=Frame.GLOBAL) for obs in self._obstacle_cluster] + \
+ [self._hull_cluster_point_location(x)]
+ if any([l < 0 for l in locs]):
+ # Interior point
+ return -1
+ if any([l == 0 for l in locs]):
+ # Boundary point
+ return 0
+ # Exterior point
+ return 1
+
+ def line_intersection(self, line, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ intersection_points = []
+ for o in self._obstacle_cluster:
+ intersection_points += o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ intersection_points += self._hull_cluster_line_intersections(line)
+ return intersection_points
+
+ # TODO: Fix if needed. Currently not considering hull.
+ def tangent_points(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ tp, tp_candidates = [], []
+ for obs in self._obstacle_cluster:
+ tp_candidates += obs.tangent_points(x, Frame.GLOBAL, Frame.GLOBAL)
+ for i in range(len(tp_candidates)):
+ if all([is_ccw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]) or \
+ all([is_cw(x, tp_candidates[i], tp_candidates[j]) for j in range(len(tp_candidates)) if j is not i]):
+ tp += [tp_candidates[i]]
+ return tp
+
+ def _compute_kernel(self):
+ self._kernel = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").kernel()
+
+ def _check_convexity(self):
+ self._is_convex = StarshapedPolygon(self.polygon(), xr=self.xr(), id="temp").is_convex()
+
+ def boundary_mapping(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL):
+ # intersection_points = [p for ps in self.line_intersection([self._xr, self._xr+10*(x-self._xr)]) for p in ps]
+ intersection_points = self.line_intersection([self._xr, self._xr+10*(x-self._xr)])
+ if not intersection_points:
+ return None
+ dist_intersection_points = [np.linalg.norm(ip - self._xr) for ip in intersection_points]
+ return intersection_points[np.argmax(dist_intersection_points)]
+
+ def vel_intertial_frame(self, x):
+ boundary_obs_idx = 0
+ max_dist = -1
+ for i, ps in enumerate(self.line_intersection([self._xr, self._xr+10*(x-self._xr)])):
+ o_intersection_dist = max([np.linalg.norm(p-self._xr) for p in ps] + [-1])
+ if o_intersection_dist > max_dist:
+ boundary_obs_idx = i
+ max_dist = o_intersection_dist
+ if boundary_obs_idx >= len(self._obstacle_cluster):
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ return self._obstacle_cluster[boundary_obs_idx].vel_intertial_frame(x)
+
+ def normal(self, x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL, x_is_boundary=False, type='weigthed_polygon_approx'):
+ if type == 'sub_normal':
+ boundary_obs_idx = 0
+ max_dist = -1
+ line = [self._xr, self._xr+10*(x-self._xr)]
+ for i, o in enumerate(self._obstacle_cluster):
+ intersection_points = o.line_intersection(line, Frame.GLOBAL, Frame.GLOBAL)
+ for p in intersection_points:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = i
+
+ hull_ip = self._hull_cluster_line_intersections(line)
+ if hull_ip:
+ for p in hull_ip:
+ p_dist = np.linalg.norm(p - self._xr)
+ if p_dist > max_dist:
+ max_dist = p_dist
+ boundary_obs_idx = len(self._obstacle_cluster)
+ if boundary_obs_idx < len(self._obstacle_cluster):
+ return self._obstacle_cluster[boundary_obs_idx].normal(x, input_frame=Frame.GLOBAL, output_frame=Frame.GLOBAL)
+ else:
+ boundary_obs_idx -= len(self._obstacle_cluster)
+ hull_vertices = np.array(self._hull_cluster.exterior.coords[:-1])
+ vertex_angles = np.array([np.arctan2(v[1] - self._xr[1], v[0] - self._xr[0]) for v in hull_vertices]).flatten()
+ idcs = np.argsort(vertex_angles)
+ vertex_angles = vertex_angles[idcs]
+ hull_vertices = hull_vertices[idcs, :]
+ vertex_angles = np.hstack((vertex_angles, vertex_angles[0] + 2 * np.pi))
+ hull_vertices = np.vstack((hull_vertices, hull_vertices[0, :]))
+
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(vertex_angles > angle)
+ # Adjust for circular self.vertices (self.vertices[0] == self.vertices[-1])
+ if v_idx == 0:
+ v_idx = -1
+ n = np.array([hull_vertices[v_idx, 1] - hull_vertices[v_idx - 1, 1],
+ hull_vertices[v_idx - 1, 0] - hull_vertices[v_idx, 0]])
+ n /= np.linalg.norm(n)
+ return n
+ elif type == 'polygon_approx' or type == 'weigthed_polygon_approx':
+ if self.vertices is None:
+ self._update_vertex_angles()
+ angle = np.arctan2(x[1] - self._xr[1], x[0] - self._xr[0])
+ v_idx = np.argmax(self.vertex_angles > angle)
+
+ if v_idx == self.vertices.shape[0]:
+ v_idx = 0
+
+ if type == 'polygon_approx':
+ n = np.array([self.vertices[v_idx, 1] - self.vertices[v_idx - 1, 1],
+ self.vertices[v_idx - 1, 0] - self.vertices[v_idx, 0]])
+ else:
+ edge_neighbors = [(self.vertices[(v_idx - 2 + i) % self.vertices.shape[0]],
+ self.vertices[(v_idx - 1 + i) % self.vertices.shape[0]]) for i in range(3)]
+ edge_neighbors_normal = np.array([[e[1][1] - e[0][1],
+ e[0][0] - e[1][0]] for e in edge_neighbors])
+ edge_closest = [shapely.ops.nearest_points(shapely.geometry.LineString(e),
+ shapely.geometry.Point(x))[0].coords[0] for e in
+ edge_neighbors]
+
+ dist = [np.linalg.norm(np.array(e) - x) for e in edge_closest]
+ w = np.array([1 / (d + 1e-10) for d in dist])
+ w /= sum(w)
+ n = edge_neighbors_normal.T.dot(w)
+
+ n /= np.linalg.norm(n)
+ return n
+
+ def set_xr(self, xr, input_frame=Frame.OBSTACLE, safe_set=False):
+ super().set_xr(xr, input_frame, safe_set)
+ self._update_vertex_angles()
+
+ def init_plot(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ if "fc" not in kwargs and "facecolor" not in kwargs:
+ kwargs["fc"] = 'lightgrey'
+
+ line_handles = []
+
+ lh, _ = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ line_handles += lh
+
+ # Reference point
+ line_handles += ax.plot(*self.xr(), '+', color='k') if show_reference else [None]
+ # Name
+ line_handles += [ax.text(*self.xr(), self._name)] if show_name else [None]
+ return line_handles, ax
+
+ def update_plot(self, line_handles):
+ pass
+
+ def draw(self, ax=None, show_reference=True, show_name=False, **kwargs):
+ line_handles, ax = self.init_plot(ax, show_reference, show_name, **kwargs)
+ self.update_plot(line_handles)
+ return line_handles, ax
+
+ def _hull_cluster_point_location(self, x):
+ if self._hull_cluster is None:
+ return 1
+ x_sh = shapely.geometry.Point(x)
+ if self._hull_cluster.contains(x_sh):
+ return -1
+ if self._hull_cluster.disjoint(x_sh):
+ return 1
+ return 0
+
+ def _hull_cluster_line_intersections(self, line):
+ if self._hull_cluster is None:
+ return []
+ line_sh = shapely.geometry.LineString(line)
+ intersection_points_shapely = line_sh.intersection(self._hull_cluster.exterior)
+ if intersection_points_shapely.is_empty:
+ return []
+ if intersection_points_shapely.geom_type == 'Point':
+ return [np.array([intersection_points_shapely.x, intersection_points_shapely.y])]
+ elif intersection_points_shapely.geom_type == 'MultiPoint':
+ return [np.array([p.x, p.y]) for p in intersection_points_shapely.geoms]
+ elif intersection_points_shapely.geom_type == 'LineString':
+ return [np.array([ip[0], ip[1]]) for ip in intersection_points_shapely.coords]
+ elif intersection_points_shapely.geom_type == 'MultiLineString':
+ return [np.array([l[0], l[1]]) for line in intersection_points_shapely.geoms for
+ l in line.coords]
+ else:
+ print("[_hull_cluster_line_intersections]: Shapely geom_type not covered!")
+ print(intersection_points_shapely)
+
+ def _update_vertex_angles(self):
+ self.vertices = np.array(self._polygon.exterior.coords[:-1])
+ self.circular_vertices = np.array(self._polygon.exterior.coords)
+ self.vertex_angles = np.arctan2(self.vertices[:, 1] - self._xr[1], self.vertices[:, 0] - self._xr[0])
+ idcs = np.argsort(self.vertex_angles)
+ self.vertex_angles = self.vertex_angles[idcs]
+ self.vertices = self.vertices[idcs, :]
+ self.circular_vertices = np.vstack((self.vertices, self.vertices[0, :]))
+ self.vertex_angles = np.hstack((self.vertex_angles, self.vertex_angles[0] + 2 * np.pi))
+
+ def _compute_polygon_representation(self):
+ obs_pol = [obs.polygon() for obs in self._obstacle_cluster]
+ if self._hull_cluster is not None:
+ obs_pol += [self._hull_cluster]
+ self._polygon = shapely.ops.unary_union(obs_pol)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt
new file mode 100644
index 0000000..3689a74
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/requirements.txt
@@ -0,0 +1,4 @@
+numpy
+scipy
+matplotlib
+shapely
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py
new file mode 100644
index 0000000..6776181
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/setup.py
@@ -0,0 +1,14 @@
+from setuptools import setup, find_packages
+
+setup(name='starworlds',
+ version='1.0',
+ packages=find_packages(),
+ install_requires=[
+ 'pyyaml',
+ 'numpy',
+ 'scipy',
+ 'matplotlib',
+ 'shapely'
+ ]
+
+)
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
new file mode 100644
index 0000000..45406b1
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/__init__.py
@@ -0,0 +1,2 @@
+from .starshaped_hull import admissible_kernel, kernel_starshaped_hull
+from .cluster_and_starify import ObstacleCluster, get_intersection_clusters, cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
new file mode 100644
index 0000000..2e825e8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/cluster_and_starify.py
@@ -0,0 +1,500 @@
+import shapely
+import numpy as np
+from obstacles import Frame, StarshapedPrimitiveCombination, Ellipse, StarshapedPolygon
+from utils import is_ccw, is_collinear, equilateral_triangle, Cone, tic, toc, draw_shapely_polygon
+from scipy.spatial import ConvexHull
+import starshaped_hull as sh
+import matplotlib.pyplot as plt
+
+
+class ObstacleCluster:
+ def __init__(self, obstacles):
+ self.name = '_'.join([str(o.id()) for o in obstacles])
+ self.obstacles = obstacles
+ self.cluster_obstacle = None if len(obstacles) > 1 else obstacles[0]
+ self.kernel_points = None
+ self.admissible_kernel = None
+ self._polygon = None
+ self._polygon_excluding_hull = None
+
+ def __str__(self):
+ return self.name
+
+ def polygon(self):
+ if self._polygon is None:
+ if self.cluster_obstacle is None:
+ print("[OBSTACLE CLUSTER]: WARNING, cluster_obstacle must be defined before accessing polygon.")
+ else:
+ self._polygon = self.cluster_obstacle.polygon()
+ return self._polygon
+
+ def polygon_excluding_hull(self):
+ if self._polygon_excluding_hull is None:
+ self._polygon_excluding_hull = shapely.ops.unary_union([o.polygon() for o in self.obstacles])
+ return self._polygon_excluding_hull
+
+ def draw(self, ax=None):
+ if self.cluster_obstacle is not None:
+ ax, _ = self.cluster_obstacle.draw(ax=ax, fc="green")
+ for obs in self.obstacles:
+ ax, _ = obs.draw(ax=ax)
+ return ax, _
+
+
+def get_intersection_clusters(clusters):
+ No = len(clusters)
+ intersection_idcs = []
+
+ # Use polygon approximations for intersection check
+ cluster_polygons = [cl.polygon() for cl in clusters]
+
+ # Find intersections
+ intersections_exist = False
+ for i in range(No):
+ intersection_idcs += [[i]]
+ for j in range(i + 1, No):
+ if cluster_polygons[i].intersects(cluster_polygons[j]):
+ intersection_idcs[i] += [j]
+ intersections_exist = True
+
+ if not intersections_exist:
+ return clusters, intersections_exist
+
+ # Cluster intersecting obstacles
+ for i in range(No - 1, 0, -1):
+ for j in range(i - 1, -1, -1):
+ found = False
+ for l_j in intersection_idcs[j]:
+ if l_j in intersection_idcs[i]:
+ found = True
+ break
+ if found:
+ intersection_idcs[j] = list(set(intersection_idcs[j] + intersection_idcs[i]))
+ intersection_idcs[i] = []
+ break
+
+ # Create obstacle clusters
+ cluster_obstacles = [cl.obstacles for cl in clusters]
+ new_clusters = []
+ for i in range(No):
+ if intersection_idcs[i]:
+ new_clusters += [ObstacleCluster([o for j in intersection_idcs[i] for o in cluster_obstacles[j]])]
+
+ return new_clusters, intersections_exist
+
+
+def compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace):
+ triangle_center_prev = np.mean(cl_prev.kernel_points, axis=0) if cl_prev else None
+
+ t0 = tic()
+ ts = {}
+ # Find triangle center selection set (TCSS)
+ tcss = cl.admissible_kernel
+ # If obstacle is in exterior of workspace limit TCSS to workspace exterior
+ if not cl.polygon_excluding_hull().within(workspace.polygon()):
+ tcss_tmp = tcss.difference(workspace.polygon())
+ tcss_tmp = tcss_tmp.intersection(cl.polygon_excluding_hull()) # NOTE: Added for exlcluding undesired extremas in other workspace exterior than close to obstacles
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+ tcss_tmp = tcss
+ ts['ws check'] = toc(t0)
+ # Try to use intersection of all obstacle kernels in cluster if all starshaped
+ if all([o.is_starshaped() for o in cl.obstacles]):
+ for o in cl.obstacles:
+ tcss_tmp = tcss_tmp.intersection(o.kernel())
+ ts['kernel intersection'] = toc(t0)-list(ts.values())[-1]
+ # Else, try to use union of all obstacles in cluster
+ if tcss_tmp.area < 1e-6:
+ tcss_tmp = tcss.intersection(cl.polygon_excluding_hull())
+ ts['cluster intersection'] = toc(t0)-list(ts.values())[-1]
+ if tcss_tmp.area > 1e-6:
+ tcss = tcss_tmp
+
+ # If not tc from previous iteraion, use closest point to TCSS in ad ker as triangle center
+ if triangle_center_prev is None:
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, tcss.centroid)
+ triangle_center = np.array(tc.coords[0])
+ while is_collinear(x, xg, triangle_center):
+ triangle_center += np.random.uniform(-1e-4, 1e-4, 2)
+ # Else, use previous triangle center if still in selection set
+ elif tcss.contains(shapely.geometry.Point(triangle_center_prev))\
+ and not is_collinear(x, xg, triangle_center_prev):
+ triangle_center = triangle_center_prev
+ # Else, try to maintain triangle center on same side of l(x,xg) as previous time step
+ else:
+ x_xg_line = shapely.geometry.LineString([x, xg])
+ splitted_tcss = shapely.ops.split(tcss, x_xg_line).geoms
+ triangle_center_selection_set = splitted_tcss[0]
+ if len(splitted_tcss) > 1:
+ for i in range(1, len(splitted_tcss)):
+ if is_ccw(x, xg, splitted_tcss[i].centroid.coords[0]) == is_ccw(x, xg, triangle_center_prev):
+ triangle_center_selection_set = splitted_tcss[i]
+ break
+ tc, _ = shapely.ops.nearest_points(cl.admissible_kernel, triangle_center_selection_set.centroid)
+ triangle_center = np.array(tc.coords[0])
+
+ ts['tc selection'] = toc(t0)-list(ts.values())[-1]
+
+ # if cl.name == '5_6_7':
+ # hs, _ = draw_shapely_polygon(tcss, plt.gca(), fc='g')
+ # hs += plt.plot(*triangle_center, 'kd')
+ # while not plt.waitforbuttonpress(): pass
+ # [h.remove() for h in hs]
+
+ # Select kernel points as largest equilateral triangle in TCSS (with maximum side length epsilon)
+ if tcss.geom_type == 'Polygon':
+ dist = tcss.exterior.distance(shapely.geometry.Point(triangle_center))
+ else:
+ tc = shapely.geometry.Point(triangle_center)
+ dist = min([p.exterior.distance(tc) for p in tcss.geoms])
+ triangle_length = min(epsilon, 0.9 * dist)
+ kernel_points = equilateral_triangle(triangle_center, triangle_length)
+ ts['triangle generation'] = toc(t0)-list(ts.values())[-1]
+ tot_time = sum(ts.values())
+ for k in ts.keys():
+ ts[k] = int(ts[k] / tot_time * 100)
+ # print(ts)
+ return kernel_points
+
+def extract_cluster(cl, cl_list):
+ if cl_list is None:
+ return None
+ for cl_i in cl_list:
+ if cl.name == cl_i.name:
+ return cl_i
+ return None
+
+# Input: Convex obstacles, excluding points x and xg, kernel width epsilon
+def cluster_and_starify(obstacles, x, xg, epsilon, workspace=None, max_compute_time=np.inf, previous_clusters=None,
+ make_convex=False, exclude_obstacles=False, max_iterations=np.inf, verbose=False,
+ timing_verbose=False, return_history=False):
+ t0 = tic()
+
+ if workspace is None:
+ workspace = Ellipse([1e10, 1e10])
+
+ # Exit flags
+ INCOMPLETE = 0
+ COMPLETE = 1
+ MAX_COMPUTE_TIME_CONVEX_HULL = 2
+
+ # Variable initialization
+ kernel_time, hull_time, cluster_time, convex_time = [0.], [0.], [0.], 0.
+ adm_ker_robot_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_goal_cones = {obs.id(): None for obs in obstacles}
+ adm_ker_obstacles = {obs.id(): {} for obs in obstacles}
+ cluster_iterations = []
+ n_iter = 0
+
+ def default_result():
+ if return_history:
+ cl_history = cluster_iterations if n_iter > 0 else [ObstacleCluster([o]) for o in obstacles]
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter, cl_history
+ else:
+ return [ObstacleCluster([o]) for o in obstacles], [0.] * 4, INCOMPLETE, n_iter
+
+ # Compute admissible kernel for all obstacles
+ for o in obstacles:
+ if max_compute_time < toc(t0):
+ return default_result()
+ # Find admissible kernel
+ adm_ker_robot_cones[o.id()] = sh.admissible_kernel(o, x)
+ adm_ker_goal_cones[o.id()] = sh.admissible_kernel(o, xg)
+
+ if adm_ker_robot_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Robot position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ if adm_ker_goal_cones[o.id()] is None:
+ if verbose:
+ print("[Cluster and Starify]: Goal position is not a free exterior point of obstacle " + str(o.id()))
+ return default_result()
+
+ # Admissible kernel when excluding points of other obstacles
+ if exclude_obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id():
+ continue
+ o_x_exclude = o_ex.extreme_points()
+ if not all([o.exterior_point(x_ex) for x_ex in o_x_exclude]):
+ adm_ker_obstacles[o.id()][o_ex.id()] = None
+ continue
+ adm_ker_obstacles[o.id()][o_ex.id()] = sh.admissible_kernel(o, o_x_exclude[0]).polygon()
+ for v in o_x_exclude[1:]:
+ adm_ker_obstacles[o.id()][o_ex.id()] = adm_ker_obstacles[o.id()][o_ex.id()].intersection(
+ sh.admissible_kernel(o, v).polygon())
+ init_kernel_time = toc(t0)
+
+ # -- First iteration -- #
+
+ ker_sel_time, hull_compute_time, star_obj_time = [0], [0], [0]
+ # Initialize clusters as single obstacles
+ clusters = []
+ for o in obstacles:
+ # Ensure not xr in l(x,xg)
+ # while is_collinear(x, o.xr(), xg):
+ # o.set_xr(o.xr(output_frame=Frame.OBSTACLE) + np.random.normal(0, 0.01, 2))
+ cl = ObstacleCluster([o])
+
+ # New---
+ t1 = tic()
+ cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ kernel_time[0] += toc(t1)
+
+ t1 = tic()
+ cl_prev = extract_cluster(cl, previous_clusters)
+ # if cl_prev is None:
+ # cl_prev.kernel_points = equilateral_triangle(o.xr(), epsilon)
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[0] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[0] += toc(t1)
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ if cluster_hull_extensions is None:
+ cl.cluster_obstacle = o
+ cl.cluster_obstacle.set_xr(k_centroid, input_frame=Frame.GLOBAL)
+ else:
+ # Non-starshaped polygons are included in the cluster hull
+ # cl_obstacles = [o] if o.is_starshaped() else []
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, cluster_hull_extensions, xr=k_centroid,
+ id=cl_id)
+ star_obj_time[0] += toc(t1)
+ # cl.polygon()
+ # if o.is_starshaped and o.kernel().contains(shapely.geometry.Point(k_centroid)):
+ # cl.cluster_obstacle
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+
+
+ clusters += [cl]
+
+
+ # if not o.is_starshaped():
+ # cl = clusters[-1]
+ # t1 = tic()
+ # cl.admissible_kernel = adm_ker_robot_cones[o.id()].intersection(adm_ker_goal_cones[o.id()])
+ # kernel_time[0] += toc(t1)
+ #
+ # t1 = tic()
+ # cl_prev = None
+ # if previous_clusters:
+ # for p_cl in previous_clusters:
+ # if cl.name == p_cl.name:
+ # cl_prev = p_cl
+ # cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ # # -- Compute starshaped hull of cluster
+ # k_centroid = np.mean(cl.kernel_points, axis=0)
+ # cl._polygon = sh.kernel_starshaped_hull(o, cl.kernel_points)
+ # cl.cluster_obstacle = StarshapedPolygon(cl._polygon, xr=k_centroid, id=o.id())
+ # hull_time[0] += toc(t1)
+ hull_time[0] = ker_sel_time[0] + hull_compute_time[0] + star_obj_time[0]
+
+ # Set cluster history
+ cluster_history = {cl.name: cl for cl in clusters}
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # -- Cluster obstacles such that no intersection exists
+ t1 = tic()
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[0] = toc(t1)
+
+ n_iter = 1
+ # -- End first iteration -- #
+
+ while intersection_exists:
+ kernel_time += [0.]
+ hull_time += [0.]
+ cluster_time += [0.]
+ ker_sel_time += [0.]
+ hull_compute_time += [0.]
+ star_obj_time += [0.]
+
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time.")
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Find starshaped obstacle representation for each cluster
+ for i, cl in enumerate(clusters):
+ # If cluster already calculated keep it
+ if cl.name in cluster_history:
+ clusters[i] = cluster_history[cl.name]
+ continue
+
+ # ----------- Admissible Kernel ----------- #
+ t1 = tic()
+
+ # If cluster is two convex obstacles
+ # if len(cl.obstacles) == 2 and cl.obstacles[0].is_convex() and cl.obstacles[1].is_convex():
+ # cl.admissible_kernel = cl.obstacles[0].polygon().intersection(cl.obstacles[1].polygon()).buffer(0.01)
+ # else:
+ adm_ker_robot = Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ adm_ker_goal = Cone.list_intersection([adm_ker_goal_cones[o.id()] for o in cl.obstacles], same_apex=True)
+ cl.admissible_kernel = adm_ker_robot.intersection(adm_ker_goal)
+
+ if cl.admissible_kernel.is_empty or cl.admissible_kernel.area < 1e-6:
+ if verbose:
+ print("[Cluster and Starify]: Could not find disjoint starshaped obstacles. Admissible kernel empty for the cluster " + cl.name)
+ cluster_iterations += [clusters]
+ return default_result()
+
+ # Exclude other obstacles
+ adm_ker_o_ex = cl.admissible_kernel
+ if exclude_obstacles:
+ for o in cl.obstacles:
+ for o_ex in obstacles:
+ if o_ex.id() == o.id() or adm_ker_obstacles[o.id()][o_ex.id()] is None:
+ continue
+ adm_ker_o_ex = adm_ker_o_ex.intersection(adm_ker_obstacles[o.id()][o_ex.id()])
+ if not (adm_ker_o_ex.is_empty or adm_ker_o_ex.area < 1e-6):
+ cl.admissible_kernel = adm_ker_o_ex
+
+ kernel_time[n_iter] += toc(t1)
+ # ----------- End Admissible Kernel ----------- #
+
+ # ----------- Starshaped Hull ----------- #
+ t1 = tic()
+ # Check if cluster exist in previous cluster
+ cl_prev = None
+ if previous_clusters:
+ for p_cl in previous_clusters:
+ if cl.name == p_cl.name:
+ cl_prev = p_cl
+
+ # -- Kernel points selection
+ cl.kernel_points = compute_kernel_points(cl, x, xg, epsilon, cl_prev, workspace=workspace)
+ ker_sel_time[n_iter] += toc(t1)
+ t1 = tic()
+
+ # -- Compute starshaped hull of cluster
+ cl_id = "new" if cl_prev is None else cl_prev.cluster_obstacle.id()
+ cluster_hull_extensions = sh.kernel_starshaped_hull(cl.obstacles, cl.kernel_points)
+ hull_compute_time[n_iter] += toc(t1)
+
+ t1 = tic()
+ k_centroid = np.mean(cl.kernel_points, axis=0)
+ # Non-starshaped polygons are included in the cluster hull
+ cl_obstacles = [o for o in cl.obstacles if o.is_starshaped()]
+ cl.cluster_obstacle = StarshapedPrimitiveCombination(cl_obstacles, cluster_hull_extensions, xr=k_centroid, id=cl_id)
+ cl.polygon()
+
+ star_obj_time[n_iter] += toc(t1)
+ # ----------- End Starshaped Hull ----------- #
+
+ # -- Add cluster to history
+ cluster_history[cl.name] = cl
+
+ hull_time[n_iter] = ker_sel_time[n_iter] + hull_compute_time[n_iter] + star_obj_time[n_iter]
+
+ if return_history:
+ cluster_iterations += [clusters]
+
+ # ----------- Clustering ----------- #
+ t1 = tic()
+ # -- Cluster star obstacles such that no intersection exists
+ clusters, intersection_exists = get_intersection_clusters(clusters)
+ cluster_time[n_iter] = toc(t1)
+ # ----------- End Clustering ----------- #
+
+ n_iter += 1
+
+ if n_iter >= max_iterations:
+ break
+
+ # ----------- Make Convex ----------- #
+
+ if make_convex:
+ # Make convex if no intersection occurs
+ t1 = tic()
+ for j, cl in enumerate(clusters):
+ if not cl.cluster_obstacle.is_convex():
+ # Check compute time
+ if max_compute_time < toc(t0):
+ if verbose:
+ print("[Cluster and Starify]: Max compute time in convex hull.")
+ return clusters, [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), toc(t1)], MAX_COMPUTE_TIME_CONVEX_HULL, n_iter
+
+ v = np.array(cl.polygon().exterior.coords)[:-1, :]
+ hull = ConvexHull(v)
+ hull_polygon = shapely.geometry.Polygon(v[hull.vertices, :])
+ if not any([hull_polygon.contains(shapely.geometry.Point(x_ex)) for x_ex in [x, xg]]) and not any(
+ [hull_polygon.intersects(clusters[k].polygon()) for k in range(len(clusters)) if k != j]):
+ # clusters[j].cluster_obstacle = StarshapedPrimitiveCombination(cl.obstacles, hull_polygon, cl.cluster_obstacle.xr(Frame.GLOBAL))
+ clusters[j].cluster_obstacle = StarshapedPolygon(hull_polygon, xr=cl.cluster_obstacle.xr(Frame.GLOBAL))
+
+ convex_time = toc(t1)
+ # ----------- End Make Convex ----------- #
+
+ timing_vec = [sum(cluster_time), init_kernel_time+sum(kernel_time), sum(hull_time), convex_time]
+ if timing_verbose:
+
+ print("------------\nTotal timing (Cluster/AdmKer/StHull/ConvHull): {:.1f} [{:.1f}, {:.1f}, {:.1f}, {:.1f}]".format(sum(timing_vec), *timing_vec))
+ print("Init kernel timing: {:.1f}".format(init_kernel_time))
+ for i in range(n_iter):
+ print("Iteration {} timing (Cluster/AdmKer/StHull): [{:.1f}, {:.1f}, {:.1f}]".format(i, cluster_time[i], kernel_time[i], hull_time[i]))
+ print("\t Hull timing divide: (KerSel/Hull/Obj) calculation [{:.1f}, {:.1f}, {:.1f}]".format(ker_sel_time[i], hull_compute_time[i], star_obj_time[i]))
+
+ if return_history:
+ return clusters, timing_vec, COMPLETE, n_iter, cluster_iterations
+ else:
+ return clusters, timing_vec, COMPLETE, n_iter
+
+
+def draw_clustering(clusters, p, pg, xlim=None, ylim=None):
+ color = plt.cm.rainbow(np.linspace(0, 1, len(clusters)))
+ fig, ax = plt.subplots()
+ for i, cl in enumerate(clusters):
+ [o.draw(ax=ax, fc=color[i], show_reference=False, ec='k', alpha=0.8) for o in cl.obstacles]
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_star_hull(clusters, p, pg, xlim=None, ylim=None):
+ fig, ax = plt.subplots()
+ for cl in clusters:
+ if cl.cluster_obstacle is not None:
+ cl.cluster_obstacle.draw(ax=ax, fc='g', alpha=0.8)
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='lightgrey', ec='k')
+ else:
+ draw_shapely_polygon(cl.polygon_excluding_hull(), ax=ax, fc='r', ec='k')
+ ax.plot(*p, 'ko')
+ ax.plot(*pg, 'k*')
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return fig, ax
+
+
+def draw_adm_ker(clusters, p, pg, xlim=None, ylim=None):
+ valid_adm_ker_cls = [cl for cl in clusters if not (cl.admissible_kernel is None or cl.admissible_kernel.is_empty)]
+ n_col = int(np.sqrt(len(valid_adm_ker_cls))) + 1
+ fig, axs = plt.subplots(n_col, n_col)
+ for i, cl in enumerate(valid_adm_ker_cls):
+ ax_i = axs[i//n_col, i%n_col]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in cl.obstacles]
+ if not cl.admissible_kernel.geom_type == 'Point':
+ draw_shapely_polygon(cl.admissible_kernel, ax=ax_i, fc='y', alpha=0.3)
+ if cl.kernel_points is not None:
+ draw_shapely_polygon(shapely.geometry.Polygon(cl.kernel_points), ax=ax_i, fc='g', alpha=0.6)
+ ax_i.plot(*p, 'ko')
+ ax_i.plot(*pg, 'k*')
+ if xlim is not None:
+ ax_i.set_xlim(xlim)
+ if ylim is not None:
+ ax_i.set_ylim(ylim)
+ return fig, axs
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
new file mode 100644
index 0000000..8fde482
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starshaped_hull/starshaped_hull.py
@@ -0,0 +1,207 @@
+import shapely
+import numpy as np
+from obstacles import Polygon
+from utils import is_ccw, is_cw, line, ray, Cone, convex_hull
+import matplotlib.pyplot as plt
+
+
+def admissible_kernel(obstacle, x):
+ # Find tangents of obstacle through x
+ tps = obstacle.tangent_points(x)
+ if not tps:
+ # Interior point
+ return None
+ return Cone(x, x-tps[0], x-tps[1])
+
+
+# Computes the starshaped hull of a list of obstacles for specified kernel points
+def kernel_starshaped_hull(obstacles, kernel_points):
+ if not type(obstacles) is list:
+ if obstacles.is_convex():
+ return convex_kernel_starshaped_hull(obstacles, kernel_points)
+ if issubclass(obstacles.__class__, Polygon):
+ return polygon_kernel_starshaped_hull(obstacles.polygon(), kernel_points)
+ else:
+ print("[kernel_starshaped_hull]: Bad obstacle class.")
+ print(obstacles)
+
+ sub_pols = [kernel_starshaped_hull(o, kernel_points) for o in obstacles]
+ hull_polygon = shapely.ops.unary_union(sub_pols)
+ if hull_polygon.is_empty:
+ return None
+ return hull_polygon
+
+
+def convex_kernel_starshaped_hull(convex_obstacle, kernel_points):
+ tps = []
+ for k in kernel_points:
+ tps += convex_obstacle.tangent_points(k)
+ if not tps:
+ return shapely.geometry.Polygon([])
+
+ tps = np.unique(tps,axis=0)
+ ch_points = np.vstack((tps, kernel_points))
+ pol = convex_hull(ch_points)
+ return shapely.geometry.Polygon(pol)
+
+
+# TODO: Improve computational consideration
+def polygon_kernel_starshaped_hull(polygon, kernel_points, debug=0):
+ kernel_points = kernel_points.reshape((kernel_points.size//2, 2))
+
+ if kernel_points.shape[0] > 2:
+ # NOTE: Assumes kernel points convex
+ # convex_kernel_subset = shapely.geometry.Polygon(kernel_points[ConvexHull(kernel_points).vertices, :])
+ convex_kernel_subset = shapely.geometry.Polygon(kernel_points)
+
+ # polygon_sh = polygon.polygon() # Shapely represenation of polygon
+ # vertices = np.asarray(polygon_sh.exterior.coords)[:-1, :] # Vertices of polygon
+ vertices = np.asarray(polygon.exterior.coords)[:-1, :] # Vertices of polygon
+ star_vertices = [] # Vertices of starshaped hull polygon
+ v_bar = kernel_points[0].copy() # Last vertex of starshaped hull polygon
+ e1_idx = 0
+ e2_idx = 0
+ k_centroid = np.mean(kernel_points, axis=0)
+ k_included = [False] * kernel_points.shape[0]
+
+ # Arrange vertices such that v_1 is the one with largest x-value and vendv1v2 is CCW, (assumes no collinear vertices in P)
+ start_idx = np.argmax(vertices[:, 0])
+ vertices = np.roll(vertices, -start_idx, axis=0)
+ if is_cw(vertices[-1], vertices[0], vertices[1]):
+ vertices = np.flip(vertices, axis=0)
+ vertices = np.roll(vertices, 1, axis=0)
+ # print("Initial sort: {:.1f}".format(toc(t0)))
+
+ # Iterate through all vertices
+ for v_idx, v in enumerate(vertices):
+ adjust_e1 = False
+ # Check if no ray r(v,kv) intersects with interior of polygon
+ if all([ray(v, k, v).disjoint(polygon) for k in kernel_points]):
+ # Add current vertex
+ if kernel_points.shape[0] < 3 or not convex_kernel_subset.contains(shapely.geometry.Point(v)):
+ star_vertices += [v]
+ if star_vertices:
+ # Intersections of lines l(k,v) and l(e1,e2)
+ e1, e2 = star_vertices[e1_idx], star_vertices[e2_idx]
+ e1_e2 = line(e1, e2)
+
+ for k in kernel_points:
+ kv_e1e2_intersect = line(k,v).intersection(e1_e2)
+ # Adjust to closest intersection to e2
+ if not kv_e1e2_intersect.is_empty:
+ adjust_e1 = True
+ e1_candidate = np.array([kv_e1e2_intersect.x, kv_e1e2_intersect.y])
+ if np.linalg.norm(e2 - e1_candidate) < np.linalg.norm(e2 - star_vertices[e1_idx]):
+ star_vertices[e1_idx] = e1_candidate
+
+ if not adjust_e1:
+ for k_idx, k in enumerate(kernel_points):
+ kps = [kp for kp in kernel_points if not np.array_equal(kp, k)]
+ kv_P_intersect = line(k, v).intersection(polygon)
+
+ # If l(k,v) intersects interior of P
+ if not kv_P_intersect.is_empty:
+ # Find last intersection of l(k,v) and polygon boundary
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for ip in kv_P_intersect.coords]
+ elif kv_P_intersect.geom_type == 'MultiLineString':
+ intersection_points = [np.array([ip[0], ip[1]]) for l in kv_P_intersect.geoms for ip in
+ l.coords]
+ elif kv_P_intersect.geom_type == 'GeometryCollection':
+ intersection_points = []
+ for g in kv_P_intersect.geoms:
+ if g.geom_type == 'Point':
+ intersection_points += [np.array(g.coords[0])]
+ if kv_P_intersect.geom_type == 'LineString':
+ intersection_points += [np.array([ip[0], ip[1]]) for ip in g.coords]
+ else:
+ intersection_points = []
+
+ u = None
+ u_v = None
+ for u_candidate in intersection_points:
+ u_v = line(u_candidate, v)
+ if u_v.disjoint(polygon):
+ u = u_candidate
+ break
+ if u is None:
+ continue
+
+ # If no ray r(u,k'v) intersect with interior of polygon
+ if not any([ray(u, kp, v).intersects(polygon) for kp in kps]):
+ # Adjust u if l(k',v_bar) intersects l(u,v)
+ for kp in kps:
+ kpvb_uv_intersect = line(kp, v_bar).intersection(u_v)
+ if not kpvb_uv_intersect.is_empty:
+ u = np.array([kpvb_uv_intersect.x, kpvb_uv_intersect.y])
+ # Append u to P*
+ star_vertices += [u]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(u, v, vertices[v_idx-1]):
+ # if is_ccw(v_bar, v, u):
+ # if is_cw(k_centroid, v, u):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ else:
+ # Check if no ray r(k,k'v) intersect with interior of polygon
+ if (not k_included[k_idx]) and (not any([ray(k, kp, v).intersects(polygon) for kp in kps])):
+ k_included[k_idx] = True
+ # Append k to P*
+ star_vertices += [k]
+ # Update last augmented edge
+ e1_idx, e2_idx = len(star_vertices)-1, len(star_vertices)-2
+ # Swap last vertices if not CCW
+ if is_ccw(k, v, vertices[v_idx-1]):
+ # if is_cw(k_centroid, v, k):
+ star_vertices[-2], star_vertices[-1] = star_vertices[-1], star_vertices[-2]
+ e1_idx, e2_idx = e2_idx, e1_idx
+ adjust_e1 = True
+ # Update v_bar
+ v_bar = star_vertices[-1]
+
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*k, 'kx') for k in kernel_points]
+ [plt.plot(*line(k,v).xy, 'k--') for k in kernel_points]
+ if adjust_e1:
+ plt.plot(*star_vertices[e1_idx], 'ys')
+ plt.show()
+
+ # Check not added kernel points if they should be included
+ for j in range(len(star_vertices)):
+ v, vp = star_vertices[j - 1], star_vertices[j]
+ for k_idx, k in enumerate(kernel_points):
+ if (not k_included[k_idx]) and is_cw(k, v, vp):
+ k_included[k_idx] = True
+ # Insert k
+ star_vertices = star_vertices[:j] + [k] + star_vertices[j:]
+ # Visualize debug information
+ if debug == 1:
+ plt.plot(*k_centroid, 'ko')
+ plt.plot(*polygon.exterior.xy, 'k')
+ plt.plot([p[0] for p in star_vertices], [p[1] for p in star_vertices], 'g-o', linewidth=2)
+ [plt.plot(*ki, 'kx') for ki in kernel_points]
+ plt.plot(*line(k, v).xy, 'r--*')
+ plt.plot(*line(k, vp).xy, 'r--*')
+ plt.plot(*k, 'go')
+ plt.show()
+ # print("Final kernel check: {:.1f}".format(toc(t0)))
+
+ if debug:
+ ax = plt.gca()
+ ax.plot(*polygon.exterior.xy, 'k')
+ ax.plot([p[0] for p in star_vertices] + [star_vertices[0][0]],
+ [p[1] for p in star_vertices] + [star_vertices[0][1]], 'g-o', linewidth=2)
+ # [ax.plot(star_vertices[i][0], star_vertices[i][1], 'r*') for i in augmented_vertex_idcs]
+ [ax.plot(*zip(k, sv), 'y--') for sv in star_vertices for k in kernel_points]
+ ax.plot(*k_centroid, 'bs')
+ plt.show()
+
+ return shapely.geometry.Polygon(star_vertices)
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
new file mode 100644
index 0000000..4dee13f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/PKG-INFO
@@ -0,0 +1,9 @@
+Metadata-Version: 2.1
+Name: starworlds
+Version: 1.0
+License-File: LICENSE
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
new file mode 100644
index 0000000..bad8983
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/SOURCES.txt
@@ -0,0 +1,28 @@
+LICENSE
+README.md
+requirements.txt
+setup.py
+obstacles/__init__.py
+obstacles/ellipse.py
+obstacles/motion_model.py
+obstacles/obstacle.py
+obstacles/polygon.py
+obstacles/starshaped_obstacle.py
+obstacles/starshaped_polygon.py
+obstacles/starshaped_primitive_combination.py
+starshaped_hull/__init__.py
+starshaped_hull/cluster_and_starify.py
+starshaped_hull/starshaped_hull.py
+starworlds.egg-info/PKG-INFO
+starworlds.egg-info/SOURCES.txt
+starworlds.egg-info/dependency_links.txt
+starworlds.egg-info/requires.txt
+starworlds.egg-info/top_level.txt
+tests/test_cluster_and_starify.py
+tests/test_obstacles.py
+tests/test_starshaped_hull.py
+tests/test_timing.py
+tests/test_utils.py
+utils/__init__.py
+utils/cg.py
+utils/misc.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
new file mode 100644
index 0000000..f849890
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/requires.txt
@@ -0,0 +1,5 @@
+pyyaml
+numpy
+scipy
+matplotlib
+shapely
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
new file mode 100644
index 0000000..35cb7de
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/starworlds.egg-info/top_level.txt
@@ -0,0 +1,3 @@
+obstacles
+starshaped_hull
+utils
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
new file mode 100644
index 0000000..0461e20
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_cluster_and_starify.py
@@ -0,0 +1,129 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify():
+ n_obstacles = 40
+ ellipse_fraction = .5
+ ell_radius_mean, ell_radius_std = 1, 0.2
+ n_vertices, pol_box = 8, [2, 2]
+ target_scene_coverage = 0.3
+ epsilon = 0.2
+
+ np.random.seed(0)
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ # Generate obstacles
+ Nell = int(n_obstacles * ellipse_fraction)
+ Npol = n_obstacles - Nell
+ obstacles = [Ellipse(a=np.random.normal(ell_radius_mean, ell_radius_std, 2), n_pol=10) for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), xr=[0, 0], is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ obstacle_area = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ scene_width = np.sqrt(obstacle_area / target_scene_coverage * 0.9)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(random_scene_point(scene_width - 2 * ell_radius_mean) + ell_radius_mean))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(mm.Static(random_scene_point(scene_width - pol_box[0]) + pol_box[0] / 2))
+ [obs.polygon() for obs in obstacles]
+
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(scene_width, obstacles)
+ # Cluster and starify
+ clusters, timing, flag, n_iter, cluster_history = cluster_and_starify(obstacles, x, xg, epsilon, make_convex=0,
+ verbose=1, return_history=1, timing_verbose=1)
+
+ # Draw iteration steps
+ for i, clusters_i in enumerate(cluster_history[1:]):
+ _, ax = draw_clustering(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Clustering, Iteration {}/{}".format(i+1, len(cluster_history)))
+ fig, axs = draw_adm_ker(clusters_i, x, xg, xlim=[-0.2*scene_width, 1.2*scene_width], ylim=[-0.2*scene_width, 1.2*scene_width])
+ fig.suptitle("Admissible Kernel, Iteration {}/{}".format(i+1, len(cluster_history)))
+ _, ax = draw_star_hull(clusters_i, x, xg, xlim=[0, scene_width], ylim=[0, scene_width])
+ ax.set_title("Starshaped Hull, Iteration {}/{}".format(i+1, len(cluster_history)))
+
+
+def test_moving_cluster():
+ obstacles = [
+ Ellipse([1, 1], motion_model=mm.Static([-1, 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([0., 0.3])),
+ Ellipse([1, 1], motion_model=mm.Static([1, 0.3]))
+ ]
+ p0 = np.array([0.1, -2.5])
+ pg = np.array([0.1, 2.5])
+ epsilon = 0.2
+ xlim = [-3, 3]
+ ylim = [-3, 3]
+
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon)
+
+ fig, axs = plt.subplots(1, 3)
+ [o.draw(ax=axs[0], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[0].plot(*p0, 'ko')
+ axs[0].plot(*pg, 'k*')
+ axs[0].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[0].plot(*zip(p0, pg), '--')
+ axs[0].set_xlim(xlim)
+ axs[0].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -0.3]))
+ obstacles[1].set_motion_model(mm.Static([0., -0.3]))
+ obstacles[2].set_motion_model(mm.Static([1, -0.3]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ [o.draw(ax=axs[1], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ axs[1].plot(*p0, 'ko')
+ axs[1].plot(*pg, 'k*')
+ axs[1].plot(*xr_prev, 'sk')
+ axs[1].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[1].plot(*zip(p0, pg), '--')
+ axs[1].set_xlim(xlim)
+ axs[1].set_ylim(ylim)
+
+ xr_prev = clusters[0].cluster_obstacle.xr()
+ obstacles[0].set_motion_model(mm.Static([-1, -1]))
+ obstacles[1].set_motion_model(mm.Static([0., -1]))
+ obstacles[2].set_motion_model(mm.Static([1, -1]))
+ clusters, timing, flag, n_iter = cluster_and_starify(obstacles, p0, pg, epsilon, previous_clusters=clusters)
+
+ x_xg_line = shapely.geometry.LineString([p0, pg])
+ kernel_selection_set = shapely.ops.split(clusters[0].cluster_obstacle.polygon(), x_xg_line).geoms[0]
+
+ [o.draw(ax=axs[2], show_reference=0, fc='lightgrey', ec='k', alpha=0.8) for o in obstacles]
+ draw_shapely_polygon(kernel_selection_set, ax=axs[2], hatch='///', fill=False, linestyle='None')
+ axs[2].plot(*p0, 'ko')
+ axs[2].plot(*pg, 'k*')
+ axs[2].plot(*xr_prev, 'sk')
+ axs[2].plot(*clusters[0].cluster_obstacle.xr(), 'gd')
+ axs[2].plot(*zip(p0, pg), '--')
+ axs[2].set_xlim(xlim)
+ axs[2].set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify()
+ # test_moving_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py
new file mode 100644
index 0000000..2839f40
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_obstacles.py
@@ -0,0 +1,147 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, Polygon, StarshapedPolygon, StarshapedPrimitiveCombination, Frame
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon, draw_shapely_polygon, generate_star_polygon
+import shapely.geometry
+
+
+def test_ellipse():
+ ell_axes = [2, 1]
+ ell_pos = [0, 0.5]
+ xlim = [ell_pos[0] - 2 * ell_axes[0], ell_pos[0] + 2 * ell_axes[0]]
+ ylim = [ell_pos[1] - 2 * ell_axes[1], ell_pos[1] + 2 * ell_axes[1]]
+ ell = Ellipse(ell_axes, xr=[0, .9], motion_model=mm.Static(ell_pos, 1))
+ while True:
+ x = np.array([np.random.uniform(*xlim),np.random.uniform(*ylim)])
+ if ell.exterior_point(x):
+ break
+ b = ell.boundary_mapping(x)
+ n = ell.normal(x)
+ tp = ell.tangent_points(x)
+ dir = ell.reference_direction(x)
+
+ _, ax = ell.draw()
+ ax.plot(*zip(ell.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*ell.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+def test_nonstar_polygon():
+ pass
+
+
+def test_star_polygon():
+ avg_radius = 1
+ xlim = [-2*avg_radius, 2*avg_radius]
+ ylim = xlim
+ pol = StarshapedPolygon(generate_star_polygon([0, 0], avg_radius, irregularity=0.3, spikiness=0.5, num_vertices=10))
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if pol.exterior_point(x):
+ break
+ b = pol.boundary_mapping(x)
+ n = pol.normal(x)
+ tp = pol.tangent_points(x)
+ dir = pol.reference_direction(x)
+
+ _, ax = pol.draw()
+ ax.plot(*zip(pol.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*pol.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = pol.xr() + 100*np.array([np.cos(i), np.sin(i)])
+ b = pol.boundary_mapping(x)
+ n = pol.normal(b)
+ ax.quiver(*b, *n)
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ print("es")
+
+def test_star_primitive_combination():
+ n_ellipses = 3
+ n_polygons = 2
+ n_vertices = 6
+ box_width = 2
+
+ ell_axes = [0.7, 0.4]
+ pol_box = [box_width, box_width]
+
+ xlim = [-box_width, box_width]
+ ylim = [-box_width, box_width]
+ ellipses = [Ellipse(ell_axes) for i in range(n_ellipses)]
+ polygons = [StarshapedPolygon(generate_convex_polygon(n_vertices, pol_box), is_convex=True)
+ for i in range(n_polygons)]
+ obstacles = ellipses + polygons
+ while True:
+ # Generate new positions
+ for obs in obstacles:
+ obs.set_motion_model(mm.Static(np.random.uniform(-0.2*box_width, 0.2*box_width, 2), np.random.uniform(0, 2*np.pi)))
+
+ # Identify if all obstacle form a single cluster
+ kernel = obstacles[0].polygon()
+ for obs in obstacles[1:]:
+ kernel = kernel.intersection(obs.polygon())
+
+ if not kernel.is_empty:
+ break
+ else:
+ _, ax = plt.subplots()
+ [obs.draw(ax=ax, fc='r', alpha=0.2, ec='k') for obs in obstacles]
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ plt.show()
+
+ xr = np.array(kernel.representative_point().coords[0])
+ star_obs = StarshapedPrimitiveCombination(obstacles, shapely.geometry.Polygon([]), xr)
+
+ while True:
+ x = np.array([np.random.uniform(*xlim), np.random.uniform(*ylim)])
+ if star_obs.exterior_point(x):
+ break
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(x)
+ tp = star_obs.tangent_points(x)
+ dir = star_obs.reference_direction(x)
+
+ _, ax = star_obs.draw()
+ draw_shapely_polygon(kernel, ax=ax, fc='g')
+ ax.plot(*zip(star_obs.xr(Frame.GLOBAL), x), 'k--o')
+ if b is not None:
+ ax.plot(*b, 'y+')
+ ax.quiver(*b, *n)
+ if tp:
+ ax.plot(*zip(x, tp[0]), 'g:')
+ ax.plot(*zip(x, tp[1]), 'g:')
+ ax.quiver(*star_obs.xr(Frame.GLOBAL), *dir, color='c', zorder=3)
+
+ for i in np.linspace(0, 2 * np.pi, 100):
+ x = star_obs.xr() + np.array([np.cos(i), np.sin(i)])
+ b = star_obs.boundary_mapping(x)
+ n = star_obs.normal(b)
+ # ax.quiver(*b, *n)
+
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+
+
+if (__name__) == "__main__":
+ # test_ellipse()
+ # test_nonstar_polygon()
+ # test_star_polygon()
+ test_star_primitive_combination()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
new file mode 100644
index 0000000..670a636
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_starshaped_hull.py
@@ -0,0 +1,53 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from starshaped_hull import admissible_kernel
+from utils import draw_shapely_polygon, Cone
+from obstacles import Polygon, Ellipse
+from obstacles import motion_model as mm
+
+def test_admissible_kernel():
+ pol = Polygon([[0, 0], [1, 0], [2, 2], [2, 4], [-0.75, 1], [-1, 4], [-2, 2], [-2, 1], [-1.5, 0], [-2, -1]])
+
+ x_exclude = np.array([0, 2])
+
+ ad_ker = admissible_kernel(pol, x_exclude)
+
+ _, ax = pol.draw()
+ draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.6)
+ ax.plot(x_exclude[0], x_exclude[1], 'ro')
+ ax.set_xlim([-3, 6])
+ ax.set_ylim([-1, 5])
+
+
+def test_adm_ker_ellipse():
+ ell = Ellipse([1.08877265, 1.06673487], motion_model=mm.Static([4.61242385, 1.87941425]))
+ p = np.array([3.641344, 4.87955125])
+ pg = [0.73012219, 8.95180958]
+ adm_ker_p = admissible_kernel(ell, p)# Cone.list_intersection([adm_ker_robot_cones[o.id()] for o in cl.obstacles])
+ adm_ker_pg = admissible_kernel(ell, pg)
+ ad_ker = adm_ker_p.intersection(adm_ker_pg)
+
+ _, ax = ell.draw()
+ draw_shapely_polygon(ad_ker, ax=ax, fc='y', alpha=0.6)
+ draw_shapely_polygon(adm_ker_p.polygon(), ax=ax, fc='None', ec='k')
+ draw_shapely_polygon(adm_ker_pg.polygon(), ax=ax, fc='None', ec='k')
+ ax.plot(*p, 'rx')
+ ax.plot(*pg, 'rx')
+ ax.set_xlim(0, 10)
+ ax.set_ylim(0, 10)
+
+
+def test_kernel_starshaped_hull_single():
+ pass
+
+
+def test_kernel_starshaped_hull_cluster():
+ pass
+
+
+if (__name__) == "__main__":
+ test_admissible_kernel()
+ test_adm_ker_ellipse()
+ test_kernel_starshaped_hull_single()
+ test_kernel_starshaped_hull_cluster()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py
new file mode 100644
index 0000000..068ff3b
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_timing.py
@@ -0,0 +1,173 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from obstacles import Ellipse, StarshapedPolygon
+from obstacles import motion_model as mm
+from utils import generate_convex_polygon
+from starshaped_hull import cluster_and_starify, draw_clustering, draw_adm_ker, draw_star_hull
+import shapely
+
+
+def test_cluster_and_starify_compute():
+ par = {'N_samples': 1000, 'pol_Nvert': 10, 'ell_n_pol': 30, 'ell_fraction': 0.5, 'pol_box': [2, 2],
+ 'ell_radius_mean': 1., 'ell_radius_std': 0.2, 'target_scene_coverage': 0.25,
+ 'No_min': 5, 'No_max': 50, 'rd_seed': 0, 'epsilon': 0.1}
+
+ plot_fail = 0
+ # ----- Data generation ------ #
+ np.random.seed(par['rd_seed'])
+ # Result data
+ res = {'compute_time': np.zeros((par['N_samples'], 4)), 'n_iter': np.zeros(par['N_samples'], dtype=np.int32),
+ 'No': np.zeros(par['N_samples'], dtype=np.int32), 'Ncl': np.zeros(par['N_samples'], dtype=np.int32),
+ 'obstacle_area': np.zeros(par['N_samples']), 'obstacle_coverage': np.zeros(par['N_samples']),
+ 'scene_width': np.zeros(par['N_samples']), 'scene_coverage': np.zeros(par['N_samples'])}
+
+ def random_scene_point(scene_width):
+ return np.random.rand(2) * scene_width
+
+ def select_x_xg(scene_width, obstacles):
+ x = random_scene_point(scene_width)
+ while any([o.interior_point(x) for o in obstacles]):
+ x = random_scene_point(scene_width)
+ xg = random_scene_point(scene_width)
+ while any([o.interior_point(xg) for o in obstacles]):
+ xg = random_scene_point(scene_width)
+ return x, xg
+
+ for i in range(par['N_samples']):
+ # Generate obstacles
+ res['No'][i] = np.random.randint(par['No_min'], par['No_max'] + 1)
+ Nell = int(res['No'][i] * par['ell_fraction'])
+ Npol = res['No'][i] - Nell
+ obstacles = [
+ Ellipse(a=np.random.normal(par['ell_radius_mean'], par['ell_radius_std'], 2), n_pol=par['ell_n_pol'])
+ for j in range(Nell)]
+ obstacles += [StarshapedPolygon(generate_convex_polygon(par['pol_Nvert'], par['pol_box']), xr=[0, 0],
+ is_convex=True) for j in range(Npol)]
+
+ # Compute area data
+ res['obstacle_area'][i] = sum([o.area() for o in obstacles])
+
+ # Setup scene
+ res['scene_width'][i] = np.sqrt(res['obstacle_area'][i] / par['target_scene_coverage'] * 0.85)
+
+ for j in range(Nell):
+ # obstacles[j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[j].set_motion_model(mm.Static(
+ random_scene_point(res['scene_width'][i] - 2 * par['ell_radius_mean']) + par['ell_radius_mean']))
+ for j in range(Npol):
+ # obstacles[Nell+j].set_motion_model(ob.Static(random_scene_point(res['scene_width'][i])))
+ obstacles[Nell + j].set_motion_model(
+ mm.Static(random_scene_point(res['scene_width'][i] - par['pol_box'][0]) + par['pol_box'][0] / 2))
+
+ # Compute coverage data
+ res['obstacle_coverage'][i] = shapely.ops.unary_union([o.polygon() for o in obstacles]).area
+ res['scene_coverage'][i] = res['obstacle_coverage'][i] / (res['scene_width'][i] ** 2)
+
+ flag = 0
+ n_failures = -1
+ while flag == 0:
+ n_failures += 1
+ # Select collision free robot and goal positions
+ x, xg = select_x_xg(res['scene_width'][i], obstacles)
+ # Cluster and starify
+ clusters, timing, flag, res['n_iter'][i] = cluster_and_starify(obstacles, x, xg, par['epsilon'],
+ exclude_obstacles=0, make_convex=0,
+ max_iterations=100,
+ verbose=0)
+
+ star_obstacles = [cl.cluster_obstacle for cl in clusters]
+ res['compute_time'][i, :] = timing
+ res['Ncl'][i] = len(clusters)
+
+ if plot_fail and flag == 0:
+ _, ax_i = plt.subplots()
+ [o.draw(ax=ax_i, fc='g', alpha=0.8) for o in star_obstacles]
+ [o.draw(ax=ax_i, show_name=1, show_reference=0, ec='k', linestyle='--') for o in obstacles]
+ ax_i.plot(*x, 'rx', markersize=16)
+ ax_i.plot(*xg, 'rx', markersize=16)
+ ax_i.set_xlim([0, res['scene_width'][i]])
+ ax_i.set_ylim([0, res['scene_width'][i]])
+ if flag == 0:
+ ax_i.set_title(
+ 'Fail\nSample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ else:
+ ax_i.set_title(
+ 'Sample: {}, #O: {}, #Cl: {}, Time: {:.1f}, It: {}, Scene coverage: {:.2f}({:.2f})'.format(
+ i, res['No'][i], res['Ncl'][i], sum(res['compute_time'][i, :]), res['n_iter'][i],
+ res['scene_coverage'][i], par['target_scene_coverage']))
+ plt.show()
+
+ if n_failures == 5:
+ break
+
+ # ----- Postprocessing ------ #
+ total_compute_time = np.sum(res['compute_time'], axis=1)
+ binc_niter = np.bincount(res['n_iter'])[min(res['n_iter']):]
+ print(np.vstack((np.arange(min(res['n_iter']), min(res['n_iter']) + len(binc_niter)), binc_niter)))
+
+ cl = ['r', 'g', 'b', 'y', 'k', 'c']
+ mk = ['o', '+', '^', 'x', 'd', '8']
+ tct_it = [None] * len(binc_niter)
+ No_it = [None] * len(binc_niter)
+ ct_it = [[]] * len(binc_niter)
+ scene_coverage_it = [None] * len(binc_niter)
+ for i in range(len(binc_niter)):
+ scene_coverage_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['scene_coverage'])
+ tct_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, total_compute_time)
+ No_it[i] = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['No'])
+ for j in range(3):
+ ma = np.ma.masked_where(res['n_iter'] != min(res['n_iter']) + i, res['compute_time'][:, j])
+ ct_it[i] += [ma.data]
+
+ timename = ['Clustering', 'Adm Ker', 'St Hull']
+ if max(res['No']) - min(res['No']) > 0:
+ _, axs = plt.subplots(3)
+ for j in range(3):
+ for i in range(len(binc_niter)):
+ axs[j].scatter(No_it[i], ct_it[i][j], c=cl[i], marker=mk[i])
+ axs[j].set_xlabel('No'), axs[j].set_ylabel('Time [ms]')
+ axs[j].set_title(timename[j])
+ plt.tight_layout()
+
+ _, axs = plt.subplots(1, 2)
+ # ax.scatter(res['No'], total_compute_time)
+ for i in range(len(binc_niter)):
+ axs[0].scatter(No_it[i], tct_it[i], c=cl[i], marker=mk[i], s=np.square(scene_coverage_it[i] * 20))
+ axs[1].scatter(scene_coverage_it[i], tct_it[i], c=cl[i], marker=mk[i], s=No_it[i] * 2)
+
+ _, ax = plt.subplots()
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 0], total_compute_time), color='r')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 1], total_compute_time), color='g')
+ ax.scatter(res['No'], np.divide(res['compute_time'][:, 2], total_compute_time), color='b')
+ ax.set_title("Clustering fraction of total"), ax.set_xlabel('No'), ax.set_ylabel('Cl time / Tot time')
+
+ _, axs = plt.subplots(2, 2)
+ axs[0, 0].scatter(res['No'], res['obstacle_area'])
+ axs[0, 0].scatter(res['No'], res['obstacle_coverage']), axs[0, 0].set_title('Obstacle area')
+ axs[0, 0].set_xlabel('#Obstacles'), axs[1, 0].set_ylabel('Obstacle area')
+ axs[1, 0].scatter(res['scene_coverage'], res['n_iter']), axs[1, 0].set_title('#Iterations')
+ axs[1, 0].set_xlabel('Scene coverage'), axs[1, 0].set_ylabel('#Iterations')
+ axs[0, 1].scatter(res['No'], np.divide(res['Ncl'], res['No'])), axs[0, 1].set_title('#Clusters')
+ axs[0, 1].set_xlabel('#Obstacles'), axs[0, 1].set_ylabel('#Clusters/#Obstacles')
+ axs[1, 1].scatter(res['scene_coverage'], np.divide(res['Ncl'], res['No'])), axs[1, 1].set_title('#Clusters/#No')
+ axs[1, 1].set_xlabel('Scene coverage'), axs[1, 1].set_ylabel('#Clusters/#Obstacles')
+ plt.tight_layout()
+
+ scene_coverage = res['scene_coverage'] * 100
+ cov_span = max(scene_coverage) - min(scene_coverage)
+ binwidth = 1
+ _, ax = plt.subplots()
+ ax.hist(scene_coverage, bins=int(cov_span / binwidth),
+ color='blue', edgecolor='black')
+ ymin, ymax = ax.get_ylim()
+ ax.plot([par['target_scene_coverage'] * 100, par['target_scene_coverage'] * 100], [ymin, ymax], 'r--')
+ ax.set_title('Histogram with Binwidth = %d' % binwidth, size=30)
+ ax.set_xlabel('Coverage', size=22)
+ ax.set_ylabel('Samples', size=22)
+
+
+if (__name__) == "__main__":
+ test_cluster_and_starify_compute()
+ plt.show()
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py
new file mode 100644
index 0000000..54cac6a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/tests/test_utils.py
@@ -0,0 +1,106 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from utils import Cone, draw_shapely_polygon
+
+
+def test_affine_transformation():
+ pass
+
+
+def test_point_in_triangle():
+ pass
+
+
+def test_orientation():
+ pass
+
+
+def test_equilateral_triangle():
+ pass
+
+
+def test_convex_hull():
+ pass
+
+
+def test_generate_convex_polygon():
+ pass
+
+
+def test_generate_star_polygon():
+ pass
+
+
+def test_cone():
+ n_cones_sqrt = 4
+ n_cones_same_apex = 4
+ n_points = 8
+ n_several_cone_intersection = 2
+ boxlim = [-1, 1]
+
+ fig_int_sev, axs_int_sev = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_int, axs_int = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ fig_p, axs_p = plt.subplots(n_cones_sqrt, n_cones_sqrt)
+ for i in range(n_cones_sqrt ** 2):
+ # Test cone intersection
+ c_params = np.random.uniform(*boxlim, (2, 3))
+ c1 = Cone(c_params[:, 0], c_params[:, 1] - c_params[:, 0], c_params[:, 2] - c_params[:, 0])
+ c2_params = np.random.uniform(*boxlim, (2, 3))
+ c2_apex = c_params[:, 0] if i < n_cones_same_apex else c2_params[:, 0]
+ c2 = Cone(c2_apex, c2_params[:, 1] - c2_apex, c2_params[:, 2] - c2_apex)
+ c_several_intersect = [c1, c2]
+ for j in range(1, n_several_cone_intersection):
+ c3_params = np.random.uniform(*boxlim, (2, 3))
+ c3_apex = c_params[:, 0] if i < n_cones_same_apex else c3_params[:, 0]
+ c_several_intersect += [Cone(c3_apex, c3_params[:, 1] - c3_apex, c3_params[:, 2] - c3_apex)]
+
+ two_cones_intersection = c1.intersection(c2)
+ several_cones_intersection = Cone.list_intersection(c_several_intersect, same_apex=i<n_cones_same_apex)
+
+ # Two cones intersection plot
+ fig_int.suptitle("Intersection of two cones")
+ ax_int_i = axs_int[i//n_cones_sqrt, i%n_cones_sqrt]
+ c1.draw(ax=ax_int_i, color='b', alpha=0.2, ray_color='b')
+ c2.draw(ax=ax_int_i, color='r', alpha=0.2, ray_color='r')
+ if not two_cones_intersection.is_empty and not two_cones_intersection.geom_type == 'Point':
+ draw_shapely_polygon(two_cones_intersection, ax=ax_int_i, color='k', alpha=0.2, hatch='///')
+ ax_int_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Several cones intersection plot
+ fig_int_sev.suptitle("Intersection of several cones")
+ color = plt.cm.rainbow(np.linspace(0, 1, 2*n_several_cone_intersection))
+ ax_int_sev_i = axs_int_sev[i//n_cones_sqrt, i%n_cones_sqrt]
+ for j, c in enumerate(c_several_intersect):
+ c.draw(ax=ax_int_sev_i, color=color[j], alpha=0.2, ray_color=color[j])
+ if not several_cones_intersection.is_empty:
+ draw_shapely_polygon(several_cones_intersection, ax=ax_int_sev_i, color='k', alpha=0.2, hatch='///')
+ ax_int_sev_i.set_xlim(1.2 * np.array(boxlim))
+ ax_int_sev_i.set_ylim(1.2 * np.array(boxlim))
+
+ # Test point in cone
+ fig_p.suptitle("Point in cone")
+ ax_p_i = axs_p[i//n_cones_sqrt, i%n_cones_sqrt]
+ xs = np.linspace(*boxlim, n_points)
+ ys = np.linspace(*boxlim, n_points)
+ XS, YS = np.meshgrid(xs, ys)
+ c1.draw(ax=ax_p_i, color='b', alpha=0.2, ray_color='b')
+ for j in range(n_points):
+ for k in range(n_points):
+ col = 'g' if c1.point_in_cone([XS[j, k], YS[j, k]]) else 'r'
+ ax_p_i.plot(XS[j, k], YS[j, k], marker='.', color=col)
+ ax_p_i.set_xlim(1.2 * np.array(boxlim))
+ ax_p_i.set_ylim(1.2 * np.array(boxlim))
+
+
+if (__name__) == "__main__":
+ test_convex_hull()
+ test_orientation()
+ test_affine_transformation()
+ test_equilateral_triangle()
+ test_orientation()
+ test_generate_convex_polygon()
+ test_generate_star_polygon()
+ test_point_in_triangle()
+ test_cone()
+ plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py
new file mode 100644
index 0000000..428e2f7
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/__init__.py
@@ -0,0 +1,2 @@
+from .misc import *
+from .cg import *
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py
new file mode 100644
index 0000000..bb7525d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/cg.py
@@ -0,0 +1,593 @@
+import numpy as np
+import shapely.geometry
+import shapely.ops
+import matplotlib.pyplot as plt
+from typing import List, Tuple
+from utils import draw_shapely_polygon
+
+DEFAULT_RAY_INFINITY_LENGTH = 100000.
+COLLINEAR_THRESHOLD = 1e-10
+
+
+def affine_transform(x, rotation, translation, inverse=False):
+ if inverse:
+ x_t = [x[0] - translation[0], x[1] - translation[1]]
+ if rotation == 0:
+ return np.array(x_t)
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x_t[0] + s*x_t[1], -s*x_t[0] + c*x_t[1]])
+ else:
+ if rotation != 0:
+ c, s = np.cos(rotation), np.sin(rotation)
+ return np.array([c*x[0]-s*x[1]+translation[0], s*x[0]+c*x[1]+translation[1]])
+ else:
+ return np.array([x[0]+translation[0], x[1]+translation[1]])
+
+
+# Line segment from a to b, excluding a and b
+def line(a, b):
+ return shapely.geometry.LineString([a + .0001 * (b - a), a + .9999 * (b - a)])
+
+
+# Random point on the triangle with vertices a, b and c
+def point_in_triangle(a, b, c):
+ """
+ """
+ x, y = np.random.rand(2)
+ q = abs(x - y)
+ s, t, u = q, 0.5 * (x + y - q), 1 - 0.5 * (q + x + y)
+ return s * a[0] + t * b[0] + u * c[0], s * a[1] + t * b[1] + u * c[1]
+
+# TODO: Dynamic length of ray
+# Ray emanating from a in direction of b->c
+def ray(a, b, c, ray_inf_length=DEFAULT_RAY_INFINITY_LENGTH):
+ return shapely.geometry.LineString([a + .0001 * (c - b), a + ray_inf_length * (c - b)])
+
+
+def np_orientation_val(a, b, c):
+ return (b[:, 1] - a[:, 1]) * (c[:, 0] - b[:, 0]) - (b[:, 0] - a[:, 0]) * (c[:, 1] - b[:, 1])
+
+
+def orientation_val(a, b, c):
+ return (b[1] - a[1]) * (c[0] - b[0]) - (b[0] - a[0]) * (c[1] - b[1])
+
+
+def is_collinear(a, b, c):
+ return abs(orientation_val(a, b, c)) < COLLINEAR_THRESHOLD
+
+
+def is_cw(a, b, c):
+ return orientation_val(a, b, c) > COLLINEAR_THRESHOLD
+
+
+def is_ccw(a, b, c):
+ return orientation_val(a, b, c) < -COLLINEAR_THRESHOLD
+
+
+# TODO: Use [NOT is_cw and NOT is_ccw] instead?
+# Returns true if point b is between point and b
+def is_between(a, b, c):
+ return np.isclose(np.linalg.norm(a-b) + np.linalg.norm(c-b), np.linalg.norm(a-c), rtol=1e-7)
+
+
+def intersect(a1, a2, b1, b2):
+ return (is_ccw(a1, b1, b2) != is_ccw(a2, b1, b2) and is_ccw(a1, a2, b1) != is_ccw(a1, a2, b2))
+
+
+class Point:
+
+ def __init__(self, x: float, y: float):
+ self.x = x
+ self.y = y
+ self.xy = [x, y]
+
+ def __str__(self):
+ return "Point {:s}".format(str(self.xy))
+
+ def __iter__(self):
+ return iter(self.xy)
+
+ def __getitem__(self, item):
+ return self.xy[item]
+
+ def draw(self, ax=None, marker='o', **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(self.x, self.y, marker=marker, **kwargs)
+ return handles, ax
+
+
+class Line:
+
+ def __init__(self, p1: Point, p2: Point):
+ self.p1 = p1
+ self.p2 = p2
+
+ def __str__(self):
+ return "Line --({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_intersection(self, other: 'Line') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ tmp1 = self.p1.x * self.p2.y - self.p1.y * self.p2.x
+ tmp2 = other.p1.x * other.p2.y - other.p1.y * other.p2.x
+ ip_x = (tmp1 * other_dx - self_dx * tmp2) / den
+ ip_y = (tmp1 * other_dy - self_dy * tmp2) / den
+ return Point(ip_x, ip_y)
+
+ def intersects(self, other):
+ return (is_ccw(self.p1, other.p1, other.p1) != is_ccw(self.p2, other.p1, other.p1) and is_ccw(self.p1, self.p2, other.p1) != is_ccw(self.p1, self.p2, other.p2))
+
+
+class LineSegment(Line):
+
+ def __str__(self):
+ return "Line segment ({:.2f},{:.2f})--({:.2f},{:.2f})".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def line_segment_intersection(self, other: 'LineSegment') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or t > 1 or u < 0 or u > 1:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+class Ray(Line):
+
+ def __str__(self):
+ return "Ray ({:.2f},{:.2f})--({:.2f},{:.2f})--".format(self.p1.x, self.p1.y, self.p2.x, self.p2.y)
+
+ def ray_intersection(self, other: 'Ray') -> Point:
+ self_dx = self.p1.x - self.p2.x
+ other_dx = other.p1.x - other.p2.x
+ self_dy = self.p1.y - self.p2.y
+ other_dy = other.p1.y - other.p2.y
+ p1_dx = self.p1.x - other.p1.x
+ p1_dy = self.p1.y - other.p1.y
+ den = self_dx * other_dy - self_dy * other_dx
+ if abs(den) < 1e-10:
+ # Parallel or coincident lines
+ return None
+ t = (p1_dx * other_dy - p1_dy * other_dx) / den
+ u = (p1_dx * self_dy - p1_dy * self_dx) / den
+ if t < 0 or u < 0:
+ return None
+ ip_x = self.p1.x - t * self_dx
+ ip_y = self.p1.y - t * self_dy
+ return Point(ip_x, ip_y)
+
+ def draw(self, ax=None, linestyle='--', color='k', markersize=16, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ handles = ax.plot(*zip(self.p1, self.p2), linestyle=linestyle, color=color, **kwargs)
+ orient = np.rad2deg(np.arctan2(self.p2.y-self.p1.y, self.p2.x-self.p1.x))
+ handles += ax.plot(*self.p2, marker=(3, 0, orient-90), markersize=markersize, linestyle='None', color=color)
+ return handles, ax
+
+
+# Get intersection of line a and line b
+def get_intersection(a1, a2, b1, b2):
+ if not intersect(a1, a2, b1, b2):
+ if is_between(b1, a1, b2):
+ return a1
+ if is_between(b1, a2, b2):
+ return a2
+ if is_between(a1, b1, a2):
+ return b1
+ if is_between(a1, b2, a2):
+ return b2
+ return None
+ da = a2 - a1
+ db = b2 - b1
+ dp = a1 - b1
+ dap = np.array([-da[1], da[0]])
+ denom = np.dot(dap, db)
+ num = np.dot(dap, dp)
+ return (num / denom.astype(float)) * db + b1
+
+
+def equilateral_triangle(centroid, side_length, rot=0):
+ triangle = np.array(centroid) + np.array([[0, 1 / np.sqrt(3) * side_length],
+ [1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length],
+ [-1 / 2 * side_length, -1 / (2*np.sqrt(3)) * side_length]])
+ if not rot:
+ return triangle
+ c,s = np.cos(rot), np.sin(rot)
+ return np.array([[c * triangle[0, 0] - s * triangle[0, 1], s * triangle[0, 0] + c * triangle[0, 1]],
+ [c * triangle[1, 0] - s * triangle[1, 1], s * triangle[1, 0] + c * triangle[1, 1]],
+ [c * triangle[2, 0] - s * triangle[2, 1], s * triangle[2, 0] + c * triangle[2, 1]]])
+
+
+def convex_hull(points):
+ n = len(points)
+
+ # Find the leftmost point
+ l = 0
+ for i in range(1, n):
+ if points[i][0] < points[l][0]:
+ l = i
+ elif points[i][0] == points[l][0]:
+ if points[i][1] > points[l][1]:
+ l = i
+
+ hull = []
+ '''
+ Start from leftmost point, keep moving counterclockwise
+ until reach the start point again. This loop runs O(h)
+ times where h is number of points in result or output.
+ '''
+ p = l
+ while (True):
+ # Add current point to result
+ hull.append([points[p][0], points[p][1]])
+
+ '''
+ Search for a point 'q' such that orientation(p, q,
+ x) is counterclockwise for all points 'x'. The idea
+ is to keep track of last visited most counterclock-
+ wise point in q. If any point 'i' is more counterclock-
+ wise than q, then update q.
+ '''
+ q = (p + 1) % n
+
+ for i in range(n):
+
+ # If i is more counterclockwise
+ # than current q, then update q
+ if is_ccw(points[p], points[i], points[q]):
+ q = i
+ '''
+ Now q is the most counterclockwise with respect to p
+ Set p as q for next iteration, so that q is added to
+ result 'hull'
+ '''
+ p = q
+
+ # While we don't come to first point
+ if (p == l):
+ break
+
+ return hull
+
+
+class Cone:
+ bb_width = 1e6
+ bottom_right = Point(bb_width, -bb_width)
+ top_right = Point(bb_width, bb_width)
+ bottom_left = Point(-bb_width, -bb_width)
+ top_left = Point(-bb_width, bb_width)
+ right_line = Line(bottom_right, top_right)
+ top_line = Line(top_left, top_right)
+ left_line = Line(top_left, bottom_left)
+ bottom_line = Line(bottom_left, bottom_right)
+ bb_edges = [bottom_line, right_line, top_line, left_line]
+ bb_corners = [bottom_right.xy, top_right.xy, top_left.xy, bottom_left.xy]
+ bb_corner_angles = np.pi / 4 * np.array([1, 3, 5, 7])
+
+ def __init__(self, apex, dir1, dir2):
+ self.apex = np.array(apex)
+ self.dir1 = np.array(dir1)
+ self.dir2 = np.array(dir2)
+ self.is_convex = is_ccw([0, 0], self.dir1, self.dir2)
+ self.ray1 = Ray(Point(*self.apex), Point(*(self.apex+self.dir1)))
+ self.ray2 = Ray(Point(*self.apex), Point(*(self.apex+self.dir2)))
+
+ def __str__(self):
+ return "Cone: ({:s}, {:s}, {:s})".format(str(self.apex), str(self.dir1), str(self.dir2))
+
+ def polygon(self) -> shapely.geometry.Polygon:
+
+ angle1 = np.arctan2(self.dir1[1], self.dir1[0]) + np.pi
+ angle2 = np.arctan2(self.dir2[1], self.dir2[0]) + np.pi
+
+ if Cone.bb_corner_angles[0] <= angle1 < Cone.bb_corner_angles[1]:
+ ray1_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle1 < Cone.bb_corner_angles[2]:
+ ray1_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle1 < Cone.bb_corner_angles[3]:
+ ray1_intersection_idx = 2
+ else:
+ ray1_intersection_idx = 3
+
+ if Cone.bb_corner_angles[0] <= angle2 < Cone.bb_corner_angles[1]:
+ ray2_intersection_idx = 0
+ elif Cone.bb_corner_angles[1] <= angle2 < Cone.bb_corner_angles[2]:
+ ray2_intersection_idx = 1
+ elif Cone.bb_corner_angles[2] <= angle2 < Cone.bb_corner_angles[3]:
+ ray2_intersection_idx = 2
+ else:
+ ray2_intersection_idx = 3
+
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 1) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ ray1_intersection_idx = (ray1_intersection_idx + 2) % 4
+ r1_border = self.ray1.ray_intersection(Cone.bb_edges[ray1_intersection_idx])
+ if r1_border is None:
+ print("SOMETHING WRONG!")
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 1) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ ray2_intersection_idx = (ray2_intersection_idx + 2) % 4
+ r2_border = self.ray2.ray_intersection(Cone.bb_edges[ray2_intersection_idx])
+ if r2_border is None:
+ print("SOMETHING WRONG!")
+
+ vertices = [self.apex, r1_border.xy]
+ c_idx = ray1_intersection_idx
+ if not (self.is_convex and (ray1_intersection_idx == ray2_intersection_idx)):
+ while True:
+ vertices += [Cone.bb_corners[c_idx]]
+ c_idx = (c_idx + 1) % 4
+ if c_idx == ray2_intersection_idx:
+ break
+ vertices += [r2_border.xy]
+
+ return shapely.geometry.Polygon(vertices)
+
+ def point_in_cone(self, x):
+ if self.is_convex:
+ return is_ccw(self.apex, self.ray1.p2, x) and is_cw(self.apex, self.ray2.p2, x)
+ else:
+ return not (is_ccw(self.apex, self.ray2.p2, x) and is_cw(self.apex, self.ray1.p2, x))
+
+ def intersection(self, other: 'Cone', same_apex: bool=False) -> shapely.geometry.Polygon:
+ if same_apex:
+ intersection_list = self.intersection_same_apex(other)
+ if len(intersection_list) == 1:
+ return intersection_list[0].polygon()
+ else:
+ return shapely.ops.unary_union([i.polygon() for i in intersection_list])
+ else:
+ return self.polygon().intersection(other.polygon())
+
+ def intersection_same_apex(self, other: 'Cone') -> List['Cone']:
+ other_dir1_in_self = self.point_in_cone(other.apex+other.dir1)
+ other_dir2_in_self = self.point_in_cone(other.apex+other.dir2)
+ self_dir1_in_other = other.point_in_cone(self.apex+self.dir1)
+
+ if other_dir1_in_self and other_dir2_in_self:
+ # Check several cones
+ if self_dir1_in_other:
+ return [Cone(self.apex, self.dir1, other.dir2),
+ Cone(self.apex, other.dir1, self.dir2)]
+ else:
+ return [other]
+ elif other_dir1_in_self:
+ return [Cone(self.apex, other.dir1, self.dir2)]
+ elif other_dir2_in_self:
+ return [Cone(self.apex, self.dir1, other.dir2)]
+ elif self_dir1_in_other:
+ return [self]
+ else:
+ return []
+
+ @staticmethod
+ def list_intersection(cones: List['Cone'], same_apex=False) -> shapely.geometry.Polygon:
+ if not same_apex:
+ intersection = cones[0].polygon()
+ for c in cones[1:]:
+ intersection = intersection.intersection(c.polygon())
+ return intersection
+
+ # List of cones
+ cones_intersect = [cones[0]]
+ for c in cones[1:]:
+ # Find intersection of current cones and next in list
+ cones_intersect_new = []
+ for i, ci in enumerate(cones_intersect):
+ cones_intersect_new += ci.intersection_same_apex(c)
+ cones_intersect = cones_intersect_new
+
+ # If empty intersection
+ if not cones_intersect:
+ return shapely.geometry.Polygon([])
+
+ if len(cones_intersect) == 1:
+ ret = cones_intersect[0].polygon()
+ return ret
+ else:
+ return shapely.ops.unary_union([c.polygon() for c in cones_intersect])
+
+ def draw(self, ax=None, ray_color='k', **kwargs):
+ handles, ax = draw_shapely_polygon(self.polygon(), ax=ax, **kwargs)
+ if ray_color is not None:
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir1), linestyle='--', color=ray_color)
+ handles += ax.plot(*zip(self.apex, self.apex + Cone.bb_width * self.dir2), linestyle='-', color=ray_color)
+ return handles, ax
+
+
+def generate_convex_polygon(n, box=None):
+ if box is None:
+ box = [1, 1]
+
+ # Generate two lists of random X and Y coordinates
+ xPool = [box[0]*np.random.uniform() for i in range(n)]
+ yPool = [box[0]*np.random.uniform() for i in range(n)]
+
+ # Sort them
+ xPool = np.sort(xPool)
+ yPool = np.sort(yPool)
+
+ # Isolate the extreme points
+ minX = xPool[0]
+ maxX = xPool[-1]
+ minY = yPool[0]
+ maxY = yPool[-1]
+
+ # Divide the interior points into two chains & Extract the vector components
+ lastTop = minX
+ lastBot = minX
+
+ xVec, yVec = [], []
+ for i in range(1, n-1):
+ if np.random.randint(2):
+ xVec += [xPool[i] - lastTop]
+ lastTop = xPool[i]
+ else:
+ xVec += [lastBot - xPool[i]]
+ lastBot = xPool[i]
+
+ xVec += [maxX - lastTop]
+ xVec += [lastBot - maxX]
+
+ lastLeft = minY
+ lastRight = minY
+
+ for i in range(1, n - 1):
+ if np.random.randint(2):
+ yVec += [yPool[i] - lastLeft]
+ lastLeft = yPool[i]
+ else:
+ yVec += [lastRight - yPool[i]]
+ lastRight = yPool[i]
+
+ yVec += [maxY - lastLeft]
+ yVec += [lastRight - maxY]
+
+ # Randomly pair up the X- and Y-components
+ np.random.shuffle(yVec)
+
+
+ # Combine the paired up components into vectors
+ # vec = [[xVec[i], yVec[i]] for i in range(n)]
+
+ # Sort the vectors by angle
+ angle = [np.arctan2(yVec[i], xVec[i]) for i in range(n)]
+ xVec = [x for _, x in sorted(zip(angle, xVec))]
+ yVec = [y for _, y in sorted(zip(angle, yVec))]
+
+ # Lay them end-to-end
+ x, y = 0, 0
+ minPolygonX, minPolygonY = 0, 0
+ points = []
+
+ for i in range(n):
+ points += [[x, y]]
+ x += xVec[i]
+ y += yVec[i]
+
+ minPolygonX = min(minPolygonX, x)
+ minPolygonY = min(minPolygonY, y)
+
+ xShift = minX - minPolygonX
+ yShift = minY - minPolygonY
+
+
+ # Move the polygon to have center in origin
+ for i in range(n):
+ points[i][0] += xShift - box[0]/2
+ points[i][1] += yShift - box[1]/2
+
+ xr = np.mean(points, axis=0)
+ for i in range(n):
+ points[i][0] -= xr[0]
+ points[i][1] -= xr[1]
+
+ if not shapely.geometry.box(-box[0],-box[1],box[0],box[1],).contains(shapely.geometry.Polygon(points)):
+ _, ax = plt.subplots()
+ draw_shapely_polygon(shapely.geometry.Polygon(points), ax=ax)
+ ax.set_xlim([-2*box[0], 2*box[0]])
+ ax.set_ylim([-2*box[1], 2*box[1]])
+ plt.show()
+
+ return points
+
+def generate_star_polygon(center: Tuple[float, float], avg_radius: float,
+ irregularity: float, spikiness: float,
+ num_vertices: int) -> List[Tuple[float, float]]:
+ """
+ Start with the center of the polygon at center, then creates the
+ polygon by sampling points on a circle around the center.
+ Random noise is added by varying the angular spacing between
+ sequential points, and by varying the radial distance of each
+ point from the centre.
+
+ Args:
+ center (Tuple[float, float]):
+ a pair representing the center of the circumference used
+ to generate the polygon.
+ avg_radius (float):
+ the average radius (distance of each generated vertex to
+ the center of the circumference) used to generate points
+ with a normal distribution.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive
+ vertices.
+ spikiness (float):
+ variance of the distance of each vertex to the center of
+ the circumference.
+ num_vertices (int):
+ the number of vertices of the polygon.
+ Returns:
+ List[Tuple[float, float]]: list of vertices, in CCW order.
+ """
+ # Parameter check
+ if irregularity < 0 or irregularity > 1:
+ raise ValueError("Irregularity must be between 0 and 1.")
+ if spikiness < 0 or spikiness > 1:
+ raise ValueError("Spikiness must be between 0 and 1.")
+
+ irregularity *= 2 * np.pi / num_vertices
+ spikiness *= avg_radius
+ angle_steps = random_angle_steps(num_vertices, irregularity)
+
+ # now generate the points
+ points = []
+ angle = np.random.uniform(0, 2 * np.pi)
+ for i in range(num_vertices):
+ radius = np.clip(np.random.normal(avg_radius, spikiness), 0, 2 * avg_radius)
+ point = (center[0] + radius * np.cos(angle),
+ center[1] + radius * np.sin(angle))
+ points.append(point)
+ angle += angle_steps[i]
+
+ return points
+
+def random_angle_steps(steps: int, irregularity: float) -> List[float]:
+ """Generates the division of a circumference in random angles.
+
+ Args:
+ steps (int):
+ the number of angles to generate.
+ irregularity (float):
+ variance of the spacing of the angles between consecutive vertices.
+ Returns:
+ List[float]: the list of the random angles.
+ """
+ # generate n angle steps
+ angles = []
+ lower = (2 * np.pi / steps) - irregularity
+ upper = (2 * np.pi / steps) + irregularity
+ cumsum = 0
+ for i in range(steps):
+ angle = np.random.uniform(lower, upper)
+ angles.append(angle)
+ cumsum += angle
+
+ # normalize the steps so that point 0 and point n+1 are the same
+ cumsum /= (2 * np.pi)
+ for i in range(steps):
+ angles[i] /= cumsum
+ return angles
diff --git a/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py
new file mode 100644
index 0000000..05e09b0
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/starworlds/utils/misc.py
@@ -0,0 +1,51 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import time
+import inspect
+import traceback
+import shapely
+import numpy as np
+
+def tic():
+ return time.time()
+
+
+def toc(t0):
+ return (time.time()-t0) * 1000
+
+
+def draw_shapely_polygon(pol, ax=None, xlim=None, ylim=None, **kwargs):
+ if ax is None:
+ _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+ pol_list = []
+ handles = []
+ if pol.geom_type == 'Polygon':
+ pol_list += [pol]
+ else:
+ for p in pol.geoms:
+ if p.geom_type == 'Polygon':
+ pol_list += [p]
+ for p in pol_list:
+ if xlim is not None and ylim is not None:
+ pol_plot = p.intersection(shapely.geometry.box(xlim[0] - 1, ylim[0] - 1, xlim[1] + 1, ylim[1] + 1))
+ else:
+ pol_plot = p
+ handles += [patches.Polygon(xy=np.vstack((pol_plot.exterior.xy[0], pol_plot.exterior.xy[1])).T, **kwargs)]
+ ax.add_patch(handles[-1])
+ if xlim is not None:
+ ax.set_xlim(xlim)
+ if ylim is not None:
+ ax.set_ylim(ylim)
+ return handles, ax
+
+
+def logprint(message=None, print_stack=0):
+ callerframerecord = inspect.stack()[1] # 0 represents this line
+ # 1 represents line at caller
+ frame = callerframerecord[0]
+ info = inspect.getframeinfo(frame)
+ if print_stack:
+ traceback.print_stack()
+ print(info.function + ", line: " + str(info.lineno))
+ if message:
+ print(message)
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
index c335b98..07f336d 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/PKG-INFO
@@ -1,10 +1,10 @@
Metadata-Version: 2.1
-Name: starworld-tunnel-mpc
+Name: starworld_tunnel_mpc
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
-
-UNKNOWN
-
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
+Requires-Dist: casadi
+Requires-Dist: opengen
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
index 0be315e..b0c2aa0 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/SOURCES.txt
@@ -26,5 +26,7 @@ starworld_tunnel_mpc.egg-info/SOURCES.txt
starworld_tunnel_mpc.egg-info/dependency_links.txt
starworld_tunnel_mpc.egg-info/requires.txt
starworld_tunnel_mpc.egg-info/top_level.txt
+tests/test_motion_control.py
+tests/test_soads.py
visualization/__init__.py
visualization/scene_gui.py
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
index c75a0c3..057d7bc 100644
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/starworld_tunnel_mpc.egg-info/requires.txt
@@ -1,7 +1,7 @@
-casadi
-matplotlib
-numpy
-opengen
pyyaml
+numpy
scipy
+matplotlib
shapely
+casadi
+opengen
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
index 7043340..4dee13f 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/PKG-INFO
@@ -1,11 +1,9 @@
Metadata-Version: 2.1
Name: starworlds
Version: 1.0
-Summary: UNKNOWN
-Home-page: UNKNOWN
-License: UNKNOWN
-Platform: UNKNOWN
License-File: LICENSE
-
-UNKNOWN
-
+Requires-Dist: pyyaml
+Requires-Dist: numpy
+Requires-Dist: scipy
+Requires-Dist: matplotlib
+Requires-Dist: shapely
diff --git a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
index 16fb717..f849890 100644
--- a/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
+++ b/python/ur_simple_control/path_generation/starworlds/starworlds.egg-info/requires.txt
@@ -1,5 +1,5 @@
-matplotlib
-numpy
pyyaml
+numpy
scipy
+matplotlib
shapely
diff --git a/python/ur_simple_control/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF b/python/ur_simple_control/robot_descriptions/TODO_PUT_IN_FILES_FROM_HERE_INTO_YUMI_LOCAL_URDF
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-312.pyc
index 723cdbade5d2d11d0a64fb97184f739e1c29fade..115adf251a4a1d068f6e90128ff1d264ae83feaa 100644
GIT binary patch
delta 80
zcmdnac$bmqG%qg~0|Ntto5`Qli9ANeHToI(xvBc;`NakKDTzg3o)LnZQ<|5eUr>}-
Yky)%?P?VpQnp{#m(c6ph*2HOw03;?Ha{vGU
delta 42
ycmcc1xSf&bG%qg~0|Ns?s7gfoL>?pFF#V$Z{1W}L{G8I<)QN>&jK3!CQ3L??g$-)}
diff --git a/python/ur_simple_control/robot_descriptions/yumi_local.urdf b/python/ur_simple_control/robot_descriptions/yumi_local.urdf
new file mode 100644
index 0000000..47f8d9f
--- /dev/null
+++ b/python/ur_simple_control/robot_descriptions/yumi_local.urdf
@@ -0,0 +1,1160 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from yumi.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="abb_yumi">
+ <!-- - - - YuMi - - - -->
+ <link name="base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/base_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.122236 0.028996 0.017672"/>
+ <mass value="30.0"/>
+ <inertia ixx="1.61664025" ixy="0.0" ixz="0.0" iyy="1.90523836" iyz="0.0" izz="1.34940618"/>
+ </inertial>
+ </link>
+ <link name="base">
+ </link>
+ <joint name="base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="base"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>False</turnGravityOff>
+ </gazebo>
+ <link name="robl_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.5"/>
+ <inertia ixx="0.00083333" ixy="0.0" ixz="0.0" iyy="0.00083333" iyz="0.0" izz="0.00083333"/>
+ </inertial>
+ </link>
+ <link name="robl_link_1">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_1_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_1_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0102903 0.0225876 0.0585935"/>
+ <mass value="0.9629233"/>
+ <inertia ixx="0.0030634" ixy="0.00025149" ixz="0.00052912" iyy="0.00302651" iyz="-0.00085524" izz="0.00165401"/>
+ </inertial>
+ </link>
+ <link name="robl_link_2">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_2_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_2_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0178625 -0.0146039 0.0642901"/>
+ <mass value="1.4105"/>
+ <inertia ixx="0.00705656" ixy="-0.00022304" ixz="-0.00110629" iyy="0.00698719" iyz="-0.00115557" izz="0.00227954"/>
+ </inertial>
+ </link>
+ <link name="robl_link_3">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_3_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_3_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0201733 0.0266755 0.0620484"/>
+ <mass value="0.7187233"/>
+ <inertia ixx="0.00156497" ixy="-0.00022216" ixz="-0.00042641" iyy="0.00177346" iyz="-0.00041747" izz="0.00113318"/>
+ </inertial>
+ </link>
+ <link name="robl_link_4">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_4_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_4_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0559113 -0.015687 0.0226745"/>
+ <mass value="1.1807964"/>
+ <inertia ixx="0.00187228" ixy="-0.00093911" ixz="-0.00117309" iyy="0.00529598" iyz="-0.00029449" izz="0.00518088"/>
+ </inertial>
+ </link>
+ <link name="robl_link_5">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_5_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_5_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0708236 0.0278214 -0.0074085"/>
+ <mass value="0.3434158"/>
+ <inertia ixx="0.000364277603" ixy="-0.000253381412" ixz="0.000131795094" iyy="0.000720976878" iyz="5.00695671e-05" izz="0.000787541057"/>
+ </inertial>
+ </link>
+ <link name="robl_link_6">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_6_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_6_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0123616 -0.009411 0.0113342"/>
+ <mass value="0.4703021"/>
+ <inertia ixx="0.0005976" ixy="3.69e-05" ixz="-7.41e-05" iyy="0.0006515" iyz="-2.32e-05" izz="0.0005894"/>
+ </inertial>
+ </link>
+ <link name="robl_link_7">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_7_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_7_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.01155172 0.00013113 7.43e-06"/>
+ <mass value="0.04167441"/>
+ <inertia ixx="2.75680598e-05" ixy="0.0" ixz="0.0" iyy="2.03557822e-05" iyz="0.0" izz="2.03553446e-05"/>
+ </inertial>
+ </link>
+ <link name="robl_flange">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <link name="robl_tool0">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <joint name="robl_base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="robl_base_link"/>
+ <origin rpy="-0.9790773504912591 0.5692042289529107 -0.8254185631456783" xyz="0.047607 0.070008 0.411486"/>
+ </joint>
+ <joint name="robl_joint_1" type="revolute">
+ <parent link="robl_base_link"/>
+ <child link="robl_link_1"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="1.06" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_2" type="revolute">
+ <parent link="robl_link_1"/>
+ <child link="robl_link_2"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.03 0.0 0.1075"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.504547476611863" upper="0.7592182246175333" velocity="3.141592653589793"/>
+ <dynamics damping="1.09" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_3" type="revolute">
+ <parent link="robl_link_2"/>
+ <child link="robl_link_3"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.03 0.0 0.1603"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="0.61" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_4" type="revolute">
+ <parent link="robl_link_3"/>
+ <child link="robl_link_4"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0405 0.0 0.0912"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.155481626212997" upper="1.3962634015954636" velocity="3.141592653589793"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_5" type="revolute">
+ <parent link="robl_link_4"/>
+ <child link="robl_link_5"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1396 0.0 0.0405"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-5.061454830783556" upper="5.061454830783556" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_6" type="revolute">
+ <parent link="robl_link_5"/>
+ <child link="robl_link_6"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1254 0.0 -0.027"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="1.0" lower="-1.53588974175501" upper="2.356194490192345" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_7" type="revolute">
+ <parent link="robl_link_6"/>
+ <child link="robl_link_7"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.036 0.0 0.027"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-3.9968039870670147" upper="3.9968039870670147" velocity="6.981317007977318"/>
+ <dynamics damping="0.039" friction="0.0"/>
+ </joint>
+ <joint name="robl_joint_7_flange" type="fixed">
+ <parent link="robl_link_7"/>
+ <child link="robl_flange"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robl_flange_tool0" type="fixed">
+ <parent link="robl_flange"/>
+ <child link="robl_tool0"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <transmission name="robl_transmission_1">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_1">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_2">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_2">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_3">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_3">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_4">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_4">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_5">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_5">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_6">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_6">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_6_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robl_transmission_7">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robl_joint_7">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robl_joint_7_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="robl_base_joint">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo reference="robl_link_1">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_2">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_3">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_4">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_5">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_6">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_link_7">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_flange">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_tool0">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_flange_tool0">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo>
+ <plugin filename="libabb_gazebo_libegm.so" name="gazebo_robl">
+ <controller_rate>250.0</controller_rate>
+ <base_link>robl_base_link</base_link>
+ <end_effector_link>robl_tool0</end_effector_link>
+ <egm_server_ip>127.0.0.1</egm_server_ip>
+ <egm_server_port>6511</egm_server_port>
+ <joint name="robl_joint_1" position="0.0"/>
+ <joint name="robl_joint_2" position="-2.2689280275926285"/>
+ <external_joint name="robl_joint_3" position="2.356194490192345"/>
+ <joint name="robl_joint_4" position="0.5235987755982988"/>
+ <joint name="robl_joint_5" position="0.0"/>
+ <joint name="robl_joint_6" position="0.6981317007977318"/>
+ <joint name="robl_joint_7" position="0.0"/>
+ </plugin>
+ </gazebo>
+ <link name="robr_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.5"/>
+ <inertia ixx="0.00083333" ixy="0.0" ixz="0.0" iyy="0.00083333" iyz="0.0" izz="0.00083333"/>
+ </inertial>
+ </link>
+ <link name="robr_link_1">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_1_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_1_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0102903 0.0225876 0.0585935"/>
+ <mass value="0.9629233"/>
+ <inertia ixx="0.0030634" ixy="0.00025149" ixz="0.00052912" iyy="0.00302651" iyz="-0.00085524" izz="0.00165401"/>
+ </inertial>
+ </link>
+ <link name="robr_link_2">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_2_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_2_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0178625 -0.0146039 0.0642901"/>
+ <mass value="1.4105"/>
+ <inertia ixx="0.00705656" ixy="-0.00022304" ixz="-0.00110629" iyy="0.00698719" iyz="-0.00115557" izz="0.00227954"/>
+ </inertial>
+ </link>
+ <link name="robr_link_3">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_3_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_3_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0201733 0.0266755 0.0620484"/>
+ <mass value="0.7187233"/>
+ <inertia ixx="0.00156497" ixy="-0.00022216" ixz="-0.00042641" iyy="0.00177346" iyz="-0.00041747" izz="0.00113318"/>
+ </inertial>
+ </link>
+ <link name="robr_link_4">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_4_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_4_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0559113 -0.015687 0.0226745"/>
+ <mass value="1.1807964"/>
+ <inertia ixx="0.00187228" ixy="-0.00093911" ixz="-0.00117309" iyy="0.00529598" iyz="-0.00029449" izz="0.00518088"/>
+ </inertial>
+ </link>
+ <link name="robr_link_5">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_5_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_5_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0708236 0.0278214 -0.0074085"/>
+ <mass value="0.3434158"/>
+ <inertia ixx="0.000364277603" ixy="-0.000253381412" ixz="0.000131795094" iyy="0.000720976878" iyz="5.00695671e-05" izz="0.000787541057"/>
+ </inertial>
+ </link>
+ <link name="robr_link_6">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_6_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_6_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0123616 -0.009411 0.0113342"/>
+ <mass value="0.4703021"/>
+ <inertia ixx="0.0005976" ixy="3.69e-05" ixz="-7.41e-05" iyy="0.0006515" iyz="-2.32e-05" izz="0.0005894"/>
+ </inertial>
+ </link>
+ <link name="robr_link_7">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/visual/rob_7_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_irb14000_description/meshes/irb14000_05_50/collision/rob_7_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.01155172 0.00013113 7.43e-06"/>
+ <mass value="0.04167441"/>
+ <inertia ixx="2.75680598e-05" ixy="0.0" ixz="0.0" iyy="2.03557822e-05" iyz="0.0" izz="2.03553446e-05"/>
+ </inertial>
+ </link>
+ <link name="robr_flange">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <link name="robr_tool0">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <mass value="0.001"/>
+ <inertia ixx="1.66666667e-08" ixy="0.0" ixz="0.0" iyy="1.66666667e-08" iyz="0.0" izz="1.66666667e-08"/>
+ </inertial>
+ </link>
+ <joint name="robr_base_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="robr_base_link"/>
+ <origin rpy="0.9790773504912591 0.5692042289529107 0.8254185631456783" xyz="0.047607 -0.070008 0.411486"/>
+ </joint>
+ <joint name="robr_joint_1" type="revolute">
+ <parent link="robr_base_link"/>
+ <child link="robr_link_1"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="1.06" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_2" type="revolute">
+ <parent link="robr_link_1"/>
+ <child link="robr_link_2"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.03 0.0 0.1075"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.504547476611863" upper="0.7592182246175333" velocity="3.141592653589793"/>
+ <dynamics damping="1.09" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_3" type="revolute">
+ <parent link="robr_link_2"/>
+ <child link="robr_link_3"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.03 0.0 0.1603"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="10.0" lower="-2.9408797896104453" upper="2.9408797896104453" velocity="3.141592653589793"/>
+ <dynamics damping="0.61" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_4" type="revolute">
+ <parent link="robr_link_3"/>
+ <child link="robr_link_4"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0405 0.0 0.0912"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="10.0" lower="-2.155481626212997" upper="1.3962634015954636" velocity="3.141592653589793"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_5" type="revolute">
+ <parent link="robr_link_4"/>
+ <child link="robr_link_5"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1396 0.0 0.0405"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-5.061454830783556" upper="5.061454830783556" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_6" type="revolute">
+ <parent link="robr_link_5"/>
+ <child link="robr_link_6"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1254 0.0 -0.027"/>
+ <axis xyz="0 1 0"/>
+ <limit effort="1.0" lower="-1.53588974175501" upper="2.356194490192345" velocity="6.981317007977318"/>
+ <dynamics damping="0.08" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_7" type="revolute">
+ <parent link="robr_link_6"/>
+ <child link="robr_link_7"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.036 0.0 0.027"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="1.0" lower="-3.9968039870670147" upper="3.9968039870670147" velocity="6.981317007977318"/>
+ <dynamics damping="0.039" friction="0.0"/>
+ </joint>
+ <joint name="robr_joint_7_flange" type="fixed">
+ <parent link="robr_link_7"/>
+ <child link="robr_flange"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robr_flange_tool0" type="fixed">
+ <parent link="robr_flange"/>
+ <child link="robr_tool0"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <transmission name="robr_transmission_1">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_1">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_2">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_2">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_3">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_3">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_4">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_4">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_5">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_5">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_6">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_6">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_6_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="robr_transmission_7">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="robr_joint_7">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="robr_joint_7_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <gazebo reference="robr_base_joint">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo reference="robr_link_1">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_2">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_3">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_4">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_5">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_6">
+ <material>Gazebo/Grey</material>
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_link_7">
+ <mu1 value="1.0"/>
+ <mu2 value="1.0"/>
+ <kp value="10000000.0"/>
+ <kd value="1000.0"/>
+ <fdir1 value="1 0 0"/>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_flange">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_tool0">
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_flange_tool0">
+ <preserveFixedJoint>True</preserveFixedJoint>
+ </gazebo>
+ <gazebo>
+ <plugin filename="libabb_gazebo_libegm.so" name="gazebo_robr">
+ <controller_rate>250.0</controller_rate>
+ <base_link>robr_base_link</base_link>
+ <end_effector_link>robr_tool0</end_effector_link>
+ <egm_server_ip>127.0.0.1</egm_server_ip>
+ <egm_server_port>6512</egm_server_port>
+ <joint name="robr_joint_1" position="0.0"/>
+ <joint name="robr_joint_2" position="-2.2689280275926285"/>
+ <external_joint name="robr_joint_3" position="-2.356194490192345"/>
+ <joint name="robr_joint_4" position="0.5235987755982988"/>
+ <joint name="robr_joint_5" position="0.0"/>
+ <joint name="robr_joint_6" position="0.6981317007977318"/>
+ <joint name="robr_joint_7" position="0.0"/>
+ </plugin>
+ </gazebo>
+ <!-- - - - Grippers - - - -->
+ <link name="robl_sg_base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/base_link.dae"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0002 -0.007 0.0426"/>
+ <mass value="0.35"/>
+ <inertia ixx="8.73e-05" ixy="-2e-07" ixz="-4e-07" iyy="9.11e-05" iyz="-8.7e-06" izz="2.33e-05"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_vacuum_1_frame_link">
+ </link>
+ <link name="robl_sg_vacuum_2_frame_link">
+ </link>
+ <link name="robl_sg_camera_frame_link">
+ </link>
+ <joint name="robl_sg_vacuum_1_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_vacuum_1_frame_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robl_sg_vacuum_2_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_vacuum_2_frame_link"/>
+ <origin rpy="-3.141592653589793 1.5707963267948966 0.0" xyz="-0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robl_sg_camera_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_camera_frame_link"/>
+ <origin rpy="-1.5707963267948966 1.5707963267948966 0.0" xyz="-0.0073 0.0283 0.0351"/>
+ </joint>
+ <gazebo reference="robl_sg_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <link name="robl_sg_servo_frame_link">
+ </link>
+ <link name="robl_sg_fingers_frame_link">
+ </link>
+ <link name="robl_sg_finger_1_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_2_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_1_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <link name="robl_sg_finger_2_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <joint name="robl_sg_servo_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_servo_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.084"/>
+ </joint>
+ <joint name="robl_sg_fingers_frame_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_fingers_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1142"/>
+ </joint>
+ <joint name="robl_sg_finger_1_base_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_finger_1_base_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0125 0.0065 0.084"/>
+ </joint>
+ <joint name="robl_sg_finger_2_base_joint" type="fixed">
+ <parent link="robl_sg_base_link"/>
+ <child link="robl_sg_finger_2_base_link"/>
+ <origin rpy="0.0 0.0 3.141592653589793" xyz="0.0125 -0.0065 0.084"/>
+ </joint>
+ <joint name="robl_sg_finger_1_joint" type="fixed">
+ <parent link="robl_sg_finger_1_base_link"/>
+ <child link="robl_sg_finger_1_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robl_sg_finger_2_joint" type="fixed">
+ <parent link="robl_sg_finger_2_base_link"/>
+ <child link="robl_sg_finger_2_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="robl_sg_finger_1_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_2_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_1_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robl_sg_finger_2_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <joint name="robl_sg_joint" type="fixed">
+ <parent link="robl_link_7"/>
+ <child link="robl_sg_base_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <link name="robr_sg_base_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/base_link.dae"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/base_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0002 -0.007 0.0426"/>
+ <mass value="0.35"/>
+ <inertia ixx="8.73e-05" ixy="-2e-07" ixz="-4e-07" iyy="9.11e-05" iyz="-8.7e-06" izz="2.33e-05"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_vacuum_1_frame_link">
+ </link>
+ <link name="robr_sg_vacuum_2_frame_link">
+ </link>
+ <link name="robr_sg_camera_frame_link">
+ </link>
+ <joint name="robr_sg_vacuum_1_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_vacuum_1_frame_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robr_sg_vacuum_2_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_vacuum_2_frame_link"/>
+ <origin rpy="-3.141592653589793 1.5707963267948966 0.0" xyz="-0.0274 0.0185 0.0375"/>
+ </joint>
+ <joint name="robr_sg_camera_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_camera_frame_link"/>
+ <origin rpy="-1.5707963267948966 1.5707963267948966 0.0" xyz="-0.0073 0.0283 0.0351"/>
+ </joint>
+ <gazebo reference="robr_sg_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <link name="robr_sg_servo_frame_link">
+ </link>
+ <link name="robr_sg_fingers_frame_link">
+ </link>
+ <link name="robr_sg_finger_1_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_2_base_link">
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00139 -0.014982"/>
+ <mass value="0.05"/>
+ <inertia ixx="3.08333333e-06" ixy="0.0" ixz="0.0" iyy="6.56666667e-06" iyz="0.0" izz="4.01666667e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_1_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <link name="robr_sg_finger_2_link">
+ <visual>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/visual/finger_link.stl"/>
+ </geometry>
+ <material name="light_grey">
+ <color rgba="0.6 0.6 0.6 1.0"/>
+ </material>
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ <geometry>
+ <mesh filename="file:///opt/ros/humble/share/abb_myumi_smart_gripper_description/meshes/sg/collision/finger_link.stl"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0.0 0.0 0.0" xyz="0.011259 -0.004784 0.017852"/>
+ <mass value="0.01"/>
+ <inertia ixx="3.55e-06" ixy="-4.5e-07" ixz="1.68e-06" iyy="4.53e-06" iyz="6.8e-07" izz="1.54e-06"/>
+ </inertial>
+ </link>
+ <joint name="robr_sg_servo_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_servo_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.084"/>
+ </joint>
+ <joint name="robr_sg_fingers_frame_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_fingers_frame_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.1142"/>
+ </joint>
+ <joint name="robr_sg_finger_1_base_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_finger_1_base_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="-0.0125 0.0065 0.084"/>
+ </joint>
+ <joint name="robr_sg_finger_2_base_joint" type="fixed">
+ <parent link="robr_sg_base_link"/>
+ <child link="robr_sg_finger_2_base_link"/>
+ <origin rpy="0.0 0.0 3.141592653589793" xyz="0.0125 -0.0065 0.084"/>
+ </joint>
+ <joint name="robr_sg_finger_1_joint" type="fixed">
+ <parent link="robr_sg_finger_1_base_link"/>
+ <child link="robr_sg_finger_1_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <joint name="robr_sg_finger_2_joint" type="fixed">
+ <parent link="robr_sg_finger_2_base_link"/>
+ <child link="robr_sg_finger_2_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+ <gazebo reference="robr_sg_finger_1_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_2_base_link">
+ <material>Gazebo/White</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_1_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <gazebo reference="robr_sg_finger_2_link">
+ <material>Gazebo/Grey</material>
+ <turnGravityOff>True</turnGravityOff>
+ </gazebo>
+ <joint name="robr_sg_joint" type="fixed">
+ <parent link="robr_link_7"/>
+ <child link="robr_sg_base_link"/>
+ <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
+ </joint>
+</robot>
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc
index 11e5fceb19b1ac1e286cd8e4d4939298f13090c7..388924771e93bdbcaeb01a6e13de839f5951627b 100644
GIT binary patch
delta 79
zcmZ3<c%G5_G%qg~0|NttMzi%qZe!y-{fzwFRQ>e);)48?#3C@y2*J%M%}dcQC`zoz
XEY>e5%FjwoE-9YqZNqqLVuvCC)?^zu
delta 41
xcmX@lxRR0kG%qg~0|Ns?s7k~{Zev~#{i6K*68*CLoYLIXiG?<dzb39z1OVcY47~sV
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 5683b3c8a5c1c9547d5161bdd4dbf9898905d83e..aa9f8054b344ab0333701bfe20c412a83188c33e 100644
GIT binary patch
delta 1874
zcmcaw|D&IgZ#ge77Xt$WgL+^@dcFTfK7ZNC-3pvc25ggK7~3YllQp0GlZlU;9i#+=
zYuF~+$(c&BmvDo45S)c7D>-?ooQydKiVC>u8n$JO3=FFoAvzftYB^EV@l1YbEH2BH
z!d$}@&kIw7qJj^kLXkU#rIsg!wU)O8?0AUEEPe>hz_37IvY>?$TP<G=*8;)Gf)*ky
zHM}L<lP8*qOnzz1$5O+$Kp3Q;zJ_NR3j@Py7MQ+T{uH(vt}GE4KZVtXfq?-{j2(*@
z8(55^hAT@Hi(Z5nL@!L8GFWX2STl-8#J~~^3=9jzL9Ulc;Y{I5;Z8w!tw0Ue0tv8!
z8vYsq6cbQ`LV5CST>)ofn5hg5SyC`I)L>=?hSkh4v06bqW+_d6!OAZz4O5%KTf<)?
zSR(`uQDs@w;1n*ggsEd-$daA>gH=YLMmSy$CRoG0Kpy0Nu^M62WWmT#!?i#GB&C27
z`-(7~Da<weDXg%Flt6K>5=>E+^5imoH479IRA7=dTs3Sp+%@be>?oR11J0D4xt6tt
zd4Vb@xp35q)G*huf_<vQTf<r-lExCupvmu7r80RxhstDmdG5(yRjauZ7#J9e;u#ni
zG}R{S$lGl0SL0@6(iEMXB`@c2iz_!jB{e5Lu_(9rmS9<CacN>sJd%(|a(+%uW^rbI
z9<nHBN@7VOSkL6Q>J^N!lhZV`7(FNRSu0EPrKgs}SC-~xLev+vFfcHzWME(@{>wCZ
zmwaHokmwab^$QwS7X_`a2wFFI-Ib8OBB8q?>w=Zf+2kvJaaa6OFQjK*$gR65QGZ3E
z{sN2WU1_;1(gqu(F4+2C4Gg&wn0O^H`$BHv2L^@`Mpvef3=A@EOkcp{1r~{?0>V=`
zW=LHWP`M(Y(%|`(gF#Ye4(CknIouamL~qDyP3G4$W@MW@!_btGWAZseEyf#@e;R5r
za!gh++BbQkmYaFh1<&{kiA5JmO0SkzT`8~o&cIN?=)v>}LVW~LULfl#7`;GtOM8Pj
z(%zFd*h)^8vQwBmLtAX~6oo`qg`zM}u!n;PCJ+J2bVZ<?ToeJ~Mou;~kTnnjF@-@<
zBVXhVVk>}%L=d3`B9cG^D3KK<gIFn(%M8o}K*_a89>mWF5e1X?7${AitsZ1lR1M<S
zfQVWUQ3oRGK|}+HXao_lAfgFGG&3+TOzzk4VeMvMV0g9pfrbscJYhG>Xx?Hi$<NO*
z(3C17)3Fk!Wgs7Oljz*Z!iI*deGCi?rkgzt@3Aw6P1dmVV}l2#%;XuCvUa$Gj)8&U
z7LQkcW?qReD8&aw6{VpG1w>A6HqgWxyxN+e5Yq+`Z6LRc7PW)e9U!6;M09}&Nbpat
zw9;pT1pVZ7R_4OkgI#K}khK}>Oa=yqlFh!>j?9cXll$!?OhDPC2oxbjAfFb2<X18j
y`G8FKVqjqS#bJ}1pHiBWYFD(7fq?;3Z54l>eA{k`fE}aN2L?Fd#5B3j-W>q9?uyC)
delta 697
zcmey-&v><-Z#ge77Xt$W!)e2a^eyfi`TS+M*%=rZm>C!tKKC$Aj$v$@{7%+<vMm$e
z<k=>iOf}4t?c_|ESxdMlPcRXIa3v=%m6MsA$;7A04KfLYYnYcYGBB)W1hGN5maT@l
zhNXtJhAoXHm_d_y@*HI&CQY8nAu2XZn(UM3s>&G^c``6C6oc$mfPf+|1_p*(eCeqr
z@s*{ynen;#DXBR{VGIlmD?uvvGEBa!7C8B?Oy}nBnu)BF&nZ{4B{48C6eUics4uyB
zf(kd|WF3Rx$-h-A7~?0GsA)~!WZ=!(&cMKMU^A1U4ZCOrNO2^HU;+`MAVOsFJVzPL
zqIeKH0c1UENq&Bgfu>ZE0!SzsL}-DC6cC{}`KZ2`Kt70B03zf-g#2VbCneJg1_lOA
z?xH%7NIi&X01=HKq6tJagNPOo0k*3Z#A=&tYH7&Y#lXP8y*b<R9y?>iWG_d*$%`B%
z*r6_(yxmd34&4zTt*Ia)4dh@Rul&rs5?@eQ1w|EQpa}&;PL_9)$LeJHa*!DycNb}b
zSlS?>666ihqACUkhRO8?l9T5->99dOG5La%xd4U-))~l7HgPs%oyx$#;I%p5*^!ws
zZ}LVr2@_DX6@dc32;{#ako-!9A|H?)plJTZVUwGmQks)$S2UM_fdQ0mia$;M>$Zen
Ql+o-11Dw#CywTkq0KM?2*#H0l
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 42e86edcddac775dd8dd65d294a9d30d13a39372..4431c6814afa3ccd929c1c6f3ebfff7f72ffa3be 100644
GIT binary patch
delta 228
zcmX>-f$`=9M()$Ryj%<n3=GFF$8Y4G$IF;Md5(-0M?q13R%&ud@#Jm1L5z}{Ir%J@
zn6C)vOztxgpPV2d#K$10`+<i+P~p0O{uKdzm@qSgpuy&i0^6innQK_GL?%1R2v4rp
zabjei{6I&1a=i}Q=KDHztc=x@<BVnrJ<!y;z#@8`MfoC&@&d<;ENTxX^B5}%Jy6#G
z3oBk^QCy&SkwxXfW=CUJMyTyp%nX9MlMUSEH@`C3$<H`%a-5Of=0`4^FxSE4S2Nz4
Ie8)W>0F*pPmH+?%
delta 207
zcmcaPf${VNM()$Ryj%<n3=Ee8A~tf*<K^|#FUrp^(J#x-Da}ote1|uPQF*gIp9K@g
zMFE{p46Lm3lNSnT3bV4xf8b#dRJbmne^EdmF3!x#D!=)xz&7d03w0zW&(?8bWSM+G
zM||^FofcNcmdRB{vzR`xO)k(>nw)7Z$oheeK|^!$LRW>!bFG(7<~Qkw*ue&}!^Dc2
mja7d0J(C;!jEg5%8QE?A>cR<i1d@WucJ8Yge@%Yno(}*NjzSjz
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index 78ef9cb72fde8a2173f7b05fe10cb5bb70c9784d..a70360a9ceaab78c4f4dbc30377f789bb8b68029 100644
GIT binary patch
delta 176
zcmca;cGryiG%qg~0|Ntt%X-_5-05t_#rhffxvBc;`NakKDTzg3o)LnZQ<|5eUr>}-
zky)%?P?VpQnp{%6xt`6IMK*?kfuVunhN9|nxg~NJcw}#gOHZ$xSa(6q{EE26<fmMQ
z91IecAD9`G3^prp2XioHO)e2uXIdgRd9kn~SoDRk3M=EQ$-I(H%(o>qCr_6QVP=rh
Q+WbyZnUV3<WFcvL03RGUZ2$lO
delta 164
zcmca>cF~OcG%qg~0|Ns?hf2go?sPU@U;U!|{1W}L{G8I<)XhuTY*{q37#J8D7;Y%4
zE|*&<_lbdtGmG&8kL(R`>FIS7>n^C7Ulh0a#K6d##W?vtmmwF3@qrnnXtM=(Fb8Ad
z<Q`#l4v<DhPCur}TZJ82K#a{lg;iJ?-%pm4Y~r{fsriY4ozr&mV#yF@c23*Pza*6z
L8GlWdlePx{wiPft
diff --git a/python/ur_simple_control/util/encapsulating_ellipses.py b/python/ur_simple_control/util/encapsulating_ellipses.py
index ec0e6ac..80dd208 100644
--- a/python/ur_simple_control/util/encapsulating_ellipses.py
+++ b/python/ur_simple_control/util/encapsulating_ellipses.py
@@ -115,6 +115,19 @@ def visualizeVertices(args, robot : RobotManager):
# TODO: make this for every robot.
def computeEncapsulatingEllipses(args, robot : RobotManager):
+ """
+ computeEncapsulatingEllipses
+ ----------------------------
+ make convex approximations of the robot's links
+ so that (self-) collision avoidace can be calculated more quickly
+ and easily.
+ this includes the need to group related links because otherwise
+ we have multiple ellipses for the same part of the robot.
+ the grouping of links has to be hardcoded and done manually
+ and as it is non-trivial to make the grouping algorithmically,
+ and since this is done once per robot there's no point
+ to writing the algorithm (more work overall).
+ """
model, collision_model, visual_model, data = (robot.model, robot.collision_model, robot.visual_model, robot.data)
viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
#q = np.zeros(model.nq)
@@ -127,18 +140,57 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
pin.computeAllTerms(model,data,q,np.zeros(model.nv))
time.sleep(3)
+ # vertex groups will have to be hardcoded per robot,
+ # there's no getting around it
+ vertices_grps = []
+ vertices_grps_indeces = [
+ #[0, 1], [2], [3], [4,5,6,7,8,9,10]
+ #[0, 1], [2], [3,4], [5,6,7,8,9,10]
+ [0, 1], [2], [3,4], [5,6,7,8,9,10]
+ ]
+ vertices_grps_joint_parents = [collision_model.geometryObjects[0].parentJoint,
+ collision_model.geometryObjects[2].parentJoint,
+ collision_model.geometryObjects[3].parentJoint,
+ #collision_model.geometryObjects[4].parentJoint]
+ collision_model.geometryObjects[5].parentJoint]
+ # index of parent joint needs to be here
+ # we also maybe need parent frames
+ #vertices_grps_joint_parents_indeces = [collision_model.geometryObjects[0].]
+ # we'll np.vstack related vertices into vertices_grp
+ for v in vertices_grps_indeces:
+ # put them into their group
+ vertices_grp = np.empty((0,3))
+ for j in v:
+ geom = collision_model.geometryObjects[j]
+ # plot em in meshcat
+ vertices = collision_model.geometryObjects[j].geometry.vertices()
+ for i in np.arange(0, vertices.shape[0]):
+ viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
+ vertix_pose = pin.SE3.Identity()
+ vertix_pose.translation = vertices[i]
+ #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
+ #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ viz.applyConfiguration(f"world/point_{i}", vertix_pose)
+
+ vertices_in_joint_frame = []
+ for g_v in vertices:
+ # g_v is the vertex v expressed in the geometry frame.
+ # Convert point from geometry frame to joint frame
+ j_v = geom.placement.act(g_v)
+ vertices_in_joint_frame.append(j_v)
+ vertices_in_joint_frame = np.array(vertices_in_joint_frame)
+
+ #vertices_grp = np.vstack((vertices_grp, collision_model.geometryObjects[j]))
+ vertices_grp = np.vstack((vertices_grp, vertices_in_joint_frame))
+ vertices_grps.append(vertices_grp)
+
+
ellipses = []
- for i, geom in enumerate(collision_model.geometryObjects):
- vertices = geom.geometry.vertices()
-
- for i in np.arange(0, vertices.shape[0]):
- viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
- vertix_pose = pin.SE3.Identity()
- vertix_pose.translation = vertices[i]
- #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
- vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
- #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
- viz.applyConfiguration(f"world/point_{i}", vertix_pose)
+ # go over grouped vertices and compute their ellipse fits
+ #for i, geom in enumerate(collision_model.geometryObjects):
+ # vertices = geom.geometry.vertices()
+ for i, vertices in enumerate(vertices_grps):
cw = casadi.SX.sym("w", 3)
exp = casadi.Function("exp3", [cw], [cpin.exp3(cw)])
@@ -164,13 +216,15 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
A = R @ casadi.diag(1 / var_r**2) @ R.T
totalcost = var_r[0] * var_r[1] * var_r[2]
-
opti.subject_to(var_r >= 0)
-
- for g_v in vertices:
+# for g_v in vertices:
+ for j_v in vertices:
# g_v is the vertex v expressed in the geometry frame.
# Convert point from geometry frame to joint frame
- j_v = geom.placement.act(g_v)
+
+# print(j_v)
+# j_v = geom.placement.act(g_v)
+
# Constraint the ellipsoid to be including the point
opti.subject_to((j_v - var_c).T @ A @ (j_v - var_c) <= 1)
@@ -194,14 +248,15 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
# TODO: add placement=pin.SE3(P, ellipse.center) and id=robot.model.getJointId(e.name)
ellipse = SimpleNamespace(
- name=model.names[geom.parentJoint],
+ name=model.names[vertices_grps_joint_parents[i]],
A=sol_A,
center=sol_c)
ellipses.append(ellipse)
e, P = np.linalg.eig(sol_A)
ellipse_placement = pin.SE3(P, ellipse.center)
#ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
- ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ #ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ ellipse_placement = data.oMi[vertices_grps_joint_parents[i]].act(ellipse_placement)
viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
print(ellipse)
@@ -259,7 +314,7 @@ if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- computeEncapsulatingEllipses(args,robot)
+ computeEncapsulatingEllipses(args, robot)
# get expected behaviour here (library can't know what the end is - you have to do this here)
if not args.pinocchio_only:
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 1b088c6..b691c5e 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -262,14 +262,14 @@ def heron_approximation():
# TODO: make these parameters the same as in mpc_params in the planner
model_mobile_base.velocityLimit[0] = 2
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- #model_mobile_base.velocityLimit[1] = 0
- model_mobile_base.velocityLimit[1] = 2
+ model_mobile_base.velocityLimit[1] = 0
+ #model_mobile_base.velocityLimit[1] = 2
model_mobile_base.velocityLimit[2] = 2
# TODO: i have literally no idea what reasonable numbers are here
model_mobile_base.effortLimit[0] = 200
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
- #model_mobile_base.effortLimit[1] = 0
- model_mobile_base.effortLimit[1] = 2
+ model_mobile_base.effortLimit[1] = 0
+ #model_mobile_base.effortLimit[1] = 2
model_mobile_base.effortLimit[2] = 200
#print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
#body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-312.pyc
index f047d1996c3cab390860f9ade0a64f578c4b48bf..ef461beee0f1f95021b9d5b6ac3926ed0403e6b3 100644
GIT binary patch
delta 80
zcmZ3-c!iPYG%qg~0|Ntto5`Qli9ANe#rhffxvBc;`NakKDTzg3o)LnZQ<|5eUr>}-
Yky)%?P?VpQnp{#m(c6*n*2G>#031CWMgRZ+
delta 42
ycmcb@xQ>zMG%qg~0|Ns?s7gfoL>?nvU;U!|{1W}L{G8I<)QN?TjK3ysRs;a@o((<#
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 876bff867c79676f94a98cbd0ad7a9d2f1e30182..31e46a697ef6ce9f033463f59951d89785f53a96 100644
GIT binary patch
delta 2414
zcmccC#`vLwk?%AwFBby?1H;{=sp&QD8~I$Aj7#)0@^e%5)ANf9@>3Fvz&s-aH>Wf&
zMZcgZu_Cirzn~~TD>b>Kcym0{G*(8l$=}!mnXid!Pxj}?XTBzAGWj@%za)c@=>rb_
z2^<$WB(HNQT;x!=%Axdun?XWnvH_<WYYry^!}Q6qLK2&cIcpdh#W#Q8n#aVHA~yL3
zkL=`n-fc|cRg-)9q?y)8Ob+4e6_;EiwVDy+UIqq6h6zkPQY;KLELql*H)_gG7Uhp(
zl%8C`Z^$l_t--)hBsn=wNN@8s{?$y9atKY43_Z5x3>D0g4CRcP@|$M~buqHuVooh8
z+N>h-fsxT?vZz=mW6<RJVy;}73=9k|3=9m#os-{*ZD77Gt~GhNxSY!c4$}+#rg!*-
zCRohyxG1c8U0C~~u=Ztsoehka_zf>`7+&Bv{KCedpn4?rf=~Pvw}i{8i5J}xc7!g-
zSW$6V&EkSv!o|sd#jj4@C1KBeP0)BUlVqJHgOJGw9tKXO1sWGQw6AmMUF6Wa&S83y
z!*qwv1rF0|9PS?+85tzBKe$a^C7Hy0TSjHFs8j^=HA#obWl}wy3{sAa9|Rd>RVVXE
zn{yU3GN>gn{`itJ*+*JOG>Dt)jD{;ukO0Fa0fiuA=1WG5LB^ANrNx<8*d{NNcF?L}
z%VLEDDKc5ZlENg(z`%eAxf1rteTFhDHEda2lRv5ni!y_xQ&`rpu4aO0WMn{B!bVVu
zAv+c&yb$vk7*aS8)^jp&GSskU@lW2UDJ50QQp4iHFo7|44+BFjdkuRECpgN>YM3Wa
zP!N$R;x0j$!G$mb?DmVA!jscwMJ9K#2v5EyD+loqvY>*T$Rq(S5k`gyjCqbV>?zD(
zv$esRKdK3{f;~R@p@s-o4ND4`lO;2Gqo(j=9u2O^yXANo!QsOS_B>1(n1fKp%bvoH
zW-2%!V9LQ9gmPu}6eh585!{hA*-pNnQEu`%c`>0HmK1()u=0XP5O4AZd2wz5uo0p)
zY+0(H2vilUVM!4HsX#<v4SR|ZNL4Y@1je2fHEdbxAeDw7l|ra0CouNxXJMGY*mFgh
zp+o}~VJZwoWsD5iVnLX^Sm29fsAPcWSCPpMidsyJg_AQC0~p;W?^1NqXD!Ij%qzLY
zpPy7*l9-&68ed$Hky?~m1WJ=d><kPHw|L-EN%<9%C6)XYM2kQP{uX~uequ^kW@>qA
zk#BxVYR)b8l+5CSoW#n>^-2odo(v2OMd2XAWAZ8`$9j&$l#~FlU4n@zDZyY{{GpcK
z;!8|PamufN^Vva)6H78~@g%0CxD_SlrUoRIWE90RFfeFx6y<<S%>@y8AVMGHO6KI;
zl%fPMt1PqX7ISuLWibZ>1A_t-OpaBS<?01xNKlE?Gr2`sf%gFmd%Jg|_XOq%&J#H%
zFI0{cQDxP=!6$S>NPN2HM9s<C9nN>er6voj{ExaIrF}_Q`+<n~brH>rBAP3dFX-7{
z)N(i=cu~vwK=1|6sEe-A7h+;By2f1*iN7H%Hbd`{um)JU+C>qy1;s1uE~;DYFubU4
zyQBDk-9`JL3&9~5?L)7KgiSVAHRrWtkdXe!%D^xD<BJ31<SNxT32iq^F9wE_YEoVd
z45t_zZM^N7Punqi+i(7$dW=<T0?6M8BAbDMAw-k$7E69XYMv%b5kh9d<OkXTY=vOu
zlg)H;+1wcz7^;FM|J6~Rd`4#qqyOYYT?NLf$xXWIjCGS&>8b>SqpTVnkOieBMc`=k
z1?iUu5gs6-9z-;Nh(-|61R}su!-p+uin=B%>M2NdgN$U!%TGzY#Zs18Wps<F(C`*h
zp;1x(<Y+xxiRBCo4EI4%<Icpu(7<p*K<2uD&J_Wj$y@alnHdCiH{aA_<zWL=Cq=H4
zpIAM%06Q!MWOXRWWWMD5g39=`qWs+Wq@?_cqA*ZWVY$VcT9%quQj`Hw=K~@VC;zuL
zXRMrTZWG5iZSpi5c}Dli+iX%q!Nw_oRCs|1ZxG=#S>IMd1?+hl5Z@0(D1!(U5TObp
z)IdbQ<ZN4=`XCTf8$`gJRkRMo1-a!GcWyyWd~$wXUTShl(R`3FI4nWIeT%OswYVg|
zC^fz)H9a#wuP7MgBhKQ~lK6_0#FE6KXpr(45CID2qBxM>#BVX@<fj*b5`R$_NJ?M<
zh+GIFQb0uNWHmb_#-PcbcCu_>|8JO_V<*Q34TTAIhFp6X7#JKF85oKsCZDz|XPRC&
z*~?y*^#X^%1^&sI_Jz#X1ob9Awhv@x5YnG);o!l{AfYz7#bF!sZ5jE=&W^6k46+KF
zdmPOe?dO5)s{s+BAi^9(fNK9DP=+f4MQsttu9XZ$Vjw9IkoP!ja`RJ4b5iY!j!kBC
hwvbI>WEB47$E?q&`zeDll#x;QvlDX?Bcm`_3jiNjcI5y7
delta 2259
zcmey+!Fa8Wk?%AwFBby?14D;OM7o08Mm`rNUO)Y!{QMIAvizLV+|<pjOw(8yohHk0
z1ae#!*Z#!7%BnoMfFqytvY^Q)21d?$#>tO3{G~vQ2ORtpI4*KXUguD_$f0nRL+JxI
zNU;~E8l&CjLe3eCj0~H(xaToVzQ((pNvvw}ML}t%HR6-!@b!vItdU&J2yzAk10%x(
zrXFS%h8mVE>&c2TvXf)^qZp+o@8CCNl%D*GUz1U0vxvY}CP`U@hDe4U+j52q=17Ke
zMoqcRkA%7yHy4V0U}W^3oFvxC7&!Tjm@8KX0|SE#0|P^G{bXzL4V;(7wLUY*v3f90
zekU%+bdh7SpoFCCMGnI+YzzvjM^Z2N#9wwxxTc!8BXmK=ii*o>7MCUmNL-z4E2++T
zS<v_s11o1P<K%S7Iu<bFg`_;YEy%PFPLuhhk~nY3sC;7J=1pXrTr3sAd0Eoo69cng
zBID$9QaxNC#s@*LWU{n5XFel?S`y=rFBy{;OY7(bnsA*Fbma-OWVmRl5G2EVNt!W8
zhB=uH5;kBG#Aaq-VE8<Zb@E?nhsozSxhKyy5@xAk%i@@Pkwt<fg?SCj<T;!g5Rr{6
z!kny<3=9ky!iH=x;nhrFlNlHpO1L2`28I-NL;!O#a5B`eX7Nr|l#!CDWvO9tVVJ-e
zJClK-mc52Og(F*ofuX3ZhB=FG@<drtU4#*w2qQqQgBgvWvjisZmsMvJovbJ$FUo-s
zhp`~?lj{^^MHm?-Fy_hCu%|GAt<we>2=XpdmgHnd8L7z+j727&m*eGTMUvjgBFql<
zJ6Me<6RYIpyT;;^&&!E3a!dwEf<qunax$a5^yG)e!e9fqzy^rauw}_kc9aoi<eI!!
zzMomNhHY|!iHKT?Kn+U@KPcc3fmXwwA_&q{%rt?qr?rMHOBH0eF-WB#s>%tBJ#$zX
zCNTDFRAwkqgZWa0p{R_J0ZaG^Q7`-^e^=yX6rRkgq&3+{$)C-Qfq|jQW^$~O_~dy?
zYO;)lw^$4EGxJJrapWfzmn0_Vq!xkFL=ig!1H<IcO8!nFMXU@A47d1m@)J|SGE>V_
zi+uA_Qgd$c<)#*ABqx@{7iT0Eq!!;|O)E;wO})id23Aw#!N9;!6b2&PC-*8lMzE)3
z78m3sR^H-BOi2j<+sK`mlHw1w^cFivII$$-7EfYIiW|tNfW(rFq8J7S22GBlY>+`Y
zAR-q;=!0CuoSd6d6hGNm#hj~)fq`KqD2sbfE>TgKyhJ5TQi)af2A|LkA@S*&6E!Dm
zcR1e=m-@^g$!jy2PxZg*46RGTDi1`&r~6OzU*NogYoY%J$BQavJD7KP?c_hfc+tY^
ziir2*bTxAxV@Y0{kE{&*!au%PO<t`QCn4@8=E=!$LSM?0li?(%qm4HQ^J#WQZ;s81
z>c?2MdO^-Z5LpZi3?Z6~w^;HEQu8!fiV!lrlLd7H*z&>3C#UP?LLw$;vYwvu<iEO8
zl>9(m<OGGWbAC>K5hN10GV?M^Adyg1F?q9|I%D<Z+j=VX;BamN>0mA>ErEo+56EbF
z5aA9YYCuFSh^PY*^&kQqxO~_Gx2PRt7-w>BN_=5yYH8{%&cx(wD3c{UH9xng1Ehc@
zFFz&q77JflW|h$`rb5G8Ooc{8MU#8<ZDp4*FfiNzMTrj+149GD4FQ?!0y-B3bUraK
zv06@ksISP(#A>;j)qs_U%^Bntm&pP)k1fEdCK$vI0h!O2oL^8GpH`Hg8=sVvUr`ha
zN)0TxSX0YV^Gb@+LF&9gM8afkTXV*u$;q~HijzSJT++wiJwCw4KP1F8$V$O2)W;`M
z!Qaov)6ccYZSo`A6o^F%AibU-!V5%zjbZ_@L_q{N?36$(a8SyCSiY0%>?9cdCr`K2
zsSgARXoCp2gNs&!xFA>F;?6C|iBHbY%S%lzDVhTk21f%Zrf%^Sr52au7p2A*rKV@*
z=M@Ek{K{FJS`uH8l30>h6a`Wq4I)4>R1^#HwfHUOoc#16P|62KPLaS|kb-$2A{j)a
zOb)VFQUN>43}l`VI9tbO=Eav}=BCEyWagzpLTv5i8TN8)V9zE@-eYgbwUdE?!IF`I
zp;&A3cl&aV$#tI@L_zgqfrBilew^e`$az^%?-K(Hrw8L?Hpf5~FeA#*gAJtYgTZ8e
zDe=ks9OrW0kdgnyz{Fd?I62PAl?BAuyv)gr(SA0_eU%^r9I)mf7N}M(0_EExP)aER
zxnU(kkr+rygn@zK7l%!5eoARhs$J3H$<i(svPq1L!k@gDwHS3j#W995GU|SIVg`wU
GwEzG?t~Oc#
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index 6dbd513..bdd34e3 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -80,13 +80,22 @@ class MeshcatVisualizer(PMV):
material = materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Box(dims), material)
- def addObstacle(self, pose, dims):
+ def addBoxObstacle(self, pose, dims):
color = [0.5, 0.5, 0.5, 0.8]
obstacle_name = f"world/obstacle_{self.n_obstacles}"
self.addBox(obstacle_name, dims, color)
self.applyConfiguration(obstacle_name, pose)
self.n_obstacles += 1
+ def addSphereObstacle(self, radius, position):
+ color = [0.5, 0.5, 0.5, 0.8]
+ obstacle_name = f"world/obstacle_{self.n_obstacles}"
+ self.addSphere(obstacle_name, radius, color)
+ pose = pin.SE3.Identity()
+ pose.translation = np.array(position)
+ self.applyConfiguration(obstacle_name, pose)
+ self.n_obstacles += 1
+
def addEllipsoid(self, name, dims, color):
material = materialFromColor(color)
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 07a69d2..862d3bd 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -123,6 +123,8 @@ def realTimePlotter(args, log_item, queue):
ax.set_ylim(bottom=-20.0, top=20.0)
if 'tau' in data_key:
ax.set_ylim(bottom=-2.0, top=2.0)
+ if 'err' in data_key:
+ ax.set_ylim(bottom=-2.0, top=2.0)
axes_and_updating_artists[data_key] = AxisAndArtists(ax, {})
for j in range(log_item[data_key].shape[0]):
# the comma is because plot retuns ax, sth_unimportant.
@@ -169,6 +171,8 @@ def realTimePlotter(args, log_item, queue):
def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
viz.loadViewerModel()
+ # display the initial pose
+ viz.display(cmd["q"])
# set shapes we know we'll use
meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
@@ -194,9 +198,12 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.display(cmd["q"])
if key == "point":
viz.addPoint(cmd["point"])
- if key == "obstacle":
+ if key == "obstacle_sphere":
+ # stupid and evil but there is no time
+ viz.addSphereObstacle(cmd["obstacle_sphere"][0], cmd["obstacle_sphere"][1])
+ if key == "obstacle_box":
# stupid and evil but there is no time
- viz.addObstacle(cmd["obstacle"][0], cmd["obstacle"][1])
+ viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
if key == "path":
# stupid and evil but there is no time
viz.addPath("", cmd["path"])
--
GitLab