From 5dc79e421c2234e6af6c94323e6d25cfc22b966b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 16:31:48 +0100
Subject: [PATCH] cm

---
 python/examples/.cart_pulling.py.swp          | Bin 28672 -> 32768 bytes
 python/examples/cart_pulling.py               |  46 ++++++++++++++++--
 python/ur_simple_control/clik/.clik.py.swp    | Bin 36864 -> 36864 bytes
 .../.crocoddyl_optimal_control.py.swp         | Bin 0 -> 16384 bytes
 4 files changed, 42 insertions(+), 4 deletions(-)
 create mode 100644 python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp

diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp
index 83deee3585cfdf13cb710ae494ebc8cd13b7ee48..71408b9b7087f65ed9fe6b5d1f62aec4aa34e49c 100644
GIT binary patch
delta 1445
zcmZp8z}V2hB$Q+j=IN_op=ZRvz`!8Dz;HKiqtFL_el~`B1_lNukRSsCL(pVF0dY-s
zC{G$n2S8~?D9r+;y*C>QtmmJkz`|?Jz`!sYBrU+e5VToP!Jbi3iiv@NlZk<04<kfs
zCL;qwAxH^Gvx0(x!sH@Foyj?>jkbwJsV=37IqpT71qG=^#X+e#i6xn3sUi7JiN&e@
z-WrKT>BTw<>G_E{@g+rxdBth@MY%c(MfpkjC7M>7qm@?5^4c>nFtmdV7hquUo-F9#
zU%!}#fgz5Efgyy4f#D}N1H&3_28J$f1_n=V28KUe3=FTh7#NOmF)$qDVqj?EVqnPO
zVqoy(VqnnWVqjq5Vqkd3$-r=plY!v~Cj-M_P6mdpoD2-JIT;voI2jnSIT;vYIT;v?
zIT;ufI2jn;axgHg<6vOu<zQf_;$UFV;9#g{_{q+|(9O=kP|D80pw7;~u$_&8!JUnP
zL57WiVGb(;Ll-Lp0|zSuLmLYNLn{jdLjnr}!#idMhQ-Vb3{#mI7$TS%7$le(7_Kof
zFsx=`V2Ed8VDM#PV31*AV0gg@@ooVV1A`?K1H)}b28M1%28I++5P-s$VRD?KqyQ+C
zk@4n6mw!yG3?QM&_3j}|@kNtuJxnIIyZbQ3=YW~h-GnB$yEjbU=&C(=pCZR(ZVw}v
zDmWLSa-SlHld^)ZV{ou%n5#mFzl*<>f^(3oV~DGQqk@a4o11Hpt6zvhkSmzs?5g1A
zAEe;w=<K237~~6f0@xY-Zc<Q-(k8EU)t_AG;kTL3vzoacB%5B8SX|)mt)l>D#24fj
zrz+SgP?B807N_PEr&@toxGkAH-$#4$Mpxy{r+mzm>l0HL7}U5J7}jw?@(wIxY~*KP
z$mC~Wu;6E4IK;=m(8I^T5YETI5XQ&ApvcF-@Q{~*;Q=oL!$w{PhH7301}9zy23}qU
zhHE?w47+$37&3Sm7<70T7*29CFl^;!V3^6xz%YZGfx(uWf#Dq&1H&^e28L@~4D4Ww
zA9HPHWD($+yupHHa+#vY<|0Kl#>v`hGEo|N1$sG|d5JmcdU^Rpxf-cOMe${+$tC$k
z@j0n!C7KF03PxInsk(-m3W<3s3V4(kWu|8!tDl^uwyQoKEUI9uP>`9Ymy@4vrV;F#
zT2z!@1d-6xgV?124pB6_vFQT42}Ki%c?z})A@Sw$sSwlk5@V7}JoCyl5J><cp$WDQ
zrW0f)TqB61mx!SaBn;67F(1UAoad)l4>k$id<9#iR0lFm1GNCs%gifFEh<jc(1e(Y
Uq#tB3A?=zBa9cN@R9?sk05gDbv;Y7A

delta 526
zcmZo@U}|{4D3oLn=IN_op=ZRvz`!8Dz;G&NqtFL_epZHh1_lNukRSsCL-1rl0dY<i
zD9?U#pul?mi4PPeDX{QzF)%O`F+%hPZx&RrXB1;(Vqn<B$iT3Qk%3_rBSgB8adKCc
z%H&Q(k;%8JZf&km>XGHmWM*J!1Q{y8z+gXF(80gHm4|`Bf`@@YjE8~YDmMc|2R8#l
z88-t%5H|yZFE;}NA2$QT4=x6Vt6U5WJGdAaCU7w@RB<sdBylk?1aL7h_;WEZIC3#C
zsB<we{NQ9@_|D0|@RE~(VLvAW!x~NohDuHb1}jbm21!l^1~yIxhV2{-45=Ip48PeK
z80y&>>KUTg85mx$F)++!V_@iKV_*nkV_*<sV_>+z%D}Lcm4P9Wm4U&Xm4QKwm4V?A
z3&i<HSQr?#u`n<!WMN?NXJKHFU}0boXJKHt%gn&g%*?=`%gn&=lZk=h5fcN$UM2>H
z0wx9qOC|<}+l&kh-HZ$jDWK2;2kqoIN6E=8IWn8i<-BK_oZuw4d70yVrpfC(I3^3Y
zvP@p%S~GdJd)8zV55LJ8PCQn*iP@<NnI#H&`6UV&nI#G(8L0{xiFqkGsY!`N3i(9}
r`6U^tMGD0rxy(GUbU{&ma%yq$=1Pwi=E;mcVv}opv^SsfF;fNr&-iVL

diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 1716263..aaf3bac 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -76,6 +76,32 @@ def isGripperRelativeToBaseOK(args, robot):
         isOK = True
     return isOK, grasp_pose 
 
+def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
+    isOK = False
+    # we want to be in the back of the base (x-axis) and on handlebar height
+    T_w_base = robot.data.oMi[1]
+    # rotation for the gripper is base with z flipped to point into the ground
+    rotate = pin.SE3(pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0), np.zeros(3))
+    # translation is prefered distance from base
+    translate = pin.SE3(np.eye(3), np.array([args.base_to_handlebar_preferred_distance, 0.0, args.handlebar_height]))
+    #grasp_pose = T_w_base.act(rotate.act(translate))
+    grasp_pose = T_w_base.act(translate.act(rotate))
+
+    grasp_pose_left = grasp_pose.act(goal_transform)
+    grasp_pose_right = grasp_pose.act(goal_transform.inverse())
+
+    T_w_e_left, T_w_e_right = robot.getT_w_e()
+    SEerror_left = T_w_e_left.actInv(grasp_pose_left)
+    SEerror_right = T_w_e_right.actInv(grasp_pose_right)
+    err_vector_left = pin.log6(SEerror_left).vector 
+    err_vector_right = pin.log6(SEerror_right).vector 
+    # TODO: figure this out
+    # it seems you have to use just the arm to get to finish with this precision 
+    #if np.linalg.norm(err_vector) < robot.args.goal_error:
+    if (np.linalg.norm(err_vector_left) < 2*1e-1) and (np.linalg.norm(err_vector_right) < 2*1e-1):
+        isOK = True
+    return isOK, grasp_pose
+
 def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
                            path_planner : ProcessManager, i : int, past_data):
     """
@@ -99,7 +125,10 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     priority_register = ['0','1','1']
     # TODO implement this based on laser scanning or whatever
     #priority_register[0] = str(int(areObstaclesTooClose()))
-    graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
+    if robot.robot_name != "yumi":
+        graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
+    else:
+        graspOK, grasp_pose = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
     # NOTE: this keeps getting reset after initial grasp has been completed.
     # and we want to let mpc cook
     priority_register[1] = str(int(not graspOK)) # set if not ok
@@ -127,10 +156,19 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
         # TODO: make goal an argument, remove Mgoal from robot
         robot.Mgoal = grasp_pose
         if usempc:
-            # TODO: make it not hit the handlebar or other shit in the process
             for i, runningModel in enumerate(solver_grasp.problem.runningModels):
-                runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
-            solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+                if robot.robot_name != "yumi":
+                    runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+                else:
+                    # MASSIVE TODO: CREATE A DIFFERENT REFERENCE FOR EACH ARM
+                    runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
+                    runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+            if robot.robot_name != "yumi":
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+            else:
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+
             robot.Mgoal = grasp_pose
             #breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
             CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 40352fd91f8c3c3fb526e55c0bc68ca62442726d..ff59baee54e7127200fd0fece90f165788b8c014 100644
GIT binary patch
delta 33
ocmZozz|^pSNi4}A%+puFLeGeSfq_AQf#F3=MEW+Njbg9*0hbI3tN;K2

delta 33
ocmZozz|^pSNi4}A%+puFLeGeSfq_AQfgw63BK-s3MzL4@0Fu%PRR910

diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..007e01f6afc624b2a6efd9ea7c340e95ac661700
GIT binary patch
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|{f%j7Xm^RKsv%9RovpesMv5N@5X63Lnl*EY8l%%fqJx
zELR6JP(LF-H`PcV&dn*!OVKYVO038%)-Nc^&q_@$Db_D7iZ9O0EyziYPtMOPDay~$
zFQ_cZ$j`$fkzY`fnVXmcSD0LspPZkPQkfHvCZktS33kaSF&Y9RF$8!SjExKpKt5Mi
zQdAHY3I#Do@n{H)hQMeDjE2By2#kinXb6mkz-S1JhQJ63fsz7FhI$4D1}3O~y`VHB
z8vP#%+@a#$P#UK0A5=a9Dn1EH!{q-$<%6K&ub?zc{tr~%7b<Q8m4H$VzoGJBQ29=%
z1e9X<1(k>CzYmpwQVc(#@*z<DI#3V8<e8!3f>62vO&%&dN{xoVXb6mkz-S1JhQMeD
zjE2By2#kinXb6mkz-S1JhQN>vfy5LB1{MYe1`kk2Sb%{6*8hLY&%kh%pMl{BKLbNL
zKLbNJKLdjxKLf)hJ_d#zd<+b0_!t<b@i8!D@i8z2^D!_4@i8#)@G&sF<Yizu&C9?r
zk(YrXi<f~xmzRNolb3<vFAoF5bRGtVC>{m|eI5pe58Mn4`?whx=5sSJBylq^sB$wf
z+~8thSjNS`P{_r=AjHMMaF3IL;Vvfw!zNA!hEh%j1_w?C1};tphKC#s3=26J7*aSG
z81y(87~ZfmFsx^1U}$7#V2EL7VBlwGV7SP}z|h0Sz>vwtz@W&+z`(`Ez_6E<fuV+#
zfx(-VfkBUzfkA|ofngsD14A|o0|O5W1H&<928Q*_3=DP53=E;n3=G=L3=CJ97#RAP
z7#Q3@;{i+z4Br?T7}hW{Fid5Hh8Z;070`gPLWsYMzm-CAMrv}lLS~vmNk(R|LT0f-
zNk*zdT2W$dszQ0HLSk}BX<|-Jr9ydPUWtN6YN|p>W`QOHnpq0PC5a`eI&fjI_JEwk
z<kZ~Mypo{Q;>?uN#GLpX1zQF9$_G7|uy1}!YK|MoLbwhMut}*o#i>?c7Rb7i)S}$X
zyu=*m{NfU@7QMuj6c9&4sVp_e4J4(c195m*YEFJ~W=SPTR!6}w)fnMmJf@@<Wfl~q
z76s%Nrz+_vq~|B*gcKzvXJ_W6!^|;4m{U}mmj{(Wa|}twfCCEIxhNq7vKMSRET}Xf
z;f(4_m?Xk+px^@Gg3LU<pwzU~qSU<PRB#CEh5HBjxL{Z5n_rTdpQn*mpjVYzlwYi2
zrm2aDQIK&UT$G=bU!s=_iZZ?Q)Dp0vo+%niCHeU|21-~oKwJ!uMHCOiT#n*tm>}5W
zVDn+&14$JiS15xBFiuKMPAn}>RVc|wEKx63NK`0DEGkjR2c_mzXsT35%u4|!<&^w9
z^%8~by!>*7G*D_$$jZ;mD^V!OFU|ycyciKG$_mN(#U%<RnoxzQsS0J#Bn8SK1_~O<
zi6FC5GK*4^OL8g|((;RPOLG!SQlVzTR3pL<JvpIx8Gk&(a|JA95Do<gfr0|Wqk4H|
zm@*)H^^)@oDmAdg4at$I3@#ipQ$V4Rn5U4LSCX2ZS_BSYXatmJB$g;77G#3Ui(+`R
z!yHQgLI)+FVIhb}TkybzB(BUf1xTQP$@n}_DX3_xpj26!o2djTi<A{Ka`N-D6%tFp
ziB6#;U!gcPRUsq49K=k{&n?KzN!3xvP0UVJfUE~oNXpDh$;?YHR!A*LhC~rKAd6B#
zDK<|bwIUI;iVT!$Q!7%zN)g4RLS~*qa(+&3L9#+oYC%3YM3q4{rQ|CV=j$j`=9enu
zrKYBUgusi@Kqan1qCye0&;@5zPy&Ux7GxT1?U^3bFi1TBb}+aONXyU3$u9@zrIHMo
zB5;l{)>KHzEG|jROHKvZ2TEKDx^@b|t{TRg3aLd!`9)wOD}qwNWhP44<4oiguvmcv
z4V;Uj8>R*vbHSk6238hU80es-e`xkK(p0E`2ZMq}epzZ!Vor`iVs3tE9=M3e%`XGj
zPY`=cNpf^4%t3I?kU9>xqn#mZ1Yynwmy7T$3w5%gra~!_lfjj5Nq!NyID%M&QGcOW
z2oK}n(p-o|5MAiC6;Ya$71A<`ic1uX6%vb5At?x2A3_5hobRDa9~FvH(?KO*W_}(x
z2`Ga~-15wv9EGG*h2qlO+|0am1rvpo%rtOyR08%=F#}jF#G9G<c__|u1hqUs1v5Cx
z-AZ$EkbR^KPWZW*=@}*9q6U;45y>Cagh|aS$t(glU_d5AvO!6HzCvb7wgRY>QZH60
zN(DD<@=}vii;EMBDitzPi&DW(LgY_q7KY~0Vn{e-WESfvq*mxDBqoCk3Rorq)ik9A
zDWHk~<b>q>qN3E|f_zApPDw4wEK4j&Ee5&O0BTKIei67#&CO5B%t=*9N-R!Q&;?PM
z$zZo-l@^yM6s3ZaDp*~m0*DFD<C%G25s((BVUUcRT$BoOKq69LBSIJw4jTDI;4nxn
zOD%$=5O~3ypI4ljl3JvI7W_H+i6xnN=}w8osUZb9;84g&ErLgOa(-?u$n%+b1*Ihl
zp1}$w`9+1LsS03+fs;B)+=4T(Z)r|RW>S7ir3NGn!A?VU4Y*9oEKw-UOGzz)cp^0g
zR3ah;mwK^6r7ol<h9^FVfhmb4i3+yR0v&9+9wcmB5=#;_pf*92LS$3Z((;Q+d@^%0
zOVIQo%V;SW=$V2&rc91zjl`n#V!g!V<kXzhqC`*vSI~#L(bq8|-qG3F)yFl+F~rl~
z59-9!ip&z2a|?<x^Gc8dOb^LvQ1uFa{vobb;Dioq)`J4FSRpa5QlU6CuQ*jlA*r-P
zAtx2oWJ=Em$7W^;#7m%BC^xYpGq*HXArVv$q^2mKI2Ni55|XfZPRcLMODR?;&&W*9
zP(UPmP)aXIElN(!D@ja;<SbB2090?p=Rn%n@kI)@;KnxCTx|8PEu#KSNljCLSnLd{
z#5}$Ioddw(sRIc?1uKOhPy^LBF)uMawMa(+R5^fbQ?OD1HHL#-jWw+xg-&K}L4Hw*
zLT+Ymd~tqGS!z)+LuQ%+L^LNeNw2gdGe<8iGcP5+xF9uILmf$>x~3HaL?N1bm}p66
zZYqooEjZ%ANhKbXBpK3*@^ck17LV(dWfqqv=44i-B3MxK6$*0lOWca`b6ql%OK@pV
zN-WMyF4lukFpZF^+&eQbH8-&&GuaE&;teTE%nE=xFR>`SAhD=86_=^GkdQ2f8JM10
z;+vTVTFdTOlpX-mS)`)?t=fF@^9$f1j~ov$%iw)Km_R{hUVd_NMrOW3VlgO!VB&eD
zxdoLVvAhBXQ2&1#4+FzD9?1HC*!=e?eg=jK{0t1C{0s~x{0t1d{0t07`4|{N`4|}F
z`4|}3`4|{3^D;0j;bmYb=4D`r<z-;-;ALR2;ALRo<As>_6=Wu~e>_T!hQMeDjE2By
z2#kinXb6mkz-S1JhQMeDjE2C#gaD|U48owUq+W4OYHERop`HPxgAWo<$t*6&Nvza^
z(xA>^Ft{750qM;`RF-Fe29-jJN}=;;P}O>AMX7}fwhD&eJ~vFT2sCPBtDuV_s(?IW
z=bKubk(^lK0<l{owF1(sh1dldnNUhh&d5wHOHBa{W0vNWC_#JXpvj_;`11Hv4Na(W
zq%N&S5p0r5M*%!Kk{6$ymtRt>psipC5rOD|E7L2;FVIj=DN%=r<Lu=kj}7Q3z_mkk
zfr1g<Bh>?Sv7Pht%2JEcL9=BVIr-^&pfT@a9R(1xpeQvR%1l8p(~A;QprXYk`2}!h
zDu|huneLgFl3HPlG;N4#pTBc(eoiT92ncLl1yp}&v5o><v!RYcaYlZ*EhrKZ?#{_i
zSFlxpL^UXAofC6%k`j}%i#0T(4P(L4nwbU(ONb%`&?rh$ennbJN&##l10EjH>V?JX
zv7jL@SUf;QR24LgwDJn{3NkenbQQo1u-V|z-TXYzv~Ye=wnBMkNk(R#f^I>kg04bA
qrUGbq(LmRzAQL<Wm6%tVn_rX)aXj2k9jNOSY!$GN<v@mqG#LP({Dp=9

literal 0
HcmV?d00001

-- 
GitLab