From 254cabd4090e129d8cf5839c252d7402af174b8b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 14:41:40 +0100
Subject: [PATCH] cm

---
 python/ur_simple_control/.managers.py.swp | Bin 81920 -> 86016 bytes
 python/ur_simple_control/managers.py      |  29 ++++++++++++----------
 2 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index 5243d519843b3b34678e49de7524e8fd6cebf026..3a2910e7ffffed89116bbd36f515fd0e9b24f0e7 100644
GIT binary patch
delta 898
zcmZo@U~O2yDw<>v=IN_op=ZRvz`!8Dz`*Vrk>0pb^esQ5@MdO#UPeZN$rA;|1w}y8
z3=9lzP&#PyOhI?PO$saqto9rX44)@+1}tX$ym@Va8walf5>Vb0$ugby91jD77B9pC
zfysg&jO!)&7#KeCGBEV<GBDKeLS*moFfd%<VPM$K!@!Wn!@ywA!@wZR!@%&Cn}K03
zHv>aIHv>Z-Hv>ZnHv>ZkHv>Z?Hv@w=Hv_{5E(V6FTnr4BTnr2nTnr36Tnr3bIT;wr
zI2jlMI2jmBI2jn8b1*P$<zQea<X~X%<Y1_0_{Yw`u#ugCVLm$pgBLpkg91AP0}DF?
z!!0%jhCOTy40G8S80y#<7-HBM80^>>7{0PHFnnQUU^vUlz%YxIfx&^5fkBg%fq|Qq
zf#DDf1H(ZU28QV@3=AnO3=DoO3=BFf3=Hfn3=9vM85lM)GceRJGcedOGcbH(Vqn<7
zq{+aro{52>or!@Vgo%N{j){RmhKYgUJ0k<bMMegOg^UagIgH?-p1g3X@a7F4<G3fk
zishMnU>eKh1<yGqm;5!GJmI;-<dVNmlXpFpnk>-CI{Dmh&dD4e`jc7z$xXjy#3(TN
zL!iXuuAjn_b&d&dcK^52u|6?{fngI91H&_BNL0Wgb00qg!#sWlhFX3G1`U1&hQE9a
z4A=P>7#8y}Fy!+wFxc`jFzE9!FmUoQFmUiOFudbsV0gsKz;KzDf#E1G149-s0|P%V
z1H)z>28KyI3=C~N3=H}_3=AK*85mA+Gcc^<W?)F<W~gV-=VoAd&BefQn2Uj-f{TGc
zn~Q-#l8b@i2qy!>EKUZ7I!*?L5KaaLGfoBuaZUz?9~=w}*Ekp$c5pB-<a01E<Z&=C
zxN|Tth;T44>|$qNSj^7A(8|uhV8hP9V9n0JAjr<ZaF30F;RG84!!kAohDJ69hG;eh
z25mM5hHtC}3=BJ185jy!85p#nvB${Dz;KR*fnf&=1H&{H28Kcw1_o~y1_o&sP$+G@
zxPxhO{zIP0>!z}8-tbY9Yx-JSMow@*Z#-!@`O0tQ$)A6lOjiD5GCBB<+T@BjzRAsh
tWGDYT!!!B)cGk&j|9DKFXT-=k`PUzj$u;pjlg0i@OwQj7F~H`p3IMb>ztaE!

delta 438
zcmZozz}nEjDw<>v=IN_op=ZRvz`!8Dz%bb*BE5d2=v#h9q0P(!y^M^(lP3y_Z$2St
z!8h@N!X^b416BtP28J(_IRh3me%ZV>z>R}3b@Ia~oy`i7Pp0vjF)%R9V}uwLJX!FA
zalJGj1H&F(28MOK3=9dp3=B%V3=C|%3=A)M7#L>oFfbJIFfjP@FfhpTFfhF5W?(qT
z&A@Pgn}J~&Hv_{gZU%;KZU%;0ZUzP=ZU%<qTnr4^Tnr3eTnr3$Tnr4)IT;vMa56A7
zaWXKZaWXJSaxySH=U`x1%)!7=&B4H6z`;<@@R*%};UYT&Lk&9vLl8RygBd#m0}nd`
z!#g$xhVyI;3>(-O82Z>47z)@J7}VJq7}VGp82+;|Fq~m!U?^f`V2EaAV6bInVEDqq
z!0?%cf#D<z1H&{H28IR}28K8m1_nzO1_ogk28PGX3=Hd-85jzf85lH}85kZhF)%!2
zQf6S-$;80W#>Bu-z{J4d$Hc&($;80G%*4QOnGq6JbEh{7GTKhgJIFD4-Bh;Cbsyun
fr?2v7RG8k+&B!|4&ySIFbI(6f#?68M);j_KmTp&a

diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 00841ae..fdaaa3b 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -782,7 +782,7 @@ class RobotManager:
             self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
             self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
             # NOTE: VERY EVIL, to bugfix things that depend on this like wrench (which we don't have on yumi)
-            self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy()
+            #self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy()
 
     def _getQd(self):
         """
@@ -841,19 +841,22 @@ class RobotManager:
 
 
     def _getWrenchInEE(self, step_called=False):
-        if not self.pinocchio_only:
-            self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+        if self.robot_name != "yumi":
+            if not self.pinocchio_only:
+                self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+            else:
+                # TODO: do something better here (at least a better distribution)
+                self.wrench = np.random.random(self.n_arm_joints)
+            if not step_called:
+                self._getT_w_e()
+            # NOTE: this mapping is equivalent to having a purely rotational action 
+            # this is more transparent tho
+            mapping = np.zeros((6,6))
+            mapping[0:3, 0:3] = self.T_w_e.rotation
+            mapping[3:6, 3:6] = self.T_w_e.rotation
+            self.wrench = mapping.T @ self.wrench
         else:
-            # TODO: do something better here (at least a better distribution)
-            self.wrench = np.random.random(self.n_arm_joints)
-        if not step_called:
-            self._getT_w_e()
-        # NOTE: this mapping is equivalent to having a purely rotational action 
-        # this is more transparent tho
-        mapping = np.zeros((6,6))
-        mapping[0:3, 0:3] = self.T_w_e.rotation
-        mapping[3:6, 3:6] = self.T_w_e.rotation
-        self.wrench = mapping.T @ self.wrench
+            self.wrench = np.zeros(6)
 
     def sendQd(self, qd):
         """
-- 
GitLab