Local infeasibility in MATLAB

The infeasible solution is caused by the terminal constraints:

  tf * z = 4
  tf * z = 5
  tf * z = 6

When tf=0, the constraints are evaluated to 0=4, 0=5, 0=6 and the solver reports that these can not be satisfied by the solver. Instead, you can pose the constraints as:

  tf * (x-4) = 0
  tf * (y-5) = 0
  tf * (z-6) = 0

That way, the constraint is valid when tf=0 and when tf=1 at the final time. A potential better way yet is to convert the terminal constraints to objective terms with f=1000 such as:

  minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
  minimize f*tf*(xp^2 + yp^2 + zp^2)
  minimize f*tf*(roll^2 + pitch^2 + yaw^2)

That way, the optimizer won't report an infeasible solution if it can't reach the terminal constraints as discussed in the pendulum problem. I made a few other modifications to your model and script to achieve a successful solution. Here is a summary:

• Converted terminal constraints to objective function (soft constraints)
• Parameters t1-t4 should be variables
• Fixed degree of freedom issue by making w1-w4 variables and w1p-w4p variables. w1-w4 are differential states.
• Added constraints to w1p-w4p between -10 and 10 to help the solver converge
• Added initialization step to simulate the model before optimizing. There are more details on initialization strategies in this paper: Safdarnejad, S.M., Hedengren, J.D., Lewis, N.R., Haseltine, E., Initialization Strategies for Optimization of Dynamic Systems, Computers and Chemical Engineering, 2015, Vol. 78, pp. 39-50, DOI: 10.1016/j.compchemeng.2015.04.016

Model

Parameters
tf

 w1p = 0 > -10 < 10
 w2p = 0 > -10 < 10
 w3p = 0 > -10 < 10
 w4p = 0 > -10 < 10


Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359

#Motor Coefficients
 J = 4.1904e-5 !kg*m^2
 kt = 0.0104e-3 !N*m/A
 kv = 96.342 !rad/s/volt
 Dv = 0.2e-3 !N*m*s/rad
 R = 0.2 !Ohms

#Battery parameters
 Q = 1.55 !Ah
 Rint = 0.02 !Ohms
 E0 = 1.24 !volt
 K = 2.92e-3 !volt
 A = 0.156
 B =2.35

#Quadrotor parameters
  l = 0.175 !m
  m = 1.3 !kg
  Ix = 0.081 !kg*m^2
  Iy = 0.081 !kg*m^2
  Iz = 0.142 !kg*m^2
  kb = 3.8305e-6 !N/rad/s
  ktau = 2.2518e-8 !(N*m)/rad/s

#Parametrizacion del polinomio
  a1 = -1.72e-5
  a2 = 1.95e-5
  a3 = -6.98e-6
  a4 = 4.09e-7
  b1 = 0.014
  b2 = -0.0157
  b3 = 5.656e-3
  b4 = -3.908e-4
  c1 = -0.8796
  c2 = 0.3385
  c3 = 0.2890
  c4 = 0.1626

Variables
!------------------CONDICONES INICIALES------------------!
 x = 0
 xp = 0
 y = 0
 yp = 0
 z = 0
 zp = 0
 pitch = 0, >=-pi/2, <=pi/2   !theta - restricciones
 pitchp = 0
 roll = 0, >=-pi/2, <=pi/2    !phi - restricciones
 rollp = 0
 yaw = 0                      !psi
 yawp = 0  %, >=-200/180, <=200/180

#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000

 t1 = 0, >=0
 t2 = 0, >=0
 t3 = 0, >=0
 t4 = 0, >=0

#Función objetivo
  of = 0 !condición inicial de la función objetivo
Intermediates

#Motor 1
  aw1 = a1*w1^2 + b1*w1 + c1
  bw1 = a2*w1^2 + b2*w1 + c2
  cw1 = a3*w1^2 + b3*w1 + c3
  dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
  aw2 = a1*w2^2 + b1*w2 + c1
  bw2 = a2*w2^2 + b2*w2 + c2
  cw2 = a3*w2^2 + b3*w2 + c3
  dw2 = a4*w2^2 + b4*w2 + c4 
