From df4f2047307db0d1ceb2e2f54db0cc493e6acf06 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 18 Nov 2023 23:01:43 +0100
Subject: [PATCH] nearly done i swear
---
.../.drawing_from_input_drawing.py.swp | Bin 28672 -> 32768 bytes
python/examples/drawing_from_input_drawing.py | 82 +++--
.../forcemode_example.py | 0
python/examples/path_in_pixels.csv | 41 +++
.../test_traj_w_movej.py | 0
.../test_traj_w_speedj.py | 0
python/ur_simple_control/.managers.py.swp | Bin 53248 -> 16384 bytes
.../__pycache__/__init__.cpython-310.pyc | Bin 312 -> 332 bytes
.../__pycache__/managers.cpython-310.pyc | Bin 0 -> 6218 bytes
.../clik/.clik_point_to_point.py.swp | Bin 16384 -> 20480 bytes
.../clik/.clik_trajectory_following.py.swp | Bin 16384 -> 20480 bytes
.../clik_point_to_point.cpython-310.pyc | Bin 0 -> 3943 bytes
.../clik/clik_point_to_point.py | 13 +-
.../clik/clik_trajectory_following.py | 35 +-
.../__pycache__/create_dmp.cpython-310.pyc | Bin 4336 -> 4354 bytes
.../dmp/__pycache__/dmp.cpython-310.pyc | Bin 0 -> 6620 bytes
.../temporal_coupling.cpython-310.pyc | Bin 2508 -> 2526 bytes
.../dmp/{create_dmp.py => dmp.py} | 99 +++++-
.../drawing_gen/lasso_selector_demo_sgskip.py | 113 ------
.../dmp/drawing_gen/path_in_pixels.csv | 327 ------------------
.../ur_simple_control/dmp/robotiq_gripper.py | 291 ----------------
.../dmp/temporal_coupling.py | 82 -----
python/ur_simple_control/managers.py | 12 +-
.../util/.calib_board_hacks.py.swp | Bin 0 -> 24576 bytes
.../util/.robotiq_gripper.py.swp | Bin 16384 -> 0 bytes
.../__pycache__/draw_path.cpython-310.pyc | Bin 0 -> 2061 bytes
.../util/calib_board_hacks.py | 225 ++++++------
python/ur_simple_control/util/draw_path.py | 43 +--
.../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 199 bytes
.../__pycache__/visualize.cpython-310.pyc | Bin 0 -> 816 bytes
30 files changed, 372 insertions(+), 991 deletions(-)
rename python/{ur_simple_control/dmp/leftovers => examples}/forcemode_example.py (100%)
create mode 100644 python/examples/path_in_pixels.csv
rename python/{ur_simple_control/dmp/leftovers => examples}/test_traj_w_movej.py (100%)
rename python/{ur_simple_control/dmp/leftovers => examples}/test_traj_w_speedj.py (100%)
create mode 100644 python/ur_simple_control/__pycache__/managers.cpython-310.pyc
create mode 100644 python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
create mode 100644 python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
rename python/ur_simple_control/dmp/{create_dmp.py => dmp.py} (60%)
delete mode 100644 python/ur_simple_control/dmp/drawing_gen/lasso_selector_demo_sgskip.py
delete mode 100644 python/ur_simple_control/dmp/drawing_gen/path_in_pixels.csv
delete mode 100644 python/ur_simple_control/dmp/robotiq_gripper.py
delete mode 100644 python/ur_simple_control/dmp/temporal_coupling.py
create mode 100644 python/ur_simple_control/util/.calib_board_hacks.py.swp
delete mode 100644 python/ur_simple_control/util/.robotiq_gripper.py.swp
create mode 100644 python/ur_simple_control/util/__pycache__/draw_path.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
create mode 100644 python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index 424abe01a262c1f14f8d73501a3dc4a74581419a..6cb848bc0e1c7948199fc53aa4c7507705b8526d 100644
GIT binary patch
delta 2985
zcmZp8z}V2hB$i|l=IN_op=ZRvz`!8Dz`$)7nfgn8qu3jMel~`B1_lNukOTt*gT-V)
z0eeP+$$<j$jP{cY1>_kOCQlR)mtcp=J3#51P<nEpqs`_G0xbNK6j*o<FflOfVTRaX
zF<DR{yFQ(Zfgyy8fx(1}fkA_dfkA?cf#D%11H&Rt28J9?1_pIb28PER3=E4n7#Jcs
z7#KJ?7#I$+GcX)rXJDvcXJ9a3XJB~2#=vlnje%i58v{cz8v}zn8w0~_RtAPXRtAP}
zRt5%PRtAP+EDQ`~EDQ`5EDQ|ynIWz?%)+qwx57cD$s1KfHuI};GBL(X-k|O~`M0(F
z<QDY?M#s%;8iI@lxrup+>8VA<3YobD`9&oP&iQ#IMfo{C`S}ID5Gfsnp!}r#5-4x-
ze+?zQlA^?{)Z~)<qRRNR{G6Qp^31$+m;t$o1x79bi6t2!`Nl2*If;3xlmBaY)hjE6
z_`CQkB$bvZq-7Qrmnc-`mn!6?rlu&A<SQiSq$cJmlolu?=VWH<6;vuD7Aq7Nq$X#k
zWr7rBq!y(zKn=)EElN*SD9K1IPE|<D%t<W<sVz^<$<YA`Rw|_A=c$({q@)(77L}!f
z%uY&GC{8V4O)N?*0l7p%Hzl_~FD17C=3W=y038Lt{19iI5a+Pe9LMBj=ls0llA^@S
zywnth$^SJZCePPol}ss0ERQcpEXjcB0||h<309GynZ}V=lwOcnRGd0_pJv47daYJg
z-V>Y*3@KcY1feil&>*{h7B>S!4mSgXFE;}NBR2!XRxSpHOfHD34_pik$G8|6HgYjA
z^l&jS7;-T%$Z;_+h;cD6h;lJ7T;*h7m<BSDlYv2%lY!wG2Lr=;4hDt<4hDvJ4h9A;
z4hDuz><kRe><kQ+><kRg*%%lmvoSCPurV;0u`w{bW@V^nxWdZ7uz{6<VHqm}Lk%kf
zLpCb|g9$4G!*>=2hW#uI3{5Ny3>7R440<dK3|E*L7&@347;Kms7(OsDFkEC}VA#sU
zz)-`)z~Ilsz@Wm!z`)7G!0?KZfnggX1H*hq28LOT3=9>F3=HLr3=CzA&|Ctg6cpSP
z6qFSb^HLP@^2-&<Qx%dE^Ar-3OG*=Saw-*yO7j>L6ciM|L{5HwK|C~V>J^pdX=s9l
zi$Ez+Z*rqiCM$&X(kKbaigy#+oM4>C?ysO?09Fg5i}NxI6trzYMxzTQ<yS--$0mT4
zgOuqNl_}UNfJBXA!Ga133eZf$ZvcvLP&!Xk03`z*h1|q!2sbM~Gp|GektY<=@{1HQ
zOC}qb#7_3LHf7Gq%%0q6EyJ0fTH;ugUaX-BHS(TNW=U!;1K6UR{PcJbZ}Mv!T^2Ay
z##R}^aJJRu2Z=<h7Z$6>D%dI%noVZ372Z6_wup&Wot1%snGKS}3?>UYh}SdoGcX+I
zV_=xY$G{N5$H1V>$G~u(mw{m!F9SmhF9SmoF9U-EF9X919tMUvJPZtZJPZsPJPZu{
zJPZu?xfvLGxEUCnxEUC<xfvKfaWOC~=3-#T=3-z7;$mQs=VD+u#mT_1k&}U;o|A#W
zk&}Tzjgx`lD+dF^b`A!HIUEdn48a@>4D1{X47=GG81mT}7+l#I82H#37!I;AFf3<d
zV8~}<U{GXZVBlb5U|`;S)xnUJQDL&Kr#O>|=H!ct!sZ#NnduoN3i)ZE46TrqpIDTl
zppluUkegbPT2!nF76qpRh4Rdt9EI|t%#u`v{JhD9J_<Zw)w&Q9CKoEpPPX^vU^UP)
zHU%4PR-Tzsl0lZq+z^w&223tg7FC58M~TIyMW6yHvltxjB^jv-d8y?PcS9puhNUF2
zID2wfl<H&$o8ZY8-1R2wct~ts=y9B7@_S>>$pyYFlZ$=LC%-pl+q~Yllo6C-HY@o*
z65zFCW?%?ofkdzUWWj{$`aPTs3@bPp7?yJ~FqCpKFc@<(FvxN;FudSkV7S1+z_1Sz
z4Gi@h3=HlZ3=Bpb3=Fay3=B`$85s7mGcZ)KGcbg*Gcf#PV_>+?#=x+Eje#MXje#MQ
zje$Xxje+4ND+9wURtAO?Rt5%fRtAQTEDQ_}SQr>ivM?~LU}0cq=w$&pX)_~>3-ja+
z;VhdErsS|d5~^Y5gOrkdg|y7P6ovePlFa-(1!GNExeY4#6LX656>{?PvlTM)O7azo
z^K(;6GC+AKEio^-QXwZZI~6Rbkdm65UzAvqU!;&<1QvlP1C<;lMU|WRvOclaC#Enk
zECAJY(BOep&G+~j7!L6>Fzn@LU|7S?z!1yNz+l7Az`(`Nz;J?(fnfn3149-c1A`SG
z0|OHu1H&_328O4+3=DgC85rj9GB8x~GBBj_GB8B*GB8N<GBDiXVPIIr!@!Wu!@!`!
z!@%&Bn}J~&H$y$cQf>x@0B#0`A6yI!C%G6H>bV#gbh#K9ZbHlH8cqfVLrw+;DNY84
zZyXE^Z#Wnj4skFrY~)~IsN!H?u;pN2VC7(7IKj@qu#cUAp@f}*L5dwx**8JUY;iUQ
zhTp6V42M`57^bo^Fl4bZF!-}FFc`2(FfjaKVPJR)jh=lh3=Df&7#Q}jY-VIx0gf3L
z8W*)Y*~P&nUp%7ZgA~%rlMSs!>p>X-Ap<Gp(A6v0f?EaP5>QzoxhOTUBvm0%0jx}+
zxF9h(6}ebVN-R!IQOM5&mjIwL7_1M(P_PA;$qL#Ec?EijMMa5~8uifzdImZQU_#eG
z&%n~yA{OdAu$@+*@*A3OLA6j(dTDNIUWr0}9>~&+RE46{;u3|-Vg-M1ur-kUZl#cx
zS)ov+kXVsftdN<fVB`X7Vi>!Ct%s^hOi58l%&S+(PfIIKg_mQwiAC9|MG6`v86cg-
zph74)zqF`0RUxsUpeVm0GdHm$71e>z@JLR~$xJFr1m~xs)SSeU)D(q8sB?=UwI$4P
zMWuNPQ04Vt_28BhsOE!)5@zE}qad>)H3!_X(E%A-tOqXX;=v^yiU_1sg*h=dF*{Wu
qvqZtz1?+7wfiOzJR!;#`UxFG)(5hP*EC7!?SUeUd7p11=F#rJlPzFK(
delta 1458
zcmZo@U}|{4D3)Xp=IN_op=ZRvz`!8Dz!3i{B6V8mMzJ^i{HzT13=9lRAPEKr2II+s
z0``omlLH0h8Eq#Q3dl2hO`a$qJ~`6SWb*|975+&IEWE-j3=F?nAO;yv7F5Wt_v2z<
z5aVKC_{Yh>@S2l>;WQ@$!+uT%h7L{!22V}~1_n+BhW{K44BI&v81gw77}Pl!7<f4t
z82+*|Fl=OJU`S<WV31^IU^vUhz%Y%Cfgy;Efq|8cfnh5v14B701A`nZ#Kvzd3=B=I
z3=9#h3=Gat@xv?(n|~`DWSYzzoibS<mThuygf_pDjzUUmT4HHViLGI(uJL5WXxYh|
zBeW+^%9EY!pw2tlL5+3t1!ZLjTb*O`1!YdAN!Edr9gL+XyQwuW#V2k)t|rJhxj@5c
z@*>UJ$@*FW=4nOwxe6tzxdr(}i8=Af`K1LpnR)37nIM4@1;6|dXB~wQ=djcq$K+(^
z{Ji3lqQuO+)Rf5!H6<idatrj5i&7IyQsYx{3t)O&d;>PuX|=KPS}-y&)PSNufPukl
zvY<hB{UmM%h74{722XAV25D{v25xQ!h8J863|F`q7*269Fm!P-FnDt@Fi3JSFx=u~
zV3^Ixz>vbpz#z)Wz#sw&eGUeObPfgv3l0W`ckB!dbJ!UeJlGi+SlJmEUb8VUEMQ|`
z=wxGHNM&PSh-PD8Fl1w3P+((Vc*)AZu!fZ(ouP!4fx(=WfkBs*f#ER=14AbZ1A{FK
z1H(sV28N}~3=D0|3=CP!3=HPX3=CY%3=B`17#L15F)*xVVqmCcf_O)qiGkrWBLjmc
zGXsM%GXsMVGXuj%CI*ICObiU&ObiU|OcO6on!F*LW%2<-c~%Am1qFr42MqP$ELjm~
zs$^hbPyjQbN-`(w8zoMbbCuY<&M2FGa(u0~geF+KvO-RNVv0gZQDRnVa!GzsrOxDo
zwW3U!C6j%uttNL_OA8hx78R!!>7}QZ#3vS|7i(xjqFrXPw9P)Dl+5H32CzXn`RVZ>
z-sIOdx-4LZjIA<+;cTnR4-$!1FDzD%Rj^ekG@Hz5E4+D<Z4ndiYIX*OXbwo;P@OF3
zAYRYG&%khokAY!69|J=y9|MC19|OY^UIvEMybKIoybKJPybKH;ybKH<co-O#@Gvlx
z@h~tL@Gvll^Dr<x=VoA-!p*?o#m&HA%+0{?i;IC_H5UUzF&6_v6c+=7Iu`@OB~Av0
zotz8|?a;iY$H~C(mxF;}KL-QD5)KAEuyb5E7#RMsGca6WXJGik&cLvSoq-{Roq<7y
zoq<7`oq^#k8w0~*HU@@Eo3A<;vQFl+2$TZ_b)kZ-LQzRdYJ5>@a%yH-Dk#_-lS@hy
zb0+I$*s!IPXewwaOs>h0@&pGOD3uqNq!wtTl)$1RCAWYfB{fYUIX|zYC_l$1KffTn
zD6=FpFI}T3KPkUN2PTt~TBM_pscAJ?K5yIPU=O{?!5(hnAU7y0g!sGoD<r0*DCFdq
zD-<La7jF*s;ANd`5XHfql3Or2*VlZqK@{ucvr!zA<GeUF@A6G$oc!CDbF;4hbphVB
z%nS^Bp@o(0WWj{$dOj`&hC7@L46`^H7+N?P82mXI7|b~t7*shK7??R37@l!3Fs$QX
zV3^6lz|g|Mz~Ifnz@Wmx!0?crfng7{uySK(U|?rwV7SD_z_5pnfuV<ufx&@|fq{{Y
zf#EbO14A9OkaA~bU@%~1VBlwEVEDoUb_p{B59?+|mi5e_gugi|C5Hu)tWIS-*esa!
Go)rN3BuHfd
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 89d1dab..31e7cb8 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -23,25 +23,30 @@
# package it and then have it one place instead of copying it everywhere
# 12. make this a mainfile, put everything that could be turned into a function elsewhere
-from ur_simple_control.dmp.create_dmp import DMP
-from temporal_coupling import NoTC, TCVelAccConstrained
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt
import copy
+import argparse
from ur_simple_control.util.get_model import get_model
from ur_simple_control.visualize.visualize import plotFromDict
+from ur_simple_control.util.draw_path import drawPath
+from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
+# TODO merge these files as well, they don't deserve to be separate
+# TODO but first you need to clean up clik.py as specified there
from ur_simple_control.clik.clik_point_to_point import getClikController
+from ur_simple_control.clik.clik_trajectory_following import map2DPathTo3DPlane
+from ur_simple_control.managers import ControlLoopManager, RobotManager
#######################################################################
# arguments #
#######################################################################
# TODO sort these somehow
-def get_args():
+def getArgs():
#######################################################################
# generic arguments #
#######################################################################
- parser = argparse.ArgumentParser(description='Make a drawing on screen,
+ parser = argparse.ArgumentParser(description='Make a drawing on screen,\
watch the robot do it on the whiteboard.',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
# TODO this one won't really work but let's leave it here for the future
@@ -85,16 +90,16 @@ def get_args():
This is used when generating the joint trajectory from the drawing.", \
default=1e-2)
# TODO add the rest
- parser.add_argument('--controller', type=str, \
+ parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
default='dampedPseudoinverse', \
choices=['dampedPseudoinverse', 'jacobianTranspose'])
# maybe you want to scale the control signal
parser.add_argument('--controller-speed-scaling', type=float, \
default='1.0', help='not actually_used atm')
- ############################
- # dmp specific arguments #
- ############################
+ #############################
+ # dmp specific arguments #
+ #############################
parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
help="whether you want to use temporal coupling", default=True)
parser.add_argument('--kp', type=float, \
@@ -118,6 +123,17 @@ def get_args():
help="force feedback proportional coefficient", \
default=0.003)
# TODO add low pass filtering and make it's parameters arguments too
+ #######################################################################
+ # task specific arguments #
+ #######################################################################
+ # TODO measure this for the new board
+ parser.add_argument('--board-width', type=float, \
+ help="width of the board (in meters) the robot will write on", \
+ default=0.35)
+ parser.add_argument('--board-height', type=float, \
+ help="height of the board (in meters) the robot will write on", \
+ default=0.4)
+
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -147,13 +163,15 @@ def controller():
# you'd still need to specify what you're saving with self.that_variable so no matter
# there's no running away from that in any case.
# it's 1) for now, it's the only non-idealy-clean part of this solution, and it's ok really.
+# TODO but also look into something fancy like some decorator or something and try
+# to find option 3)
# control loop to be passed to ControlLoopManager
def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
- log_dict = {}
+ log_item = {}
dmp.step(robot.dt) # dmp step
# temporal coupling step
tau = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
@@ -193,22 +211,40 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
breakFlag = True
# log what you said you'd log
- log_dict['qs'] = q6.reshape((6,))
- log_dict['dmp_poss'] = dmp.pos.reshape((6,))
- log_dict['dqs'] = dq.reshape((6,))
- log_dict['dmp_vels'] = dmp.vel.reshape((6,))
+ log_item['qs'] = q6.reshape((6,))
+ log_item['dmp_poss'] = dmp.pos.reshape((6,))
+ log_item['dqs'] = dq.reshape((6,))
+ log_item['dmp_vels'] = dmp.vel.reshape((6,))
- return breakFlag, save_past_dict, log_dict
+ return breakFlag, save_past_dict, log_item
if __name__ == "__main__":
#######################################################################
# software setup #
#######################################################################
- args = parser.get_args()
- clik_controller = getController(args)
+ args = getArgs()
+ clik_controller = getClikController(args)
robot = RobotManager(args)
- # load trajectory, create DMP based on it
- dmp = DMP()
+ #######################################################################
+ # drawing a path, making a joint trajectory for it #
+ #######################################################################
+
+ # draw the path on the screen
+ pixel_path = drawPath()
+
+ # make it 3D
+ path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
+ # TODO: run calibration here
+ # TODO: create calibration related arguments
+ # add an offset of the marker (this is of course approximate)
+ # TODO: fix z axis in 2D to 3D path
+ # TODO: make this an argument once the rest is OK
+ path = path + np.array([0.0, 0.0, -0.0938])
+ # create a joint space trajectory based on the path
+ joint_trajectory = TODO
+
+ # create DMP based on the trajectory
+ dmp = DMP(joint_trajectory)
if not args.temporal_coupling:
tc = NoTC()
else:
@@ -246,10 +282,14 @@ if __name__ == "__main__":
#######################################################################
# TODO: add marker picking
# get up from the board
- current_pose = rtde_receive.getActualTCPPose()
+ current_pose = robot.rtde_receive.getActualTCPPose()
current_pose[2] = current_pose[2] + 0.03
- rtde_control.moveL(current_pose)
+ robot.rtde_control.moveL(current_pose)
# move to initial pose
# this is a blocking call
- rtde_control.moveJ(dmp.pos.reshape((6,)))
+ robot.rtde_control.moveJ(dmp.pos.reshape((6,)))
+ loop_manager.run()
+
+ # and now we can actually run
+
diff --git a/python/ur_simple_control/dmp/leftovers/forcemode_example.py b/python/examples/forcemode_example.py
similarity index 100%
rename from python/ur_simple_control/dmp/leftovers/forcemode_example.py
rename to python/examples/forcemode_example.py
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
new file mode 100644
index 0000000..b236f1c
--- /dev/null
+++ b/python/examples/path_in_pixels.csv
@@ -0,0 +1,41 @@
+0.16240,0.57914
+0.16389,0.57914
+0.16537,0.57914
+0.19359,0.58663
+0.27228,0.59862
+0.32574,0.60311
+0.34801,0.60311
+0.38661,0.60311
+0.42819,0.60311
+0.47273,0.60311
+0.50986,0.59862
+0.55292,0.58963
+0.58261,0.58064
+0.58558,0.57764
+0.58558,0.57914
+0.57964,0.57465
+0.57073,0.57315
+0.56925,0.57315
+0.56776,0.57315
+0.56479,0.57315
+0.55589,0.57315
+0.54698,0.57764
+0.54549,0.58064
+0.53064,0.59113
+0.51579,0.60011
+0.50689,0.60760
+0.50540,0.60760
+0.49501,0.61210
+0.48610,0.61959
+0.48610,0.62108
+0.48610,0.62258
+0.50540,0.64655
+0.58261,0.68699
+0.62122,0.70497
+0.68655,0.73643
+0.72516,0.75140
+0.74743,0.75740
+0.81128,0.77837
+0.82910,0.78436
+0.83355,0.78436
+0.83355,0.78436
diff --git a/python/ur_simple_control/dmp/leftovers/test_traj_w_movej.py b/python/examples/test_traj_w_movej.py
similarity index 100%
rename from python/ur_simple_control/dmp/leftovers/test_traj_w_movej.py
rename to python/examples/test_traj_w_movej.py
diff --git a/python/ur_simple_control/dmp/leftovers/test_traj_w_speedj.py b/python/examples/test_traj_w_speedj.py
similarity index 100%
rename from python/ur_simple_control/dmp/leftovers/test_traj_w_speedj.py
rename to python/examples/test_traj_w_speedj.py
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index 0b0b860270552b6cb3f46e23f92ce3b1971f0e75..f697a6cfa3b4672a101af84fd21aa323b24910de 100644
GIT binary patch
delta 687
zcmZozz}(QlD3)Xp=IN_op=ZRvz`!8Dz|f)`nYv$nqu3jMel~`B1_lNukOTt*L&#)7
z0rC3(P!J3i4TaK-Q2HNKJ^;#J0i|K`f1&a){Xfv;|3Kwo`n{nVp_CK@GgN~Nl%5Th
zVce{kV9!5EfrVFrfq|h1Bq{)Qw1Rv61YQP)eqIKKZe9k4T3!Z*TwVqSWnKn`J3I^w
zvv?R70(lr1IC&Tt4stUvG;=dB#B(z+@N+XTyy9YDc*(`Uu!@U;p@EBm!JLbM;Rz=L
z!*WgrhD1&V23<}D1|3cY202a!1_4e6hA$ip44*j|7#?vjFr4FHV3^3kz>vtnP|py{
z!N3s1!N3r~!NB0k!N6e1!N72xoq=I3I|D;HI|G9ZI|IW9HU@_MYzz#oYzz#IYzz$P
zYzz#-Yzzz!Ss55Mure^TvNABlu`)2YvNABpvobI+ure^*WMN>~$il!7!ot9y&BDNN
zkC}mC4l@HoAu|Jm95VyM4<-hN?@S^L46RHI44O;~496HDVbjex@uJe?KV2M~%_~o`
zZT8g&WS#s$S7!4mT`Q(~H!cQ-OI(oT01M_#{0t09{0t1L{0t1d{0t0>`4|}b_!t-p
z`4|`+`4|{@`4|{3^D;2(=4D`*!pp$m%*()_#>>FK!OOsKhKGS+6AuGJDGvifIu8Sb
w5Dx>x9&QGPB5no-du|2>7H&u|U*Te4xXi`C4z~Ov*Jef*FD|ef<t<fM0nvAPfB*mh
literal 53248
zcmYc?2=nw+u+TGNU|?VnU|`_?5s|t&w1y#UCL=?7esMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y=KJ
zn3tHIT2!o8PzkYMlpGC#(Gb8M0;MHsx)!_)#zuw)AO*@wiVDI)p&;fc9u0xf5Eu=C
z(GVC7fzc2c4S~@R7!85Z5EvmLP*T9nP|v`?zy$R#cpQL%fq@aqhtcd%z8;hoh0-v2
zE+}6YO2<QKm^>$xuLz~9p)^dM1<E&s(y34yCeH)qt3v4;P#PxB2Ia&2u?5P9$+JTF
zo=_THo*OEz0i}(g8le;e2UJ`VN((_HpcDf$R9p#4vq2@G6jXGS8V!Nb5Eu=C(GVC7
zfzc2c4S~@R7!85Z5Eu=C(GVC7fq@Bu#1saGOm+qaUr+~EfPn$l|G&-8z_65`fuWb5
zfuV|@fgz8dfx(8KfkA<vf#DM$1H%zM28IQE3=DC63=Bbh3=A%O3=Hag3=AK585q{_
zGBC{JWngIFWnif1Wnif0WnjqVWnhTqWnd8IWnehN!@$tR!@%Ik!@%&9n}J~?Hv>a4
zHv@w|Hv_|eE(V5MTnr31xfmE`aWOCya4|5bb1^Vn;bdT#&dI<K$jQJU%gMkX!^yzF
z2O3o1WMFv0!NBmCgMr}^2Lr=C4hDus4hDum4v3!;I2agAIT#oou`@92VP|02&CbBE
znVo@QF*^gpWOfFISat>mc6J7ar)&%i2iO=GcCaxpEM;S0sAgkeuwi3hkY{6H_{++`
zu#uI4VG%0>!$MXDhMBAk3=>%y7&=)Q7_wLy7#vs`7(THuFq~&$U^vdgz_5jdfuWs+
zfgzoRfx()EfkBalfq|Qaf#EGP1H(CH28Q*_3=GB03=C1s3=AI33=Fc&3=H3x7#NN)
zF)(anVqnN-VqkD$VqlPDVqlP9Vqg$qVqjonVqo~j$iT1zI-sDdP?VXTQKFESU#^f@
zqL5jvkXT$?nwy%UP?=w<UX-d(RGOEUnU}6ml95@gke{az8f2QPP@Jz&oS&P@po<C^
zit>~4OQ6dstQeG(lo%KkkU(*2PMThENq&JxVqQv4YLSLteqO4M0*KUvE6hw&$jdKL
z0IN+bN-x$c$jr-6PR_{8kI&D`skDMC1Q}dVl$lqeq2%W1>0_mkl9>Y0TAG}knxc@I
zr=+8hn^+N_S&~|mSdy8aSFEF;gy1M4>`cumPQ_|)G03RmVu)FpDB2Z~^k=3iWGdJ|
z+>C0Yg03N|!5~+cWag&o73ZX;7HELInNp<B{hJmWFT<NExQ9K%$`KESY(wsVLz9
zl7NOqZf0?DYKlTiYGO)GW?rg7Ql*j(*l-11hyznfP_4;KgNZBHfEA)T800PmTZPoT
z6p#TWiA5zSHiN`%6~I0M5gMpwgWQ!=l$w~0q9-#=0W9d2lbDWb8C+Pw7UC$7Z;SGC
zeDd=PG%``mE(W<p0lVF4`9%tu3YmEdMTvRosgO`cjTlXc#VM(2pd=1f0TluzTadfK
zDL%7UAtgUARiP|1QK2Lw72$5MhcZjli^0N3&Qvc}C`v6Z$j>XzOv=p3EU8o|$%klD
z040+Wg~a5H%+xYa0tM+#&ri%L)&rZCm{XjukO9sbU_<jtbCXhw6!Oy)5Q!F|G9@1r
z;^1tV4|2XjVu^yOfq_RAL|Ru@7i?frX&#sc`3TGac`LOdvji=cz=;r?cjKX%SFflf
z1-^1huecyJHN{J#EHx)SIX6W|!9dSYN5N3fKtUg5D{8Dj6)D&%<Q3>ur55EEYnUNa
zpv8-!fdMqoK`w(NVD;kEycCev3=Iqvz&2+UE2Nd?<bVPKY@kA7UWx+98<~0O>JYO*
z(TH5IfPAE*P@I{bSDLG%kXDqKo2qHWpbWCbp|~WmBr`cTwIm}y1sp1%-~q`fgUU5<
zltPr~C=?XsC*`E(Dir7El%+xxD=Xxrg5oVHRUtXQs3<kL1Qf01sS3%7c@VcKgY8Ys
zfktJKLPlaSC=FyJmZgH?wK!G5NFlK(y)-v9ucTN(Beg6wPoX3uzcf7qBwYzAbwMRy
zX>n?bjzUr?B)=(xEKn~|0Gk5xo<d@=LRw}~aS7Zk9fhRS<iyfquq_H;uOXGsU_Me9
zf%zb>gBcM2m#3=d<R}!UrYe+Yq~<A<rz)hUmVn%z32_8ORc0EBJCi|4JtehBAu}%{
zwJ5U$6nCH)DlSM(&P>e7tO5yuGgE$%LUBokLUCqpW=>*}4wA(s8L7po3W-Ij3W+&6
z3Lsa3Oh_(DO)N=ONY2R2Nl{47Nh~f-Emi;}p@PJslFa1NoWvppaI!7dV{k4lQOGYX
z0l5q!rVF+|Ei+jGq`DX^nGbfk0yL4sjMPD|$3UURkdqIp^-_y+70NP;GxPHp7+lLz
ziz-VpK;FqLRtOCOrOxEcw9I5Z1ymCll1qz<Qu9g_K<OwIRtJ`3B$g;-l@^yMq~t54
zRwO2u<Wwq@g9Iz{OBKo!^T3YJ1qF9XJ~V7Vo&^;<MX8B75O0AKe_3KtW`1cgC?rdB
zAXS?}c_uh$^U`xbDKj@WF)yVUY>KWzMrH~mO(f>!<yV5;pQa0{U5Y_@3RJ+Sf}QIa
z;0Y?c3QIGKQd6J_2$JAHu>x`j%;JJXP?jo5Eh<*XOHBlMB)=#<F)y<!H3jUz%sd59
zshOObS5lOiqmYxI40a{dJxTeQIjKbo1v%i@%})Wve12(O3OMqhMX5qgetv;Mc}8Y(
z2Dnv{Q=V7}DQk-pb5j{YGEx-^@-y>FKt6z^9%!r<gOr!R3Q{YE5KzM<EfX9nc_oRN
zdBqBe(4d6s0J}I*0mWjdlpX^^Nj}(FNvXv!yA%>jOEU6{)Qc4oAtk&X*g0vLd5Jj+
zDXE}RM<F>QKQlSC7@VYY6G6eBuaKCWoS&PUp91nV#L1vW6sXoLR>&_X$S(%P47fgE
z2uUmg<-F2jPzZyv1<Y}&dFh#Xsi4KG49OXwx~*8DJTnJW)_@8Zs2vb1GQl~wBqKGm
z2%b_E3X1ZxQj<#{E(RG73a!+Ng4E=a)D%$CD@sj-L}6)ON@`JY2`C?eVm>vmB(n&V
zvQko$5{pw|MGZrKS!z*Er9yH(XsvC9LSj;JNl{`lxU~fi*Nnt6khefaB!gqR7!=US
znV{@mtWc0&T%4Jdld8vntN^lb7gWH(<rO^h!1)-G>vbT_(Sp<>NQ9Q;D<qcXXM(B?
z23UhNA5^HODj;%>o&qS*6sIQV=Yg6>;AjF>Imwv?IjNvv04JKVR8Wkk=PRTY<>x9O
zQbl4;dVW!6Nk%TH5nPa1kXi%^Es(20=@H~7&>~|6gfeKF02R>CG=Z=TTn=RxE97J*
z6(tr`LM_kD%*)Kptb$}!P_j_SOHEBlO+hVObaffhQ}a?m*(xu;B()gQ%m%wQIX@>S
z6&m)Lxdr(}B?>92g{7$sP)=e|dO>1QaVi5?HO2~Qz0#7*96fM>npqg1UX)n?@+DMn
z5U6RNS?CTC#cM)(YDs)<eoAVN0#q+tfI(Ry#NWkVAvZA_;>&`><m|-sRE6^VqHLHe
zK@pRf1LGy<7gXZ4jRg0C8#S5vFlPmYxVU=yd*+p-7NsR7Lqi3urYJQzHM1-gSz!=F
z2%BnH-h}!VWUMn}%{fA~GE_1>wIH>mBwsH%zX+6#ipo-pAijh5a`ehFi%UV(Cfs^=
zh@LQ*co8gYKxqWV&MVC=s03x1yaJdI+(?jcL1rE-^ovsS5_2InZ9!3H8K^h{mFtN~
zIjP`=3Z!8Tu20nC)nU4lb5awFz_n;;US4W)YH=|*<${x0F+*Ys14AJP1A{22FD<~p
z0Gq#V<7Z&d<Y!>`&d0zolaGNRhmV25nU8_tD=!1XQeFmz3|<C?0A2=$XFLoH7kL;M
zw(u}8wD2%61n@90i19Em{NrX|xX#VM(8kTckjc%!;LXjzz|YOV@Rf^!VFwoj!zwNY
zhEOgBhX0%l3|l!F7@9a47~DA-7<4!p7{oal7{oXsZV~~xi(wRuhQMeDjE2By2#kin
zXb6mk0L?=HG++Y4MX4pFMR^Lopxzz06ATgrjpFEmg%oVjq?AG9J__IwgIw_Vd=j+1
zT$GxYS_JBygU2Cs6yPH^a8s2P{QN^)6*ARx!Gqx`si_5^HZ-IY1{uRIC@m_;FHVJP
z0XYVSLH#{g3lyR!RRQE`$RL`YLU4XjQKg=ObAB#Jwm3gOPY*f<2QvXiC#5Q+WmZ6X
zm5@QHOz4P}o`NH2s1ww?0d=AZit^Ko5_3Vrwc!3#P9~`5sStzWzM@1>m(ef3#1quf
z1Pz^~rnnXr<ritF2PBqcfQIP7{q)kJlr#m<KvO<wgb(CK&_D;cCk3(#VJJu#G6oED
zFsOf4qM-yHs>&>e_!<;Wps5grl+?7$ywnt{7|^T&IMN`aeK5^18a#rH;&PY_Sd(5!
zQDR;(WXuLKMw(bulvt?&3gY;j%wn{$d63@BG=)?JTU!OC{A?vu2ZLlm7~=3$><)rR
zDuMdGnRx}J;5i=9@M0#Ue+=*Yr{x#rCYI<agcMaOB&H{3=IJ3#aDZ%q;Z)Grd}Xm-
zYDIE<W?ou8+F&wBS!zXcYC#Eh$0{g*Ll*4W=*(CJTZOco{KOJO2t;SbV)qGXFbru>
zKPNR0p-EE{yKV4rRj>tHt5;l*lUbsnu7e}df;<NbXp9&vDMFii0{IhUx`M3&I9xT9
z0&-G8BO|Gx4kb7(mOw^vQu9)DQ`6E?lS}f8KqDTRpwSUnYOm0%(5cj`)Tz>|0u62E
zCYD$!papz+259suq^J~aj!GFckeCP>Bg;vJ440MWm1O2X#zr!WLE|N$nIPyebxtLU
zpOmpBM@%to&@etEiAEdf8Gy$A4RjQA4fG7rB1su;Z6avQr8qwi)Spx+PXy0@q~wEU
z7fbT<K{IldN#L$+PAVuDgTz7OfJG>-13N^)RsmFU>A{LH4d0Ud{2bJ@p$r=TgG`hZ
zgYzk@P${V_NL9#$%zuEQ3WOn}4^|3^DJkIjOK2HdP?VpPn3PkgP*hL}iUWoGqRiAh
z&@4fI9&DUT0X#1NDxN@*T%rK662;BR3Pq{8iJ5tjkywyY*f21Xc_=zSZh~e{@U&`(
zbAXkSj)GQRfgU8t!8HW(h=g8xYKdcVNoitEh;u+desQXXrlwvIXzWzOL=!b+K!(Gx
z7TBPa#F9k4eBaDyvsmO}2(Dj8L8~A$Pp_z;QZF~Lq$sl@BtNL25<E4ghpe|K9}+gm
zHiP>AYgri>dO-{51sE7$>+kRIGcc^+XJ9DcXJ9blXJ8QLXJEL&$H1_GkAWeVkAcC6
zkAZ=QkAdL=F9XA5UIvDK=sdp-F9QQVF9XA69tMW3JPZtTco-P6co-NQco-POc^DX&
zco-OVa5FIEax*ZvaWgPzb2Bhp<6>Z#z{S8&&&9yt#>K!O#>K$!o0EZI11AH+Bu)l~
zd`<?2AWjAb2~GxvHyjKM-5d-IW*iI*=h+z;8rT^alGqs-oY@%|l-U^=nAsT^?y*7q
zvmHvOvN14(voSD;vN14RW@TWQ&C0+qi<N<)l$C)Ynw5b;j1@9>-@?Mckj28l;LgIp
zpuobw@R^x`VLvkiLp3u4g8?%G13xnZ!$T$phP_M-3{6Z73;|3G4022i4Cfgc7*ZJ-
z7(y5s7?v=?rVSJnV96Gqc9az|Q?fz1EGMTDJVOLYok^)BC6HlB@FWqq9?Quu2M@z0
zl@=9crlx?CcX47`YDpzn7(95U0GjGZEdxze7NsT@=jTD@4M2%USs^zOlwA=s6rh21
z&`@(mYOX>?W_kwLnB>F)aDi6@8YBk~9Dyh96pF!BxjmA@LBoGmAhn?JU+{EKZhmoz
zLNa6|H>VP8W-6jE04H43nOg;0(2Q5H9;ztZ4v@3Ky1@A+GheSbwImocff)>Tj0Sj3
z0oZx*5a%JX4Rq-NSOt9W3~ki`*yzOM<kTGKq5_z6kj3DRfkj<%Mrv|4q&)|&Tagr$
zr@|)uiW4DoGob0k!ju%GqDLXKI8VJqAte(u-<VUWppjTmkO-Qx$*DxR9yC2x1fC5n
z)<brTf{m?$p`JN7qQG85k+f4V&@(`C4ygSHYda?AWEK>FnoyY~I-tT8<j}Ot3Wx}j
z9#HClCO%NhA{{hznwzSST$ET<sZd-}nwADv3=Y1O5(QfYLj`@v6n|*}XhtNyD6s_5
zA_Xf$l2EV(%`_`0dsLyC<mK<_7ZUI3qF}3F2A2WP<AU4H;6fNO6bqf0Ni9-H$xj7`
zCcLJ|$jr-DNK69F9u}7-XDB2gr+-j*6ldlogWBJj;JKyJyv)3k(#(>~vQ$v}8(f=!
zEXps*NG(!;jif4+<ijTMKsh)kvA9Gb3p`;3o1}%OFpvqw3ZUU&NFy6GTx(F00Z|T_
z(T6AMym+u7#b_QbMVjKtO-#>BhE`C;3W#C^ZU!`CDA+21hN_W9z%`T%jm-57^bGV2
z(V8IO_yFsJ4UI!IqmPOcV*+fr9AX0cNI7;BFdOS|-$3Un^gxqNps{QXO^_5AD=QSI
zmM9eJ6sG7DrljbARHhci7vvY~D1g`nAWmX&393k0Y7VHQkW!kPTd7c%m{XdHlpMg}
zfUZ};7Bvr{D#{1N6-pih%Y!mF_B9g9P<crA0KDP>c?ktn8kejBcohkxm`%$}FD*(1
zkJ)GBmxFu-p2Y;OW&oAJ@K7jA&B;#&mH(ioP)=$tQdR;7qyl(l1ZZslc>X#k6|$5e
zvsfW1RS(=%P0OqRHvknC6%|4222%1t1r}sdJ6|ECv>+!l88p!V>M(%%8JYQcii()l
zWaO8pmVuTMK&GAZ!0TL6K`zhCD@!dd$xH_s0-iL_Q~=G8=7aJx$SM#AlmQWC2(0Wa
z0ZjmB7Axc@m4PPBb1FfrWgxA%L{J;KBoj2Zo|;mcmy(#5T&Ym5SEN^>2M!-Z76r9`
z6!McmbEj}ugMtPeSI`6j%1|h?WJo<D*di;a()hHT#B^|p3@R|ehQRGYt8hTJD3@hc
z>7`^A7vv;XY7`nE%6zzFPJUuaSY~Q@YLPE!<_*baxRT7g%o2zaEV2r=3K-K&8sJGh
z9R<*|B6tY|WL8fHv>>81F$cm!a|>un3c9q-7)=Vx)R$f{Xbo3<aVlsY4Bb`Wg-x)b
zh}0s;ge-UtSpyVkpeaL8O_W%Yi)v9pVlg7ef_x9d_y!~h4{K0;03tID(vE?95v>Oc
zpEZRvS>VbLz6GZnXe5Ez7%>GFh3JGtmky-P1t(`)q#+h%L@a}oB}9(`*l@@yhCI-M
z4h%Pg1}5ARbC3p;lodc}5uy{GRlx2*N&C=798?TdVR2?IbXg0mafJ{;Rf(EuV5(6?
zQFVY?Ku~pH3DC;1_;^s>h>r(%b#%b70v%3JR>*}7zd@E;q$q$ptDrSs;1))4YHDt=
zLT)~&DO*rblwVMk32VoIn<0td8C8^-{lwh-Qpma$(E3d1+COE;${<kM1uuaDuMhyO
zi-9c@%SZ)Tk(Z)STmrJJ1l0ac1hq#Pzzd@k!1MFIkdw?HOK%mx1v9wc3R!>vZT_bv
z7MFn9>6t01iQtkIHlPMxqN4z6hv_lsf=hGI!URy20PY=wG99R$18%VArIzc0mye`@
z`pPLem7q0prJ#Wg(6X*Xa2FNSqb|(@mA2ruiQsevG6WP_pk_@XxZ?^gXkl#vq(wiF
zg^<Otg^tCbWjKi?pk^q80@$b$&=R$jqWl8T5JF}ic%4pWu>#2Dps)fhWmYIJN(3)r
zNGr_)uZsX_S12egVbBG$%8N2fQbDXD*a8mFx}@S%(DJ5|)SOC?U-F9}QBw@6n?Ox9
z@bFC{c$g(G9W?F;U5x@7aRDDOr;wNi>W3xf<S-~e1_N>v^FR&Z)PiCig`CXnRE3n>
z0#FDfm4erNDS)FQ6P#3_i|9ap0*!Zp!V0pu23&uFmPlphr57{kf`S!XvlW+s7Epkr
z8C*FhDnOQ3K~`54rKXfZd=5?~3<{8V1+N)ON>xa$C`rvrNlj76hqQ20i&8<$njpia
zAp0{*K<-3sQi9eafvp0KCYFFkcJ#pW|G$|a>(oK#|9L`()}Qe+Fr4RSV3@$qz>vnz
zz!1yNz!1vMz~IBrz~IEsz#zcS!0>>Nfnh!$1A_-21H(JeS^!=KhD*@(|DbjMp!NRR
zybKH<c^DWD^Dr<p@-Q%X@h~uG@Gvmk;$~o&&&|M)!p*=C#Ld7U$IZaNz|Fw$h>L+?
z5*GtQAQuCJDHj8S92WzF02c$pH%<nIr=Yd|oD2+iI2jl&aWXKR;ACLf$jQKv$H~9|
zS|{+EgMncM2Lr<b=ompdbeup0v@d|2fngRq1A_%S1H*mjI{zj%28Kj71_m)U28P$H
z3=9WY85q{FGB8xJGBCujGBDV&GBCVjVPIIx!obka!oX0*!oZNh!oc7QUDwad%D}Li
zg@K`ug@K`sg@GZ5g@Hkog@NH3GXujuW(I~*W(Ed#W(Ed!W(J1GObiTjm>3v>nHU&c
znHU(pFfuT#XJlYl$H>627P_V%8uTy<R{g=~f+8$^f<gsb1<>G6W?o5ZI;aQ$mxX%Z
z@*i4f6zV7xrYLAZD-g7SYUuDEsE#N}%myud%gIr&P)LOg@`2YG7N#gbI#Q4xZ*d7|
z-CAZI$XdwyesFmVawTSKA6$PFVOf)0m;zan3|4^Lv4uJseX+YTXenH3N|r`pijG2Y
za$-(mQG8BnVoE&d(8&12<YXPtvfcF3f_P9%CO$te9<*jJF)syC>q18fz&4`RG>8fu
z;w(rT9n_1~fC%U)=o-R85Hx%Nb_8fs1cZaILs>xsR5pM**2#%^kTyNITb)^=0OEt@
z43Jgl73hJi0hQPJ*^n|P2{bAQYM_G}Nf4jvfJzlm_ZPD4G_ypZI3vF_2ePmk6z~vN
zD%dI%rbJtr#lj;*89o*PZeoK<OsG~+(;L>2&IWBF0hdXMd6ngfi1B7+&@@$1Drg-l
z$T6TwB(<WXC{e)(WGM_o=g0~X&H>3lMZuwsB!wf16sCY34|h1o1h5!<NVO0tCP5>%
zi7C)UoS?8v1P=>?yBEc!;Kn{^D5DUx91ApHhE%MB3<me>!HdDcr6G9GEh)bwLm|Ha
zR1rd!DyOHGfLBBpf&yQ?7*dxcDp;gIracrub3Q1}1({r^2dcL6QZx+o&>H)MWuYyJ
zG;r=Jfusd+CsF}CKvABF)K&wTh8dC|QMe1h0S|4Z#h0a`4ZoA96jVx-7As^V=0Fx=
zrzaMGg55yRz{oTgT&b6*Du8zRfMzy|GZjknGE0gTKusTvAOabcssJ^>G#Sxl1vv?X
zAqw=3Oie*nl;nfEi<zK7wA|D@(1PxqR4n}jbnl||?Lh{EFt}}psB?2t(@GRd^FS;1
z!6U4ojG(Ik9TEfQ+q``6WCLgjFi)Yp2(-2w)D_0`78VC+K!YDN_#Y3QQvi)wfEywj
znxJhtpfUui3M8Wm8(T|3tlI}U8-$fXEeB91yAss2N-Zu)Ex>dIc$p?>oEE(Q1u|X+
z>Tto!0Z17RYR#qQ6_*x)vNd?14;0qPi8(p2_2VELK^WHm1vS6*pe{uemmn43Yzp>0
zxSWTMGlBdJneYfqLFyn9DuY3-`t;P2z!Y#b25roPwiSSE0yUe#YxDC`iV@r2Kx29E
z5e{%W9Fl7?5pzJEpuIrqC6Lw;xS(?k@dXDSG<ktaHrQASsNDo@ZRMsW=7DFYK<2~S
z)3Cu2@QQ0_w8Ezci@~`8+{8@-bxu-sVaxQBQz0w!K|?&?seUVk#FAW4D;nH(%gcw&
zxq(JRV8%j2TmjW_#h_)Nphd)>5l2vPLEH@5{s7rP1QLT3$DptS`xF#_&@#CYE`THm
zjzNSBX6XV^04k<Yw1XNTh(a2<ZUcqSXc>(*_X`Sfco_{&<gi=?%Kr-b3Pz@;nhIK=
zfk`7%L}-GvfiMMHLFFp$GF$^RUXH5}*My`mkON>CWEyfY4(f5i<RCOe1%YxL(_nBi
zgC=`$kl4aTJ3&PXmSPkoHG-@LSqvHR2X8b1RTm0Ll?s_9Afqu=gG3Q=03B8aEtNnk
zn=y4l3tRA__{<XUT6oY3mZH?$e9#(n)FPaSf)r*cj&hWYVicUf&`MFzDkEqO0S{)F
zuOW3^X0ZaOf&kSUp!y-PBo{ha2`UD&Q&YhmcTnXHDt;2dp#TmX&<HlDPn`;jF>s{<
znZ^NCEQtO&Xjr;DRXqh-BIkpe4d8Maw6O*><edm^h(ZcX=#UC{%2NlT6V#nY3Th+J
zM$24qHyl)wgJ)X64Kzsd0=39XGEyr+wtzgA2wu|xDtQ$^yI??jq(CtSTGasd2Y4w`
z31|;nG2ADh@u2d2g@U5Y;#|-+M93shu@1zEs3kAh5KzGjN{t{47636IX)Ql7Cq5Ol
z1PVHyr3YG$0uh0$gbad1I{lf&3duQ{*`OW}c-xu|Xe0#GCCJQENQAXk3UWYOAwZUZ
zu(ATgf}G6U%o2zjLBe4BA$=Bb1IZNJ{|D{y-^dR+|DOrk`+UmJzyKD5@<*xB5Eu=C
z(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4FNia0H_-e!jSD^RtlL48HwQ41)xJ#
zAj35ZMxae9;8`OD(7G%5paN3&A2i=)sAmrD|0}aHFnk9ckT1Z%06X9RKR*M*Z+-@b
z7yJwiSD<GA2Jka5aPc!R?B`=(n9Rq(ki^Hppuxw$pu)$%z{SVFu#K02p@o-$A()qe
zL4%ipfsvPi;RO!^!yz69hQ&M#3^hCq3?V!W4BxpK7|wAsFf?*AFxYc5Fo<w7FdXD!
zV3-X(^G}0|f#D1%14AY!1A{&%1H(-Y28Kl(3=G~J3=F~?3=Buv85mO785qRb85n-E
zF)*xRV_@)MgZN<v8v{cC8w0~XRtAQntPBiCSQ!{fSQ!{XSQ!|8vM?|lVqsu7$il#I
zfQ5l!2MYtk3Kj;287vG8Nh}NuJS+?hub3GajxsYa>}6(PSjEi1(7?>V;K<Ctpu)_+
zz{CtWpKm!61H&>V28MY|3=Gql7#RAP7#Q-Q^ZMTz85piGGBBKBWMJ6I$iUFU$iR@p
z$iOg(32pKow17@w^boWnq=iP1(Ynz?&_LOA^bj-!1@NdUga8*|SPwx1%YX_*$ni6J
z5K03==>>bbd-{by*OVaXN6f5%W(%OJr$BpKL5noNi{L=(^$I{MxIhPTLB_vf2izGH
zgU9-dL1WSIEf=6eoj}De%86^B(P;1v0`R_-EQOrh#C%992f0`ov>Fk#R0O;X0pvQ+
zpe{%cY-cTaOOA6%QI4)NXc+?J$SBa7JJ3OrpmnyOBR&xYi!x|gAb2?g=;%VwHahSi
zH)yE>XuDlvSz=~RVp0ysZYT!(ULid{A6$jxrGrN7!2|K2{eK{*fyVR|GIMiNQ$Xt@
zb8;#nW6zL1sGtc>&?0Q~)%Ku(0k5(I&$NSg^nq5xfVL0jLAODJm&rgTqd`lFAnWfy
zYXvI{Qo%<zs+T}FN`b=~X$DGJp*XQDHNF5eXOWVb3_4j4Jn|3mcSfp0b}D3#lp*M_
zHqaVLkl(>;L!d{a7K4wrLL?l}L=VKc#F9h>TZQUcxSX<ruCAQ|c!meEaua_1A9z**
zJTjkHTnwJ|C;}fwm6s12kuT2A0WYjXG7Pegsw5-77<6<JXbleNI6_d;0Ua3#TI2xT
z;{vv^I5iJ+CLr?8t;Ax`Nnqez*YJ}FkrNas%HRQ!lbV-ak^xzN4N6sT9gyS<QVWh=
z$T}cM$U&F2f|3Vh3c^YOb2makegSBmZz?FcLel`!E>&ev>jPw8vbMHDW-eqU8F-B<
zXcH;)04eYqRLC+;$Vq4U<#`IlB}JvlC7_8N&^!=$G78yqpr`=N<$wbRlu?U|Q;SN#
z3pYU4Wv5magVsoZw+J8wdM0@5G)M)=QrIz{#Ym@of(<MNZMgtV6M=S16%^%{fsZAF
z9~KE(d5!EC(0nOq(hsy;2b_OF(`DezK%hmL3ZRsknV$zbY6;{nWS!tlk%+7Uv|uwg
zKLvCyVrEIDda*)rVs<L>sgRk);Kc!;7z1S_PzC@m1p_Sy0Waz*Ps~G1H!8!H?}67M
zKvpDyil#&ba9N}Q2@tTO6BTkY^RiP@z_Vov;G^TfMMegwM95E5NX{=UDo#boedx~5
zOa-qJ1Rq!iUuFW~fmX(5q$+?H9w+DLm8Ir^_UA&5Z-f_BAkTxROhHZr#S-LjOyp7x
zeptf59&ZV99C+!42FOP_l_<dijuX%-znmOU`bUZ1{4|BU6!6Yq$np^AIt)+|iIh&^
z!426%11^RUF#<9_z91*Rq*wvG2L<J*1n9|}NQNaU6z75_HbJW)b;~pJQb7AjA?vmw
zOM^i<9JCNFvjlWNDP-|hQYvT>WeRAn7CgNR+wlhSIH=Ys$_E_@nO^`=ot#(%S}zLT
ze+N=gP?TDnnpdKa<O=Xw7s!He1yBY872Tj{2APh?5}@{u9=Ur9k!&j{N`;>Gpio+*
zfU@=(t_o}yXfYFX?iE!6>PgV4#R{NJ?BEN?K|EAtMWEe6up@UBY$2yo!uW8d$_kl~
z`EBrgKGbN?9CS&(0<sWX1K4e#*;&M~!BAIX+cOJVDuuQI0>xdR{XcN`>wq^$=zzi>
zsp`~G$jMKS2NhMCR?zJT&~1GnKcj3)fE<AgIuRGS7D<GZ=HMehz-<-qdj0(ToMJuD
z;jNj8IeH9`^#-7#3%sBXoUB2~1++Y<QWt#2CM;|~y9o3czzgkBgFy#e+Ch>~Nj}&Y
z3aKT@dJLYRJ-3O_5(SoAGfNaes}w-181i&sdvA52mgs^k(Je{^Z3jsKZ6iy}O$D8Y
z%HWv?S!fArZ=@81np@B_qLcFTvvolRf>JKh-UrBL3D^b%@IelG;4>&wOB52}I~b5O
zKwSV;8Jq*!p^{Unkd~O2oC!Ks6>^*k_<R-(&_Xt_eG1?p&CJgOFSdX-KfnchDfrw`
zurzXrYoZ@J32H$zV8$!t{7KOM|5`2vhQFW-`~(;nB%wnYp#J}Neg=le{0t0d_!$`L
z`571-_!$@^_!$`PK+gmW<YQo9;$vXg#LK`?3O(cBkC%bLjF*8yftP{d9uEV<Mji%+
zULFPp4;}^v1s(>5&D;zO?%WXj{($U++BZs#hQMeDjE2By2#kinXb6mkz-S1Jh5)TY
z0MZ3UBB5t`A)V-gegs@9WHb%FeGqc2AmVUBm|;+pJRl>><)Hp9cw7bCLjw)6LN|Ei
zRO%_Xf;Qr$f`@UyJMq9nm7pFHk~`ohFruCB0<sTkmnURQHZ4CVC%-&1FI`6gGLT{n
zKT-y?|F=S?Ql|>OTSW)lk$@hpZw$Wlph%}c2Q+?~o|9UPW*MQ)J0Qn^Fw{Yj`K5@V
z1<0iymEb@?K1vaOixu*zg~%3x`s#?I)e&2dz-P+AR?LIO7QiQ@fxFPK!-Ww0AVE81
z)AEbT6N^&3GxJixE4Yini{nvt%7Tp1K^&Y{h;%ZWvI1mi0el!CyvGjO`~tQTwBZK2
zJ|EOChp2VWPs|Ab9UTO=32hlZ^ccrv(8+_)acRie4d8oMAS0!q9;^an6c{|43*Q3>
z8U+Op-GaK+DWIK0pi4-g1I5T&8TF7(PXVb-&xa3}LJrY_oVWoV(*q9(mZVmcK+Z)3
zD+FzbLv*V_9pe0wM9?fiX))-SKpn`oNYJAC;?jcDq9n+m4tUrHw2cXriXpxNtz^$j
z2PbmSbzhlz3Ylfeps{Sw$t;k~j!*}IH_bxK0VO7oS3xcZdlzhGzCu}MD&%yu#Jt3u
zO6bK4plx~36KWtSA0A8KAqUumMp}Lm_)IA9xlno8pgwj|Y8v!t28G;w(5)BHRsA6A
za})DQ!5#*m6$u*f2H&s)y{rP^OwevK&=%6rAcd0T0@!&_c_|RD!OIJ9euE9r=Ry|d
zgNL<22RneA2)@e#bjcbxr+_vFfJzI{840O5prZ>?6+na7pmES5kPnczT7dlv>g=Op
zurOMJ1dD-oD1hhxi?|pVgh2c61sE6<p@r^zeg=jzeg+10eg=kpd<+bepy&SC^D!{U
z^D!{o;$>hs%M0oIZ{%fQm<m1rFO`>p!5upP&k0fky>=4+BFv^nL(29tH*h9tMV^
z+zbqJpm+ZjaWgRFaWgO^a5FFja6{b#b;~F<8UmvsFd71*Aut*OqaiRF0;3@?8UnNn
zfzg|lLH!=g^B<tS`q7(}!FMgw?Pg_V#2v(-3yl%WVnDG3I<paU#dj)rY8>3F0||pL
z3I`+r9;bjWbBoSSt&D}8Rs@<0FD{8M$&UxGG65|J0Lvp*NrDUjVP){n`@Gb01<(zl
zh?Al~av+S&l7jpK&^bHM;SrE37zUl=1loD8ke`#1T8za=(Apr-a=y$w@bQgkZbS?Q
zfechu&;T9B2O69Jclg04;N(G%B?29jr=tKGrvNRPf}EfOK1%?!$SfPwodz9{r4KrK
z0JJIxJZ_bgssO&6G8O6MDTqa&@eL~l$m%u3IyBG)?BD}+K%*X@MPK0YG|*_CI_NmB
z)I88|M{Z^@^g?2gQ6Q|W068yP0Tee1Mfo{7pfL%^k{y&t1m92sx_Vm&TVO!ea)4t6
zyrKg(GzMLC09w$YkqKQ03$iI5G}xsH?*Er?Ffh1)?%5MyV9<pYtatet7&h}WFy!+y
zFxc=jF#O_UU^u|Xz);A?z)%2P_kWF-fnhE$1H&9%1_mEq1_m8o$X$T5co-OF@-Q&W
z;9+2xz{9}Mz{9{$z{9{`#lyhxgqwlk5H|zETy6%2soV?<P23C&5!?(6lH3dop!EQ^
zxEL7vpmzb(aWOEIaWOFDaWOEYaWOEsb1^W;axpNR=VV}5!^yy~gp+|`A}0ew3Ma(<
zCLljR-9Ji=hQMeDjE2By2#kinXb6mk0NEh`YNFx6po8#{r9phqEh(UN*LolVso4u^
z%_7}{25u{(u5*RX5rU==i%RpbO&g*#ZObwf!P7+WmLa&k2%gLXwLy`Zez08+dC1!y
zz}i52kU-1UlQS|?%TiMyHxhy7ouE5G!1Hd<CEOrGAs1+Yw$vb2DMO~gQoxr!fEur$
z1<s@U`am<r;9X=MiJ%F+B5-t$?&}-f*Ov>K6sG#VKG1|P=#Y6tw_O=LnGWr#g6>I5
z$xkj;$j<{Wrw1Jv58V-%ssKL!5qxYZ^g1eNzZEiZ3^_{_RL(&*g+W%}gSJ3HO9s%!
z6!3A$pk5_t{|l(7NUcarE>QsQm`O`4&dAKq(?cD;02v4_1>qf1@UR1HSOWDDLGaK7
zL}6xf30NW0`V5d!Agm0YVJ}aG_27#^7pZ}_CO|i;WmbW=qCtu*kPa9It?+<cxe8g-
zkeR0dTATseh6TQ!4Rre&<QU*$$W^M~!WO0*t!xLcS^+y2(vgoZ&a6s>mhLbu%4qv4
zVVmk8BNUJwRLCwzztjk14wleUuvGx-MXPwA!ziesXa(;7gZ9@?1@)Z;7#K956(wx_
z|4V)bhWq>s3?2Lo42Jv+3^(~0820cnFl^ytV3^Ivz)%jo3lMY$z&X%-KQ9Brc3#L?
z0PA=e7#8y~FwEp-U~uAPV0g{Lz%ZGIfuW3tfguli7QkO_28Mmy3=GS-85m-@85s1r
z85rJkF)*CwVqj?DVqmc5Vqnnaf}Ht3os)rK8Ycq-*sV}^j#8r`Fd71*Aut*OqaiRF
z0;3@?OhN$Ewu51C8xOS77_^Fc^qwEk7WL74e&}`2588p}3dI?zd5L+M>7a`-(u(rI
z_gBETK!ayQA$LH7hOj{A<v?%p09`=^?f!t49fOiK_+B%GJkV7Z$ammDI<Mdj6R?Hn
z;4v@IL~VMpLP}~uY99E64Nwmgz877gJR>tX13clH3%N@VeEW=oMn)?1f;7llYv@XQ
z&~9q*@iLID`|wMWq073#w+ShLX1SsF7v>i!B;_Q6Fz9|!$mIf%o-d?l47=l_1hUM%
z7_#platIG76rj!qjg5gj->IP4X3%1I&{-oeOA{5Kwq@q$L9SB(FXRSyGeL#~LhJ&C
zA6N_&h|rTH%8OF-k~2^{*SPP!RB{1d@(Njoo|y}|rWJHh4(J4YkPnb<S3%yEfw?9G
zVlU*nEb#dj;4v;e@aYCcsmZCCWvQTB8z2|Kgg6JdffnCuAa&c3cY44V$s=8)3Q9)l
zsU_iH|A7r3O-1CVB6vuG(+@Zyf%gA{&eS{10XhF)6<UE_;Adc1#?Qde#?QbI$<M%G
z&d<Q`k&l64H6H^*J|6=^5FZ1BC?5mEYhDJ1!@LX(TX`86ig_6r40#zCe)2Fd{NQ0=
zxXi=Au#<;@p@fHlL6?Vt;So0j!wGH%hNavL40YTL41wGX46@t|3^$>3{<&NX4E9_M
z4ANW-3=cRN7}jwzFw}4|Fz9kJFo4{*3FJno`$nnJ5Eu=C(GVC7fzc2c4S|sq0-$Cu
z2t#I~tRU@rh(KmaHn>%vQwcfY5IhqOz0DE4v9b(2MOLf;niK-fUL}<l6=g!N*9Oh+
zfG!KoOM!F>6ms(O(o@SaODaKT7U$<dw#q_m%1x|Hf^69aoxBG>%MN@-WJYFs26z@K
zIk5mdJzoUg+?iRD0lL8{HAMlk(bXQ@2!j|0Sx{P%ky;G8Iy_Y&F()^_xI`g2wWuU9
zGcTu70c2(>q-O=v0Ui=ST{?j0q%e>U5C-c4x6L#2^@>wVf(uepQ-X6cQ&Ni{SJi`0
zDUF9X57G|<X@j=WAyZnJd5}AQk=p1WHDIHWuknRB2U!fm!^s(`$=RSewGxHI^u)|O
zBn6P8K|u?4ixVqBEnaY37N(@Yj~E1>VhB1y6L!Lvf<|INL1IyAUP(?R!u8PUOt9;b
z9iw1lt6->S4vr{HNjn7tJp&}?fD#M*sMnm#f&z$>;2p=b%nFDIk{-~mWoY69`2&3V
zS#GLAa#3Ox=u+|0v^2P4aPX0P0-l1hM-{3`unq7EwhCr&8SvGoB^imJK58nw%MQI^
zzDNOdy)Y;=A%}s2E^W`u%T`DPpYB&&nw+7Kgq;3C;Q>3$6w)0o&CARyDa|a&EK3F5
z$O&F24(g(VH?Av`B^G6ZHjFEj<mV%~2z(wT<hnLkXCK}R1es6_K2Q_3o4Q!Rpd<rw
z@fu?K794bW@nA!W(L7vQ1j=#XJJV2)15`lmDuc~{W();eg`f}@S5JS>ypq(Sw8Z38
z4JAV(b3Fq+13g2u0}&AVpq*W)<{(Jdmn0LQi*=wTK#yx9U;<``9G(}USNrKDCYNNE
zC6=Tj%?5+ifH3InUr5}6MmF-3!F%CBcfi6U2&4*x!TyAqn4F)NmzrFnp=4-jq-SVm
zp=YRPXketIqhMxiY-)tacp#0LY2b4QVb?37F4zZ~iZ9er%ttjCp_gFD6J-j)(8pm)
zY7TPgh#0@Y;#1fNZeD6>Nl{`BcrXKU`UJvjuuCH0nE+cci?9V&$Rb4GQ3pDNH?t%&
YF(<Ps6<o$aqYYdkfd-MmNxPT<08AwO7XSbN
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index f0e52da6259b072f7fdf395595c1bd877a6aea0b..bf5a97e07d2695543ee14d691c26e196a331187f 100644
GIT binary patch
delta 107
zcmdnNbcTsHpO=@5fq{V`xhEoZ>O@{yMvaNulG3@XQLKy%=?p21ix{KWQka4nG?`z5
zlxea~j8^5l#aWhFT$-4ZS(WOi$v&}JUA%~mfq~%`dwhIiPELIMN`@j%1_p*A&WU>}
JSY#M@7y(yc8Yln&
delta 87
zcmX@Zw1bH^pO=@5fq{X+Zbf)%?L=N#MwN-$lH!aE=?p21ix{I=Q<#DoG?`z5lxVU{
pj8|p$(`1`CNnPj`dwhIiPELIMN`@j11_p*Aj)@m4SR@#D7y(x46a)YO
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..82676cbf4c37426f13dd04a2b2699f68f9ee3f24
GIT binary patch
literal 6218
zcmd1j<>g{vU|?ubj!ez+Wng#=;vi#Y1_lNP1_p-Wbqov)DGVu$ISf&ZV45k48BDW8
zv7|7hFy*l3vPH2mg48hQu;+3_ae&z@Ih?s%QCwg)YYulVPZSTB&6dNP%NNB5W;5pS
zNAZKr5{ME=VMt-m5zG~e5(3L1tK-NK&J~FgVPxQB;AC)TNa0N3YGFv>YG#TO1@pO6
zcv=`zc$%4_#8Mc888mrcf_&nq$#_f9J+&aUq$EEqv$!-dC$lQGNR#oFNKlB2t8;!{
zNl|`|XI@EaQCebhDoi{mH90l2EEQRtAEe&XA6bYmC_gE`B(u=HD6^mdY$#`XYDs)<
zeoAVNCgUyEl+?n~)MSt=kTDZ1Of?u77*fIE8O4;sn8MV;5XH>Nkiwk8(!vnM!pV@r
zn!?t?5XG9p9?YQ0aZ3p5SfBj-0^h{E#PrmnWHuy&IT#oiK-d`+Up)*A3^mMI3^fe#
z3^j~34DpOLOf?MgOf}3k4Drk<AQ6@nrW%GU7HN<g)=Ut|R>R`L5Gz~DR>I!QP{WYL
zR>Nk)P{L8eoW<G9=)w@oU&{^=m1J1JRl>c1r-r45O_E_D<3dJ8hC+=(=^EA=hIrl*
zz6E?Gd|CW83|RsTnLzOv%%I8a_woxP1H(&31_lOA!CQQ}i52mgC8<S;C7Jno#kY8i
zKtUOw3-Q-2?qo>Bfc$!kC9x>I_!f6*K}uptYJ5>*N$M@8l#*MV1&PHa@hOQViA5l9
z-(oFF%u7$bB>)yK&&*57FOM(ItV+GbmRL}bnwN5mB{{#K@)k!*YH9(5m6M+ypOTqe
za*M4vGd(Xc=N4PAr@N<L$Sv;TlKcXX#JrT8)FMreTP($?IcY__3=9lKd?13Kfq~%`
zUvXktYCPC7kQqf1AVKEr)XH0onJXEJ6c`v7ekJH<<maa9r{@<J<fkMSfq6y<Zcb@l
zihe;+Vnt@LerZvBab|8oPHH^V>-q(iB^mj7SR|l<RICT`O?-T2US>&rygew^Wk5-Q
zk&TIsk&Tg!iHDI*l!Jwbk&j7+ndvVZmk4W>G-evogPD*F%IvW8#LB?H07_io#AL(3
zz)-@F!r08n$WX$V!URqX0=3K~ObeJ(m=`kEvVam&7V84G8kQ866xM~zj0}YwphN_X
zd^W!--Qv`|l+3(z1w#V^h2nzL)Re4Zg|yO~9EJQeg{st|{9=W~ycC7hip-MCy!4kT
z3=9k}K@LctH4_AO+Nb|n`SFr(Qi8oE$1T=^qRhOKB4JRFGvyWBVg(xjj-gvTMI|Zl
zK)l5Uw#w@k3&>zi)*?^{L87}f_ZDkfQDSatkvzy4_OjHR_~hIaaKu3fMNnKzGcYjJ
zfx;G4P;xNxFmf@9Fmf?g$zcgeq*Rj(O6Rb^<U|h42jIZ0VaQ?x2ct(VV+qp&riBc(
zOf?J(m_d0pi)A5G40A1WElVvcI7bTCvVj5^l-CxpE@WT?<xOzT0|hup6rBIWYnY4l
z85#2Aq2{sIu-7n2Fw}C?Fl4dUaAY$Sxt4IGFxG(i5)92uj0}aWB}@xAA@ULoU{hGZ
zE?`Vyl3>VYD2gfJNMWvFOkrteW@N}?E8$3CEn!Sy1MzAYvbeIii}Pw27VxAnq%gp8
zdJ01@gC@IQl~8VGadB#jLP~04N={~8szOp_5jO(^LlswXX>xLEaj`;XUKKZ@2vDpN
zcXRagu~JCMOi{?oFHrz1OHs(oyTyf2rO6A9^&%cnoUxQ-=B7fjq!5VDUXWjqlbTji
zBnpZ-K@gz|5@#*WNlh&%f@Ir*%)I>M<c!Sx`24(_$|6wwYI1_p1tcM`7MCOzmE7V?
zDoRbvcFRdjza^ZLSdv;?5?_)Z56<(T?0$<6RCuK3l@wKii;yBYkVVYkVkRXsEe(=_
zK!rdNC>h;iE-KCI1tlX;q0h(2!6?Ga!NSKR#>B$P!pOnM!N|oZz{tcX#KgzQ$H>FP
z!6?AU^1n(9OCl>O&GXY_yTuV7pO=`M8Xtd)D;`wil;(igJn`{`rHMHZnIa`n2!K3T
zWC>z{U4tM%?keJBU|_HVxeFAa91I+cOlBOMOzb=yU>V$Xmm86F7bimsdkRMjLlhe)
zw8oBN=VV9$*VR!RDZIf9ntZo-!1XAsP6O4Cu$%*8gNh<>G33L@0IACuY8bOXH5g+J
zV-}=VW6T28Yb+(KHH=wo&5SWDH4O3WH4O0_H4O2bH4O1wAn_vQ5^hi-*2D<PO+~t(
zJXow-!UL-7;+f%QF=R0o*_7}u-~;80g&=$Q^9(_<#c?G9pu(?)AzrXV2+9&J5rMKq
zYZ$Ybi(+dSvX~c$r7)#1gK`9zCl1Q9dHf{;DJ;z(^FX>GERb#pOEd*kcS?Xs$rQF4
zhIpwIP;Dy(uAilA7_($jII<aw+Q2N%Y?k5<FNP*i2*}hh#LIv}q<C5lLzd_Q*&4<y
zIZ$4%Va$@RVThMc0fmA>3Rew7ykZSwmQoEvyb?H<a{E=~2PBqcfU=@ONk*zdX;Dg=
zLVl4#aei)UNd~C2PR%J!Rme+CO;ISxSHK36QWerND<JZv#i<G<8JWcjX_-aEC3*_M
z`9(#QdJ4|@xgev8^Yiocs(1}8jr0u7Ec6WZ3=NDjE50)@RB;*_nd=$o8R!`nselqH
zsHg>(73s!c@Z0`5EJ3707!T|<mBIPB$Ox40xQa7#OLM?YiCgUHkk;5O9w;L|EhjO(
zh#jPkvkcY(yu}J`rQPCB&d<roEY8f&17)JroLf8)B~Ts<sCjWq*e}1t6I3_nrskET
zrnnXr<rfu!DyLh_1(|ub*z;0LONtV6ZZQ@Xf$GpAP#!O00a*yCjEW$w$s$lnzQqo%
zpEC1ru_x!}<)tQ<+~P<~F3BtdHETHX;<NHI^Gb?uad`QA`h~=Mx)ce3LIhNn-Qr11
zPEO5%Hkoem6{nU2gX+=XoXnKeqFX%Rnl8Q=%!8DV?4U+gVM@v^HVESuS9*S8PJAlJ
z{hCaWG7nTJq7_y}pmO5~s8H|#Rb1fWiGz`YMd&{p69*#?7Y8E;4;!NZBO4<Z7_u?)
zF>*06HE=P3_-xE<ENqNCj1tTo%pe&KMm82UCLZQ0QRG~XXmuc0l%VVm3O8_euL3n;
z8L}9Ptw0$STt$L1E^`)3ky;50s3NRkh-a%|TmZ@@Y$=S=3@J<^;7SaeSRNNh6x3Q_
zWI(V=I2Lf$FfL@QWdv1wpkg8zRJn2mGib8-z5LI>zyNK3%(O286_c9W-~?CX4T@Z@
z^wbi^<dV|FoIprg1of0|iGaEQ$)!a_sd**w1^LC9pa$+OZdhV0$S;OePii0|SQCqi
z5-W?mK#gK(gn-y+5ps(qJ+&lIl97R-7}UxHg*yi$7mEms7$X-m7b6!l8zT!d+y5#-
zln@5#L1~77LKl=i!J(T53ta<@(3Jsq4_Hgsz#+>DDoT*qd0Ze~3b@ga#KIktOn#VQ
z$BhWP6r{j|2UA&U4mea0VO10j3NWU^6j;iQ2W1S@>g*OP$kQp-s3FD1%EQ8k8d5?S
zA(es=RPcld$__Q4gveXMxB%2=VqU<qkO7itf*DF!A$5u-lV6pNOMaeui9$wVS*il4
zZ2~GZ!Hs%Q!2u~6i!#$QN)+<)%k?zbAZZDd5Wt>;q$)vp$b>itxaAimrxv**lE*E!
z@}ktd<P7u_0_u2%gJohto(FY^Kv|T9h2vk97`kU63X?(U0aBbYFo4nps6+#&jW|%+
z0C)3SnIsu%nMxQIFxD`EyNgUBp!~tKkQvlm&0;QA09B<6SV5i3EKnnr9n|&*RW9J1
z#gN5S!;r-d%3E3NA`A;bl{7a<R+HHeT-3k(|NsAgO`al97~En>&dDq&3I#<gs8oj(
zqnaRYAcz2!fkmJa=@w^ZUP)>?s3TYeaxXZcXfi{}Gf=9y#a5Vtr7eGp4b%q-Ovwd>
z5U7sfVHRU%W0YW$W3CcL2_uL)loSC9QBWNMvamP^)DmG>z_5^^mJyaFKmkz#>H;w?
zWCR5Qh@S;2nm9l~Si_LTnZ;GS0MY_s1O*~babyWMsO`YWP{N(U4DMWUr?8}h3OSZ6
z_7ql0hAiF%d?{=T85c2@FlO;D5P+BomJwXY2$Kt9=wkqz!Jf_tb^$|{&;nsl3Pn>P
zvOu(kX(3}RGbcldSPEw|V=YUSR0<cUm$5)Rg?k}mEnA61iDV61Go)|ARm)x?Rl`=p
zF3Hf$2%>Em3Jb#-@&p(eK(Iu5fy_dNTGkTo6ds7{Ygj>aBts2r3RgB$(M+&w*cLL@
zFvs)eF&4&`aHsIrFoSv?c`PNeDSXY0O^o0X3Aq~PU<OTozbgI6{8EMF#5@I1;~dma
zOwLTJ1h-8}iW2jRA$7b0A`?{wd**?q)ADn2^2;;x(sdxM4+Ud|ywcpH)S_aA#G+J%
z3Y|)wDg{_^tOM?A7N;iX=cOnZgB29%6zEhcB<7{(q!w2Nd88JlsuwGiXQUQ^nlz~j
z;Nb~{%wh#-$(~cGr{J1bTw0W>P?=w<ke!!bu27zlSORuSQettc0=RDp(gA4<Rp}y$
zrsk#SrlzH(CYR(FfgA-=ou8`!@0wU~L2?JE2&&=?4Khtt2yqUu3aRo8$Vmkm4r+%K
zf$b_uEJ{x;Q9w5lT<a)gLc*p(uR^C%uTrN<uSy{;zbH4c#7f~7Q-1a@Mx83-kc?D?
z%)Elq5|CR#{Vu4c)QW=C<dW1Bs17}akfKV3#Pr0>JiRJ;a4RpfSOL_YfOr5D3Mr{+
znR%%xR$NubY1yoF%qh-?w@C|%^3#hFb75YEG=GY*wN$}z4r$$GR)HF%pcXIK1K_B_
z+FaF?FNy(`J>X(TFfG5RJh3RnJ2NjeH?br$xfoLXfciq<5)535-(t@9%`6H4=>oT{
zoIos4Hxyjj-{M9}$G2FE@=NnlZZQ`XRNmswO)M$OtO&^u0uAdF<(Gh)vyeJL0J%;G
z$S+PU5(Akk4yqAZ!I5{1wYVTB6FMZqoRgY&i#07LKe6N%b8%(yEsoTR<oL|Iw0v+?
za*Nd$<ZVq3NQnt*aNJ_`Ey>T%DFU^*Zn1!5Z!xCc;si0`b25udia=u|=yeW1B);AA
z6LSJUt=2A3xe5vl4n_$kP#>3rkqz8<WMdR!Wcn|~$njr^iHn7ag@ciakp<l26=Gxo
zH!4B>U(nzR6C=}S9zG5xHdYqKDjAeA8O<_3P0k`KP#{=?2+#->xY-Jct^|-6sPkHs
z0b<pI2t-Q(%mVj*Z5bFC4uBfEpk@^Z0|yI}4+kfcBnKyx6bA<r2L}g}01s~wNR_7e
zElzm$gw#T;d8N4pm5_+n2gQUiT!9`enWYwiDp^QP2C8Dg(FyJv6#0N01?~~3VT}Fh
zm6l}Y=z+_e%))qR)3FFtKSLs35@ekM4sD?JC%AJ{1gft<L#a7AsmY*LXfb%86x_21
zdkQH+;bC%%!zMRBr8Fni4%9F&2KC1{m_P#*Jd8X{Jd7X+=7HHvOiT)V+)RZ+f&g7T
BGDiRa
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
index 864b03fab06bf205414a942d9890f8385c8f24fb..44e6abddd15938267cc7d739954e6a98cbf46ee0 100644
GIT binary patch
delta 1245
zcmZo@U~E{xD3)Xp=IN_op=ZRvz`!8Dz!0Y%nYuP;qu3jMerASx1_lNukOTt*1J`6h
z0dY+hD9;K?b3tiFD9sF|12-EAtmmJkz{0D~$G}j`2Qi#$vY>);Jqter!%?UxGd}~v
zZ9WEu*?bHPUVID;UwIiAmhmz$gzz#jnD8<%eCJ_cn9ak$Fr9~ip`C|;!GMQ>;SM(g
z!whZ)1`loqhCf^k3@f-87=pPN7+AO%7^ZVFFnDq@FtBnmFr4RLU})rEVDRK%V0h2Y
zz_6X2fgy;Up`PIb8w0~8HU@@jHU<WJHU@@2tPBi?Ss56*SQ!}XSQ!}9SQ!}JvM?}g
zWMN<kU}0d8W?^8s&dk6tkC}mCE;9o|9Ww(%C^G{CGcyCj3MK}I8B7ce)0r3;{FxXS
z*qIm@_A@dtbTBe7s6azSK|w*GFh#*u!Ambh!C|wlVkpbx>l$*KH>j#JPoAVIxB06^
zCu_a3LP<tuu|j6CLVA9FijG27X>o}{UVe!}acV(gQDRAIib6?7szP~Ur9x(jLUBfZ
zX-<klQmT$ZPG)whLP~A{NNZYtkwR)kVs1fBsvd)~LQ-joLQ1|uZen(-LSk`oYEemM
zeqOOcNk(FcLUL+RNn&Q6LSj*RX>Mv>d`YoFMq*j2LP@?tYDH#o2}pm4zl*;@Zhl#+
zLSk}BX<|-Jr9x?OW?nkPsT#Q;{b`vwsS1gCDKLXdia|ys=E2;d08*f-H#ty$Gw)_b
z1_ohJ8WCV%2%Icvu)JQKi-F-KCj-MeP6mb<oD2*NoD2-!oD2+RoD2*MoD2+CI2afv
zb1*Pua4;|!aWF7^XJ=q|z|O!hjh%tPlbwM<m7Rg%FB=2HRW=5O18fWoC2R}~;%p2I
zOl%AcXIU8-Cb2Rwgt9U)D6ujy++|^4ILN}lu!MzyVKEB>!y*<2b%r(;28IF_28KWu
z1_oUg28RF43=Dgi85oL~85p#fA<h+I-uQ3}BcsCRI^&6qn*~hGS?hHa3KEM;;!_e!
z5;d(DlocQ$1qx?yEG8;smnRmbE94g`lxHNCq?V-?fkGuIRUtROC{-aRu|lCdvm^r?
zjtt6RvlKvPffOa@=N9Bt!sJsG90NQVlofLGQ%Z9Zi!w_p6)N)?Hb+@4W35k2VPLqy
zz`$?^l+Xki7+|^bKR*M*Z+-@b-TVv;lld7Kg83O3nD`kO*77kh#PKmO@bED(yy0bF
zSkKGA5X{TK;LppzV9(3I@PdbdVG0ieLjVs00|O5O!xnA^h6HW~25xQ!hCN&i4E|gU
z42)b140|~l7}_}*>KQCK85kHj85ovxFfinEFfed&FfgoOXJE);XJ9a9XJGiw#=x)z
zn$w)v7#O~>GBDg@Wnfs!%D|Ax%D}+E%D`}$g@K`;g@GZAg@GZIg@M70g@J*Wg@Iu=
wGXp~cGXp~aGXsM^GXn!VGXukZCI*IbCI$uvCXlBmKD@VCUojaYGsUq10AGs&pa1{>
delta 599
zcmZozz}V2hD3)Xp=IN_op=ZRvz`!8Dz)<=jBGoH&qu3jMekO)`1_lNukOTt*gTrJ&
z0dY=dC@)}hpul?mi60auDX{PgGcYjdGeYz`Ocqp7uJ_|-VBq6tV0gyIz_5&ufgzNS
zfq{XKfng&r14BG71A{#;0|OH;1H&>N28KmE3=ETa7#OU17#N;&GcYXXW?%^BW?*3F
zW?<ON#lR59#lXPN#lWzLlYt?GlYv2ilY!wD2LnS72LnS02Lr<|b_Rw+><kRC><kRQ
z*%%o1vN6;%w6QTTc(E}su(L5RoMUBRn99n);K|CsV93hA@PmbcVGj!fLo^EmgBlA1
z!y{$}hLy|=3@ex!7&@657!sHn82FeO7&bF8Ff3+bU|7V&z!1g6z#zoLz;KF@fnhQu
z#JkEMFGIsa0R?Qn@0h?c`HY6#=2$m%=E?SMa+_aibh1v?_P;tgSbrn05)%VM9VjRT
z7#IR33mPo1m*QezVBun5c*DuSu$>d)*&I#=1_w?ChW{K43|BZ980K*>FeGy@F!*pV
zFfeg2Fl=RKV3^3xz>vw#z~IQvz@Wp<!0>>LfuWg=fgz8Lfx(iEf#Dx31H(a928KSU
z|8-dz7z9`u82DKk82DHj7<gG37(TKv$TQrA2FeT;28Jva1_pH&28O%L3=C75K~CNH
sP?Sj+<i(u){DS!0#Jt4x)FQp2(#^ug6B#!zHnCu3(zM#_X|<gd05{fR3IG5A
diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
index 80ba7275dae3fbd9f50fa6b4ef4c35cbf0713b9d..2712234ef37d6e72b70c659ecaa1c192a007dda8 100644
GIT binary patch
delta 1629
zcmZo@U~E{xD4b*v=IN_op=ZRvz`!8Dz_3I+a-;AyX4cY@G+m3$@0dk+_?a2%85kIt
zKnfTb7{n$E3Wy7`KzY7UT4=MRz;XUb3M{;jxfvM3c_4;~O%_xzuRqGiz);J_z+lS9
z!0?urfnf<R14BG70|P%V1H(=p28MPXh>kP73=DfgV!R9tb9fjSI(Zlv%6J$Uf_NAh
zba@yU-f=T99N}hQILyt!aEP0Mp^=+`A%>fQL57=w;RP21!x}CIhSgjQ40E^`7*e?y
z7}U5J7(Q__)HCekWMJszWMDAoWMDAkWMI(bWMKHo!N9PYgMp!ugMlHJgMq<<gMopY
zgMr~KI|IXGb_Rwu><kPw><kQ^><kP-><kQ#*ccervoSDKu`w_NurV;GvN14xU}a!9
z#>&7jo0WlK7Apfo8Y=^XEh__qFe?Lt5Gw<NAS(ld0Bb!113N1N!vhuuhTSX-47*qu
z7}l^bFwAFRU?^f?U~pq$V31^CV31&8V0g{Uz;J<?fnfnN1H%+%1_mEy1_l;p28KOM
z3=F%O7#Px+7#MV!7#NtD7#OZHGB8|WWMF7xVqhp`VqgeiVqo~o$iVQ9k%8eABQ#n-
z)a0Xz{F{#|Hgnadq^2q4CKedE1SFPZgyb8$1mq;<rD_x;mSn^mx#%dAXQq^7=qO~Q
zW~OJ9Xj(BSD?mVgUUI5JW{E<1W=@VmVsc4oVopw_LQ<+iUTSJeYKlT?PH}2^Mru(i
zgR+93e~7D<LP<tuu|h7$7=^@Qh4R##9EGIRdWFn9g_4X^h0>DD9EG&ZoKzi!q|y?F
z@{Gii)Uwnfg|z%4g}nT79k^XdsR}9ic_3p${9XLPic2yQOVo=M^72a*5*3O-Ht1#~
z=7C(1R+N~Vs*sbLn6IOdoROH9o~i&+tN?Zy$jJKQjQrA^6on*^fr)u3Ah+ZxWP*)L
zEGaEY%uy)GSI9^#18GcEs8UF*$ShVU$j{6x$;?Yv$S*BX$WH@lQb@{AEJ|TeR;bXe
zR46V<EGhx}EWbcECpE1^AvwP&FSSTPBQFsYQea<|rxur_iWlW6<Rq44<|z~>7iAWd
zD3ljvf=r%#&@z5<jlRreVLidg6Es;STPu5QKB{LeSf7}}z~IHqz%UDx-2@mIV7cu9
zKLf))eg=jm{0t0D{0t27{0s~-{0t1A_!t<D^D!{Y;$vXw;$vXw<YQpy;A3EL=VM?{
z;$vWV#LK|2j+cR<h?jw(ke7iWiI;&vo0oy%4G#mu4ju-E79Iu$e;x*gzuXKAf4CXy
z8Qya<Fzn}MV5sC~U~uPVU@+!pVED(yz;Ka^fnhTj1H(!#28JRo1_mcC1_pjE28J7)
z3=Atc85l}A85kTm85qPk85mx3FfeT6U|{IsU|>k(U|>k#U|`VUU|?Y8U|_h(&cJYk
zoq^#xI|IWtb_RyC><kRc*clia*%=rb*i#r7ir5(#QrQ_8EZ7+sc-R>j?z1s4++$;4
z*vQ7fFpZ6YA&rfJA&QNGL5Pik;S?(aLlY|lLnA8#gAOYL!)F!-hLbD|46|7vAsENP
zz#z-Q!0?xufx&=<fkB^zf#Esx#)oZ8qPeNX47sTcxv9k&@hO=_@rg;r`8ku1Dk@Aq
zs^|!!#gOEXn49-2e&ehM<&vV*lG36)1yEjM0P#VLXe%AVSOr@JT|)&euna^%TOqM1
zy;u*D&A^J46><}^!C5RlH7^yE5J6cXQ6a6gq_ikip(r&cHL*AqsuY|Rz!@yFSRqj%
zB~?5(U!k}lH90daGg%=cF*#dD0jzZLeQkRdkXt6p=}2TESqF8S0my9#wiYD+gS9Fv
x6elO<K=J`d1t??Wf@MIwcw-j@TZOy=y^P|L#N=!Za0#MQrKt&4G5M{I3;;z}d`tiU
delta 912
zcmZozz}V2hD4b*v=IN_op=ZRvz`!8D!0^!}c%$$&=7|Zco8K{u@bEJ+)H5(JFo9$l
z7#LC}3kryD_7K>?Kk<XYBn1{;Nd^Xnql^%(DU$^i%<C8NF)-xwF)-xuF)*m{F)-ZY
zWnh@b%fK*|mw};xmw~~Rmw|zkmx19e4+Fy`9tMVsJPZt5c^DXac^DX~co-Pcc^DX+
zc^DYvco-P&b2Bh(<z`^m!p*?2nVW&3h?{}Imz#lsmz#m%CKm(4TrLKNIa~}3y<7|o
zVO$Ih;#>^%439Y(7}j$#FjR3eFlcZxFsO4fFi3GSFudYmU|7Pzz)-}&z!1m5z@W*&
z!0?Bif#DuI1H%<|28Oxp3=FyK3=CH63=GWd3=Egq7#J3?F)(DaF)%o?F))a+F)%!2
zWnkFO%D~XW%D~Xg%D@oL%D|w<%D}+F%D}+P%D}+Hn!>=q$jZR*gN1?NA`1h<8WskI
z)hrAQb6FS|Ca^FtB(pFun6of2aIr8jaI!Ek++k*5ILOSvFp-&op_Q3|!IqhU;VTmZ
z!&)W=hBZtK4B<=+4022i3||--7>+SQ!ukl~#D_AIk16s^&UF!-Z0n-9`L@e?&dEo#
zC)Zo)7{n^rDikD^WJH76S_(<|iA5>#<(VlZ84M71MrvkyMu~#0f`Oh1R0u2$7BMzu
zP*x~URmjUPS13+S%t=)!$;d2LNGvEQ$}cF&Oe{&w$*EK*$ydl_P*y0(PsvxvO-(E=
zElO2LN-e1`Ni9;yO{`2xWncigLBUobuRt%OxFj(-TSEgRqEn@*$xsCmuSzY-FV@h=
zNzDUGY3dkiY9eV%Pt8j!%FivSDA7>Y(+6pe&&-Q2$gD`sDb`CaE>qW0NJ-7f%*`xG
zEwWYDQP*TB@l&u>(8w#$ODsyvOHb7>G%(Oq&{r@t(9|nREzU?RNYwy|f$dRN$Vn|x
zFIGrS%*j#6EKx8tP$*7K&d*CJ)=?<WNGwS$OD&q*;2J%-L|<icnx5cf0XMI329Sdk
z(lg6a<8xEvOEOa9lM{0?lZp~cQd8n{^HWlD6f$!Q@{39o(o;(y0u05K#d;ve>t*H@
crxulH80aXd>*?u3P-#hKj=JXNqk7hY03hJr&Hw-a
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0dfee1861238ee36321ba4d1ea3b45c06492a33d
GIT binary patch
literal 3943
zcmd1j<>g{vU|<lIi%i`w&A{*iYv3=9ko3=9m#yBHW4QW#Pga~Pr+!8B78Gni(H
zVgb{vQLHHpDNH$Rx$IHwj3BklIUKp1QJlG4QCy4+oD3-}DXc9FQQRqPDeNr_Q9LOe
zDV!|~QM{ZCDO@SsEeuh7DLg5>EeuiosRAi{*-S;zsoc#BQG%&lse;XnQ9`Lg3xpRk
zM2V#Gf%&4T?5X^zLaCz7%u!;goT);oVyS|e44DkgtWn~r;tM1eGB7fvFb0FMCjU#2
zulzI_Z?P977L{Zs=4dkA5^~PZD=Es)@yX9G@J-B1OiwMk#S@gDlwShnC4)4<FcSj<
z13Lo)gEJ^NSQr@?N*J;jQy5DavY1ksdYNh&Y8bMZQ<zH_vRFV~O=0V0VT6dWgT**f
zSdqjy!D3t}V%%Uc9uzU&64n&H6#f(exM_kVY$-x1!bl<_C9El;DPl+>;w7vp5-E~M
zB2p!+Dbgu2NFuT&>?v|7@+tBu3h7KKioL8**D95;rYNVVq^Lq{T)?rAp_Z|RF^jXP
zp@uPwtEi!bJ4LOTv5B#UF_=M9-LI-7s5DO@IVZn3HANvOKfgdBGp{VQs5n(2J2Nje
zH?br$xmW=i<fkc=B^G7omllImq!jBZ_$FqjDioI%r7BeBmnvlE<(DgzXC#&=lw_nT
zq~|B*C}b8ZB&DY1LlqR2<|%42-QrG3Elw`VEGWs$&%4E+mS2>cSW=Q&6rY@vSX^Ai
zqpMq-nOmBZ2$HVSD9=bO$w(~%TaZ|ks!&v#mzSBB4z?mRNCB!ezsOCK`4(GZGDy=c
zmW<S#f?Mn<scDI&IVDvBy1E6KdHKo78JYRI`FS~&Rffo>mM7+wD3s(YWR(_|C}if9
zq^1`omZU0_XO?6r!1Yye>*|(e7MCXGWLBkCd1BKKmxgFfPc29-DaqGSNGdH+@beFG
zwNl8-FHy+MEyzjDP0cGwO;M;!Eve$v)lDzTEGS4Vs!~8QRlO)xp|lw0QJ4xIU0qPH
z>ZTSI<rh_HgMv9NGcPemp&-9F6XbOTunat~G7<|4D#6Z4uNGP7{;;aWUX%G2OG#xx
zY7qwm149uf0|Nsnd~y>jbTdm*i@<?iTxFV@Sdp1qnyZkQlapVbn3R*M0GCzBE6q(x
zEmF|PECEGEu|i^rf~kRlN0nygf;L8mDn4D^#N_1E9H?Pc_lrOoQ@vOrD?c-@q*wu2
zQcs~cIWZ@(2<rZ_RE6aHyyB9?yb>LS+{6k6Lp^gHg~YrR1!z<%80eYkAty-~+sReI
z*)hn~E!0OL+%v>OA;iNoSPz^=N{b+AO(7*eS)n+wvRI)cBeNKkFx7KX6H^otD>6~t
zq>jyk5PuhcD~05Y)Z}bf3ePN7$W2YmD^52CgUj|HpBERTrl#l?=VYd&7FA73PApJ>
z`5r6-jta1|AY6z-m?u!^l6-~Y{M^)%3{V<KN-Zf*P0dp<00)Mlj)H-nsX|gEJXlb4
zLOEE2Bpp&HKCrLi*VQe_%+AQq%P-SSNz5&%(gcwp%b-z`nWs>anVpxPkzb}zl$u_e
zlUS5l1x_F7e^!3H<eL;=UnQuko1BxGt(y!fk8@Iss#J?pb5fH_6v{I)lQR^Ob25{&
z6%upO^NTV|GIGHQ6O`X>34?4)O$jJYEltUXmOHluvl5f@lQI+YLW&aeiVO0KQ#DzN
zK=m9VvEE`&&dAS9PA#sI(nYdX7aX3tAg5>MrB^W<>KRmtg0fp;a!F}oPEKWfDX5f7
zEXmbmhL{LS`CwzKT3s?zz*#dbGaXcv<(HNyWaO8FiiCnvXjulyp$eIK(7;7dpgagI
zgfjE<Kv`Nxp*X*&s8Yd7UqMeHGQU(IIWbQmBe5)1AwMstQo#@u|HY{aX*r4M#R^54
z=@}&odHLm<JhwO!i_!}ci;7ck@i`Wymx2n301&t6mMo0#0x8mpJyLTD++YRgEuO@b
zl=wubmRq7u`T04iiFy9u0yZ(n5mG3!rsd=(mfT{_%qzLYTwGFgiwkTQNMG?Sc4!H6
ziwohGTf%<%C7y`l%C)E{zetnm78_V!(Jhw5qV(dG3`H^w3=F>t^fU5vQ}xsHiwp8o
z5{tk*BLp|6G%rQJpa_)N^h=B4i@~K}JT%$p7gUyH<mX|L03|Mc5D{MhDm>y#@*xbp
zg34PQ>8T}P$BTgK^)Lno1~x_(CboZE%q)y-|GAjZNe-|GjAUYDVq*Kt!2+W}syUdM
z7&#c({&BECcsz_8j4Vt=atsU%$;^<9114D*7#Kjw2i%~rVPIgWVa#GEVa#G$z+3~W
za2W&`N*EWg)G%hTrZ7r^WWn@8rWytj262Xki~<ZbOactG%r(rx44O=SMT`s#44Q1W
z7(;F`<rUmw%K;VX>7bY@yTzPZnR<)0I3uwjRg>iwFE~f$<(I{Svd=9>uUlNHMMd#t
zsmUezMYos=Q^1h{A!I@2J2wLZgFPsGKyJ2RWMM1<i6k?D{Q)6BJ_GT=zTpG;h9Qfg
zhEaf_mZ^p*m_d`#Pm{4o9^^viA~lei>L5Y`M1W!*$ub^zBa4B7feU0ANF*8LJBU>b
z3?MehLa=29P|H#nvzdyxOBhR-KrOLmreIL(iy?(6o2f{sge8~(D#Hqr(PZ`md!Yzq
zv?kjvesJo5CZwFyq9Tx2z$(Cj1uoyffd$r3qzdu}W05qb>u(9Br<OS9WM)I#mN}_K
z{-A&Z6+c{zJd7a!@CX%YgAxNMFkwCdg(s-74ffF!P-C01mL-#+mbHdq0mDLuTDB6#
z1z>;GFl4dRuq|X}1o@67i?xO!i%pVY0ecNY7RN%yTJ{pg1)MeP3mLPxYB@@{vv?Np
z)^IFjtYOIF%VsDtEa6XKYG!0)NMWvJDPde7017FHi6C<pvea_cFxGI^a5OX3a@8;_
z5Uk-^$jHc0!<51z#!#GF!k+@N4{mES6C*<je+tNL5IcpSA_!LqLq#}hI2MR3WB~DM
zSU}D6G*F`D^mD7ycFqU27t>Sol2a8DlQS|?%TiNx6pB(4lQU95Rdz9`MFOrd6hks!
zFf%aRVlK!puHw(h%u7uyf;Lrb6{-Xh^T5>$LWC2})D*qNlAc-;c#F9pGw+sQT7FS^
zVo{1Wyn%X)H8(#cHRl#fN@7XkE#`dR%v&5@{+@mz@t!W=_Mm58*)3LIP^;+{OHO{e
z*)29mZYfd#r2$0-28LTKdHF@Tx41xR;=%2wTdV~|pkh)aIX}0cv?SFFRQPy7>!w?5
z#i@BIfhpj8nwnU2iyi9xTbw2EYK!|8E4bn>(gaz+n0bq{AhEb49_&cYq@vWsY`2`m
z^jmzziDjwr5UI@Ml3R?0x7dSS!EPu5XEShy!;;r-@k5h5s0##2wb`Jws=~m)z{1GF
z$iXDUD8|IYC<H3N7{!>ln7J4^m_-<c7=;-57`d2>+(EA5h>y=r%*>0A*Oa`)32*2Y
z>4D@}^Gb6ID#2lNizPX~pt1;5GTh=!E6qzT$<NOz2IsR|QW#~qUM{3RR$K(i=0%{K
z0xm$oSrU>5L0Jgyg1p4s)cANvh6MFjZ}F$3re)@(y5}e61b~9Q$P^S$;9%rN^7Abo
zP>&@ZYTGU5qS8D_NrYM^-Quvx%}*)KNwou&JjFT;3=A9$9E>2y!zjSW#KghFq0Pa<
Z#KOVBA;Tfh2A1Jq<Y5$G;$dX@0RZjET!H`q
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index c98b050..d5ce006 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -5,6 +5,10 @@ import argparse
from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
+# TODO move actually using this (main file and arguments to an example file).
+# but do make assertions that certain arguments have to exist.
+# this is good, just not separated the way it should be, like dmp is for example.
+
"""
Every imaginable magic number, flag and setting is put in here.
This way the rest of the code is clean.
@@ -30,7 +34,7 @@ def get_args():
parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
help="whether you're using the gripper", default=False)
parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with", default=1e-3)
+ help="the final position error you are happy with", default=1e-2)
parser.add_argument('--max-iterations', type=int, \
help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
parser.add_argument('--acceleration', type=float, \
@@ -42,7 +46,7 @@ def get_args():
to something between 0 and 1, 0.5 by default \
BE CAREFUL WITH THIS.", default=0.5)
parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tiknohov regularization", default=1e-2)
+ help="damping scalar in tiknohov regularization", default=1e-3)
# TODO add the rest
parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
@@ -102,7 +106,9 @@ def getClikController(args):
# modularity yo
-def controlLoopClik(robot, clik_controller, i):
+# past data to comply with the API
+# TODO make this a kwarg or whatever to be more lax with this
+def controlLoopClik(robot, clik_controller, i, past_data):
breakFlag = False
save_past_dict = {}
# know where you are, i.e. do forward kinematics
@@ -132,7 +138,6 @@ def controlLoopClik(robot, clik_controller, i):
return breakFlag, {}, {}
-
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index 01d274b..dd2e244 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -10,27 +10,26 @@ import numpy as np
import pinocchio as pin
import os
import sys
-sys.path.insert(0, '../../../util')
-from give_me_the_calibrated_model import get_model
#######################################################################
-# STEP 1: map the pixels to a 3D plane #
+# map the pixels to a 3D plane #
#######################################################################
-
-# let's call it 10 seconds, whatever
-t = (np.arange(100) / 10).reshape((100,1))
-path = np.genfromtxt('./path_in_pixels.csv', delimiter=',')
-z = np.zeros((len(path),1))
-path = np.hstack((path,z))
-
-# todo measure better maybe
-# we now scale this appropriatelly to m
-board_width = 0.35
-board_height = 0.4
-path[:,0] = path[:,0] * board_width
-path[:,1] = path[:,1] * board_height
-# in the new coordinate system we're going in the -y direction
-path[:,1] = -1 * path[:,1] + board_height
+# x-y start in top-left corner (natual for western latin script writing)
+# and then it's natural to have the z axis pointing out of the board
+# TODO but that's not a right-handed frame lmao, change that where it should be
+# NOTE: this might as well be in the util file, but whatever for now, it will be done
+# once it will actually be needed elsewhere
+def map2DPathTo3DPlane(path_2D, width, height):
+ z = np.zeros((len(path),1))
+ path_3D = np.hstack((path,z))
+ # scale the path to m
+ path[:,0] = path[:,0] * width
+ path[:,1] = path[:,1] * height
+ # in the new coordinate system we're going in the -y direction
+ # TODO this is a demo specific hack,
+ # make it general for a future release
+ path[:,1] = -1 * path[:,1] + args.height
+ return path
# now the path is appropriately scaled and in the first quadrant
diff --git a/python/ur_simple_control/dmp/__pycache__/create_dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/create_dmp.cpython-310.pyc
index 11b1d7280e1674320a8aea0e7af9628e66f5e298..e606d08bbcd11c09106b3e85b974b8b55a51c64a 100644
GIT binary patch
delta 210
zcmeyM*rddl&&$ijz`($e&mNj8BfgQ(go!b2vNMy7eL-bOMt+`tX;FM}W^O@FYJ75j
zUP)1Yj($pRfqrsPYGO%hd`fPCUP0w8j`;Y@yv&mLcs>ROhGGo{1_n0n&1;xk82PP1
z0wD8QnD`il7$^T{{?F(<`9Di1qxa-a)>_7Z$?R+;jG>cj*wPrICO>9NV~m~b#r~Z!
zVY3rQI3r`)<XN1X81p9ka{XW|-t5L5%EH(%c@F;@L0*u#G7Jn191L8H9J(AFlV=Kq
HG4cQaEbKa%
delta 192
zcmZot`k=^{&&$ijz`(%ZTH=+eD7=x+go!a^vNMy7Zc1)}er{!aaej_|a#3nxNossb
zZh>Aw<t>i*_{_Y_lK6NQ1_p*=4F(1VHm1${m|Ph7LF{6XX)G*!j6%$l1zG+xnoJgC
z?PRo>Je#$a(Q&dQTM47v<UY1EM(@dA+0q#OCdaUUXAImN!V%8M7(RIo=O)JZ$?;r2
o7*jS!aEG!m7EWHr|3;7*WRVO50|N&G7bAxX2hZfy0%43y02(AQT>t<8
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e35c4b88f393d22f12a90e35d11ca47eef6680e6
GIT binary patch
literal 6620
zcmd1j<>g{vU|`5qicI~d$iVOz#6iZ)3=9ko3=9m#W(*7rDGVu$ISf&Z?hGkRDa<Vl
zDa_4GQB3X(DJ&_hEet8F%}i0u?hGkxDeNr_DeTQmQ7kEp!3>%lFF_{yC4)$03^%)p
zfq@~FA&N1DA&MzQ9&8$86f?w}D3%nC6wVfgDAp8(6z&wB7RD&H6onMt6uuV5DE1VE
z6#f)}7RD%!6u}gs7KSL!6yX$+7KSLU6onMg6tNb@DDD)66!8>^7RD%^6onMY6sZ=*
zDBcw56qy!=D83Zg6uB0LDE<_#U<OUaTg)!L0WVV+7#LoPfCxqg28Lu-BqxK|ObiSR
z&Y%!!U|?XVVXR?@XQ*MSVTfm}VXk3_XR2YTVTfl=0g1DuFw`)_vw}%BFv$)kIlv?*
znB)SJ++dOiO!9(BJ}}7-CI!HxU=70pp@j^L3^fc3gh6x)LomZiCch$(D>X%KF=pRl
zOu5CJ7+-vgH7`CXt@svW-Yup|gIg>miKPa&7}IYtmn4?nVl2PKo}8Ljl3G-Ji>*8}
zr6i;H7GuRN){>0WlEhog1^LCdn9EXgZZRh&C*NWzNG!>?#h;U(m=a%7l$e#8T#{c@
zd5bwMvqY2i7E5tzPFfK=0|P@52Z-PV5uoI>lA%b3fq~&yynaT0ZmNEIesMv5N@5Y1
zXN2J9l;)-A7ZfE{WESg}7R485<`(3n#wX|Jl@#UY=oeI$WaQ^zkx0oc&`-%N&?~6C
z#StH$nU`4-AI}Gh5Df+f1~zUsMlfUslOQ&P1tGzF4i+B9DjpZ#06m!YWKbBw;u(}I
z*<g_z14{OcwTvYUS&TJ|pk&|6RLfMul){k0(8`p;lEx$nie#o5hAgH9%qgrTEFe}h
z<3do9U@Bo)zzSE(3eo{mzmPGDt%f0<9Tb6@Y<^XIdir^(<=_C;OD-<^#i*mnc#AV7
zH77GSvm~_$9Kl7Pq^HSoiz%<*7FT*|URqIpZb?PSEtZnZ+|*m_MXALZi3O>*m~&F|
zZm}j76(v^QVhp*(SgFZeB+bCUa7(xZ9)0nk=mbYBOG;u%A~>obge)imB^ekPtU<vE
zib)nGK1Lx%F-9fEDq&E7fwY1h3P}kl;RXsrc(?^IFff!bEMP2QSO5we<`l*>riF|(
z3|TBGOliy@Ry=DB12h~!8HWQTSHlp`nZgWWiGV_t3yIAQ4lNcxa6qI(;_8DvIBYa|
ziWEV9WGgE!NlebZ#afkGlwVxL4e|{Sh!6%5ydZ)fM1b;kktm2I1|q~67#K7e!NCLx
z1{shjC@gNV7Nr)amVk3R$RGtq4n{Uc4n`hE0mdp$P<Vjl{gOdt0>nuS3?Mcr34wKi
zjH+QwVJKxNVy|JyVyFcrB2X|ewlYaF1Tz$Y0&FEy5!i(w2d`v;I0P&J(hYGE*foq*
z-V6*3pFvtc?%-prVg<RO3d4F9Sn5>*7g4Cz*Dz!;f{LdsMiB;xBUUo`y#%?sNDt(3
zeGq|WGizmhT4`PYC{@607lDQVSO&udL<E2c12_PTK-S^3n6Wq%ByY&TzyR@VF@{wj
zmxIFkGbp`6%41McS;^?9$yfv`7c`+k3SvV-6YM*1ka85Kmc*y$C+0+fLKb8c7qn=A
zN#L}v2vj2$nL`o}C|$wq0kP5TVFwvkl2{rG@{J|PH{778gNi1Df(w>@Kx|MTgZxxn
z0*XMU6oyj9BE=Gh8pdYE6vk|(BE=HM1x%pAsD>enxrRxSA%$6(L4-jXTpwkzfJ7vq
zVhb6A89+r)DMOJ3vMy8|$f`l1rwPqyFaQ7l|G&r><Vn^dRgmMsIgL5BqM%3!#1#Zd
zLCTmSQ&1*k%tZG)b3t)t0x0Q#B1nW$kCB5>jgjSl6+0*d3W_sPLKsxc!F}HY@;z$q
zVOYRe11eG&7cwqnTmUXAAVo_psF-10z*fVwkg<e)0Y?e@0!|P+g=ry^G(!zT7FP{J
z7Pkb02&n#Ks$pEnD8f(+($52tTfhqvgUDy`)i5mJ2k}K1Bp7O##TjZ@YFL69G?}5r
zk5!MsqrOU4J52#d&b0#t1<3nFDj*ifqeTLs&|pr<FM$+J?74{*nYpF8x0s7dbHVAd
z2vnp%s!tP;E^r-c018i5q%r_x^ex7+TZ|cK;aUVrAZeg*1(^dX5M&rl7_}H#pv6K4
zO1OfOHpl{yreX_>a0R7|8iobTAV1eIEMS4qS*+Zk&|(AeL>NHDb}$1Z#j`{F4N43}
z+91b+%q;>rzK9dV1&0N;l7ulW59DM}6oZ_s1$As1ieo`p0E9t?7K55&prW&eVFBYp
zP(*^FlTn0098^Pq_~6D?7P2TPT3KqDY8V!<f^@{Q)i5kz2eH7a7~(;7G)oPW1VarY
zC~3rVfRa3@DFsS>nw${7xq$ou&dcEX0$f#q;|W}IfV~Q?IUtGu7E?;eEsnC(ocMyG
z)H1ZFy2Vmll3Gv<@;E52q?p7Q*_gQ)1sFvbt2jWp03?l4mx3xQcyt|MVgUIQTy9H(
zd<{zKpvE|;1;PkwQGhc7QwkHLjs^P<CeMsT9$fRLuz<~CFX71I1lI?k3W_U*HHEF0
zrIxXTV*z&!LlzGx0c5d*3KkItaIMW-!m)s_gku4J4MP?$C==E)m2fN&r~$Q!m?as~
zq4j*cASlkDtt2K`%Lhq}A%(e>3FI~jkl)x+Ky51qhzO`%kiwXPq(YjZmbru@OQ43i
zgkymasD%abYZ|B!Wbb9FWv*eyunQ)FWDeA1WcPq<Mp7ZoP|E_gi3NvEAaT@?VaQ?w
zg-tChOb4hyfyRVzi3rHG&5SM#u~oHfC44muS*$f|ppegE0nri+&5S7=*$hRW3Ln;h
z;ss4sgrOD`FYGCt!VEPmHEa-Hg2D|{kY$M?na6-E3o6LKAuCqHki{;-P{WKY3(7sU
z9ALL`f!(HE%UQ#)K)i;vhI1iPEmsZu0*M-~8V(VLEXjq8wcJoXJCx5;!dJu7%$UNJ
z%`kznC}09(p?(c_4G$<UWeL}?)o`XU*)V`a3m?`nERceARy4UGofU8st_ak|)MNrT
z;K1w<O~zXssd*^{`I&honvAztK>dRBj~S0Uw*Gix2d;zCd*5zd@cq~ZJ5A{#P-FiV
zE4bAQY3YGWUQqUe6t2pk><h|9km8pkCo`|OATc?$2wXVZfMmfX6{rHc#hjQ_e2XO|
zGcD~FcXEDSa$-qpUSdfqq>9r4sRftjtVM}=>8X$+8<abXK!tOWJxE?1MCgJFZ(eZW
zeT%as9@@<Un{tc2G9^AGH8=kjM`a3>#abL+oLX393#usC(&Ez-GxKh7q{Wvc7Nw__
z++xhS#ad9D8J~5FIS*8Aq@?Dd6)Qzj3=9mQUT86>ht9{Oz^K5;!zjiq1#YqjFv~Ie
zFv>AXF^e$rG0HG9F-kDXFiJ79{0Ct^CQu_*fRT@pg;9)==@$<tq&^1q>{yCGg>MlH
z$V;FS5yAzxbOb<MuDrzD)cE*YT=DU_`6;D2AU02Yd|_!~4n(F%4x|&*k0=6#2&7s7
z7oFg00aW`GS%H**gAY{57l8{~50DthPe}fh0f~W{YDJ(L3>2TmAR!J0E=CSr4h}93
zE><p94pt6kE@mz^E)EV>9$v6YjKKzwc2F=CgGzJwU;|?cQwu{Ba|&}XgC@%@7Qg%u
zXN<-es9yteWHBf}Km!`!MmyMzu-+AON^U`s8_203$AP7aK#sn}R$7pfSdzLDRP=*f
zz`|I?4YEWJBJQWjQWOo+2I^)Ofg6XQmJ-AORHKUG7#J8<fXo3ogM)#G5iEk?Pf*l>
z{i%jLsKN<qEVh6KRlq|vQ7q8G71k7vU<OUDTf!mEVW~Ne$;r<7dBr6~iJ5t+DbN3`
z{CLSXDZoA%R9nNEryw@S1>g_?MIfZf4IKhu1rLERFJLZVsbK~+R-xq}YYhunj0H)I
z4OHtExq|Wx`z_w|#N6D(_`Lkw%)G>$TkH^S;w|RXf?`Oc3e@_%#ab4hn^<v+H4#K<
zvVpT&5hx6bK+S|AP;;mV)Hy6l1%)bF2!fL3I#39LiU7#)fC!@mW0edR&m)EgKmiK#
zJXj;Fb9DgP#jRmnz_gHon*l<DJY35R>iUA(=HTj+nVSJD$^sT;T)?srq?QGw2Q1DC
z7H5F8wm>9Ul&yv_i>-!n0Xv9Z$XLUe#Uag5!vrq6*}*y(7ckYZFJy$+3}S)xaX{r*
zKq57a3&1qg1)Md^H7qr(HEcB;HJoW|u+|(WxQv7}<iHIZa1erG7!snwpuhxYQBZqG
zlM52;@gPx9qQAxHc#F~L7NhemMweS`74dng6(zUW9HF!`m_|y{;EW-do|>0hlvt7)
zpPN`xl$o4bycHBPpkh{nQH&9UWf(zFfU!yyTl`??N6ELKF%M8(0P;g|6}bJh0Mt2R
zTF4Z`R?A$=Qp;M)R?A+?0dDv*r!ZzSEM%<Ztl?b1ypW+tw*(YyEH#{x3^j~dtSL;O
z&K0O`lw=SGMKm)=MjE7pE03jwF^i3xp_U775(|<^K_#FFB*rA}JZ6wdwcI5fC7d<v
z&5X^AwVYsgu!C&lW&qWzk_=#5d1`n-?vh}rVUlL3<t<@czy;B>kP++(c8Gr@8EW~k
z%CRhDlw_#o2ieEXP%BWw=fV){P|I1v@4^skT`LGm9Fhzr+%@bqf|3l)j3Nv*93U20
zgcBkn&QL2<Bb36J%~Eu}gu4dfL$G=ghGs^vh@d1x4cP4<5pjlE;S$CLJT*c!!eBp$
z)QHpwfOOZ0)Cv?%DV$IvfaE998pbSMB)%9pB)DpLYeZ|r(m)|BUc<P650sH0@l(T)
z#SiKL*GkkdWC?)WHG#267Q`-LTp(B@0gA&C#sxwt%n&xXZ6r{`kR>d_P{O!Cq=d6Z
zqM0#;8I*-Y7(iV{a0gK`k12(*R<ed63&}+y;8al~3C@aA;24&w;Rflf1@TyFq-w+=
zyc#J`y#j6ZfO^s3rjMprQ9dZ|D1s_}_Ts|Q)YPifTg-`hmA9C46Dx{9nd}yGW?tGY
z=G@G@BG9l4xbJ<7EfLZH@dasQDK0E3DFW4q;GQ6)*aMe>jv&K9bpW^&yv35Ams$)i
zBsHbca&A!uNNFaB$N~}BAR-q;fO=_=mI+r$VrfczZf0J5;w>mUu_8Y47H3IfDOk7+
zCY+fUUv`TrGxHWxRu)1!ttcF1U?hkDjk4TgF3BxG(vo&d0B!@U_~(F$fSOGy@kxnA
zw^$4EOX3s3gV`x)g-H>p&fE_wK0qx92_`;f4n`eD4kkWE5Y5KK!z2M}0Wrxia)6-*
zBL|}fBS@Bok*R^{7azL-lMs^xGY_K+BMXxZ6CX1lBMVcN6t-dqR&O&FC4nj~HuUNn
zRL4MK7vv};hl1-OP|;VE$iTp`2IN{$6Ox00i;07shYKvK$$E=5uQa!y5>g(68Vuk{
g9xRE?tXmv5kS3TNsF6|(s-8F)_;?uV`DBDx0BXTeBme*a
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/dmp/__pycache__/temporal_coupling.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/temporal_coupling.cpython-310.pyc
index 31d5b4d59ffa1b2859605ad73e9336eadddcd992..3862d1996a25cbedc00c96c38fb2a2a1341665e0 100644
GIT binary patch
delta 104
zcmX>jd{3A+pO=@5fq{YH{-@AXpNYJe8FME-w2>*OEXl~v(=RQGFV4&@$VrV)&d)0;
z%FoeH$t{?y&M3>sIoXvlhLL0QB1T_EM*htom?knZifwLUab#gsn0$m|EhF#bBF<1&
J7CvSnRsblqAJG5+
delta 86
zcmca7d`6fzpO=@5fq{YHF^gv^??m3qjL{Px+Hj@h7U<_z#uw-3Otxf{Wn`Zm&KSeU
rws{MqFC!!GW+vu|jEo|ir?5D(Fv?B7#<7->dvXhBC@T{mvk)r)8g~|Z
diff --git a/python/ur_simple_control/dmp/create_dmp.py b/python/ur_simple_control/dmp/dmp.py
similarity index 60%
rename from python/ur_simple_control/dmp/create_dmp.py
rename to python/ur_simple_control/dmp/dmp.py
index b9d7a6d..2e6c734 100644
--- a/python/ur_simple_control/dmp/create_dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -5,13 +5,15 @@ import numpy as np
# 2. change hand-written numerical differentiation
# to numpy calls (for code style more than anything)
# 3. separate x into z and s variables, this is unnecessarily complicated
+# 4. ask mentors if there's ever a reason not to use temporal coupling,
+# and if not, integrate it into the DMP class
# k,d are constanst which determine the baseline uncostrained dynamics
# these work fine, but it could be good to play around with them just to
# see their effect. normally people set them so that you get critical damping
# as the uncostrained system
class DMP:
- def __init__(self, k=100, d=20, a_s=1, n_bfs=100):
+ def __init__(self, trajectory, k=100, d=20, a_s=1, n_bfs=100):
# TODO load the trajectory here
# and then allocate all these with zeros of appopriate length
# as this way they're basically declared twice
@@ -50,16 +52,23 @@ class DMP:
self.path = None
# actually init
- # TODO pass other trajectories as arguments and etc to make this nice
- self.load_trajectory()
+ # TODO handle this better, this is not optimal programming
+ if type(trajectory) == str:
+ self.load_trajectory_from_file(trajectory)
+ else:
+ self.load_trajectory(trajectory)
self.fit()
+ def load_trajectory_from_file(self, file_path):
+ # load trajectory. this is just joint positions.
+ data = np.genfromtxt(file_path, delimiter=',')
+ self.time = data[:, 0]
+ self.time = self.time.reshape(1, len(self.time))
+ self.y = np.array(data[:, 1:]).T
- # TODO pass other trajectories as arguments and etc to make this nice
- def load_trajectory(self):
+ def load_trajectory(self, trajectory):
# load trajectory. this is just joint positions.
- trajectory_loadpath = './new_traj.csv'
- data = np.genfromtxt(trajectory_loadpath, delimiter=',')
+ data = np.genfromtxt(file_path, delimiter=',')
self.time = data[:, 0]
self.time = self.time.reshape(1, len(self.time))
self.y = np.array(data[:, 1:]).T
@@ -163,3 +172,79 @@ class DMP:
# Reset state
self.reset()
+
+
+class NoTC:
+ def update(self, dmp, dt):
+ return 0
+
+class TCVelAccConstrained:
+
+ def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
+ self.gamma_nominal = gamma_nominal
+ self.gamma_a = gamma_a
+ self.eps = eps
+ self.v_max = v_max.reshape((len(v_max), 1))
+ self.a_max = a_max.reshape((len(a_max), 1))
+
+ def generate_matrices(self, dmp, dt):
+ A = np.vstack((-dmp.z(), dmp.z()))
+ B = np.vstack((-self.a_max, -self.a_max))
+ C = np.vstack((dmp.h(), -dmp.h()))
+ D = np.vstack((-self.v_max, -self.v_max))
+ x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
+ A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
+ C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
+ return A, B, C, D, A_next, C_next
+
+ def update(self, dmp, dt):
+
+ A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
+
+ # Acceleration bounds
+ i = np.squeeze(A < 0)
+ if i.any():
+ taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_min_a = -np.inf
+ i = np.squeeze(A > 0)
+ if i.any():
+ taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_max_a = np.inf
+ # Velocity bounds
+ i = range(len(A_next))
+ tau_min_v = np.max(-A_next[i] / D[i])
+ taud_min_v = (tau_min_v - dmp.tau) / dt
+ # Feasibility bounds
+ ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
+ jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
+ tau_min_f = -np.inf
+ for i in ii:
+ for j in jj:
+ num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
+ if num > 0:
+ den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
+ tmp = np.sqrt(num / den)
+ if tmp > tau_min_f:
+ tau_min_f = tmp
+ taud_min_f = (tau_min_f - dmp.tau) / dt
+ # Nominal bound
+ taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
+
+ taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
+
+ # Base update law
+ ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
+ if self.gamma_a > 0:
+ pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ else:
+ pot_a = 0
+ #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
+
+ # Saturate
+ taud = np.min((taud, taud_max_a))
+ taud = np.max((taud, taud_min))
+
+ return taud
diff --git a/python/ur_simple_control/dmp/drawing_gen/lasso_selector_demo_sgskip.py b/python/ur_simple_control/dmp/drawing_gen/lasso_selector_demo_sgskip.py
deleted file mode 100644
index f5a206f..0000000
--- a/python/ur_simple_control/dmp/drawing_gen/lasso_selector_demo_sgskip.py
+++ /dev/null
@@ -1,113 +0,0 @@
-"""
-==============
-Lasso Selector
-==============
-
-Interactively selecting data points with the lasso tool.
-
-This examples plots a scatter plot. You can then select a few points by drawing
-a lasso loop around the points on the graph. To draw, just click
-on the graph, hold, and drag it around the points you need to select.
-"""
-
-
-import numpy as np
-
-from matplotlib.path import Path
-from matplotlib.widgets import LassoSelector
-
-
-class SelectFromCollection:
- """
- Select indices from a matplotlib collection using `LassoSelector`.
-
- Selected indices are saved in the `ind` attribute. This tool fades out the
- points that are not part of the selection (i.e., reduces their alpha
- values). If your collection has alpha < 1, this tool will permanently
- alter the alpha values.
-
- Note that this tool selects collection objects based on their *origins*
- (i.e., `offsets`).
-
- Parameters
- ----------
- ax : `~matplotlib.axes.Axes`
- Axes to interact with.
- collection : `matplotlib.collections.Collection` subclass
- Collection you want to select from.
- alpha_other : 0 <= float <= 1
- To highlight a selection, this tool sets all selected points to an
- alpha value of 1 and non-selected points to *alpha_other*.
- """
-
- def __init__(self, ax, collection, alpha_other=0.3):
- self.canvas = ax.figure.canvas
- self.collection = collection
- self.alpha_other = alpha_other
-
- self.xys = collection.get_offsets()
- self.Npts = len(self.xys)
-
- # Ensure that we have separate colors for each object
- self.fc = collection.get_facecolors()
- if len(self.fc) == 0:
- raise ValueError('Collection must have a facecolor')
- elif len(self.fc) == 1:
- self.fc = np.tile(self.fc, (self.Npts, 1))
-
- self.lasso = LassoSelector(ax, onselect=self.onselect)
- self.ind = []
-
- def onselect(self, verts):
- path = Path(verts)
- print(verts)
- print(path)
- self.ind = np.nonzero(path.contains_points(self.xys))[0]
- self.fc[:, -1] = self.alpha_other
- self.fc[self.ind, -1] = 1
- self.collection.set_facecolors(self.fc)
- #self.canvas.draw_idle()
-
- def disconnect(self):
- self.lasso.disconnect_events()
- self.fc[:, -1] = 1
- self.collection.set_facecolors(self.fc)
- self.canvas.draw_idle()
-
-
-if __name__ == '__main__':
- import matplotlib.pyplot as plt
-
- # Fixing random state for reproducibility
- np.random.seed(19680801)
-
- data = np.random.rand(100, 2)
-
- subplot_kw = dict(xlim=(0, 1), ylim=(0, 1), autoscale_on=False)
- fig, ax = plt.subplots(subplot_kw=subplot_kw)
-
- pts = ax.scatter(data[:, 0], data[:, 1], s=80)
- selector = SelectFromCollection(ax, pts)
-
- def accept(event):
- if event.key == "enter":
- print("Selected points:")
- print(selector.xys[selector.ind])
- selector.disconnect()
- ax.set_title("")
- fig.canvas.draw()
-
- fig.canvas.mpl_connect("key_press_event", accept)
- ax.set_title("Press enter to accept selected points.")
-
- plt.show()
-
-# %%
-#
-# .. admonition:: References
-#
-# The use of the following functions, methods, classes and modules is shown
-# in this example:
-#
-# - `matplotlib.widgets.LassoSelector`
-# - `matplotlib.path.Path`
diff --git a/python/ur_simple_control/dmp/drawing_gen/path_in_pixels.csv b/python/ur_simple_control/dmp/drawing_gen/path_in_pixels.csv
deleted file mode 100644
index 50cb591..0000000
--- a/python/ur_simple_control/dmp/drawing_gen/path_in_pixels.csv
+++ /dev/null
@@ -1,327 +0,0 @@
-0.30482,0.61994
-0.30334,0.61994
-0.30186,0.62144
-0.29891,0.62442
-0.29595,0.62890
-0.29299,0.63189
-0.28263,0.64383
-0.27227,0.65278
-0.26635,0.65726
-0.26191,0.66174
-0.25747,0.66323
-0.25155,0.66920
-0.24711,0.67368
-0.24416,0.67518
-0.23972,0.68115
-0.23824,0.68264
-0.23676,0.68562
-0.23528,0.69309
-0.23380,0.69757
-0.23380,0.70503
-0.23380,0.71249
-0.23380,0.71697
-0.23528,0.72444
-0.24120,0.74534
-0.24711,0.75429
-0.25747,0.76773
-0.26339,0.77370
-0.26783,0.77967
-0.27967,0.78713
-0.28559,0.79460
-0.29003,0.79758
-0.30038,0.80803
-0.30926,0.81549
-0.32998,0.82594
-0.33886,0.82893
-0.34478,0.83042
-0.35661,0.83341
-0.36253,0.83341
-0.36993,0.83341
-0.38325,0.83341
-0.39657,0.83192
-0.40249,0.83042
-0.41432,0.82744
-0.41728,0.82594
-0.42172,0.82445
-0.42912,0.81997
-0.43060,0.81848
-0.43356,0.81549
-0.44244,0.80803
-0.44984,0.79609
-0.45280,0.78863
-0.45576,0.77668
-0.45872,0.76773
-0.46168,0.75877
-0.46463,0.74683
-0.46759,0.72891
-0.46907,0.72444
-0.46907,0.72145
-0.46907,0.71548
-0.46907,0.71249
-0.46907,0.70652
-0.46907,0.70354
-0.46907,0.69906
-0.46907,0.69757
-0.46907,0.69607
-0.46907,0.69458
-0.46759,0.69607
-0.46463,0.70354
-0.46315,0.70951
-0.46020,0.72294
-0.46315,0.73936
-0.47647,0.75877
-0.47647,0.76026
-0.47795,0.76176
-0.47795,0.76325
-0.47943,0.76474
-0.52826,0.78116
-0.55934,0.78713
-0.57561,0.79161
-0.58005,0.79310
-0.58153,0.79310
-0.58301,0.79310
-0.58597,0.79310
-0.62149,0.78713
-0.65108,0.78265
-0.65552,0.78265
-0.66440,0.77818
-0.67624,0.77370
-0.69843,0.76474
-0.71027,0.75877
-0.72359,0.75280
-0.72951,0.75131
-0.73247,0.74832
-0.74134,0.74235
-0.74726,0.73787
-0.75318,0.73041
-0.76354,0.70951
-0.76798,0.69607
-0.76946,0.68712
-0.76946,0.66920
-0.76798,0.66174
-0.76502,0.65129
-0.76058,0.63338
-0.75910,0.63189
-0.75910,0.63039
-0.75614,0.62741
-0.75022,0.62293
-0.74282,0.61994
-0.73542,0.61696
-0.71767,0.61248
-0.70879,0.61099
-0.69991,0.60949
-0.69695,0.60949
-0.69103,0.60949
-0.68511,0.60800
-0.67180,0.60502
-0.66736,0.60502
-0.65996,0.60502
-0.65700,0.60502
-0.65108,0.60502
-0.64516,0.60651
-0.63776,0.60800
-0.62001,0.61397
-0.61557,0.61397
-0.60965,0.61397
-0.60817,0.61397
-0.60521,0.61397
-0.60373,0.61397
-0.60817,0.61248
-0.61853,0.60949
-0.63332,0.60352
-0.64516,0.60054
-0.65700,0.59606
-0.67032,0.59009
-0.69251,0.58113
-0.70731,0.57217
-0.71767,0.56770
-0.73690,0.55874
-0.74578,0.55426
-0.75910,0.54680
-0.76502,0.54232
-0.77538,0.53486
-0.78130,0.53187
-0.79165,0.51993
-0.79609,0.50948
-0.80201,0.49455
-0.80497,0.48559
-0.80793,0.47067
-0.80793,0.46619
-0.80793,0.46320
-0.80793,0.45574
-0.80793,0.45425
-0.80497,0.45126
-0.80053,0.44380
-0.78130,0.42439
-0.76946,0.41693
-0.74726,0.40648
-0.73986,0.40349
-0.72951,0.40200
-0.72211,0.40200
-0.70583,0.40349
-0.69695,0.40648
-0.68067,0.40946
-0.67476,0.41245
-0.66440,0.41544
-0.66144,0.41693
-0.65700,0.41842
-0.65108,0.42141
-0.63924,0.42887
-0.63332,0.43186
-0.62001,0.43633
-0.60669,0.44230
-0.60225,0.44529
-0.59189,0.45126
-0.58893,0.45275
-0.57709,0.46470
-0.57265,0.46917
-0.56674,0.47664
-0.56230,0.48410
-0.54898,0.50351
-0.54750,0.50948
-0.54454,0.51844
-0.54454,0.51993
-0.54306,0.52590
-0.54306,0.53038
-0.54158,0.53933
-0.54158,0.54232
-0.54158,0.54381
-0.54158,0.54232
-0.54306,0.54083
-0.54602,0.53336
-0.56082,0.50202
-0.56378,0.48410
-0.56526,0.47664
-0.56674,0.46470
-0.56674,0.45425
-0.56082,0.42439
-0.55786,0.41096
-0.55046,0.39454
-0.54750,0.39006
-0.54158,0.38409
-0.53270,0.37364
-0.53122,0.37065
-0.52234,0.36170
-0.51790,0.35572
-0.51495,0.35572
-0.50163,0.34826
-0.49127,0.34528
-0.47055,0.34229
-0.46168,0.34229
-0.44984,0.34229
-0.44392,0.34378
-0.42764,0.34677
-0.42024,0.34826
-0.41284,0.35125
-0.39805,0.35423
-0.39213,0.35572
-0.38473,0.35722
-0.37881,0.36020
-0.37585,0.36170
-0.37141,0.36468
-0.36253,0.37364
-0.35513,0.38259
-0.34626,0.40648
-0.34626,0.41245
-0.34626,0.42141
-0.34922,0.43484
-0.35218,0.44230
-0.35365,0.45275
-0.35661,0.46022
-0.35957,0.47365
-0.36549,0.48709
-0.36993,0.49455
-0.37141,0.50052
-0.37289,0.50351
-0.37437,0.50649
-0.37585,0.50799
-0.37585,0.50500
-0.37141,0.48858
-0.36549,0.47067
-0.35809,0.45873
-0.34626,0.44380
-0.33590,0.43335
-0.31518,0.41693
-0.30926,0.41245
-0.29003,0.39752
-0.27967,0.39304
-0.26191,0.38707
-0.25303,0.38558
-0.23676,0.38409
-0.22788,0.38409
-0.20568,0.38409
-0.19384,0.38409
-0.17609,0.38558
-0.16869,0.38707
-0.16129,0.39006
-0.14797,0.39603
-0.13909,0.40200
-0.12430,0.41096
-0.11098,0.41842
-0.10358,0.42290
-0.09618,0.42887
-0.09174,0.43186
-0.08730,0.43783
-0.08139,0.44828
-0.07695,0.46320
-0.07547,0.46917
-0.07399,0.48559
-0.07399,0.49306
-0.07399,0.50202
-0.07547,0.50799
-0.07547,0.51097
-0.07843,0.51993
-0.08286,0.53038
-0.08730,0.53933
-0.09174,0.54978
-0.09322,0.55277
-0.09470,0.55426
-0.09766,0.55725
-0.09914,0.55874
-0.10210,0.56173
-0.10950,0.56620
-0.11542,0.57068
-0.12726,0.57964
-0.13318,0.58412
-0.13466,0.58561
-0.14205,0.59158
-0.14501,0.59457
-0.15685,0.60203
-0.16425,0.60800
-0.17165,0.61248
-0.17905,0.61546
-0.18201,0.61696
-0.18497,0.61845
-0.19384,0.62144
-0.19680,0.62293
-0.20124,0.62293
-0.20716,0.62293
-0.21012,0.62293
-0.21752,0.62293
-0.22196,0.62144
-0.22788,0.62144
-0.23232,0.62144
-0.23676,0.61994
-0.23972,0.61994
-0.25599,0.61994
-0.26191,0.61845
-0.26635,0.61845
-0.27523,0.61845
-0.27819,0.61696
-0.28263,0.61696
-0.28559,0.61696
-0.28707,0.61546
-0.29151,0.61546
-0.29447,0.61546
-0.29595,0.61397
-0.30038,0.61248
-0.30334,0.61248
-0.30482,0.61248
-0.30926,0.61099
-0.31074,0.60949
-0.31666,0.60800
-0.31814,0.60651
-0.31962,0.60651
-0.32110,0.60651
-0.32110,0.60502
-0.32110,0.60502
diff --git a/python/ur_simple_control/dmp/robotiq_gripper.py b/python/ur_simple_control/dmp/robotiq_gripper.py
deleted file mode 100644
index a17ab18..0000000
--- a/python/ur_simple_control/dmp/robotiq_gripper.py
+++ /dev/null
@@ -1,291 +0,0 @@
-"""Module to control Robotiq's grippers - tested with HAND-E"""
-
-import socket
-import threading
-import time
-from enum import Enum
-from typing import Union, Tuple, OrderedDict
-
-class RobotiqGripper:
- """
- Communicates with the gripper directly, via socket with string commands, leveraging string names for variables.
- """
- # WRITE VARIABLES (CAN ALSO READ)
- ACT = 'ACT' # act : activate (1 while activated, can be reset to clear fault status)
- GTO = 'GTO' # gto : go to (will perform go to with the actions set in pos, for, spe)
- ATR = 'ATR' # atr : auto-release (emergency slow move)
- ADR = 'ADR' # adr : auto-release direction (open(1) or close(0) during auto-release)
- FOR = 'FOR' # for : force (0-255)
- SPE = 'SPE' # spe : speed (0-255)
- POS = 'POS' # pos : position (0-255), 0 = open
- # READ VARIABLES
- STA = 'STA' # status (0 = is reset, 1 = activating, 3 = active)
- PRE = 'PRE' # position request (echo of last commanded position)
- OBJ = 'OBJ' # object detection (0 = moving, 1 = outer grip, 2 = inner grip, 3 = no object at rest)
- FLT = 'FLT' # fault (0=ok, see manual for errors if not zero)
-
- ENCODING = 'UTF-8' # ASCII and UTF-8 both seem to work
-
- class GripperStatus(Enum):
- """Gripper status reported by the gripper. The integer values have to match what the gripper sends."""
- RESET = 0
- ACTIVATING = 1
- # UNUSED = 2 # This value is currently not used by the gripper firmware
- ACTIVE = 3
-
- class ObjectStatus(Enum):
- """Object status reported by the gripper. The integer values have to match what the gripper sends."""
- MOVING = 0
- STOPPED_OUTER_OBJECT = 1
- STOPPED_INNER_OBJECT = 2
- AT_DEST = 3
-
- def __init__(self):
- """Constructor."""
- self.socket = None
- self.command_lock = threading.Lock()
- self._min_position = 0
- self._max_position = 255
- self._min_speed = 0
- self._max_speed = 255
- self._min_force = 0
- self._max_force = 255
-
- def connect(self, hostname: str, port: int, socket_timeout: float = 2.0) -> None:
- """Connects to a gripper at the given address.
- :param hostname: Hostname or ip.
- :param port: Port.
- :param socket_timeout: Timeout for blocking socket operations.
- """
- self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.socket.connect((hostname, port))
- self.socket.settimeout(socket_timeout)
-
- def disconnect(self) -> None:
- """Closes the connection with the gripper."""
- self.socket.close()
-
- def _set_vars(self, var_dict: OrderedDict[str, Union[int, float]]):
- """Sends the appropriate command via socket to set the value of n variables, and waits for its 'ack' response.
- :param var_dict: Dictionary of variables to set (variable_name, value).
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- # construct unique command
- cmd = "SET"
- for variable, value in var_dict.items():
- cmd += f" {variable} {str(value)}"
- cmd += '\n' # new line is required for the command to finish
- # atomic commands send/rcv
- with self.command_lock:
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
- return self._is_ack(data)
-
- def _set_var(self, variable: str, value: Union[int, float]):
- """Sends the appropriate command via socket to set the value of a variable, and waits for its 'ack' response.
- :param variable: Variable to set.
- :param value: Value to set for the variable.
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- return self._set_vars(OrderedDict([(variable, value)]))
-
- def _get_var(self, variable: str):
- """Sends the appropriate command to retrieve the value of a variable from the gripper, blocking until the
- response is received or the socket times out.
- :param variable: Name of the variable to retrieve.
- :return: Value of the variable as integer.
- """
- # atomic commands send/rcv
- with self.command_lock:
- cmd = f"GET {variable}\n"
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
-
- # expect data of the form 'VAR x', where VAR is an echo of the variable name, and X the value
- # note some special variables (like FLT) may send 2 bytes, instead of an integer. We assume integer here
- var_name, value_str = data.decode(self.ENCODING).split()
- if var_name != variable:
- raise ValueError(f"Unexpected response {data} ({data.decode(self.ENCODING)}): does not match '{variable}'")
- value = int(value_str)
- return value
-
- @staticmethod
- def _is_ack(data: str):
- return data == b'ack'
-
- def _reset(self):
- """
- Reset the gripper.
- The following code is executed in the corresponding script function
- def rq_reset(gripper_socket="1"):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
- sync()
- end
-
- sleep(0.5)
- end
- """
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- time.sleep(0.5)
-
-
- def activate(self, auto_calibrate: bool = True):
- """Resets the activation flag in the gripper, and sets it back to one, clearing previous fault flags.
- :param auto_calibrate: Whether to calibrate the minimum and maximum positions based on actual motion.
- The following code is executed in the corresponding script function
- def rq_activate(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_reset(gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_reset(gripper_socket)
- sync()
- end
-
- rq_set_var("ACT",1, gripper_socket)
- end
- end
- def rq_activate_and_wait(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_activate(gripper_socket)
- sleep(1.0)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 1 or not rq_get_var("STA", 1, gripper_socket) == 3):
- sleep(0.1)
- end
-
- sleep(0.5)
- end
- end
- """
- if not self.is_active():
- self._reset()
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- time.sleep(0.01)
-
- self._set_var(self.ACT, 1)
- time.sleep(1.0)
- while (not self._get_var(self.ACT) == 1 or not self._get_var(self.STA) == 3):
- time.sleep(0.01)
-
- # auto-calibrate position range if desired
- if auto_calibrate:
- self.auto_calibrate()
-
- def is_active(self):
- """Returns whether the gripper is active."""
- status = self._get_var(self.STA)
- return RobotiqGripper.GripperStatus(status) == RobotiqGripper.GripperStatus.ACTIVE
-
- def get_min_position(self) -> int:
- """Returns the minimum position the gripper can reach (open position)."""
- return self._min_position
-
- def get_max_position(self) -> int:
- """Returns the maximum position the gripper can reach (closed position)."""
- return self._max_position
-
- def get_open_position(self) -> int:
- """Returns what is considered the open position for gripper (minimum position value)."""
- return self.get_min_position()
-
- def get_closed_position(self) -> int:
- """Returns what is considered the closed position for gripper (maximum position value)."""
- return self.get_max_position()
-
- def is_open(self):
- """Returns whether the current position is considered as being fully open."""
- return self.get_current_position() <= self.get_open_position()
-
- def is_closed(self):
- """Returns whether the current position is considered as being fully closed."""
- return self.get_current_position() >= self.get_closed_position()
-
- def get_current_position(self) -> int:
- """Returns the current position as returned by the physical hardware."""
- return self._get_var(self.POS)
-
- def auto_calibrate(self, log: bool = True) -> None:
- """Attempts to calibrate the open and closed positions, by slowly closing and opening the gripper.
- :param log: Whether to print the results to log.
- """
- # first try to open in case we are holding an object
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed opening to start: {str(status)}")
-
- # try to close as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_closed_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position <= self._max_position
- self._max_position = position
-
- # try to open as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position >= self._min_position
- self._min_position = position
-
- if log:
- print(f"Gripper auto-calibrated to [{self.get_min_position()}, {self.get_max_position()}]")
-
- def move(self, position: int, speed: int, force: int) -> Tuple[bool, int]:
- """Sends commands to start moving towards the given position, with the specified speed and force.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with a bool indicating whether the action it was successfully sent, and an integer with
- the actual position that was requested, after being adjusted to the min/max calibrated range.
- """
-
- def clip_val(min_val, val, max_val):
- return max(min_val, min(val, max_val))
-
- clip_pos = clip_val(self._min_position, position, self._max_position)
- clip_spe = clip_val(self._min_speed, speed, self._max_speed)
- clip_for = clip_val(self._min_force, force, self._max_force)
-
- # moves to the given position with the given speed and force
- var_dict = OrderedDict([(self.POS, clip_pos), (self.SPE, clip_spe), (self.FOR, clip_for), (self.GTO, 1)])
- return self._set_vars(var_dict), clip_pos
-
- def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[int, ObjectStatus]: # noqa
- """Sends commands to start moving towards the given position, with the specified speed and force, and
- then waits for the move to complete.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with an integer representing the last position returned by the gripper after it notified
- that the move had completed, a status indicating how the move ended (see ObjectStatus enum for details). Note
- that it is possible that the position was not reached, if an object was detected during motion.
- """
- set_ok, cmd_pos = self.move(position, speed, force)
- if not set_ok:
- raise RuntimeError("Failed to set variables for move.")
-
- # wait until the gripper acknowledges that it will try to go to the requested position
- while self._get_var(self.PRE) != cmd_pos:
- time.sleep(0.001)
-
- # wait until not moving
- cur_obj = self._get_var(self.OBJ)
- while RobotiqGripper.ObjectStatus(cur_obj) == RobotiqGripper.ObjectStatus.MOVING:
- cur_obj = self._get_var(self.OBJ)
-
- # report the actual position and the object status
- final_pos = self._get_var(self.POS)
- final_obj = cur_obj
- return final_pos, RobotiqGripper.ObjectStatus(final_obj)
\ No newline at end of file
diff --git a/python/ur_simple_control/dmp/temporal_coupling.py b/python/ur_simple_control/dmp/temporal_coupling.py
deleted file mode 100644
index 0a43b4d..0000000
--- a/python/ur_simple_control/dmp/temporal_coupling.py
+++ /dev/null
@@ -1,82 +0,0 @@
-#!/usr/bin/env python
-
-# TODO
-# make this comply with changes (to be) made in the dmp
-import numpy as np
-
-
-class NoTC:
- def update(self, dmp, dt):
- return 0
-
-
-class TCVelAccConstrained:
-
- def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
- self.gamma_nominal = gamma_nominal
- self.gamma_a = gamma_a
- self.eps = eps
- self.v_max = v_max.reshape((len(v_max), 1))
- self.a_max = a_max.reshape((len(a_max), 1))
-
- def generate_matrices(self, dmp, dt):
- A = np.vstack((-dmp.z(), dmp.z()))
- B = np.vstack((-self.a_max, -self.a_max))
- C = np.vstack((dmp.h(), -dmp.h()))
- D = np.vstack((-self.v_max, -self.v_max))
- x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
- A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
- C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
- return A, B, C, D, A_next, C_next
-
- def update(self, dmp, dt):
-
- A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
-
- # Acceleration bounds
- i = np.squeeze(A < 0)
- if i.any():
- taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
- else:
- taud_min_a = -np.inf
- i = np.squeeze(A > 0)
- if i.any():
- taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
- else:
- taud_max_a = np.inf
- # Velocity bounds
- i = range(len(A_next))
- tau_min_v = np.max(-A_next[i] / D[i])
- taud_min_v = (tau_min_v - dmp.tau) / dt
- # Feasibility bounds
- ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
- jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
- tau_min_f = -np.inf
- for i in ii:
- for j in jj:
- num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
- if num > 0:
- den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
- tmp = np.sqrt(num / den)
- if tmp > tau_min_f:
- tau_min_f = tmp
- taud_min_f = (tau_min_f - dmp.tau) / dt
- # Nominal bound
- taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
-
- taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
-
- # Base update law
- ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
- if self.gamma_a > 0:
- pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
- else:
- pot_a = 0
- #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
- taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
-
- # Saturate
- taud = np.min((taud, taud_max_a))
- taud = np.max((taud, taud_min))
-
- return taud
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 3afdb81..e55168d 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -14,7 +14,6 @@ import signal
# TODO make the package work
from ur_simple_control.util.get_model import get_model
from ur_simple_control.util.robotiq_gripper import RobotiqGripper
-import argparse
from collections import deque
"""
@@ -102,9 +101,12 @@ class ControlLoopManager:
# and we can be slow with initialization.
self.past_data[key].append(copy.deepcopy(save_past_dict[key]))
- # TODO save and handle plotting data in the exact same fashion.
- # also write out docs on what's available (and then assert what's defined
- # to have to be in the list of available things)
+ # similar story for log_dict as for past_data,
+ # except this is not used in the control loop,
+ # and we pre-declare the sizes
+ # TODO the itialization is done on client side,
+ # but i want to do it here. this is a big ick
+ self.log_dict = log_dict
# if you just stop it the program with Ctrl-C, it will continue running
# the last speedj lmao.
@@ -139,6 +141,8 @@ class ControlLoopManager:
def run(self):
for i in range(self.max_iterations):
start = time.time()
+ # TODO make the arguments to controlLoop kwargs or whatever
+ # so that you don't have to declare them on client side if you're not using them
breakFlag, latest_to_save_dict, log_entry_dict = self.controlLoop(i, self.past_data)
# update past rolling window
# TODO: write an assert assuring the keys are what's been promised
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..654ccdea21df40efc4deb0f2c418c7e9e9736fc5
GIT binary patch
literal 24576
zcmYc?2=nw+u+TGNU|?VnU|_H^iA*gGtzih8$;gnNUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8Fut=1a
zWaj86C+1`(#V6$_7Nx{zBqnDU>lIW&tQ#dqLtr!nU?EUilBR3H%V2C|XaEvYR#H?D
z777J1NAYL~jE2By2#kinXb6mkz-S1JhQMeDjE2An34xLV7KVBT1_mamf32Z3BO1*L
z<!eJ}ODGLf#{%UGL1}*|4U=bv@|B=;F_ebML%E~WXb6mkz-S1JhQMeDjE2By2#kin
zXb6mkz-S1JhQMeDjD`RWLLf1Pfgy^Gfgy<<GXD?j|Nr1;U^v0gz|hRkz~IWyz~I8q
zz@Wp=z#z%bz`)1Pz`(@M!0?Taf#DDz1H(Z+28O+S3=D023=Cm>3=CX+3=AiE85kz;
zGB9NGGBCLCGB7ysGB7CcGBEt%VPM$E!@w|whk>Dmhk+rAhk?PHhk-$thk@ZSHv_|I
zZU%;R+zbqBxfvL$xfvL&xfvMvxfvL4b1^V<axpOYb1^VDaxpMSb1^Xd;ACL9&B?$p
zpOb+hkCTBxmy?0vI|l>9H4X-b4IB&%T^tMynH&rZ9vln|N*oLfOdJdhkJuR)4zV*Z
zEMjM1Xkv%>TZ5f}fuEg$fr*`gfe{oA><kQd*ccejvoSE#urV+=vN14dvN15cXJuem
z!pgvq$I8Iq#>&9p%F4jN%F4iShJ}G)6$=ByOcn-)02T%YbruGOkIW1VJD3?5x|kUl
zqL~>ORG1kUt}-z&G%_(T<S;QXm@zRhd}Cx_SkB16V9mt9@PUzm;Upsi!vaPI20Io8
z1|b#(20<1E1_cBNiqFg|$xO_NPc1IV%uOswRj^e^&M&CcOG!;F05LR*@{{sQ^imQ_
z67}+ZGov9~FMm(Jka$m*SiPeBlEjkC{5(ytsmcl^8JWcji3P>^Ii)43Ih6_-iNy*f
z`3gy?3Q!X>Q&SWYvlGF#=qBgq6_*s1CYPk9fb|z-=IN#77nLU#rFduNrRFA<WF{9w
zosgTKlA5EV0C51wJvs`75L*isY@x#GsU?9L5J6>y5PuhcD}~(rvQ&lW#H9SP)cE4m
zypq(sWY98@l+;*-%)F9(h=bEg^O8Zn068@^ueh`*RiPv!u|&OCp(G<!Avv))73@4^
z1wa1~S1X8~%wmP&(t?8g;?xw7wxm>rqS8Et{Jfk>g~YUy)FQCf%)Crc@G68j2Y`bj
zDK#}up*Xb!YFv71i9(@{LQ1{@)Yl5xaBu4<fMk<Pi;7b7O27sLDL}MED;Vf0=)fXT
z*FewI%+%D<)ZAQ0LDxXf!pOwH#N5a{HWnHXAmwn?$SQOc6cBo2bs&ZZDcCCH73d`v
z6(v?`L`NIyA+#ExunmljjLl4qO<)?KVUwSeqL7@QlB!UYnwyxJSFDhcT9m3#npcvU
zqmY(ap^#auP?A~<i*RLy%)H`~)Wj5p{4|A(#Inr1bV%rC<QEm?7lAcH+y#x!w0uaw
z=76F#KTRP$KSiM+CpED+RUt9Cq%<)nr&6H^WF9ynmS?2qDOBc{DkP<*XXb$o%8LhC
zU#wuOU}yjqC@x4%O;NB_h&IqsFaU?3F_;13#)4IVQgy0AL4Ial3CQ{B`3j{4Itodp
zCEzGdD@x2w1&M=_3Mihz0#J>hG^miAk(iebN>Q1`ItrNzS*67#3dI@uWuSzUk*ZLT
znx{~bQIubro&h!xyzU5+OY>68<3TA5BUggb6<7um$8at@Clr;Wq{bJeCZ}eWrRt@p
zmN<d~C&W1*Aip>jo<$TCic%qg2XcH`QEF;RQD#}HLSkMD$SHXW#U+VFB?>9|kaz)Q
zg`CW!B5;mZC@9KLPAx8m`czk;v^Z6vI6pU4Av?7a><5L+yn@mah3H8AyjX?AVueJ7
zq@4WZY>-*Wi8(pY1gD@2wFPcH)I<evw#qNbP{_+KQAjFEP0R+RjlA??u=^5oit`b1
zUR;`-oC+;PlofOpN>X!jKwbd3sz{+cBe4WzT1vh`d1gt5LP1exUP-ZzLSAAnsE{fL
zr_qvp1(0(=A(5JwqMMqQmYQ6WUj%gxC?>5G7*bNx6u`^FiV{mw19B4cQX$2Yjsi4c
zX<9Kr;;tyQq_ikcAqcD=Y@vo$kd8ueYJsh~PE4LUB<3Iz>P7h_3b_zB=B1V^fRm7w
zx+cUVXil#J(MUvmUTS%?Q7kmSCKYQ`K}8@YLwJT5@`liI0U~dJA#VVcuY{V0FdrrY
zF(1N1HXkN}ERSqHL>^|oGQ<H`J)oeaplb*YIH)RQYasey-he7Vwg;jBZV$v-WP4yD
znn?CQ#E|WQiD0TiwhW>QZkYnS{3|X=ti)FO6%^%{fpUDlLV12sHpG*ln1y0c)&rLi
zY57ITNQD}_f(`;D<c!2JP=S&Pub6ao?G!RoQWLE}30|Q%Gd-_ZA+0DsR{>NsB<3gt
zDZmVX+6YO{X_<K`3W=Z+vp7F5F)0U>gL6t@RUx<x1(^lP>q$AOxlp|j_hnXqGZ;v|
zSRped87v774oH$oF3K-1)~Em_BOL{BoInx=SOlU3TFV&ffeKqF0Zo2j4IovJEC*{S
zxPeMrx1#)9zx<-y#GEj27S+IL#^@;IX<9KTE94g`low@|q$;H3m***zXH<e~gOvO{
z^%8|VP`RI3qM(tfo~w|NpO;!(l2ZvTYl=Y)fW(qqNFkpIF7<N~OHwr%Agu~;MFuJ-
z(h`$Fl|W(%*jR9|Bo-H!=7Qr!p+W()`ji1wePkAcYKZ(ah2;FwqT*DA<b2R_RFHj%
zMM;?@MTtd~Its=449W`SsgMc+!yJ$S1&Kw8IXS60kWviff+A4!OuZOk7r04Pnp~og
z2q{Gr5{vRnA;q~u8km&_4mpJ~aL^%27)VK^l$?@ZqNJlxpx~eYZPbCP+yaHnJcWY%
z;!IG|D7FGeHB>`p8aNrG<>%z&mxIdobWjZfs&v6sT23Wcx)@YR=9T6qr55QZWR`%_
zerk#mBt_(b;#ol-)b7a1%uCEk*8{mx18ToAL|Rt?S|Wg30^qnz%vHz)M;NSv0<}HU
z6EpMd!46bb02z~*lUW6AzCm4_r=+8xl?N+|p#?G6k^()b{W--Y#f2Jh59@%O21+3s
zIjMOFVNFdPg`(vAycAo%{Jd1nXah)l0j>??Em%_uE~p7k&}o?^C@B~rZN;FZq{I*e
zu04tsf)tWclM_KzE3`ohuF5<W$`kVx5)~LS^-}e~;S2F8G*Uq&1SoM7fa)hbhLDU>
zNJBP1Ev+~eRGp_5gM%$Mv7`hPALXf#b}KkXfHWrNrRSvTFhI%(P<shn<K`x2rz(K+
z23QTKNe!yHOHy+|EmTkv&r<;Bh~)gDqSWLPJqA!=21*GfAQvFzx{`baNXe6@0CkfF
zNExJ5%U8(G%P$8dlFU4X#FVncyyR3(J%-5qQb?99$p_h5R07Eai3%n81qwN-X(bBD
z`9*oDMWB{sW{D1iE38M8r~nQJg``S_+(dBeBeyiCB(op~lnwJr5|c}cAx34E=qZ2$
zp8;HQVyqh1%T3HnOiwK;R>;gP$S*2U2m)n3Ux*+BR1DN<g0V|7b5mjL;>uzeGp{ta
zpc2#|$}4~g6=de+CnslQ=7WR_KnvfM6^b&`GfEWli!xL5Abl(yP)rx4BK!f023WDK
zkegqWs!&jrnw(jj3MxmyZ5mLhLJ|*Ht}HXZv>09!fRjm4C5Xz%1eIf{3Xpais3DR8
zG6YnkfLhexm<QJeppIK!NoHOt$e_}K6i}BD))0ggj#dny{(lx10|Os7r2h|_pWnyN
zz)-`_z!1#Oz+lSHz`(=L!0?Wbf#D(_1H*DY28KpH28IMa1_paR1_n7k28OS^3=HRZ
z85kDuGB8Z$Wnh@Z%fJxN%fMj5%fKMY%fRr3hk@Y|4+FzR9tMVOJPZu8c^DX~co-Nm
zc^DXCco-Nsc^DYZaWgP%<7Qx};bvek<c7M524fzhMvjKSXb6mkz-S1JhQMeDjE2By
z2#kgRBm_X?I`~Hnp`F#y!B|oUV?jX+9d<>G=R<lSpspTh6b3XRkf)GXlwJzzq=G{l
zJY=F^i_&}3&}7I=Q;3fT4G71_E7;m9D8<Kv<}c#om8>9RB;XNs=>B0n(5#Y=254*z
zX%rrd3fKTXbesb^oDUhbh3k$sf{Yu$6l09`LnncdtU?w6jrkK8?FaiC6j~q*=YXbq
zz{5qL88|nr18dMB7?3v50156<3%H>mr33>4qzHszK8`j84MP_dRKkJ+BniTB1tu^B
zhLErVNrEt3fiX-0bPxt43Bt+><r$C>g<=KpP!(vLqF5mxGV%&i0mC5c!6Um+hOUB<
zR$hT#K_-gdGSd{G@^%UadIpf8Fqje00j8pYN+lhTGDx(;WQz(a6>JqibDTv5m3p~}
zB}JJPA^AZCl^Q{)3YEbFj7U9y6c>;ly5Lc5L~=8XRnP?uXcZ;qrR3-8K`0H-Ft>r8
z5sFpt&;^GJ!UTg@1#Mg=7=!!&VLS{ByLce|f7tr{|NIOLZ}=G)&hs-cEP{>&r13K_
z1n@I3NboZ-tm0!}h==z5nfVwPPVh1?Oyy-@2<2s9;NoRqcnmWWQm~GaqaiRF0;3@?
z8UmvsFd71*Aut*OqaiRF0;3^7K?uNlY9N~QZWefn5u%d>?bU<K0%6Fwgcb6@1Zb&9
zK}lwQo&sn#A38>%16s5K?nr{Bz!OUpKnwIB3r0c4fUvScdcHzZVsf@ZX@P<Uc%=wv
z;YCS)fi8F=JhNCKBQrfCwYWqfGcP$OGcOgje+lc>5i!sJvJQkv9B=?>0pZLvg-it-
z*xEP+T|-2F8NT8{FR`E?H7`X2G;@wvcMMwL3tn+-7OPiMl$ckX173BgiSP+Hj5L%=
zi%hM+3p!vcEx-|nd+DR59(X~Fh6$!o1(|tZqj1;_*QleQg{>EltPQ>{8m<Y+;aFEW
zL)SNh3;{1Ufvk-xL~>;zETjzd3=pQ_5XCtv1X~~tiXL!KA&+T=g4XFe=Yv;8Xn<G7
zLM#HU0m=le0V)D5Wz~QVx@kgU6CRVGRkX1T;Qs#;b_Rwu9FYD$Y=8ebeg=j<eg=j_
zeg+0reg=j+d<+bG_!t;w@G&r?^D!_4@i8z2@-Z+3@G&sx@-Z-o@G&s3^D!{6@i8#4
z@-Z+x<Yi#E!pp$W$jiXs#>>E<2VDcOl81qz7`o=)i-&=M3$!MHn}K0HHv_{wZU%->
zZUzP&ZU%;LTnr3*xfmGwxfmD{xEL5TxfmF3aWXKpaWXIzaxySjb22de=774Jf>LBu
z>u3m!hQMeDjE2By2#kin;0^&$D+putI7Yh|JY_h#2@X6rG`a~6lxD!#KyP#t9Aw=v
zc*7EChXQCx^5`bG(M@nv-2?|NcfdJLK|ul3UCqoZg>3172|@-}6rg)|6LWH)Tc8rb
zJ9$$TK-*8?rhq!LS%}VTkX~^{VnJ%OVJx&i3zA3b?84*?VDg|P1)0eipshNfExO>%
z0-$sc8v4mh%t5TL*C@{bsY_1G18=uT0xd{MECFqKfb^<C8&t}X9St%CvY!g>@J!H7
zF3{c&@D`);#Jm#7zDdyHjl2TAjN+2S<ZKO%Am|n=y`ogGZ!|QFbqqB%p`!<AiorXu
z&{XOufVXa;i9+=&D^!9qcY1z)ih_D_elcidsywk+AwQ`kF*7eU1+sr4Q31T=iva*Z
C$o@kB
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/.robotiq_gripper.py.swp b/python/ur_simple_control/util/.robotiq_gripper.py.swp
deleted file mode 100644
index c8c21fa7784dcb063d1e16b0a39ee21db92da85c..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|=|K+&fh^sD>eICL=?7esMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y-{y
zl9{7ll%JGel35s^UX)o-kXocyPzkYYlpGC#(GWlj0bT}UBSQm_pt6#pg0N61h&hT!
zLtr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMo0*h6tFPVGcYhPLH!#Jr5Vxa|4<MO6_0?@
zFm?Z+@?KE!%}^R9&kW_OL+QUz8YU0rj#8r`Fd71*Aut*OqaiRF0;3@?8UmvsFd71*
zAut*OqaiRF0yGGL#1sYwAqED9E>K5YfPn$l|3ARbz_6d6fng&*1H%G-28Jp83=DPr
z3=Dz%3=9GM3=CHM3=Bs63=I4D7#OzkF)&QvV_-1jV_>+$%fPUJmw~~Jmw`c^mx192
z4+Fyy9tMW>JPZt3JPZsfJPZtbxEUBKxfvKLxEUC1xEUC%xfvK_xEUDMaxpN>=3-!I
z;bLGg=3-z_;9_8q=VD-B<YHi$%*nvez{$W6&&j}G$H~B;#mT^+$;rSV$jQL)je~*V
zHU|U4It~VgehvnPJPrniAPxow6AlIj2@VE^U+fGFKiL@=7PB)j<gha^II}Y_Xs|Od
zJY-{FxXQ-B(96ca5YNWIV8F(}pwGs@z{|$KaGjNbVK*xS!!A|^h9Xu5hTkj<469if
z7;IP=801+P7=&3E7(OsFFx+8gU|7w}z)-`?z+lGAz#z=b!0?%gfnf&|14ADZ149lI
z14A|w1A`V51H%?ZNH}+b!Wttk6`;VXAh9ShSD`GiC^IoBC)G+J49Zd{$yX>&Ezx6u
zDpXKXQVLGZODR?;$w*a5EGQ_-FDS}PEJ;;J&d<$F%u7)y%S==#&QH!xErIF<t1L^*
zDNR+#Pg6*QTcV=?QeK{zSyHT!mS3a*qSX_Vv(*)fQi}`n^NLd;wx*<}Da042mc*AO
z7HJfx=A`Msjj&QEE-3<Wb4pXK6hiYd^YfxJ^Gb9S(sJ?>OJX&x7~t+MN-ZfZ%2NR8
z*Ne|Aj!#U^)<{V#Nz{a^0QnFkqF@VA1@VMlQEGCThM|Fx393r4K~N>dsd*`hIXN21
zxhZ<7dCB=HsbF{Nx%xT#yLkG!Ya+CkXO?6@jE9C-d`^CHwiVnF$_j}k`MH_Nu;46K
z02!`dlw1Z^lAN2Oplz$59+Ri8prEXfms+lnlbM&QkXfuylv-GtS(KUr4o^@7K`jBr
zL|SHEW^o4G6p(MBCZ#DURKsGvR-wAMq(}oCU7EE@aOI!~f~kQ9xI$(gC?euhGLuX6
zGD}i(i#0S69s!wctDqF@8iFuHSs^(;uehYBG`U2fG%vHT6rO6}_9!VKlxHNCr79$)
zrsgT6rlqAOmt>YfQv}HER*(p_QV1ymC9FJ!;?m^g)Z*f_(j0}N)a2BHlFa-(aDq$D
z)=@}H%qdP)$V^kn%U4KD&Q>T-1O--dYGzq#ijG2NUP@+iVo7FRIykKsr<N$>CRQrs
z<(I(Sg~%Xa|5_=yWF~`bNGz%ZSpo~1Vo*qf^lQLG;`0)7Q=uWI2}#f(=Tn-Ez(Ehr
ztHtExqhfFZ1v#q->L352l+>cs6p*)~!I=spH=$%F<m8{6lV6;wfk@*@N=nWkAxIgK
zoS&BmioE<h1#sdAB}>@4DOhZSvUo~nF;pqoZJG+Yb_#y^d8tTw7}?U|)RL0S+|>Ni
z5)DW$jE8a&?m$)!Gg>1fzqkYvB?b9KB}h3FNj0bh0GD8T5Tl_s=sCK@d-}PC=s?Ri
zy<mT5@A%-5AXi6Ugsm_?fnph&Y9OY-9B8Ew0&zGvmnMPoKPcrv6e;8vq!uNDQdTh{
zu|SOhIoL`eAioHyIEG5Wd}XEJ0rj5(s6;J5m;s6uNKAm!Xrcl%4ipkgz==LRvn(}F
zAu%PTD76@BH7HhK;RXsF9R-9n;3NWZ5V*jFxE|p}Z~?4ft6-#OfHNlJa}z7#)AEaw
zQ$Y$%P2rghWSL%kZe|`_)&N~Du_C^>AT>1wr%sqGHk}3e#hIWW#;F-e0h)ejfeJ3Q
z6>JqsGKx|YQ!?|?^?dS^vyt)u*b9(AQLqK4W4I3>F##zC^NaLg=>-(p@$s2?nI-Y@
zkQ{)N@*P9sU0j1h6l@iY;pPN~_y+{Iy2N|>`MCzg`#X8LI)fA#VJPqq4MA352v^|i
zALi-ju3)Qx@T8KGl7CWGYI2D}aY<rHDX5eL#d2zjLQ*9poj{j7DuiUDDrDxBq^749
zDS)esVg+!4U6QYmn^=;Zp-`RyOIi>u;F_!$?%(8`#NuKFh-JZGyEI($N^_CxY)9u1
z&oEaMPb({g`h^C&x`3h%RQrWwWEMlL2i1ScrA0-lc_lgEvRk3F7{l2LX_-a2<%vb9
z@Gt<|;TQr52Sm^Wxdyv}LK7uq++iL8J6EA7m8L-hwGB(yz}giGwhHQ@A#S=B;D&%>
zu(PKpxJ?BXQ%K4OH#JgoL3L7jeo;20G34e0@|U`sPY767T4HHViGqfKZGN_nLUC%U
zLT+MSX<`nzY)UOE$}cL0RGTFVRjEb!kYdi?$qQt)zmpf(h<r$bNl7h%mh&103bqQl
z`DK}T={gFA3bqRQr6s9F;1Wki!AQYYAu}%z&No)D1=Y$>LlR3st-lh8Jpn<W$X5>t
zas}H1&y}ENPHJ(9f<|g`Mn0(e%}FdSfwh=YQxsqdA*KX}ID%Xo9O4Kz1)8qGZUi@(
zic?D<u1o|qF~H3L9R-l9!2DE*A<7Ctu8uAWVU9tbj!r(V!Qk{0;2#V!Ilw;{Y;r+<
zv4R!cas>?oT_aOdO&tYLXyg~9=7IGD2Y~$)902iCaY3qrl>(%&f@%OObMp@Z8SUmD
z1U4Gf{IUYYNpdR8T(B}n7qBu%7l^XNlp+Nyg~Za5eBGkdoYchPRE3nxB3P_|+@N8o
z391<&RkneqLP}{7sPaM90`ah82*l(NsL3T5CTpbTrWU2A<|S7u6zAlZgCZOnSneVI
zAji9h_=6pvUXrh1rI4NvN{1TdnK?NM1*t`8`9--<F?h286yW)JpeAn#sBu}4U#tW2
zvkoYHpsoTJaq5oFki?sqTmtehB;HaLGz=BWGct2h6+i}+C6<8NcFBo(3Q4J;L<Q-P
zB<G|i7AZhdJtQeWOjcG15AqCgMZ~azhO?uef}>BcKRD_ks$hk+bAE1aX&$K2m|Bd{
ztbvAkPNfcVX9(=e;u3IhLu-*@9fh3Kvecr)bWj{X<v@)SNQVg4goIYdAiE)jW)Nig
zIkbSVVqi!s%Fk6OsVo2)lbH)Dl^~rV9fgq6f}B(x1*Fy;L~&|fX|4iPDX7?GfU-f2
zC^#EdxxoY=^%(<$l9G~deoARhDkw&wE9MoTj#4iM=ktQpqGAPIg_6|blGGGP2zfaA
zx#+rrd;#kJOS3aDTxN%?|A)=b|L135xWv!EFrS}+VIH*4&&|)kaEy<E;V2&iLjxZJ
z10S@%f18(qA)A+h;V%yZ!v!7&hBh7s1`i$v20k7JhB@2}3~JmA4Ewkk7&dV+FqCmI
zFvN2)Fi3MTFi3GRFzn`JVCdvzV94fVV94TRVDRE(U{K&>U^vFXz%YY@fnhoa149f4
z1A_qv1A_<$#GRKwZpG+7j|z;2z-S1JhQMeDjE2C74gt_;2M8BIhFQ}x^AdC7!3{WQ
zy8zl)(L?F*L6qeuWkFh3AOqk+3bqQ#rA6`iNm<~QE<z5}%!Urp#-}3<3+n~=2P4c;
zhIY@uoz}$U5>TH8)-M2el0f57&`t-W9SG?jf$RZcsNJBUpwyf+Jye_goxBh>L7JTS
zya_c+Q^D32)=a_{9D0!cIMNs@#Q#LNL0JJbj;>IeSCW|n8n*;B#xwKMQN09e+vyeO
zq^1^V80Z-o7@~UtJZ6FF(SRUVO$9|;h2-26P;4TchGHQoDB;6N$=P}N<vFP->8YUc
zzr+%S%n}7~qrar664YLV^aDUzpxrp64jZa_ixM-7Qx$?hp2|&i1@%QWl-v?Cb5c`4
z=72_7;VooPHv#15R6QkxufW5-C7|hx_<W=xcF<G;sIv?86D)Ld^UG2-U?~LL<p6ab
zz{67rBSEcKQ0f5T643ZYW(jEAp&-AwI5P><9tMel{01`!Jkp$(59-w>CTD<5%uG{A
z%u|4-JkU5aq~DyH0_%(A=7Ywj5F>YBdq8G_T&R$dm;&nQ7UZOYyElojaT=sC?2P<!
zaH<EZPt8k7O;OMQ_3V+eo&vZ70P0qzq?ROR<`iq{Dfs1=AjU=D<LizJC7?b8qz#{#
z2Osk(N-Zc#El$mY51WHK;c$OL3Ifaln8Y;DP(Wr0D3xZWWu~UUb2oG-6CC1J3U1(W
z!IFH?D2_rjd@N8$0X}FLi>?#gi?&h-29FDZhN1GyQbEI;VBKIzh;A?wLpRLjpb=K+
z@F2{Ll6;5@U`imyz<3CgN-|ROz!?$Je}l(aNj^Nv5YY;s0f2P|!HK#gv8V);kU>4U
zlKk?-BIryTWOx>4r4A%EKo%4iq$Y#I5!6XeO#v5e;2;D~3xI<NG_nXv3-O70De<6k
z0yL}&%Ff_nMjiNwg%v1np(!8A2Ac$CgGX_}y-COn8**ZbwE_*r<>eP9A~G-}{vlIc
zu-Us}(5wh}zClw*Avq_r0OSa;6G8c@ED<snp<s*LUDb#NyAo8mBlKwMfKqf$nqF{#
zD^#?&AQdj^<{tzVP0KHWi@JyS>nIp%#v*dFGAN`$#Z*2xzo3ROB8Va4;8cSMUW8A<
z9s%VfTZo^^5_2F<MvP#ChcS{<K?7`{x&)qt5l#Uc0vZg$Y6#eFXv`oR0v1Oz1X4YM
z%|mqwBJm(tkRl)C9+;RW!egLFfktC)Vuc1M50)k7=qTi7=7A;%A-SL|F$ZZ91(ej_
SR>L(wbR|~AgA`jaFaQ9DW}8$1
diff --git a/python/ur_simple_control/util/__pycache__/draw_path.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/draw_path.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4e8050f7d0467e3e6bb82fad86dbd2b2d79b9265
GIT binary patch
literal 2061
zcmd1j<>g{vU|{f9h)hl4W?*;>;vi#Y1_lNP1_p*=9|i`76owSW9EK=HFwK<99L2&2
z5@X6?&1H*XV`Ok=NMTN4X<<lVX=aLIPhm}AYhj4uNaajn&t@u8N#$r}U}Q*P3<hIO
zj+Y>N{4^PF@%khd7v~43=A<T<<QFA_gpn~5%n3RS3=FAY>!X-b7*m*97^0XV7Dll^
zEn`h#PvK}`h+<3O3}(>ey2bBOlvo~+Sd!tJn3tHIT9gbj3}!fp&BDOI;0*Gg90LPG
z4PzEV7Gn)VJYxw{4Py;M7IO+?FH;ReJWC2gFoPx&$gv#xdBtFtYO>y9OUq0zElRz`
zmYkSZmRMW_GO!3_;w{!3ke4)>Zm|@n=A_+XN~~DPPy`C;UupUo`MIh3>G{P4`6-D-
zV4e|zn^T&XqF+#ySdm$*Us@DjoS9pYlNz6#pI1_ppQB$;S(1^Thee{aBr``pr6{pH
zz96wAL$9Fn7Ds%1W?p7Ve7pq6cOXY`F^Vu&NuULY9!weTkdb0wU?^c&zz7NQ8b%j}
zX2yk}P+|gy3bS7_BP>K27#Kidz{<eD5Dc<bhJk@0ouP&ymaCSrhB1XflA(q%g;A0r
zooOOdAyY8JN~R)^-j^T(96Xv#x0v({ZZT$pc@Tn|fq~%`mrYJ)aY=H1Zh;*rriww<
z$S_ptpnC@vJ9;)b`N@en#ddlKjhf82nDPp4u_hK3C05>I0fm$%QxONqc-FGiqLN~W
zeV}M60$B=<N{|=8wn;Hz1|G~dln?-U2IM?&vI2#E4MP^g0>*_5j0`mlSxgI<K`e04
zG5Tq;6tRQs7fi`4PR`HEOHD3`Pc2K$D=98w1Br8j<1{lRCsmUX>{2u*-r_=NR04%K
zB%=8ktEA9F2B8omszFwPT^j(+1SyQ!OhsxXj44dbjEoE=psWC9)i6L@U&4~bn!*h7
zO$s>Y^|F9erLcolvn^nU`4H?&4!>Khsd*)-MO8eYAXg~J&&(?+wyF}*(+Bb6GxOpL
zGAmMZiuIC<%YHHHRI#Y)nWkwn-QrA1&B@HoEJ-c8#hjK~Qe_e1@8WNzP*j?ykerj5
zt&pFmP?C|V0M@JkGD@K&Um-U!J5?dGL?Kb3q$n{9T%uHJa@=ChPOZGfT2PdkS5hRx
zz`#%hikKoHP#CipCzhp_RFvFeF32gl#hRRxUz`e07vRW14|KM~<mA+X5=~Hm3otM+
za4~W)g0Kh^2O|#?%a1BC^uUK`@Y7@m<;%Rp+|>B^TU_z+x%nxjIUqJqe0*VPVh%*6
zhzI0AP)tHxsSM(RQze1`r%zDfEDmx6NSuR#gOP)UgN=v12qcbDjDpMpWlC@)`+@Uw
z3OGNfF!wUoGL$f8G1V}nu=Fy<Fx4{FGL<mbFf}vQGS@I>u`FOsVXa}#Vq3_>$WX(y
zfIWq6Avj4a-~dH(4RaQ#pk?<f;$>i9aMNVI#Zr-znR|<+5=8POmX_ofCnx5l#^>j0
zGT!1UE=?-P$uEh|F2BW}omv@RP?TC+3`y`+fgu^G3ZUeknU}7Rkys3h%%oHWLj_Rb
zmzh_ZUs|k?lbM&Qrw{<rrJ(K#N>l0}WsuMXCkBuKdEmsLr=ZDoizOv9xui$|6wMq^
z%ZtIm1Ib?8xdl0(!WES7ia^19i?cYjB)%lGBq#M2OL0biIXJOsvJ`27j9^a7OfTXD
zu{a=gKz<Q=(%=9&094_afZ~OZfq{XKU4W5;Nq_~EK-d_IjKSLD<8u=;^Wx(*xo)xM
zmF5;yLXx6jZej^2fO0aE^a?6LtXpgl8sY~bB>D2pl=RdRXr%*+aj?-vDmW6$Ee;z<
dYO@2CPsN~Q%fZ3J$iv3R&LPVo!o|q)9{_TC_&@*v
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 44309a4..84539ec 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -3,135 +3,152 @@
# where you use the previous estimate to try to hit the board at the
# right orientation, thereby making the estimate more precise
-
import pinocchio as pin
import numpy as np
import sys
-import os
-from os.path import dirname, join, abspath
import time
-from pinocchio.visualize import GepettoVisualizer
-import gepetto.corbaserver
-from rtde_control import RTDEControlInterface as RTDEControl
-from rtde_receive import RTDEReceiveInterface as RTDEReceive
-from rtde_io import RTDEIOInterface
-import os
import copy
-import signal
-from give_me_the_calibrated_model import get_model
-
-def handler(signum, frame):
- print('sending 100 speedjs full of zeros')
- for i in range(100):
- vel_cmd = np.zeros(6)
- rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
- exit()
-
-
-rtde_control = RTDEControl("192.168.1.102")
-rtde_receive = RTDEReceive("192.168.1.102")
-rtde_io = RTDEIOInterface("192.168.1.102")
-rtde_io.setSpeedSlider(0.2)
-while not rtde_control.isConnected():
- continue
-print("connected")
-
-signal.signal(signal.SIGINT, handler)
-
-urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
-urdf_path_absolute = os.path.abspath(urdf_path_relative)
-mesh_dir = "../robot_descriptions/"
-mesh_dir_absolute = os.path.abspath(mesh_dir)
-model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
-
-init_pose = rtde_receive.getActualTCPPose()
-new_pose = copy.deepcopy(init_pose)
-#new_pose[0] += 0.05
-#new_pose[1] -= 0.1
-#rtde_control.moveL(new_pose)
-#exit()
-
-# max offset is (+0.3, -0.3)
-# generate 10 and try them out
-# but let's try to get it done with only 3 to begin with
-
-
-def estimate_R(positions):
- # find the angle
+from ur_simple_control.managers import RobotManager
+
+"""
+Estimate a plane by making multiple contacts with it.
+You need to start with a top left corner of it,
+and you thus don't need to find an offset (you have to know it in advance).
+TODO: test and make sure the above statement is in fact correct.
+Thus the offset does not matter, we only need the angle,
+i.e. the normal vector to the plane.
+Returns R because that's what I wan a
+"""
+def fitNormalVector(positions):
positions = np.array(positions)
- #print("positions")
- #print(*positions, sep=',\n')
- # the offset does not matter, we only need the angle
- # i.e. the normal vector to the plane
n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
#print("n", *n, sep=',')
# normalize
+ # TODO why am i not doing this again?
#n = n - 1
n = n / np.linalg.norm(n)
- #print("n normalized:", *n, sep=',')
+ print("if the following give you roughly the same number, it worked")
for p in positions:
print("cdot", p @ n)
+
+# constuct a frame around the found normal vector
+# we just assume the x axis is parallel with the robot's x axis
+# this is of course completly arbitrary, so
+# TODO fix the fact that you just assume the x axis
+# or write down why you don't need it (i'm honestly not sure atm, but it is late)
+def constructFrameFromNormalVector(R_intial_estimate, n):
z_new = n
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
# just fix the signs idc
+ # TODO: find a reasonable solution to this sign problem
+ # --> idea: use signs from inital R estimate
+ # and just force actually estimate R to have these
+ # old code stays here until fix is proven to work
+ #y_new[0] = np.abs(y_new[0])
+ #y_new[1] = np.abs(y_new[1]) * -1
+ #y_new[2] = np.abs(y_new[2]) * -1
+ #z_new[0] = np.abs(z_new[0])
+ #z_new[1] = np.abs(z_new[1])
+ #z_new[2] = np.abs(z_new[2]) * -1
y_new[0] = np.abs(y_new[0])
- y_new[1] = np.abs(y_new[1]) * -1
- y_new[2] = np.abs(y_new[2]) * -1
+ y_new[1] = np.abs(y_new[1])
+ y_new[2] = np.abs(y_new[2])
z_new[0] = np.abs(z_new[0])
z_new[1] = np.abs(z_new[1])
- z_new[2] = np.abs(z_new[2]) * -1
- # y is good tho
+ z_new[2] = np.abs(z_new[2])
+ # y is good 'cos it was obtained with a cross
R = np.hstack((x_new.reshape((3,1)), y_new.reshape((3,1))))
R = np.hstack((R, z_new.reshape((3,1))))
+ # now ensure all the signs are the signs that you want,
+ # which we get from the initial estimate (which can not be that off)
+ for i in range(R.shape[0]):
+ for j in range(R.shape[1]):
+ # TODO ensure all signs are the same
+ continue
+
print('rot mat to new frame:')
print(*R, sep=',\n')
-
return R
-# TODO change this, i just shoved the pen through the board
-speed = [0, 0, -0.3, 0, 0, 0]
-n_tests = 10
-positions = []
-R = np.array([[1., 0., 0.03236534],
-[ 0., -0.82404727, 0.56559577],
-[ 0. , -0.56559577, -0.82404727]])
-
-for i in range(n_tests):
- rtde_control.moveUntilContact(speed)
- q = rtde_receive.getActualQ()
- q.append(0.0)
- q.append(0.0)
- pin.forwardKinematics(model, data, np.array(q))
- print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
- print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
- positions.append(copy.deepcopy(data.oMi[6].translation))
- if i < n_tests -1:
- current_pose = rtde_receive.getActualTCPPose()
- new_pose = copy.deepcopy(current_pose)
- new_pose[2] = init_pose[2]
- rtde_control.moveL(new_pose)
- new_pose[0] = init_pose[0] + np.random.random() * 0.3
- new_pose[1] = init_pose[1] - np.random.random() * 0.2
- rtde_control.moveL(new_pose)
- # fix orientation
- rpy = pin.rpy.matrixToRpy(R)
- print("rpy", rpy)
- if rpy[0] > 0.0:
- rpy[0] = rpy[0] - 2*np.pi
- # who knows if this is ok
- new_pose[3] = rpy[0]
- new_pose[4] = rpy[1]
- new_pose[5] = rpy[2]
- rtde_control.moveL(new_pose)
- R = estimate_R(positions)
-
-
-current_pose = rtde_receive.getActualTCPPose()
-new_pose = copy.deepcopy(current_pose)
-new_pose[2] = init_pose[2]
-rtde_control.moveL(new_pose)
-rtde_control.moveL(init_pose)
-
+def calibratePlane(robot, n_tests):
+ # TODO:
+ # - tell the user what to do with prints, namely where to put the end-effector
+ # to both not break things and also actually succeed
+ # - start freedrive
+ # - use some keyboard input [Y/n] as a blocking call,
+ # release the freedrive and then start doing the calibration process
+ init_pose = robot.rtde_receive.getActualTCPPose()
+ new_pose = copy.deepcopy(init_pose)
+
+ # TODO change this, i just shoved the pen through the board
+ # the point is go up, but the frame is the TCP frame
+ speed = [0, 0, -0.3, 0, 0, 0]
+ n_tests = 10
+ # TODO: for the love of god please actually read this when you begin
+ # instead of having this horror here
+ # NOTE: old code remains here until fix is tested
+ #R = np.array([[1., 0., 0.03236534],
+ #[ 0., -0.82404727, 0.56559577],
+ #[ 0. , -0.56559577, -0.82404727]])
+ # get q, do forward kinematics, get current TCP R
+ # NOTE: this is supposed to be run only after the initial TCP has been set
+ # TODO: move [above_sentence_code] into this function to ensure that's the case
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, q)
+ # this apsolutely has to be deepcopied aka copy-constructed
+ R_intial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+
+ positions = []
+ for i in range(n_tests):
+ rtde_control.moveUntilContact(speed)
+ q = rtde_receive.getActualQ()
+ q.append(0.0)
+ q.append(0.0)
+ pin.forwardKinematics(model, data, np.array(q))
+ print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
+ print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
+ positions.append(copy.deepcopy(data.oMi[6].translation))
+ if i < n_tests -1:
+ current_pose = rtde_receive.getActualTCPPose()
+ new_pose = copy.deepcopy(current_pose)
+ # go back up (assuming top-left is highest incline)
+ # TODO: make this assumption an argument, or print it at least
+ new_pose[2] = init_pose[2]
+ rtde_control.moveL(new_pose)
+ new_pose[0] = init_pose[0] + np.random.random() * 0.3
+ new_pose[1] = init_pose[1] - np.random.random() * 0.2
+ rtde_control.moveL(new_pose)
+ # fix orientation
+ rpy = pin.rpy.matrixToRpy(R)
+ print("rpy", rpy)
+ if rpy[0] > 0.0:
+ rpy[0] = rpy[0] - 2*np.pi
+ # who knows if this is ok
+ new_pose[3] = rpy[0]
+ new_pose[4] = rpy[1]
+ new_pose[5] = rpy[2]
+ rtde_control.moveL(new_pose)
+ n = fitNormalVector(positions)
+ R = constructFrameFromNormalVector(n)
+
+
+ current_pose = rtde_receive.getActualTCPPose()
+ new_pose = copy.deepcopy(current_pose)
+ new_pose[2] = init_pose[2]
+ rtde_control.moveL(new_pose)
+ rtde_control.moveL(init_pose)
+
+if __name__ == "__main__":
+ robot = RobotManager()
+ # TODO make this an argument
+ n_tests = 10
+ # TODO:
+ # - tell the user what to do with prints, namely where to put the end-effector
+ # to both not break things and also actually succeed
+ # - start freedrive
+ # - use some keyboard input [Y/n] as a blocking call,
+ # release the freedrive and then start doing the calibration process
+ calibratePlane(robot, n_tests)
diff --git a/python/ur_simple_control/util/draw_path.py b/python/ur_simple_control/util/draw_path.py
index 41498f2..b93166f 100644
--- a/python/ur_simple_control/util/draw_path.py
+++ b/python/ur_simple_control/util/draw_path.py
@@ -20,7 +20,7 @@ from matplotlib.widgets import LassoSelector
# documentation and examples on Lasso and Path to see more.
#from matplotlib.path import Path
-class DrawPath:
+class DrawPathManager:
def __init__(self, ax):
self.canvas = ax.figure.canvas
self.lasso = LassoSelector(ax, onselect=self.onselect)
@@ -36,21 +36,20 @@ class DrawPath:
self.canvas.draw_idle()
-# function we bind to key press even
-# made to save and exit
-def accept(event):
- if event.key == "enter":
- print("path points:")
- print(selector.path)
- selector.disconnect()
- ax.set_title("")
- # TODO: ensure this runs fine after packaging
- np.savetxt("./path_in_pixels.csv", selector.path, delimiter=',', fmt='%.5f')
- print("TODO: run clik on the pixel path to make it a trajectory")
- # plt.close over exit so that we can call this from elsewhere and not kill the program
- plt.close()
+ # function we bind to key press even
+ # made to save and exit
+ def accept(self, event):
+ if event.key == "enter":
+ print("path points:")
+ print(self.path)
+ self.disconnect()
+ # TODO: ensure this runs fine after packaging
+ np.savetxt("./path_in_pixels.csv", self.path, delimiter=',', fmt='%.5f')
+ print("TODO: run clik on the pixel path to make it a trajectory")
+ # plt.close over exit so that we can call this from elsewhere and not kill the program
+ plt.close()
-if __name__ == '__main__':
+def drawPath():
# normalize both x and y to 0-1 range
# we can multiply however we want later
# idk about the number of points, but it's large enough to draw
@@ -60,11 +59,15 @@ if __name__ == '__main__':
subplot_kw = dict(xlim=(0, 1), ylim=(0, 1), autoscale_on=False)
fig, ax = plt.subplots(subplot_kw=subplot_kw)
- selector = DrawPath(ax)
-
+ selector = DrawPathManager(ax)
- fig.canvas.mpl_connect("key_press_event", accept)
+ # map key press to our function
+ # thankfully it didn't mind have self as the first argument
+ fig.canvas.mpl_connect("key_press_event", selector.accept)
ax.set_title("The drawing has to be 1 continuous line. Press 'Enter' to accept the drawn path. ")
-
- # this keeps running until you close the window or hit Enter
plt.show()
+ return selector.path
+
+
+if __name__ == '__main__':
+ drawPath()
diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..88efef940c25032d0c34f0ec64c58af7190861fb
GIT binary patch
literal 199
zcmd1j<>g{vU|`7ViAV*}k3j@7W@KPsaA06yC}v?`U`SyIX3%8xTggxa5=Icevh*|Z
zb5r%x^NS1eQxc27JR<}*r!+4`zo00wBC}Y(v?#tfGq)foH9k2%ucRnHN57!5BqKi$
ui$qywacN>sW>u<we0*kJW=VX!UP0w84x8Nkl+v73JCGZSnHU%tSQr49+%|~-
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..68ca46ae7f81ca421c192f85191a01c73b82c31f
GIT binary patch
literal 816
zcmd1j<>g{vU|<lL7?B#m!octt#6iYP3=9ko3=9m#3JeSkDGVu$ISf&ZV45kHIf{jm
zA%!W0xrHH$HH9&lL6hYrNUvWqNEe6&!rTlD3>*v$49*}^4lyt=lrSt{tYOSzTF6+-
zl)_lcoWfYkQo>xr+{{?QT*K1LD9%vJn#oYhR>G3Sx`3^OVF5eHjuMU<hGxcv%nKQ7
z*-JP<Y!`-D(-@{&j#|zVmIYijtR>7f93b5b8Ee>TIBgh8xIubqI3*dH8Jihh7-Ge1
zxj-_K3|TxiOf?K3nG}W;hE}E;t~4e|h8p%74oQX*-V~Nz7Dk3b8;IHkd<z*E846WP
zSQhYuSSbv_44SNdFF{@`Vq{=ocnKmjnQpNr=jY@X-D1s2OiIns6u8BdS8$6hu_!Sw
zJ@pn}Zem4zW=U#MVo7FxUhysF;*z3U%mq0mx0sT1Z!u@3mfYgV$;>M*NK8(>#hjCx
zcZ)MMuQV5=KJ^xRacNRPPJYQPR)}FNU=DjqVr6`GYUM4qoYeHxyp&rk#TohKn%uWI
zLDKOli6x1*SQ3lUi*GTO++xd%PtMON2GK?N<;A!7p+<s4<BRgkZ?Pv<#HVB?m)v3l
z`=a<3W9BUmkQrbr7_(L~6e%(=F#O8a&&bbB)lbhaF33+wECTb45Zs*7ycGR{qQr{K
zV*S#h_~Ojmf}GU&<ovvnqWm2Fg36MN{5&iYWtqjLi8+~7srqo1UP0w89*|Gmit=+^
zGLuU{NvK$Yfq{XCg@uWQQGkhsQHn``QH)W9QHPO_nTN4RnSp^plkFC3UTJPYWf2<#
z1H&!B+{6-)Avu{zdIgmr)-5&&T?9(@U@c&eArVR-jT|<)`6;D2sdk{qEe4sz!N9}F
G!v+9|;pD6U
literal 0
HcmV?d00001
--
GitLab