static int run_teacher_task ( void )
{
if (startFlag==1)
{
temp_time1 = gettime();
startFlag=0;
}
temp_time2 = gettime();
if ((temp_time2-temp_time1)/1000.0 > 2.0) //save teach-data every 10 ms
{
startFlag=1;
saveUnifyData(); // save vision data and joint state
//saveJoint_state(1, 1, 1, 1, 1, 1); // save jointPos,jointVel,jointAcc:(actual and desired); save cartpos,vel,acc:(actual);
}
int i = 0;
for ( i = 1; i <= N_DOFS; i++ )
{
joint_des_state[i].th = joint_state[i].th+joint_state[i].thd/task_servo_rate; //+joint_state[i].thdd/task_servo_rate/task_servo_rate;
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
}
SL_InvDyn( NULL, joint_des_state, endeff, &base_state, &base_orient );
check_range( joint_des_state );
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note run_sine_task
\date Dec. 1997
\remarks
run the task from the task servo: REAL TIME requirements!
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
run_sine_task(void)
{
int j, i;
task_time += 1./(double)task_servo_rate;
trans_mult = task_time/trans_period;
if (trans_mult > 1.0)
trans_mult = 1.0;
for (i=1; i<=n_dofs; ++i) {
joint_des_state[i].th = off[i];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
for (j=1; j<=n_sine[i]; ++j) {
joint_des_state[i].th +=
trans_mult*amp[i][j] * sin(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
joint_des_state[i].thd += trans_mult*amp[i][j] * 2.*PI*speed*freq[i][j] *
cos(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
joint_des_state[i].thdd += -trans_mult*amp[i][j] * sqr(2.*PI*speed*freq[i][j]) *
sin(2.*PI*speed*freq[i][j]*task_time+phase[i][j]);
}
}
if (invdyn)
SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : run_sample_task
Date : Dec. 1997
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
run_zero_task(void)
{
int i;
for(i=1; i<=N_DOFS; i++) {
joint_des_state[i].th = target[i].th;
}
SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
/*
* Check the safety of the desired joint states explicitly
* and then calculate the u_ff with inverse dynamics
*
* If friction compensation is turned on, then add some compensation on top of u_ff
*
*/
static void control_arm() {
check_safety();
// control the robot
// calculate the feedforward commands with inverse dynamics
SL_InvDyn(NULL, joint_des_state, endeff, &base_state, &base_orient);
if (friction_comp) {
addFrictionModel(joint_des_state);
}
}
/*******************************************************************************
*******************************************************************************
\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;
}
/*!*****************************************************************************
*******************************************************************************
\note run_goto_task
\date Dec. 1997
\remarks
run the task from the task servo: REAL TIME requirements!
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
run_goto_cart_task(void)
{
int j, i;
double sum=0;
double aux;
/* has the movement time expired? I intentially run 0.5 sec longer */
if (tau <= -0.5 || (tau <= 0.0 && special_flag)) {
freeze();
return TRUE;
}
/* progress by min jerk in cartesian space */
calculate_min_jerk_next_step(cnext,ctarget,tau,time_step,cnext);
tau -= time_step;
/* shuffle the target for the inverse kinematics */
for (i=1; i<=n_endeffs; ++i) {
for (j=1; j<=N_CART; ++j) {
aux = cnext[i].x[j] - cart_des_state[i].x[j];
cart[(i-1)*6+j] = cnext[i].xd[j] + 20.*aux;
}
}
/* inverse kinematics */
for (i=1; i<=n_dofs; ++i) {
target[i].th = joint_des_state[i].th;
}
if (!inverseKinematics(target,endeff,joint_opt_state,
cart,cstatus,time_step)) {
freeze();
return FALSE;
}
/* prepare inverse dynamics */
for (i=1; i<=n_dofs; ++i) {
aux = (target[i].thd-joint_des_state[i].thd)*(double)task_servo_rate;
target[i].thdd = filt(aux,&(fthdd[i]));
joint_des_state[i].thdd = target[i].thdd;
joint_des_state[i].thd = target[i].thd;
joint_des_state[i].th = target[i].th;
if (joint_des_state[i].th > joint_range[i][MAX_THETA]) {
joint_des_state[i].th = joint_range[i][MAX_THETA];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
}
if (joint_des_state[i].th < joint_range[i][MIN_THETA]) {
joint_des_state[i].th = joint_range[i][MIN_THETA];
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
}
}
/* compute inverse dynamics */
SL_InvDyn(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
