From 8d0786901b439a528a11f3a2eb647b17c8fd55bc Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 8 Nov 2024 14:19:03 +0100
Subject: [PATCH] fixed the problem that joint_id=6 only makes sense for ur5e.
 now i use ee_frame_id = pin.getFrameId('tool0') which gets the (correct) ee
 frame regardless of the robot. this requires pin.updateFramePlacement, but
 this can be done for only one frame so who cares. having this said, i have to
 test all other functions to see if this makes sense or not

---
 python/examples/crocoddyl_mpc.py              |   4 +++-
 python/examples/data/latest_run               | Bin 927 -> 928 bytes
 .../__pycache__/managers.cpython-312.pyc      | Bin 44836 -> 45561 bytes
 python/ur_simple_control/clik/clik.py         |   2 +-
 python/ur_simple_control/managers.py          |  18 ++++++++++++++----
 .../optimal_control/crocoddyl_mpc.py          |   2 +-
 6 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index 90cd849..a8ac8f3 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -11,6 +11,7 @@ from ur_simple_control.visualize.visualize import plotFromDict
 from ur_simple_control.clik.clik import getClikArgs
 import pinocchio as pin
 import crocoddyl
+import mim_solvers
 
 
 def get_args():
@@ -62,7 +63,8 @@ if __name__ == "__main__":
     print("final position:")
     print(robot.getT_w_e())
 
