Example #1
0
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;
  }
}
Example #3
0
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);
}
Example #4
0
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);
}