使用Ensmallen优化时遇到问题

我正在尝试使用Ensmallen优化某个目标函数，这是我在Github上找到的用于数学优化的库。不幸的是，我在文档中找到的示例并没有那么有用。
我有一个取决于两个参数的长度函数：k_i和k_f，我想将其最小化。该函数受到4个约束：constraint1，...，constraint4。这些约束取决于：k_i，k_f和s，它是从0到1的参数，表示我的长度的参数化。
基本上，这是对半无限编程问题的一种简化。变量“ s”以50步从0递增到1，对于每一步，我都希望我的目标函数满足我的4个约束。
x_i，y_i，theta_i，x_f，y_f，theta_f，速度，直径，uplim，downlim是常数。通过优化，我想找到k_i和k_f的值。
所以这是我的代码的一部分：
double Evaluate(const arma::mat& x) { 

k_i = x(0,0);
k_f = x(1,0);

return sqrt(k_f*k_f)*(1.0/3.0E1)+sqrt(k_i*k_i)*(1.0/3.0E1)+sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/3.0E1)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/3.75E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2)+sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*(1.0/7.5E2);      
  }

void Gradient(const arma::mat& x,arma::mat& g) {    

k_i = x(0,0);
g.resize(2,1);

g(0,0) = (k_i*9.8E1-x_f*cos(theta_i)*3.36E2+x_i*cos(theta_i)*3.36E2-y_f*sin(theta_i)*3.36E2+y_i*sin(theta_i)*3.36E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.28E2+x_f*cos(theta_i)*3.84E2-x_i*cos(theta_i)*3.84E2+y_f*sin(theta_i)*3.84E2-y_i*sin(theta_i)*3.84E2-k_f*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_i*1.8E1-x_f*cos(theta_i)*2.16E2+x_i*cos(theta_i)*2.16E2-y_f*sin(theta_i)*2.16E2+y_i*sin(theta_i)*2.16E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*1.28E2-x_f*cos(theta_i)*5.76E2+x_i*cos(theta_i)*5.76E2-y_f*sin(theta_i)*5.76E2+y_i*sin(theta_i)*5.76E2+k_f*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_i*5.78E2-x_f*cos(theta_i)*1.836E3+x_i*cos(theta_i)*1.836E3-y_f*sin(theta_i)*1.836E3+y_i*sin(theta_i)*1.836E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*7.938E3+x_f*cos(theta_i)*6.804E3-x_i*cos(theta_i)*6.804E3+y_f*sin(theta_i)*6.804E3-y_i*sin(theta_i)*6.804E3-k_f*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_i*9.8E1+x_f*cos(theta_i)*1.764E3-x_i*cos(theta_i)*1.764E3+y_f*sin(theta_i)*1.764E3-y_i*sin(theta_i)*1.764E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.178E3-x_f*cos(theta_i)*8.316E3+x_i*cos(theta_i)*8.316E3-y_f*sin(theta_i)*8.316E3+y_i*sin(theta_i)*8.316E3-k_f*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_i*2.0-x_f*cos(theta_i)*1.2E1+x_i*cos(theta_i)*1.2E1-y_f*sin(theta_i)*1.2E1+y_i*sin(theta_i)*1.2E1+k_f*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_i*1.0/sqrt(k_i*k_i)*(1.0/3.0E1);

g(1,0) = (k_f*9.8E1-x_f*cos(theta_f)*3.36E2+x_i*cos(theta_f)*3.36E2-y_f*sin(theta_f)*3.36E2+y_i*sin(theta_f)*3.36E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*4.9E1+(k_i*k_i)*6.4E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2-k_f*x_f*cos(theta_f)*3.36E2+k_f*x_i*cos(theta_f)*3.36E2+k_i*x_f*cos(theta_i)*3.84E2-k_i*x_i*cos(theta_i)*3.84E2-k_f*y_f*sin(theta_f)*3.36E2+k_f*y_i*sin(theta_f)*3.36E2+k_i*y_f*sin(theta_i)*3.84E2-k_i*y_i*sin(theta_i)*3.84E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.28E2+x_f*cos(theta_f)*3.84E2-x_i*cos(theta_f)*3.84E2+y_f*sin(theta_f)*3.84E2-y_i*sin(theta_f)*3.84E2-k_i*cos(theta_f-theta_i)*1.12E2)*1.0/sqrt(x_f*x_i*-1.152E3-y_f*y_i*1.152E3+(k_f*k_f)*6.4E1+(k_i*k_i)*4.9E1+(x_f*x_f)*5.76E2+(x_i*x_i)*5.76E2+(y_f*y_f)*5.76E2+(y_i*y_i)*5.76E2+k_f*x_f*cos(theta_f)*3.84E2-k_f*x_i*cos(theta_f)*3.84E2-k_i*x_f*cos(theta_i)*3.36E2+k_i*x_i*cos(theta_i)*3.36E2+k_f*y_f*sin(theta_f)*3.84E2-k_f*y_i*sin(theta_f)*3.84E2-k_i*y_f*sin(theta_i)*3.36E2+k_i*y_i*sin(theta_i)*3.36E2-k_f*k_i*cos(theta_f-theta_i)*1.12E2)*(1.0/7.5E2)+(k_f*1.8E1-x_f*cos(theta_f)*2.16E2+x_i*cos(theta_f)*2.16E2-y_f*sin(theta_f)*2.16E2+y_i*sin(theta_f)*2.16E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*9.0+(k_i*k_i)*6.4E1+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*2.16E2+k_f*x_i*cos(theta_f)*2.16E2-k_i*x_f*cos(theta_i)*5.76E2+k_i*x_i*cos(theta_i)*5.76E2-k_f*y_f*sin(theta_f)*2.16E2+k_f*y_i*sin(theta_f)*2.16E2-k_i*y_f*sin(theta_i)*5.76E2+k_i*y_i*sin(theta_i)*5.76E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*1.28E2-x_f*cos(theta_f)*5.76E2+x_i*cos(theta_f)*5.76E2-y_f*sin(theta_f)*5.76E2+y_i*sin(theta_f)*5.76E2+k_i*cos(theta_f-theta_i)*4.8E1)*1.0/sqrt(x_f*x_i*-2.592E3-y_f*y_i*2.592E3+(k_f*k_f)*6.4E1+(k_i*k_i)*9.0+(x_f*x_f)*1.296E3+(x_i*x_i)*1.296E3+(y_f*y_f)*1.296E3+(y_i*y_i)*1.296E3-k_f*x_f*cos(theta_f)*5.76E2+k_f*x_i*cos(theta_f)*5.76E2-k_i*x_f*cos(theta_i)*2.16E2+k_i*x_i*cos(theta_i)*2.16E2-k_f*y_f*sin(theta_f)*5.76E2+k_f*y_i*sin(theta_f)*5.76E2-k_i*y_f*sin(theta_i)*2.16E2+k_i*y_i*sin(theta_i)*2.16E2+k_f*k_i*cos(theta_f-theta_i)*4.8E1)*(1.0/7.5E2)+(k_f*5.78E2-x_f*cos(theta_f)*1.836E3+x_i*cos(theta_f)*1.836E3-y_f*sin(theta_f)*1.836E3+y_i*sin(theta_f)*1.836E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*2.89E2+(k_i*k_i)*3.969E3+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3-k_f*x_f*cos(theta_f)*1.836E3+k_f*x_i*cos(theta_f)*1.836E3+k_i*x_f*cos(theta_i)*6.804E3-k_i*x_i*cos(theta_i)*6.804E3-k_f*y_f*sin(theta_f)*1.836E3+k_f*y_i*sin(theta_f)*1.836E3+k_i*y_f*sin(theta_i)*6.804E3-k_i*y_i*sin(theta_i)*6.804E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*7.938E3+x_f*cos(theta_f)*6.804E3-x_i*cos(theta_f)*6.804E3+y_f*sin(theta_f)*6.804E3-y_i*sin(theta_f)*6.804E3-k_i*cos(theta_f-theta_i)*2.142E3)*1.0/sqrt(x_f*x_i*-5.832E3-y_f*y_i*5.832E3+(k_f*k_f)*3.969E3+(k_i*k_i)*2.89E2+(x_f*x_f)*2.916E3+(x_i*x_i)*2.916E3+(y_f*y_f)*2.916E3+(y_i*y_i)*2.916E3+k_f*x_f*cos(theta_f)*6.804E3-k_f*x_i*cos(theta_f)*6.804E3-k_i*x_f*cos(theta_i)*1.836E3+k_i*x_i*cos(theta_i)*1.836E3+k_f*y_f*sin(theta_f)*6.804E3-k_f*y_i*sin(theta_f)*6.804E3-k_i*y_f*sin(theta_i)*1.836E3+k_i*y_i*sin(theta_i)*1.836E3-k_f*k_i*cos(theta_f-theta_i)*2.142E3)*6.666666666666667E-4+(k_f*9.8E1+x_f*cos(theta_f)*1.764E3-x_i*cos(theta_f)*1.764E3+y_f*sin(theta_f)*1.764E3-y_i*sin(theta_f)*1.764E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*4.9E1+(k_i*k_i)*1.089E3+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4+k_f*x_f*cos(theta_f)*1.764E3-k_f*x_i*cos(theta_f)*1.764E3-k_i*x_f*cos(theta_i)*8.316E3+k_i*x_i*cos(theta_i)*8.316E3+k_f*y_f*sin(theta_f)*1.764E3-k_f*y_i*sin(theta_f)*1.764E3-k_i*y_f*sin(theta_i)*8.316E3+k_i*y_i*sin(theta_i)*8.316E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.178E3-x_f*cos(theta_f)*8.316E3+x_i*cos(theta_f)*8.316E3-y_f*sin(theta_f)*8.316E3+y_i*sin(theta_f)*8.316E3-k_i*cos(theta_f-theta_i)*4.62E2)*1.0/sqrt(x_f*x_i*-3.1752E4-y_f*y_i*3.1752E4+(k_f*k_f)*1.089E3+(k_i*k_i)*4.9E1+(x_f*x_f)*1.5876E4+(x_i*x_i)*1.5876E4+(y_f*y_f)*1.5876E4+(y_i*y_i)*1.5876E4-k_f*x_f*cos(theta_f)*8.316E3+k_f*x_i*cos(theta_f)*8.316E3+k_i*x_f*cos(theta_i)*1.764E3-k_i*x_i*cos(theta_i)*1.764E3-k_f*y_f*sin(theta_f)*8.316E3+k_f*y_i*sin(theta_f)*8.316E3+k_i*y_f*sin(theta_i)*1.764E3-k_i*y_i*sin(theta_i)*1.764E3-k_f*k_i*cos(theta_f-theta_i)*4.62E2)*6.666666666666667E-4+(k_f*2.0-x_f*cos(theta_f)*1.2E1+x_i*cos(theta_f)*1.2E1-y_f*sin(theta_f)*1.2E1+y_i*sin(theta_f)*1.2E1+k_i*cos(theta_f-theta_i)*2.0)*1.0/sqrt(x_f*x_i*-7.2E1-y_f*y_i*7.2E1+k_f*k_f+k_i*k_i+(x_f*x_f)*3.6E1+(x_i*x_i)*3.6E1+(y_f*y_f)*3.6E1+(y_i*y_i)*3.6E1-k_f*x_f*cos(theta_f)*1.2E1+k_f*x_i*cos(theta_f)*1.2E1-k_i*x_f*cos(theta_i)*1.2E1+k_i*x_i*cos(theta_i)*1.2E1-k_f*y_f*sin(theta_f)*1.2E1+k_f*y_i*sin(theta_f)*1.2E1-k_i*y_f*sin(theta_i)*1.2E1+k_i*y_i*sin(theta_i)*1.2E1+k_f*k_i*cos(theta_f-theta_i)*2.0)*(1.0/6.0E1)+k_f*1.0/sqrt(k_f*k_f)*(1.0/3.0E1); 
 }

size_t NumConstraints() {
return 50;
}

double EvaluateConstraint(const size_t i,const arma::mat& x){
k_i = x(0,0);
double index_ensmallen = i;
double s = index_ensmallen/49;  

double v = sqrt((s*s*s*s)*pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0+(s*s)*pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+(k_i*k_i)*pow(cos(theta_i),2.0)+(k_i*k_i)*pow(sin(theta_i),2.0)+(s*s*s*s)*pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+(s*s)*pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0-(s*s*s)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1-(s*s*s)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1-k_i*s*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0-k_i*s*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*(s*s)*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0+k_i*(s*s)*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0);

double w = -(s*(k_i*sin(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0-k_i*cos(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-(s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*6.0-(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0)-k_i*sin(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*2.0+k_i*cos(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*2.0)/(-s*(k_i*sin(theta_i)*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*4.0+k_i*cos(theta_i)*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*4.0)+(k_i*k_i)*pow(cos(theta_i),2.0)+(s*s*s*s)*(pow(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i),2.0)*9.0+pow(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i),2.0)*9.0)-(s*s*s)*((x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0)*1.2E1+(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0)*1.2E1)+(s*s)*(pow(x_f*-3.0+x_i*3.0+k_f*cos(theta_f)+k_i*cos(theta_i)*2.0,2.0)*4.0+pow(y_f*-3.0+y_i*3.0+k_f*sin(theta_f)+k_i*sin(theta_i)*2.0,2.0)*4.0+k_i*sin(theta_i)*(y_f*-2.0+y_i*2.0+k_f*sin(theta_f)+k_i*sin(theta_i))*6.0+k_i*cos(theta_i)*(x_f*-2.0+x_i*2.0+k_f*cos(theta_f)+k_i*cos(theta_i))*6.0));


double constraint1 = w - tan(uplim)*v;    
double constraint2 = tan(downlim)*v - w;
double constraint3 = v*(speed-1) + diameter*speed*w;  
double constraint4 = v*(speed-1) - diameter*speed*w;

  if ( (constraint1 >0) || (constraint2 >0) || (constraint3 >0) || (constraint4 >0) ){
    return DBL_MAX; //constraint unsatisfied
  }    

return 0;
}//close EvaluateConstraint

void GradientConstraint(const size_t i,const arma::mat& x,arma::mat& g){

g.resize(2,1);
k_i = x(0,0);
double s=i/49; 
//g(0,0) = 0
//g(1,0) = 0
 }//close gradientconstraint
当我尝试使用此优化器时，我发现k_i和k_f等于Nan。但是我用Matlab的优化程序“ fmincon”对其进行了测试，并获得了一些有效的结果！所以我不知道该怎么办，我想也许我在EvaluateConstraint方法中在这里写错了，因为当我尝试打印它们的值时会发现“ Nan”。
编辑：提高初始点的值（例如x0：（500，500））“解锁”模拟，优化器不会在每次迭代中使用相同的值，它会按原样改变，但仍然没有办法。
使用Ensmallen优化时遇到问题

xxd1111 回答：使用Ensmallen优化时遇到问题

大家都在问
