From 40de5baf261880d4a225757485da76a69a93f1df Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Tue, 26 Nov 2024 15:28:17 +0100 Subject: [PATCH 1/6] Examples 10.73 and 10.74 from Betts' book --- examples-gallery/plot_betts_10_73_74.py | 280 ++++++++++++++++++++++++ 1 file changed, 280 insertions(+) create mode 100644 examples-gallery/plot_betts_10_73_74.py diff --git a/examples-gallery/plot_betts_10_73_74.py b/examples-gallery/plot_betts_10_73_74.py new file mode 100644 index 00000000..6e7eccc2 --- /dev/null +++ b/examples-gallery/plot_betts_10_73_74.py @@ -0,0 +1,280 @@ +""" +Linear Tangent Steering +======================= + +These are example 10.73 and 10.74 from Betts' book "Practical Methods for +Optimal Control Using NonlinearProgramming", 3rd edition, +Chapter 10: Test Problems. +They describe the **same** problem, but are **formulated differently**. +More details in section 5.6. of the book. + + + +Note: I simply copied the equations of motion, the bounds and the constants +from the book. I do not know their meaning. + +""" + +import numpy as np +import sympy as sm +import sympy.physics.mechanics as me +from opty.direct_collocation import Problem +import matplotlib.pyplot as plt + +# %% +# Example 10.74 +# ------------- +# +# **States** +# +# - :math:`x_0, x_1, x_2, x_3` : state variables +# +# **Controls** +# +# - :math:`u` : control variable +# +# Equations of motion. +# +t = me.dynamicsymbols._t + +x = me.dynamicsymbols('x:4') +u = me.dynamicsymbols('u') +h = sm.symbols('h') + +#Parameters +a = 100.0 + +eom = sm.Matrix([ + -x[0].diff(t) + x[2], + -x[1].diff(t) + x[3], + -x[2].diff(t) + a * sm.cos(u), + -x[3].diff(t) + a * sm.sin(u), +]) +sm.pprint(eom) + +# %% +# Define and Solve the Optimization Problem +# ----------------------------------------- +num_nodes = 301 + +t0, tf = 0*h, (num_nodes - 1) * h +interval_value = h + +state_symbols = x +unkonwn_input_trajectories = (u, ) + +# Specify the objective function and form the gradient. +def obj(free): + return free[-1] + +def obj_grad(free): + grad = np.zeros_like(free) + grad[-1] = 1.0 + return grad + +instance_constraints = ( + x[0].func(t0), + x[1].func(t0), + x[2].func(t0), + x[3].func(t0), + x[1].func(tf) - 5.0, + x[2].func(tf) - 45.0, + x[3].func(tf), +) + +bounds = { + h: (0.0, 0.5), + u: (-np.pi/2, np.pi/2), +} + +# %% +# Create the optimization problem and set any options. +prob = Problem(obj, + obj_grad, + eom, + state_symbols, + num_nodes, + interval_value, + instance_constraints= instance_constraints, + bounds=bounds, + ) + +prob.add_option('max_iter', 1000) + +# Give some rough estimates for the trajectories. +initial_guess = np.ones(prob.num_free) * 0.1 + +# Find the optimal solution. +for i in range(1): + solution, info = prob.solve(initial_guess) + initial_guess = solution + print(info['status_msg']) + print(f'Objective value achieved: {info['obj_val']*(num_nodes-1):.4f}, ' + + f'as per the book it is {0.554570879}, so the error is: ' + + f'{(info['obj_val']*(num_nodes-1) - 0.554570879)/0.554570879 :.3e} ') + +solution1 = solution + +# %% +# Plot the optimal state and input trajectories. +prob.plot_trajectories(solution) + +# %% +# Plot the constraint violations. +prob.plot_constraint_violations(solution) + +# %% +# Plot the objective function as a function of optimizer iteration. +prob.plot_objective_value() + +# %% +# Example 10.73 +# ------------- +# +# There is a boundary condition at the end of the interval, at :math:`t = t_f`: +# :math:`1 + \lambda_0 x_2 + \lambda_1 x_3 + \lambda_2 a \hspace{2pt} \text{cosu} + \lambda_3 a \hspace{2pt} \text{sinu} = 0` +# +# where :math:`sinu = - \lambda_3 / \sqrt{\lambda_2^2 + \lambda_3^2}` and +# :math:`cosu = - \lambda_2 / \sqrt{\lambda_2^2 + \lambda_3^2}` and a is a constant +# +# As *opty* presently does not support such instance constraints, I introduce +# a new state variable and an additional equation of motion: +# +# :math:`hilfs = 1 + \lambda_0 x_2 + \lambda_1 x_3 + \lambda_2 a \hspace{2pt} \text{cosu} + \lambda_3 a \hspace{2pt} \text{sinu}` +# +# and I use the instance constraint :math:`hilfs(t_f) = 0`. +# +# +# **states** +# +# - :math:`x_0, x_1, x_2, x_3` : state variables +# - :math:`lam_0, lam_1, lam_2, lamb_3` : state variables +# - :math:`hilfs` : state variable +# +# +# Equations of Motion +# ------------------- +# +t = me.dynamicsymbols._t + +x = me.dynamicsymbols('x:4') +lam = me.dynamicsymbols('lam:4') +hilfs = me.dynamicsymbols('hilfs') +h = sm.symbols('h') + +# Parameters +a = 100.0 + +cosu = - lam[2] / sm.sqrt(lam[2]**2 + lam[3]**2) +sinu = - lam[3] / sm.sqrt(lam[2]**2 + lam[3]**2) + +eom = sm.Matrix([ + -x[0].diff(t) + x[2], + -x[1].diff(t) + x[3], + -x[2].diff(t) + a * cosu, + -x[3].diff(t) + a * sinu, + -lam[0].diff(t), + -lam[1].diff(t), + -lam[2].diff(t) - lam[0], + -lam[3].diff(t) - lam[1], + -hilfs + 1 + lam[0]*x[2] + lam[1]*x[3] + lam[2]*a*cosu + lam[3]*a*sinu, +]) +sm.pprint(eom) + +# %% +# Define and Solve the Optimization Problem +# ----------------------------------------- + +state_symbols = x + lam + [hilfs] + +t0, tf = 0*h, (num_nodes - 1) * h +interval_value = h + +# %% +# Specify the objective function and form the gradient. +def obj(free): + return free[-1] + +def obj_grad(free): + grad = np.zeros_like(free) + grad[-1] = 1.0 + return grad + +instance_constraints = ( + x[0].func(t0), + x[1].func(t0), + x[2].func(t0), + x[3].func(t0), + x[1].func(tf) - 5.0, + x[2].func(tf) - 45.0, + x[3].func(tf), + lam[0].func(t0), + hilfs.func(tf), +) + +bounds = { + h: (0.0, 0.5), +} +# %% +# Create the optimization problem and set any options. +prob = Problem(obj, + obj_grad, + eom, + state_symbols, + num_nodes, + interval_value, + instance_constraints= instance_constraints, + bounds=bounds, + ) + +prob.add_option('max_iter', 1000) + +# %% +# Give some estimates for the trajectories. This example only converged with +# very close initial guesses. +i1 = list(solution1[: -1]) +i2 = [0.0 for _ in range(4*num_nodes)] +i3 = [0.0] +i4 = solution1[-1] +initial_guess = np.array(i1 + i2 + i3 + i4) + +# %% +# Find the optimal solution. +for i in range(1): + solution, info = prob.solve(initial_guess) + initial_guess = solution + print(info['status_msg']) + print(f'Objective value achieved: {info['obj_val']*(num_nodes-1):.4f}, ' + + f'as per the book it is {0.55457088}, so the error is: ' + + f'{(info['obj_val']*(num_nodes-1) - 0.55457088)/0.55457088:.3e}') + +# %% +# Plot the optimal state and input trajectories. +prob.plot_trajectories(solution) + +# %% +# Plot the constraint violations. +prob.plot_constraint_violations(solution) + +# %% +# Plot the objective function as a function of optimizer iteration. +prob.plot_objective_value() + +# %% +# Compare the two solutions. +difference = np.empty(4*num_nodes) +for i in range(4*num_nodes): + difference[i] = solution1[i] - solution[i] + +fig, ax = plt.subplots(4, 1, figsize=(8, 6), constrained_layout=True) +names = ['x0', 'x1', 'x2', 'x3'] +times = np.linspace(t0, (num_nodes-1)*solution[-1], num_nodes) +for i in range(4): + ax[i].plot(times, difference[i*num_nodes:(i+1)*num_nodes]) + ax[i].set_ylabel(f'difference in {names[i]}') +ax[0].set_title('Difference in the state variables between the two solutions') +ax[-1].set_xlabel('time'); +prevent_print = 1 + +# %% +# sphinx_gallery_thumbnail_number = 5 From 89b5edd780568071b648e41db7845912220d8ce0 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Thu, 28 Nov 2024 07:16:19 +0100 Subject: [PATCH 2/6] example 10.50 from Betts' book --- examples-gallery/betts_10_50_solution.npy | Bin 0 -> 14672 bytes examples-gallery/plot_betts_10_50.py | 203 ++++++++++++++++++++++ 2 files changed, 203 insertions(+) create mode 100644 examples-gallery/betts_10_50_solution.npy create mode 100644 examples-gallery/plot_betts_10_50.py diff --git a/examples-gallery/betts_10_50_solution.npy b/examples-gallery/betts_10_50_solution.npy new file mode 100644 index 0000000000000000000000000000000000000000..7c3082e151afb68713579fcb2467a7dfff9992af GIT binary patch literal 14672 zcmeI$c|26n|37ddNgLWr*6dp%A$qHnN|K_iMb;FNN|uz!Rw9bXPWB~BcK6t~2xS>F z!x(19zLcayes?~<$M5_5|M&a%Gyl9E_s*Sj&pmVQz32UmxM-+*$(WVpA&aNDwT-LQ zJ#jTbadlf2aT!5z+q?Jf-?MbOard6J4Rik)%R8<%_Rb2?*)}CHaS(=2IlE&eVj!CHBtajmV>Lip!@_euHnuH7|)*ybv zNvM=>Y~L<72{o%n?;B;PP*3^{$$L@>5%Bolz4|n-Z*6>N){eRChj8 zx-)? zt-T(9KL&$BQo);i#vuB0a09X(gYS`_|FG+h0f({aC{b|?*1LNgJF$NZT+FnqkFt$H z^8L1h*c1ay=2+!>dKh58c_!K&W5C9JQ3vxf7|^~~IMgMA0r}gyW9q#bu(ra(mFmEN z$iRR^-fIk)H{=!Brp18Z&BSZb3JjRwzRFP`z<`u_!zIJ53@CWEaWP<(4#%oFKcAVQ zgYZeyGvR}D&`Wu8#Ni7arf=M{|AOcc^}hHcRzQabVV9|DsdPA6J@Msa6dlN&d3t@p zbP&1ddTh~~4$doCw-a3Hu*~LC*<(+KAB2}1-d?A}P3l9rVFNli44QG5=-}<}h=uMt zMTc#QZm(F6(V=%w$-+ZLI`GJCNqZ^ zi-V00ny-r5z1GozMfk&i@{Ql+*GGsn*nIe={8S+g5~^b>qh8V=y}#P=h#d_a>gp4) zlQejva-u_4fCimi1>uiZMnQDED<^|E3cC9jmNLsnA=&WeU9ad-*y33wv)^MB);=28 z`IwHv{c};SSsJ6zuJ2s;T5uGcnu>=HvyX!G?N<+j#z)|Z_T-t@9V4)}aYF7)(FmA0 zT@_7@9)XT?d(_gOj==WLytg{`BOvdR^C;xf2qcv&9lNDE0@vrfE|7#rKv7HY@bKml zn0po$cz=lsiYhHEZ%3$5s+Vnf{0kKf1v32|E2waIz~hEMIu$lu|1@{$4Hf+D%`dL` zP{D}H`-8YM6((J46`HS8fn#mcNs=xV1Rm@?YN>xhhtFl!1`_v+{>;M&(|4wT> z-%15jsx-ZPZ5VvbUSBczJ`7(bPwQSD9fo&zzkVp_9fmaRFW}rV46h7AH@~bLhSuMt zO>FtYaAGxH=UehH2pGqWEk_Q+V2|v^M=ypU(dP%_?9*ZRwHncI`@t}PkN1xuhhf;8 zp4gFSF$|}EN~8R%!w|FenmU*6F!Ut}HRYWehOx=L6YtfAVRTePqf22JQn^mroR=I1 zt*RT_HVO?xufTzyTzm2J#KiVp6{noEHit$)ep$rSiH|C7P? zjso5m>x_53qJX|W!zIFx0`?-CD&0LP(EZ^2m*#sExUN%d=;c6x@B*VvkvH)i-Cn-k zlmZ5$W-33gP{7@H*F9xjd_Vc-^?@_^?3d}r9Zp`MTK)s}e;tCH19Kw^FQt{N}W87sX zk8aV0%AZ59cDgdqpD+Z0agymQ)kE+&HS^}deX~TnHSC=QD@OcpIy*$6H zd>RCadi!l!NrQ0p`#>sN$RHFfpSD+VAB0HPDSe8~Ae3*6a9g}Q2w{ykHa*fDgm11n zCf0I;AQHdWUdlHJ%TGH;?rs_cuKQz3!OLVw(d2ezpCrTPaU(1802vZ2WvScR$Z%lj zDLJ-+497%RjeWAoa4zbyV`VHEw!|2mJokzWdhL$9l26GXF-?&$zDI_;^TO1AYchE8 zXs4xIBf|q1^B)!G$?$LQm@eOOGWc_z*K1NB!|3`8F{PqpVA&W!ncYhU(=jQs!&Wlz z?&UU?Vk5(l5t5SmO@f)p)TW2;os1|8$NMVV~1ukpKn}G_5b?+S7;-c;v0?{ShLhoYuLT zHB1D{A_EmJ3K2w&9M>WSiE#g;@EdJ15t6dnsRv0!=rIcDlP2Qd`^j3F4G^J}-@vnk zKm_TCGdtDr+&0f9HP%mr6SL}{s_`U?kc*f(vsYxixCQTT!@{xoa=aa1-ZM_wL?{cK z$&!sD!o!8-{j#A%@GcX$cH4&t?1*1%%#{dqY}@q&TO!Qb4-SPJ6G8V*TX@X{BCP&c zaTM1ig2xeVrzS-rY;4btjuj_D+ATqr%>6{DEZA5+xs3=NLqA6?*on~lb5jP_p8-%l z-l#Y>GXTf?BC8fh27taH&-(~r0LIQ|Rek6ffY&*u=yd%6RNeRM-BCUO62!PGy!ivb z7n=~F@nHbUjoW^{eLn!XKKDuc!UsU1bl~IXKs;YekEA~tfDi{MTHeC}SV?h_9(Ejn zrCS4v$7}|`x8#KvG8=#szY_!R7z}`q%j(dH3j=UZ@emhi4M0KSm3LO^15j!;w=Z95 z03w@6VnVV5P;4S*P%b(ERLYWu8~*@YH1&LDvU>oc^qT5!au2}bhPvd?jRSCVVrT2{ zx&hFv=A{^}5Fm9TK5TZ80KzXtUS`b^;OQNYXF(GLI9E*;dofCYmc*}e`GW+oxFvFV zv7Z2~FPqMre#7P%`5^0su-Dm`9Oe%nYYHCNd#ckZOqY*BS55urKn;w0bs`O zt;Smd$UPhBvVBbeM@CgnUI+okDU`Jx!34;?6rCLHPXOukLML@!0_ev(!0HnMWS+nK zo#aJ;3>U&pf;$0XC4^mnJS0F}nK+x&eFBI!@h^J25a3{GxXgDa0(hHPyn1?v04!|< zEV2#+m````(35WE%o_YDRyiSP=lT6OCH4BtXfJt?Sh-2ymu)J=@#s__!UJ zf5K%>fKB7$w_{BS&~ZY&?7Rs9xZQ29?KCC;Zx)Zn$~6K!=HGFB?J7RrwN4-T@pP}z zkv1_TK(WDUQ-J{i5-L`_MDY~YpV0nvg#e#N14JzG6y>kmEQx2YL>lWBJpXrHGYsuw zf=AaGd;@Rgz?Ub!e1mPv-yHnT_5dl7ZC!I+58St)WUklfg(v2V!|J8Ika;&w=axbr zh~=#-3drsQxsQk0--`Cb?K_9{UM2KHeBz!a=e-2r$(F5BjUa$|HTMU`W_+Gs8rlB9 ze*nsAT`NY`2EcjM(_Yq{2n4}yt}8!?;PfjkOaC?rb}D_nBTgg1KMU^HU(Lwy_^g48 zelHo+hj;lT>kfi=K+c7R#zDyUjeqn>1Aot@an|1dGz0~ImVdLzQedM6WutK_1%d)x z{#0)nhS#GC)=KB_^@{OF<50h0cy`5c^9D_urxL9<>nfN zuhf#Bty-htmzpU!_jnZCVlFonl#T+cQ(pJg@1yvpE;J=-}d{*TK0!hmBq0>Y>tj zUGYTX&axE){Q5YH&&M*L>)w&OPw~2BVlI32{pK;qGVZUB(;9=S%Z^h1USps;t`l!q zJO%}ujlwohkAZBF*@JA6aTxZWY&>i_4oXK={w0TxgQNjl{7UOMh<k&Q${ohR_zqL%k>>*0Ufwd|4 zt#ez^^vE>O?)7QBygdz94c_?|Bu#^?U_s^Q{%N>U`}!qm%M66-uxC+E%|Jl(j;0~^ z87SKQC*oWF3>1|8ZLXouK=U@M-Rb*h;b(}cN`U?>yd4^*S_aI5tVR0gqt$qQU9vuJ zmGc{HUNMJs`Z)LZskRmw5zuOm~0zLRv&b(VTo z?M8=2d&*WuHXRmE5!&L)f%*=QJuZLqlMVw!?9D}kY?zi@#W3Z#LU0$VX48DjhZM-`^24X@& zo49w3gSnf{c77NK?V8tr-`^Ss^@WRXUq*~WZSJ}3A64VVJ z<4SEieP9y4*ebtDwVH&4)*E}x113SrxmhVUYZ5a5t{6q(P7&N#^XI@kJ`ZC={ZH{u z;p^2xdoHCZNIL1W;ep{42!vQQJbyR^)XV=?jz>*_SM3y6O6e5zvJ+1g_fG+1f7YM- zOH=Up+$)czI@T|GO{aL8>WOLgYItz1Yn|_RB&O)Gmr*UM%EX)`f)GJVCp^z>9Wa*z- zc$d;JDY0V?+&^#r5-2wZ!|DH431{bkOY-rH{Z?~u@xY+}<;QdILTzR3Nz@!f?KR>^ zE0}`|VxpN%pXWd~z-DQb{(tY2sU!E-|36Dw;Eb#sK=p zGyBkPr@$Ct{2H+z(Rzfr*1R@iVhu^_wr|eg^mB$Pp%BO!C-M*v0#A2 zpa;X?A_D|CZ=b3^#(E+-xJ_)lpYnqallf9RONZ$o zdT?#mFWgU-*PJt~s_C%1ESbD6hYpAQymR_u=-{)}Z;2X2hxoxTKXG?Dh{dF=%fh{+ z{Zh=vJ4SR6>Ym!?j6aW6&YC5csn9`6Iro>gBP5Ilm1`N}np;T_e^j8{i{nXi&)<}aHg)c0nWi*hf^!F0Xp}`tm{SXbm zk253=7qh*g0i!uN{AnN!^p2i7YxbB1Ehj3p-rb`CKlh%gBla|y5I!HSe4Pf31}~q# zH^83IA~BKQ(y6%6>tC6drRBZQSdU} z?pOY26ke46nR@bT6pVUB%qtc~VS2UX?JfNMmbTUT;;Xq)pj@3$lbjuetDlE99-A2j zyBKGihUrn@le1oE#PfcJm7ylS{~6<$q!NCd<*vOOlkwjzPSw5$#@kVT+O0KqQ;-$g+ zck_>u_TX9QdFLMPL^n4-BG!x1AU{RzL?r(F6dXIxKdDTEU%a8S2=M=@)lF{CIU4By zkTO}gOoRH-+!=jy8a$rZYNLdYx1IPa_4lqcC?9K__kTizO{QP}Exn+D*SANs*>^OE z@x3T`CyfTFtwRdV#rQaH>yG|gM+5V7TZvo0(qPwa#)B*>4d$a1e|-5)`=9rTgHGll zk+4DVwUCd>avX%=3Uk3%7Y3o^p6y(d48E;&NB!c~LHOHXVtC*?8MM!(^D_ z*UlRjAj80@_nZ)3|3+|aiLzTI0srak0y%iy`�-{cSpMvo_G@O6`*CwINBMgs}5 z8{f;FEhE9fo9xu1xg^NkrT%zB3JD~GA1fC|li-rSpo!vZ5|}C6*Ygh|!RC=SQEAUe z@Zgt5N2nKG7jwT$)xhiBd)(|(+4u1JQf}10&xr(L_srbN@%7$uhCt7yTO_zvFW&La z4zF_u+_FM!Ng(e^`Yes-_a8|ack!fOHR&|OztpG?&JNeGd^hOPJ*WTdcDiuBp`Prm=E}pK>NepyVlresNuanlk|BmVJV5Is!`~ZC0-57vuzX{%)QEeF zD%)DI+Ll2$5g$~^Avg#f+|!>%h{7R`NS9|D!1T6!`2 zhd}P&wuw)Nhk$Q=9Yyr~5a`C;x0PTEfg; z=rs&M=ghB%XmAKldx}c=FAe?AeR3+aQk3akCF9R6ncgMQS@MMGU2JuK)R^AIB`oU8 z^sa|1YaUGRs#O(w$n>tW>|?*?aqn6_`#gu~UA`xzT$tXa!me_b>0La+;bu(lYOwwH z?-1@?T?0|6Oz$$5xJwVB=(|IY0w)4M2!QlFUKrD$&ck?CD= zPM8eSyPPM#Ycjn{T&jO3)4Nz(dgv0kcbR+2tQg_mby(?YG1I%cL~gn>z3YUHWh2wO zmX>=2ncgKz5*B89msK;jBh$OCx*;U2;wR8BFi`9M-Uj>0O=MH0Jshv{7(^qbO{-j%BK)0OF6 zMl$^zOz$#&BNoH-u71v~&P?yBdpO;_3HPoYLp!;d-ldiu<-_!@71gHwmg!wq z)NwhccNO`rf5h~zpkri7rgw3E&}LF0Z;I9;SD7 z6Mf{E-sOGGUzF)xhZjoPnBH|WSbEVP_pa7UYu-%n`naZbn(19}zyFFez3a3Q2N%=3 z{w)f0GrddiLZ1!OyWVO06fwPP|8MmSrg!ZSr71DJ>npZN_dnhxx5iM#y=(GG^@+J)TE>{kj3mEQQ_xp0BL~-vDJ;Nf`fO}Vk&=<29 z+`FPL+BK!&-nA#m?%6o*UFyDOAFt!yB`EIf#)*5^BHLaLI__Pe79Fn_aPLwnthlO( zdskER)2I~OyZkQ9#h${wOIo${6EE&vXQ~2}xpD6rD;IV@hI`lIPa-)2_pbSW98G(0 z@A@4(64#1**Wy!)uOD#lq8xg1Khh9 zK`%m#aPRW+%j>kjz3Zv>qk<6JyGnk94GiJlh0xjWS8(rgwD3=z#=Xm*qbMl@_b&gC zjOYN|yJU|gLJ;m<&$NWk@7U4hPZc4U0ihYEh4}WPtEym+`FPK@7e^ocR6`l__gES<=yCSuhAS80bQ}Tx*p%rj;NB(gL$5oAdzX`i)|xf$U7dZq$vL=pEq;o>@gQ<>h zQMh-x&NnBj;oi0Wh0(pgxOZ_AWkrd&cPZuGi0Z?=D@DX1V;=V|#V`p~3EaEX6T74z zOm2>;1@^;}N)bl_ic=a+>2#wNo`C2KTNX1=OEsaqns`GJdli_pVBH9gk() zyZ*g@`F<65F0DHLSw1`ysnWNMaPKNt;aL9>cdk%Q@k$Xqql;zzi*VOU-b}H)fu}{y zK^<{C?N&BxZo*Tx>`kAROF6_$-=+OCWS}})r7C3)1N~kX*;Y-WqX_wb-m&p?)TGBP z{n(O@PUt@mwLC~i(q|P+bjE0CCc|4yJ&T4W(ixABIMPsCt1Ut05DgXNFT`puj3PIw z2_dVJQFJBAE%&ASC@SsW`mj-b6de?3O5Vyciq5gt>D;OxL2vFxdGz^@pfPUv#*WOamXy|<;XG}$Rsxk)-9H1g8?Bgj>>M$zK@k}`V zVHk<~KJ$HYYZ$HQaeID~9Y%BWd|Uiz6f~p~do;?Cf<9kJxIq0igjOVYwpl$LLgJhC z(=SWDz z|7LVx4-xHgT3^4@f`~NE=^74A4Ip#%@T((k1L${24kpAtfOtNexW$GM(5gs^+2sQS z^f&K_#;L4+}lvjVKxVMKDgY6woeu{Gta5idfpr?(IQyH$DbXGrpl7mVEI|D&Nqb%d=4q&0o=teQ$-7LH|+1!)_F4BAUIx(T(in?duJqx=b7Y`8CSXW zwgtDKb}5N#F?r31>wLMj^S37Sq4C+C(BF;dlC{Fdp9dOI9d}!O?5PIybC9>)|5iO( zC|Hio53NJB>1!D~N@`KE${Uj+at%7K?zt<-N%3QWTgnnEzw=6RKAl-`}81^FVSlx2lXs-ay?Uwx6dzQqUq@Go?o*H%mf%DH|3Q9ZBeRY*JCo3=x?Luze-o zB_g-B-zn1z1L#%bL_~}G09w8DUf7Ox04?%XoB*p@AUV#L$2)WK+m9EhOKC96UY0q5Mu1D(p}g zz}AWA|H^p6jK3gm#fGeof)2!bZs78f%^j%de$;ZMeLIppsh@kb`7>(4?uIRke?}(K z{Ufo#ZRpx5-Ic?0t>}+1^+KUxE0XIqFyqEr5NB9m>q&_g#YiK%3L`Q)hK?yT~XZcc(t57Yu69{JG^F>5C2! z?>J`jd7$W`M=_jy4k%m)##&a)z-FhPNuohFdL$w-_>Qw1X^&jsU8(Lu-F9|L=zbS! z{#p3BU#1I{JvwaU*4~L;hUhVBPjn(3ql25h%D^%hGI-O z*>VP3QFhVj-A1(c+(^amsRq>X>Fz0b z(SXKJ{#?kFZa`e~jlRb^>Jg9e=h>mV^{7YQ+ck81JtAYvVOnK%DEa{3`d{XCsI(@v z?LB)Ps^E3qEtgY^4C-r5J{Z&@rN^$vmRD<#K=G|e(To~2dYs`Uc%=rNqRra0tRlqA zAe;7FMQGzsOw_0VL%W__bI{&`A(J>ssYr)v)V@$qrP5l3^luh)h)7l;w(o%^7lJF% zX`i(G^yvx|+{|h#eW?P~?##64C@x1|o|w1!?kh)~42|uleag^bkEgR6CQ1=Es9mbp zFGaHoO1GUVOOQ=eM;?n<37XhG)!_2_6N(f05n8@ljP||ilIU_MM(y{u4dxPxP-)0_ z86E8+)MXIcnpjeZw!hYiNEa(acDq8OjG_wA;}hZAaybjoJ?fq7@g5&hp5rqA1!6uL zx%oEJvnCI{Q5fMu>A7euQe1^AEC)RcC~(ks&qj*^eSASyS?DN*b3@w2OtksMU%oiC z4D|hAd$5&wI=W&Ag|j=-(D)|~2_?1<$livxK=MZl+IVRO8mY-BPf+H#KHG$hjP!!NVwzC?uLoP+Yd4_>nEH!?#YVJ)@mIGjwGoAIDo>>wG@|g= zlA7-88&U1omrkOk4M;QW-oS@j4QOrNK=>zL0~#c0iE(w;qh;E$ztrdTsGl=*=E1Rg zBoX}dan1KSq;-AHtSX}pIcEwxT3ghijN$yMqCIuUPSxZ2hwfSg!fjGVUeqG7Fwdjc zPuC)^l@n%sEVXF=8jI8I${MuWTg6)2tp+uF_+HCVtU+CZ`Yf_kgaU#d-eNaHC^Rpo zyT2bpfx*|WB^qEzKG^%>wXfC4l;fqwuS@v6ova}}?5aZQTGLyRuTf29@Zz-@k&deHDlhBpzX9Qh}TslK&MAm80J=8$M%J<;XhM@Voa!8Co9qc`u$33PgwqIZRl7)l#T(ZKGIQkAe0RAwZ8yl-y_8WkZO-oYRZuB(r| zZhpx?KYcp{4Hwf<9Pf$S{e0;tjbr!cQLQvo{aCu@zT*dEvtNwp7nzEDH|3XZtw}-B zC$iRWnM_6jF^{}Gb|fRQ$m0|Gkc3*a5AA*BkcfDnHVj;mNkF`c$KPxE#i74V&&&zr z_vo>f$UYwB7!-Em5_vfE9TG}PMH-WlNNxJp?19s7QFim86t~0(bY+8#@QIbzNOj^( zwX;zevSLk6`Ex4->6P~cm5Kx-%Y_v#&LMvku+MTk;hita_SA^oVek}j+u2<)6!1W@ zL0+%7M!6!^Y}rR+!;XhP++5 z6(;qLs{Xy-3Jc%Cucq?L3ZtgdxutekV@4xC86^tVm_)YfY}Z9=?1M)1p*y#&F|nJ= z*4_cunBrac)_=*?n3BiwWqP$WR$jc}t1Q79tN6O@LicxT3~VvGb}k!i`A%c^=+{CEXQRR>};^Rfgi3te{6$2aN2BR7jA=nd0fpKlV*d-d{7PM zudu;xX_j33)oz2$tTq~nP;9VLo%^Nf^EQ|TPZ;Gnt1Y%A<>Z4J9$U(HI6hioxYQ3=z7i_WEt>( zJ-fTs79(DrFB)vI#ZE`dJE(QpV&7^!!W8;#F~P}&sHQ>uysNAeE2FlU!Kxvte*$m! zR#2tMoGsRHO`|37r!5x7Xpyt|V~ZWEk>_4mwZ*-+1L+cG$yTKlsY`*kM`?OBXrz+hHbz zfgHgDcG%bZwMkq8cGydm8s8>CJ1jZL>itz=J1p$zkYKfl9hTpuqqcI;4m-^?z`9G! z4l`2*ug&6i*ma-lsZ=~K@%G+~lCZ;W_&J2jOWI-m!PnQa@jUOUZ?{#-4ht+Q_b|e< z%l#PX1)jparUF@bl9R?eit%jF6t>RA)Aa1(rMGzYt1kX_#Pj-UNs$VkS3f`M`h~X_ zGv@X>6VFrpv+|emtba5}`i`G>pzBd(5S}6j1*Z2g`*DBdSsJrnj=w`>@jRjFy1fAJ zw~oj&-oto~X!H9Oh}vN%-zJ$I!gFa~b6`GxeXINHf5_vBHRe7k#IJjlFwdicXUxEf zz*>AfcBaYt9{rLJ=bvAXUObwD%^yB(qq;W*8|?5?SX)lU6mCB0(;7_1c8c*I=d4M_ zybU6)v{RC?-)V4}B_J77J+3UOa61`OdlJM!(8Kd~o7{p@GFEo~H0|lWWXw}G+v5pK zGG;XR@bU~J39Dr|5L!Btu+R+N=iHx?u;s!h{}uh zE+Zi3i4w6ihmoCkvJ{`8;fZ12%6?iEmU^>m0qu%XEz?Ap4I_Uk5 z$I3&u5E97o*p*=Ch``!-jGS9(%$XjKZC=TkI}j3&X>R`{)Z!YCU3ao*SZ^ASS?}j7 z>OC2cRRqd+D@(^?idiX~5oAT!pR~*(7Ad_(NZ!Gq$;JIPRbS&2T;Q8(Bp;#>1 z8&eYMj>R0?-)`L95Q{b0gZ9hPSWJyGLhosIEG8cDJ@7|DEOtZa?e&p2v6xMLEa(Nt zVqX>3C8|7)#Xer$_&e@lEVgI1uio1+7W*W`??<$b#ay$jt3R5>V#+D6f3q0IVsC7? z>)LcM-ye&O_eS0e X*%ynQI= 0.3: + print(f"Minimal value of the q\u1D62 is: {min_q:.3f} >= 0.3, so satisfied") +else: + print(f"Minimal value of the q\u1D62 is: {min_q:.3f} < 0.3, so not satisfied") + +# %% +# sphinx_gallery_thumbnail_number = 4 From b9a02fcfca2c3873a2a91034029a133c49075809 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker <83544146+Peter230655@users.noreply.github.com> Date: Thu, 28 Nov 2024 07:25:18 +0100 Subject: [PATCH 3/6] Delete examples-gallery/plot_betts_10_73_74.py --- examples-gallery/plot_betts_10_73_74.py | 280 ------------------------ 1 file changed, 280 deletions(-) delete mode 100644 examples-gallery/plot_betts_10_73_74.py diff --git a/examples-gallery/plot_betts_10_73_74.py b/examples-gallery/plot_betts_10_73_74.py deleted file mode 100644 index 6e7eccc2..00000000 --- a/examples-gallery/plot_betts_10_73_74.py +++ /dev/null @@ -1,280 +0,0 @@ -""" -Linear Tangent Steering -======================= - -These are example 10.73 and 10.74 from Betts' book "Practical Methods for -Optimal Control Using NonlinearProgramming", 3rd edition, -Chapter 10: Test Problems. -They describe the **same** problem, but are **formulated differently**. -More details in section 5.6. of the book. - - - -Note: I simply copied the equations of motion, the bounds and the constants -from the book. I do not know their meaning. - -""" - -import numpy as np -import sympy as sm -import sympy.physics.mechanics as me -from opty.direct_collocation import Problem -import matplotlib.pyplot as plt - -# %% -# Example 10.74 -# ------------- -# -# **States** -# -# - :math:`x_0, x_1, x_2, x_3` : state variables -# -# **Controls** -# -# - :math:`u` : control variable -# -# Equations of motion. -# -t = me.dynamicsymbols._t - -x = me.dynamicsymbols('x:4') -u = me.dynamicsymbols('u') -h = sm.symbols('h') - -#Parameters -a = 100.0 - -eom = sm.Matrix([ - -x[0].diff(t) + x[2], - -x[1].diff(t) + x[3], - -x[2].diff(t) + a * sm.cos(u), - -x[3].diff(t) + a * sm.sin(u), -]) -sm.pprint(eom) - -# %% -# Define and Solve the Optimization Problem -# ----------------------------------------- -num_nodes = 301 - -t0, tf = 0*h, (num_nodes - 1) * h -interval_value = h - -state_symbols = x -unkonwn_input_trajectories = (u, ) - -# Specify the objective function and form the gradient. -def obj(free): - return free[-1] - -def obj_grad(free): - grad = np.zeros_like(free) - grad[-1] = 1.0 - return grad - -instance_constraints = ( - x[0].func(t0), - x[1].func(t0), - x[2].func(t0), - x[3].func(t0), - x[1].func(tf) - 5.0, - x[2].func(tf) - 45.0, - x[3].func(tf), -) - -bounds = { - h: (0.0, 0.5), - u: (-np.pi/2, np.pi/2), -} - -# %% -# Create the optimization problem and set any options. -prob = Problem(obj, - obj_grad, - eom, - state_symbols, - num_nodes, - interval_value, - instance_constraints= instance_constraints, - bounds=bounds, - ) - -prob.add_option('max_iter', 1000) - -# Give some rough estimates for the trajectories. -initial_guess = np.ones(prob.num_free) * 0.1 - -# Find the optimal solution. -for i in range(1): - solution, info = prob.solve(initial_guess) - initial_guess = solution - print(info['status_msg']) - print(f'Objective value achieved: {info['obj_val']*(num_nodes-1):.4f}, ' + - f'as per the book it is {0.554570879}, so the error is: ' + - f'{(info['obj_val']*(num_nodes-1) - 0.554570879)/0.554570879 :.3e} ') - -solution1 = solution - -# %% -# Plot the optimal state and input trajectories. -prob.plot_trajectories(solution) - -# %% -# Plot the constraint violations. -prob.plot_constraint_violations(solution) - -# %% -# Plot the objective function as a function of optimizer iteration. -prob.plot_objective_value() - -# %% -# Example 10.73 -# ------------- -# -# There is a boundary condition at the end of the interval, at :math:`t = t_f`: -# :math:`1 + \lambda_0 x_2 + \lambda_1 x_3 + \lambda_2 a \hspace{2pt} \text{cosu} + \lambda_3 a \hspace{2pt} \text{sinu} = 0` -# -# where :math:`sinu = - \lambda_3 / \sqrt{\lambda_2^2 + \lambda_3^2}` and -# :math:`cosu = - \lambda_2 / \sqrt{\lambda_2^2 + \lambda_3^2}` and a is a constant -# -# As *opty* presently does not support such instance constraints, I introduce -# a new state variable and an additional equation of motion: -# -# :math:`hilfs = 1 + \lambda_0 x_2 + \lambda_1 x_3 + \lambda_2 a \hspace{2pt} \text{cosu} + \lambda_3 a \hspace{2pt} \text{sinu}` -# -# and I use the instance constraint :math:`hilfs(t_f) = 0`. -# -# -# **states** -# -# - :math:`x_0, x_1, x_2, x_3` : state variables -# - :math:`lam_0, lam_1, lam_2, lamb_3` : state variables -# - :math:`hilfs` : state variable -# -# -# Equations of Motion -# ------------------- -# -t = me.dynamicsymbols._t - -x = me.dynamicsymbols('x:4') -lam = me.dynamicsymbols('lam:4') -hilfs = me.dynamicsymbols('hilfs') -h = sm.symbols('h') - -# Parameters -a = 100.0 - -cosu = - lam[2] / sm.sqrt(lam[2]**2 + lam[3]**2) -sinu = - lam[3] / sm.sqrt(lam[2]**2 + lam[3]**2) - -eom = sm.Matrix([ - -x[0].diff(t) + x[2], - -x[1].diff(t) + x[3], - -x[2].diff(t) + a * cosu, - -x[3].diff(t) + a * sinu, - -lam[0].diff(t), - -lam[1].diff(t), - -lam[2].diff(t) - lam[0], - -lam[3].diff(t) - lam[1], - -hilfs + 1 + lam[0]*x[2] + lam[1]*x[3] + lam[2]*a*cosu + lam[3]*a*sinu, -]) -sm.pprint(eom) - -# %% -# Define and Solve the Optimization Problem -# ----------------------------------------- - -state_symbols = x + lam + [hilfs] - -t0, tf = 0*h, (num_nodes - 1) * h -interval_value = h - -# %% -# Specify the objective function and form the gradient. -def obj(free): - return free[-1] - -def obj_grad(free): - grad = np.zeros_like(free) - grad[-1] = 1.0 - return grad - -instance_constraints = ( - x[0].func(t0), - x[1].func(t0), - x[2].func(t0), - x[3].func(t0), - x[1].func(tf) - 5.0, - x[2].func(tf) - 45.0, - x[3].func(tf), - lam[0].func(t0), - hilfs.func(tf), -) - -bounds = { - h: (0.0, 0.5), -} -# %% -# Create the optimization problem and set any options. -prob = Problem(obj, - obj_grad, - eom, - state_symbols, - num_nodes, - interval_value, - instance_constraints= instance_constraints, - bounds=bounds, - ) - -prob.add_option('max_iter', 1000) - -# %% -# Give some estimates for the trajectories. This example only converged with -# very close initial guesses. -i1 = list(solution1[: -1]) -i2 = [0.0 for _ in range(4*num_nodes)] -i3 = [0.0] -i4 = solution1[-1] -initial_guess = np.array(i1 + i2 + i3 + i4) - -# %% -# Find the optimal solution. -for i in range(1): - solution, info = prob.solve(initial_guess) - initial_guess = solution - print(info['status_msg']) - print(f'Objective value achieved: {info['obj_val']*(num_nodes-1):.4f}, ' + - f'as per the book it is {0.55457088}, so the error is: ' + - f'{(info['obj_val']*(num_nodes-1) - 0.55457088)/0.55457088:.3e}') - -# %% -# Plot the optimal state and input trajectories. -prob.plot_trajectories(solution) - -# %% -# Plot the constraint violations. -prob.plot_constraint_violations(solution) - -# %% -# Plot the objective function as a function of optimizer iteration. -prob.plot_objective_value() - -# %% -# Compare the two solutions. -difference = np.empty(4*num_nodes) -for i in range(4*num_nodes): - difference[i] = solution1[i] - solution[i] - -fig, ax = plt.subplots(4, 1, figsize=(8, 6), constrained_layout=True) -names = ['x0', 'x1', 'x2', 'x3'] -times = np.linspace(t0, (num_nodes-1)*solution[-1], num_nodes) -for i in range(4): - ax[i].plot(times, difference[i*num_nodes:(i+1)*num_nodes]) - ax[i].set_ylabel(f'difference in {names[i]}') -ax[0].set_title('Difference in the state variables between the two solutions') -ax[-1].set_xlabel('time'); -prevent_print = 1 - -# %% -# sphinx_gallery_thumbnail_number = 5 From 8a7620dc68ae2e7dbdb5546ad1d2baae3381762f Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Thu, 28 Nov 2024 11:59:13 +0100 Subject: [PATCH 4/6] error with bounds corrected --- examples-gallery/plot_betts_10_50.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples-gallery/plot_betts_10_50.py b/examples-gallery/plot_betts_10_50.py index d06fac08..8997e7d0 100644 --- a/examples-gallery/plot_betts_10_50.py +++ b/examples-gallery/plot_betts_10_50.py @@ -1,3 +1,4 @@ +# %% """ Delay Equation (Göllmann, Kern, and Maurer) =========================================== @@ -141,6 +142,7 @@ num_nodes, interval_value, instance_constraints= instance_constraints, + bounds=bounds, ) prob.add_option('max_iter', 1000) From 7dd59e21353c565c7b4f20a3dc6f03df456abe97 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Sun, 1 Dec 2024 13:05:21 +0100 Subject: [PATCH 5/6] print each eom violation separately --- opty/direct_collocation.py | 55 ++++++++++++++++++++++++++++++-------- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py index 67175244..6b04417f 100644 --- a/opty/direct_collocation.py +++ b/opty/direct_collocation.py @@ -436,7 +436,7 @@ def plot_trajectories(self, vector, axes=None): return axes @_optional_plt_dep - def plot_constraint_violations(self, vector, axes=None): + def plot_constraint_violations(self, vector, axes=None, detailed_eoms=False): """Returns an axis with the state constraint violations plotted versus node number and the instance constraints as a bar graph. @@ -446,6 +446,12 @@ def plot_constraint_violations(self, vector, axes=None): The initial guess, solution, or any other vector that is in the canonical form. + detailed_eoms : boolean, optional. If True, the equations of motion + will be plotted in a separate plot for each state. Default is False. + + axes : ndarray of AxesSubplot, optional. If given, it is the user's + responsibility to provide the correct number of axes. + Returns ======= axes : ndarray of AxesSubplot @@ -523,16 +529,43 @@ def plot_constraint_violations(self, vector, axes=None): con_nodes = range(1, self.collocator.num_collocation_nodes) if axes is None: - fig, axes = plt.subplots(1 + num_plots, 1, - figsize=(6.4, 1.50*(1 + num_plots)), - layout='compressed') + if detailed_eoms == False or self.collocator.num_states == 1: + num_eom_plots = 1 + else: + num_eom_plots = self.collocator.num_states + fig, axes = plt.subplots(num_eom_plots + num_plots, 1, + figsize=(6.4, 1.75*(num_eom_plots + num_plots)), + constrained_layout=True) + + else: + num_eom_plots = len(axes) - num_plots axes = np.asarray(axes).ravel() - axes[0].plot(con_nodes, state_violations.T) - axes[0].set_title('Constraint violations') - axes[0].set_xlabel('Node Number') - axes[0].set_ylabel('EoM violation') + if detailed_eoms == False or self.collocator.num_states == 1: + axes[0].plot(con_nodes, state_violations.T) + axes[0].set_title('Constraint violations') + axes[0].set_xlabel('Node Number') + axes[0].set_ylabel('EoM violation') + + else: + for i in range(self.collocator.num_states): + k = i + 1 + if k in (11,12,13): + msg = 'th' + elif k % 10 == 1: + msg = 'st' + elif k % 10 == 2: + msg = 'nd' + elif k % 10 == 3: + msg = 'rd' + else: + msg = 'th' + + axes[i].plot(con_nodes, state_violations[i]) + axes[i].set_ylabel(f'{str(k)}-{msg} EOM violation') + axes[num_eom_plots-1].set_xlabel('Node Number') + axes[0].set_title('Constraint violations') if self.collocator.instance_constraints is not None: # reduce the instance constrtaints to 2 digits after the decimal @@ -575,11 +608,11 @@ def plot_constraint_violations(self, vector, axes=None): inst_constr = instance_constr_plot[beginn: endd] width = [0.06*num_ticks for _ in range(num_ticks)] - axes[i+1].bar(range(num_ticks), inst_viol, + axes[i+num_eom_plots].bar(range(num_ticks), inst_viol, tick_label=[sm.latex(s, mode='inline') for s in inst_constr], width=width) - axes[i+1].set_ylabel('Instance') - axes[i+1].set_xticklabels(axes[i+1].get_xticklabels(), + axes[i+num_eom_plots].set_ylabel('Instance') + axes[i+num_eom_plots].set_xticklabels(axes[i+num_eom_plots].get_xticklabels(), rotation=rotation) return axes From 0ee923e4e170cbeef2141ecc81ad2d6921d57809 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker <83544146+Peter230655@users.noreply.github.com> Date: Sun, 1 Dec 2024 13:12:52 +0100 Subject: [PATCH 6/6] Delete examples-gallery/plot_betts_10_50.py --- examples-gallery/plot_betts_10_50.py | 205 --------------------------- 1 file changed, 205 deletions(-) delete mode 100644 examples-gallery/plot_betts_10_50.py diff --git a/examples-gallery/plot_betts_10_50.py b/examples-gallery/plot_betts_10_50.py deleted file mode 100644 index 8997e7d0..00000000 --- a/examples-gallery/plot_betts_10_50.py +++ /dev/null @@ -1,205 +0,0 @@ -# %% -""" -Delay Equation (Göllmann, Kern, and Maurer) -=========================================== - -This is example 10.50 from Betts' book "Practical Methods for Optimal Control -Using NonlinearProgramming", 3rd edition, Chapter 10: Test Problems. - -Let :math:`t_0, t_f` be the starting time and final time. There are instance -constraints: :math:`x_i(t_f) = x_j(t_0), i \\neq j`. -As presently *opty* does not support such instance constraints, I iterate a -few times, hoping it will converge - which it does in this example. - -There are also inequalities: :math:`x_i(t) + u_i(t) \\geq 0.3`. -To handle them, I define additional state variables :math:`q_i(t)` and use -additional EOMs: - -:math:`q_i(t) = x_i(t) + u_i(t)`, and then use bounds on :math:`q_i(t)`. - -**States** - -- :math:`x_1, x_2, x_3. x_4. x_5, x_6` : state variables -- :math:`q_1, q_2, q_3. q_4. q_5, q_6` : state variables for the inequalities - -**Controls** - -- :math:`u_1, u_2, u_3, u_4, u_5, u_6` : control variables - -Note: I simply copied the equations of motion, the bounds and the constants -from the book. I do not know their meaning. - -""" - -import numpy as np -import sympy as sm -import sympy.physics.mechanics as me -from opty.direct_collocation import Problem -from opty.utils import create_objective_function -import matplotlib.pyplot as plt - -# %% -# Equations of Motion -# ------------------- -t = me.dynamicsymbols._t - -x1, x2, x3, x4, x5, x6 = me.dynamicsymbols('x1, x2, x3, x4, x5, x6') -q1, q2, q3, q4, q5, q6 = me.dynamicsymbols('q1, q2, q3, q4, q5, q6') -u1, u2, u3, u4, u5, u6 = me.dynamicsymbols('u1, u2, u3, u4, u5, u6') - -#Parameters -x0 = 1.0 -u_minus_1, u0 = 0.0, 0.0 - -eom = sm.Matrix([ - -x1.diff(t) + x0*u_minus_1, - -x2.diff(t) + x1*u0, - -x3.diff(t) + x2*u1, - -x4.diff(t) + x3*u2, - -x5.diff(t) + x4*u3, - -x6.diff(t) + x5*u4, - -q1 + u1 + x1, - -q2 + u2 + x2, - -q3 + u3 + x3, - -q4 + u4 + x4, - -q5 + u5 + x5, - -q6 + u6 + x6, -]) -sm.pprint(eom) - -# %% -# Define and Solve the Optimization Problem -# ----------------------------------------- -num_nodes = 101 -t0, tf = 0.0, 1.0 -interval_value = (tf - t0) / (num_nodes - 1) - -state_symbols = (x1, x2, x3, x4, x5, x6, q1, q2, q3, q4, q5, q6) -unkonwn_input_trajectories = (u1, u2, u3, u4, u5, u6) - -# %% -# Specify the objective function and form the gradient. -objective = sm.Integral(x1**2 + x2**2 + x3**2 + x4**2 + x5**2 + x6**2 + u1**2 - + u2**2 + u3**2 + u4**2 + u5**2 + u6**2, t) - -obj, obj_grad = create_objective_function( - objective, - state_symbols, - unkonwn_input_trajectories, - tuple(), - num_nodes, - interval_value -) - -# %% -# Specify the instance constraints and bounds. I use the solution from a -# previous run as the initial guess to save running time in this example. -# It took 8 iterations to get it. -initial_guess = np.random.rand(18*num_nodes) * 0.1 -np.random.seed(123) -initial_guess = np.load('betts_10_50_solution.npy')*(1.0 + - 0.001*np.random.rand(1)) - -instance_constraints = ( - x1.func(t0) - 1.0, - - x2.func(t0) - initial_guess[num_nodes-1], - x3.func(t0) - initial_guess[2*num_nodes-1], - x4.func(t0) - initial_guess[3*num_nodes-1], - x5.func(t0) - initial_guess[4*num_nodes-1], - x6.func(t0) - initial_guess[5*num_nodes-1], - - q1.func(t0) - 0.5, - q2.func(t0) - 0.5, - q3.func(t0) - 0.5, - q4.func(t0) - 0.5, - q5.func(t0) - 0.5, - q6.func(t0) - 0.5, - ) - -limit_value = np.inf -bounds = { - q1: (0.3, limit_value), - q2: (0.3, limit_value), - q3: (0.3, limit_value), - q4: (0.3, limit_value), - q5: (0.3, limit_value), - q6: (0.3, limit_value), -} - -# %% -# Iterate -# ------- -# Here I iterate *loop* times, and use the solution to set the instance -# constraints for the next iteration. -loop = 2 -for i in range(loop): - - prob = Problem(obj, - obj_grad, - eom, - state_symbols, - num_nodes, - interval_value, - instance_constraints= instance_constraints, - bounds=bounds, - ) - - prob.add_option('max_iter', 1000) - - -# Find the optimal solution. - solution, info = prob.solve(initial_guess) - initial_guess = solution - print(info['status_msg']) - print(f'Objective value achieved: {info['obj_val']:.4f}, as per the book '+ - f'it is {3.10812211}, so the error is: ' - f'{(info['obj_val'] - 3.10812211)/3.10812211*100:.3f} % ') - print('\n') - - instance_constraints = ( - x1.func(t0) - 1.0, - - x2.func(t0) - solution[num_nodes-1], - x3.func(t0) - solution[2*num_nodes-1], - x4.func(t0) - solution[3*num_nodes-1], - x5.func(t0) - solution[4*num_nodes-1], - x6.func(t0) - solution[5*num_nodes-1], - ) - -# %% -# Plot the optimal state and input trajectories. -prob.plot_trajectories(solution) - -# %% -# Plot the constraint violations. -prob.plot_constraint_violations(solution) - -# %% -# Plot the objective function. -prob.plot_objective_value() - -# %% -# Are the instance constraints satisfied? -delta = [solution[0] - 1.0] -for i in range(1, 6): - delta.append(solution[i*num_nodes] - solution[i*num_nodes-1]) - -labels = [r'$x_1(t_0) - 1$', r'$x_1(t_f) - x_2(t_0)$', r'$x_2(t_f) - x_3(t_0)$', - r'$x_3(t_f) - x_4(t_0)$', r'$x_4(t_f) - x_5(t_0)$', - r'$x_5(t_f) - x_6(t_0)$'] -fig, ax = plt.subplots(figsize=(8, 2), constrained_layout=True) -ax.bar(labels, delta) -ax.set_title('Instance constraint violations') -prevent_print = 1 - -# %% -# Are the inequality constraints satisfied? -min_q = np.min(solution[7*num_nodes: 12*num_nodes-1]) -if min_q >= 0.3: - print(f"Minimal value of the q\u1D62 is: {min_q:.3f} >= 0.3, so satisfied") -else: - print(f"Minimal value of the q\u1D62 is: {min_q:.3f} < 0.3, so not satisfied") - -# %% -# sphinx_gallery_thumbnail_number = 4