From 85b50aebf20685abb34036bcf68c71c367ccd099 Mon Sep 17 00:00:00 2001
From: marko <marko.guberina@control.lth.se>
Date: Sat, 7 Dec 2024 20:04:19 +0000
Subject: [PATCH] pretty close to porting to heron w/ ros2. unfortunatelly
there's no time to finish. but the last todo would be to use
robot_localization on top of the existing ros2 nav2 stack to get sensors
fusion AND the gps-type capability of amcl. other than that ur_rtde can be
used simultaneously with the ros2 comm on the mir platform. and, of course,
parameters need to be tuned so that they actually correspond to the robot
---
python/examples/smc_node.py | 37 +++++++++--
.../__pycache__/managers.cpython-310.pyc | Bin 34124 -> 34343 bytes
python/ur_simple_control/managers.py | 35 ++++++++++-
.../optimal_control/crocoddyl_mpc.py | 7 ++-
.../crocoddyl_optimal_control.py | 6 +-
.../__pycache__/get_model.cpython-310.pyc | Bin 5915 -> 6609 bytes
python/ur_simple_control/util/get_model.py | 58 ++++++++++++++++++
7 files changed, 129 insertions(+), 14 deletions(-)
diff --git a/python/examples/smc_node.py b/python/examples/smc_node.py
index 7fc29a8..6d55386 100644
--- a/python/examples/smc_node.py
+++ b/python/examples/smc_node.py
@@ -2,12 +2,15 @@
import rclpy
from rclpy.node import Node
from geometry_msgs import msg
+from nav_msgs.msg import Odometry
from rclpy import wait_for_message
import pinocchio as pin
import argcomplete, argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
import threading
@@ -16,14 +19,15 @@ def get_args():
parser.description = 'Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!'
parser = getClikArgs(parser)
+ parser = get_OCP_args(parser)
argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
-class ROSCommHandler(Node):
+class ROSCommHandlerHeron(Node):
def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
- super().__init__('ROSCommHandler')
+ super().__init__('ROSCommHandlerHeron')
self.publisher_vel_base = self.create_publisher(msg.Twist, '/cmd_vel', 5)
print("created publisher")
@@ -35,20 +39,29 @@ class ROSCommHandler(Node):
history = rclpy.qos.HistoryPolicy.KEEP_LAST,
depth = 1,
)
- self.subscription = self.create_subscription(msg.PoseWithCovarianceStamped,
+# MASSIVE TODO: you want to be subsribed to a proper localization thing,
+# specifically to output from robot_localization
+ self.subscription_amcl = self.create_subscription(msg.PoseWithCovarianceStamped,
'/amcl_pose', self.pose_callback, qos_prof)
+ self.subscription_odom = self.create_subscription(Odometry,
+ '/odom', self.odom_callback, qos_prof)
#wait_for_message.wait_for_message(msg.PoseWithCovarianceStamped, self, '/amcl_pose')
#print("subscribed to /amcl_pose")
- self.subscription # prevent unused variable warning
+ self.subscription_amcl # prevent unused variable warning
+ self.subscription_odom # prevent unused variable warning
self.robot_manager = robot_manager
self.loop_manager = loop_manager
self.args = args
+ # ONE TRILLION PERCENT YOU NEED TO INTEGRATE/USE ODOM IN-BETWEEN THESE
def pose_callback(self, mesg):
self.robot_manager.q[0] = mesg.pose.pose.position.x
self.robot_manager.q[1] = mesg.pose.pose.position.y
# TODO: check that z can be used as cos(theta) and w as sin(theta)
# (they could be defined some other way or theta could be offset of something)
+ # ONE TRILLION PERCENT THIS ISN'T CORRECT
+ # TODO: generate a rotation matrix from this quarternion with pinocchio,
+ # and give this is the value of the x or the y axis
self.robot_manager.q[2] = mesg.pose.pose.orientation.z
self.robot_manager.q[3] = mesg.pose.pose.orientation.w
@@ -56,6 +69,17 @@ class ROSCommHandler(Node):
print("received new amcl")
#self.publisher_vel_base.publish(vel_cmd)
+ # HIGHLY LIKELY THAT THIS IS NOT THE THE VELOCITY OF THE BASE FRAME
+ def odom_callback(self, mesg : Odometry):
+ self.robot_manager.v_q[0] = mesg.twist.twist.linear.x
+ self.robot_manager.v_q[1] = mesg.twist.twist.linear.y
+ # TODO: check that z can be used as cos(theta) and w as sin(theta)
+ # (they could be defined some other way or theta could be offset of something)
+ self.robot_manager.v_q[2] = mesg.twist.twist.angular.z
+
+ #vel_cmd = msg.Twist()
+ print("received new amcl")
+ #self.publisher_vel_base.publish(vel_cmd)
@@ -65,8 +89,9 @@ if __name__ == '__main__':
robot = RobotManager(args)
goal = robot.defineGoalPointCLI()
#goal = pin.SE3.Identity()
- loop_manager = compliantMoveL(args, robot, goal, run=False)
- ros_comm_handler = ROSCommHandler(args, robot, loop_manager)
+ #loop_manager = compliantMoveL(args, robot, goal, run=False)
+ loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+ ros_comm_handler = ROSCommHandlerHeron(args, robot, loop_manager)
thread = threading.Thread(target=rclpy.spin, args=(ros_comm_handler,), daemon=True)
thread.start()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 9450f01b9ff3639106a03cef0cc1522b1ebe61c2..3b476c272538d9a1f12c522c5ae7d32bfa054cde 100644
GIT binary patch
delta 5849
zcmX@p#k9PKi7%g*my3acfnjQHaJurtiF`7QDHFA8>vP4T#26V;7*aTL#B(L0B*1LW
z9LZd%D5+fODCu08C>gLASB`A1T$CJ`&7C8ks}QA-s~Dx2s}!Y_s~n{a7H7;+iBd^n
zNMX!TjZ#fvNa4v*%T<q32g~v1Xyj@}X@c2&Ia;~eQQC|QDd`L;{5d+gx>34d3C0|~
zC_S(N`cVd{23dv+j8X&^G8RosVMr0oG0ruKGGS!kWJnQ85pH3KGIeK25lInkVMq~e
zW{NU%XGjrC5pQ8g5pQOSGIwW4kw}qjVMvi|W{R?y+{Kv9$U6BOqqp!a!Q9NE_{4&O
zqWp@?+{BX1{JhPMOo}YbtVObuQ`uBGZn5QN78T_ePwrw%VN+&cU?@_V{Dm!nTM5L3
z@HfY^e`91+-yFut#|{x!0ZVSq7udqcXuerRNS2Y&a<h~0H7-W+$&zyMj69Po<&qis
zCf}8l<#q(AbpjDClUe1}xXnRKFk!LTOg@>(S&o5$;Uyyj1H&!0kocs;;#5uHB9NwA
ztntMqsRcz~y>1{~&LBnX1^ERzsc9ueo**F~5aBZUj-m}?_+&{XRUu!HfIkBRgC=`X
z0EitjIY3E8DhR|31`$yp0&G8+u-M$Ew1kl{a<Z|CiweksB6pAi2ar`PC7HRYMQI>D
z*g;GwB}FciXRA1}L2Q})NX5w)>;jM*Z?UA6<|W_ah>y=p%uS7tFR}x<f(_~lka?`d
zC5c5PMJXUQb8247EtZtbw6r3yJHUj+=0ep!OpK9}^E4uD#X<JkgNSSp0dm$Yro4h%
ztW~K+`Nhy6%FRzn&AG*tS9Xi7I5jUNFr~-^q&R0Xi>9$#0f<=$B8or++@(duAZ`hW
z0QvY9dvRh}s!x7;5yV>{@q7>w3nJns*J{ccfr9cDTXAN3UTN+v*0iF;+|;5FkU!YV
zQgh;yb5oFg0n%nS`K)FfW65M4Ep^RMkN`LsD?ltzxF9Kq_#7l*HMvGhlF@4N|4iA<
zo3$3OGFdTC4mDU=&ydBM!j{cclw88w%n-v;!w}D2!w}C=!w}C|!w}C^!<fZaRA0hh
zB2dGS#a_dZ#ZkkM#o5eK%Tyv*!&t+V!d@b@Ko}&KC9;q)lQD&(8DxfN4P%yAQG1Da
z4P!H74MV&{3dl4`kh%p@DO?LdV$wB?Su*uC4Dqrl+%*hYveFDS4DoU)JT(mQ@->WE
z3N;K_^4W|<D?#iO-fWiQ)m{uuj38bOL%c!_W0qnHUp8aWPLK##em6)yg};U&Ua5vL
zOSy(2ODUVN=p={@mOl-W2k~kc;+0cCwyD(AGh{JjsiX*GGZ+1-VaQ@E29Y2U!EC0Y
zPbI3&5M>NmDht$V7_*pDgcdR}GSq<h>I)gbERbs9Y^I_=B^nUb$RSe05U*JS3S;JM
zrU{Hi{3(1j3|X2fA~qm1SW{T)!NO8#!mMCXRfH(qNG)*CX`}IVY8bN^ComSrl<202
zHZw9Zl<0vR3l4hy5(7Bfu*3+?Hm+gFGG0(`0t(25j3A#ZFkJ}ptyvx$$oLdauvKOt
z`5K0JMTiwe(`y*B%qK7wO-DAih9S#vfyF|GTIL$&1(qqiVDsRt5}^fFHOvbcL9*cR
zV9eqJnNY)!C0f70I)%N4Ig=5rg<*jWC{R-vf*CZ${Hiz&jm-57^bGV2i)2A*7*v`S
z$%9ymAVL8|go6lB3HOo>lx0{;^7C^HiXs^p7}AZw;J5vAP;CXmMOq+X`PSEgJLEVW
ziqshx7<h^_7#J8bE50)@6lqT0Xe`HJ3(Mu#jTN~QL8h~Vi0aAACdzI}ASMTh;06((
zdawwblO;f0eh}dYA}T>d705wMd4)xBARf5D5CE|XC#Rb@GiFX+ZIU4mc76>=A#+)L
z;VtIG_`)J^k;DX2SvT3pw9Fq=7~f(|EGkN@EQ$vSM}rDK7D(ae0@BM{oSBju4=N%-
zl~57bP;k}<6BZynpmJgsBLhS6gvsAc6($Rs*>dqPb20KTg7D-3vqCknE-wZKhLwyE
zqrsZN1jvM2Ea|Bwfy$Ggo0*%f1DUuU<O-&u#UM6FThRs(3#=YYfHkp#G^MC9F)$Qu
znjB)jS{$s4uP6ef7OV?g8CFgfvrrYN1_^?L7!-6xHIuz9r1WNhJjk5yo2JPGcGfNS
z!ua&eveZ1JVh&_gQ3A+4lP6nv+9Emc7F&5yYF=_i5!fzlF62y4EeQv!u>resv!vxQ
zrh0H3)Pbz#0aXcZCBdnA#rZ{$3M>QU&ZQtp!SvJ;$K;aI#GDZ40Jr?2<Wxx2nF&&n
z2O_{JVGT$vdtzB?QDS;((Mpgw$UV?%^cHtoNqkXiVoGLSdNH`<FH$cB>0nALDJlag
zE(a0dKm`+E@2+5AV6flZYGcD%zZE3C4MZS95FDG}xY-Vp0EJ@F4iF1uF*x{d@x{Zv
z?wRN6dW$(FrLbrTNDY>7Dgq_CWF`iNVm<~21`aU}MhP|%MiE9HCceoIPWwGTS*xgz
zfq}tKlNFkSKqf5%5z9fuTu}5tqmZ>MF{d;YJpw_>x;MK!-()fcWy+WT85kIz&zcDa
z_K*?)VgXN44#-4M3>2*fu|S!iXu;%AH+4%;W+?(^TX@KW^8+;XfZ6C4A7)@+n6`PV
zTL*JJ$l{_CATv*b2#|f661SKWb8^7h@)n<CNl8&=QfW!5Yf({t5q5`wbe;kcr$GeR
zMd0WF2fa5)4B<A^xbrRoCx5gk17#LXXnx-bN*F99sl_GOymE^Z6sIBa<?*RIH?w=^
zGG&5%;HSxjaOf>=c$$T^XcmCX<W4WjEGS4ViZ4sefh1ytBSGm18i2Q$3NhLvM;I6w
zUTnVW%gH1Pju?bSa1`yE{M%nz1JueX+6D4FI1PgdP(r@N3vx0f2L>gUb4|7js1FC1
z5}Xi6qnidY0-S|FhTUQ-O3X{i&n=n;62_8lZ*d`vmzXRPc%{Avq!LVks|{utSCfwj
z`--lDl!Glh4`P7}Dp35~;wZ{50rvrJF@|U|gNv%7L!f|V&rK{S$jnQJraDmeDFS)C
z=qLjNgTmy}pg_hIn~w)cF)}Wjyd>CCoHv`P=u!=13R4PmE0ZKcEmI9c7EAGg$zOsU
z8TmGwg~T(~gK9&+BCsE<LD`BC!Un~45x9~B`3zhMLaNJtkW=Axrlu4kgWO_C&dDq&
znh#O{jyrIf$5vdBnwsKOGzTOMD$8!M7o`?wBo?IJV#&!Y)`4~qK*buQ{0F7tTb!AB
zC8_B}i6yB;;Bp9@=A|ZEhdG*nBmWX8G}sDLKuy|P?8&()@wvsYPyty44wPC31_pg*
z28Lp{$s5AtM4334q!_uFIM_KDc|eefi<N_Y^Yt(_Mpsa(g_JZ|Al={+djg0B^2aTf
z{DRaxtW^>>NX8wK*y|@dMR-NL1nGJO3P|=M{HC%d=j0csVl|crECn^UlbL~`=<Vhw
z5m#6ldnWIUJLUu`V~RF{ECPj95n5@DNaJW(AM7iThl-LXAB@*xy8$v`>Ew^`V)dZX
z<Q8jYUO{QeE!N_KoXiq%?R1MdCpE9g6r@fRM0^Gj-~tldPyx3^K&=Q(4oDgW#}X(7
z7lBmXVoA?W%(=ywdW#dph|kF^E`d}@;0y~UK=E`-XmVSE5aaaCGZPe;<-vx6yEWKy
zpyXtqL`lZWlRqUd^V0@}G6y8uz}^CT5R~YO8bJ;PSFPZRvl+z2W*ld6Nq#|4eo}tP
z!^zK53fMrIp-5qJOsatyxZFn=b{UruJjJOcZbhl7DMguOsV^oUN_F;RVq{<_VrFDu
z&}4z+4zMv`N8mJuuQV?h$)I<jNN3&bm)61Pf+fI<;K>wPloqjr>;wlXm;i?>XK6u7
zVo7RPX4RL;QW=ekPeFbF1rkgXSOBaZOju0blOfH-#4`D2hG9Lp*kp9mlq~{xKs!Ks
zJ3&MjhyXPKAxUsEhzn{j73~JGKurm7{aysBf{VaO6<oT33ihHaASKs81gO9&0%g#m
zHz4kN5CN*Bix@y2VPy1RU?^e%G1(Xy7(gBH<dPx|5T6r7fSrpVK)x;NW?*2b1GP%c
zK#d9p7Df(fhI$UJ$rm%Fg#$RaIQTehIQTf&IoSBvc=(FCC;!hZV)WZwl(m>m035=g
zc774Kbh|!TwNRfiX>w?xnHWgw7GH9HZf<-^W>IP~xKS)Ld10Zjf*`02;sH083ySiS
zQ;Umlait`dB*qt(rk17_iA=sysH_9djaxt}S;2)~5kJU3%t@tbxA=-P5{pt(;z5Qd
z=A{$~PgX86ivA4>2_+B#?gzk!0B*5>hAVDy=OltE?0AqxMWB`)Qc~cA<U){>i`Igy
z<jKs-EP<OZG<idjjtE+)fg)0A@`s`VMxM=y#mg8ubwTZ}mCQxLlLbm8<-mn44{DHu
zLmQOsBPP3-x-hD5?kV+TlLiH!Uy%k#sWyl}vS;##Y6-^F&73t;nH<1As|0xu6evZK
zAf?hELIxBr?1{;RrI|&kpgJNaHL*Cg2*a#<b@GfIlmFH2tiKM*|9nM4Al;%M-K?p(
z1tpcYxYAQg;`8#$6EjPS6hV?8ql<JwEIF{UoczR;;v!j)fE0+(0}-Giun63s!d6hQ
zr>B;<B$g!3WMN<^2KBtS7$=)G{A0X6*|ae^8SDfJkSXs#!Om8imy?;74Jn!6IkiXx
zq(A{g2!jYMkXtxQQj2mk^Fa07Sx}&0vAYOV7%ratyU`~CoF}<)XGkuPxuBRS(gCsb
zL4+}g02u*oGk|OX#|=0dK|M@1mdW#*By2%UP0L%H1(|vI$;la+`9+}G<rZsRX>LJf
z5je`gB`UZuV*(iiZjpi8Jj*73X{zA{2cZop1QaI!Ym%5exmiY}2-LZ~#hsj=lLN^-
z#YH@m_cdz^@PbSLH()?r-CHV?|22tE7HHvP6qqdEA{qwDTtz}4HNqf51Y|0AVo`c>
zer`cdYDsF59!P|_xU#qiR5jgVEvYO>EiMuRi8AFE7lB6QZgGOUvHAHq#YN!EBsaOM
z#Xdv|WS}L8umTaFa-#_3d29i6i^C>2KczG$)eh7QDh3U2a<FnRLR%1^Rs<u6Co@^L
LRhNaKUWgX}l)H9)
delta 5753
zcmZ49!*r&Li7%g*my3acf#JJJaQe186ZvEq6DDfc)-y7sFr;wgh~<h$iG$gkITE>&
zQIfe*QBt|mQPN;Bt{j<M*(g~sn>$A?S3XKUS0PFvS20R4S1C#fEY6st9HpGXkiwXw
z5~Y&DkiwIrnyVJ22A1Q^QP0(g(g3shax`<bqO`zl{+wj(T%9N#Fo!WmH%b?5fL@e-
zs(zNi0>c!6g^Wd0QW#PMbBuD0ql_6DI2lreQiNL=qD<TwQbbZjTNqMAo0+0a-5FBE
zQp8&rQpB5?qRiYGQY2C&TNqL#o0+1_C-*UCGqOzn#pt~`f=Q8unWac(at)j6<dtkG
zY>EsF3`I(lh1nCh6+lb~e{&uCH%3O)&83`t>=1D!u;k`Gfh~-TrkkCGWEmOFHzx{T
z<6;z>Y#|rV$US+6Trwl?<Uew<-1Z=~4j{s5vWmPKw<(ATCd@X6$|o~9%Q7%9ykulx
zV7SE=5}%Y<oT@2Y1k!YiHNLncwV(*B*9D}@5u}K{Aip3dHLaw`9VFxhBAh1wR<vOZ
zoou0`D&!3k@MU0N&}1+21F?f9=PRj51%Q}=AR+=pfb9npW}DY4En#E~pB$v(q5`s@
z$Q7i(4rCQeNoH<pQ3{9;b`VoaNs-g!T`G=j5L+fQsyg|CT>x_9Eta&>yyROP@$q?y
zxvBB-MK&N;ut8k`GLN;mB(bQZC<(-7PR&cX#gdYlmR1CI2beJ1JVEsj6Jz+~UX4gw
zF_68sAR-e)fSh%UDX-ubYgKAdelawNa`RJCb8a!^mEB@1PR&aROet~#DbAX#tZD3)
z2V&-fhyoA+cWF@}h+70AKt8_3UYuB#>XV;d1o0L~JQqYngNT^Pb2R0QKtXwntvEA1
zuQc}-Yg$oaZfa2w$RF%wsX6h<xhcrL0BN(C{6;g5v1qclmbzvzNB|s+Wgr$PT#%GQ
zd=8SZm^@oclF?#vqOi#3lUgfTncgr>E-_eH&zQwqlwZPE!e7IX#a_dZ#ZkkM#o5eK
z%Tyvz!&t+V!d4=<KnNt4CA^R^lQD(8nWcsyUZjRGOSGuGM68CfnX!f;UOa`Ph9O=8
zq;7#^3g<$Qm{bj8mUIn6yi5vL4MUcUG(!zTyle`04MV(K4P%yk4MUb(He-F!Ob{o9
zC!3{swiiPaBZybS5HDZDn5B@yo6T6X5+nkaUk#E^;j3YYSFB;oQmSFdQp{#7+6iKV
z<#&VRLA)A<c%>AOb;>miSqxdqDg4>YMQ>^tvKWg&BuGRco2lqgiAr-lOqucm)f&bu
z<`lt&OpFXQ3|Yzx)D|*;Ss>*?*-S-mO4OUds*yvah9O>~h9QeJg*lt4=wAwN4MUbj
zim(j>T#Y6;EVR)0+BJ+>j1w4(P3uc^Qbd{=85v4+LDqu<Q?EoH&Ne79gtLum7_y8O
z7=uD)AtT6X3rrS*9AugYj-(V0uvMlY`5K0J1&9?z1vQLWW)m2T3Xn~$VaPIAV7`!{
zww}3$d4WX=57<CBt3+^tWexK}##-hYX0Q(#vv@&9)G%a;EU-#pt6|P$1nXc}U=0eT
z6oz01O;NupPD3MeJp(-hJ;NdyP@)4BKSgpNmOO|E1reaq=p`E{d9#+}=d<J(6opTI
zZ!G4b!N9<fF5mh(aEBbHLy;;-sTu<VLuSQy28JSaP&vlLz`#%??CkI7=jt5d>F2Hx
z;;#@IWSXj|X#>ldo+gUi@gS}2Afj?|sfn^<0*J{0B0#lL5jf+CgSdPk!Usf@gNO={
zo0#$ni()`LaQ@~8vGOP1GjV22pR8n>ArJOJ6-Xg-S$yFw=EV5IB5(-;a$8Z&<dvpn
z{-Cn+7HeWrQDS9LEXaUJP+7$SDXTyw$SvOD%#_r4P>EKOnV(k#HWZvw!Gsw|4=7vD
zVq{<_Zkn8DrZ9PmnK=^|^W>{$g=%2c9t;c&D;Xh1f;E8&kj`5y>8T}wGLv)6&CS+;
zOj--FovCOchz-(Kv<}1qs|OQcO{^eIDe_DV3`H9z-!xw>4%Wq26b4cY)&;J!$|ujZ
zP!*^I34%iv6l6tJlP_3E=}iL%O1^KJCKK3Mx7Z8g(=*Fb^N>m-kX1!-3=9lPlO-%Y
zZIPUJi><sUH7_}%2y7QN7jmYjmV|@Vn1Wrod7kAlrh0JL*MO|%0hQctCBdnA#rZ{$
zdL|9z&cz@}!SvJ;$K;aI#GDZ40Jr?2<Wxuvmkv^q10ujFU=>I$dtzB?QDS;((Q=SD
z$UV^7?iP1iNqkXiVoGLSdNH_wE>bTB>0nALDJlUeE(H<bKm`+E?=E9tV6fQCWoyHl
zwHYM61w<f15FDG}xY-Jl0EJ@FHV_MBF*x{d@x{Zv?wRN6dW$(FrLbrbNDY>7Dgq_8
zn8_QQu6TelR8bED1B0I?D>Q$AG%W!UOF_hJNXh_LKdfbmIi;!SaRpM=viXejO(s)N
zUVQnVfq~)qteIe756R*X3wVmMKqi6$yJ#hd1xn{d^CsVNQ?~@=hazyMga<M>$wLzg
zn2m1nK?Vkf{>@hI9nAG0i;Iqd%sdVvK=x@$++t45$pPoQTYQcsB}JJ@r6sAZMMe2V
z*c}4Wc>+Y71QB2tfkPe~%AO!GgxgS~$-4-g($QiElrJ=)S$aDt4X~7?7MEc2$}LV%
z^o7Ki$ER-E+~b|gR1fljpC%W=p|`l<$rIMbm<KYGJH05gpdhs<zAQBdl3o#x1SJ`0
z0N!FM#AqQLVqjo+$i%=<3~K1GadR<pFtV}ou<|i-F>kK$`^+c`jxL08;E3BZxgkJW
z1C$(!c7TEeoQS~$C?((G1$hLL1%ndHnI|6(s1FAh6PysQpqmCV0-T3HhTUQ-O3X{i
z&n=n)62_8nZ*d`v7o0pZ@Jf9bNF|s6R~gJOt|lK5_7z<QDF<727Q_OVRG?V9#Zi=B
zk_akyZ!v~wGK0&iq647dX3tG5D9Fr9ho(hv?Evz4(P0J#2Fc04f&v+{Hv0riF$!<4
zVN794VQyuTWT<7TVaQ@BUO2fiSY-0)5M4&z%|Aop8S6pSpI;H!^_HM)#t319;=Tx6
zD}ww~6b^D4xPI&fu|SoirW7Kt++s=2$t)<E3z7pz7`UWkD=tV)P4Oz41ri38V7J(d
zQj0SZ3sP^f<YX3u8}I52MWDhAQtE>e^exWJypq)PqQsKaB5>&gPX3aUnZq4Tz%hOS
z6a;LADWLY|E%xNxl=$3YSWtj00*6F30|SEwGXq1h)#QqBIZkE{CMiZPMh>RUtHae8
zT|tQ!QqW|8bb!m|J`fA!_FF9Z1*v&hD<5u<j5{Q)CruWM^on>6()9uqZtO+)O=V5a
z$uCaDYAg>}3Tkc+GXq1>>&-hNudp(9P4109<^(EYiq?ZH0)<ZzTDgo!%xF0u>?@Fm
ziV`PJP0*^p1~LJZeQpWBb8(1sKmf>9pwi_QYi3?SX~`|t;)0ya5^%+Ii#aDXugDmr
zQ3OPM1QFl@6WlfdH$OlP2Tcx0>I6p>DD4)3RNi7q&ri&`#h7}FGd(|XvRxv(2B@<D
zO|>ASKv8u|C?z#5GcVOWKQSjDKQphy*~hbLb8eynvpm>*a32I)Hk6#KkR-{tZSt{{
zWqz8V7~p_JDcH+k&w~<SQ9Z~aa0LslJ{v(?Y{qdGm*f`&<tOEr9GSc;wSWzjFN)+R
z8>AVSfvW_BVHa^3!Bd=C;#QQJno^WmmU?>fv@~Z=21W*kB1T3A22B=7ZUGwub_7mi
z_)7DFkqo*BN)XJOmC`#HU9g0G5j+V)OW7h;ke%St5KMprnX|MYC9xzmEVJtR<aZg3
zijP5l00k0E6IcMO9!!``o{%Zc^ptt>+DyZGaM{V|rYTzl?k%=~^tOYD4iEuq078=I
zMi3X&YAV_ZVu2bI;99;2R67@elQFo61C{wjmq1FcfCx~jRRqegMXx~Iw;<v#i1-I0
z{xcXcFcdL?{KUe@zyRtkCzlklf%xno0_<D_0rG88=j8q@QGI3(W-ew9VGd>v6%Gv!
zZZ2gGeGVlKJ}wRpehw=RE)G5p8xB4Wb`CZ^HXgpBPEdz)^7E`XMxV`Y*^Ai(z+nw8
z_rOKo)yY2#^%)Z;YZjR?I!#U}5)l;y6-2zr`MJ4?c`5OQrKzQ<MFNvMi{u!EC$A_{
z76RwYO&~?A;6ks6ck<mLNk*Z`-;0bye}eo5ZmNP?_>jK%WV>Q<eNamcDN;EhSqto%
z)gZ%oGV?M^;-S_S2~2J&))7JTKPXCACm$&;VB=zBU`S?~e9v2ab6`mqqk}f6m9>(&
zNDvg1tcAsyRjIeQ3Q9}j^YY6RGfP%Nn#4sQZD{!v+(WRQEM4ZpsJc0!%#+Oo6gGZE
zYM@Ba1QDo?xFrl4if{uJ_^HJu&X7>@*nGEUDw8YN-|*lm5(nv)1QAjoXR#+H7nWug
zrGjdUoYchP)FN!gOHH0oE5(>S`BMGP`m3Pq&sQV>GC~++1Z!$;K}qE;uJlxx&x+(h
zk|6Vov_UKxu(F)|#FXM9X^?;fh|mEMpu(^S+|<HWPOzt^mbfIAB-XGnFcb?gFfed2
zPUdR-$9Q!zXH#-A*a>1FQ{I5$imfy+Co?Y_QZT_YYLO5~fgFec#Y2$>$Ss^DsYSV&
zd7x_VG$?IgvAYOV7Pd`3-{b?ylH9oSBL~QEP|;kZ1!C!f2tyD7G6E@P!Epx8GobF}
zOHhpo@)HLG3nP~_pBW!7hcbsC4_6ULR@34ZXF+COesXe#US@s~s4}|6npc`zP+0_y
zOK>3zF5j3y9spG}Mc}s3lF28UYq-I|Y6S`vg~<Xf5|h(gWcY7!C+Fwnq$Yz}ImJcX
zlc%?6bMt^S^MVMz$!A-%>Oo!GTl~4DIVG6|Mfu68#l@he56G28pd45v1X3ytGM78C
zC_Onpw;(6AB(+EfB*I)=SzH9Fx^A(SR2HNb7m0#InevN^K+VltoZy~qetu4I5jYou
z69r#pZb5!gNls>xUTH~Y&g8OI9UDoI9p)gy0z`Cz2vCq<3&~p?Ho5sJr8%i~pq5jy
T%jA2l;*1iLzqRTz3FQL-bT4DA
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index df733ac..9a943bc 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,7 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
-from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e
+from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e, mir_approximation
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
@@ -86,7 +86,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e'])
+ choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e', 'mirros'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -424,6 +424,9 @@ class RobotManager:
if self.robot_name == "heronros":
self.model, self.collision_model, self.visual_model, self.data = \
heron_approximation()
+ if self.robot_name == "mirros":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ mir_approximation()
#self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
#self.publisher_vel_base = publisher_vel_base
if self.robot_name == "gripperlessur5e":
@@ -494,6 +497,10 @@ class RobotManager:
# so it can't be all zeros
if self.robot_name == "heron":
self.q[2] = 1.0
+ if self.robot_name == "heronros":
+ self.q[2] = 1.0
+ if self.robot_name == "mirros":
+ self.q[2] = 1.0
# v_q is the generalization of qd for every type of joint.
# for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
self.v_q = np.zeros(self.model.nv)
@@ -836,13 +843,35 @@ class RobotManager:
self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
- # TODO: implement real thing
if self.robot_name == "heron":
# y-direction is not possible on heron
qd[1] = 0
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
if self.robot_name == "heronros":
# y-direction is not possible on heron
+ # TODO: REMOVE OBVIOUSLY
+#############################################33
+# qd[:] = 0.0
+#############################################33
+
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ #print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # good to keep because updating is slow otherwise
+ # it's not correct, but it's more correct than not updating
+ #self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+ if self.robot_name == "mirros":
+ # y-direction is not possible on heron
+ # TODO: REMOVE OBVIOUSLY
+#############################################33
+ #qd[:] = 0.0
+#############################################33
+
qd[1] = 0
cmd_msg = msg.Twist()
cmd_msg.linear.x = qd[0]
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 6446153..9f5debc 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -60,7 +60,7 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data):
return breakFlag, save_past_dict, log_item
-def CrocoIKMPC(args, robot, goal):
+def CrocoIKMPC(args, robot, goal, run=True):
"""
IKMPC
-----
@@ -92,7 +92,10 @@ def CrocoIKMPC(args, robot, goal):
}
save_past_dict = {}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- loop_manager.run()
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
"""
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 09157b3..2da5229 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -104,7 +104,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
@@ -293,7 +293,7 @@ def createCrocoEEPathFollowingOCP(args, robot : RobotManager, x0):
# from one to point to the other.
# point activation input and the residual need to be of the same length obviously,
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
@@ -447,7 +447,7 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
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 f3163af30f2c40fddc884a5ec95aabd5f6cbf222..7d915c240c73fce908dbaf592187296265279892 100644
GIT binary patch
delta 586
zcmbQOchQ(HpO=@5fq{YHSypiRGN*}rGK^&twexuSQutdKq6AZzf*CXgHqKisSkKMC
zz`)7Cz~Iclz)-Bh$iPs-uz+zPLoGuJV+}(*QwhTY=7kKkj44dDOexH@%q0w2EDKl{
zGSsrvFf3rJVJ=}<z`l^7hGhZALWUZqg)FtKDJ(S%S)3`%HVi4uU>2(l15}K~h9QNm
zh9Qd!F3VM)0+NOZve+<`Fl2Er;7MUm;Yi_3;abR2%T~j%fVYOVh7D%%LS{yW621le
zP!-Iz>@b;HjuM6i0x8@ztTpU493TfRWU1vW5nLdY&XB^hh;bof4QIS?4dVil8qS4`
zj0`ml3q(s?7#4`7@Yb-VFhl%M!jL7NC9#kdsz7ofLoHVgLk%OyI4H@L#uCh+$>&!T
z$iTo*6vV*5Fj-f`n1i(>KR?GnQ)+UVh%#4E0!S_qB)3Z>u)dIifkBhIs2n6%0U|0v
zL=}jr1`#zNq83C%fQUK}Q4b>AL4*gK@MK_MxFwjISrngGP*9X#k(rxVl9`|P9u#**
z3=9k$%xsJtj4X^Sj9g4S+#HN7|GC(>7`Z0%35l_@a4?B56*X<P7COw#q{lc(ERL-Q
g<Se1dDPjd|O&~jZCSMg(W0afxM@*K5!=1+-04<D$CjbBd
delta 94
zcmca;JX?=1pO=@5fq{YH?t;K{F^h?OGK_f>wewh+f*CaVHZEH$xcQgJW@e@@43pPO
w#IbcTFfeEeP8OCd05gj`CL8h!^K;nb=BJeAq}nkuFfbI$Ox_?V%f#aY0L#M|vH$=8
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 71fcdc3..f687b61 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -235,6 +235,64 @@ def heron_approximation():
return model, visual_model.copy(), visual_model, data
+
+def mir_approximation():
+ # mobile base as planar joint (there's probably a better
+ # option but whatever right now)
+ model_mobile_base = pin.Model()
+ model_mobile_base.name = "mobile_base"
+ geom_model_mobile_base = pin.GeometryModel()
+ joint_name = "mobile_base_planar_joint"
+ parent_id = 0
+ # TEST
+ joint_placement = pin.SE3.Identity()
+ #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ #joint_placement.translation[2] = 0.2
+ # TODO TODO TODO TODO TODO TODO TODO TODO
+ # TODO: heron is actually a differential drive,
+ # meaning that it is not a planar joint.
+ # we could put in a prismatic + revolute joint
+ # as the base (both joints being at same position),
+ # and that should work for our purposes.
+ # this makes sense for initial testing
+ # because mobile yumi's base is a planar joint
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
+ joint_placement.copy(), joint_name)
+ # we should immediately set velocity limits.
+ # there are no position limit by default and that is what we want.
+ # TODO: put in heron's values
+ # TODO: make these parameters the same as in mpc_params in the planner
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 0
+ model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 0
+ model_mobile_base.effortLimit[2] = 200
+ #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # box_dimensions[1], box_dimensions[2])
+
+ # pretty much random numbers
+ # TODO: find heron (mir) numbers
+ body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
+ # maybe change placement to sth else depending on where its grasped
+ model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
+ body_placement = pin.SE3.Identity()
+ geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+
+ geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+ # have to add the frame manually
+ # it's tool0 because that's used everywhere
+ model_mobile_base.addFrame(pin.Frame('tool0',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+ data = model_mobile_base.createData()
+
+ return model_mobile_base, geom_model_mobile_base.copy(), geom_model_mobile_base.copy(), data
+
# TODO:
# try building the mobile base as a rotational joint,
# on top of which is a prismatic joint.
--
GitLab