From 9bfa066191c4c52e713714281732e75966ab766c Mon Sep 17 00:00:00 2001
From: gg <marko.guberina@control.lth.se>
Date: Wed, 11 Dec 2024 12:08:17 +0100
Subject: [PATCH] transforms for clik make some kinda sense

---
 python/examples/clik.py                       |   3 ++-
 .../__pycache__/__init__.cpython-310.pyc      | Bin 379 -> 379 bytes
 .../__pycache__/managers.cpython-310.pyc      | Bin 35320 -> 35320 bytes
 .../__pycache__/__init__.cpython-310.pyc      | Bin 192 -> 192 bytes
 .../basics/__pycache__/basics.cpython-310.pyc | Bin 7100 -> 7100 bytes
 python/ur_simple_control/clik/.clik.py.swp    | Bin 36864 -> 40960 bytes
 .../clik/__pycache__/__init__.cpython-310.pyc | Bin 188 -> 188 bytes
 python/ur_simple_control/clik/clik.py         |   7 +++++--
 .../dmp/__pycache__/__init__.cpython-310.pyc  | Bin 186 -> 186 bytes
 .../dmp/__pycache__/dmp.cpython-310.pyc       | Bin 9311 -> 9311 bytes
 .../__pycache__/__init__.cpython-310.pyc      | Bin 181 -> 181 bytes
 .../util/__pycache__/__init__.cpython-310.pyc | Bin 167 -> 167 bytes
 .../__pycache__/get_model.cpython-310.pyc     | Bin 7638 -> 7638 bytes
 .../__pycache__/logging_utils.cpython-310.pyc | Bin 4363 -> 4363 bytes
 .../__pycache__/__init__.cpython-310.pyc      | Bin 172 -> 172 bytes
 .../__pycache__/visualize.cpython-310.pyc     | Bin 8744 -> 8744 bytes
 16 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/python/examples/clik.py b/python/examples/clik.py
index 1c3a4d7..e58580b 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -30,12 +30,13 @@ if __name__ == "__main__":
                     {"Mgoal" : Mgoal})
     else:
         Mgoal = robot.defineGoalPointCLI()
+        Mgoal.rotation = np.eye(3)
     #compliantMoveL(args, robot, Mgoal)
     if robot.robot_name != "yumi":
         moveL(args, robot, Mgoal)
     else:
         goal_transform = pin.SE3.Identity()
