paper stuff

Author: tylerhanks

examples/ConstrainedLQR.jl

@@ -9,30 +9,26 @@ Q = 5*[1.0 0; 0 1.0]
 R = 3.0
 x₀ = [3, 1]
 N = 10
+dim_x = 2
+dim_u = 1
 
 cost(uₖ,xₖ) = quadform(xₖ,Q) + R*square(uₖ)
-set_constraints(uₖ,xₖ) = [
+dynamics(uₖ,xₖ) = A*xₖ + B*uₖ
+constraints(uₖ,xₖ) = [
     uₖ <= 1, uₖ >= -1,
     xₖ[1] <= 3, xₖ[1] >= -3,
     xₖ[2] <= 2, xₖ[2] >= -2
 ]
-dynamics(uₖ,xₖ,xₖplus1) = xₖplus1 == A*xₖ + B*uₖ
-
-one_step = one_step_bifunction(cost, set_constraints, A, B)
 
+one_step = one_step_bifunction(dim_x,dim_u,cost, constraints, dynamics)
 MPC_bifunc = MPC_bifunction(one_step, N)
 
-us = [Variable(1) for i in 1:N]
-#x1 = Variable(2)
-#x1 = repeat([1],2)
+us = [Variable(1) for i in 1:N-1]
 x_N = Variable(2)
-#x_N = zeros(2)
 
 MPC_prob = to_cvx(MPC_bifunc, us, x₀, x_N)
 
-#fix!(x1, repeat([1], 2))
 solve!(MPC_prob, SCS.Optimizer)
-o = MPC_prob.optval
 
 function simulate(A, B, x₀, us, N)
     x = x₀
@@ -41,3 +37,28 @@ function simulate(A, B, x₀, us, N)
     end
     return x
 end
+
+### Compare to hand written implementation
+xs = Variable(2, N)
+us = Variable(1, N-1)
+
+constraints = Constraint[
+    xs[:, i+1] == A*xs[:,i] + B*us[:,i] for i in 1:N-1
+]
+
+for i in 1:N-1
+    push!(constraints, xs[:,i][1] <= 3)
+    push!(constraints, xs[:,i][1] >= -3)
+    push!(constraints, xs[:,i][2] <= 2)
+    push!(constraints, xs[:,i][2] >= -2)
+    push!(constraints, us[:,i] <= 1)
+    push!(constraints, us[:,i] >= -1)
+end
+
+push!(constraints, xs[:,1] == x₀)
+
+objective = sum([quadform(xs[:,i], Q) + R*square(us[:,i]) for i in 1:N-1])
+
+prob = minimize(objective, constraints)
+
+solve!(prob, SCS.Optimizer)

src/CMPC.jl

@@ -28,12 +28,22 @@ function one_step_bifunction(f::Function,g::Function,A,B)
     return OpenParaConvexBifunction(n,n,[m],impl)
 end
 
+function one_step_bifunction(x_dim, u_dim, f::Function,g::Function,h::Function)
+    impl = (u1,x1,x2) -> ConvexBifunction(
+        f(u1[1],x1),
+        vcat(g(u1[1],x1), x2 == h(u1[1],x1))
+    )
+    return OpenParaConvexBifunction(x_dim,x_dim,[u_dim],impl)
+end
+
 function MPC_bifunction(one_step::OpenParaConvexBifunction, N::Int)
     res = one_step
-    for i in 1:N-1
+    for i in 1:N-2
         res = compose(ParaConv(), res, one_step)
     end
     return res
 end
 
-end
+
+end
+