/*****************************************************************************
******************************************************************************
Function Name : add_test_sine_task
Date : Feb 1999
Remarks:
adds the task to the task menu
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
void
add_test_sine_task( void )
{
int i, j;
static int firsttime = TRUE;
char string[100];
if (firsttime) {
firsttime = FALSE;
off = my_vector(1,n_dofs);
amp = my_matrix(1,n_dofs,1,MAX_SINE);
phase = my_matrix(1,n_dofs,1,MAX_SINE);
freq = my_matrix(1,n_dofs,1,MAX_SINE);
n_sine = my_ivector(1,n_dofs);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
addTask("Test Sine Task", init_test_sine_task,run_test_sine_task, change_test_sine_task);
for (i=1; i<=N_DOFS; ++i) {
sprintf(string,"%s_tar_th",joint_names[i]);
addVarToCollect((char *)&(target[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_tar_thd",joint_names[i]);
addVarToCollect((char *)&(target[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_tar_thdd",joint_names[i]);
addVarToCollect((char *)&(target[i].thdd),string,"rad/s^2", DOUBLE,FALSE);
}
updateDataCollectScript();
}
}
BaseStateEstimation::BaseStateEstimation()
{
I_rot_B_ = my_matrix(1,3,1,3);
I_linpos_B_ = my_vector(1,3);
O_rot_I_= my_matrix(1,3,1,3);
O_angrate_I_ = my_vector(1,3);
O_linacc_I_ = my_vector(1,3);
O_angacc_I_ = my_vector(1,3);
O_linvel_I_ = my_vector(1,3);
O_linpos_I_ = my_vector(1,3);
O_rot_B_= my_matrix(1,3,1,3);
O_linacc_B_ = my_vector(1,3);
O_linvel_B_ = my_vector(1,3);
O_linpos_B_ = my_vector(1,3);
prev_O_angrate_I_ = my_vector(1,3);
// gravity_ = my_vector(1,3);
for(int i=1; i<=3; i++)
{
prev_O_angrate_I_[i] = I_linpos_B_[i] = O_linvel_I_[i] = O_linpos_I_[i] =
O_linacc_B_[i] = O_linvel_B_[i] = O_linpos_B_[i] = 0.0;
}
leakage_factor_ = 0.0;
update_rate_ = 250;
}
/*
* Loads the first dataset into a matrix structure
*/
static int load_real_ball_data(int dataset, int numSet) {
int numCols;
static char fileName[100];
static char dirName[100];
if (dataset == 1) {
sprintf(dirName, "../../Desktop/okanKinestheticTeachin_20141210");
numCols = 25;
dist_to_table = -0.80;
}
else if (dataset == 2) {
sprintf(dirName, "../../Desktop/okanKinestheticTeachin_20151124");
numCols = 25 + 6; // cartesian robot pos and vel also saved for this dataset
dist_to_table = -1.15;
}
else {
printf("Dataset not found!\n");
return FALSE;
}
static char savefile[] = "unifyData";
sprintf(fileName,"%s//%s%d.txt",dirName, savefile, numSet);
int numRows = read_row_size(fileName);
realBallData = my_matrix(1,numRows,1,numCols);
if (load_mat(realBallData,numRows,numCols,fileName))
//print_mat_size("ballData", realBallData, numRows, 5);
return TRUE;
return FALSE;
}
/*
* Predict the ball for Tpred seconds
* Using ballflight + bounce model
*
*/
void predict_ball_state(double *b0, double *v0) {
int N = TPRED/TSTEP;
ballMat = my_matrix(1, N, 1, 2*CART);
SL_Cstate ballPred;
bzero((char *)&(ballPred), sizeof(ballPred));
int i,j;
for (j = 1; j <= CART; j++) {
ballMat[0][j] = ballPred.x[j] = b0[j-1];
ballMat[0][j+CART] = ballPred.xd[j] = v0[j-1];
}
// predict Tpred seconds into the future
int bounce = FALSE;
for (i = 1; i <= N; i++) {
integrate_ball_state(ballPred,&ballPred,TSTEP,&bounce);
for (j = 1; j <= CART; j++) {
ballMat[i][j] = ballPred.x[j];
ballMat[i][j+CART] = ballPred.xd[j];
}
}
//print_mat("Ball pred matrix: ", ballMat);
/*for (i = 1; i <= N; i++) {
for (j = 1; j <= 2*CART; j++)
printf("%.4f ", ballMat[i][j]);
printf("\n");
}*/
}
/*
* Called during run_traj_task operation
* Saves joint_state info to trajectory buffers
*/
static void save_act_traj(int idx) {
int i,j;
static char fileName[30];
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
traj_pos_act = my_matrix(1, traj_len, 1, N_DOFS);
traj_vel_act = my_matrix(1, traj_len, 1, N_DOFS);
}
for (i = 1; i <= N_DOFS; i++)
{
traj_pos_act[idx][i] = joint_state[i].th; //each column means all the joints states at a certain time
traj_vel_act[idx][i] = joint_state[i].thd;
}
return;
}
/*!*****************************************************************************
*******************************************************************************
\note init_matrices
\date Jun 2000
\remarks
initializes things for matrix computation
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
init_matrices(void)
{
B = my_matrix(1,B_M,1,B_N);
V = my_matrix(1,B_N,1,B_N);
y = my_vector(1,B_M);
w = my_vector(1,B_N);
x = my_vector(1,B_N);
Pl = my_matrix(1,P_M,1,P_N);
Pr = my_matrix(1,P_M,1,P_N);
intl = my_vector(1,I_M);
intr = my_vector(1,I_M);
if (!init_stereo_matrices (config_files[STEREOPARAMETERS]))
return FALSE;
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note add_sine_task
\date Feb 1999
\remarks
adds the task to the task menu
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
add_sine_task( void )
{
int i, j;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
off = my_vector(1,n_dofs);
amp = my_matrix(1,n_dofs,1,MAX_SINE);
phase = my_matrix(1,n_dofs,1,MAX_SINE);
freq = my_matrix(1,n_dofs,1,MAX_SINE);
n_sine = my_ivector(1,n_dofs);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
addTask("Sine Task", init_sine_task,run_sine_task, change_sine_task);
}
}
NeuroNet::NeuroNet(int neuronsOnFirstLayer, int neuronsOnSecondLayer, double alpha, QObject* receiver):
mFirstLayer(neuronsOnFirstLayer, neuronsOnSecondLayer)
, mAlpha(alpha)
, m_receiver(receiver)
{
mFirstLayer.randomInitialize();
int cols = mFirstLayer.getCol();
int rows = mFirstLayer.getRow();
mSecondLayer = my_matrix(mFirstLayer.transpose());
cols = mSecondLayer.getCol();
rows = mSecondLayer.getRow();
}
int main(void) {
//nlopt_example_run();
//pthread_example_run();
//return TRUE;
Vector ballLand = my_vector(1,CART);
double landTime;
double q0[DOF];
double b0[CART];
double v0[CART];
double x[OPTIM_DIM]; /* initial guess for optim */
Matrix lookupTable = my_matrix(1, LOOKUP_TABLE_SIZE, 1, LOOKUP_COLUMN_SIZE);
load_lookup_table(lookupTable);
load_joint_limits();
set_des_land_param(ballLand,&landTime);
/* initialize ball and racket */
// predict for T_pred seconds
init_ball_state(b0,v0);
init_joint_state(q0);
predict_ball_state(b0,v0);
calc_racket_strategy(ballLand,landTime);
/* run NLOPT opt algorithm here */
lookup(lookupTable,b0,v0,x); // initialize solution
nlopt_optim_poly_run(x,q0);
// test the lookup value to see if constraint is not violated
printf("================== TEST ==================\n");
printf("Lookup values:\n");
lookup(lookupTable,b0,v0,x);
test_optim(x,q0);
return TRUE;
}
/*
*
* Computes the analytical solution to 3by3 matrix inversion using Kramer's rule
* M and Minv can be the same matrix, code copies M first
*
*/
int inv_mat_3by3(Matrix M0, Matrix Minv) {
int i, j;
double thresh = 1e-6;
// copy M to a matrix
Matrix M = my_matrix(1,3,1,3);
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
M[i][j] = M0[i][j];
double det = M[1][1]*M[2][2]*M[3][3];
det = det + M[2][1]*M[3][2]*M[1][3];
det = det + M[3][1]*M[1][2]*M[2][3];
det = det - M[1][1]*M[3][2]*M[2][3];
det = det - M[3][1]*M[2][2]*M[1][3];
det = det - M[2][1]*M[1][2]*M[3][3];
if (det < thresh) {
freeze();
return FALSE;
}
Minv[1][1] = (M[2][2]*M[3][3] - M[2][3]*M[3][2])/det;
Minv[1][2] = (M[1][3]*M[3][2] - M[1][2]*M[3][3])/det;
Minv[1][3] = (M[1][2]*M[2][3] - M[1][3]*M[2][2])/det;
Minv[2][1] = (M[2][3]*M[3][1] - M[2][1]*M[3][3])/det;
Minv[2][2] = (M[1][1]*M[3][3] - M[1][3]*M[3][1])/det;
Minv[2][3] = (M[1][3]*M[2][1] - M[1][1]*M[2][3])/det;
Minv[3][1] = (M[2][1]*M[3][2] - M[2][2]*M[3][1])/det;
Minv[3][2] = (M[1][2]*M[3][1] - M[1][1]*M[3][2])/det;
Minv[3][3] = (M[1][1]*M[2][2] - M[1][2]*M[2][1])/det;
return TRUE;
}
/*
* Function that calculates a racket strategy : positions, velocities and orientations
* for each point on the predicted ball trajectory (ballMat)
* to return it a desired point (ballLand) at a desired time (landTime)
*
* TODO: shall we switch to a structure for racketMat?
*/
void calc_racket_strategy(Vector ballLand, double landTime) {
int N = TPRED/TSTEP;
racketMat = my_matrix(1, N, 1, 3*CART);
static SL_Cstate ballIncomingPos;
Vector ballOutVel = my_vector(1,CART);
Vector ballInVel = my_vector(1,CART);
Vector racketVel = my_vector(1,CART);
Vector racketNormal = my_vector(1,CART);
int i,j;
for (i = 1; i <= N; i++) {
for (j = 1; j <= CART; j++) {
ballIncomingPos.x[j] = ballMat[i][j];
ballInVel[j] = ballMat[i][j+CART];
}
// determine the desired outgoing velocity of the ball at contact
calc_ball_vel_out(ballIncomingPos, ballLand, landTime, ballOutVel);
//print_vec("ball out vel: ", ballOutVel);
calc_racket_normal(ballInVel, ballOutVel, racketNormal);
calc_racket_vel(ballInVel, ballOutVel, racketNormal, racketVel);
//print_vec("racket vel = ",racketVel);
//print_vec("racket normal = ",racketNormal);
for (j = 1; j <= CART; j++) {
racketMat[i][j] = ballIncomingPos.x[j];
racketMat[i][j+CART] = racketVel[j];
racketMat[i][j+2*CART] = racketNormal[j];
}
}
//print_mat("Racket matrix:", racketMat);
}
/*!*****************************************************************************
*******************************************************************************
\note read_traj_file
\date June 1999
\remarks
parse a script which describes the traj task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] flag : true= use desired data, false use actual data
******************************************************************************/
static int read_traj_file() {
int j,i,r,k,rc;
static char string[100];
static char fname[100] = "traj_strike.txt";
FILE * fp = NULL;
int found = FALSE;
Matrix buff;
int aux;
int ans = 0;
double data = 0.0;
/* open the file, and parse the parameters */
while (TRUE) {
if (!get_string("Name of the Traj File\0",fname,fname))
return FALSE;
/* try to read this file */
sprintf(string, "%s/%s", "saveData", fname);
/* try to count the number of lines */
traj_len = count_traj_file(string);
get_int("What percentage of trajectory do you want to keep?", 100, &ans);
traj_len = (int)(traj_len * (double)ans / 100.0);
printf("Keeping the first %d indices...\n", traj_len);
//printf("%d\n", traj_len);
if (traj_len == -1) { // problem
return FALSE;
}
fp = fopen(string,"r");
if (fp != NULL) {
found = TRUE;
break;
}
else {
printf("ERROR: Could not open file >%s<\n", string);
}
}
/* get the number of rows, columns, sampling frequency
and calc the buffer_size */
//rc = fscanf(fp, "%d %d %d %lf", &buffer_size, &N_COLS, &traj_len, &SAMP_FREQ);
/* read file into a buffer and check if the matrix size is correct */
buff = my_matrix(1,traj_len,1,N_COLS);
q0 = my_vector(1,N_DOFS);
int out = 0;
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_COLS; k++) {
out = fscanf(fp, "%lf", &data);
buff[r][k] = data;
}
}
fclose(fp);
// print buffs first 10 elements
//print_mat("Buffer:\n", buff);
//print_mat_size("Buffer:\n", buff, 10, N_COLS);
/* create the pos, vel, acc , uff matrices that define the trajectory */
traj_pos = my_matrix(1, traj_len, 1, N_DOFS);
traj_vel = my_matrix(1, traj_len, 1, N_DOFS);
traj_acc = my_matrix(1, traj_len, 1, N_DOFS);
traj_uff = my_matrix(1, traj_len, 1, N_DOFS);
// save q0
for (j = 1; j <= N_DOFS; j++)
q0[j] = buff[1][2*j];
for (r = 1; r <= traj_len; r++) {
for (k = 1; k <= N_DOFS; k++) {
traj_pos[r][k] = buff[r][2*k];
traj_vel[r][k] = buff[r][2*k+1];
}
}
//print_mat_size("Vel:\n", traj_vel, 10, n_dofs);
// numerically differentiate traj_vel instead!
num_diff(traj_acc, traj_vel, SAMP_FREQ);
/* free up memory by deallocating resources */
my_free_matrix(buff,1,traj_len,1,N_COLS);
return found;
}
/*******************************************************************************
*******************************************************************************
\note run_traj_task
\date Jun. 1999
\remarks
run the task from the task servo: REAL TIME requirements!
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int run_traj_task(void) {
int j, i;
double time_of_traj_index;
double time_of_next_traj_index;
static int flag_reset = FALSE;
static double dist = 1e-1;
static int num_it = 1;
static int firsttime = TRUE;
static double tauReturn = 1.5;
const int returnLen = (int)(tauReturn * task_servo_rate);
static int return_iter = 1;
static int return_total_len = 1;
static SL_Jstate j0[N_DOFS+1];
static SL_Jstate j1[N_DOFS+1];
static Matrix trajReturn;
static double time1 = 0;
static double time2 = 0;
// checking for limits
if (!check_joint_limits(joint_state, 0.01)) {
printf("Joint limits are exceeding soft limits! Freezing...\n");
freeze();
}
if (!check_des_joint_limits(joint_des_state, 0.01)) {
printf("Joint des limits are exceeding soft limits! Freezing...\n");
freeze();
}
// calculate the racket orientation from endeffector
calc_racket(&racket_state, &racket_orient,
cart_state[RIGHT_HAND], cart_orient[RIGHT_HAND]);
if(collision_detection(racket_state)){
printf("Collision detected! Freezing...\n");
freeze();
}
if (firsttime) {
firsttime = FALSE;
traj_index = 1;
//print_vec("q0:",q0);
//turn_off_integrator(); // keep integrators off throughout the trajectory
bzero((void *)j0, sizeof(SL_Jstate)* (N_DOFS+1));
for (i = 1; i <= N_DOFS; i++) {
j0[i].th = q0[i];
j0[i].thd = 0.0;
j0[i].thdd = 0.0;
j1[i].th = traj_pos[traj_len][i];
j1[i].thd = traj_vel[traj_len][i];
j1[i].thdd = traj_acc[traj_len][i];
}
trajReturn = my_matrix(1,returnLen,1,3*N_DOFS);
//entirePoly5th(trajReturn, j1, j0, tauReturn);
save_act_traj(traj_index);
fprint_joint_act_traj(traj_index);
fprint_joint_des_traj(traj_index);
fprint_cart_des_traj(traj_index);
fprint_cart_act_traj(traj_index);
}
time_of_traj_index = ((double)(traj_index-1))/SAMP_FREQ;
time_of_next_traj_index = ((double)(traj_index))/SAMP_FREQ;
/* the statement below takes care of numerical rounding problems */
if (task_time >= time_of_next_traj_index-0.00001 && !flag_reset) {
// here we can move on to the next point on the trajectory
//printf("Traj index: %d \n", traj_index);
traj_index = traj_index + 1;
if (traj_index > traj_len) {
//traj_index = 1;
// revert back to usual PD control to bring it back to starting position
toggle_fb();
printf("Number of iter: %d\n", num_it);
flag_reset = TRUE;
rms_traj_error(traj_pos_act,traj_vel_act,traj_pos,traj_vel);
entirePoly5th(trajReturn, joint_state, j0, tauReturn);
//entirePoly5th(trajReturn, joint_state, joint_default_state, tauReturn);
//return_iter = 1;
//task_time = 1./(double)task_servo_rate;
//break;
}
else {
// saving the actual joint positions
// this will be used by ILC
save_act_traj(traj_index);
fprint_joint_act_traj(traj_index);
fprint_joint_des_traj(traj_index);
fprint_cart_des_traj(traj_index);
fprint_cart_act_traj(traj_index);
}
time_of_traj_index = ((double)(traj_index-1))/SAMP_FREQ;
time_of_next_traj_index = ((double)(traj_index))/SAMP_FREQ;
}
// make sure vel and acc are zero
if (flag_reset) {
//printf("Distance to starting pos: %f\n", euclidian_dist(joint_state,traj_pos[1]));
//printf("Resetting...\n");
if (euclidian_dist(joint_state,q0) > dist || return_total_len < 2000) {
for (i = 1; i <= N_DOFS; ++i) {
joint_des_state[i].th = trajReturn[return_iter][3*i-2]; //q0[i];
joint_des_state[i].thd = trajReturn[return_iter][3*i-1];
joint_des_state[i].thdd = trajReturn[return_iter][3*i];
}
if (return_iter < returnLen) {
return_iter++;
return_total_len++;
}
else {
return_total_len++;
if (return_total_len % 500 == 0) {
printf("euclidian dist: %f\n", euclidian_dist(joint_state,q0));
}
/*
if (return_total_len == 1000) {
printf("Initialization problem with PD. Turning on PID (if enabled with ck)...\n");
turn_on_integrator();
}*/
}
SL_InvDyn(NULL,joint_des_state,endeff,&base_state,&base_orient);
if (friction_comp) {
addFrictionModel(joint_des_state);
}
/*if (!check_range(joint_des_state)) {
printf("q0 is out of range! Freezing...\n");
freeze();
}*/
// add feedback from task servo
//add_fb(traj_index);
fprint_joint_act_traj(0);
fprint_cart_act_traj(0);
}
else {
if (repeat_flag) {
return_iter = 1;
return_total_len = 1;
traj_index = 1;
task_time = 0.0; //1./(double)task_servo_rate;
flag_reset = FALSE; //go back to start
save_act_traj(traj_index);
fprint_joint_act_traj(traj_index);
fprint_joint_des_traj(traj_index);
fprint_cart_des_traj(traj_index);
fprint_cart_act_traj(traj_index);
toggle_fb(); // revert to lqr
// get feedforward part to be changed
if (num_it > 1) {// this is due to extreme good initialization firsttime
//turn_on_integrator();
time1 = get_time();
ilc_main();
//update_basic();
time2 = get_time();
printf("ILC takes %.3f ms...\n",(time2-time1)/1000);
//turn_off_integrator(); //turn integrators off again
}
num_it = num_it + 1;
}
else {
printf("Switching to NO TASK!\n");
setTaskByName(NO_TASK);
return TRUE;
}
}
}
else { // feedforward along the trajectory
for (i = 1; i <= N_DOFS; i++) {
joint_des_state[i].th = traj_pos[traj_index][i];
joint_des_state[i].thd = traj_vel[traj_index][i];
joint_des_state[i].thdd = traj_acc[traj_index][i];
joint_des_state[i].uff = traj_uff[traj_index][i];
}
//SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient);
task_time += 1./(double)task_servo_rate;
if (!check_range(joint_des_state)) {
printf("ILC exceeded torque limits. Freezing...\n");
freeze();
}
// add feedback from task servo
add_fb(traj_index);
}
return TRUE;
}
/*****************************************************************************
******************************************************************************
Description: initialization for task
Parameters:
none
*****************************************************************************/
static int init_mpc_task(void) {
int i,j;
int ans;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
lookupTable = my_matrix(1, LOOKUP_TABLE_SIZE, 1, LOOKUP_COLUMN_SIZE);
}
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("Task can only be run if no other task is running!\n");
return FALSE;
}
get_int("Use simulated ball?", FALSE, &simulation);
if(simulation) {
printf("Testing table tennis algorithms on the simulator...\n");
}
/* go to a save posture */
bzero((char *)&(init_joint_state[1]), N_DOFS * sizeof(init_joint_state[1]));
init_joint_state[1].th = 1.0;
init_joint_state[2].th = -0.2;
init_joint_state[3].th = -0.1;
init_joint_state[4].th = 1.8;
init_joint_state[5].th = -1.57;
init_joint_state[6].th = 0.1;
init_joint_state[7].th = 0.3;
for (i = 1; i <= N_DOFS; i++) {
current_joint_state[i].th = init_joint_state[i].th;
current_joint_state[i].thd = init_joint_state[i].thd;
}
go_target_wait(init_joint_state);
if (!go_target_wait_ID(init_joint_state))
return FALSE;
printf("Loading lookup table...\n");
load_vec_into_mat(lookupTable, LOOKUP_TABLE_SIZE,
LOOKUP_COLUMN_SIZE, LOOKUP_TABLE_NAME);
// just testing
if (simulation) {
// initialize ball cannon position
init_ball_cannon();
reset_sim_ball();
}
else {
// real robot mode
}
/*get_int("Friction Compensation?", FALSE, &friction_comp);
if (friction_comp) {
printf("Loading stiction model...\n");
loadStictionModel("./prefs/stiction.pref");
}*/
// for real setup
setDefaultEndeffector();
endeff[RIGHT_HAND].x[_Z_] = .3;
// indices for ball and robot trajectory
trj_time = 0; // clear planned trajectory
// debug task if you like
//debug_task();
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans, &ans))
return FALSE;
}
if (ans != 1)
return FALSE;
busy = FALSE;
// turn off real time
changeRealTime(TRUE);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note compute_kinematics
\date June 99
\remarks
computes kinematic variables
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
compute_kinematics(void)
{
int i,j,r,o;
static int firsttime = TRUE;
static SL_DJstate *temp;
static Matrix last_J;
static Matrix last_Jbase;
if (firsttime) {
temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP);
last_J = my_matrix(1,6*n_endeffs,1,n_dofs);
last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART);
}
/* compute the desired link positions */
linkInformationDes(joint_des_state,&base_state,&base_orient,endeff,
joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des,
link_pos_des,Alink_des,Adof_des);
/* the desired endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i];
}
}
/* the desired quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j]));
}
/* addititional link information */
linkInformation(joint_state,&base_state,&base_orient,endeff,
joint_cog_mpos,joint_axis_pos,joint_origin_pos,
link_pos,Alink,Adof);
/* create the endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_state[j].x[i] = link_pos[link2endeffmap[j]][i];
}
}
/* the quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j]));
}
/* the COG position */
compute_cog();
/* the jacobian */
jacobian(link_pos,joint_origin_pos,joint_axis_pos,J);
baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase);
jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes);
baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes);
/* numerical time derivative of Jacobian */
if (!firsttime) {
mat_sub(J,last_J,dJdt);
mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt);
mat_sub(Jbase,last_Jbase,dJbasedt);
mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt);
}
mat_equal(J,last_J);
mat_equal(Jbase,last_Jbase);
/* compute the cartesian velocities and accelerations */
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
cart_state[j].xd[i] = 0.0;
cart_state[j].xdd[i] = 0.0;
cart_orient[j].ad[i] = 0.0;
cart_orient[j].add[i] = 0.0;
cart_des_state[j].xd[i] = 0.0;
cart_des_orient[j].ad[i] = 0.0;
/* contributations from the joints */
for (r=1; r<=n_dofs; ++r) {
cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd;
cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd;
cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd;
cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i+3][r] * joint_state[r].thd;
}
/* contributations from the base */
for (r=1; r<=N_CART; ++r) {
cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r];
}
}
/* compute quaternion derivatives */
quatDerivatives(&(cart_orient[j]));
quatDerivatives(&(cart_des_orient[j]));
for (r=1; r<=N_QUAT; ++r)
cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far
}
/* reset first time flag */
firsttime = FALSE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_sensor_processing
\date May 2000
\remarks
Initializes all sensory processing
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_sensor_processing(void)
{
int i,j;
FILE *in;
char string[100];
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
joint_lin_rot = my_matrix(1,n_dofs,1,6);
pos_polar = my_vector(1,n_dofs);
load_polar = my_vector(1,n_dofs);
joint_raw_state = (SL_Jstate *)
my_calloc((unsigned long)(n_dofs+1),sizeof(SL_Jstate),MY_STOP);
misc_raw_sensor = (double *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(double),MY_STOP);
fth = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthdd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fload = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fmisc_sensor = (Filter *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(Filter),MY_STOP);
}
/* initalizes translation to and from units */
if (!init_user_sensor_processing())
return FALSE;
/* initialize filtering */
if (!init_filters())
return FALSE;
/* read several variables from files */
/* first, get the calibration values for dealing with the linear to
rotary conversion */
if (!read_sensor_calibration(config_files[SENSORCALIBRATION],
joint_lin_rot,pos_polar,load_polar))
return FALSE;
/* second, get the max, min , and offsets of the position sensors */
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
/* third, get the filter cutoff values for all variable */
if (!read_sensor_filters(config_files[SENSORFILTERS]))
return FALSE;
/* add function to man pages */
addToMan("where_off","sensor readings without offsets",where_off);
addToMan("where_raw","raw sensor readings",where_raw);
addToMan("monitor_min_max","records min/max values of sensors",monitor_min_max);
if (!real_robot_flag)
addToMan("toggle_filter","toggles sensor filtering on and off",toggle_filter);
/* make raw variables available for output */
for (i=1; i<=n_dofs; ++i) {
sprintf(string,"%s_rth",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_rthd",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_rload",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].load),string,"Nm", DOUBLE,FALSE);
}
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s_r",misc_sensor_names[i]);
addVarToCollect((char *)&(misc_raw_sensor[i]),string,"-", DOUBLE,FALSE);
}
return TRUE;
}
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);
}
}
}
/*!*****************************************************************************
*******************************************************************************
\note compute_force_sensors
\date March 2003
\remarks
This function computes simulated force sensor information for both
feet. The results are force and torque values for in link coordinates
of the last ankle joint. This informatin can be extracted from the
contact force computation, which has world coordinates. Thus, only
a simple coordinate transformatino from world to local link coordinates
is needed. A small correction is need to transform the force/torque
information to the exact position of the load cell center.
*******************************************************************************
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_force_sensors(void)
{
int i,j;
static int firsttime = TRUE;
static Matrix M;
static Vector v;
static Vector vv;
static Vector r;
if (firsttime) {
firsttime = FALSE;
M = my_matrix(1,N_CART,1,N_CART);
v = my_vector(1,N_CART);
vv= my_vector(1,N_CART);
r = my_vector(1,N_CART);
}
// rotation matrix from world to L_AAA coordinates
mat_trans_size(Adof_sim[L_AAA],N_CART,N_CART,M);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = ucontact[L_AAA].f[i] - uext_sim[L_AAA].f[i];
mat_vec_mult(M,v,vv);
misc_sim_sensor[L_CFx] = vv[_X_];
misc_sim_sensor[L_CFy] = vv[_Y_];
misc_sim_sensor[L_CFz] = vv[_Z_];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = ucontact[L_AAA].t[i] - uext_sim[L_AAA].t[i];
mat_vec_mult(M,v,v);
// correct for load cell offset (need to use local coordinates)
r[_X_] = FTA_Z_OFF;
r[_Y_] = FTA_X_OFF;
r[_Z_] = -FTA_Y_OFF;
vec_mult_outer(vv,r,vv);
misc_sim_sensor[L_CTa] = v[_A_] + vv[_A_];
misc_sim_sensor[L_CTb] = v[_B_] + vv[_B_];
misc_sim_sensor[L_CTg] = v[_G_] + vv[_G_];
// rotation matrix from world to R_AAA coordinates
mat_trans_size(Adof_sim[R_AAA],N_CART,N_CART,M);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = ucontact[R_AAA].f[i] - uext_sim[R_AAA].f[i];
mat_vec_mult(M,v,vv);
misc_sim_sensor[R_CFx] = vv[_X_];
misc_sim_sensor[R_CFy] = vv[_Y_];
misc_sim_sensor[R_CFz] = vv[_Z_];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = ucontact[R_AAA].t[i] - uext_sim[R_AAA].t[i];
mat_vec_mult(M,v,v);
// correct for load cell offset (need to use local coordinates)
r[_X_] = FTA_Z_OFF;
r[_Y_] = FTA_X_OFF;
r[_Z_] = FTA_Y_OFF;
vec_mult_outer(vv,r,vv);
misc_sim_sensor[R_CTa] = v[_A_] + vv[_X_];
misc_sim_sensor[R_CTb] = v[_B_] + vv[_Y_];
misc_sim_sensor[R_CTg] = v[_G_] + vv[_Z_];
}
/*!*****************************************************************************
*******************************************************************************
\note parm_opt
\date 10/20/91
\remarks
this is the major optimzation program
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] tol : error tolernance to be achieved
\param[in] n_parm : number of parameters to be optimzed
\param[in] n_con : number of contraints to be taken into account
\param[in] f_dLda : function which calculates the derivative of the
optimziation criterion with respect to the parameters;
must return vector
\param[in] f_dMda : function which calculates the derivate of the
constraints with respect to parameters
must return matrix
\param[in] f_M : constraints function, must always be formulted to
return 0 for properly fulfilled constraints
\param[in] f_L : function to calculate simple cost (i.e., constraint
cost NOT included), the constraint costs are added
by this program automatically, the function returns
a double scalar
\param[in] f_dLdada : second derivative of L with respect to the parameters,
must be a matrix of dim n_parm x n_parm
\param[in] f_dMdada : second derivative of M with respect to parameters,
must be a matrix n_con*n_parm x n_parm
\param[in] use_newton: TRUE or FALSE to indicate that second derivatives
are given and can be used for Newton algorithm
\param[in,out] a : initial setting of parameters and also return of
optimal value (must be a vector, even if scalar)
\param[out] final_cost: the final cost
\param[out] err : the sqrt of the squared error of all constraints
NOTE: - program returns TRUE if everything correct, otherwise FALSE
- always minimizes the cost!!!
- algorithms come from Dyer McReynolds
NOTE: besides the possiblity of a bug, the Newton method seems to
sacrifice the validity of the constraint a little up to
quite a bit and should be used prudently
******************************************************************************/
int
parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(),
void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(),
void (*f_dLdada)(), int use_newton, double *final_cost, double *err)
{
register int i,j,n;
double cost= 999.e30;
double last_cost = 0.0;
double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */
double **dMda=NULL, **dMda_t=NULL;
double *dLda;
double *K=NULL; /* the error in the constraints */
double eps = 0.025; /* the learning rate */
double **aux_mat=NULL; /* needed for inversion of matrix */
double *aux_vec=NULL;
double *new_a;
double **dMdada=NULL;
double **dLdada=NULL;
double **A=NULL; /* big matrix, a combination of several other matrices */
double *B=NULL; /* a big vector */
double **A_inv=NULL;
int rc=TRUE;
long count = 0;
int last_sign = 1;
int pending1 = FALSE, pending2 = FALSE;
int firsttime = TRUE;
int newton_active = FALSE;
dLda = my_vector(1,n_parm);
new_a = my_vector(1,n_parm);
if (n_con > 0) {
mult = my_vector(1,n_con);
dMda = my_matrix(1,n_con,1,n_parm);
dMda_t = my_matrix(1,n_parm,1,n_con);
K = my_vector(1,n_con);
aux_mat = my_matrix(1,n_con,1,n_con);
aux_vec = my_vector(1,n_con);
}
if (use_newton) {
dLdada = my_matrix(1,n_parm,1,n_parm);
A = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
A_inv = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
B = my_vector(1,n_parm+n_con);
if (n_con > 0) {
dMdada = my_matrix(1,n_con*n_parm,1,n_parm);
new_mult = my_vector(1,n_con);
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1+n_parm; j<=n_con+n_parm; ++j) {
A[i][j] = 0.0;
}
}
}
while (fabs(cost-last_cost) > *tol) {
++count;
pending1 = FALSE;
pending2 = FALSE;
AGAIN:
/* calculate the current Lagrange multipliers */
if (n_con > 0) {
(*f_M)(a,K); /* takes the parameters, returns residuals */
(*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */
}
(*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */
if (n_con > 0) {
mat_trans(dMda,dMda_t);
}
if (newton_active) {
if (n_con > 0) {
(*f_dMdada)(a,dMdada);
}
(*f_dLdada)(a,dLdada);
}
/* the first step is always a gradient step */
if (newton_active) {
if (firsttime) {
firsttime = FALSE;
eps = 0.1;
}
/* build the A matrix */
for (i=1; i<=n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[i][j] = dLdada[i][j];
for (n=1; n<=n_con; ++n) {
A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j];
}
}
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[j][i] = A[i][j] = dMda[i-n_parm][j];
}
}
/* build the B vector */
if (n_con > 0) {
mat_vec_mult(dMda_t,mult,B);
}
for (i=1; i<=n_con; ++i) {
B[i+n_parm] = K[i];
}
/* invert the A matrix */
if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) {
rc = FALSE;
break;
}
mat_vec_mult(A_inv,B,B);
vec_mult_scalar(B,eps,B);
for (i=1; i<=n_parm; ++i) {
new_a[i] = a[i] + B[i];
}
for (i=1; i<=n_con; ++i) {
new_mult[i] = mult[i] + B[n_parm+i];
}
} else {
if (n_con > 0) {
/* the mulitpliers are updated according:
mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t) */
mat_mult(dMda,dMda_t,aux_mat);
if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) {
rc = FALSE;
break;
}
mat_vec_mult(dMda,dLda,aux_vec);
vec_mult_scalar(K,1./eps,K);
vec_sub(K,aux_vec,aux_vec);
mat_vec_mult(aux_mat,aux_vec,mult);
}
/* the update step looks the following:
a_new = a - eps * (dLda + mult_t * dMda)_t */
if (n_con > 0) {
vec_mat_mult(mult,dMda,new_a);
vec_add(dLda,new_a,new_a);
} else {
vec_equal(dLda,new_a);
}
vec_mult_scalar(new_a,eps,new_a);
vec_sub(a,new_a,new_a);
}
if (count == 1 && !pending1) {
last_cost = (*f_L)(a);
if (n_con > 0) {
(*f_M)(a,K);
last_cost += vec_mult_inner(K,mult);
}
} else {
last_cost = cost;
}
/* calculate the updated cost */
cost = (*f_L)(new_a);
/*printf(" %f\n",cost);*/
if (n_con > 0) {
(*f_M)(new_a,K);
if (newton_active) {
cost += vec_mult_inner(K,new_mult);
} else {
cost += vec_mult_inner(K,mult);
}
}
/* printf("last=%f new=%f\n",last_cost,cost); */
/* check out whether we reduced the cost */
if (cost > last_cost && fabs(cost-last_cost) > *tol) {
/* reduce the gradient climbing rate: sometimes a reduction of eps
causes an increase in cost, thus leave an option to increase
eps */
cost = last_cost; /* reset last_cost */
if (pending1 && pending2) {
/* this means that either increase nor decrease
of eps helps, ==> leave the program */
rc = TRUE;
break;
} else if (pending1) {
eps *= 4.0; /* the last cutting by half did not help, thus
multiply by 2 to get to previous value, and
one more time by 2 to get new value */
pending2 = TRUE;
} else {
eps /= 2.0;
pending1 = TRUE;
}
goto AGAIN;
} else {
vec_equal(new_a,a);
if (newton_active && n_con > 0) {
vec_equal(new_mult,mult);
}
if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD)
newton_active = TRUE;
}
}
my_free_vector(dLda,1,n_parm);
my_free_vector(new_a,1,n_parm);
if (n_con > 0) {
my_free_vector(mult,1,n_con);
my_free_matrix(dMda,1,n_con,1,n_parm);
my_free_matrix(dMda_t,1,n_parm,1,n_con);
my_free_vector(K,1,n_con);
my_free_matrix(aux_mat,1,n_con,1,n_con);
my_free_vector(aux_vec,1,n_con);
}
if (use_newton) {
my_free_matrix(dLdada,1,n_parm,1,n_parm);
my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con);
my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con);
my_free_vector(B,1,n_parm+n_con);
if (n_con > 0) {
my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm);
my_free_vector(new_mult,1,n_con);
}
}
*final_cost = cost;
*tol = fabs(cost-last_cost);
if (n_con > 0) {
*err = sqrt(vec_mult_inner(K,K));
} else {
*err = 0.0;
}
/*
printf("count=%ld rc=%d\n",count,rc);
*/
return rc;
}
void expm(Matrix A, Matrix B, double h) {
static int firsttime = TRUE;
static Matrix M;
static Matrix M2;
static Matrix M3;
static Matrix D;
static Matrix N;
if (firsttime) {
firsttime = FALSE;
M = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M2 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M3 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
D = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
N = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
}
int norm = 0;
double c = 0.5;
int q = 6; // uneven p-q order for Pade approx.
int p = 1;
int i,j,k;
// Form the big matrix
mat_mult_scalar(A, h, A);
mat_equal_size(A, 2*N_DOFS, 2*N_DOFS, M);
for (i = 1; i <= 2*N_DOFS; ++i) {
for (j = 1; j <= N_DOFS; ++j) {
M[i][2*N_DOFS + j] = h * B[i][j];
}
}
//print_mat("M is:\n", M);
// scale M by power of 2 so that its norm < 1/2
norm = (int)(log2(inf_norm(M))) + 2;
//printf("Inf norm of M: %f \n", inf_norm(M));
if (norm < 0)
norm = 0;
mat_mult_scalar(M, 1/pow(2,(double)norm), M);
//printf("Norm of M logged, floored and 2 added is: %d\n", norm);
//print_mat("M after scaling is:\n", M);
for (i = 1; i <= 3*N_DOFS; i++) {
for (j = 1; j <= 3*N_DOFS; j++) {
N[i][j] = c * M[i][j];
D[i][j] = -c * M[i][j];
}
N[i][i] = N[i][i] + 1.0;
D[i][i] = D[i][i] + 1.0;
}
// set M2 equal to M
mat_equal(M, M2);
// start pade approximation
for (k = 2; k <= q; k++) {
c = c * (q - k + 1) / (double)(k * (2*q - k + 1));
mat_mult(M,M2,M2);
mat_mult_scalar(M2,c,M3);
mat_add(N,M3,N);
if (p == 1)
mat_add(D,M3,D);
else
mat_sub(D,M3,D);
p = -1 * p;
}
// multiply denominator with nominator i.e. D\E
my_inv_ludcmp(D, 3*N_DOFS, D);
mat_mult(D,N,N);
// undo scaling by repeated squaring
for (k = 1; k <= norm; k++)
mat_mult(N,N,N);
// get the matrices out
// read off the entries
for (i = 1; i <= 2*N_DOFS; i++) {
for (j = 1; j <= 2*N_DOFS; j++) {
A[i][j] = N[i][j];
}
for (j = 1; j <= N_DOFS; j++) {
B[i][j] = N[i][2*N_DOFS + j];
}
}
}
void
test_constraint(void)
{
int i,j,n,m;
static Matrix Jbig;
static Matrix dJbigdt;
static Vector dsbigdt;
static Matrix JbigMinvJbigt;
static Matrix JbigMinvJbigtinv;
static Matrix Minv;
static Matrix MinvJbigt;
static Matrix Jbigt;
static Vector dJbigdtdsbigdt;
static Matrix Jbart;
static Matrix Jbar;
static Vector lv;
static Vector lambda;
static Vector f;
static Matrix R;
static Vector v;
static int firsttime = TRUE;
int debug_print = TRUE;
if (firsttime) {
firsttime = FALSE;
Jbig = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
Jbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dJbigdt = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
JbigMinvJbigt = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
JbigMinvJbigtinv = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
Minv = my_matrix(1,N_DOFS+6,1,N_DOFS+6);
MinvJbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dsbigdt = my_vector(1,N_DOFS+6);
dJbigdtdsbigdt = my_vector(1,N_ENDEFFS*6);
Jbar = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
Jbart = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
lv = my_vector(1,N_ENDEFFS*6);
lambda = my_vector(1,N_ENDEFFS*6);
f = my_vector(1,N_DOFS+6);
R = my_matrix(1,N_CART,1,N_CART);
v = my_vector(1,N_CART);
// the base part of Jbig is just the identity matrix for both constraints
for (i=1; i<=6; ++i)
Jbig[i][N_DOFS+i] = 1.0;
for (i=1; i<=6; ++i)
Jbig[i+6][N_DOFS+i] = 1.0;
}
// build the Jacobian including the base -- just copy J into Jbig
mat_equal_size(J,N_ENDEFFS*6,N_DOFS,Jbig);
mat_trans(Jbig,Jbigt);
// build the Jacobian derivative
mat_equal_size(dJdt,N_ENDEFFS*6,N_DOFS,dJbigdt);
// build the augmented state derivate vector
for (i=1; i<=N_DOFS; ++i)
dsbigdt[i] = joint_state[i].thd;
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+i] = base_state.xd[i];
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+N_CART+i] = base_orient.ad[i];
// compute J-bar transpose
my_inv_ludcmp(rbdInertiaMatrix,N_DOFS+6, Minv);
mat_mult(Minv,Jbigt,MinvJbigt);
mat_mult(Jbig,MinvJbigt,JbigMinvJbigt);
my_inv_ludcmp(JbigMinvJbigt,N_ENDEFFS*6,JbigMinvJbigtinv);
mat_mult(MinvJbigt,JbigMinvJbigtinv,Jbar);
mat_trans(Jbar,Jbart);
// compute C+G-u
for (i=1; i<=N_DOFS; ++i)
f[i] = -joint_state[i].u;
for (i=1; i<=6; ++i)
f[N_DOFS+i] = 0.0;
vec_add(f,rbdCplusGVector,f);
// compute first part of lambda
mat_vec_mult(Jbart,f,lambda);
// compute second part of lambda
mat_vec_mult(dJbigdt,dsbigdt,dJbigdtdsbigdt);
mat_vec_mult(JbigMinvJbigtinv,dJbigdtdsbigdt,lv);
// compute final lambda
vec_sub(lambda,lv,lambda);
// ----------------------------------------------------------------------
// express lambda in the same coordinate at the foot sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[L_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CFx]);
printf("LEFT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CFy]);
printf("LEFT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CFz]);
}
misc_sensor[L_CFx] = v[1];
misc_sensor[L_CFy] = v[2];
misc_sensor[L_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CTa]);
printf("LEFT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CTb]);
printf("LEFT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CTg]);
}
misc_sensor[L_CTa] = v[1];
misc_sensor[L_CTb] = v[2];
misc_sensor[L_CTg] = v[3];
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[R_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CFx]);
printf("RIGHT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CFy]);
printf("RIGHT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CFz]);
}
misc_sensor[R_CFx] = v[1];
misc_sensor[R_CFy] = v[2];
misc_sensor[R_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CTa]);
printf("RIGHT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CTb]);
printf("RIGHT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CTg]);
}
misc_sensor[R_CTa] = v[1];
misc_sensor[R_CTb] = v[2];
misc_sensor[R_CTg] = v[3];
if (debug_print)
getchar();
}
/*!*****************************************************************************
*******************************************************************************
\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
}
/*
* This is the constraint that makes sure we hit the ball
*/
void kinematics_eq_constr(unsigned m, double *result, unsigned n, const double *x, double *grad, void *data) {
static double ballPred[CART];
static double racketDesVel[CART];
static double racketDesNormal[CART];
static Matrix link_pos_des;
static Matrix joint_cog_mpos_des;
static Matrix joint_origin_pos_des;
static Matrix joint_axis_pos_des;
static Matrix Alink_des[N_LINKS+1];
static Matrix racketTransform;
static Matrix Jacobi;
static Vector qfdot;
static Vector xfdot;
static Vector normal;
static int firsttime = TRUE;
double T = x[2*DOF];
int N = T/TSTEP;
int i;
/* initialization of static variables */
if (firsttime) {
firsttime = FALSE;
link_pos_des = my_matrix(1,N_LINKS,1,3);
joint_cog_mpos_des = my_matrix(1,N_DOFS,1,3);
joint_origin_pos_des = my_matrix(1,N_DOFS,1,3);
joint_axis_pos_des = my_matrix(1,N_DOFS,1,3);
Jacobi = my_matrix(1,2*CART,1,N_DOFS);
racketTransform = my_matrix(1,4,1,4);
qfdot = my_vector(1,DOF);
xfdot = my_vector(1,2*CART);
normal = my_vector(1,CART);
for (i = 0; i <= N_LINKS; ++i)
Alink_des[i] = my_matrix(1,4,1,4);
// initialize the base variables
//taken from ParameterPool.cf
bzero((void *) &base_state, sizeof(base_state));
bzero((void *) &base_orient, sizeof(base_orient));
base_orient.q[_Q1_] = 1.0;
// the the default endeffector parameters
setDefaultEndeffector();
// homogeneous transform instead of using quaternions
racketTransform[1][1] = 1;
racketTransform[2][3] = 1;
racketTransform[3][2] = -1;
racketTransform[4][4] = 1;
}
if (grad) {
// compute gradient of kinematics = jacobian
//TODO:
grad[0] = 0.0;
grad[1] = 0.0;
grad[2] = 0.0;
}
// interpolate at time T to get the desired racket parameters
first_order_hold(ballPred,racketDesVel,racketDesNormal,T);
// extract state information from array to joint_des_state structure
for (i = 1; i <= DOF; i++) {
joint_des_state[i].th = x[i-1];
joint_des_state[i].thd = qfdot[i] = x[i-1+DOF];
}
/* compute the desired link positions */
kinematics(joint_des_state, &base_state, &base_orient, endeff,
joint_cog_mpos_des, joint_axis_pos_des, joint_origin_pos_des,
link_pos_des, Alink_des);
/* compute the racket normal */
mat_mult(Alink_des[6],racketTransform,Alink_des[6]);
for (i = 1; i <= CART; i++) {
normal[i] = Alink_des[6][i][3];
}
/* compute the jacobian */
jacobian(link_pos_des, joint_origin_pos_des, joint_axis_pos_des, Jacobi);
mat_vec_mult(Jacobi, qfdot, xfdot);
/* deviations from the desired racket frame */
for (i = 1; i <= CART; i++) {
//printf("xfdot[%d] = %.4f, racketDesVel[%d] = %.4f\n",i,xfdot[i],i,racketDesVel[i-1]);
//printf("normal[%d] = %.4f, racketDesNormal[%d] = %.4f\n",i,normal[i],i,racketDesNormal[i-1]);
result[i-1] = link_pos_des[6][i] - ballPred[i-1];
result[i-1 + CART] = xfdot[i] - racketDesVel[i-1];
result[i-1 + 2*CART] = normal[i] - racketDesNormal[i-1];
}
}
/*!*****************************************************************************
*******************************************************************************
\note monitor_min_max
\date Dec. 2000
\remarks
monitors the min & max values of all joints continuously until keyboard
is hit. Then, an appropriate min_max structure is printed out for usuage
in SensorOffsets.cf
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
monitor_min_max(void)
{
int i,j;
Matrix minmax;
int n_bytes;
double offset = 0.05;
if (!servo_enabled) {
beep(1);
printf("ERROR: motor servo is not running!!\n");
return;
}
minmax = my_matrix(1,n_dofs,MIN_THETA,MAX_THETA);
for (i=1; i<=n_dofs; ++i) {
minmax[i][MIN_THETA] = 1.e10;
minmax[i][MAX_THETA] = -1.e10;
}
printf("Hit any key to stop monitoring .....");
fflush(stdout);
/* just check for min & max values */
while (TRUE) {
for (i=1; i<=n_dofs; ++i) {
/* update min/max values */
if (joint_state[i].th > minmax[i][MAX_THETA])
minmax[i][MAX_THETA] = joint_state[i].th;
if (joint_state[i].th < minmax[i][MIN_THETA])
minmax[i][MIN_THETA] = joint_state[i].th;
}
/* check whether the keyboard was hit */
#ifdef VX
ioctl(0 , FIONREAD, (int) (&n_bytes));
#else
ioctl( 0, FIONREAD, (void *) (&n_bytes) );
#endif
if (n_bytes != 0)
break;
taskDelay(ns2ticks(10000000)); // wait 10ms
}
getchar();
get_double("Safety margin from joint stop?",offset,&offset);
/* print the min/max values in SensorOffset.cf format */
for (i=1; i<=n_dofs; ++i) {
printf("%10s %f %f %f %f %f %f\n",
joint_names[i],
minmax[i][MIN_THETA]+offset,
minmax[i][MAX_THETA]-offset,
joint_default_state[i].th,
joint_opt_state[i].th,
joint_opt_state[i].w,
joint_range[i][THETA_OFFSET]);
}
printf("\n");
fflush(stdout);
my_free_matrix(minmax,1,n_dofs,MIN_THETA,MAX_THETA);
}
/*****************************************************************************
******************************************************************************
Function Name : init_gravityComp_task
Date : Dec. 1997
Remarks:
initialization for task
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
init_gravityComp_task(void)
{
int ans;
int j,i;
static int firsttime = TRUE;
if (firsttime == TRUE) {
xd = my_vector(1,3);
B = my_matrix(1,3,1,3);
J7T = my_matrix(1,7,1,3);
u_field = my_vector(1,7);
B[1][1] = -10.1;
B[1][2] = -11.2;
B[1][3] = 0.0;
B[2][1] = -11.2;
B[2][2] = -11.1;
B[2][3] = 0.0;
B[3][1] = 0.0;
B[3][2] = 0.0;
B[3][3] = 1.0;
//for (i=1;i<=3;i++) {
// for (j=1;j<=3;j++) {
//if (i==j) {
// B[i][j] = -1.0;
//}
//else {
// B[i][j] = 0.0;
//}
//}
//}
firsttime = FALSE;
}
/* go to a save posture */
bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
for (i=1; i<=N_DOFS; ++i) {
target[i].th = joint_default_state[i].th;
}
if (!go_target_wait_ID(target))
return FALSE;
/* initialize filter variables */
for(i=1; i<N_DOFS; i++) {
for(j=1; j<NZEROS; j++) {
xv[i][j]=0.0;
xa[i][j]=0.0;
}
for(j=1; j<NPOLES; j++) {
yv[i][j]=0.0;
ya[i][j]=0.0;
}
}
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
return TRUE;
}
