From 21e3d86836fb0d5a67adee4f1bd610563de17a1f Mon Sep 17 00:00:00 2001 From: Federico-PizarroBejarano Date: Wed, 1 Nov 2023 21:31:11 -0400 Subject: [PATCH] Attempting to use far stricter constraints for the 3D quadrotor experiment --- .../quadrotor_3D/nl_mpsc_quadrotor_3D.yaml | 26 ++--- .../quadrotor_3D/quadrotor_3D_track.yaml | 90 +++++++++--------- .../mpsc_parameters/nl_mpsc_quadrotor_3D.pkl | Bin 5225 -> 5225 bytes experiments/mpsc/mpsc_experiment.py | 4 +- .../safety_filters/mpsc/nl_mpsc.py | 2 +- 5 files changed, 61 insertions(+), 61 deletions(-) diff --git a/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml b/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml index 3367c01fe..56458882d 100644 --- a/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml +++ b/experiments/mpsc/config_overrides/quadrotor_3D/nl_mpsc_quadrotor_3D.yaml @@ -2,20 +2,20 @@ safety_filter: nl_mpsc sf_config: # LQR controller parameters r_lin: - - 90 + - 10000 q_lin: - - 0.001 - - 0.06 - - 0.001 - - 0.06 - - 0.00025 - - 80 - - 0.00001 - - 0.00001 - - 0.75 - - 1 - - 1 - - 1 + - 1.0e-01 + - 6.0e-03 + - 3.0e-00 + - 1.0e-05 + - 8.0e-00 + - 1.0e-5 + - 1.0e-08 + - 1.0e-08 + - 1.0e-02 + - 5.0e-01 + - 5.0e-01 + - 5.0e-01 # MPC Parameters use_acados: True diff --git a/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml b/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml index 813d94a74..848ca59a8 100644 --- a/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml +++ b/experiments/mpsc/config_overrides/quadrotor_3D/quadrotor_3D_track.yaml @@ -10,9 +10,9 @@ task_config: init_state: init_x: 0.4 init_x_dot: 0 - init_y: 0.4 + init_y: -0.1 init_y_dot: 0 - init_z: 1.4 + init_z: 0.9 init_z_dot: 0 init_phi: 0 init_theta: 0 @@ -26,52 +26,52 @@ task_config: init_state_randomization_info: init_x: distrib: 'uniform' - low: -2 - high: 2 + low: -0.95 + high: 0.95 init_x_dot: - distrib: 'uniform' - low: -1 - high: 1 - init_y: distrib: 'uniform' low: -2 high: 2 + init_y: + distrib: 'uniform' + low: -0.475 + high: 0 init_y_dot: distrib: 'uniform' - low: -1 - high: 1 + low: -2 + high: 2 init_z: distrib: 'uniform' - low: 0.3 # Just so it doesn't crash into the ground - high: 2 + low: 0.525 # Just so it doesn't crash into the ground + high: 0.95 init_z_dot: distrib: 'uniform' - low: -1 - high: 1 + low: -2 + high: 2 init_phi: distrib: 'uniform' - low: -0.2 - high: 0.2 + low: -0.5 + high: 0.5 init_theta: distrib: 'uniform' - low: -0.2 - high: 0.2 + low: -0.5 + high: 0.5 init_psi: distrib: 'uniform' - low: -0.2 - high: 0.2 + low: -0.5 + high: 0.5 init_p: distrib: 'uniform' - low: -1 - high: 1 + low: -2 + high: 2 init_q: distrib: 'uniform' - low: -1 - high: 1 + low: -2 + high: 2 init_r: distrib: 'uniform' - low: -1 - high: 1 + low: -2 + high: 2 task: traj_tracking task_info: @@ -103,31 +103,31 @@ task_config: - constraint_form: default_constraint constrained_variable: state upper_bounds: + - 0.95 + - 2 + - 0 + - 2 + - 0.95 + - 2 + - 0.5 + - 0.5 + - 0.5 - 2 - - 1 - 2 - - 1 - 2 - - 1 - - 0.2 - - 0.2 - - 0.2 - - 1 - - 1 - - 1 lower_bounds: + - -0.95 + - -2 + - -0.475 + - -2 + - 0.525 + - -2 + - -0.5 + - -0.5 + - -0.5 + - -2 - -2 - - -1 - -2 - - -1 - - 0 - - -1 - - -0.2 - - -0.2 - - -0.2 - - -1 - - -1 - - -1 - constraint_form: default_constraint constrained_variable: input upper_bounds: diff --git a/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_3D.pkl b/experiments/mpsc/models/mpsc_parameters/nl_mpsc_quadrotor_3D.pkl index 573960b953734bcff0d685f2868239a8eb2ddafe..1e538f666307c68c7aeae53fb3f747d490900325 100644 GIT binary patch literal 5225 zcmZvfc|276|HmzZVzPt?H<2QWQqp#Oj)ZE;R!L+T>kx4VNu@;gCCl&?DSM>FT3W_u zBBfhtv384)lqFknE8@qT^YM7x$M4P`-j6w-@jS1`>-~CvKIa5eQtYI7xSt@FTC4z* z;cf54G6wa@eh)m({E8Jgz;N-SvutPAq>uS~`2=b@c{5!!z5G4>-04gvJ&+YE=<7uH zq%&E`tXKi=PiCvLU!af6+!a38&8(=%2$ls)jm96#yPaiXVzOsu_QU-{8PTt3S{h0nvezSarP^{1~XF^Ew?7xPw z40s(`#O-)#R2ns$#h^y*2&VAxT!m`ajk^Xx6D@Eg=u01D#d5lRlOJ4?& zXV*%>gDkM(+_uRBs+Cag+y*HX-^)P8D0**j{{v_nzS!k5B@=vh4e>wInFVLo4G)Uu zwSwgQdhL#cYKXYLjs|D-imAAw(vK*W1%XgCx+GX<%@c4NE*bSSkB0X%DmfdCzXHX^ z_|>PBIbcJQuYMJy6Y8%@&Hb4421Fh+v3&(z0j5#u)aHYuut}2Xl%mrNkY2gkPiO z*WLeK4R<7j^GP`;LJfPWB18EWKt3S7i0ko;#^zfz2;Mq5=9`pBy4H(_FC;?N_A{6#(>y@;#O zkaD1E*;SC2=w%e;QUyhxM7@;bWq>G4)4s@|2zcKn&yaHG9H4m~DYyGG9W+^Mn%<*D z!qY4N7^`mfhNy4IU*rSQi@0(vs}DEPM!?AXtx{s0Rp4P%Mf2gb8aUv=v-S6vd{}7t z(QoJRS3o-EZKiy58}uLPTt!0qI3tzr7xGWwl3OMcjB-`_EFylkIS6 z-whT#nj<^VEAk4idc4|o^n4jOS-D%qO|57?XDAP-Z^&Qd1JaAQEgE+^W@cW32QGub zy4jWRGA>OSjJ^XT9UMDH&o@ECt?gU)No4?(Uz9VH2h=y@FY*EDMO^DMHdnz&El|G~ z_FgRHe?VmV=@r_}AaHTobJ~2-Ul7d&$}h?p$^+^f@)!Al^dhc-LeBKej2M2s;LQco zJ#8>DUT1^ux32klLvw-hi*knYfcl2~MLr)d}PDnbgnU)N8t2y-?;Lil< z5{0tsY2h#YB?*B)uJnW}>2Q%a2PD`2ryxZCj?`B3SqvwxXGD0nNOe?UX4 z7Eb=DV_leD4t=+0AX|nczK6Q8~1>Yfm%~hpi0$etBcHKA@0xc_4IUV#==#`sn8DN_P zT|>+Nd33x5_Sx9J7I|_5p3HNu8Qv<6i(c{SDOCRk7ai6a&YigrODiWsE^L1XEHsVj zDrRg5NbWbE8%pIVKR{!ZxDK@>2BckzZ(O*m2Dny5cO<$#f}!H}Vg1gZp#C<)dO@8= zc>US;<_2CW?sDk(_kNY1kf%YSzjc5E_uOG=gtd2oYr^(o7OHiyCZ5pyIEmmc=2f;b zpzOsf`d`yG$7|z9uRVJ-_PYy?<}Q0lE0}@XjG8wG^%!!R)7K`LX2^1caCvDlT{-;Q z($>cs9-^FG*&2#9J!SCX!ztBw-VI;}Bz$PiAoSiMxqS^ix=${u;JZ63KbNKcfWQ6@ zvtJ%D0}q>?7>9Hnj)2bAxFd97&gqJ{mBWUD__eGDf$<`I9P78Mhkksjhv6!mdSA_Y z;QW&CSDQ`v@H&ssYy5%WrnF`(eAA_m!vWvhT5>Y@UM;JwZfTu>x~rvE^rZs+v2pW? zBT)pENEvvH6wk{+DPb?%pkZ!jw-`j0%-Up^(76hS}SnzmlbNM6cLW<;j^}ZFQxE_^TI57 z$5xPb@Ycv_`8P0>%vm*=hw+O<-Rq`&;658|%~y;n#s_F~xU zcfSVSJ{?%tP3?l(u?6?Xzi@#2txqRivig9d6)_jjUJ&_hCUcfe=3y1tHzw)tD$)m; z^Mqb^lDni-+OS`-A9Q#BY2bKR1&S|iUu0mp8|IVqWw~SQd*660#$O36UljF))I4JxRS{4uTmms11>}! zEG&t>JtX~QkUmTp5_;n;2yVm=`0)CNM!?;s7CO6l`qAw@y`5OZGxGyM7r~#YO~f;E zEdT7oJh>3_SEoaI=P>lZyfYbBpBChW+bWZYx|yOJ4L7 z_5tKlNl5GZzGW%o62-~h!}mWn&E>NP-RuK6&+N$-lQcYgunz-NuXYT0e%U<)P(3pE zb@oMI1fc4F^Hyg2csoF~LVA$9UPjiv8EHlAm7M_9cj||8Zn*RTR69~^hUe~ws_6r( zDf=CL096H1^V#>1VSs9AK|fa?s&%h=W*-oS0jfN>Tkza>RIS6`7R`M}m9Gkj8*~hT z`O5xMIJeG6R&L7#+~{6jbkf3}&Mj;B%ioa}@|(?dm{$&ues}Wf zM-g#+-Kq1zBGQ-Gx7G|csEMSrzt1dBn%G!|saVQf?GmfRLQ6g<6bw{hV=2<^)6%Q) z7@FHak)ttNYJYhGZG8Zic0gkP2h+d7!1E*xmES=eQ$DW7;nK9>mKzb3k z!{vR@)#EyNbIM(@-Fn09mljQjZA?C~^O~XSo59^2J||;&ivDA^d{I+QPySK1N9h82 zn?1ka(zb46nL=N-NJRnltd+&QzsLuq7jcEV+%2qboWPaMH;KEoyWtn__42P$lhW&p zGp`D|*o5tv{;2Yg;uW@t5MTJWi8^d>eQ)pLrZ5cuvTM^0A9MD6-$;Lv4@fWK+N~a5 z&M%+?GkyW-e>=i9-K7?~rePj;)(8Ru<0YA^2&%+!T0`;}87vi$! z=kNWBiosBRQO-~vP~VWh$Oohsal1mk<|`yo@gvtMgIS^uSlf3HR8};yeR*8+m>BPK zfaU__7v&7)0rd^}i+n(O5%=43inrjCabPv&doGcE5L?r(y;*6E-HUGqOv z#IeIq`_GN)U#~H`y4r00Q48>cCVWLN+X^gBRKMwE8VJPmjki^lGlAXj7BLHlSQzIO z@Fr4&04117@+Jcz0Q?%B%zwTOgjq;LkvUzfiW7%Qwm)_*f;ukNCyq!+NRFIX>(&CHTu~xqK+`jmw8I zS_J=s1y?_O9mVBCwk?+r&+c#L@}K1RbM?Euot)DTZUzy2#exy8{`X>BKCmn!_}CDk z|Kd@?pRP>8AL;W1Kg);UAKgIkcb+Er(~|_>hC}$1=g#HBMtOoyyHEJDkKhxpF~VNn zvrh>2j_&mTI-056quIbgTBsy*?}CzQ1py-=?4cyodw4GIR8EnefSU52h#p};_1Ges ztlU|x^E^DPw?P=3yr_~*FZ`;f)+(cp)8159kF7WIOMawx(|(J|`;C%N;MRo)k||1H RNF4s%bnlnm+_~qk`F}1-0Pp|+ literal 5225 zcmZvfc{r8n8^`S>TcK?vlthbaNKG8inNrD?>}$uqFFD9kWF2KGaY&RUS&F9YiQ{Qf z(LyB@Qi#x)K}I7De!TB{yRNx@bN+B&?|Jw>-}iHW@Ar9y@uVCV=O8{IEctj|ri(Yj zo^=>CCi~U8g-*xw8o4<7*|MzX*H(J@d!7hVwD)E@Dth{R__^9LnYKZ!cs^fyTMt_% zE14C~OZ>`wb?^&1;kfV(m(yO>xwA1W1D3otcRZ&WOHWTvYi|BS{Ltp`W7!>8ntplV z^8~Ygng2a)4i`Qb!No5T@ug!try0wICti@jbhP*O^7Ui-+xyMyyDW)c;^jaZNuK}H zXx0HvI~KVer?#NB;4dr}!E^h=csMw8_gKzvD?)Em6CSFNAI9!7IOzI>5#q97HcTE4 z#_M}Tjb&vMv7SPP%!$ZCG&o#cBl(vXlI@e8^jTGk8R1$UQp{AOITh+a>t}BU&*ma+ zH}Qa9c$oOB@RfGw3)k^KqyEdkI2t2S`{xr6gVPXOpxh)(t`(>J_&FzpKNo>t&@Ga+ zAns>UgIQb8GmMSGieH}NO2Vm2RpoU@@=>(oUh&*VB}i8N2=~_|s`yu#GJVmj8Tj+P zoh9G1%W!1L+fieYV>nOs8NI$RW|0H<1>G<9X~g}AuCPqWGEYRpPRS9r+VswgIQv-M+#R+J_@Vm8ei*pp=70yxX2ecfM3u}whtus z;gHP~=qZ1NSpz-4D5~q>+na>ldfy*HPQOLQS{|}RQS##a&VOZ~rggyqU#-&cSf%{U zH5!)|`v!c0!=hhGH}#k=vCkSUZ8seyH zbPgW;yXE$?h{LGN$ysJ=j53CtK^~xQz!x}xU(n4?Tt@8kp>I!H>2JCiJzjs#V>1rOP$VL2*azq z#~;mgreT;1$S>p!@&J7UzQ6(eg6=`%OGJM{Hg6Z`7Yo1}Wujv0LtPf<4dw#!3ps;4 zK;M8bZ~(ucs~|znsb`&FU#@^VhB=0LgSmkGLe3x$&^O=<9KbK=);NR^_rsch)WvgZ zvs_lYtX+6exk$icif8dL#mT`D#iiXDenB6fi0HjEvBno?)vs>jwLFMe4XUI0yN!^4 zqxX^hVrTHHFL5r1##6EU=VSI=oGG}yZ$wi~H3lUq2&wg(=A)dgo5H1Ye37M{zVsi` z(Rjcy&;LeZ5#l?i@Tc`XW3;z_La6^~0OBr>5`3YkgW7K;9X!c)Loao`b(^@N@ttm; zNYBK}*uzV(dr0L9-t~K&MA7yrBx1W=s5&tV8y+dvGO}>NYR`|v%J4ZN*PQ%~kt)Tw zNYjgRdGUPAmnj_=dUX=3UvqG`zq}n~PU{a1-`a#d&CkqQOo!sbOd%g**Hp|?mZHC} zHXL8qWGQyH*ds313|p5gA6ZA^m)Unh&~wF)>y!p!uyo&4qf%-S`WkPzUfIbCO}{MX zdo~h+A{h4@#Z5KPL^Q6dP<2GAM!U-%wnX8O(p2m0+t7;I7{B)& zZRn?5*?I<#D!w2z%h=Me3X5coUp=tf7w6Y zY~G6sMK!MRD;D5hPtq@+CaJqWRkB;!rw#GFyPajQr5%lZOwmma@xi&3kNnzKT*l^s zNA~tV9>X^4->^a|i}Cx2bXr9FV^kipq+pm`hkp<`<5qRm8JQqU=ec?xq>wv9aIgp= z{SJ1Kx_lw|TbfnAp-iuB6(-4P`0nx@%WT${;FYy}7H5_D?mO*S5;P|93V&a9STP5e zp`nOpKdsd0!&cq31KG_?B*S%bCPV%rdYwb^U298nc+^Syou+hkOn120IgX=bS(ai$ z>}%v+^=Q&(t1}XiyW1q^7KgP~`bD-mrsIEZ#E7k$$-?;@*VT^dUq%VvRKlaIvQRzM zHxY`jOEt+Mnev-T>B=Pdbw(hPX@!ZmZ#04TH{26b%A+k)!0evVk1*LMCI%am4{o>WZxKjNxlb{ksM+u zzqSRWuBn!mrotO_x-JtnstSBScV*u6|M~7QvM-&w8lLbS?KIH4XV`iVou~3^MCGh9 zlFS3Y1lhL%MUrpVA(F%Mf7TOz3w=r5sqEe9&BZ^_-31*kZ@5&8q`FR7cBMTj{0 zBYFZUeIFFx=Urb{Pt9=)HE*A&xwu2+w|a`qS#Kbjhgr7(qHhMrNWQ@ohm)3{2)}!N z9wv0nvW|0=>5U=cF~#rT=;3F}@bI>0k?+iO?cFv93o6NX=6LS;mw9R-?rD!rde11e zu*8>PXUk+bQH$q~{{Hp}|7&WIH(+6rTFTK1S<#$w0hT;%0~rC-(zNTYD!t1I)Dn+9 z!_D_U9-7kg2i^P&ILG|Sb|>Q&aS%e4ntDoHL$wkm*P)K@dQPlEeY>lcxQ6oE>*5QIf|eXE#q~z_4Kr*5Z*YyOHAU_Cki3A{z6Co@9p4U z$4+ro^yzyKyHKaeM`JJLx9GJM&#JMrGBmLb)$|RY^=Lner&=_uH`DveW(keeUmJ|o&L&=_D5^?{>#G3GymrS&FX+Zj#t`?Dz?t^L)uckUR6$Owguy{}^TXT)-}ld)o+?vNE2ix!e7>RTOa!g2 zWop4U_Bc0i(Yy8ljr+X)r%1ZkB46MDenGeVs`H%Gk#+QI(no_MuVOSJ+r6^p_G_BP zrxpiRaRTiWpHI?fjU3v~-J#2FWxCR6QX5KT(MDQ!Lu76IgFTCV1HQlk{DQ7xToUJ= z089Gaq>VuN9fp*U$*qKKSiG9M9rQ1@&`_&-7 zkTb{w^bPm|2k;BJ4Q*Kmot7P<%SDYoJcXNHf_?+7q`V^e)<-UClyX)Zl6RKOrYhm7CE+D^8=y3~RKw#BE$>D6zre$*h5A~yCE%ng;f0Du zf40+*MG+{nkfABu7=$@Wh>C0j`F{L^8I80p2z@-gl+ed!4@mt}{R4!4MWYI#|2L&S zmnul;H<%Op_}X?t9}nk|`a5oubu+1tm9t3wsiUO+CRsusM{*PXvC!sSg#Pc8e&I(# zANkxT^>cQR{x1}ezW0;<@eay=`X-W3@+`?ipySN~AMDyi_{R#Z|JF~tLF%WQkbH^{ zlm3t8llnrWf8;^=Ck~rsp5F6s2n;({+y6S61&O2ifDP|z5%F(IDRn}o6OYem|6SLj zKi=VcEJ=OXx91Ow(Q);bt`{%MZ0^?eT}>F_m>E&Om?#|(iHFqb1~D^_M%wFKwlJ(j kzE`U^UfDVt9w&yCO6yoDtJk0c)uZL?$wu`BEB=cA17u+bNdN!< diff --git a/experiments/mpsc/mpsc_experiment.py b/experiments/mpsc/mpsc_experiment.py index 467b8fe5d..41b30d5b4 100644 --- a/experiments/mpsc/mpsc_experiment.py +++ b/experiments/mpsc/mpsc_experiment.py @@ -192,7 +192,7 @@ def determine_feasible_starting_points(num_points=100, num_steps=25): if config.algo in ['ppo', 'sac', 'safe_explorer_ppo']: # Load state_dict from trained. - ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_best.pt') + ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_latest.pt') # Remove temporary files and directories shutil.rmtree('./temp', ignore_errors=True) @@ -334,7 +334,7 @@ def run_uncertified_trajectory(n_episodes=10): if config.algo in ['ppo', 'sac', 'safe_explorer_ppo']: # Load state_dict from trained. - ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_best.pt') + ctrl.load(f'./models/rl_models/{system}/{task}/{config.algo}/none/model_latest.pt') # Remove temporary files and directories shutil.rmtree('./temp', ignore_errors=True) diff --git a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py index 28a43126b..1a2427591 100644 --- a/safe_control_gym/safety_filters/mpsc/nl_mpsc.py +++ b/safe_control_gym/safety_filters/mpsc/nl_mpsc.py @@ -1038,7 +1038,7 @@ def setup_acados_optimizer(self): ocp.solver_options.tf = self.dt * self.horizon solver_json = 'acados_ocp_mpsf.json' - ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=False, build=False) + ocp_solver = AcadosOcpSolver(ocp, json_file=solver_json, generate=True, build=True) for stage in range(self.mpsc_cost_horizon): ocp_solver.cost_set(stage, 'W', (self.cost_function.decay_factor**stage) * ocp.cost.W)