Got MBH working! It can find better-than-basic-spiral trajectories

2021-09-02 17:21:40 -06:00
parent 58ad3c5293
commit 83886188db
8 changed files with 318 additions and 381 deletions
--- a/julia/src/conversions.jl
+++ b/julia/src/conversions.jl
@@ -72,9 +72,9 @@ end
 """
 Converts a series of thrust vectors from R,Θ,H frame to cartesian
 """
-function conv_T(Tr::Vector{Float64},
-                TΘ::Vector{Float64},
-                Th::Vector{Float64},
+function conv_T(Tm::Vector{Float64},
+                Ta::Vector{Float64},
+                Tb::Vector{Float64},
                init_state::Vector{Float64},
                m::Float64,
                craft::Sc,
@@ -84,29 +84,32 @@ function conv_T(Tr::Vector{Float64},
  Txs = Float64[]
  Tys = Float64[]
  Tzs = Float64[]
-  state = init_state
-  for i in 1:length(Tr)
-    mag, α, β = Tr[i], TΘ[i], Th[i]
+  n::Int = length(Tm)

+  for i in 1:n
+    mag, α, β = Tm[i], Ta[i], Tb[i]
    if mag > 1 || mag < 0
-      throw(ErrorException("ΔV input is too high: $mag"))
+      @error "ΔV input is too high: $mag"
    elseif α > π || α < -π
-      throw(ErrorException("α angle is incorrect: $α"))
+      @error "α angle is incorrect: $α"
    elseif β > π/2 || β < -π/2
-      throw(ErrorException("β angle is incorrect: $β"))
+      @error "β angle is incorrect: $β"
    end
+  end

+  state = init_state
+  for i in 1:n
+    mag, α, β = Tm[i], Ta[i], Tb[i]
    thrust_rθh = mag * [cos(β)*sin(α), cos(β)*cos(α), sin(β)]
    _,_,i,Ω,ω,ν = xyz_to_oe(state, μ)
    θ = ω+ν
    cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
    DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
-    sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
-    si*sθ si*cθ ci]
-    ΔV = DCM*thrust_rθh
-    Tx, Ty, Tz = max_ΔV(craft.duty_cycle, craft.num_thrusters, craft.max_thrust, time, 0., m) * ΔV
-    x, y, z, dx, dy, dz = state
-    state = prop_one_simple(Tx, Ty, Tz, x, y, z, dx, dy, dz, time/length(Tr), μ)
+           sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
+           si*sθ          si*cθ           ci      ]
+    Tx, Ty, Tz = DCM*thrust_rθh
+
+    state = prop_one([Tx, Ty, Tz], state, craft, μ, time/n)[1]
    push!(Txs, Tx)
    push!(Tys, Ty)
    push!(Tzs, Tz)
--- a/julia/src/find_closest.jl
+++ b/julia/src/find_closest.jl
@@ -1,6 +1,15 @@
-using JuMP, Ipopt
+using NLsolve

-export nlp_solve
+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
+  return ans/n
+end

 function nlp_solve(start::Vector{Float64},
                   final::Vector{Float64},
@@ -8,77 +17,14 @@ function nlp_solve(start::Vector{Float64},
                   μ::Float64,
                   t0::Float64,
                   tf::Float64,
-                   Txi::Vector{Float64},
-                   Tyi::Vector{Float64},
-                   Tzi::Vector{Float64};
-                   solver_options=(),
+                   x0::Matrix{Float64};
                   tol=1e-6)

-  n::Int = length(Txi)
-  if length(Tyi) != n
-    throw("Bad number of Tys")
-  elseif length(Tzi) != n
-    throw("Bad number of Tzs")
+  function f!(F,x)
+    F .= 0.0
+    F[1:6, 1] .= prop_nlsolve(tanh.(x), start, craft, μ, tf-t0) .- final
  end

-  # First propagate the initial guess 
-  path, masses, final_state = prop(hcat(Txi, Tyi, Tzi),
-                                   start,
-                                   craft,
-                                   μ,
-                                   tf-t0)
-  @assert final ≈ final_state
-  model = Model(optimizer_with_attributes(Ipopt.Optimizer, solver_options...))
-
-  @variables(model, begin
-    Tx[i=1:n], (start=Txi[i])
-    Ty[i=1:n], (start=Tyi[i])
-    Tz[i=1:n], (start=Tzi[i])
-    x[i=1:n], (start=path[1][i])
-    y[i=1:n], (start=path[2][i])
-    z[i=1:n], (start=path[3][i])
-    dx[i=1:n], (start=path[4][i])
-    dy[i=1:n], (start=path[5][i])
-    dz[i=1:n], (start=path[6][i])
-    mass[i=1:n], (start=masses[i])
-  end)
-
-  # Fix initial conditions
-  fix(x[1], start[1], force = true)
-  fix(y[1], start[2], force = true)
-  fix(z[1], start[3], force = true)
-  fix(dx[1], start[4], force = true)
-  fix(dy[1], start[5], force = true)
-  fix(dz[1], start[6], force = true)
-  fix(mass[1], craft.mass, force = true)
-
-  # Fix final conditions
-  fix(x[n], final[1], force = true)
-  fix(y[n], final[2], force = true)
-  fix(z[n], final[3], force = true)
-  fix(dx[n], final[4], force = true)
-  fix(dy[n], final[5], force = true)
-  fix(dz[n], final[6], force = true)
-
-  @NLexpression(model, r_mag[i = 1:n], sqrt(x[i]^2 + y[i]^2 + z[i]^2))
-  @NLexpression(model, v_mag[i = 1:n], sqrt(dx[i]^2 + dy[i]^2 + dz[i]^2))
-  @NLexpression(model, σ0[i = 1:n], (x[i]*dx[i] + y[i]*dy[i] + z[i]*dz[i])/sqrt(μ))
-  @NLexpression(model, a[i = 1:n], 1/(2/r_mag[i] - v_mag[i]^2/μ))
-
-  # Elliptical Specific
-  @NLexpression(model, ΔM_ell[i = 1:n], sqrt(μ) / sqrt(a[i]^3) * (tf-t0)/(2n))
-
-  # Parabolic/Hyperbolic Specific
-  @NLexpression(model, ΔN_hyp[i = 1:n], sqrt(μ) / sqrt(-a[i]^3) * (tf-t0)/(2n))
-
-  for i in 2:n
-    @NLconstraint(model, x[i] == a[i-1])
-    @NLconstraint(model, mass[i] == mass[i-1] - craft.mass_flow_rate*√(Tx[i-1]^2 +
-                                                                       Ty[i-1]^2 +
-                                                                       Tz[i-1]^2)*(tf-t0)/n)
-  end
-
-  optimize!(model)
-  return model
+  return nlsolve(f!, atanh.(x0), ftol=tol, autodiff=:forward, iterations=1_000)

 end
--- a/julia/src/monotonic_basin_hopping.jl
+++ b/julia/src/monotonic_basin_hopping.jl
@@ -1,19 +1,20 @@
-function perturb(x::AbstractVector, n::Int)
-  perturb_vector = 0.02 * rand(Float64, (3n)) .- 0.01
-  return x + perturb_vector
+function pareto(α::Float64, n::Int)
+   s = rand((-1,1), (n,3))
+   r = rand(Float64, (n,3))
+   ϵ = 1e-10
+   return (s./ϵ) * (α - 1.0) ./ (ϵ ./ (ϵ .+ r)).^(-α)
 end

-function mass_better(x_star::AbstractVector,
-                     x_current::AbstractVector,
-                     start::AbstractVector,
-                     final::AbstractVector,
-                     craft::Sc,
-                     μ::AbstractFloat,
-                     t0::AbstractFloat,
-                     tf::AbstractFloat)
-  mass_star = prop(treat_inputs(x_star), start, craft, μ, tf-t0)[2]
-  mass_current = prop(treat_inputs(x_current), start, craft, μ, tf-t0)[2]
-  return mass_star > mass_current
+function perturb(x::AbstractMatrix, n::Int)
+  ans =  x + pareto(1.01, n)
+  map!(elem -> elem > 1.0 ? 1.0 : elem, ans, ans)
+  map!(elem -> elem < -1.0 ? -1.0 : elem, ans, ans)
+  return ans
+end
+
+
+function new_x(n::Int)
+  2.0 * rand(Float64, (n,3)) .- 1.
 end

 function mbh(start::AbstractVector,
@@ -22,35 +23,51 @@ function mbh(start::AbstractVector,
             μ::AbstractFloat,
             t0::AbstractFloat,
             tf::AbstractFloat,
-             n::Int,
-             num_iters::Int=10,
-             tol=1e-6)
-  i::Int = 0
+             n::Int;
+             num_iters=50,
+             patience_level::Int=400,
+             tol=1e-6,
+             verbose=false)
+
  archive = []
-  x_star = nlp_solve(start, final, craft, μ, t0, tf, rand(Float64,(3n)), tol=tol)
-  while converged(x_star) == false
-    x_star = nlp_solve(start, final, craft, μ, t0, tf, rand(Float64,(3n)), tol=tol)
-  end

-  x_current = x_star
-  push!(archive, x_current)
-
-  while i < num_iters
-    x_star = nlp_solve(start, final, craft, μ, t0, tf, perturb(x_current.zero,n), tol=tol)
-    if converged(x_star) && mass_better(x_star.zero, x_current.zero, start, final, craft, μ, t0, tf)
-      x_current = x_star
-      push!(archive, x_star)
-    else
-      while converged(x_star) == false
-        x_star = nlp_solve(start, final, craft, μ, t0, tf, rand(Float64,(3n)), tol=tol)
-      end
-      if mass_better(x_star.zero, x_current.zero, start, final, craft, μ, t0, tf)
-        x_current = x_star
-        push!(archive, x_star)
-      end
-    end
+  i = 0
+  if verbose println("Current Iteration") end
+  while true
    i += 1
+    if verbose print("\r",i) end
+    impatience = 0
+    x_star = nlp_solve(start, final, craft, μ, t0, tf, new_x(n), tol=tol)
+    while converged(x_star) == false
+      x_star = nlp_solve(start, final, craft, μ, t0, tf, new_x(n), tol=tol)
+    end
+    if converged(x_star)
+      x_current = x_star
+      while impatience < patience_level
+        x_star = nlp_solve(start, final, craft, μ, t0, tf, perturb(tanh.(x_current.zero),n), tol=tol)
+        if converged(x_star) && mass_est(tanh.(x_star.zero)) < mass_est(tanh.(x_current.zero))
+          x_current = x_star
+          impatience = 0
+        else
+          impatience += 1
+        end
+      end
+      push!(archive, x_current)
+    end
+    if i >= num_iters
+      break
+    end
  end

-  return x_current, archive
+  current_best_mass = 1e8
+  best = archive[1]
+  for candidate in archive
+    if mass_est(tanh.(candidate.zero)) < current_best_mass
+      current_best_mass = mass_est(tanh.(candidate.zero))
+      best = candidate
+    end
+  end
+
+  return best, archive
+
 end
--- a/julia/src/propagator.jl
+++ b/julia/src/propagator.jl
@@ -16,38 +16,19 @@ end
 This function propagates the spacecraft forward in time 1 Sim-Flanagan step (of variable length of time),
 applying a thrust in the center.
 """
-function prop_one(ΔV::Vector{T},
-                  state::Vector{T},
+function prop_one(thrust_unit::Vector{<:Real},
+                  state::Vector{<:Real},
                  duty_cycle::Float64,
                  num_thrusters::Int,
                  max_thrust::Float64,
                  mass::T,
                  mass_flow_rate::Float64,
                  μ::Float64,
-                  time::Float64) where T
+                  time::Float64) where T<:Real

-  mag, α, β = ΔV
-
-  # if mag > 1 || mag < 0
-    # throw(ErrorException("ΔV input is too high: $mag"))
-  # elseif α > π || α < -π
-    # throw(ErrorException("α angle is incorrect: $α"))
-  # elseif β > π/2 || β < -π/2
-    # throw(ErrorException("β angle is incorrect: $β"))
-  # end
-
-  thrust_rθh = mag * [cos(β)*sin(α), cos(β)*cos(α), sin(β)]
-  a,e,i,Ω,ω,ν = xyz_to_oe(state, μ)
-  θ = ω+ν
-  cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
-  DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
-  sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
-  si*sθ si*cθ ci]
-  ΔV = DCM*thrust_rθh
-
-  thrust = max_ΔV(duty_cycle, num_thrusters, max_thrust, time, 0., mass) * ΔV
-  halfway = laguerre_conway(state, μ, time/2) + [0., 0., 0., thrust[1], thrust[2], thrust[3]]
-  return laguerre_conway(halfway, μ, time/2), mass - mass_flow_rate*norm(ΔV)*time
+  ΔV = max_ΔV(duty_cycle, num_thrusters, max_thrust, time, 0., mass) * thrust_unit
+  halfway = laguerre_conway(state, μ, time/2) + [0., 0., 0., ΔV...]
+  return laguerre_conway(halfway, μ, time/2), mass - mass_flow_rate*norm(thrust_unit)*time

 end

@@ -64,6 +45,99 @@ function prop_one(ΔV_unit::Vector{T},
  return state, Sc(mass, craft.mass_flow_rate, craft.max_thrust, craft.num_thrusters, craft.duty_cycle)
 end

+
+"""
+This propagates over a given time period, with a certain number of intermediate steps
+"""
+function prop(ΔVs::Matrix{T},
+              state::Vector{Float64},
+              duty_cycle::Float64,
+              num_thrusters::Int,
+              max_thrust::Float64,
+              mass::Float64,
+              mass_flow_rate::Float64,
+              μ::Float64,
+              time::Float64) where T <: Real
+
+  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
+  n = size(ΔVs)[i]
+
+  for i in 1:n
+    state, mass = prop_one(ΔVs[i,:], state, duty_cycle, num_thrusters, max_thrust, mass,
+                           mass_flow_rate, μ, time/n)
+  end
+
+  return state, mass
+
+end
+
+"""
+The same function, using Scs
+"""
+function prop(ΔVs::Matrix{T},
+              state::Vector{S},
+              craft::Sc,
+              μ::Float64,
+              time::Float64) where {T <: Real, S <: Real}
+
+  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
+  n = size(ΔVs)[1]
+
+  x_states = [state[1]]
+  y_states = [state[2]]
+  z_states = [state[3]]
+  dx_states = [state[4]]
+  dy_states = [state[5]]
+  dz_states = [state[6]]
+  masses = [craft.mass]
+
+  for i in 1:n
+    state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
+    push!(x_states, state[1])
+    push!(y_states, state[2])
+    push!(z_states, state[3])
+    push!(dx_states, state[4])
+    push!(dy_states, state[5])
+    push!(dz_states, state[6])
+    push!(masses, craft.mass)
+  end
+
+  return [x_states, y_states, z_states, dx_states, dy_states, dz_states], masses, state
+
+end
+
+function prop_nlsolve(ΔVs::Matrix{T},
+                      state::Vector{S},
+                      craft::Sc,
+                      μ::Float64,
+                      time::Float64) where {T <: Real, S <: Real}
+
+  n = size(ΔVs)[1]
+  for i in 1:n
+    state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
+  end
+
+  return state
+
+end
+
+function prop_simple(ΔVs::AbstractMatrix,
+                     state::AbstractVector,
+                     craft::Sc,
+                     μ::Float64,
+                     time::Float64)
+
+  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
+  n = size(ΔVs)[1]
+
+  for i in 1:n
+    state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
+  end
+
+  return state
+
+end
+
 function prop_one_simple(Tx, Ty, Tz, x, y, z, dx, dy, dz, t, μ)

  # perform laguerre_conway once
@@ -154,83 +228,5 @@ function prop_one_simple(Tx, Ty, Tz, x, y, z, dx, dy, dz, t, μ)
  end

  return [F*[x,y,z] + G*[dx,dy,dz]; Ft*[x,y,z] + Gt*[dx,dy,dz]]
-  return repeat([sqrt(Tx^2 + Ty^2 + Tz^2)],6)

-end
-
-"""
-This propagates over a given time period, with a certain number of intermediate steps
-"""
-function prop(ΔVs::Matrix{T},
-              state::Vector{Float64},
-              duty_cycle::Float64,
-              num_thrusters::Int,
-              max_thrust::Float64,
-              mass::Float64,
-              mass_flow_rate::Float64,
-              μ::Float64,
-              time::Float64) where T <: Real
-
-  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
-  n = size(ΔVs)[i]
-
-  for i in 1:n
-    state, mass = prop_one(ΔVs[i,:], state, duty_cycle, num_thrusters, max_thrust, mass,
-                           mass_flow_rate, μ, time/n)
-  end
-
-  return state, mass
-
-end
-
-"""
-The same function, using Scs
-"""
-function prop(ΔVs::AbstractMatrix,
-              state::AbstractVector,
-              craft::Sc,
-              μ::Float64,
-              time::Float64)
-
-  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
-  n = size(ΔVs)[1]
-
-  x_states = [state[1]]
-  y_states = [state[2]]
-  z_states = [state[3]]
-  dx_states = [state[4]]
-  dy_states = [state[5]]
-  dz_states = [state[6]]
-  masses = [craft.mass]
-
-  for i in 1:n
-    state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
-    push!(x_states, state[1])
-    push!(y_states, state[2])
-    push!(z_states, state[3])
-    push!(dx_states, state[4])
-    push!(dy_states, state[5])
-    push!(dz_states, state[6])
-    push!(masses, craft.mass)
-  end
-
-  return [x_states, y_states, z_states, dx_states, dy_states, dz_states], masses, state
-
-end
-
-function prop_simple(ΔVs::AbstractMatrix,
-                     state::AbstractVector,
-                     craft::Sc,
-                     μ::Float64,
-                     time::Float64)
-
-  if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
-  n = size(ΔVs)[1]
-
-  for i in 1:n
-    state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
-  end
-
-  return state
-
-end
+end