I am not sure that I understand exactly what is happening. But it sounds to me as if the nonlinear constraints are not being satisfied, and the objective function value is overpowering the constraint values for the solver.
So I suggest that you multiply your nonlinear constraint function by a large number, say 1e3 or 1e4, and see whether that helps. Of course, you should also check your nonlinear constraint function by setting some trajectories and seeing that the constraint values make sense for those trajectories.
As an aside, your finite differencing options do not make sense. I suuggest that you remove the DiffMinChange option call. See Set Larger Finite Differences.
MATLAB mathematical toolbox documentation