robotc.c

sqp程序包。用sqp算法实现非线性约束的优化求解

/*              compute the gradient of the i-th inequality constraint          */
/*          not necessary for bounds, but constant gradients must be set        */
/*                      here e.g. using dcopy from a data-field                 */
/* **************************************************************************** */
void egradg(INTEGER i,DOUBLE x[],DOUBLE gradx[]) {
    #define  X extern
    #include "o8fuco.h"
    #undef   X

    static INTEGER ip,k,j;
    static DOUBLE  sum,sum1,sum2;
    
    if ( gunit[1][i+nh] != 1 ) cgres[i+nh] = cgres[i+nh]+1;
    
    ip   = (i-1)/18+1;
    k    = i-18*(ip-1);
    sum  = 0.e0;
    sum1 = 0.e0;
    sum2 = 0.e0;
    for (j = 1 ; j <= n ; j++) {
        sum  = sum +vi [ip][j]*x[j];
        sum1 = sum1+vi1[ip][j]*x[j];
        sum2 = sum2+vi2[ip][j]*x[j];
    }
    switch (k) {
    case 1:
    case 2:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = c11*vi[ip][j];
        }
        return;
    case 3:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c12*pow(sum,2)*vi[ip][j]-(v12[ip]*vi[ip][j]
                      -v11[ip]*vi1[ip][j]);
        }
        return;
    case 4:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c12*pow(sum,2)*vi[ip][j]+(v12[ip]*vi[ip][j]
                      -v11[ip]*vi1[ip][j]);
        }
        return;
    case 5:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c13*pow(sum,4)*vi[ip][j]-(2.e0*v13[ip]*sum*
                      vi[ip][j]-3.e0*v12[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v11[ip]*sum1*vi1[ip][j]+v11[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    case 6:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c13*pow(sum,4)*vi[ip][j]+(2.e0*v13[ip]*sum*
                      vi[ip][j]-3.e0*v12[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v11[ip]*sum1*vi1[ip][j]+v11[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    case 7:
    case 8:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = c21*vi[ip][j];
        }
        return;
    case 9:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c22*pow(sum,2)*vi[ip][j]-(v22[ip]*vi[ip][j]
                      -v21[ip]*vi1[ip][j]);
        }
        return;
    case 10:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c22*pow(sum,2)*vi[ip][j]+(v22[ip]*vi[ip][j]
                      -v21[ip]*vi1[ip][j]);
        }
        return;
    case 11:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c23*pow(sum,4)*vi[ip][j]-(2.e0*v23[ip]*sum*
                      vi[ip][j]-3.e0*v22[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v21[ip]*sum1*vi1[ip][j]+v21[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    case 12:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c23*pow(sum,4)*vi[ip][j]+(2.e0*v23[ip]*sum*
                      vi[ip][j]-3.e0*v22[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v21[ip]*sum1*vi1[ip][j]+v21[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    case 13:
    case 14:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = c31*vi[ip][j];
        }
        return;
    case 15:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c32*pow(sum,2)*vi[ip][j]-(v32[ip]*vi[ip][j]
                      -v31[ip]*vi1[ip][j]);
        }
        return;
    case 16:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 3.e0*c32*pow(sum,2)*vi[ip][j]+(v32[ip]*vi[ip][j]
                      -v31[ip]*vi1[ip][j]);
        }
        return;
    case 17:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c33*pow(sum,4)*vi[ip][j]-(2.e0*v33[ip]*sum*
                      vi[ip][j]-3.e0*v32[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v31[ip]*sum1*vi1[ip][j]+v31[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    case 18:
        for (j = 1 ; j <= n ; j++) {
            gradx[j] = 5.e0*c33*pow(sum,4)*vi[ip][j]+(2.e0*v33[ip]*sum*
                      vi[ip][j]-3.e0*v32[ip]*(sum1*vi[ip][j]+sum*vi1[ip][j])
                      +6.e0*v31[ip]*sum1*vi1[ip][j]+v31[ip]*
                      (sum2*vi[ip][j]+sum*vi2[ip][j]));
        }
        return;
    }
}
DOUBLE f(DOUBLE t) {
    return pow(t,3)*((6.e0*t-15.e0)*t+10.e0);
}
DOUBLE f1(DOUBLE t) {
    return 30.e0*t*t*((t-2.e0)*t+1.e0);
}
DOUBLE f2(DOUBLE t) {
    return 60.e0*t*((2.e0*t-3.e0)*t+1.e0);
}
DOUBLE f3(DOUBLE t) {
    return (360.e0*t-360.e0)*t+60.e0;
}
DOUBLE th11(DOUBLE t) {
    DOUBLE f1(DOUBLE t);
    
    return 1.5e0*f1(t);
}
DOUBLE th12(DOUBLE t) {
    DOUBLE f2(DOUBLE t);
    
    return 1.5e0*f2(t);
}
DOUBLE th13(DOUBLE t) {
    DOUBLE f3(DOUBLE t);
    
    return 1.5e0*f3(t);
}
DOUBLE th21(DOUBLE t) {
    DOUBLE f(DOUBLE t);
    DOUBLE f1(DOUBLE t);
    
    return -.5e0*(cos(4.7e0*f(t))*4.7e0*f1(t));
}
DOUBLE th22(DOUBLE t) {
    DOUBLE f(DOUBLE t);
    DOUBLE f1(DOUBLE t);
    DOUBLE f2(DOUBLE t);

    return -.5e0*(-sin(4.7e0*f(t))*pow(4.7e0*f1(t),2)+cos(4.7e0*f(t))*
    4.7e0*f2(t));
}
DOUBLE th23(DOUBLE t) {
    DOUBLE f(DOUBLE t);
    DOUBLE f1(DOUBLE t);
    DOUBLE f2(DOUBLE t);
    DOUBLE f3(DOUBLE t);

    return -.5e0*(-cos(4.7e0*f(t))*pow(4.7e0*f1(t),3)-sin(4.7e0*f(t))*
    3.e0*pow(4.7e0,2)*f1(t)*f2(t)+cos(4.7e0*f(t))*f3(t)*4.7e0);
}
DOUBLE th31(DOUBLE t) {
    DOUBLE f1(DOUBLE t);

    return -1.3e0*f1(t);
}
DOUBLE th32(DOUBLE t) {
    DOUBLE f2(DOUBLE t);

    return -1.3e0*f2(t);
}
DOUBLE th33(DOUBLE t) {
    DOUBLE f3(DOUBLE t);

    return -1.3e0*f3(t);
}
DOUBLE phil(INTEGER i, DOUBLE t) {
    #define  X extern
    #include "o8fuco.h"
    #undef   X
    
    static DOUBLE  h;
    static INTEGER k;
    
    h = 1.e0/(DOUBLE)(n-1);
    k = t/h+1.e-6+1;
    if ( i <= k-2 || i >= k+3 ) {
        return 0.e0;
    } else {
        switch (k-i+3) {
        case 1: 
            return pow(t-(DOUBLE)(k-1)*h,3)/6.e0/pow(h,3);
        case 2:
            return 1.e0/6.e0+(t-(DOUBLE)(k-1)*h)*.5e0/h
                    +pow(t-(DOUBLE)(k-1)*h,2)*.5e0/pow(h,2)
                    -pow(t-(DOUBLE)(k-1)*h,3)*.5e0/pow(h,3);
        case 3:
            return 1.e0/6.e0+((DOUBLE)(k)*h-t)*.5e0/h
                    +pow((DOUBLE)(k)*h-t,2)*.5e0/pow(h,2)
                    -pow((DOUBLE)(k)*h-t,3)*.5e0/pow(h,3);
        case 4:
            return pow((DOUBLE)(k)*h-t,3)/6.e0/pow(h,3);
        }
    }
    return 0.;
}
DOUBLE phil1(INTEGER i, DOUBLE t) {
    #define  X extern
    #include "o8fuco.h"
    #undef   X
    
    static INTEGER k;
    static DOUBLE  h;
    
    h = 1.e0/(DOUBLE)(n-1);
    k = t/h+1.e-6+1;
    if ( i <= k-2 || i >= k+3 ) {
        return 0.e0;
    } else {
        switch (k-i+3) {
        case 1:
            return pow(t-(DOUBLE)(k-1)*h,2)/2.e0/pow(h,3);
        case 2:
            return .5e0/h+(t-(DOUBLE)(k-1)*h)/pow(h,2)
                    -pow(t-(DOUBLE)(k-1)*h,2)*1.5e0/pow(h,3);
        case 3:
            return -.5e0/h-((DOUBLE)(k)*h-t)/pow(h,2)
                    +pow((DOUBLE)(k)*h-t,2)*1.5e0/pow(h,3);
        case 4:
            return -pow((DOUBLE)(k)*h-t,2)/2.e0/pow(h,3);
        }
    }
    return 0.;
}
DOUBLE phil2(INTEGER i, DOUBLE t) {
    #define  X extern
    #include "o8fuco.h"
    #undef   X
    
    static INTEGER k;
    static DOUBLE  h;
    
    h = 1.e0/(DOUBLE)(n-1);
    k = t/h+1.e-6+1;
    if ( i <= k-2 || i >= k+3 ) {
        return 0.e0;
    } else {
        switch (k-i+3) {
        case 1:
            return (t-(DOUBLE)(k-1)*h)/pow(h,3);
        case 2:
            return 1.e0/pow(h,2)-(t-(DOUBLE)(k-1)*h)*3.e0/pow(h,3);
        case 3:
            return 1.e0/pow(h,2)-((DOUBLE)(k)*h-t)*3.e0/pow(h,3);
        case 4:
            return ((DOUBLE)(k)*h-t)/pow(h,3);
        }
    }
    return 0.;
}

/* **************************************************************************** */
/*                        user functions (if bloc == TRUE)                      */
/* **************************************************************************** */
void eval_extern(INTEGER mode) {
    #define  X extern
    #include "o8comm.h"
    #include "o8fint.h"
    #undef   X
    #include "o8cons.h"

    return;
}