bool readSLParameterPoolData(std::string filename, std::string keyword, int n_values, std::vector<double>& values)
{
int i,rc;
FILE *in;
char key[keyword.size()];
strcpy(key, keyword.c_str());
in = fopen_strip(filename.c_str());
if (in == NULL) {
printf("ERROR: Cannot open file >%s<!\n",filename.c_str());
return FALSE;
}
// find keyword
if (!find_keyword(in, key)) {
printf("ERROR: cannot find keyword %s\n", key);
fclose(in);
return FALSE;
} else {
for (i=1; i<=n_values; ++i) {
rc=fscanf(in,"%lf",&(values[i]));
if (rc != 1)
{
printf("ERROR rc != 1\n");
return FALSE;
}
}
}
fclose(in);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_pp
\date June 1999
\remarks
initialize the post processing
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] name : name of the post processing file
******************************************************************************/
int
init_pp(char *name)
{
int i,j,n,rc;
char string[100];
FILE *in;
/* get the post processing information */
sprintf(string,"%s%s",PREFS,name);
in = fopen_strip(string);
if (in == NULL) {
printf("ERROR: Cannot open file >%s<!\n",string);
return FALSE;
}
/* find all blobs and read their information */
sprintf(string,"PREDICT");
if (find_keyword(in, string)) {
rc=fscanf(in,"%lf",&predict);
} else {
predict = 0.0;
}
for (i=1; i<= max_blobs; ++i) {
sprintf(string,"%s_KALMAN",blob_names[i]);
if (find_keyword(in, string)) {
blobpp[i].filter_type = KALMAN;
for (j= _X_; j<= _Z_; ++j)
for (n= _X_; n<= _Z_; ++n)
rc=fscanf(in,"%lf",&(blobpp[i].kal[j][n]));
} else {
for (j= _X_; j<= _Z_; ++j)
for (n= _X_; n<= _Z_; ++n)
blobpp[i].kal[j][n]=1.0;
}
sprintf(string,"%s_BUTTER",blob_names[i]);
if (find_keyword(in, string)) {
blobpp[i].filter_type = BUTTER;
for (j= _X_; j<= _Z_; ++j)
for (n= _X_; n<= _Z_; ++n)
rc=fscanf(in,"%d",&(blobpp[i].fblob[j][n].cutoff));
} else {
for (j= _X_; j<= _Z_; ++j)
for (n= _X_; n<= _Z_; ++n)
blobpp[i].fblob[j][n].cutoff=100;
}
sprintf(string,"%s_ACCELERATION",blob_names[i]);
if (find_keyword(in, string)) {
for (j= _X_; j<= _Z_; ++j)
rc=fscanf(in,"%lf",&(blobpp[i].acc[j]));
} else {
for (j= _X_; j<= _Z_; ++j)
blobpp[i].acc[j]=NOT_USED;
}
}
/* do any blobs coincide with the endeffector? */
sprintf(string,"ENDEFFECTOR");
if (find_keyword(in, string)) {
for (i=1; i<=max_blobs; ++i) {
rc=fscanf(in,"%d",&blob_is_endeffector[i]);
if (blob_is_endeffector[i] > n_endeffs || blob_is_endeffector[i] < 0)
blob_is_endeffector[i] = FALSE;
}
} else {
for (i=1; i<=max_blobs; ++i)
blob_is_endeffector[i]=FALSE;
}
learn_transformation_flag = FALSE;
for (i=1; i<=max_blobs; ++i) {
if (blob_is_endeffector[i])
learn_transformation_flag = TRUE;
}
fclose(in);
remove_temp_file();
/* keep track of which post processing script we are using */
strcpy(current_pp_name,name);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note read_sine_script
\date June 1999
\remarks
parse a script which describes the sine task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
read_sine_script(void)
{
int j,i,rc;
static char string[100];
static char fname[100] = "default.sine";
FILE *fp;
int found = FALSE;
double total_amp;
double ratio;
/* clear the current parameters */
for (i=1; i<=n_dofs; ++i) {
off[i] = joint_default_state[i].th;
n_sine[i] = 0;
for (j=1; j<=MAX_SINE; ++j) {
amp[i][j] = phase[i][j] = freq[i][j] = 0.0;
}
}
/* open the script, and parse the parameters */
while (TRUE) {
if (!get_string("Name of the Sine Script File\0",fname,fname))
return FALSE;
/* try to read this file */
sprintf(string,"%s%s",PREFS,fname);
fp = fopen_strip(string);
if (fp != NULL)
break;
}
for (i=1; i<= n_dofs; ++i) {
if (find_keyword(fp, &(joint_names[i][0]))) {
found = TRUE;
total_amp = 0.0;
rc=fscanf(fp,"%d %lf",&n_sine[i],&off[i]);
/* check for out of range */
if (off[i] > joint_range[i][MAX_THETA]) {
off[i] = joint_range[i][MAX_THETA];
printf("Reduced offset of joint %s to %f\n",joint_names[i],off[i]);
}
if (off[i] < joint_range[i][MIN_THETA]) {
off[i] = joint_range[i][MIN_THETA];
printf("Reduced offset of joint %s to %f\n",joint_names[i],off[i]);
}
for (j=1; j<=n_sine[i]; ++j) {
rc=fscanf(fp,"%lf %lf %lf",&(amp[i][j]),&(phase[i][j]),&(freq[i][j]));
total_amp += amp[i][j];
}
/* check for out of range */
if (total_amp+off[i] > joint_range[i][MAX_THETA]) {
ratio = total_amp/(joint_range[i][MAX_THETA]-off[i]+1.e-10);
for (j=1; j<=n_sine[i]; ++j)
amp[i][j] /= ratio;
printf("Reduced amplitude of joint %s to %f\n",joint_names[i],
amp[i][j]);
}
if (-total_amp+off[i] < joint_range[i][MIN_THETA]) {
ratio = total_amp/(off[i]-joint_range[i][MIN_THETA]+1.e-10);
for (j=1; j<=n_sine[i]; ++j)
amp[i][j] /= ratio;
printf("Reduced amplitude of joint %s to %f\n",joint_names[i],
amp[i][j]);
}
}
}
fclose(fp);
remove_temp_file();
return found;
}
void
read_script(void)
{
int i,j,k,m,rc;
char fname[100];
double dummy;
int idummy;
FILE *in,*out;
int n_train_data_columns;
int n_test_data_columns;
Vector row;
char identification_string[100];
char string[100];
int num;
char vnames[50][100];
int ans = 0;
double o_noise, c_noise;
int old_indx_flag = FALSE;
Matrix D_train=NULL;
char **vnames_train=NULL;
char **units_train=NULL;
double freq_train;
int n_cols_train;
int n_rows_train;
Matrix D_test=NULL;
char **vnames_test=NULL;
char **units_test=NULL;
double freq_test;
int n_cols_test;
int n_rows_test;
/* I need the filename of the script file: first check whether the
user provided it in the argv_global variables */
if (argc_global > 0) {
in = fopen(argv_global[1],"r");
if (in != NULL) {
fclose(in);
strcpy(fname,argv_global[1]);
} else {
if (!getFile(fname)) exit(-1);
}
} else {
if (!getFile(fname)) exit(-1);
}
/* this allows to generate the LWPR */
if (argc_global > 1) {
sscanf(argv_global[2],"%d",&new_model);
} else {
get_int("Generate new LWPR = 1; Read from file = 0",ans,&ans);
if (ans) new_model = TRUE;
}
if (!readLWPRScript(fname,new_model,LWPR1)) {
fprintf(stderr,"Error when reading script file >%s<\n",fname);
exit(-1);
}
/* now read additional variables from the script */
in = fopen_strip(fname);
/* check for included files */
if (find_keyword(in,"include")) {
char fname_include[100];
FILE *fp_include;
rc=fscanf(in,"%s",fname_include);
fp_include = fopen_strip(fname_include);
fseek(fp_include, 0, SEEK_END);
rewind(in);
while ((rc=fgetc(in)) != EOF)
fputc(rc,fp_include);
fclose(in);
in = fp_include;
rewind(in);
}
/* All the names in file will be parsed. Here I define the names of the
variables. Note that the variables defining the dimensionality of
the LWPR must come first since we need them to allocate other
variables */
i=0;
/* this block has all variables needed to create a LWPR */
sprintf(vnames[++i],"n_in_w");
sprintf(vnames[++i],"n_in_reg");
sprintf(vnames[++i],"n_out");
sprintf(vnames[++i],"lwpr_name");
sprintf(vnames[++i],"n_in_reg_2nd");
sprintf(vnames[++i],"n_out_2nd");
/* this block specifies all variables needed to get the data
for training and testing, as well as some other parameters */
sprintf(vnames[++i],"sampling_method");
sprintf(vnames[++i],"index_function");
sprintf(vnames[++i],"max_iterations");
sprintf(vnames[++i],"eval_time");
sprintf(vnames[++i],"cutoff");
sprintf(vnames[++i],"blending");
sprintf(vnames[++i],"file_name_train_data");
sprintf(vnames[++i],"file_name_test_data");
sprintf(vnames[++i],"name_train_in_w_columns");
sprintf(vnames[++i],"name_train_in_reg_columns");
sprintf(vnames[++i],"name_train_out_columns");
sprintf(vnames[++i],"name_test_in_w_columns");
sprintf(vnames[++i],"name_test_in_reg_columns");
sprintf(vnames[++i],"name_test_out_columns");
sprintf(vnames[++i],"name_train_in_reg_2nd_columns");
sprintf(vnames[++i],"name_train_out_2nd_columns");
sprintf(vnames[++i],"name_test_in_reg_2nd_columns");
sprintf(vnames[++i],"name_test_out_2nd_columns");
out = fopen("lwpr_test_varnames","w");
for (j=1; j<=i; ++j)
fprintf(out,"%s\n",vnames[j]);
fclose(out);
remove_temp_file();
/* parse keywords */
i = 0;
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%d",&n_in_w);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%d",&n_in_reg);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%d",&n_out);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%s",lwpr_name);
if (!find_keyword(in,vnames[++i])) {
if (lwprs[LWPR1].use_reg_2nd) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
} else {
rc=fscanf(in,"%d",&n_in_reg_2nd);
}
if (!find_keyword(in,vnames[++i])) {
if (lwprs[LWPR1].use_reg_2nd) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
} else {
rc=fscanf(in,"%d",&n_out_2nd);
}
/* at last the parameters need to steer the training and testing of
the LWPR */
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%d",&sampling_method);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%d",&index_function);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%ld",&max_iterations);
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
rc=fscanf(in,"%ld",&eval_time);
if (find_keyword(in,vnames[++i])) {
rc=fscanf(in,"%lf",&cutoff);
}
if (find_keyword(in,vnames[++i])) {
rc=fscanf(in,"%d",&blending);
}
if (!find_keyword(in,vnames[++i]) && argc_global <= 2) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
if (argc_global > 2) {
strcpy(fname_train_data,argv_global[3]);
} else {
rc=fscanf(in,"%s",fname_train_data);
}
if (!find_keyword(in,vnames[++i]) && argc_global <= 3) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
if (argc_global > 3) {
strcpy(fname_test_data,argv_global[4]);
} else {
rc=fscanf(in,"%s",fname_test_data);
}
// at this point the data files can be read -- they are expected to be in
// MRDPLOT format
printf("Reading training data from >%s<...",fname_train_data);
if (!mrdplot_convert(fname_train_data, &D_train, &vnames_train, &units_train,
&freq_train, &n_cols_train, &n_rows_train)) {
printf("Problems reading MRDPLOT file >%s<\n",fname_train_data);
exit(-999);
}
printf("done\n");
printf("%d rows with %d columns read\n",n_rows_train,n_cols_train);
printf("Reading test data from >%s<...",fname_test_data);
if (!mrdplot_convert(fname_test_data, &D_test, &vnames_test, &units_test,
&freq_test, &n_cols_test, &n_rows_test)) {
printf("Problems reading MRDPLOT file >%s<\n",fname_test_data);
exit(-999);
}
printf("done\n");
printf("%d rows with %d columns read\n",n_rows_test,n_cols_test);
// allocate memory for all arrays
Xw_train = my_matrix(1,n_rows_train,1,n_in_w);
Xreg_train = my_matrix(1,n_rows_train,1,n_in_reg);
Xreg_train_2nd = my_matrix(1,n_rows_train,1,n_in_reg_2nd);
Xw_test = my_matrix(1,n_rows_test,1,n_in_w);
Xreg_test = my_matrix(1,n_rows_test,1,n_in_reg);
Xreg_test_2nd = my_matrix(1,n_rows_test,1,n_in_reg_2nd);
Y_train = my_matrix(1,n_rows_train,1,n_out);
Y_train_2nd = my_matrix(1,n_rows_train,1,n_out_2nd);
Y_test = my_matrix(1,n_rows_test,1,n_out);
Y_test_2nd = my_matrix(1,n_rows_test,1,n_out_2nd);
x_w = my_vector(1,n_in_w);
x_reg = my_vector(1,n_in_reg);
x_reg_2nd = my_vector(1,n_in_reg_2nd);
y = my_vector(1,n_out);
y_2nd = my_vector(1,n_out_2nd);
conf = my_vector(1,n_out);
conf_2nd = my_vector(1,n_out_2nd);
var_y = my_vector(1,n_out);
var_y_2nd = my_vector(1,n_out_2nd);
mean_y = my_vector(1,n_out);
mean_y_2nd = my_vector(1,n_out_2nd);
// sort the test and training data into the appropriate arrays
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_w; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_train,n_cols_train))) {
printf("Couldn't find column >%s< in training data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_train; ++m)
Xw_train[m][j] = D_train[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_reg; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_train,n_cols_train))) {
printf("Couldn't find column >%s< in training data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_train; ++m)
Xreg_train[m][j] = D_train[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_out; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_train,n_cols_train))) {
printf("Couldn't find column >%s< in training data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_train; ++m)
Y_train[m][j] = D_train[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_w; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_test,n_cols_test))) {
printf("Couldn't find column >%s< in test data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_test; ++m)
Xw_test[m][j] = D_test[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_reg; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_test,n_cols_test))) {
printf("Couldn't find column >%s< in test data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_test; ++m)
Xreg_test[m][j] = D_test[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_out; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_test,n_cols_test))) {
printf("Couldn't find column >%s< in test data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_test; ++m)
Y_test[m][j] = D_test[m][k];
}
}
if (lwprs[LWPR1].use_reg_2nd) {
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_reg_2nd; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_train,n_cols_train))) {
printf("Couldn't find column >%s< in training data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_train; ++m)
Xreg_train_2nd[m][j] = D_train[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_out_2nd; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_train,n_cols_train))) {
printf("Couldn't find column >%s< in training data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_train; ++m)
Y_train_2nd[m][j] = D_train[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_in_reg_2nd; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_test,n_cols_test))) {
printf("Couldn't find column >%s< in test data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_test; ++m)
Xreg_test_2nd[m][j] = D_test[m][k];
}
}
if (!find_keyword(in,vnames[++i])) {
printf("Could not find variable >%s<\n",vnames[i]);
exit(-i);
}
for (j=1; j<=n_out_2nd; ++j) {
rc=fscanf(in,"%s",string);
if (!(k=findIndex(string,vnames_test,n_cols_test))) {
printf("Couldn't find column >%s< in test data\n",string);
exit(-i);
} else {
for (m=1; m<=n_rows_test; ++m)
Y_test_2nd[m][j] = D_test[m][k];
}
}
}
fclose(in);
n_train_data = n_rows_train;
n_test_data = n_rows_test;
if (NORMALIZE_BY_TRAIN) {
for (i=1; i<=n_train_data; ++i) {
vec_add_size(mean_y,Y_train[i],n_out,mean_y);
}
vec_mult_scalar(mean_y,1./(double)n_train_data,mean_y);
for (i=1; i<=n_train_data; ++i) {
for (j=1; j<=n_out; ++j) {
var_y[j] += sqr(Y_train[i][j]-mean_y[j]);
}
}
vec_mult_scalar(var_y,1./(double)n_train_data,var_y);
if (lwprs[LWPR1].use_reg_2nd) {
for (i=1; i<=n_train_data; ++i) {
vec_add_size(mean_y_2nd,Y_train_2nd[i],n_out,mean_y_2nd);
}
vec_mult_scalar(mean_y_2nd,1./(double)n_train_data,mean_y_2nd);
for (i=1; i<=n_train_data; ++i) {
for (j=1; j<=n_out_2nd; ++j) {
var_y_2nd[j] += sqr(Y_train_2nd[i][j]-mean_y_2nd[j]);
}
}
vec_mult_scalar(var_y_2nd,1./(double)n_train_data,var_y_2nd);
}
} else {
for (i=1; i<=n_test_data; ++i) {
vec_add_size(mean_y,Y_test[i],n_out,mean_y);
}
vec_mult_scalar(mean_y,1./(double)n_test_data,mean_y);
for (i=1; i<=n_test_data; ++i) {
for (j=1; j<=n_out; ++j) {
var_y[j] += sqr(Y_test[i][j]-mean_y[j]);
}
}
vec_mult_scalar(var_y,1./(double)n_test_data,var_y);
if (lwprs[LWPR1].use_reg_2nd) {
for (i=1; i<=n_test_data; ++i) {
vec_add_size(mean_y_2nd,Y_test_2nd[i],n_out,mean_y_2nd);
}
vec_mult_scalar(mean_y_2nd,1./(double)n_test_data,mean_y_2nd);
for (i=1; i<=n_train_data; ++i) {
for (j=1; j<=n_out_2nd; ++j) {
var_y_2nd[j] += sqr(Y_test_2nd[i][j]-mean_y_2nd[j]);
}
}
vec_mult_scalar(var_y_2nd,1./(double)n_train_data,var_y_2nd);
}
}
}