From 2954e748eedf7c87a6e51c5f5f5872068d5f4b63 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 21 Oct 2024 18:43:45 +0200
Subject: [PATCH] hopefully didn't break while trying to get the demo to run.
 there's nice interpolation for the following oc solutions

---
 .../__pycache__/managers.cpython-312.pyc      | Bin 42107 -> 42125 bytes
 .../basics/__pycache__/basics.cpython-312.pyc | Bin 7809 -> 8500 bytes
 python/ur_simple_control/basics/basics.py     |  60 +++++++++++++-----
 python/ur_simple_control/clik/clik.py         |  10 ++-
 .../dmp/__pycache__/dmp.cpython-312.pyc       | Bin 16448 -> 16312 bytes
 python/ur_simple_control/dmp/dmp.py           |   3 -
 python/ur_simple_control/managers.py          |   5 +-
 .../optimal_control/optimal_control.py        |  12 +++-
 .../calib_board_hacks.cpython-312.pyc         | Bin 15413 -> 15476 bytes
 .../util/calib_board_hacks.py                 |   7 +-
 10 files changed, 69 insertions(+), 28 deletions(-)

diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index e8f0cf9924bbd4c123fcdc9cf58e2b3c93cdaf56..c40c23c490c43a9b9001a93bc44c2fce0a62027b 100644
GIT binary patch
delta 397
zcmex;f~of;6W?iGUM>a(1_reZv2^oI8~LmSIMTp?&0%w(fD{wsnaw#uQ<%gah=^a1
z%l*L0z$)>Dfr&Mr@dFzJEB9s*(a!>mJ2rEune#9{-Rxi_#lz^hIoi&PiLrO{GKZ_e
zjB_^EgiK>$6q_s@xtdXO^Zv;BOpG#{<6|rs8Fe<#hz(+8G~E0;sh*uNbaQsrTSms(
z&5=2$SQtAds}$a1)SLXjFoE&n<b)z|#vPL{c!*E#FLLKdVb0cIU?@tOJW)ez^1Gr?
zc9tq`28I;Y$zH|QaCT3z`egk{oRh<fB_>ap$hG-iu`?rgS~)`nb0kAKqbB=iol+ep
z76}FhhRNaO*V$GvF)$R{Og5>A=C~-VvV!xHu;JuM6;F6s8IG}XyNNI#6J~T1+1y;Y
zjG6J~X02K;X2#!}OB)U{GIDM<Z&GDu6x<x!s=&&qu(_`5ITNGZWWRp@&0BkCFfw{<
q_UjknW(?b$Jgbb2v3K*W1)PkG4V#}VEMa2vW@Hrl9y2*^i7Wu(Xn@23

