{"id":4641,"date":"2021-08-18T08:17:07","date_gmt":"2021-08-18T12:17:07","guid":{"rendered":"https:\/\/blogs.mathworks.com\/loren\/?p=4641"},"modified":"2021-08-18T17:43:16","modified_gmt":"2021-08-18T21:43:16","slug":"cone-programming-and-optimal-discrete-dynamics","status":"publish","type":"post","link":"https:\/\/blogs.mathworks.com\/loren\/2021\/08\/18\/cone-programming-and-optimal-discrete-dynamics\/","title":{"rendered":"Cone Programming and Optimal Discrete Dynamics"},"content":{"rendered":"
Today's guest blogger is Alan Weiss, who writes documentation for Optimization Toolbox™ and other mathematical toolboxes.<\/span><\/div>
Table of Contents<\/span><\/div>
Cone Programming\r\n<\/span><\/a>Discrete Dynamics With Cone Constraints\r\n<\/span><\/a>Find Optimal Time\r\n<\/span><\/a>Final Thoughts\r\n<\/span><\/a>Helper Functions<\/span><\/a><\/div><\/div>

Cone Programming<\/span><\/h2>
Hi, folks. The subject for today is cone programming, and an application of cone programming to controlling a rocket optimally. Since R2020b the <\/span>coneprog<\/span><\/a> solver has been available to solve cone programming problems. What is cone programming? I think of it as a generalization of quadratic programming. All quadratic programming problems can be represented as cone programming problems. But there are cone programming problems that cannot be represented as quadratic programs.<\/span><\/div>
So again, what is cone programming? It is a problem with a linear objective function and linear constraints, like a linear program or quadratic program. But it also incorporates cone constraints. In three dimensions [x, y, z], you can represent a cone as, for example, the radius of a circle in the x-y direction is less than or equal to z. In other words, the cone constraint is the inequality constraint<\/span><\/div>
$ x^2+y^2\\le z^2 $<\/span>,<\/span><\/div>
or equivalently<\/span><\/div>
$ \\|[x,y]\\|\\le z $<\/span> for nonnegative <\/span>z<\/span>.<\/span><\/div>
Here is a picture of the boundary of the cone <\/span>$ \\|[x,y]\\|\\le z $<\/span> for nonnegative <\/span>z<\/span>.<\/span><\/div>
[X,Y] = meshgrid(-2:0.1:2);<\/span><\/span><\/div><\/div>
Z = sqrt(X.^2 + Y.^2);<\/span><\/span><\/div><\/div>
surf(X,Y,Z)<\/span><\/span><\/div><\/div>
view(8,2)<\/span><\/span><\/div><\/div>
xlabel(<\/span>\"x\"<\/span>)<\/span><\/span><\/div><\/div>
ylabel(<\/span>\"y\"<\/span>)<\/span><\/span><\/div><\/div>
zlabel(<\/span>\"z\"<\/span>)<\/span><\/span><\/div>
<\/div><\/div><\/div><\/div><\/div>
Of course, you can scale, translate, and rotate a cone constraint. The formal definition of a general cone constraint uses a matrix <\/span>Asc<\/span>, vectors <\/span>bsc<\/span> and <\/span>d<\/span>, and scalar <\/span>gamma<\/span> with the constraint in <\/span>x<\/span> represented as<\/span><\/div>
norm(Asc*x - bsc) <= d'*x - gamma;<\/span><\/span><\/div><\/div>
The <\/span>coneprog<\/span> solver in Optimization Toolbox requires you to use the <\/span>secondordercone<\/span> function to formulate cone constraints. For example,<\/span><\/div>
Asc = diag([1,1\/2,0]);<\/span><\/span><\/div><\/div>
bsc = zeros(3,1);<\/span><\/span><\/div><\/div>
d = [0;0;1];<\/span><\/span><\/div><\/div>
gamma = 0;<\/span><\/span><\/div><\/div>
socConstraints = secondordercone(Asc,bsc,d,gamma);<\/span><\/span><\/div><\/div>
f = [-1,-2,0];<\/span><\/span><\/div><\/div>
Aineq = [];<\/span><\/span><\/div><\/div>
bineq = [];<\/span><\/span><\/div><\/div>
Aeq = [];<\/span><\/span><\/div><\/div>
beq = [];<\/span><\/span><\/div><\/div>
lb = [-Inf,-Inf,0];<\/span><\/span><\/div><\/div>
ub = [Inf,Inf,2];<\/span><\/span><\/div><\/div>
[x,fval] = coneprog(f,socConstraints,Aineq,bineq,Aeq,beq,lb,ub)<\/span><\/span><\/div>
Optimal solution found.<\/div><\/div>
x = <\/span>3\u00d71<\/span><\/div><\/div>
0.4851\r\n 3.8806\r\n 2.0000\r\n<\/div>
<\/div>
<\/div><\/div><\/div><\/div>
fval = -8.2462<\/div><\/div><\/div><\/div>
It can be simpler to use the problem-based approach to access cone programming. This functionality was added in R2021a. For the previous example using the problem-based approach:<\/span><\/div>
x = optimvar(<\/span>'x'<\/span>,3,<\/span>\"LowerBound\"<\/span>,[-Inf,-Inf,0],<\/span>\"UpperBound\"<\/span>,[Inf,Inf,2]);<\/span><\/span><\/div><\/div>
Asc = diag([1,1\/2,0]);<\/span><\/span><\/div><\/div>
prob = optimproblem(<\/span>\"Objective\"<\/span>,-x(1)-2*x(2));<\/span><\/span><\/div><\/div>
prob.Constraints = norm(Asc*x) <= x(3);<\/span><\/span><\/div><\/div>
[sol,fval] = solve(prob)<\/span><\/span><\/div>
Solving problem using coneprog.\r\nOptimal solution found.<\/div><\/div>
sol = struct with fields:<\/span><\/span><\/div>
x: [3\u00d71 double]\r\n<\/div><\/div><\/div>
fval = -8.2462<\/div><\/div><\/div><\/div>
Notice that, unlike most nonlinear solvers, you do not need to specify an initial point for <\/span>coneprog<\/span>. This comes in handy in the following example.<\/span><\/div>

Discrete Dynamics With Cone Constraints<\/span><\/h2>
Suppose that you want to control a rocket to land gently at a particular location using minimal fuel. Suppose that the fuel used is proportional to the applied acceleration times time. Do not model the changing weight of the rocket as you burn fuel; we are supposing that this control is for a relatively short time, where the weight does not change appreciably. There is gravitational acceleration g = 9.81 in the negative z direction. There is also linear drag on the rocket that acts in the negative direction of velocity with coefficient 1\/10. This means after time <\/span>t<\/span>, without any applied acceleration or gravity, the velocity changes from <\/span>v<\/span> to <\/span>$ v\\exp(-t\/10) $<\/span>.<\/span><\/div>
In continuous time the equations of motion for position <\/span>$ p(t) $<\/span>, velocity <\/span>$ v(t) $<\/span>, and applied acceleration <\/span>$ a(t) $<\/span> are<\/span><\/div>
$ \\frac{dp}{dt} = v(t) $<\/span><\/div>
$ \\frac{dv}{dt} = -v(t)\/10 + a(t) + g*[0,0,-1] $<\/span>.<\/span><\/div>
Here are some approximate equations of motion, using discrete time with <\/span>N<\/span> equal steps of length <\/span>$ t = T\/N $<\/span>:<\/span><\/div>
$ p(i+i) = p(i) + t*(v(i) + v(i+1))\/2 $<\/span> (trapezoidal rule)<\/span><\/div>
$ v(i+1) = v(i)*\\exp(-t\/10) + t*(a(i) + g*[0, 0, -1]) $<\/span> (Euler integration).<\/span><\/div>
Therefore,<\/span><\/div>
$ p(i+1) = p(i) + t*v(i)*(1 + \\exp(-t\/10))\/2 + t^2*(a(i) + g*[0, 0, -1])\/2 $<\/span>.<\/span><\/div>
Now for the part that leads to cone programming. Suppose that the applied acceleration at each step is bounded by a constant <\/span>Amax<\/span>. These constraints are<\/span><\/div>
$ \\|a(i)\\| \\le {\\rm Amax} $<\/span> for all <\/span>i<\/span>.<\/span><\/div>
The cost to minimize should be the sum of the norms of the accelerations times <\/span>t<\/span>. Cone programming requires the objective function to be linear in optimization parameters. You can reformulate this cost to be linear by introducing new optimization variables <\/span>s(i)<\/span> that are subject to a new set of cone constraints:<\/span><\/div>
$ {\\rm cost} = \\sum s(i)*t $<\/span><\/div>
$ \\|s(i)\\| \\le a(i) $<\/span>.<\/span><\/div>
Suppose that the rocket is traveling initially at velocity <\/span>$ v0 = [100,50,-40] $<\/span> at position <\/span>$ p0 = [-1000,-800,1200] $<\/span>. Calculate the acceleration required to bring the rocket to position <\/span>$ [0,0,0] $<\/span> with velocity <\/span>$ [0,0,0] $<\/span> at time <\/span>$ T = 40 $<\/span>. Break up the calculation into 100 steps (<\/span>$ t=40\/100 $<\/span>). Suppose that the maximum acceleration <\/span>$ \\rm{Amax} = 2g $<\/span>.<\/span><\/div>
The <\/span>makeprob<\/span> function at the <\/span>end of this script<\/span><\/a> accepts the time <\/span>T<\/span>, initial position <\/span>p0<\/span>, and initial velocity <\/span>v0<\/span>, and returns a problem that describes the discrete dynamics and cost.<\/span><\/div>
p0 = [-1000,-800,1200];<\/span><\/span><\/div><\/div>
v0 = [100,50,-40];<\/span><\/span><\/div><\/div>
prob = makeprob(40,p0,v0)<\/span><\/span><\/div>
prob = <\/span><\/div>
OptimizationProblem with properties:\r\n\r\n Description: ''\r\n ObjectiveSense: 'minimize'\r\n Variables: [1\u00d71 struct] containing 4 OptimizationVariables\r\n Objective: [1\u00d71 OptimizationExpression]\r\n Constraints: [1\u00d71 struct] containing 4 OptimizationConstraints\r\n\r\n See problem formulation with show.\r\n<\/div><\/div><\/div><\/div><\/div><\/div>
Set options to solve the cone programming problem using an optimality tolarance 100 times smaller than the default. Use the <\/span>\"schur\"<\/span> linear solver, which can be more accurate for this problem.<\/span><\/div>
opts = optimoptions(<\/span>\"coneprog\"<\/span>,<\/span>\"OptimalityTolerance\"<\/span>,1e-8,<\/span>\"LinearSolver\"<\/span>,<\/span>\"schur\"<\/span>);<\/span><\/span><\/div><\/div>
[sol,cost] = solve(prob,Options=opts)<\/span><\/span><\/div>
Solving problem using coneprog.\r\nOptimal solution found.<\/div><\/div>
sol = struct with fields:<\/span><\/span><\/div>
a: [99\u00d73 double]\r\n p: [100\u00d73 double]\r\n s: [99\u00d71 double]\r\n v: [100\u00d73 double]\r\n<\/div><\/div><\/div>
cost = 312.7740<\/div><\/div><\/div><\/div>
The <\/span>plottrajandaccel<\/span> function <\/span>at the end of this script<\/span><\/a> plots both the trajectory and the norm of the acceleration as a function of time step.<\/span><\/div>
plottrajandaccel(sol)<\/span><\/span><\/div>
<\/div><\/div>
<\/div><\/div><\/div><\/div><\/div>
The optimal acceleration is nearly \"bang-bang.\" The rocket accelerates at about <\/span>$ 2g $<\/span> at first, then has close to zero acceleration until the near end. Near the end, the rocket accelerates at maximum to slow the descent and land with zero velocity. The total cost of this control is about 313.<\/span><\/div>

Find Optimal Time<\/span><\/h2>
Find the optimal time T for the rocket to land, meaning the time that causes the rocket to use the least possible fuel. The <\/span>findT<\/span> function <\/span>at the end of this script<\/span><\/a> calls <\/span>fminbnd<\/span> to locate the minimal-cost time. I experimented briefly to find that [20,60] is a reasonable range for times <\/span>T<\/span> for the minimum, and I used those bounds in the <\/span>fminbnd<\/span> call. If you take a time much less than 20 you get an infeasible problem:<\/span><\/div>
badprob = makeprob(15,p0,v0);<\/span><\/span><\/div><\/div>
badsol = solve(badprob,Options=opts)<\/span><\/span><\/div>
Solving problem using coneprog.\r\nProblem is infeasible.<\/div><\/div>
badsol = struct with fields:<\/span><\/span><\/div>
a: []\r\n p: []\r\n s: []\r\n v: []\r\n<\/div><\/div><\/div><\/div><\/div><\/div>
(As an aside, if you try to make <\/span>T<\/span> an optimization variable then the problem is no longer a <\/span>coneprog<\/span> problem. Instead, it is a problem for <\/span>fmincon<\/span>, which takes much longer to solve in this case, and requires you to provide an initial point.)<\/span><\/div>
Topt = findT(opts)<\/span><\/span><\/div>
Solving...\r\nDone<\/div><\/div>
Topt = 22.3294<\/div><\/div><\/div><\/div>
Plot the optimal trajectory and acceleration.<\/span><\/div>
probopt = makeprob(Topt,p0,v0);<\/span><\/span><\/div><\/div>
[solopt,costopt] = solve(probopt,Options=opts)<\/span><\/span><\/div>
Solving problem using coneprog.\r\nOptimal solution found.<\/div><\/div>
solopt = struct with fields:<\/span><\/span><\/div>
a: [99\u00d73 double]\r\n p: [100\u00d73 double]\r\n s: [99\u00d71 double]\r\n v: [100\u00d73 double]\r\n<\/div><\/div><\/div>
costopt = 171.1601<\/div><\/div><\/div>
plottrajandaccel(solopt)<\/span><\/span><\/div>
<\/div><\/div>
<\/div><\/div><\/div><\/div><\/div>
The optimal cost is about 171, which is roughly half of the cost for the original parameters. This time, the control is more nearly bang-bang. The rocket accelerates at maximum at first, then stops accelerating for some time. Again, during the final times the rocket accelerates at maximum to land with zero velocity.<\/span><\/div>

Final Thoughts<\/span><\/h2>
Cone programming is a surprisingly versatile framework for solving many convex optimization problems. For another nontrivial example, see <\/span>Minimize Energy of Piecewise Linear Mass-Spring System Using Cone Programming, Problem-Based<\/span><\/a>. For other problems that can be put in the cone programming framework, see Lobo, Miguel Sousa, Lieven Vandenberghe, Stephen Boyd, and Herv\u00e9 Lebret. \u201cApplications of Second-Order Cone Programming.\u201d<\/span> <\/span>Linear Algebra and Its Applications<\/span> <\/span>284, no. 1\u20133 (November 1998): 193\u2013228.<\/span> <\/span>https:\/\/doi.org\/10.1016\/S0024-3795(98)10032-0<\/span><\/a><\/div>
Do you find cone programming or discrete dynamics useful? Do you have any examples of your own to share? Let us know <\/span>here<\/span><\/a>.<\/span><\/div>

Helper Functions<\/span><\/h2>
This code creates the <\/span>makeprob<\/span> function.<\/span><\/div>
function <\/span>trajectoryproblem = makeprob(T,p0,v0)<\/span><\/span><\/div><\/div>
N = 100;<\/span><\/span><\/div><\/div>
g = 9.81;<\/span><\/span><\/div><\/div>
pF = [0 0 0];<\/span><\/span><\/div><\/div>
Amax = 2*g;<\/span><\/span><\/div><\/div>
p = optimvar(<\/span>\"p\"<\/span>,N,3);<\/span><\/span><\/div><\/div>
v = optimvar(<\/span>\"v\"<\/span>,N,3);<\/span><\/span><\/div><\/div>
a = optimvar(<\/span>\"a\"<\/span>,N-1,3);<\/span><\/span><\/div><\/div>
s = optimvar(<\/span>\"s\"<\/span>,N-1,<\/span>\"LowerBound\"<\/span>,0,<\/span>\"UpperBound\"<\/span>,Amax);<\/span><\/span><\/div><\/div>
trajectoryproblem = optimproblem;<\/span><\/span><\/div><\/div>
t = T\/N;<\/span><\/span><\/div><\/div>
trajectoryproblem.Objective = sum(s)*t;<\/span><\/span><\/div><\/div>
scons = optimconstr(N-1);<\/span><\/span><\/div><\/div>
for <\/span>i = 1:(N-1)<\/span><\/span><\/div><\/div>
scons(i) = norm(a(i,:)) <= s(i);<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div>
acons = optimconstr(N-1);<\/span><\/span><\/div><\/div>
for <\/span>i = 1:(N-1)<\/span><\/span><\/div><\/div>
acons(i) = norm(a(i,:)) <= Amax;<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div>
vcons = optimconstr(N+1,3);<\/span><\/span><\/div><\/div>
vcons(1,:) = v(1,:) == v0;<\/span><\/span><\/div><\/div>
vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t\/10) + t*(a + repmat([0 0 -g],N-1,1));<\/span><\/span><\/div><\/div>
vcons(N+1,:) = v(N,:) == [0 0 0];<\/span><\/span><\/div><\/div>
pcons = optimconstr(N+1,3);<\/span><\/span><\/div><\/div>
pcons(1,:) = p(1,:) == p0;<\/span><\/span><\/div><\/div>
pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + (1+exp(-t\/10))\/2*t*v(1:(N-1),:) + t^2\/2*(a + repmat([0 0 -g],N-1,1));<\/span><\/span><\/div><\/div>
pcons((N+1),:) = p(N,:) == pF;<\/span><\/span><\/div><\/div>
trajectoryproblem.Constraints.acons = acons;<\/span><\/span><\/div><\/div>
trajectoryproblem.Constraints.scons = scons;<\/span><\/span><\/div><\/div>
trajectoryproblem.Constraints.vcons = vcons;<\/span><\/span><\/div><\/div>
trajectoryproblem.Constraints.pcons = pcons;<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div><\/div>
This code creates the <\/span>plottrajandaccel<\/span> function.<\/span><\/div>
function <\/span>plottrajandaccel(sol)<\/span><\/span><\/div><\/div>
figure<\/span><\/span><\/div><\/div>
psol = sol.p;<\/span><\/span><\/div><\/div>
p0 = psol(1,:);<\/span><\/span><\/div><\/div>
pF = psol(end,:);<\/span><\/span><\/div><\/div>
plot3(psol(:,1),psol(:,2),psol(:,3),<\/span>'rx'<\/span>)<\/span><\/span><\/div><\/div>
hold <\/span>on<\/span><\/span><\/div><\/div>
plot3(p0(1),p0(2),p0(3),<\/span>'ks'<\/span>)<\/span><\/span><\/div><\/div>
plot3(pF(1),pF(2),pF(3),<\/span>'bo'<\/span>)<\/span><\/span><\/div><\/div>
hold <\/span>off<\/span><\/span><\/div><\/div>
view([18 -10])<\/span><\/span><\/div><\/div>
xlabel(<\/span>\"x\"<\/span>)<\/span><\/span><\/div><\/div>
ylabel(<\/span>\"y\"<\/span>)<\/span><\/span><\/div><\/div>
zlabel(<\/span>\"z\"<\/span>)<\/span><\/span><\/div><\/div>
legend(<\/span>\"Steps\"<\/span>,<\/span>\"Initial Point\"<\/span>,<\/span>\"Final Point\"<\/span>)<\/span><\/span><\/div><\/div>
figure<\/span><\/span><\/div><\/div>
asolm = sol.a;<\/span><\/span><\/div><\/div>
nasolm = sqrt(sum(asolm.^2,2));<\/span><\/span><\/div><\/div>
plot(nasolm,<\/span>\"rx\"<\/span>)<\/span><\/span><\/div><\/div>
xlabel(<\/span>\"Time step\"<\/span>)<\/span><\/span><\/div><\/div>
ylabel(<\/span>\"Norm(acceleration)\"<\/span>)<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div><\/div>
This code creates the <\/span>fvalT<\/span> function, which is used by <\/span>findT<\/span>.<\/span><\/div>
function <\/span>Fval = fvalT(T,opts)<\/span><\/span><\/div><\/div>
p0 = [-1000,-800,1200];<\/span><\/span><\/div><\/div>
v0 = [100,50,-40];<\/span><\/span><\/div><\/div>
tprob = makeprob(T,p0,v0);<\/span><\/span><\/div><\/div>
opts = optimoptions(opts,<\/span>\"Display\"<\/span>,<\/span>\"off\"<\/span>);<\/span><\/span><\/div><\/div>
[~,Fval] = solve(tprob,Options=opts);<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div><\/div>
This code creates the <\/span>findT<\/span> function.<\/span><\/div>
function <\/span>Tmin = findT(opts)<\/span><\/span><\/div><\/div>
disp(<\/span>\"Solving...\"<\/span>)<\/span><\/span><\/div><\/div>
Tmin = fminbnd(@(T)fvalT(T,opts),20,60);<\/span><\/span><\/div><\/div>
disp(<\/span>\"Done\"<\/span>)<\/span><\/span><\/div><\/div>
end<\/span><\/span><\/div><\/div><\/div>
Copyright 2021 The MathWorks, Inc.<\/span><\/div>\r\n<\/div>