21 #ifndef _MPCW_OBJ_QLD_EIGEN_H_ 22 #define _MPCW_OBJ_QLD_EIGEN_H_ 25 #include <Eigen/Eigen> 37 using Eigen::MatrixXd;
38 using Eigen::VectorXd;
70 int solve(Map<MatrixXd>& C,
const Map<VectorXd>& d,
const Map<MatrixXd>& A,
const Map<VectorXd>& b,
int me,
71 Map<VectorXd>& x,
const Map<VectorXd>& xl,
const Map<VectorXd>& xu,
bool factorizedC =
false);
76 double getEps(
void)
const {
return _eps;}
77 void setEps(
double eps) {_eps = eps;}
94 static int ql0001_(
int *m,
int *me,
int *mmax,
int *n,
int *nmax,
int *mnn,
95 double *c,
double *d,
double *a,
double *b,
double *xl,
96 double *xu,
double *x,
double *u,
int *iout,
int *ifail,
97 int *iprint,
double *war,
int *lwar,
int *iwar,
int *liwar,
99 static int ql0002_(
int *n,
int *m,
int *meq,
int *mmax,
100 int *mn,
int *mnn,
int *nmax,
102 double *a,
double *b,
double *grad,
103 double *g,
double *xl,
double *xu,
double *x,
104 int *nact,
int *iact,
int *maxit,
107 double *diag,
double *w,
114 inline static int abs(
int x) {
if (x >= 0)
return x;
else return -x;}
115 inline static double abs(
double x) {
if (x >= 0.)
return x;
else return -x;}
116 inline static int min(
int a,
int b) {
if (a <= b)
return a;
else return b;}
117 inline static int max(
int a,
int b) {
if (a >= b)
return a;
else return b;}
118 inline static double min(
double a,
double b) {
if (a <= b)
return a;
else return b;}
119 inline static double max(
double a,
double b) {
if (a >= b)
return a;
else return b;}
150 #endif //_OCRA_OBJ_QLD_H_ 288 typedef char *address;
289 typedef short int shortint;
291 typedef double doublereal;
292 typedef struct {
real r, i; } complex;
293 typedef struct { doublereal r, i; } doublecomplex;
294 typedef long int logical;
295 typedef short int shortlogical;
302 #define Extern extern 310 typedef short ftnlen;
311 typedef short ftnint;
404 typedef union Multitype Multitype;
414 typedef struct Vardesc Vardesc;
421 typedef struct Namelist Namelist;
423 #define abs(x) ((x) >= 0 ? (x) : -(x)) 424 #define dabs(x) (doublereal)abs(x) 425 #define min(a,b) ((a) <= (b) ? (a) : (b)) 426 #define max(a,b) ((a) >= (b) ? (a) : (b)) 427 #define dmin(a,b) (doublereal)min(a,b) 428 #define dmax(a,b) (doublereal)max(a,b) 432 #define F2C_proc_par_types 1 434 typedef int (*U_fp)(...);
435 typedef shortint (*J_fp)(...);
436 typedef integer (*I_fp)(...);
437 typedef real (*R_fp)(...);
438 typedef doublereal (*D_fp)(...), (*E_fp)(...);
439 typedef VOID (*C_fp)(...);
440 typedef VOID (*Z_fp)(...);
441 typedef logical (*L_fp)(...);
442 typedef shortlogical (*K_fp)(...);
443 typedef VOID (*H_fp)(...);
444 typedef int (*S_fp)(...);
446 typedef int (*U_fp)();
447 typedef shortint (*J_fp)();
448 typedef integer (*I_fp)();
449 typedef real (*R_fp)();
450 typedef doublereal (*D_fp)(), (*E_fp)();
451 typedef VOID (*C_fp)();
452 typedef VOID (*Z_fp)();
453 typedef logical (*L_fp)();
454 typedef shortlogical (*K_fp)();
455 typedef VOID (*H_fp)();
456 typedef int (*S_fp)();
462 typedef doublereal E_f;
466 #ifndef Skip_f2c_Undefs 496 #define cmache_1 cmache_ 500 static integer c__1 = 1;
509 int ql0002_(integer *n,integer *m,integer *meq,integer *mmax,
510 integer *mn,integer *mnn,integer *nmax,
512 doublereal *a,doublereal *b,doublereal *grad,
513 doublereal *g,doublereal *xl,doublereal *xu,doublereal *x,
514 integer *nact,integer *iact,integer *maxit,
517 doublereal *diag, doublereal *w,
534 int ql0001_(
int *m,
int *me,
int *mmax,
int *n,
int *nmax,
int *mnn,
535 double *c,
double *d,
double *a,
double *b,
double *xl,
536 double *xu,
double *x,
double *u,
int *iout,
int *ifail,
537 int *iprint,
double *war,
int *lwar,
int *iwar,
int *liwar,
541 int ql0001_(m, me, mmax, n, nmax, mnn, c, d, a, b, xl, xu, x,
542 u, iout, ifail, iprint, war, lwar, iwar, liwar, eps1)
543 integer *m, *me, *mmax, *n, *nmax, *mnn;
544 doublereal *c, *d, *a, *b, *xl, *xu, *x, *u;
545 integer *iout, *ifail, *iprint;
547 integer *lwar, *iwar, *liwar;
552 static char fmt_1000[] =
"(/8x,\002***QL: MATRIX G WAS ENLARGED\002,i3\ 553 ,\002-TIMES BY UNITMATRIX\002)";
554 static char fmt_1100[] =
"(/8x,\002***QL: CONSTRAINT \002,i5,\002 NOT CO\ 555 NSISTENT TO \002,/,(10x,10i5))";
556 static char fmt_1200[] =
"(/8x,\002***QL: LWAR TOO SMALL\002)";
557 static char fmt_1210[] =
"(/8x,\002***QL: LIWAR TOO SMALL\002)";
558 static char fmt_1220[] =
"(/8x,\002***QL: MNN TOO SMALL\002)";
559 static char fmt_1300[] =
"(/8x,\002***QL: TOO MANY ITERATIONS (MORE THA\ 560 N\002,i6,\002)\002)";
561 static char fmt_1400[] =
"(/8x,\002***QL: ACCURACY INSUFFICIENT TO ATTAI\ 565 integer c_dim1, c_offset, a_dim1, a_offset, i__1, i__2;
571 static doublereal diag;
573 static integer nact, info;
574 static doublereal zero;
575 static integer i, j, idiag, maxit;
576 static doublereal qpeps;
577 static integer in, mn, lw;
578 static doublereal ten;
580 static integer inw1, inw2;
583 static cilist io___16 = { 0, 0, 0, fmt_1000, 0 };
584 static cilist io___18 = { 0, 0, 0, fmt_1100, 0 };
585 static cilist io___19 = { 0, 0, 0, fmt_1200, 0 };
586 static cilist io___20 = { 0, 0, 0, fmt_1210, 0 };
587 static cilist io___21 = { 0, 0, 0, fmt_1220, 0 };
588 static cilist io___22 = { 0, 0, 0, fmt_1300, 0 };
589 static cilist io___23 = { 0, 0, 0, fmt_1400, 0 };
606 a_offset = a_dim1 + 1;
610 c_offset = c_dim1 + 1;
614 cmache_1.eps = *eps1;
620 if (fabs(c[*nmax + *nmax * c_dim1]) == 0.e0) {
621 c[*nmax + *nmax * c_dim1] = cmache_1.eps;
637 maxit = (*m + *n) * 40;
638 qpeps = cmache_1.eps;
649 for (j = 1; j <= i__1; ++j) {
655 lw = *nmax * 3 * *nmax / 2 + *nmax * 10 + *m;
656 if (inw2 + lw > *lwar) {
662 if (*mnn < *m + *n + *n) {
669 ql0002_(n, m, me, mmax, &mn, mnn, nmax, &lql, &a[a_offset], &war[inw1], &
670 d[1], &c[c_offset], &xl[1], &xu[1], &x[1], &nact, &iwar[1], &
671 maxit, &qpeps, &info, &diag, &war[inw2], &lw);
683 if (diag > zero && diag < 1e3) {
684 idiag = (integer) diag;
701 for (j = 1; j <= i__1; ++j) {
710 for (i = 1; i <= i__1; ++i) {
799 int ql0002_(integer *n,integer *m,integer *meq,integer *mmax,
800 integer *mn,integer *mnn,integer *nmax,
802 doublereal *a,doublereal *b,doublereal *grad,
803 doublereal *g,doublereal *xl,doublereal *xu,doublereal *x,
804 integer *nact,integer *iact,integer *maxit,
807 doublereal *diag, doublereal *w,
810 int ql0002_(n, m, meq, mmax, mn, mnn, nmax, lql, a, b, grad,
811 g, xl, xu, x, nact, iact, maxit, vsmall, info, diag, w, lw)
812 integer *n, *m, *meq, *mmax, *mn, *mnn, *nmax;
814 doublereal *a, *b, *grad, *g, *xl, *xu, *x;
815 integer *nact, *iact, *maxit;
818 doublereal *diag, *w;
823 integer a_dim1, a_offset, g_dim1, g_offset, i__1, i__2, i__3, i__4;
824 doublereal d__1, d__2, d__3, d__4;
831 static doublereal onha, xmag, suma, sumb, sumc, temp, step, zero;
833 static doublereal sumx, sumy;
834 static integer i, j, k;
835 static doublereal fdiff;
836 static integer iflag, jflag, kflag, lflag;
837 static doublereal diagr;
838 static integer ifinc, kfinc, jfinc, mflag, nflag;
839 static doublereal vfact, tempa;
840 static integer iterc, itref;
841 static doublereal cvmax, ratio, xmagr;
842 static integer kdrop;
843 static logical lower;
844 static integer knext, k1;
845 static doublereal ga, gb;
846 static integer ia, id;
847 static doublereal fdiffa;
848 static integer ii, il, kk, jl, ip, ir, nm, is, iu, iw, ju, ix, iz, nu, iy;
850 static doublereal parinc, parnew;
851 static integer ira, irb, iwa;
852 static doublereal one;
853 static integer iwd, iza;
854 static doublereal res;
855 static integer ipp, iwr, iws;
856 static doublereal sum;
857 static integer iww, iwx, iwy;
858 static doublereal two;
902 g_offset = g_dim1 + 1;
907 a_offset = a_dim1 + 1;
912 iwr = iwz + *nmax * *nmax;
913 iww = iwr + *nmax * (*nmax + 3) / 2;
948 for (k = 1; k <= i__1; ++k) {
951 for (i = 1; i <= i__2; ++i) {
954 d__1 = a[k + i * a_dim1];
973 sum = one / sqrt(sum);
981 for (k = 1; k <= i__1; ++k) {
994 for (i = 1; i <= i__1; ++i) {
996 w[id] = g[i + i * g_dim1];
998 d__1 = *diag, d__2 = *vsmall - w[id];
999 *diag = max(d__1,d__2);
1005 for (j = ii; j <= i__2; ++j) {
1007 d__1 = w[id], d__2 = g[j + j * g_dim1];
1008 ga = -min(d__1,d__2);
1009 gb = (d__1 = w[id] - g[j + j * g_dim1], abs(d__1)) + (d__2 = g[i
1010 + j * g_dim1], abs(d__2));
1013 d__1 = g[i + j * g_dim1];
1014 ga += d__1 * d__1 / gb;
1017 *diag = max(*diag,ga);
1022 if (*diag <= zero) {
1026 *diag = diagr * *diag;
1028 for (i = 1; i <= i__1; ++i) {
1031 g[i + i * g_dim1] = *diag + w[id];
1040 for (j = 1; j <= i__1; ++j) {
1044 for (i = 1; i <= i__2; ++i) {
1045 temp = g[i + j * g_dim1];
1050 for (k = irb; k <= i__3; ++k) {
1053 temp -= w[k] * w[ira];
1059 w[ir] = temp / w[ira];
1063 if (temp < *vsmall) {
1081 for (i = k; i <= i__1; ++i) {
1082 sum -= w[ira] * w[i];
1091 sumx += d__1 * d__1;
1095 *diag = *diag + *vsmall - temp / sumx;
1104 for (i = 1; i <= i__1; ++i) {
1106 for (j = 1; j <= i__2; ++j) {
1109 w[ir] = g[j + i * g_dim1];
1118 for (i = 1; i <= i__2; ++i) {
1124 for (j = 2; j <= i__1; ++j) {
1130 ir = iwr + (i + i * i) / 2;
1131 w[iz] = one / w[ir];
1137 for (j = i; j <= i__1; ++j) {
1142 for (k = iza; i__4 < 0 ? k >= i__3 : k <= i__3; k += i__4) {
1143 sum += w[k] * w[ir];
1149 w[iz] = -sum / w[ir];
1171 for (i = 1; i <= i__2; ++i) {
1213 for (i = 1; i <= i__2; ++i) {
1222 for (j = i; j <= i__1; ++j) {
1224 w[id] += g[i + j * g_dim1] * x[j];
1227 for (j = 1; j <= i__1; ++j) {
1230 w[iw] += g[j + i * g_dim1] * w[id];
1235 for (j = 1; j <= i__1; ++j) {
1237 w[iw] += g[i + j * g_dim1] * x[j];
1246 for (k = 1; k <= i__2; ++k) {
1254 for (i = 1; i <= i__1; ++i) {
1256 w[iw] -= w[k] * a[kk + i * a_dim1];
1258 w[is] -= x[i] * a[kk + i * a_dim1];
1268 w[is] = xl[k1] - x[k1];
1274 w[is] = -xu[k1] + x[k1];
1289 for (i = il; i <= i__2; ++i) {
1296 for (j = il; j <= i__1; ++j) {
1299 sum += w[ir] * w[j];
1304 w[i] = (w[i] - sum) / w[ir];
1311 for (i = 1; i <= i__2; ++i) {
1315 for (j = il; j <= i__1; ++j) {
1316 sum += w[j] * w[iz];
1327 for (j = i; j <= i__1; ++j) {
1329 w[id] += g[i + j * g_dim1] * sum;
1333 for (j = 1; j <= i__1; ++j) {
1336 w[iw] += g[j + i * g_dim1] * w[id];
1341 for (j = 1; j <= i__1; ++j) {
1344 w[iw] += sum * g[i + j * g_dim1];
1364 il = iws + *nact + 1;
1365 iza = iwz + *nact * *n;
1367 for (i = 1; i <= i__2; ++i) {
1371 for (j = il; j <= i__1; ++j) {
1372 sum += w[iz] * w[j];
1391 for (k = 1; k <= i__2; ++k) {
1404 if (iflag == itref) {
1415 if (w[kdrop] >= zero) {
1418 if (iact[kdrop] <= *meq) {
1425 if (kdrop < *nact) {
1439 for (k = 1; k <= i__2; ++k) {
1441 if (w[ia] <= zero) {
1446 for (i = 1; i <= i__1; ++i) {
1448 sum += x[i] * a[k + i * a_dim1];
1450 sumx = -sum * w[ia];
1454 if (sumx <= cvmax) {
1457 temp = (d__1 = b[k], abs(d__1));
1459 for (i = 1; i <= i__1; ++i) {
1461 temp += (d__1 = x[i] * a[k + i * a_dim1], abs(d__1));
1463 tempa = temp + abs(sum);
1464 if (tempa <= temp) {
1467 temp += onha * abs(sum);
1468 if (temp <= tempa) {
1479 for (k = 1; k <= i__2; ++k) {
1482 if (w[ia] <= zero) {
1488 }
else if (sum == 0) {
1514 if (cvmax <= *vsmall) {
1525 if (jfinc != ifinc) {
1531 for (i = 1; i <= i__2; ++i) {
1532 sum = two * grad[i];
1540 for (j = i; j <= i__1; ++j) {
1543 w[id] += g[i + j * g_dim1] * (w[ix] + x[j]);
1546 for (j = 1; j <= i__1; ++j) {
1548 temp = g[j + i * g_dim1] * w[id];
1556 for (j = 1; j <= i__1; ++j) {
1558 temp = g[i + j * g_dim1] * (w[ix] + x[j]);
1565 fdiff += sum * (x[i] - w[ix]);
1567 fdiffa += sumx * (d__1 = x[i] - w[ix], abs(d__1));
1570 sum = fdiffa + fdiff;
1571 if (sum <= fdiffa) {
1574 temp = fdiffa + onha * fdiff;
1582 for (i = 1; i <= i__2; ++i) {
1594 if (iterc <= *maxit) {
1600 iws = iwr + (*nact + *nact * *nact) / 2;
1605 for (i = 1; i <= i__2; ++i) {
1608 w[iw] = a[knext + i * a_dim1];
1613 for (i = 1; i <= i__2; ++i) {
1626 for (i = 1; i <= i__2; ++i) {
1639 for (i = 1; i <= i__2; ++i) {
1672 iz = iwz + *nact * *n;
1674 for (i = 1; i <= i__2; ++i) {
1677 suma += w[iw] * w[iz];
1678 sumb += (d__1 = w[iw] * w[iz], abs(d__1));
1682 sumc += d__1 * d__1;
1684 temp = sumb + abs(suma) * .1;
1685 tempa = sumb + abs(suma) * .2;
1689 if (tempa <= temp) {
1692 if (sumb > *vsmall) {
1702 temp = sumc + abs(suma) * .1;
1703 tempa = sumc + abs(suma) * .2;
1707 if (tempa <= temp) {
1730 for (i = 1; i <= i__2; ++i) {
1731 suma = a[knext + i * a_dim1];
1737 for (k = 1; k <= i__1; ++k) {
1749 temp = -w[iww + kk];
1754 temp = w[iw] * a[kk + i * a_dim1];
1761 if (suma <= *vsmall) {
1764 temp = sumb + abs(suma) * .1;
1765 tempa = sumb + abs(suma) * .2;
1769 if (tempa <= temp) {
1784 for (i = 1; i <= i__2; ++i) {
1799 for (k = 1; k <= i__1; ++k) {
1811 temp = -w[iww + kk];
1816 temp = w[iw] * a[kk + i * a_dim1];
1823 temp = sumb + abs(suma) * .1;
1824 tempa = sumb + abs(suma) * .2;
1828 if (tempa <= temp) {
1855 for (k = 1; k <= i__2; ++k) {
1857 w[k] -= parinc * w[iw];
1858 if (iact[k] > *meq) {
1860 d__1 = zero, d__2 = w[k];
1861 w[k] = max(d__1,d__2);
1878 iws = iws - *nact - 1;
1881 for (i = 1; i <= i__2; ++i) {
1897 parinc = step / sumy;
1911 temp = one - ratio / parinc;
1918 step = ratio * sumy;
1926 iwy = iwz + *nact * *n;
1928 for (i = 1; i <= i__2; ++i) {
1931 x[i] += step * w[iy];
1943 iact[*nact] = knext;
1956 if (sum < xmagr * xmag) {
1982 for (i = 1; i <= i__2; ++i) {
1985 g[i + i * g_dim1] = w[id];
2002 ir = iwr + (*nact + *nact * *nact) / 2;
2013 for (j = i; j <= i__2; ++j) {
2015 sum += w[ira] * w[iw];
2025 w[iw] = (w[is] - sum) / w[ir];
2046 for (k = 1; k <= i__2; ++k) {
2047 if (iact[k] <= *meq) {
2051 if (res * w[iw] >= zero) {
2054 temp = w[k] / w[iw];
2058 if (abs(temp) >= abs(ratio)) {
2068 switch ((
int)lflag) {
2081 ia = iwa + iact[kdrop];
2082 if (iact[kdrop] > *mn) {
2086 if (kdrop == *nact) {
2093 iz = iwz + kdrop * *n;
2094 ir = iwr + (kdrop + kdrop * kdrop) / 2;
2097 ir = ir + kdrop + 1;
2099 d__3 = (d__1 = w[ir - 1], abs(d__1)), d__4 = (d__2 = w[ir], abs(d__2));
2100 temp = max(d__3,d__4);
2102 d__1 = w[ir - 1] / temp;
2104 d__2 = w[ir] / temp;
2105 sum = temp * sqrt(d__1 * d__1 + d__2 * d__2);
2106 ga = w[ir - 1] / sum;
2112 for (i = 1; i <= i__2; ++i) {
2127 for (i = kdrop; i <= i__2; ++i) {
2128 temp = ga * w[ira] + gb * w[ira + 1];
2129 w[ira + 1] = ga * w[ira + 1] - gb * w[ira];
2138 for (i = 1; i <= i__2; ++i) {
2141 temp = ga * w[j] + gb * w[iz];
2142 w[iz] = ga * w[iz] - gb * w[j];
2149 iact[kdrop - 1] = iact[kdrop];
2150 w[kdrop - 1] = w[kdrop];
2151 if (kdrop < *nact) {
2156 switch ((
int)mflag) {
2179 if (w[is] == zero) {
2183 d__3 = (d__1 = w[is - 1], abs(d__1)), d__4 = (d__2 = w[is], abs(d__2));
2184 temp = max(d__3,d__4);
2186 d__1 = w[is - 1] / temp;
2188 d__2 = w[is] / temp;
2189 sum = temp * sqrt(d__1 * d__1 + d__2 * d__2);
2190 ga = w[is - 1] / sum;
2194 for (i = 1; i <= i__2; ++i) {
2196 temp = ga * w[iz] + gb * w[k];
2197 w[k] = ga * w[k] - gb * w[iz];
2204 switch ((
int)nflag) {
2219 for (i = 1; i <= i__2; ++i) {
2220 sum += (d__1 = x[i], abs(d__1)) * vfact * ((d__2 = grad[i], abs(d__2))
2221 + (d__3 = g[i + i * g_dim1] * x[i], abs(d__3)));
2235 xmag = max(xmag,sum);
2236 switch ((
int)jflag) {
2252 for (i = 1; i <= i__2; ++i) {
2257 for (j = jl; j <= i__1; ++j) {
2260 w[is] += w[iz] * w[j];
2263 switch ((
int)kflag) {
2271 comments from the converter: (stderr from f2c)
int solve(Map< MatrixXd > &C, const Map< VectorXd > &d, const Map< MatrixXd > &A, const Map< VectorXd > &b, int me, Map< VectorXd > &x, const Map< VectorXd > &xl, const Map< VectorXd > &xu, bool factorizedC=false)
const VectorXd & getLagrangeMultipliers(void) const
double getSizeFactor(void) const
Optimization-based Robot Controller namespace. a library of classes to write and solve optimization p...
void setSizeFactor(double f)
double getEps(void) const