delta 385
zcmeA@$@KdK6W?iGUM>a(28L(HgwuCz+{kAwz;X5r2z;^M94H{g#CU3Rj?fe)ksBi7
z7vyq3Ff*`9d|_Z@&1ZbT!nIjU^s@lt_RU;s<~)o~Hai(f@i01Uj<xe*V(i(x!r`hg
z<Lu3KA=6kGMJJ0!u4a_jd@yo86QlIz#25=kM(xeBVuM&24K}|`s%K{m*_@m8mXWb$
zb9Bxr7RL6;YK6BLH6}9_B`}_we6UTNaqHxV+TxQxv<Xi>5XU)rZISlmdquh2%vIbB
z3@I#CtPBj3_Y^Bkev=}`kpdN*Jf&Ejk#+K(Vns&Q&G(AE8M%|o87i0~8Oj+o**2S$
z>M%`CEx*RLl8J$#*nF~MMKtpzVU@{CDxUB%F&ty!b`xYiCcx+>xOr;jGG@jbn{8^n
zm>GX<Zf`iq$jGtTvq_biQDAd^s{$*d{N@Q=&zTr)Cnxm#Z$92TgOSmFb3(reH)H7L
i(phC}j6Iv*EZ}5htl#`?VF?qP7bBz4_o&GQOJo5v=Y?1R

diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 82aa642f4eee74e77dad389ccccdaa2651892031..45e09e3e1cb3ddfb458602f0e07a7f0aa3eee846 100644
GIT binary patch
delta 1515
zcmZp)-QvV|nwOW0fq{X+vqda@wZ=xi01hS#mdObm+jvUYLE<1>!a4b_gzV(+9CEBY
zH9RH!ldU-AII?3H7>WeJ9C=Zm8m1IxNd^Xn8Xgc?%L`M+ldZwPP^4bNP^4PIIeD+7
zj7SMT$Vd>bVM<{IsjlIfJb_b0qLwd@10e*GsO5th!vQwNzl3A5wulT<mgwYPoTkhw
z2(C1<%w!ENUS_D{>a&>`7>bf=Sc?--O@#BfLFQL+F)-9Hr7(fn+zbr0{54!P+;9<A
z1_qFdS^-pDHT*RKRUkPA28LR`5^<1MKsZYRM1pZWm;@0uj9F438WqDs3+xOu<H3fn
zW`^jf<*nhZ;Q%{Bu!gUO6U-7y;j9%-;i?s>;j0m@5rjFWMx;gv!nR>xs1>adO<_pk
zmSU(Ct6|KNg&2oS)`+fVgzy;|Y6NT8Q+RAZ5hyhI1eYu~Zw-5j0GQ7=nS<LAniL*P
z4&b(C<e%KbE#ttNmXn`fbc-c9H8bZHb53gBE!L{kqWt0_J_ZJcB2XmXVlK$cyTzHA
zSCX1ulvt8_i#0btB{io=kb!|gQ+)C%Zbw(%lK9NLl+=p&oc!|CqFZooX+c41(Jj`(
z_@dObTdZYZiajweJtsB3>=u7XW^QVHN@iMGYEf!la_VGZ9wkmtG$??;WJ?}PZZ8G~
z25$xihT;;Y$+bMz^`a7UnC5fL<X9lMAaX^?!uX4dCYL2lFN>Hp_}&o_pTjhteJ1+?
z#s!`$QWpAOR4~3QVRBi-w88fS8v}>v9OfCG3sPqKUzX5WQFu|p;1avR9SLbvu@2rF
zJfag)I{jz(UgXhgaJ|7HI!Aa$<bsl!@s}laH+VB%lr+4=VYspOj)2f~j)@#F$rZ5|
zWvwqu+FTZ}?PPr*AUQ*2BL4#3ivoHbtRMIoIK^P<K!!lgm~76gTz`j`zdy1wvOlgf
zZieG!UbzJ#mv~igh)8sBeqaM>Lw7QV!A{Eu!ZI^LCfYBsy(nzB!E;B-M*jo87fpgL
z3I|;Xi@%VNaxpCRqEKoF`wdB{8L=xOH<YZ5zbI>SQPQS^{|1uFHi+-AufJ&Mby2{p
zgY^p+gP_QC-if?3Qs-yS%wA!+A!Mcf4%>^`o+m6XghrmUzYv{q(L3`(PW46a>dOK(
z9jqU?82CjdBz9&txZmIthB7#L8k}x`h!1QGyh0P4W{6MpxyYliz-R^gLYs>``VFo(
zc!eeyO>pdpY;e25!Z~?AuOYW`gGWc~MHcBBa>|=|`93pAfFiv}6O{URL4+lU043qc
zQi7V?wjiDwh_IXNC#cWr$H2hQx4B7hBa4L{NEDQQiUL5aKn4beTbzX{@rlVLrHMI3
zpln?f1QO)QO{|EACg+?Yx5?RJHmtD>3=E4luMs=KtO!zrFhY~1NE>8aD2M<{1x}tQ
z6)g(VP{acgggdavXELL-9i!c32Wb`7L<R<iPn$EPg&0|jtQZ&=CO63B34px@a&i$^
wd*bAOGEH2i3=9mQ99;Zqa+j=`gqb5Fqw)rq4-6psfXrtw{mG4K@>SV20G*_hKmY&$

delta 956
zcmdnu)M(3hnwOW0fq{XcN=h{SlF~-L01l=dOp_Bhwz1T3m+(&B!zsa$9mBv-WDDlV
z3Dk1eFcfK(aDw!J@Z>}p5vvkj5EF!Jm{M3J85kI9xIyG<Mvxc-1B^z<+JM!7%~WRA
zE#a7)D<i{{B|N!}%XIR8F1C866lRbC5MydMs<;>!YIzXa!E&5nQQj2JTD}ynT7EQH
zz8an?RtAPDkVX78yfAjHK#f2OLkhPPL#<#9W0okyfyiWyz-q?H@#37^JT>eoJYa|N
zPEO#FRYr9adx`*94#mThJ9r$~XUznGos%!{STk}@=HZo@tj{aWUL?rCz@RBKIfK`c
zgS{j^GcP5zV)8p)B}Spif_$QrwfWS!9T^xHoER7wiWf0Xj^(qi=NFoyG+lk7`V7Sx
znJZXk=3kW3y)2}6nNPpL{Q*1Q6y*t-3s@%RUlvlk#IANnNEFUpA$U<ptAqU}x5OQF
z&E*vfE4DCi@Z6EI(f@$^MH8RP2ELco{W?4+q;&evaKFf-@_~<mQ(%hmgvtdX6YDPv
ztFKVJD6D;nLwj-(zjEje5s41Y2Yli)SUU4(q+jGyTOoLnPpiTG1{NVso(88IAmRfX
z1Fsm|`~^iT+!xkd<T2imw8MO3&P5)l$;ASWvg#l&tw_HpXV&1+5qptE`i2}rX!Cx7
z&rDLF04dUBU|=W$C9on31_p*(thxCqsX3ER32Sm&f@IV{gw^DK!uqUk3=9kfn~g*^
zvRGJwM8!db2Z-<l>EtX-iBC)}DNW2Nk^l*MfdqMS6D#5~^GZ_FixP8+Y$r=g+OUQ)
zFfg=ij*&dVtO!z51UEvHrAQm3(-%a5r93A)%0`QVG!*fG1VK(Lat5(nCU2CrW3-z5
zK~{w|l7WHY%4SJ9Ax73)%mtZwlMUqa1i+pHIk^a|J#z9o`6jMB1_lODrYU|u*-F7o
cR7HnTd4tOb1|3G_12UgM)TbE6$sG!706UxVk^lez

diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 47977c2..8fd4b84 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -63,6 +63,15 @@ def moveJP(args, robot, q_desired):
         print("MoveJP done: convergence achieved, reached destionation!")
 
 
+# NOTE: it's probably a good idea to generalize this for different references:
+# - only qs
+# - qs and vs
+# - whatever special case
+# it could be better to have separate functions, whatever makes the code as readable 
+# as possible.
+# also NOTE: there is an argument to be made for pre-interpolating the reference.
+# this is because joint positions will be more accurate.
+# if we integrate them with interpolated velocities.
 def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
     breakFlag = False
     save_past_dict = {}
@@ -74,29 +83,50 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
     t = i * robot.dt
     # take the future (next) one
     # TODO: turn this into interpolation
-    t_index = int(np.ceil(t / reference['dt']))
+    t_index_lower = int(np.floor(t / reference['dt']))
+    t_index_upper = int(np.ceil(t / reference['dt']))
+
     # TODO: move to a different function later
-    if t_index >= len(reference['qs']):
-        t_index = len(reference['qs']) - 1
-        if stop_at_final:
-            reference['vs'][t_index] = np.zeros(len(reference['vs'][t_index]))
+    if t_index_upper >= len(reference['qs']) - 1:
+        t_index_upper = len(reference['qs']) - 1
+    q_ref = reference['qs'][t_index_upper]
+    if (t_index_upper == len(reference['qs']) - 1) and stop_at_final:
+        v_ref = np.zeros(len(reference['vs'][t_index_upper]))
+    else:
+        v_ref = reference['vs'][t_index_upper]
     
     # TODO: check if that's really the last
-    #if t_index == len(reference['qs']) - 1:
+    #if t_index_upper == len(reference['qs']) - 1:
     #    breakFlag = True
 
-    # TODO NOTE TODO TODO: remove/change
-    if (t_index == len(reference['qs']) - 1) and \
-            (np.linalg.norm(q - reference['qs'][-1]) < 1e-2):
+    # TODO NOTE TODO TODO: move under stop/don't stop at final argument
+    if (t_index_upper == len(reference['qs']) - 1) and \
+            (np.linalg.norm(q - reference['qs'][-1]) < 1e-2) and \
+            (np.linalg.norm(v) < 1e-2):
         breakFlag = True
 
-    error_q = reference['qs'][t_index] - q
-    error_v = reference['vs'][t_index] - v
+    # TODO: set q_refs and v_refs once
+    if (t_index_upper < len(reference['qs']) - 1) and (not breakFlag):
+        #angle = (reference['qs'][t_index_upper] - reference['qs'][t_index_lower]) / reference['dt']
+        angle_v = (reference['vs'][t_index_upper] - reference['vs'][t_index_lower]) / reference['dt']
+        time_difference =t - t_index_lower * reference['dt']
+        v_ref = reference['vs'][t_index_lower] + angle_v * time_difference
+        # using pin.integrate to make this work for all joint types
+        # NOTE: not fully accurate as it could have been integrated with previous interpolated velocities,
+        # but let's go for it as-is for now
+        # TODO: make that work via past_data IF this is still too saw-looking
+        q_ref = pin.integrate(robot.model, reference['qs'][t_index_lower], reference['vs'][t_index_lower] * time_difference)
+
+
+
+
+    error_q = q_ref - q
+    error_v = v_ref - v
     Kp = 1.0
-    Kd = 0.1
+    Kd = 0.5
 
     #          feedforward                      feedback 
-    v_cmd = reference['vs'][t_index] + Kp * error_q + Kd * error_v
+    v_cmd = v_ref + Kp * error_q + Kd * error_v
     qd_cmd = v_cmd[:6]
     robot.sendQd(v_cmd)
 
@@ -104,8 +134,8 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
     log_item['error_v'] = error_v
     log_item['q'] = q
     log_item['v'] = v
-    log_item['reference_q'] = reference['qs'][t_index]
-    log_item['reference_v'] = reference['vs'][t_index]
+    log_item['reference_q'] = q_ref
+    log_item['reference_v'] = v_ref
 
     return breakFlag, {}, log_item
 
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index a422e16..3decba0 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -183,7 +183,7 @@ def controlLoopClik(robot, clik_controller, i, past_data):
     return breakFlag, {}, log_item
 
 
-def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_controller, i, past_data):
+def moveUntilContactControlLoop(contact_detecting_force, speed, robot : RobotManager, clik_controller, i, past_data):
     """
     moveUntilContactControlLoop
     ---------------
@@ -198,7 +198,8 @@ def moveUntilContactControlLoop(contact_detecting_force, speed, robot, clik_cont
     #wrench = robot.getWrench()
     # you're already giving the speed in the EE i.e. body frame
     # so it only makes sense to have the wrench in the same frame
-    wrench = robot.getWrenchInEE()
+    #wrench = robot._getWrenchInEE()
+    wrench = robot.getWrench()
     # and furthermore it's a reasonable assumption that you'll hit the thing
     # in the direction you're going in.
     # thus we only care about wrenches in those direction coordinates
@@ -241,7 +242,7 @@ def moveL(args, robot, goal_point):
     send a SE3 object as goal point.
     if you don't care about rotation, make it np.zeros((3,3))
     """
-    assert type(goal_point) == pin.pinocchio_pywrap.SE3
+    #assert type(goal_point) == pin.pinocchio_pywrap.SE3
     robot.Mgoal = copy.deepcopy(goal_point)
     clik_controller = getClikController(args)
     controlLoop = partial(controlLoopClik, robot, clik_controller)
@@ -383,6 +384,7 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
 
     transf_body_to_task_frame = pin.SE3(R, p)
     q = copy.deepcopy(q_init)
+    pin.forwardKinematics(robot.model, robot.data, q)
 
     for i in range(len(path)):
         path[i] = transf_body_to_task_frame.act(path[i])
@@ -405,6 +407,8 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
     # and we aren't pressed on time when turning it into an array later)
     qs = []
     for goal in path:
+        #print("goal", goal)
+        #print("T_w_e", robot.data.oMi[robot.JOINT_ID])
         Mgoal = pin.SE3(R, goal)
         for i in range(n_iter):
             # TODO maybe hide pin call with RobotManager call
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index 01c73c9ba60d4bd126027f16c31dbe867c811c25..55668f2b64c34f7d4c980310fea151591c9ef526 100644
GIT binary patch
delta 303
zcmX@mz__EH?=&wj7Xt$WgPxsO`VIAod=r>N7$-9tOHJIhlaX(-0Hd-Ze+e5%0SKoE
zqzJCzSj_@rGcYhR)G!1yXbMe!#TYYr2Gb;dP3BuHC6xuKMXU@A3`J~{)tOs3pF`As
zu;09zIf9XK{bUwa)6EgAOPCq8Hvi@L!N_=P@;9z2j1il=xi2s>#&3?{-O9+wIa!E5
zpD}fD6F(zcE&~HYvBG3NA<4-L1=cebO%4>?&R9N~Q)nt<&E)w)HH`I}WrUYAGB#~K
zB9g?+*s)n%;wvNLgw3*2woHsuH%H6laxpI2d{Rw-g;8_zEv+|<rzaoRc49oWnM>y=
yBjbh57j#)z7_V=>YADUc_;B+_>!r+$=O$0Lk7T?x`J=rGt0<$;cZ11N4r%~Qu3C@)

delta 396
zcmdl{f1rVp?=&wj7Xt$W!;%Nw>8TnM`6e*BO`LyaGC!jVKVJzONCt#c_)`QX3ot4x
z3L=RKr3kO#Sj_^GXJBAts9^|Z&=i@xgE1zMMOQbwph~}>D8C@Ts3bEnM<F>sucRm+
zOc$3V=9MU<<rgUw<QHd_Waj57q!tzB7ZpE;nDoJ3llc})No7H55i0`&LlMXklZBZk
zPu{`QxY>z0f|2pu<k>8y5^PICZwNZw5E1{t%D^h|g@K7x@&g+KEBEG;EUTFrbvKuC
z{9t7KH93!K3S;zUX6_4&j7giX@N8vdWS`u{m(Q57_!l1|W5ML#{Abxp85kIfH75y(
zPkt&eo3Uzgqu^A=`pM!#HH=N0rwA=)WNh7RBa+0-*u8na_*X{8$(yH2+A=ZD*nC+!
zmy2=fW=Ayv7Dk=PL0WGZ&rP=1c49ocxk>vdBje@G9=a?njJG!X8cK69KG~dUy_A{p
a;$(jNNXB22GwoGaB^ZUin@yf%uLb}aRd==k

diff --git a/python/ur_simple_control/dmp/dmp.py b/python/ur_simple_control/dmp/dmp.py
index 348cb0a..5c3c490 100644
--- a/python/ur_simple_control/dmp/dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -20,9 +20,6 @@ def getDMPArgs(parser):
     #############################
     parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
             help="whether you want to use temporal coupling", default=True)
-    parser.add_argument('--kp', type=float, \
-            help="proportial control constant for position errors", \
-            default=1.0)
     parser.add_argument('--tau0', type=float, \
             help="total time needed for trajectory. if you use temporal coupling,\
                   you can still follow the path even if it's too fast", \
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 3e4cbf9..91b4b43 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -125,7 +125,8 @@ def getMinimalArgParser():
     #  environment interaction parameters  #
     ########################################
     parser.add_argument('--contact-detecting-force', type=float, \
-            default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+            #default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+            default=2.8, help='the force used to detect contact (collision) in the moveUntilContact function')
     parser.add_argument('--minimum-detectable-force-norm', type=float, \
             help="we need to disregard noise to converge despite filtering. \
                   a quick fix is to zero all forces of norm below this argument threshold.",
@@ -701,7 +702,7 @@ class RobotManager:
         if test:
             if not self.pinocchio_only:
                 q = self.rtde_receive.getActualQ()
-                if self.args.gripper:
+                if self.args.gripper == "robotiq":
                     # TODO: make it work or remove it
                     #self.gripper_past_pos = self.gripper_pos
                     # this is pointless by itself
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
index e3fb969..4da6ba3 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -14,7 +14,7 @@ def get_OCP_args(parser : argparse.ArgumentParser):
                         help="time length between state-control-pair decision variables")
     parser.add_argument("--n-knots", type=int, default=100, \
                         help="number of state-control-pair decision variables")
-    parser.add_argument("--stop-at-final", type=int, default=True, \
+    parser.add_argument("--stop-at-final", type=int, default=1, \
                         help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
                              if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
     return parser
@@ -125,7 +125,13 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
     # NOTE: there are some possible parameters here that I'm not using
     xs = [x0] * (solver.problem.T + 1)
     us = solver.problem.quasiStatic([x0] * solver.problem.T)
-    solver.solve(xs, us, 5000, False, 1e-9)
+
+    print("set goal:", goal)
+    start = time.time()
+    solver.solve(xs, us, 500, False, 1e-9)
+    end = time.time()
+    print("solved in:", end - start, "seconds")
+
     #solver.solve()
     # get reference (state trajectory)
     # we aren't using controls because we only have velocity inputs
@@ -143,10 +149,10 @@ if __name__ == "__main__":
     robot = RobotManager(args)
     # TODO: remove once things work
     robot.max_qd = 3.14 
+    print("velocity limits", robot.model.velocityLimit)
     robot.q = pin.randomConfiguration(robot.model)
     goal = pin.SE3.Random()
     goal.translation = np.random.random(3) * 0.4
-    print("set goal:", goal)
     reference, solver = IKOCP(args, robot, goal)
     # we only work within -pi - pi (or 0-2pi idk anymore)
     #reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
index e346abfc3404c5e04c001cf4c8c41732952a3921..9550e0a266df8ffbbcd7f29cd2dc60a649e809dd 100644
GIT binary patch
delta 390
zcmdm5@uh<IG%qg~0|NuYr8KehZyR}2nAm5{1c8&AE0}_qJU22hFf=gSVBxyXB7TuY
zd<OrT)Ek1HAJ`Z~B|mU7uu6PkU}2T|z{SAI4N-7GLSs$o4M8t-6?~idSv^=8tv5$-
z39&FnZO-Pa<zt+;`K63AGh@kSU4@&>jHf2EtGs98;@kW~WiKP^c?O1B-pOku<Tl?>
z3ub1#J^7rW_vUgfA!adNkaIyeg};VlH4})%z`!_JQCnv6VqFu)`IB$xrt)86VJJZ;
z6xi&e_mz=RaI=#^JtM17jo@mg$$omllO>IoF$zyUU?eIj0uqB7#ZbdlBUr<l#vII`
zDLVO`QJlC~UTV2QL4I+nLRx;2LP}~$YH~?te%@wpV|6CBcMJ>+#SNP)O!b*LE(mF@
z@VLTbI(dmjIC~-^1E0*7l+8?*`<NJSO+ILC%__<$^g(Yjqm4GFD-)wqkt72H0|1Gv
BZdL#Q

delta 339
zcmexTv9*HtG%qg~0|NuY&qv(pT^o5*nAkx)28IipE0}_qT-P!%Ff=gSVBxyXB7TuY
zd<OrT)Ek1H4@4wBurshqd|_Z_mHNQJz{(Agz96Boru2rO7pgqZW?@zjRz}OsF<e3{
zj1imj`D*zX=WKo}<IK!hwAoPMCNtxS$=oXMnLhDu{-d&&k@YYGLoM&*4H9yj@2CYc
zGoIgUpcTtJIoC*P@_t<t#>tbv=%(@?V__)ag*cUgA%%Z)hTd1k$rge<n==hc7#Rg8
z-!uHHFSMEwqJj}wt#FOtY9^R?4Ofj|4Qm>6FoUMZWG>^l%?-wCOl)r$7#NC6H?J|(
zXP*4toOAMf3vc##Mg~5aFG-snE%z}o{+eWC%___&^g(N~gN-()3lpPKkt72H0{|MZ
BU6=p>

diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index d53d648..1d7887c 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -16,11 +16,13 @@ def getBoardCalibrationArgs(parser):
     parser.add_argument('--board-width', type=float, \
             help="width of the board (in meters) the robot will write on", \
             #default=0.5)
-            default=0.25)
+            #default=0.25)
+            default=0.20)
     parser.add_argument('--board-height', type=float, \
             help="height of the board (in meters) the robot will write on", \
             #default=0.35)
-            default=0.25)
+            #default=0.25)
+            default=0.20)
     # TODO: add axis direction too!!!!!!!!!!!!!!!!!
     # and change the various offsets accordingly
     parser.add_argument('--board-axis', type=str, \
@@ -303,6 +305,7 @@ def calibratePlane(args, robot, plane_width, plane_height, n_tests):
             # THIS IS Y
             if args.board_axis == 'y':
                 new_pose.translation[2] = init_pose.translation[2] - np.random.random() * plane_height
+            print("new pose for detection", new_pose)
             moveL(args, robot, new_pose)
             # fix orientation
             new_pose.rotation = R
-- 
GitLab