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