experiments with integration

tssos_quat.jl

@@ -18,6 +18,13 @@ function qdot(q, u)
 end
 # qdot([1;0;0;0], x)
 
+#q1 * conj(q2)
+function rot_between(q1, q2)
+    [
+        q1' * q2
+        - q1[1] * q2[2:4] + q2[1] * q1[2:4] - cross(q1[2:4], q2[2:4])
+    ]
+end
 
 function euler(f, x, u, dt)
     x + dt* f(x, u)
@@ -42,8 +49,9 @@ Xcost_dot(x, u) = [qdot(x[1:4], u); L(x, u)]
 ## multiple shooting
 n = 4
 m = 3
-N = 10
+N = 25
 T = 1
+dt = T / (N-1)
 
 q0 = [1;0;0;0.0]
 qf = -1/sqrt(2)*[0; 0; 1; 1]
@@ -59,12 +67,33 @@ h = [
 
 
 for i = 1:N-1
-    Xkp1 = rk2(Xcost_dot, [tabX[:, i]; f], tabu[:, i], T/(N-1))
-    f = Xkp1[end] #change this to allow for implicit methods
-    append!(h, Xkp1[1:4] - tabX[:, i+1])
-    push!(h, tabX[:, i]' * tabX[:, i] - 1)
+    # Xkp1 = euler(Xcost_dot, [tabX[:, i]; f], tabu[:, i], T/(N-1))
+    # f = Xkp1[end] #change this to allow for implicit methods
+    # append!(h, Xkp1[1:4] - tabX[:, i+1])
+    
+    #central difference - works much better than euler
+    # midX = (tabX[:, i] + tabX[:, i+1])/2
+    # midXdot = Xcost_dot([midX; f], tabu[:, i])
+    # f = f + dt*midXdot[5]
+    # append!(h, tabX[:, i+1] - (tabX[:, i] + dt*midXdot[1:4]))
+
+    # trapezoid rule - optimal with r = 1
+    Xdotmid = (qdot(tabX[:, i], tabu[:, i]) + qdot(tabX[:, i+1], tabu[:, i]))/2
+    f = f + dt*L(tabX[:, i], tabu[:, i])
+    append!(h, tabX[:, i+1] - (tabX[:, i] + dt*Xdotmid))
+
+    push!(h, tabX[:, i]' * tabX[:, i] - 1) #remove gives very bad bound
+    #probally comes from Putinar's Positivstellensatz
 end
 
+#final cost - doesn't work bc theta = 180
+# err = rot_between(tabX[:, N], qf)
+# f += 100 * err' * err
+
+#final cost - doesn't work great
+# err = tabX[:, N] - qf
+# f += 1 * err' * err
+
 g = [
     # [2 - tabX[:, i]' * tabX[:, i] for i = 1:N]...
     # [100 - tabu[:, i]'*tabu[:, i] for i = 1:N-1]...
@@ -76,13 +105,13 @@ g = [
 ## works in 13s, 23 iter
 opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 2, numeq=length(h), TS="block", solution=true)
 ## faster than block, stops on slow progress
-opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 2, numeq=length(h), TS="MD", solution=true)
+opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 1, numeq=length(h), TS="MD", solution=true)
 ## slow progress stops solver, bad solution
 opt,sol,data = cs_tssos_higher!(data, TS="MD", solution=true)
 ## 10GB, takes s to solve
 # opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 4, numeq=length(h), TS="MD", solution=true)
 ## <<<< 1s & optimal!!!!
-opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 2, numeq=length(h), TS="MD", solution=true, merge=true)
+opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 1, numeq=length(h), TS="MD", solution=true, merge=true)
 ## <<<< 1s & slow progress
 opt, sol, data = cs_tssos_first([f; h], [tabX..., tabu...], 2, numeq=length(h), TS="MD", solution=true, MomentOne=true)
 ## diverges
@@ -95,6 +124,8 @@ tspan = range(0, T, N)
 ##
 scatter(tspan[1:end-1], solu')
 ##
+scatter(tspan, solx')
+##
 tabx0 = repeat(q0, outer=(1, N))
 tabu0 = zeros(m, N-1)
 x0 = [tabx0...;tabu0...]