Command PID::calcCommand(Pose actPose, Pose setPose){
this->actPose = actPose;
this->setPose = setPose;
calcProportional();
calcDerivative();
calcIntegrative();
return command;
}
void logicnor::calcDerivatives (void) {
nr_double_t n = getSize () - 1;
nr_double_t x;
for (int k = 0; k < n; k++) {
for (x = 0, i = 0; i < n; i++) {
x += 2 / (1 - calcTransfer (i));
}
x *= (1 - calcTransfer (k));
g[k] = -2 * n * calcDerivative (k) / x / x;
}
}
void maxCriteria(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double a,*ai;
int i;
double *lb, *rb, Q[_Nq], Ql[_Nq], *dF_dQ, D, Z, **b;
lb = ROPUD_pTK->lbound;
rb = ROPUD_pTK->rbound;
// Приведение к абсолютному значению
// Все поисковые - тетта. Соответственно, x - массив,
// содержащий относительное значение критической точки
for(i=0; i<Nq; i++)
Q[i] = x[i]*(rb[i]-lb[i]) + lb[i];
//f
set_z(ROPUD_pTK, Q);
calcModel(d, ROPUD_pTK->z, Q);
*pfc = calcCriteria();
f[0] = 0;-*pfc;
//f с чертой
for(i=0; i<Nq; i++)
Ql[i] = 0.5*(rb[i]-lb[i]) + lb[i]; // 0.5 - середина
set_z(ROPUD_pTK, Ql);
calcModel(d, ROPUD_pTK->z, Ql);
*pfc -= calcCriteria();
if(is_linearization)
{
dF_dQ = calcDerivative(d, ROPUD_pTK->z, ROPUD_pTK->b, Ql);
for(i=0; i<Nq; i++)
*pfc -= dF_dQ[i]*(Q[i] - Ql[i]);
}
//*pfc=-pow(*pfc, 2);
*pfc=-pow(*pfc, 2);
}
void upperBound(double x[],double *pfc,double f[],double g[],DOUBLE **dfdx,
int n,int m,int me,double ko[],double komod[],double teta[])
{
double D, Z;
double *lb, *rb, *Qcmp, Q[_Nq], **b;
double *pE, ETK[_Nq];
int *piE;
double *dF_dQ, *MQi;
int iz, iq;
struct T *pT;
struct TK *pTK;
int indexq, indext, indexz, indexa,/* indexd,*/ icrit, i_komod, total_krit;
//double EF0, ET0, ETW1, EUU, EKR;
//double VKCE, VKCE0;
//double dconvdF0, dconvdT0, dconvdTW1, dconvdUU, dconvdKR;
//double dT2dF0, dT2dT0, dT2dTW1, dT2dUU, dT2dKR;
//double dQHEdF0, dQHEdT0, dQHEdTW1, dQHEdUU, dQHEdKR;
//double dFwdF0, dFwdT0, dFwdTW1, dFwdUU, dFwdKR;
//double dF1dF0, dF1dT0, dF1dTW1, dF1dUU, dF1dKR;
//double dFW, dF1;
//double *a, *ai;
//struct Q *qi,*qroot=NULL;
//struct Q *qapi,*qaproot=NULL;
double d[_Nd];
int *NQcr;
double U;
int i,j,k,s,ij;
//double *numbers;
double koef;
//double *pQap=NULL;
//double *pZap=NULL;
//double gamma;
double y[_Nq],sig[_Nq],kk;
int nqcr,nqapcr;
int N;
double F;
double *pF=NULL;
*pfc=0;
//Nf = (int)komod[0];
//Nd=(int)komod[2];
//Nt=(int)komod[3];
//Nap=(int)komod[4];
//gamma=komod[5];
//Na=(int)komod[6];
pF=malloc(Nap*sizeof(double));
if(pF==NULL) exit(2);
// читаем D из komod - не как поисковые
//i_komod=7;
// заполняем массив числа крит точек по областям
//NQcr=malloc(Nt*sizeof(int));
//if(NQcr==NULL) exit(2);
////for(indext=0;indext<Nt;indext++)
//// NQcr[indext]=0;
//total_krit=0;
//for(indext=0;indext<Nt;indext++)
//{
// NQcr[indext]=(int)komod[i_komod];
// total_krit+=NQcr[indext];
// i_komod++;
//}
//qroot=malloc(sizeof(struct Q));
//if(qroot==NULL) exit(2);
//qroot->next=NULL;
//qi=qroot;
//i=0;
//nqcr=0;
//for(indext=0; indext<Nt; indext++)
//{
// icrit = NQcr[indext];
// for(indexq=0; indexq<icrit; indexq++)
// {
// qi->next = malloc(sizeof(struct Q));
// if(!qi->next) exit(2);
// qi = qi->next;
// qi->next = NULL;
// qi->F0 = komod[i_komod++];
// qi->T0 = komod[i_komod++];
// //qi->TW1 = komod[i_komod++];
// //qi->UU = komod[i_komod++];
// //qi->KR = komod[i_komod++];
// qi->T=indext;
// nqcr++;
// }
//}
//qaproot=malloc(sizeof(struct Q));
//if(qaproot==NULL) exit(2);
//qaproot->next=NULL;
//qapi=qaproot;
//i=0;
//nqapcr=0;
//for(indexq=0;indexq<Nap;indexq++)
//{
// qapi->next=malloc(sizeof(struct Q));
// if(qapi->next==NULL) exit(2);
// qapi=qapi->next;
// qapi->next=NULL;
// qapi->F0 = komod[i_komod++];
// qapi->T0 = komod[i_komod++];
// //qapi->TW1 = komod[i_komod++];
// //qapi->UU = komod[i_komod++];
// //qapi->KR = komod[i_komod++];
// qapi->T = indexq;
// nqapcr++;
//}
kk = 3.9; // Правило трех сигм
for(i=0; i<Nq; i++)
sig[i] = (start_q[i]-start_minq[i])/kk;
ij = Nd;
pT = ROPUD_pT;
while(pT=pT->pnext)
if(pT->isSoftCons)
{
for(iq=0; iq<Nq; iq++,ij++)
{
pT->slbound[iq] = x[ij];
pT->srbound[iq] = x[ij+Nq];
}
ij+=Nq;
};
// Копирование значений b
pTK = ROPUD_pTK;
while(pTK=pTK->pnext)
for(iz=0; iz<Nz; iz++)
for(iq=0; iq<Nq+1; iq++, ij++)
pTK->b[iz][iq] = x[ij];
//ограничения
//ограничения Эф Круглое
for(i=0; i<Na; i++)
f[i] = alpha[i];
pT = ROPUD_pT;
while(pT=pT->pnext)
{
if(pT->isSoftCons)
{
for (j=0, F=1; j<Nq; j++)
F *= (Fx((pT->srbound[j]-start_q[j])/sig[j]) - Fx((pT->slbound[j]-start_q[j])/sig[j]));
f[pT->NCons] -= F;
}
}
//f[0]*=20;
//f[1]*=200;
//f[3]*=200;
//f[4]*=200;
//вероятностные
i = Na;
//qi = qroot;
pT = ROPUD_pT;
for(indext=0; indext<Nt; indext++)
{
pT = pT->pnext;
// Ограничения на области по каждому параметру (правая граница больше левой)
if(pT->isSoftCons)
{
for(j=0; j<Nq; j++,i++)
f[i] = pT->slbound[j] - pT->srbound[j];
//f[i-4] /= 10.0; f[i-3] /= 10.0;
//f[i-2] *= 100.0; f[i-1] *= 1000.0;
}
//while((qi->next)&&(qi->next->T==indext))
for(icrit=0; icrit<pT->NQcr; icrit++)
{
//qi=qi->next;
//Q[2] = qi->TW1;
//Q[3] = qi->UU;
//Q[4] = qi->KR;
//Q = pT->Qcr + icrit*Nq;
Qcmp = pT->Qcr + icrit*Nq;
lb = pT->slbound;
rb = pT->srbound;
//if(pT->isSoftCons)
for(iq=0; iq<Nq; iq++)
Q[iq] = Qcmp[iq]*(rb[iq]-lb[iq]) + lb[iq];
//else
//for(iq=0; iq<Nq; iq++)
// Q[iq] = Q[iq]*(pT->rbound[iq] - pT->lbound[iq]) + pT->lbound[iq];
set_z(pT->relK, Q);
calcModel(x, pT->relK->z, Q);
//D = x[0];
//Z = pT->relK->z[0];
//switch(pT->NCons)
//{
// // Мягкие
//case 0:
// f[i] = -D-Z*Q[0]-2*Q[1]+1; //-D-Z*Q[0]-2*Q[1]+2
// break;
//case 1:
// f[i] = -2*D-2*Q[0]-Z*Q[1]+2; //-2*D-2*Q[0]-4*Z*Q[1]+5
// break;
// // Жесткие
//case 2:
// f[i] = zmin[0] - Z;
// break;
//case 3:
// f[i] = Z - zmax[0];
// break;
//}
////if(pT->isLocked) f[i] = 0;
//i++;
f[i++] = calcConstraint(pT->NCons);
//f[i-1] *= 50;
}
}
j = 0;
pTK = ROPUD_pTK;
while(pTK = pTK->pnext)
{
pE = pTK->E;
piE = pTK->iE;
for(iq=0; iq<Nq; iq++)
Q[iq] = (pTK->lbound[iq] + pTK->rbound[iq]) /2;
set_z(pTK, Q);
calcModel(x, pTK->z, Q);
//D = x[0];
//Z = pTK->z[0];
b = pTK->b;
//dF_dQ = malloc(2*sizeof(double));
//dF_dQ[0] = D + 0.5*Q[1]*b[0][0];
//dF_dQ[1] = 0.5*(Z + b[0][1]*Q[1]);
//**************************************************************
pF[j] = calcCriteria() * pTK->a;
if(is_linearization)
{
dF_dQ = calcDerivative(x, pTK->z, pTK->b, Q);
for(iq=0; iq<Nq; iq++)
pF[j] += dF_dQ[iq]*(*pE++-Q[iq]*pTK->ai[iq]);
}
//pF[j] += dF_dQ[0]*(*pE++-Q[0]*pTK->ai[0]);
//pF[j] += dF_dQ[1]*(*pE++-Q[1]*pTK->ai[1]);
//f[i++] = zmin[0] - *pTK->z;
//f[i++] = *pTK->z - zmax[0];
for(iz=0; iz<Nz*2; iz++)
f[i++] = calcConstraintOnZ(iz);
//f[i-1] *= 100;
//f[i-2] /= 100.0;
//f[i-3] *= 100;
//f[i-4] /= 100.0;
//if(D<0.6)
// D=D;
j++;
}
//if(Nap==1) *pfc = *pF;
//else
{
for(j=0;j<Nap;j++)
*pfc+=pF[j];
}
f[i++] = -*pfc;
//for(i=0; i<Nf; i++)
// //if(f[i]>0) f[i]+=1000;
// while(f[i]<0.1 && f[i]>0.00001) f[i]*=10;
//if(*pfc<0)
// printf("%f\t%f",T1,T2);
//qi=qroot;
//while(qi)
//{
// qi=qi->next;
// free(qroot);
// qroot=NULL;
// qroot=qi;
//}
//qapi=qaproot;
//while(qapi)
//{
// qapi=qapi->next;
// free(qaproot);
// qaproot=NULL;
// qaproot=qapi;
//}
//free(NQcr);
free(pF);
}