Skip to content

Commit

Permalink
- restructure data for optimization (continuing)
Browse files Browse the repository at this point in the history 
 - helper pointer for collocation


git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@19875 f25d12d1-65f4-0310-ae8a-bbce733d8d8e
  • Loading branch information
Vitalij Ruge committed Mar 31, 2014
1 parent 9c0a69e commit c6cf635
Show file tree
Hide file tree
Showing 3 changed files with 87 additions and 75 deletions.
128 changes: 73 additions & 55 deletions SimulationRuntime/c/optimization/constraints/evalfG.c
View file
Expand Up @@ -46,12 +46,12 @@

#ifdef WITH_IPOPT
#define DF_STEP(x,s) ( (fmin(fmax(1e-4*fabs(s*x),1e-8),1e-1)))
static inline int evalG11(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline int evalG12(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline int evalG13(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline int evalG21(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline int evalG22(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline int evalG23(Number *g, IPOPT_DATA_ *iData, double *x0, int i);
static inline void evalG11(Number *g, IPOPT_DATA_ *iData, int i);
static inline void evalG12(Number *g, IPOPT_DATA_ *iData, int i);
static inline void evalG13(Number *g, IPOPT_DATA_ *iData, int i);
static inline void evalG21(Number *g, IPOPT_DATA_ *iData, int i);
static inline void evalG22(Number *g, IPOPT_DATA_ *iData, int i);
static inline void evalG23(Number *g, IPOPT_DATA_ *iData, int i);
static int diff_symColoredODE(double *v, double t, IPOPT_DATA_ *iData, double **J);
static int num_diff_symColoredODE(double *v, double t, IPOPT_DATA_ *iData, double **J);
static void printMaxError(IPOPT_DATA_ *iData, double *g,double time, double * max_err , double * tt, int *xi);
Expand All @@ -64,82 +64,82 @@ Bool evalfG(Index n, double * v, Bool new_x, int m, Number *g, void * useData)
{
IPOPT_DATA_ *iData;
int i,k,j;
double *x0;

double max_err = -1;
double max_err_time = -1;
int max_err_xi = -1;

int tmp_index;

OPTIMIZER_DIM_VARS* dim;
OPTIMIZER_TIME *dtime;
OPTIMIZER_MBASE *mbase;


iData = (IPOPT_DATA_ *) useData;
dim = &iData->dim;
dtime = &iData->dtime;
mbase = &iData->mbase;


for(i=0, k=0, x0=v; i<1; ++i, x0=iData->x3){
for(i=0, k=0, mbase->x[0] = v; i<1; ++i, mbase->x[0] = mbase->x[dim->deg]){
for(j = 1; j <dim->deg+1; ++j)
mbase->x[j] = mbase->x[j-1] + dim->nv;

iData->x1 = x0 + dim->nv; /* 0 + 3 = 3;2*/
iData->x2 = iData->x1 + dim->nv; /*3 + 3 = 6;5*/
iData->x3 = iData->x2 + dim->nv; /*6 + 3 = 9*/
for(j = 0; j <dim->deg+1; ++j)
mbase->u[j] = mbase->x[j] + dim->nx;

iData->u1 = iData->x1 + dim->nx; /*3 + 2 = 5*/
iData->u2 = iData->x2 + dim->nx; /*6 + 2 = 8*/
iData->u3 = iData->x3 + dim->nx;
for(j = 0; j <2; ++j)
functionODE_(mbase->x[j], mbase->u[j], dtime->time[j], mbase->dotx[j], iData);

/*1*/
functionODE_(x0, x0 + dim->nx, dtime->time[0], iData->dotx0, iData);
functionODE_(iData->x1, iData->u1, dtime->time[1], iData->dotx1, iData);
evalG21(g + k, iData, x0, i);
evalG21(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT_ERROR))
printMaxError(iData,g,dtime->time[1],&max_err, &max_err_time, &max_err_xi);

k += dim->nJ;

/*2*/
functionODE_(iData->x2, iData->u2, dtime->time[2], iData->dotx2, iData);
evalG22(g + k, iData, x0, i);
functionODE_(mbase->x[2], mbase->u[2], dtime->time[2], mbase->dotx[2], iData);
evalG22(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT_ERROR))
printMaxError(iData,g,dtime->time[2],&max_err, &max_err_time, &max_err_xi);
k += dim->nJ;

/*3*/
functionODE_(iData->x3, iData->u3, dtime->time[3], iData->dotx3, iData);
evalG23(g + k, iData, x0, i);
functionODE_(mbase->x[3], mbase->u[3], dtime->time[3], mbase->dotx[3], iData);
evalG23(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT_ERROR))
printMaxError(iData,g,dtime->time[3],&max_err, &max_err_time, &max_err_xi);
k += dim->nJ;
}

for(; i<dim->nsi; ++i, x0=iData->x3){
iData->x1 = x0 + dim->nv; /* 0 + 3 = 3;2*/
iData->x2 = iData->x1 + dim->nv; /*3 + 3 = 6;5*/
iData->x3 = iData->x2 + dim->nv; /*6 + 3 = 9*/

iData->u1 = iData->x1 + dim->nx; /*3 + 2 = 5*/
iData->u2 = iData->x2 + dim->nx; /*6 + 2 = 8*/
iData->u3 = iData->x3 + dim->nx;
for(; i<dim->nsi; ++i, mbase->x[0] = mbase->x[dim->deg]){

tmp_index = i*dim->deg;

for(j = 1; j <dim->deg+1; ++j)
mbase->x[j] = mbase->x[j-1] + dim->nv;

for(j = 0; j <dim->deg+1; ++j)
mbase->u[j] = mbase->x[j] + dim->nx;

/*1*/
functionODE_(iData->x1, iData->u1, dtime->time[tmp_index + 1], iData->dotx1, iData);
evalG11(g + k, iData, x0, i);
functionODE_(mbase->x[1], mbase->u[1], dtime->time[tmp_index + 1], mbase->dotx[1], iData);
evalG11(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT_ERROR))
printMaxError(iData,g,dtime->time[tmp_index + 1],&max_err, &max_err_time, &max_err_xi);
k += dim->nJ;

/*2*/
functionODE_(iData->x2, iData->u2, dtime->time[tmp_index + 2], iData->dotx2, iData);
evalG12(g + k, iData, x0, i);
functionODE_(mbase->x[2], mbase->u[2], dtime->time[tmp_index + 2], mbase->dotx[2], iData);
evalG12(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT))
printMaxError(iData,g,dtime->time[tmp_index + 2],&max_err, &max_err_time, &max_err_xi);
k += dim->nJ;

/*3*/
functionODE_(iData->x3, iData->u3, dtime->time[tmp_index + 3], iData->dotx3, iData);
evalG13(g + k, iData, x0, i);
functionODE_(mbase->x[3], mbase->u[3], dtime->time[tmp_index + 3], mbase->dotx[3], iData);
evalG13(g + k, iData, i);
if(ACTIVE_STREAM(LOG_IPOPT_ERROR))
printMaxError(iData,g,dtime->time[tmp_index + 3],&max_err, &max_err_time, &max_err_xi);
k += dim->nJ;
Expand Down Expand Up @@ -257,86 +257,105 @@ int diff_symColoredODE(double *v, double t, IPOPT_DATA_ *iData, double **J)
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG11(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG11(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = & iData->mbase;

for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->mbase.a[0][0]*x0[j] + iData->mbase.a[0][3]*iData->x3[j] + iData->scalf[j]*iData->dtime.dt[i]*iData->dotx1[j]) - (iData->mbase.a[0][1]*iData->x1[j] + iData->mbase.a[0][2]*iData->x2[j]);
g[j] = (mbase->a[0][0]*mbase->x[0][j] + mbase->a[0][3]*mbase->x[3][j] + iData->scalf[j]*iData->dtime.dt[i]*mbase->dotx[1][j]) -
(mbase->a[0][1]*mbase->x[1][j] + mbase->a[0][2]*mbase->x[2][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;

}

/*!
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG12(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG12(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = &iData->mbase;

for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->mbase.a[1][1]*iData->x1[j] + iData->scalf[j]*iData->dtime.dt[i]*iData->dotx2[j]) - (iData->mbase.a[1][0]*x0[j] + iData->mbase.a[1][2]*iData->x2[j] + iData->mbase.a[1][3]*iData->x3[j]);
g[j] = (mbase->a[1][1]*mbase->x[1][j] +
iData->scalf[j]*iData->dtime.dt[i]*mbase->dotx[2][j])
- (mbase->a[1][0]*mbase->x[0][j] + mbase->a[1][2]*mbase->x[2][j] +
mbase->a[1][3]*mbase->x[3][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;

}

/*!
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG13(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG13(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = &iData->mbase;

for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->mbase.a[2][0]*x0[j] + iData->mbase.a[2][2]*iData->x2[j] + iData->scalf[j]*iData->dtime.dt[i]*iData->dotx3[j]) - (iData->mbase.a[2][1]*iData->x1[j] + iData->mbase.a[2][3]*iData->x3[j]);
g[j] = (mbase->a[2][0]*mbase->x[0][j] +
mbase->a[2][2]*mbase->x[2][j] +
iData->scalf[j]*iData->dtime.dt[i]*mbase->dotx[3][j]) -
(mbase->a[2][1]*mbase->x[1][j] + mbase->a[2][3]*mbase->x[3][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;

}

/*!
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG21(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG21(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = &iData->mbase;

for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*(iData->dotx1[j] + iData->mbase.d[0][4]*iData->dotx0[j]) + iData->mbase.d[0][0]*x0[j] + iData->mbase.d[0][3]*iData->x3[j]) - (iData->mbase.d[0][1]*iData->x1[j] + iData->mbase.d[0][2]*iData->x2[j]);
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*(mbase->dotx[1][j] + mbase->d[0][4]*mbase->dotx[0][j])
+ mbase->d[0][0]*mbase->x[0][j] + mbase->d[0][3]*mbase->x[3][j])
- (mbase->d[0][1]*mbase->x[1][j] + mbase->d[0][2]*mbase->x[2][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;

}

/*!
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG22(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG22(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = &iData->mbase;
for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*iData->dotx2[j] + iData->mbase.d[1][1]*iData->x1[j]) - (iData->scalf[j]*iData->dtime.dt[i]*iData->mbase.d[1][4]*iData->dotx0[j] + iData->mbase.d[1][0]*x0[j] + iData->mbase.d[1][2]*iData->x2[j] + iData->mbase.d[1][3]*iData->x3[j]);
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*mbase->dotx[2][j] + mbase->d[1][1]*mbase->x[1][j])
- (iData->scalf[j]*iData->dtime.dt[i]*mbase->d[1][4]*mbase->dotx[0][j] + mbase->d[1][0]*mbase->x[0][j] +
mbase->d[1][2]*mbase->x[2][j]
+ mbase->d[1][3]*mbase->x[3][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;

}

/*!
* helper evalfG
* author: Vitalij Ruge
**/
static inline int evalG23(Number *g, IPOPT_DATA_ *iData, double *x0, int i)
static inline void evalG23(Number *g, IPOPT_DATA_ *iData, int i)
{
int j;
OPTIMIZER_MBASE *mbase = &iData->mbase;

for(j=0; j<iData->dim.nx; ++j)
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*(iData->mbase.d[2][4]*iData->dotx0[j] + iData->dotx3[j]) + iData->mbase.d[2][0]*x0[j] + iData->mbase.d[2][2]*iData->x2[j]) - (iData->mbase.d[2][1]*iData->x1[j] + iData->mbase.d[2][3]*iData->x3[j]);
g[j] = (iData->scalf[j]*iData->dtime.dt[i]*(mbase->d[2][4]*mbase->dotx[0][j] + mbase->dotx[3][j])
+ mbase->d[2][0]*mbase->x[0][j] + mbase->d[2][2]*mbase->x[2][j]) -
(mbase->d[2][1]*mbase->x[1][j] + mbase->d[2][3]*mbase->x[3][j]);

memcpy(g + iData->dim.nx, &iData->data->localData[0]->realVars[iData->data->modelData.nVariablesReal - iData->dim.nc], sizeof(double)*iData->dim.nc);
return 0;
Expand All @@ -350,7 +369,6 @@ static void printMaxError(IPOPT_DATA_ *iData, double *g,double t, double * max_e

for(j = 0; j<(int)iData->dim.nx; ++j){
tmp = fabs(g[j]);
//printf("\n time %g vs. %g | %g vs. %g",t,*tt,tmp,*max_err);
if((double) tmp > (double)*max_err){
*max_err = tmp;
*tt = t;
Expand Down
17 changes: 9 additions & 8 deletions SimulationRuntime/c/optimization/initialOptimizer/allocate_optimizer_vars.c
View file
Expand Up @@ -82,10 +82,10 @@ int allocateIpoptData(IPOPT_DATA_ *iData)
iData->mult_x_U = (double*)malloc(dim->NV*sizeof(double));
iData->lhs = (double*)malloc((int)dim->nJ*sizeof(double));
iData->rhs = (double*)malloc((int)dim->nJ*sizeof(double));
iData->dotx0 = (double*)malloc(dim->nx*sizeof(double));
iData->dotx1 = (double*)malloc(dim->nx*sizeof(double));
iData->dotx2 = (double*)malloc(dim->nx*sizeof(double));
iData->dotx3 = (double*)malloc(dim->nx*sizeof(double));

for(i =0; i< dim->deg + 1; ++i)
mbase->dotx[i] = (double*)malloc(dim->nx*sizeof(double));

iData->xmin = (double*)malloc(dim->nx*sizeof(double));
iData->xmax = (double*)malloc(dim->nx*sizeof(double));
iData->umin = (double*)malloc(dim->nu*sizeof(double));
Expand Down Expand Up @@ -218,10 +218,11 @@ static int freeIpoptData(IPOPT_DATA_ *iData)
free(iData->mult_g);
free(iData->mult_x_L);
free(iData->mult_x_U);
free(iData->dotx0);
free(iData->dotx1);
free(iData->dotx2);
free(iData->dotx3);

for(i = 0; i<dim->deg+1; ++i)
free(mbase->dotx[i]);


free(iData->xmin);
free(iData->xmax);
free(iData->umin);
Expand Down
17 changes: 5 additions & 12 deletions SimulationRuntime/c/optimization/ipoptODEstruct.h
View file
Expand Up @@ -90,6 +90,11 @@ typedef struct OPTIMIZER_MBASE{

long double invd1_4;

double *dotx[5];

double *x[5];
double *u[5];

}OPTIMIZER_MBASE;

typedef struct OPTIMIZER_TIME{
Expand Down Expand Up @@ -169,18 +174,6 @@ typedef struct IPOPT_DATA_
double *v;
double *w;
double *cv;
double *dotx0;
double *dotx1;
double *dotx2;
double *dotx3;

double *x1;
double *x2;
double *x3;

double *u1;
double *u2;
double *u3;

double *gmin;
double *gmax;
Expand Down

0 comments on commit c6cf635

Please sign in to comment.