[WIP] Add control offset to QuadraticCostWithOffset

example/SILQGamesExamples/LQ_parameters.jl

@@ -21,10 +21,10 @@ x₁ = [2.; 0.; 1.; 0.; -1.; 0; 2; 0]
 # x₁ = rand(rng, 8)
 x₁[[2, 4, 6, 8]] .= 0
 
-# Initial controls
+# Initial controls - pretty bad linearization
 us_1 = [zeros(udim(dyn, ii), T) for ii in 1:num_agents(dyn)]
 for ii in 1:num_players
-    us_1[ii][1,:] .= -1.
+    us_1[ii][1,:] .= -.1
     us_1[ii][2,:] .= -.1
 end
 # duration = (T-1) * dt

example/SILQGamesExamples/RunSILQGamesOnLQExample.jl

@@ -10,6 +10,7 @@ leader_idx=1
 num_runs=1
 
 # config variables
+# these ones test that it converges to the same 
 threshold=1.
 max_iters=1000
 step_size=1e-2

src/costs/CostUtils.jl

@@ -56,14 +56,17 @@ function evaluate(c::Cost, xs::AbstractMatrix{Float64}, us::AbstractVector{<:Abs
     return total
 end
 
-function quadraticize_costs(c::Cost, time_range, x::AbstractVector{Float64}, us::AbstractVector{<:AbstractVector{Float64}})
+# Return the second order Taylor approximation for dx = x - x0:
+# g(x0 + dx, u0s + dus) ≈ f(x, us) + dx' ∇ₓ g(x0, u0s) + (1/2) dx' ∇ₓₓ g(x0, u0s) dx
+#                                  + ∑ du' ∇ᵤ g(x0, u0s) + (1/2) du' ∇ᵤᵤ g(x0, u0s) du
+function quadraticize_costs(c::Cost, time_range, x0::AbstractVector{Float64}, u0s::AbstractVector{<:AbstractVector{Float64}})
     num_players = length(us)
 
-    cost_eval = compute_cost(c, time_range, x, us)
-    ddx2 = Gxx(c, time_range, x, us)
-    dx = Gx(c, time_range, x, us)
-    ddu2s = Guus(c, time_range, x, us)
-    dus = Gus(c, time_range, x, us)
+    cost_eval = compute_cost(c, time_range, x0, u0s)
+    ddx2 = Gxx(c, time_range, x0, u0s)
+    dx = Gx(c, time_range, x0, u0s)
+    ddu2s = Guus(c, time_range, x0, u0s)
+    dus = Gus(c, time_range, x0, u0s)
 
     # Used to compute the way the constant cost terms are divided.
     num_cost_mats = length(ddu2s)
@@ -75,6 +78,8 @@ function quadraticize_costs(c::Cost, time_range, x::AbstractVector{Float64}, us:
         add_control_cost!(quad_cost, ii, ddu2s[ii]; r=dus[ii]', cr=const_cost_term)
     end
 
+    # offset_cost = QuadraticCostWithOffset(quad_cost, x0, u0s)
+
     return quad_cost
 end
 

src/costs/QuadraticCost.jl

@@ -36,9 +36,13 @@ end
 
 function compute_cost(c::QuadraticCost, time_range, x::AbstractVector{Float64}, us::AbstractVector{<:AbstractVector{Float64}})
     num_players = length(us)
-    total = (1/2.) * (x' * c.Q * x + 2 * c.q' * x + c.cq)
+    total = (1/2.) * x' * c.Q * x 
+    total += c.q' * x
+    total += c.cq
     if !isempty(c.Rs)
-        total += (1/2.) * sum(us[jj]' * R * us[jj] + 2 * us[jj]' * c.rs[jj] + c.crs[jj] for (jj, R) in c.Rs)
+        total += (1/2.) * sum(us[jj]' * R * us[jj] for (jj, R) in c.Rs)
+        total += sum(us[jj]' * c.rs[jj] for (jj, r) in c.rs)
+        total += sum(c.crs[jj] for (jj, cr) in c.crs)
     end
     return total
 end