I'm making a trajectory propagation optimiser, running into an issue where the result of SciPy optimise seems to completely ignore a non linear constraint. I've used the exact same format in a similar optimisation problem and it's worked fine, so I'm not sure what I've got wrong.
The function works by taking an initial and final state (position & velocity), propagate a known trajectory between them (determined by vT0 & vTn), and splitting it into a number of nodes. Each node can have an additional velocity vector, with the aim to minimise the total magnitude of velocity change required. The propagation is done forwards from the initial state, and backwards from the final state, ending in a "match point". This is done to reduce the effects of small changes in early variable changes causing large changes in position.
Constraints are that the match point position for the forward and backwards propagation has to be equal, and the maximum velocity magnitude at each node must be less than a user specified value.
from numpy import array, full, zeros, inf, concatenate, split, copy
from numpy.linalg import norm
from scipy import optimize as opt
from spiceypy import prop2b
#Inputs will vary, these are chosen as I know the trajectory works
state0 = array([-7.86375668e+07, -1.29337294e+08, 5.88825525e+03, 2.49800471e+01,
-1.55820011e+01, -5.64559825e-04])
stateN = array([ 1.71833297e+08, 1.31233678e+08, -1.46653554e+06, -1.37849313e+01,
2.13241960e+01, 7.85074757e-01])
vT0 = array([ 26.91259713, -17.52627828, -0.59535233])
vTn = array([-12.98662423, 18.35932831, 0.38237022])
T = 18921599.998542428
mu = 1.32712440018e11
n = 5
max_dv = 2.5
dt = T/(n-1)
match_node_num = int((n-1) / 2)
x0 = zeros((n-1, 3))
x0[0] = vT0 - state0[3:6]
x0[-1] = vTn - stateN[3:6]
x0 = x0.flatten() #Initial guess is a known trajectory
n_cons = len(x0)
#Optimisation variables are the x,y,z components for a velocity vector
#Optimisation goal is to minimise velocity
bounds_lb = full(n_cons, -inf)
bounds_ub = full(n_cons, inf)
bounds = opt.Bounds(bounds_lb, bounds_ub)
def non_lin_cons_f(x):
s0 = copy(state0)
sn = copy(stateN)
dvs = split(x, len(x) / 3)
for_dv, rev_dv = split(x, 2)
for_dv = array(split(for_dv, len(for_dv) / 3))
rev_dv = array(split(rev_dv, len(rev_dv) / 3)[::-1])
for i in range(match_node_num):
s0[3:6] += for_dv[i]
sn[3:6] += rev_dv[i]
#Propagation function, returns the position and velocity after a series of manoeuvres
#Propagates forward and backwards from a known position
#This is tested and returns the correct values
s0 = prop2b(mu, s0, dt)
sn = prop2b(mu, sn, -dt)
match_pos_diff = s0[0:3] - sn[0:3]
mx, my, mz = match_pos_diff
match_vel_diff = norm(s0[3:6] - sn[3:6])
dv_lim = zeros(len(dvs))
for idx, dv in enumerate(dvs):
dv_lim[idx] = norm(dv)
return concatenate(([mx, my, mz, match_vel_diff], dv_lim))
non_lin_lb = concatenate((zeros(3), full(n, -inf)))
non_lin_ub = concatenate((zeros(3), full(n, max_dv)))
#Constraint set 1: No difference in position at end of forward/reverse propagation
#Constraint set 2: Velocity vector magnitude less than a set maximum
non_lin_cons = opt.NonlinearConstraint(non_lin_cons_f, non_lin_lb, non_lin_ub, jac="2-point", hess=opt.SR1())
def min_dv(x):
#Want to minimise velocity vector at each node plus the differnece in velocity at the match point between legs
s0 = copy(state0)
sn = copy(stateN)
dvs = split(x, len(x)/3)
dVT = sum(norm(dvs, axis=1))
for_dv, rev_dv = split(x, 2)
for_dv = array(split(for_dv, len(for_dv) / 3))
rev_dv = array(split(rev_dv, len(rev_dv) / 3)[::-1])
for i in range(match_node_num):
s0[3:6] += for_dv[i]
sn[3:6] += rev_dv[i]
s0 = prop2b(mu, s0, dt)
sn = prop2b(mu, sn, -dt)
match_vel_diff = norm(s0[3:6] - sn[3:6])
print(match_vel_diff + dVT)
return dVT + match_vel_diff
res = opt.minimize(min_dv, x0, method="trust-constr", jac="2-point", hess=opt.SR1(),
constraints=[non_lin_cons], bounds=bounds)
Running this gets the following results:
res.x = array([ 1.77922285e+00, -2.06103788e+00, -5.17851095e-01, 1.17303467e-03,
2.77590586e-04, -4.08771135e-04, -3.88194003e-03, -2.44162328e-01,
5.25358104e-03, 8.27718594e-01, -2.62235313e+00, -4.63621925e-01])
Which when put back into the non_lin_cons_f() function returns:
array([-2.75066495e-03, -4.46048379e-03, 3.84587795e-03, 8.59209996e-02,
2.77158454e+00, 1.27285534e-03, 2.44249692e-01, 2.78869132e+00])
Which clearly violates the constraint conditions:
non_lin_lb = array([ 0., 0., 0., -inf, -inf, -inf, -inf, -inf])
non_lin_ub = array([0. , 0. , 0. , 2.5, 2.5, 2.5, 2.5, 2.5])
Like I said, I've used this same way of defining the constraints for another problem and it works fine, I'm not sure why this one seems to be ignoring them
The function for determining the values of the non-linear constraints is working exactly as intended and giving the numbers I expect, my question is purely why that output can violate the constraint boundaries and still be considered a correct solution by scipy.