Ok, now open loop is working, sc mass changed to state, and other updates

julia/src/inner_loop/find_closest.jl

@@ -5,16 +5,13 @@ export nlp_solve, mass_est
 function mass_est(T)
   ans = 0
   n = Int(length(T)/3)
-  for i in 1:n
-    ans += norm(T[i,:])
-  end
+  for i in 1:n ans += norm(T[i,:]) end
   return ans/n
 end
 
-converged(x) = NLsolve.converged(x)
-
-function converged(_::String)
-  return false
+struct Result
+  converged::Bool
+  zero::Matrix{Float64}
 end
 
 function nlp_solve(start::Vector{Float64},
@@ -28,10 +25,21 @@ function nlp_solve(start::Vector{Float64},
                    num_iters=1_000)
 
   function f!(F,x)
-    F .= 0.0
-    F[1:6, 1] .= prop_nlsolve(tanh.(x), start, craft, μ, tf-t0) .- final
+    try
+      F .= 0.0
+      F[1:6, 1] .= prop(tanh.(x), start, copy(craft), μ, tf-t0)[2][1:6] .- final[1:6]
+    catch e
+      F .= 10000000.0
+    end
   end
 
-  return nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=num_iters)
+  result = Result(false, zeros(size(x0)))
+  try
+    nl_results = nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=num_iters)
+    result = Result(converged(nl_results), tanh.(nl_results.zero))
+  catch e
+  end
 
-end
+  return result
+
+end