/*!*****************************************************************************
*******************************************************************************
\note compute_inertial
\date Oct 2005
\remarks
This functions computes simulated inertial signals, i.e., body
quaternion, angular velocity, and linear acceleration, and
linear acceleration at the feet
*******************************************************************************
Function Parameters: [in]=input,[out]=output
All results can be computed based on global variables. Results are
assigned to the misc_sensor structures.
******************************************************************************/
static void
compute_inertial(void)
{
int i,j;
static int firsttime = TRUE;
static Matrix R;
static Matrix RT;
static Vector ad;
static Vector xdd;
static Vector r;
static Vector r0;
static Vector t1;
static Vector t2;
static Vector t3;
static Vector lv;
static Vector rv;
static Vector lv_last;
static Vector rv_last;
static Vector lacc;
static Vector racc;
if (firsttime) {
R = my_matrix(1,N_CART,1,N_CART);
RT = my_matrix(1,N_CART,1,N_CART);
ad = my_vector(1,N_CART);
xdd = my_vector(1,N_CART);
r = my_vector(1,N_CART);
r0 = my_vector(1,N_CART);
t1 = my_vector(1,N_CART);
t2 = my_vector(1,N_CART);
t3 = my_vector(1,N_CART);
lv = my_vector(1,N_CART);
rv = my_vector(1,N_CART);
lacc= my_vector(1,N_CART);
racc= my_vector(1,N_CART);
lv_last = my_vector(1,N_CART);
rv_last = my_vector(1,N_CART);
firsttime = FALSE;
}
// copy quaternion from base coordinates (perfect data)
misc_sim_sensor[B_Q0_IMU] = base_orient.q[_Q0_];
misc_sim_sensor[B_Q1_IMU] = base_orient.q[_Q1_];
misc_sim_sensor[B_Q2_IMU] = base_orient.q[_Q2_];
misc_sim_sensor[B_Q3_IMU] = base_orient.q[_Q3_];
// the angular vel. and translatory acc are first computed in world coordinates
// and then transformed to base coordinates.
// need the base coordinate rotatin matrix, which transforms world to base
quatToRotMat(&base_orient,R);
mat_trans(R,RT);
// offset is IMU_X_OFFSET and -IMU_Y_OFFSET
r[_X_] = IMU_X_OFFSET;
r[_Y_] = -IMU_Y_OFFSET;
r[_Z_] = 0.0;
// get offset vector in global coordinates
mat_vec_mult(RT,r,r0);
// w x r0
vec_mult_outer_size(base_orient.ad,r0,N_CART,t1);
// w x (w x r0) = w x t1 (which is one term of xdd)
vec_mult_outer_size(base_orient.ad,t1,N_CART,t2);
// wd x r0
vec_mult_outer_size(base_orient.add,r0,N_CART,t1);
// add to xdd
vec_add(t1,t2,t1);
// and add the base acceleration
vec_add_size(t1,base_state.xdd,N_CART,t1);
// add gravity to t1
t1[_Z_] -= gravity;
// rotate all into base coordinates
mat_vec_mult_size(R,N_CART,N_CART,base_orient.ad,N_CART,t3);
mat_vec_mult_size(R,N_CART,N_CART,t1,N_CART,t2);
// and rotate this information into IMU coordinates
// IMU_X = -Z_BASE; IMU_Y = -X_BASE; IMU_Z = Y_BASE
t1[_A_] = -PI/2.;
t1[_B_] = 0.0;
t1[_G_] = PI/2.;
eulerToRotMat(t1,R);
mat_vec_mult(R,t2,xdd);
mat_vec_mult(R,t3,ad);
// the final result
misc_sim_sensor[B_AD_A_IMU] = ad[_A_];
misc_sim_sensor[B_AD_B_IMU] = ad[_B_];
misc_sim_sensor[B_AD_G_IMU] = ad[_G_];
misc_sim_sensor[B_XACC_IMU] = xdd[_X_];
misc_sim_sensor[B_YACC_IMU] = xdd[_Y_];
misc_sim_sensor[B_ZACC_IMU] = xdd[_Z_];
// now get the foot accelerations in WORLD coordinate, computed at the L_FOOT position,
// which is not exactly the same as the load cell offset, but very close.
computeLinkVelocity(L_FOOT, link_pos_sim, joint_origin_pos_sim,
joint_axis_pos_sim, joint_sim_state, lv);
computeLinkVelocity(R_FOOT, link_pos_sim, joint_origin_pos_sim,
joint_axis_pos_sim, joint_sim_state, rv);
for (i=1; i<=N_CART; ++i) {
lacc[i] = (lv[i]-lv_last[i])*(double)simulation_servo_rate;
lv_last[i] = lv[i];
racc[i] = (rv[i]-rv_last[i])*(double)simulation_servo_rate;
rv_last[i] = rv[i];
}
// transform to local foot coordinates after adding gravity
lacc[_Z_] -= gravity;
racc[_Z_] -= gravity;
// rotation matrix from world to L_AAA coordinates
mat_trans_size(Alink_sim[L_FOOT],N_CART,N_CART,R);
mat_vec_mult(R,lacc,t1);
// rotation matrix from world to R_AAA coordinates
mat_trans_size(Alink_sim[R_FOOT],N_CART,N_CART,R);
mat_vec_mult(R,racc,t2);
#ifdef HAS_LOWER_BODY
// the final result
misc_sim_sensor[L_FOOT_XACC] = t1[_X_];
misc_sim_sensor[L_FOOT_YACC] = t1[_Y_];
misc_sim_sensor[L_FOOT_ZACC] = t1[_Z_];
misc_sim_sensor[R_FOOT_XACC] = t2[_X_];
misc_sim_sensor[R_FOOT_YACC] = t2[_Y_];
misc_sim_sensor[R_FOOT_ZACC] = t2[_Z_];
#endif
}
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);
}
}
}