-        #goal_transform.translation[1] = 0.1
+        goal_transform.translation[1] = 0.1
         moveLDualArm(args, robot, Mgoal, goal_transform)
     robot.closeGripper()
     robot.openGripper()
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-310.pyc
index 81e8ebf8d6cc559fdd307bc77f8a665ed696259d..ce6673496f7e6316a3904ce2006131b6aed286ca 100644
GIT binary patch
delta 19
acmey(^qYw*pO=@5fq{X+_UJ~gLPh{J3IxXh

delta 19
acmey(^qYw*pO=@5fq{YHTFpkTLPh{LGX)m_

diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index eed3be955219812ee460e571bab51467135ce72f..7d1daab77b3d534c89f472a45ca60728517b18a6 100644
GIT binary patch
delta 21
dcmew{nd!%5Ca!#5UM>a(1_s-s8@b+f0su}+2J8R;

delta 21
dcmew{nd!%5Ca!#5UM>a(28L@j8@b+f0sv5S2Rr}(

diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-310.pyc
index a7316ce2482bd31d5f61d5ec5cc08a8d392a32b4..4fcc48325eb50ae80151049a8bd95d410b78bf1c 100644
GIT binary patch
delta 18
ZcmX@Wcz}^BpO=@5fq{X+=IBJOZ2%}O1abfX

delta 18
ZcmX@Wcz}^BpO=@5fq{YHYRyEhZ2&3;1i}CS

diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-310.pyc
index 5f7577feb229658126fc67c97c41fdf918babaf5..b12dae4be0485859ae209b9f9ce244671d36d973 100644
GIT binary patch
delta 19
acmdmEzQ>#^pO=@5fq{X+_UJ~gP0|1})CCRz

delta 19
acmdmEzQ>#^pO=@5fq{YHTFpkTP0|20{RLhC

diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 4f63782936cd661ef1ccba5b4d9e81eb3873b0a1..43cc7c57dc1da9d88354597a6af9ed841edb7245 100644
GIT binary patch
delta 1032
zcmZozz|?SnNh--8%+puFLeGeSfq_AQfgz<RGX2S+7zSC!iBd5eXZbTve4w)VC9@<C
zBgbTB0aZrM$(jQ4oGc*K3=9m?lLH0B1zDjyXDEGnbEH5!|0abJL8cbQ&1Hste7sfM
z3=B*>5G$l73kKxZgLtw$3=D6$85nkOGcauDW?*RIW?=B<W?=B*W?(qU#lWzEi-94M
zi-CcWi-BQ3Cj&zVCj)~YCj)~KCj)~lCj)~7Cj-MX4hDwh91INk91IL591IMS91INK
z*clkUvNJGjW@liS%g(^i&CbA3#?HVH$<Dx_%g(^S%Fa;FaEpzBVLlrJLmL|dgBcqG
zgD4vV11lQ?!(mnihS{tP45h3L4F0SP3|6cR45q9M49u(y3`bZP7<yP37y?-s7|d80
z7=&3E7=&0D7}hg0Ff3<gU}$G%U?^s0V9;S^V0gmBz_5pjfnf_11H(Kf28K)~28M7Z
z1_mc41_p5^b_Ry;j0_ChCr@lN+I%Q*DW|Z4f`Wo?dVZq4qJ36sN~W!Wj>2Ta7)>1F
z#<BK__6izW@mgACi8-aI#a0UWNm;4MC0g-XIts<91y&0F1tlOel?#e8^GY<7it>~4
zOY}fC<S6MVAn`OAK+3=p@kN>G86^s~3X>0*OE~H!CYNa7P?(dN22z)vpO_P0Qk0li
zoR(jdi>_fZr$yst(b)G)^{Olk48EXrBEY}^OD4DZ85p+kGcauCXJBaJXJGK-XJGK<
zXJC-vXJB~9$H4G_kAWeckAcCBkAdMmF9XALUIvCxUIqp^UIvCIJPZscco-Nq^Dr>1
z<6&T^;bCB~<zZl8<zZkr#m&I5o|}Q8hns<+o11~blbfNQ!I+zYL6Mt*frp!c;XM}v
z!(lE4hS^*U3<X>a45nNR3^H5{3}-kQ7*=yKFwEv;V2I>oU@+ulVBqFtV0g;Gz;J<s
zf#Ea<1H()X28JjO1_mV#28QSC3=C)385mZwGcc@RXJBw;XJD{pXJC+JXJFuDXJ9zQ
z#=ua;#=sE7roq7A#m2y3!p6YxpOt~(Eh_`V4ORw*wX6&by{wRsv1DanP-ET9$nuAI
z@}WRs5m1~cD^%u}su!gyB<2*QCZ<$wJ`|YE<pzsTq!?4u0VkatNc=$}6jd20zTm1r
OoX!8@nG6;=2mk<sQ_@@j

delta 443
zcmZoTz|^pSNh--8%+puFLeGeSfq_AQfuScpB7K`s4a1Ff6QyF5GxBp&jr7y=iwp8o
z5{vY6O7l|m3yKmeGK=*Kit@8klS_&>mIg3ye#tD!!^l3FSwNMMWwNG#JSQv2I0goW
z=*fWs;+s1JD)=WJP}rnUA~<<Uqu}ON!+O5WEEE3l3nr#8Fjz4#FdSlpSRXxE&>_EG
zgNK3PJ2wNvVQvP7L);7uz1$28VcZN1q1+4%SGX7$c5^W>q;N4X@Nh9OoZ@6)n9Rw*
z5Xs5FV9UwCpux$&pv=j@@R5UoVG{=fLlp-DgFOcWg9--&10x3m0|N&G!#;Kfh864#
z4Aa;d7#i6b7*g077%bQs7zEfE7@o2*)HAGNV_=xb#=zjn#=xM!#=s!J#=vlnm4RUy
zD+5CVD+5CmD+7ZoD+7ZAD+2=`D+9xM76yjtEDQ`WEDQ{eEDQ{CEDQ{?EDQ|0m>C#0
zF*7hsVrF2dV`gA5XJ%k{&&0rRoQZ*9KNADPN+t$|5+(+QBqjz1UnT|yB_;+2CMI@B
gcpRKOvC($(cE_cho9~#dW85t2`<`htOTr(00JqFoO#lD@

diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-310.pyc
index f748900fdc31bc412b0a999b4fb3db642fcc1623..b7cc6106c1f0c88a87540694b79a5ba5fa9c9383 100644
GIT binary patch
delta 18
ZcmdnPxQCG|pO=@5fq{X+_UJ^eO#mmR1ZMyM

delta 18
ZcmdnPxQCG|pO=@5fq{YHTFpeRO#mr>1h)VH

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index c643261..661f287 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -205,8 +205,11 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
     q = robot.getQ()
     T_w_e_left, T_w_e_right = robot.getT_w_e()
     # 
-    Mgoal_left = robot.Mgoal.act(goal_transform)
-    Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+    Mgoal_left = goal_transform.act(robot.Mgoal)
+    Mgoal_right = goal_transform.inverse().act(robot.Mgoal)
+    #print("robot.Mgoal", robot.Mgoal)
+    #print("Mgoal_left", Mgoal_left)
+    #print("Mgoal_right", Mgoal_right)
 
     SEerror_left = T_w_e_left.actInv(Mgoal_left)
     SEerror_right = T_w_e_right.actInv(Mgoal_right)
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-310.pyc
index 4d1c492319ff89555b519330a66a376c7ea6107e..c52ad2d6a07ddbfb794cde23e26b76b2b7811177 100644
GIT binary patch
delta 18
ZcmdnRxQmf1pO=@5fq{X+=IBJO4FD%w1YiIF

delta 18
ZcmdnRxQmf1pO=@5fq{YHYRyEh4FD-L1h4=A

diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-310.pyc
index 0e29756c453906783f2ec862504e575fe0d94cd1..1748ec39385a4de2c51fdc093e07874945c27f85 100644
GIT binary patch
delta 19
acmccbao>Y0pO=@5fq{X+_UJ~gP!#|=Q3WCZ

delta 19
acmccbao>Y0pO=@5fq{YHTFpkTP!#|?dIfR-

diff --git a/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/__pycache__/__init__.cpython-310.pyc
index e15880dc900e8f8d54ed9375536c1ff25861b877..4205132f18c1fa9e6f360245f8563b5689393978 100644
GIT binary patch
delta 18
ZcmdnWxRsGBpO=@5fq{X+=IBJO)c_`}1W^D0

delta 18
ZcmdnWxRsGBpO=@5fq{YHYRyEh)c`1k1fc)`

diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-310.pyc
index 68f9f19de45ab20cab0014a1af94d8390b3013f7..e4807fe1c1df90399b3c25f5ebafd91b56ee7b8d 100644
GIT binary patch
delta 18
ZcmZ3^xSWwIpO=@5fq{X+=IBJOxd0@>1SbFh

delta 18
ZcmZ3^xSWwIpO=@5fq{YHYRyEhxd0}c1a|-c

diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index 66be41a9466e86d1e63cc8c6d6de71f3dc0db74c..f156703b670f5cf4630b8fbdd538088be5a9576c 100644
GIT binary patch
delta 19
acmca+ea)IHpO=@5fq{X+_UJ~gGqM0UIt69`

delta 19
acmca+ea)IHpO=@5fq{WxQ|(5sGqM0VkOh+f

diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-310.pyc
index 2e37258da435a8748e38b6df86896ab68dbc2f6b..4c09aab89ebdec862597aa374132927a8fcd0e96 100644
GIT binary patch
delta 19
acmeBH>Q>^)=jG*MU|?XdJ-U&LQ4jzqx&y}m

delta 19
acmeBH>Q>^)=jG*MU|?XlR<n_dQ4jzs;{+D~

diff --git a/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/__init__.cpython-310.pyc
index 51d48f74c07792fe02bb0a151b6b46bcc253c927..eb9ca0ddb65ee961913ca9fde50c764effc20b46 100644
GIT binary patch
delta 18
ZcmZ3(xQ3A{pO=@5fq{X+=IBJOMF1sn1U3Kw

delta 18
ZcmZ3(xQ3A{pO=@5fq{YHYRyEhMF1yC1cm?r

diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-310.pyc
index 1ba8d819410731c72985bdc5203973f93569c258..d48f6116dd1dd800f9a9f003050b74a758be83dd 100644
GIT binary patch
delta 19
acmZ4CvciQcpO=@5fq{X+_UJ|~IVAuwodk0L

delta 19
acmZ4CvciQcpO=@5fq{YHTFpi-IVAuy#stFv

-- 
GitLab