/*****************************************************************************
******************************************************************************
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 init_traj_task
\date Jun. 1999
\remarks
initialization for task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int init_traj_task(void) {
int j, i, c;
char string[100];
double max_range = 0;
static int ans = 0;
/* check whether any other task is running */
if (strcmp(current_task_name, NO_TASK) != 0) {
printf("New task can only be run if no other task is running!\n");
return FALSE;
}
//printf("Servo base rate: %d\n", servo_base_rate);
//printf("Task servo ratio: %d\n", task_servo_ratio);
printf("Running OLD SL!\n");
/* read the script for this task */
if (!read_traj_file())
return FALSE;
// make sure 7th joint stays fixed
//fix_joint(7);
if (!check_traj_range())
return FALSE;
/* check for repeatable task */
repeat_flag = TRUE;
/* do we want to repeat task */
if (repeat_flag) {
ans = 1;
get_int("Do you want this task to repeat itself? 1 = y, 0 = n", ans, &ans);
if (ans != 1)
repeat_flag = FALSE;
}
get_int("Friction Compensation?", FALSE, &friction_comp);
if (friction_comp) {
printf("Loading stiction model...\n");
loadStictionModel("./prefs/stiction.pref");
}
/* go to starting posture */
if (!goto_posture(q0))
return FALSE;
printf("Starting initialization with e0 = %f.\n", euclidian_dist(joint_state, q0));
// calculate inverse dynamics commands
static int real_flag = FALSE; // actually move the arm or not
track_with_idm(real_flag);
/* go to starting posture */
if (real_flag) {
if (!goto_posture(q0))
return FALSE;
printf("Starting trajectory tracking with e0 = %f.\n", euclidian_dist(joint_state, q0));
}
/* debug forward dynamics */
//test_ilc();
/* initialize ILC */
ilc_main();
//update_basic();
// WARN user for PID control
printf("Dont forget to change controller to PIDFF if you need to with ck()!\n");
/* do we really want to do this task? */
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;
// for real setup
setDefaultEndeffector();
endeff[RIGHT_HAND].x[_Z_] = .3;
// turn off real time
changeRealTime(TRUE);
toggle_fb();
task_time = 0.0;
//scd();
return TRUE;
}
/*
* 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];
}
}
