From f40385e51fe995c33a7555b877710596f3559738 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 21 Oct 2024 11:07:06 +0200
Subject: [PATCH] optimal control looks like it works now that the state bounds
have been added. some trajectory-tracking finessing is required to make this
operable on the real robot, but that's not a real problem
---
.../__pycache__/managers.cpython-312.pyc | Bin 42063 -> 42107 bytes
.../basics/__pycache__/basics.cpython-312.pyc | Bin 7093 -> 7809 bytes
python/ur_simple_control/basics/basics.py | 21 +++-
python/ur_simple_control/managers.py | 17 ++--
.../optimal_control/optimal_control.py | 94 ++++++++++++++++--
5 files changed, 110 insertions(+), 22 deletions(-)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index cbec686b753e00fb945018a65c3a29abc042fc33..e8f0cf9924bbd4c123fcdc9cf58e2b3c93cdaf56 100644
GIT binary patch
delta 1963
zcmX?qg6a1OCf?J$yj%<n3=GeX38(Mc$gAqcxNWns+eBuj6t2lqUP=lz3|TfHc~qP&
z#lTR+U&EMXH~C?X@Z@MOZWb^nP)B(3O+P+H2e7cM7Z)Q;fdHe!<Qy+aenpr;DGW6X
z@k*2P+~t_ET)@hYdzmqUS(^OW8Vn3Y{w3}(#jBwVkQOMDp@spZPg9uDbMnF}5vD9J
zup#B%d>}(Kg&BP(A54~?ywh6*#Lp3y6$Bf~U*Zol0NK#V|Gc9aH75V_wq*>O+^8ir
zna`JJGMkSngvV0D5U)Ks!A)zjjjuazB+P&s#w;(8VyPO2cwLxaNi2+oEUga{EJ=j1
zVA29L4Dp6=L58f<$sc2+KsFjrcJNb9hTE1RyqXcB47+tG@>uLckw;j_nB_1zP)C@V
zAzop!otqq6v1$!NR?_5$n!=2kkbrfV+^8ix+1+1i@<Tmgfufn1J_LCIZetBYRvbwC
z<cI#klP@%Ju+=ivFlFU~V`qUb4>N+z%~PTY@eu<<RsqNrljjC-a)8v=FvM$2KH(-e
zxgfxrv3T;wSeeZy0y-EOttJNsofZ`Eb&QC2bar<2aSd_|@$~oGEFEmf$oPM=Z%6^7
zzM@J8%M}jU8~no8`4ukmD=cuiu3~vn#qtur<&Ko=cK#Rb{4WHBUa||lz!7?ZKXmim
z&`(T^qLVK~6bLP3U|`T>Dw@u~z)%EA_C<>(2SsW!N^Gu)WM^VrIJqU-hjH5Ei_u<;
z(vxLkK8Zrq-(o8-O3h2ofN4>VHDuJ@92&csg>mL&&Lm65Ig_1}+!$w1?n;tnG}yc%
ziJM(-CCG|ZAfgRKbb^Rz5COIulr)QaKwPjZmQQ||Dakei#9uTiOPw)fvqsipM($Z4
zfw>@J!sKh&a*T5)f6Z24tl2D<<If^66C^$#L>PdGNs~(pf+cq`FfddJ1?4B@m-r^;
zC8nnq>BXm~mIStMepq0_#&!^-^3Y`IaxE!z?I4-MAmYg6_;Mv}Y|ckGdO0W<S&Bf}
zw`k4eHRbk<D>uI>?`4qynR<&mJ+;I!xui5PCorW5>bQwD3bG)dYchd#-C`<CDQ03|
zU{FvfQUlq#d-Ay&3C0_npVz!(Vgs2{v}N+OdIiSylfTznG5*?Y)DXsK0gesMqI{5s
z91u|oA|R?ktT`ZJJp%*7Ew-Y>yp;UhqHd7LqR9sujoCPu85oL#C(E~nZ1!#PXL7=#
zt>_X+Iatq05Gxl%fc$!kqbR>5u_QA;?-pZ-CNtOzMLQ?oYms8yGx=wW0;9lWsn%zV
za+4X`p7Yc&rZA>3Niozi)-X-}I7ei%p9hyzEn|r=EGyJ7!ZLa-V-4fvgDE1D^NV>I
zVJx1>4ei{M&7(yoC&Y74-e1Ma$TPV@L4ESUcAd!+G{h%Mb?`F^Y}V>%V$>BZ0tLe@
zmgJnwf})8a*Mh^Q02~Cx1*xeiUPb*N;TQ%6hROFjrPx4mP_%OLzfJ|lvy<h!j2Pu7
z2X!U;ec)ga5}DvMA#j3Nhtmx~@#%6C<rc797F6wUx+5q$A!)KihtmxnzJAY6&k4yr
zen`wag2K}!CQ8gunk>`dbVFRS!==Y#GjsPHM#f2#AM_|QE}G2JYszT5*{Zh-A|%Vf
znw*ngoLaPK@~b{`M)%29z5bgE`)6@#f)a1hVvuvOr9O~Pi@*`Sda}t(OSY>Zg)=7?
z&(vkT#hjCxH+kty4K`3@6@8g}d1f+W=w|a-Dr}64CdVuYWbB!|Zb1p-!pX`DmBPR_
zECuOa3nDgvh}j?_6hzDg5nz{qlfZ5e_X3Ex3?lY`h&&MS2t+&u5kEl0ugS9)>amqD
zvWk8xnS6d>HRH<3-ize9!HJ9oq+r(O(nT4Jj4YGyEl%Q3;9#`>!~i0TG#D5d0M-~C
AbpQYW
delta 1943
zcmex;g6aGTCf?J$yj%<n3=HS|h0+&p<W+TJoU+;2Z6Y&E4P%zg<id7gZmw)828JSj
z7=LoD7dJ$*Kwfn6Nl*F7abDbvV5O65y#yHTCii(s@+*MM1mP5h8ishq$$9Q_Oj%AK
z^CrLaGGhd@Z27Y_7#NEDOI$$;L3lNYU|;}ifif9t7_yvTYy_R<K6ygC2ve2^*vOgQ
ze4^MCc~5?*B{kV{t|Y`lSwXOc{3X6HgODwptl$&Ps6JW2$Cfc*a-)_MOASN3CMs8J
za)O)I<Tzh<-f)-+HH=vvAk|Ve4DmWJ!IEeg3t3tZCRh>=W5J{aY8c`T;DQWU$smV=
z1dU*Vc{VkSlO3}~BMnO6cBcrhW`xLNw;V+ti|r`#29r;DDonQX;+@RtFCm%$F&CN4
zN(Tk`<SU*clN}T|Ca3w!GG<Jks3Xij6VtmYH4O1^3u_p%Vvr4ju(Ps3eq^g<s$t5?
z1;^q7T^?oxo13RZ6QYuVAuDfkK!7r%`s5RCa+4<oSTh!a^lp9;(8b7TIk`CK^yYwI
z14hOVn;Sz47$+NsDQs2<`^3a3GFdpXKxhF21A``0(KH4Ih9Vmfv2gO#NKHoZ&HE$S
znHU#LJ|6AEICZi}j2ENS<oK9Rq7e1B*vgAi^O7@QTGC<-8MQXgh~3P>IAgL;k|pEp
z$!$q)jI$=6O_F8Q-~2j>o85N>$n=#Uq76iJf(TGbfY`kX#O(nQU{@?-U|_h#7oVP5
z5)QV=Gtbra7IR8UVbOGunuU{{v(y=bH)m%pX5^j;5}5-c`X@`~$T7~DY>}hDSiLzi
z$Dc)D21tA!hybOaqKT7t6a-7|WME*Z5(>&s$}jOv%u7s9Ez$#771*{}rO<?p?Epym
z!O3yuT2koRK{AIx#No+{%9XUS`3d0@kS8=*irhi!S5JOlZqK-4vwlS{iv-BjTiofX
zC637@rHMI#DMe7nU8zx!1x1A>6Ij<RroxnBCI$uu1%)CtkSV(+3)V_7Uf--$`;v(b
zWJ=NI$&w8UjO!*_HCQqJ++5la#%KYKBF>_GkcJ!(Q3@g;szI#TAYvUTfZ2)?^HTD2
zi@HG~3nw!-88fnPHg8(N<b*|a(M6Cdu+kGCRxXGDdGQuUQGQ8cNoIcDEyfT{X0XeP
zc1%`mm15jI*``&2k$-Y*>oZ1!$u4cr6-oqQ$+m_OmOoRNz$H&q4O5mdIIa78aI(}g
zmWWP%S0%=jB|f>ZU6Bz)$}yENPIhdOoqVBPfRh(y&}6j|`N?t}Vv}<xiB9g<keIx{
zi)FHZ2REbO=ID+lMkApjP$=ADNzTbED4GCrH8^Mrz#&jvkeZs}Rn!L(2Ia$B%sHuf
zlexO2*g)}6v|_SWmjdIN$=+Q?jB=Cfx|03Aa4-l6PcZ5(pP|@Qb3;^mdfddg1umCG
zH779L5tW>wG&y1d(+vTk>6{ZeXDCnMLt@?$6`vk4F=9s2<d_LeH>71JuuS3Dtlxcy
zk#XYWkA1T2Ecpegc|{8+8}yno+D!KA_uss=*BT<J!nkm<Sid==+h)K1mz<iQ^jowD
z<Ya6K5ain;aGbB2oG{ap?FvZYjLGw7>arHyVqjpHd~v1*8z|z6K2QEXGnp}DbMh<|
zHpYdMI~D{oc2B;wpoDS3WdDUqVPG4UfOM|`5$i$3ED#Y2BEUHg>=JNt*aZ?h4<ass
zh}|F}4@5iy5l=zHcM$P&^6`awY{iVMqMwQ;|6W+lxMFhoB6)6bf@1+Gn7Mi3q6|hx
X=E+=3lKA5}80|kXfXE^Z1_lNIzd-jP
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index e00e5fe4b51234988fa9300a62849b129253b847..82aa642f4eee74e77dad389ccccdaa2651892031 100644
GIT binary patch
delta 1941
zcmdmL-e}8rnwOW0fq{XcN=h{SlF~#z3C0r>)f1fAQ~6U^Qn}MuQaD;zq6AVnTNt7Q
z!6Hm4TrDh7LMhxW3{k?VA}KuC8Vn3YbC?)XIah-WVqjp15=|A&(w&^hAte?i&cu+)
zohrVX5h52Qkt&g8JeiSChJ%SARVq~?%WQHYhwx-wMxM#VjKWS?mJoHwWC|}#cM4w>
zD+5C+bCwmdJcJFCncT-Ht|<pLTn?s>k%5UJl`mC1RU%bxH8Vs#BSWf0mOaQNY?X|a
zjGFwLKQOwoS>NI<F3B&5Pb`T~%gjs6NoIwp29xXz3=AOMpLZ}ZFidAiVXS4%WT<7!
zWT<7YVa#Hj+{ht2Sx`Wht(LQfxrTA_LJmnruF1POG<~^hm{OP|L7~jezyNa;jIQOb
z;V$6?Tfo4;P{WkM3{nY`&5mJUD6&PCpDf5Jl2OZD!%(DE!U@xs#Rp>{Xk<+(tRO>c
zxIrY$Xc%3?1e3J^IRM2mlLa{?nRQDzCjS$YVagJoe33)Ao&)B_6gH4?Ra^)^rLbo+
zF)$P{rEt`;!c;*lt>FNvtK~sh2GU!@2^Qr|;jHCL;i~0FljW=70Yz&S$XWa~yb!h!
z14FGqjX(-R3bzzPtzZpfmMF+sliv#R)brG^r|^J%!CS+g!V6|`)Uc=Ufmxh2>?!<U
z7BrBdPGe6IfQll8QVn~GAXt<&lc9z^jX9V>Q^?P)NPvNX;TBV2@k>yGzQt5l{PO?*
z|NqmgMb^1LtYU7l51A~%rO93-4pPQA*?~)jk$ZA1mzwM?)~eK^{Nf@|JQeXXFfbGe
zGB7Y`3KeNGFfdH+;xe`<G6d@>xy4=*pP84ET2Z7AQf2@WW%4e##pIoGi?u91IXC4N
zTVV=_E*4^7U{IL6S6F;9C$}P(BLf4269WT7@uJBl+?K3g*ckYQCKqr^)<0n9o1#1+
za{<f5{L4aWm)O<r2#LbED+DhJX?3vQ<d(RjuDQHoVZ|2a4W2tvHu@iMzi8rf*}(U*
zx?hLqgp^MI8SWQ(R6g)Aa0*N@o=~|!WMchgVf7V?7lpMiacJM+<?oN|jO>r=jGN)O
zfOCb)LjDa*7Zr>z${2H8<~7++a*5Y^@<Q%rK@rggkB;C*-y1N-WMv*#6%JU|ngGsP
zC7{p+r{oeQFa=5hlQ)WrO4TsdFs){Rh}ANefU+c*lf?l_1k8+CT$8zZ^|{f+#Tm1}
zr5so(oXFyXmObn|gU%q16vjDhwJarqlOOWRGYU^;<dbI<nXJes8UspNU@xRF!E!Jo
zLnT8cgC?_|CL1ErfYTs2Q56}15|S~9kOw6?o+3~wRs>2f;N({XDt(G9Knbyx&wv|L
z1Q)4+R9j6xz^AY3#=yW(07_Sv7#J8D7@i18UXilgVRGHt_oB7$iP8&U@t1`XCUf&^
ztMdu;XLM#vD4kwCv3h#l#JbCTYIm@RO*ZA%<lto&U~BOBBCt7)KZcpLNDpNDaUpvl
zZBTk=PAM$b<Sya?Il^SJgm8=nsKkf($O9zk2~x#bm=d3uTvD2tQzQWr^a2U;<R(_c
zXXcfprWYmV6xmK*BW%MO%D}+TviYs>5oSe@nj*Lnnk+>i`-*&F+KW6VUlNZN1!;iT
z4hoebXOOhZWG4wbMytt<5-O~b3=9lcHm{OkV`NmAyiu}T{1!`6etr%lNI~IM1U4aZ
zvZYiLR~`cc11LQgzn^?m%3Ml^QF(*Q2L>HR<pVOGLDZ)hMgc}I#t#~k`K6Zu0D={c
AssI20
delta 1370
zcmZp)-D=KvnwOW0fq{Wx)nuXcB!!855{ySCswdd9q;jV*rLecKMDeF^v@k>oq(DSC
zTUeq5Q@C0fqJ&a~Q@FD=7#ND?FfpWZt_B&xz`zhCk}8s=Jy}pvcya?1yI7PM6GJL@
zs@Q5qh<ucIs(6+mOqz?4fr%kiGF3dwWU`>7lmjC}Dsz?@L?tqrWsb~)uv2(oW~T5~
zu`+;kT1@`WB<7OJoMiz~f=pI%F)&2Qf^Cz9X$0HOmnxPjo+`VV8KRnzAyquf7Un<>
zMutkpN=8k-&0iQ@*_e`9COb-qh;3qIV3^L3!dT0a$xzFh$xzEy!;r-Y)x*KeUc)f?
zqL2)0Ek_Mg&EySCqHG}d)pAZg$Dzs0S;IK_7l$-IR}EJQFT_d)h8o7n2RJ3`nNnD5
zS!x(lm?aq)7;3n{Bs<9AwcIs~DQsXqM3e(8%9Fxg%bUVc%ZDb*Tf+^qu?l1gUkwk8
zUCUp?pTdyBDaBALP{WYL53?Oc*E2BG@FT*ehP#F>h06wH2u}@L3OAUA>NvI(9;hg?
z<JeMo!J;gg3^i<N%)tzre12|40t^fcx0nixUxHFX$StO_;&h0`AMDd-%>;qT{@jw2
z3%I2jIVSgTtFadeGB7Y`3Qj)HZ7g<+vFsLO$u0Jh_{_YN)QTcq1_p*Ay~$EM(kviV
zldX6xxa=4h7@Qaw7>ea5*YH@HJmC|ZqBLE7qWTQQ8JR0sX69d%(!DICcbQMW!Tkdp
z0|);U<q4S!SSIFQ7E)Uwcu`2}61&#qDBkADOngF<8TpJR_p)*_-C~?<$0y0pSi=NM
zAGORStdkjqW%z0svp66g0FjgD3d>A3VB}ze$g+TBC(q&2XCy(fo<<7e9F|&^5_rti
zFxD{FFk~{+u&ibU*}}k3%UZ*lB?wJw>@tiDH4Iro5GiD`h^It!@(uwRBNk{-fx=54
zL>Pbwc~Ho57lD#|5h&P-Kq>JSYgKAde(_{sK?816kSaA0VK&)MP+!J{fq@|z6t)W)
z7#JEDo(M@^k+R%jdO-8Ckk8~rg4&b63Tkk0vI?*@czog6tSS`4%=+^G|Ns9dw~5*d
zX@O#%Ii;{zle>rqWTxTd>!L9hpnP5=4&u9k2oI1d&cc-V#N?9F#GE1tkf0|>kS8~>
zB0e*(BsIM#F{j9Ca<-TaYX}1aL+R!<Vn>)2L28QNMrg7Wf$S^tfoU)Dm^@J`S`?%K
zV!H!K#t}p~O=gs~V>FxWAg#h0!N9<9baSRO8zZCq<N}#;K}f)WJYEFW8!`ExOcPfQ
p0|Nsn0T;iR+$C!+EYGOC!sauBJfrd_Cq_O-FUAillW)r|0|59q6MFyv
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 444ea98..47977c2 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -63,7 +63,7 @@ def moveJP(args, robot, q_desired):
print("MoveJP done: convergence achieved, reached destionation!")
-def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data):
+def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, reference, i, past_data):
breakFlag = False
save_past_dict = {}
log_item = {}
@@ -73,10 +73,21 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
# which is pretty much true
t = i * robot.dt
# take the future (next) one
+ # TODO: turn this into interpolation
t_index = int(np.ceil(t / reference['dt']))
+ # TODO: move to a different function later
+ if t_index >= len(reference['qs']):
+ t_index = len(reference['qs']) - 1
+ if stop_at_final:
+ reference['vs'][t_index] = np.zeros(len(reference['vs'][t_index]))
# TODO: check if that's really the last
- if t_index == len(reference['qs']) - 1:
+ #if t_index == len(reference['qs']) - 1:
+ # breakFlag = True
+
+ # TODO NOTE TODO TODO: remove/change
+ if (t_index == len(reference['qs']) - 1) and \
+ (np.linalg.norm(q - reference['qs'][-1]) < 1e-2):
breakFlag = True
error_q = reference['qs'][t_index] - q
@@ -91,6 +102,8 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
log_item['error_q'] = error_q
log_item['error_v'] = error_v
+ log_item['q'] = q
+ log_item['v'] = v
log_item['reference_q'] = reference['qs'][t_index]
log_item['reference_v'] = reference['vs'][t_index]
@@ -98,10 +111,12 @@ def jointTrajFollowingPDControlLoop(robot: RobotManager, reference, i, past_data
def jointTrajFollowingPD(args, robot, reference):
# we're not using any past data or logging, hence the empty arguments
- controlLoop = partial(jointTrajFollowingPDControlLoop, robot, reference)
+ controlLoop = partial(jointTrajFollowingPDControlLoop, args.stop_at_final, robot, reference)
log_item = {
'error_q' : np.zeros(robot.model.nq),
'error_v' : np.zeros(robot.model.nv),
+ 'q' : np.zeros(robot.model.nq),
+ 'v' : np.zeros(robot.model.nv),
'reference_q' : np.zeros(robot.model.nq),
'reference_v' : np.zeros(robot.model.nv)
}
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 340d36e..3e4cbf9 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -452,7 +452,8 @@ class RobotManager:
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
# and i'm not clipping it, you're fixing it
- assert args.acceleration <= 1.7 and args.acceleration > 0.0
+ self.MAX_ACCELERATION = 1.7
+ assert args.acceleration <= self.MAX_ACCELERATION and args.acceleration > 0.0
# this is the number passed to speedj
self.acceleration = args.acceleration
# NOTE: this is evil and everything only works if it's set to 1
@@ -816,13 +817,13 @@ class RobotManager:
else:
# this one takes all 8 elements of qd since we're still in pinocchio
# this is ugly, todo: fix
- if len(qd) == 6:
- qd = qd_cmd.reshape((6,))
- qd = list(qd)
- qd.append(0.0)
- qd.append(0.0)
- qd = np.array(qd)
- self.v_q = qd
+ qd = qd[:6]
+ qd = qd_cmd.reshape((6,))
+ qd = list(qd)
+ qd.append(0.0)
+ qd.append(0.0)
+ qd = np.array(qd)
+ self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
def openGripper(self):
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/optimal_control.py
index c8daeb8..e3fb969 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/optimal_control.py
@@ -4,6 +4,9 @@ import crocoddyl
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
import argparse
from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.visualize.visualize import plotFromDict
+import example_robot_data
+import time
def get_OCP_args(parser : argparse.ArgumentParser):
@@ -11,18 +14,31 @@ def get_OCP_args(parser : argparse.ArgumentParser):
help="time length between state-control-pair decision variables")
parser.add_argument("--n-knots", type=int, default=100, \
help="number of state-control-pair decision variables")
+ parser.add_argument("--stop-at-final", type=int, default=True, \
+ help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+ if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
return parser
# TODO: use fddp and incorporate torque (i.e. velocity constraints)
# --> those will correspond to max_qd and acceleration attributes in robotmanager
def IKOCP(args, robot : RobotManager, goal : pin.SE3):
+ # create torque bounds which correspond to percentage
+ # of maximum allowed acceleration
+ # NOTE: idk if this makes any sense, but let's go for it
+ #print(robot.model.effortLimit)
+ #exit()
+ #robot.model.effortLimit = robot.model.effortLimit * (args.acceleration / robot.MAX_ACCELERATION)
+ #robot.model.effortLimit = robot.model.effortLimit * 0.01
+ #robot.data = robot.model.createData()
# starting state
x0 = np.concatenate([robot.getQ(), robot.getQd()])
state = crocoddyl.StateMultibody(robot.model)
# command input IS torque
actuation = crocoddyl.ActuationModelFull(state)
- # we will be summing 3 different costs
+
+ # we will be summing 4 different costs
+ # first 3 are for tracking, state and control regulation
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
uResidual = crocoddyl.ResidualModelControl(state, state.nv)
@@ -43,24 +59,56 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# cost 3) x residual (overall amount of movement)
xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
# put these costs into the running costs
- runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
- runningCostModel.addCost("xReg", xRegCost, 1e-1)
- runningCostModel.addCost("uReg", uRegCost, 1e-1)
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ runningCostModel.addCost("xReg", xRegCost, 1e-3)
+ runningCostModel.addCost("uReg", uRegCost, 1e-3)
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
- terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e3)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ terminalCostModel.addCost("uReg", uRegCost, 1e3)
+
+ # the 4th cost is for defining bounds via costs
+ # NOTE: could have gotten the same info from state.lb and state.ub.
+ # the first state is unlimited there idk what that means really,
+ # but the arm's base isn't doing a full rotation anyway, let alone 2 or more
+ xlb = np.concatenate([
+ robot.model.lowerPositionLimit,
+ -1 * robot.model.velocityLimit])
+ xub = np.concatenate([
+ robot.model.upperPositionLimit,
+ robot.model.velocityLimit])
+ bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
+ xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
+ xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
+ # NOTE: i have no idea how to incorporate this into anything
+ # we need to add state constraints for things to make sense
+ # NOTE!!!: there are no hard inequality constraints on states
+ # in what's implemented in crocoddyl.
+ # INSTEAD, you define quadratic barier-type costs for that.
+ # there are box constraints on control input though,
+ # but i'm pretty sure i'm not using those correctly.
+ #x_constraint_lower = x0.copy()
+ #x_constraint_upper = x0.copy()
+ #x_constraint_lower[:robot.model.nq] = -2 * np.pi
+ #x_constraint_upper[:robot.model.nq] = 2 * np.pi
+ #x_constraint_lower[robot.model.nq:] = -1 * robot.max_qd
+ #x_constraint_upper[robot.model.nq:] = robot.max_qd
+ #state_constraint = crocoddyl.ConstraintModelResidual(state, xResidual, x_constraint_lower, x_constraint_upper)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFreeFwdDynamics.
runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeFwdDynamics(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
state, actuation, runningCostModel
),
args.ocp_dt,
)
terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeFwdDynamics(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
state, actuation, terminalCostModel
),
0.0,
@@ -69,25 +117,49 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)
# and the solver
- solver = crocoddyl.SolverFDDP(problem)
+ solver = crocoddyl.SolverBoxFDDP(problem)
+ #solver = crocoddyl.SolverIpopt(problem)
+ # TODO: remove / place elsewhere once it works
+ solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()])
# run solve
- solver.solve()
+ # NOTE: there are some possible parameters here that I'm not using
+ xs = [x0] * (solver.problem.T + 1)
+ us = solver.problem.quasiStatic([x0] * solver.problem.T)
+ solver.solve(xs, us, 5000, False, 1e-9)
+ #solver.solve()
# get reference (state trajectory)
# we aren't using controls because we only have velocity inputs
xs = np.array(solver.xs)
qs = xs[:, :robot.model.nq]
vs = xs[:, robot.model.nq:]
reference = {'qs' : qs, 'vs' : vs, 'dt' : args.ocp_dt}
- return reference
+ return reference, solver
if __name__ == "__main__":
parser = getMinimalArgParser()
parser = get_OCP_args(parser)
args = parser.parse_args()
+ ex_robot = example_robot_data.load("ur5_gripper")
robot = RobotManager(args)
+ # TODO: remove once things work
+ robot.max_qd = 3.14
+ robot.q = pin.randomConfiguration(robot.model)
goal = pin.SE3.Random()
goal.translation = np.random.random(3) * 0.4
print("set goal:", goal)
- reference = IKOCP(args, robot, goal)
+ reference, solver = IKOCP(args, robot, goal)
+ # we only work within -pi - pi (or 0-2pi idk anymore)
+ #reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
+ log = solver.getCallbacks()[1]
+ crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+ crocoddyl.plotConvergence(log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps, figIndex=2)
jointTrajFollowingPD(args, robot, reference)
+ reference.pop('dt')
+ plotFromDict(reference, args.n_knots + 1, args)
print("achieved result", robot.getT_w_e())
+ display = crocoddyl.MeshcatDisplay(ex_robot)
+ display.rate = -1
+ display.freq = 1
+ while True:
+ display.displayFromSolver(solver)
+ time.sleep(1.0)
--
GitLab