#Motor 3
  aw3 = a1*w3^2 + b1*w3 + c1
  bw3 = a2*w3^2 + b2*w3 + c2
  cw3 = a3*w3^2 + b3*w3 + c3
  dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
  aw4 = a1*w4^2 + b1*w4 + c1
  bw4 = a2*w4^2 + b2*w4 + c2
  cw4 = a3*w4^2 + b3*w4 + c3
  dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
  fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
  fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
  fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
  fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
  T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
  u1 = kb * (w2^2 - w4^2)
  u2 = kb * (w3^2 - w1^2)
  u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
  wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
  Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
  Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
  Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
  Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
  Ectotal = Ec1 + Ec2 + Ec3 + Ec4

! scaling factor for terminal constraint
  f = 1000

Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
  minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
  xp = $x
  yp = $y
  zp = $z
  pitchp = $pitch
  rollp = $roll
  yawp = $yaw

  w1p = $w1
  w2p = $w2
  w3p = $w3
  w4p = $w4
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
  #tf * (x-4) = 0
  #tf * (y-5) = 0
  #tf * (z-6) = 0
  #tf * xp = 0
  #tf * yp = 0
  #tf * zp = 0
  #tf * roll = 0
  #tf * pitch = 0
  #tf * yaw = 0

  minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
  minimize f*tf*(xp^2 + yp^2 + zp^2)
  minimize f*tf*(roll^2 + pitch^2 + yaw^2)
!-----------------TORQUE DE LOS MOTORES------------------!
  t1 = J*w1p + ktau*w1^2 + Dv*w1
  t2 = J*w2p + ktau*w2^2 + Dv*w2
  t3 = J*w3p + ktau*w3^2 + Dv*w3
  t4 = J*w4p + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
  m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
  m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
  m*$zp = (cos(roll)*cos(pitch))*T-m*g
  Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
  Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
  Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
  $of = Ectotal

MATLAB Script

clear all; close all; clc

server = 'http://byu.apmonitor.com';
app = 'traj_optima';

addpath('apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');

apm_option(server,app,'apm.max_iter',1000);
apm_option(server,app,'apm.nodes',3);
apm_option(server,app,'apm.rtol',1e-6);
apm_option(server,app,'apm.otol',1e-6);
apm_option(server,app,'apm.solver',3);
apm_option(server,app,'apm.imode',6);
apm_option(server,app,'apm.mv_type',1);


costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1p');
apm_option(server,app,'w1p.status',1);
apm_info(server,app,'MV','w2p');
apm_option(server,app,'w2p.status',1);
apm_info(server,app,'MV','w3p'); 
apm_option(server,app,'w3p.status',1);
apm_info(server,app,'MV','w4p');
apm_option(server,app,'w4p.status',1);

%Salida
disp('')
disp('------------- Initialize ----------------')
apm_option(server,app,'apm.coldstart',1);
output = apm(server,app,'solve');
disp(output)

disp('')
disp('-------------- Optimize -----------------')
apm_option(server,app,'apm.time_shift',0);
apm_option(server,app,'apm.coldstart',0);
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app); 
z = y.x;

This gives a successful solution but the terminal constraints are not met. The solver optimizes the use of w1p-w4p to minimize the objective but there is no solution that makes it to the terminal constraints.

 The solution was found.

 The final value of the objective function is    50477.4537378181     

 ---------------------------------------------------
 Solver         :  IPOPT (v3.12)
 Solution time  :    3.06940000000759      sec
 Objective      :    50477.4537378181     
 Successful solution
 ---------------------------------------------------

As a next step, I recommend that you increase the number of time points or allow the final time to change to meet the terminal constraints. You may also want to consider switching to Python Gekko that uses the same underlying engine as APM MATLAB. In this case, the modeling language is fully integrated with Python.