-    robot.log_manager.plotAllControlLoops()
+    if args.save_log:
+        robot.log_manager.plotAllControlLoops()
 
     if not args.pinocchio_only:
         robot.stopRobot()
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
index ff992b84184343c3d9e4f6270f795b27a195a77f..4847c67d6795dc1aaf0da33991c1bbb123c3e83d 100644
GIT binary patch
delta 30
mcmbQwzJQ&jfo1B{i7b|EtQo0A`FR_ijF}j@HkUDJG6DdN;Rt;I

delta 29
lcmZ3$KA)YXfo1BHi7b|^ETu)JsT-Y*nHW<xmosTH0sw;$2s;1(

diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 88c6cb821ec8dc97c3b71076271c9edadd6dadf8..0a834918a14e09f0bd0fb57d66922f2d7dd23576 100644
GIT binary patch
delta 3108
zcmZ2-kLl-QCce|Wyj%<n3=FgR^wJ;i*~n+$&1A;2*}?lC^JH@)aYp;efy}~_{bzHA
z)G)*=fE0po3U>`dydsDN!YMp83|USf7AmgdW?-mch*yG%rtsD<#4E#iHH=vvlN)tK
z8Qmt&@t0=w09nM(m+iv9P|W1T(8LIp7ny7mpum&DpDo3}P{a=w3}hCTMN*g|P{R<f
zhGeWC$k@s0fx?s5`ExS*VKEq_LPQ{59i}M-<W7yra=vm*Spky+nMK&L%^4Vq=1eY_
zCN^0pP?8ZOD6A=zt--)h<X;jD)3+MRs9}iLfHE0s7_tH;KhzPPd^}K?rG_yp3~ayf
zY9@#RMz{jT$jOS?GLxSKibIT-6#*O1UlI*b$-sbY{A7h-+sW%~g(nvTb5E8J=A4|V
zE5?_?1ksYhU&D|U1G0!o)Mm0_kOgDhWWyBU$q9|@EHw=A1|ZF-LWYy)1<OsI73?l4
zwwe)QA0tCaDul(r0CqRjgvlNul8j+6^Cs7X@bH!7Vp9l;2XnXtLssEr!xSly{g#tg
zg(!2w4NnoD><}WSmm&_cxrQO$6s8YhG>W|WWV-+brg+uKUfyzS#i}(7S$W_<)(#V%
z>=qz4*+H3O^14u2#<IzRjl%r1OEO{Rq4=aGgr6}Bq>r%@<TkcirW&TKYEZcG^T8a$
z$N&=)M-nU1hZ&evGr3V$c=GBn0Y<&ad&0bhC2E*58Ig1|WYvQrmZg$GQ*yIaxIQEM
zE!L9!{2YVHnGv~+4wJ7(oR{EAPc3mPO3Y35Ou5CKni`)5V#jBuY+f1}$H+K+GfVU|
zMp60sRx_=xOB-C2HrOD3S=zFL<qC(y<{2?{%#3=I#S;pIHZm|UXfhQoXJB9`as&~Z
zCeKXJWHg+7IKhm`jA`@lgd9fB5^ku=Cp&hFPM(`BHhFc2*yM>xVw2z5DNg397MVOL
z$$|}R6yM|vN!EJPK_-=gh;jx7hFc<~1u2OosbJRz<Rm7i=BDPA6wLuiG3WcHO;%2}
zU^8K2U?|=(IWf81<qI1FufTO4m5V$o3qqDhEsWYAvR!hc<PMkXHa-_^e6H~LJ`fh4
zZaL9%y6r^U1tQlK4KFGhUKBREE^K~L*!;4v<pmzg$%YAGqF@&>-C`>*O3h2ofcY;u
z#gNf`^VF2hEQ~8BduCcPu9@7N>BhKv@~KQ&MxV_uGWi*~zA!Q{Ob2B%Mo>IVmd#$k
zRKhy>bhiBDxNM!tud;<Fzq8|=ynBYo<X9`=$<MQ;Cl^`?Po7}I0V<z(prOT3Gg;qS
zbh3Sp>f}^wVNfdO-&~!O&SE8Si#0wywIuKs8<<Kd+5rlxogktNM1X@Y1jGWz%PtTL
z6k0{QCyN&<v#kK}H%)dZRA(yyh1$Z&xrO%jcR0AOb0}QoP*@<cTxy}zijeh@D<gNX
zT(|JJXyI{%!}Ect<n+jik<()*#x5|qu55f!+4!QU$#qf7i=vj7MXfiVEYxOHngBAo
zpMinFPm{F>l%a}JKq9L^#99!sV6s@T9OK%_ro{@3vo?nnJF&<@lygH=t^_Gs4<f+2
z7f;?=9xQp1fq|h)C@4QEzr;5&FEKr}NDu7srIQsaZZXc-{HJ0!6H|%e<h%N^lN+ja
zCO@hc1&8mhY7s^qa7p~6T8dG8^8ad8#@U-yYpj_BE`Y4K2qHk%Yf4Nms8f(aw;UvW
z2}E3;ysA#g09z=-!}b<)zHeqxC@4Fy6#0NO?V0?&&YtZ!D1;L>+tdp(rV|R{4{Qu#
zQdk1HgQb`2#}``$0l7~MjGPiTgvGmEJ}@(KO8hDoWME)u(R8z7IA+D|&dGe7gVCLH
z^MnQ+W>t{C{4}``!Fr23J+;I!xui5PColyPIg2MhY*vs3B@#_0u%mA=6{Zw}stE;!
zB5jZ-PED3;kzo9>*}UZ?6B{^u4o_BVS76*X*}dJ0QEGEjdl;hyIG=DrY$^fiss|Ae
z)gaay5U~%GUD=8f^HTD2i)MmEHcb}jG-lIaW?(1|o}AwkvUzT&Ka)HbZAFhj%E5YW
zPUh}WVmvWfuSbgU^kmN-1xAC-Sv?-C;8av31`2+mqA4KdAcx#yNzTbEC|Wf6{6sOv
z`IDbbRAK{pwrIy>zDWv<cPHylGGesfoHXe=lj>rS9&nm-1F_se#3T>_O2D^R@(WV)
ziZ)G-n`+9KGP!2D|K`V2O&E1Qfb3&0!mpP#IVZn3wP@31m+9t=d6R3VS8+M8q7@Hv
zldWdd;V&YjSZbL{#3x^D6akf-EXB`D#3xM`kth*|l@=+?h#H_od~(!G<;h&t!jmV=
zWS@L>ruyU!eO!}I%;cRcs3JUhrLM^2f*E3yIjcp$@~k$(lO1MpP7c!*2C3s(#JHBN
zkAab)M0|3=EZxa7s)Z-J*@#S5=n|M5sw)Cg!UI!M!<3~BsmPc)8A@~~%gvT$)CW~(
zll^D&a2vuLS7JOlZMHI_>Ey22(b{}4(LBbUY$Olx)G#BpQJ8BO;$<f9vXf)VvH?}8
zjIxvc{Kc5E9KojN&ymoE*YvPfN-ZmrY1}Bm@j+E&^RYSKSX>l~K*dSXW>DN>%h8~8
zQv^=H@Vs5L8>ASVBtWU5==$Vu3$@suf&^DiR$FAt_-k^?B2C6$llvAWv)6#qvk24V
zAB$|v2xa&O0wU8nCvr~boya>Q<+_~SMLE5T0{WXh7O!OFIWA=DX~}TXQprn}dGf+y
zvCYCubyyfTO?Frr$hdR!q?NlF88=P#TW!dAZgRuwD8`MGZ?9I02K#C&$YDt!Vn2ug
z1x`^6hy^Zz!M?u<;+_H#4?x6Y5OEqrRDg(2AmS^CU}I!pDB=JST$3Z#=$Sh*v5J23
zV_{^K{lvz|D*u^{kJajv8Y8RHCoUyc%}*YTYOIQ%LKrz&l|G5dvx<LqVbo_e{ggC$
z?;0&uYgW-uRg>SWQDodPnQyH;H#noSfNWW{*<x)uBctf#4eL_*JvbQ+KQVyFB3%Xs
F1^}<MRyhCw

delta 2805
zcmezQm}$v9Cce|Wyj%<n3=EH*w9-R%ZR9iXX8Ojx*}?lCvquV74MV&<ND_ooxN8`)
z>_9A3T*b}6P{R<f0258&sbPp$gz;(^vs@-O>WWUD=r26^oj)g|%j8BTVSe6h7Y2r6
zCNG92MyLuAzIY`hO>Q7%+<e(m3=Bp55G72)yhz$o_$TWGD6!NqWO+<p5FkDIa;Pv*
z3X=Tf3!!2{H4O2pFhf&7u2h>W=PSpQ<ulokS%fXyoPnWe&g8~YvB^P!l8ineD>VhP
zH5eF*{7d{{`c^|3H4O1;P$okSLzd5E$7#Zhfs-%J5@E^;0%_$JTFnGezzA0`d3T`j
z<Ow#slTWV@fmk3b47Py3BpjlafdScq$qB)>lh@k{Pu>vB&8RauQCE&Hg$be|g|CJo
zD*|K@lZega!XOLAsL2l#geO1f;bPPS=|ke{Prer{H~ChuyQJu9Mu<I(3?+#W76U^K
zV^$DM@8sRVoO~szSf#>4#266@y~B7Ib0<GckYcG}h&P}7DnywZZeohq<PE_hJP31A
z#3uWM$T6jeO?C(qWoFEBo7|`?!W6GO*~?pwwOF-=A#1W?f-qB7AtV6Zpw3~R{4P|M
zv1sx`17ZHz2-A=q%xesHD??TqC>AE~4HcfOkj}}<RKt{24hkWDUWB({V&X_*CHgQk
zvMMGw>IzSO9VWo2Gx<-Lx3G8(Qzj#lZicLCke}HY87dhxB{o-u>oZQ?5RuDhH(4(7
z{ARYOSVqQ%%_-5-7&i;X)-W^bPVP%65L(B;z@W)gw1k0yp~w+Lte-59sL5zB*(%YD
z>kK0U!*qrk#s;a$52uQ4o{;FrIQfU2_+(vW;mI1wVv{?PmD#`s@J`;CY%MVjq`wqI
zlru0e++xo6%`BS9z`!tBD#e1$n2CX*c*^96lyZd!!s62{Ct6OoooKs2WV!4@*^9#Z
z*M&_k3Y%OOHoL%MHu+zCs8|u$V5VDa<wdD^$r&)m)ukFTI&a>Px|xM>+2oWgOU9Lx
z=ViIcuV7$csFDcEPs%UxP0UM7Pc70*PRz+nDoQL#bqn#>%$&{7$khe$4nr0rD4ZrA
zux6bcH%oeQSWeSqr(E&Lk8{K(E7S<HNir}nWQl^5@YXQIbHl>FX7YI}vB^hsRVK$L
zi!#OYZOTh$G3CF-8lRq85_pRZOr;cU1$loPh$sUQ;2;SBvA`j`9mJYBd0~+<+fop7
z{p3?c>TEfnpsAnyy~tknhN$H9$cd5DV<*NgFj;Q9(DtIJ;dN27i=t+iMJ+a$7Hcy)
z_JcI`F)%RrX|fiXf;cH4VmXLd1tR7`qSOZz2drg@Ii;z^pxmyYP^1l#UN!k)i2~!4
z%}k|EEV2-DxQoC>ECWfe0TEzh7fep943<0!3RxkPkOg~n{^YZjw;1&{uc+G1G}*CM
zZ1R3R(aGy-MJMytu``NI7OhidOxtW-XU!yV24vz{5CO7HQ+#q;gMt*gT_EXmAmaSw
z0}V>r*n$xfcpx>QARn?6fqYoBW3o`AJ=;N05ZY`GZxm#7et;zmJ6L+TetfZH5Rm)C
zz{n|aLs-1q<pVP#r^K&fK?Vkf7EL!+hGVSUZX(Rbgc;pLHlJ_OVO9k>*H4oR5jeNF
z(^E?vlS@hya{^N!VZUIqLYo5PlF5#3(u~I@$F@l@e%M^r_L7MW?7{t$Q#%zHcTH~V
zv|<$6e6TZ&(E^;TIU#12fIL$VA|R?ktd$^Q7bx!7iW2iu@^g!3fJD|$_U|@kQ($Ib
zC=Q;yt}kTsoo;_7c`VwB?t+wq^<0_k)u+UGWO8nw6yx#99eoOn8k<-4d9Z?Gy+{lc
zh(bk^LCQf6xy6#4lUYzSe=^?`F~&KQRi`Mifz#~P$-Yw*7;j9@n_|RhzIox4>rAQ(
zKzhJw#0|u92N4rN1SlEZV#zN^%_~|zdG2&m#+b>wXZmkenqk7I`x;~)dl7!UtjRg~
z#i>Q>CpXVDXH1*CduA2aH5PCogqmw-BIj5RP>!u-DiNJ5&>}K<yP=p=@$(W<SiVhR
zLX?puqLUTUMEH;-S<)FNC(ah1{8>qO^7%RJlP{(VPd2a?nf!hN7t12X5|+sia%Cp_
z>xoX@-z+-$ladHnk)Vq3WCvxA$s2M-Cx4j0IhlVhC!^?O$+^0ddnO7qWvPP74tA)m
zlh@~pK^1YZ)G%eKgCyB%nK>CsG$$XQE6b=2ss<+in9IYh3v+Ub{$&1n%8Z7Sjps%4
z^T0&&7<;lyL?_RkC(SDb6RTm!G6PpE=L|&{WhWn;CqCKVUU;(jd`U**$%gZV#Sy{4
z4RsI$BSVSk<PWMMoAc*?W3iGi0_E_c4WQ`5mg_)CqX?Wv5%~`s;VF|>F41Cp0MfT?
z^35f-j9({<FV$rHI@xh)GJ6Rqp?+eVyn3mP$PEFJ>6{Zer}Iwaoslv>cV_NI0iDgV
z%T_XSACtB9WMMeTqU5Qu`QmbC7RL3HMb`u}uHT%#W;Y|_`pL@c3>lA14qX?;xNh=>
zbxP4-M{WYym;@qrg9uQ`UlapkfeSydXRd&_$3Vnw5OEJg90w5<AmS~E_y8gp85tOg
zm_Y>VWZm_8<_=7(qMv+N7+GaMu`#mBe`e!mHUFf*$g1>-ONmwUlLw;;tKz2sMow0x
zPh#?{;-6g@by<x*#Z8{HUW?U|RrFKY<b&%K!DZJi*1Xc(g32Or=tYC<sss_tAOf6e
gSwO7in|U{sGcpQHZrzy5U&p~{_=y2T7J(810CJN7z5oCK

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 3decba0..f5e0926 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -138,7 +138,7 @@ def getClikController(args):
 # modularity yo
 # past data to comply with the API
 # TODO make this a kwarg or whatever to be more lax with this
-def controlLoopClik(robot, clik_controller, i, past_data):
+def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
     """
     controlLoopClik
     ---------------
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index f96d0e3..afacc9c 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -466,6 +466,7 @@ class RobotManager:
         # and since this is unintuitive, we add the other variable too
         # so that the control designer doesn't need to think about such bs
         self.JOINT_ID = 6
+        self.ee_frame_id = self.model.getFrameId("tool0")
         self.update_rate = 500 #Hz
         self.dt = 1 / self.update_rate
         # you better not give me crazy stuff
@@ -571,7 +572,10 @@ class RobotManager:
             # which is important in situations like this)
             pin.forwardKinematics(self.model, self.data, q_given, 
                                   np.zeros(self.model.nv), np.zeros(self.model.nv))
-            return self.data.oMi[self.JOINT_ID].copy()
+            # NOTE: this also returns the frame, so less copying possible
+            pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+            #return self.data.oMi[self.JOINT_ID].copy()
+            return self.data.oMf[self.ee_frame_id].copy()
 
 
     # this is in EE frame by default (handled in step which
@@ -648,7 +652,9 @@ class RobotManager:
         # NOTE: it's too slow -> has to have pinocchio with multithreading
         #pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
         pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
-        self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+        pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+        #self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+        self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
         # wrench in EE should obviously be the default
         self._getWrenchInEE(step_called=True)
         # this isn't real because we're on a velocity-controlled robot, 
@@ -746,8 +752,10 @@ class RobotManager:
         q = np.array(q)
         self.q = q
         pin.forwardKinematics(self.model, self.data, q)
+        pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
         # TODO probably remove deepcopy
-        self.T_w_e = self.data.oMi[self.JOINT_ID]
+        #self.T_w_e = self.data.oMi[self.JOINT_ID]
+        self.T_w_e = self.data.oMi[self.ee_frame_id]
 
     def _getQd(self):
         """
@@ -900,7 +908,9 @@ class RobotManager:
         q = self.getQ()
         # define goal
         pin.forwardKinematics(self.model, self.data, np.array(q))
-        T_w_e = self.data.oMi[self.JOINT_ID]
+        pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+        #T_w_e = self.data.oMi[self.JOINT_ID]
+        T_w_e = self.data.oMf[self.ee_frame_id]
         print("You can only specify the translation right now.")
         if not self.pinocchio_only:
             print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 833a629..d766a1b 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -2,6 +2,7 @@ from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager,
 from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP
 import pinocchio as pin
 import crocoddyl
+import mim_solvers
 import numpy as np
 from functools import partial
 
@@ -22,7 +23,6 @@ from functools import partial
 # and probably better. 
 # i'm pretty sure that's how croco devs & mim_people do mpc
 
-# TODO: FINISH
 def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): 
     breakFlag = False
     log_item = {}
-- 
GitLab