/*!***************************************************************************** ******************************************************************************* \note go_cart_target_wait \date \remarks go to the given cartesian target ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] ctar : cartesian states \param[in] stat : 1/0 for active target or not \param[in] mt : movement time ******************************************************************************/ int go_cart_target_wait(SL_Cstate *ctar,int *stat, double mt) { int i,j; double last_time; double last_draw_time; double aux; special_flag = TRUE; /* initialize some variables */ init_vars(); /* assign the target variables */ for (i=1; i<=n_endeffs; ++i) { for (j= _X_; j<= _Z_; ++j) { cstatus[(i-1)*6+j] = stat[(i-1)*6+j]; aux = ctar[i].x[j]; ctarget[i].x[j] = aux; } } /* the movement time */ tau = mt; if (!setTaskByName("Goto Cart Task")) { special_flag = FALSE; return FALSE; } last_time = last_draw_time = task_servo_time; while (strcmp(current_task_name,NO_TASK) != 0) { if (task_servo_time - last_time > 2*mt) { printf("time out in go_cart_target_wait\n"); special_flag = FALSE; return FALSE; } taskDelay(ns2ticks(10000000)); // wait 10ms } return TRUE; }
void enable_task_servo( int param ) { int status; int temp; if (!task_servo_initialized) { printf("ERROR: Task servo is not initialized\n"); return; } if ( param < 1 ) param = task_servo_ratio; if ( servo_enabled == 0 ) { servo_enabled = 1; task_servo_calls = 0; task_servo_time = 0; local_task_servo_ratio = param; task_servo_errors = 0; task_servo_rate = servo_base_rate/param; if (param == R60HZ) task_servo_rate = R60HZ; changeCollectFreq(task_servo_rate); setTaskByName(NO_TASK); setDefaultPosture(); task_servo_tid = taskSpawn("task_servo",0,VX_FP_TASK,10000, (FUNCPTR) task_servo,0,0,0,0,0,0,0,0,0,0); } else { fprintf( stderr, "task servo is already on!\n" ); } }
/*!***************************************************************************** ******************************************************************************* \note initSimulation \date July 1998 \remarks initializes everything needed for the simulation. ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] argc : number of elements in argv \param[in] argv : array of argc character strings ******************************************************************************/ int initSimulation(int argc, char** argv) { int i,j,n; int rc; int ans; /* initialize the servos */ init_task_servo(); /* the first servo sets the sampling rate in collect data */ read_whichDOFs("task_sim_servo"); init_motor_servo(); read_whichDOFs("motor_sim_servo"); init_vision_servo(); read_whichDOFs("vision_sim_servo"); init_invdyn_servo(); read_whichDOFs("invdyn_sim_servo"); /* we need the dynamics initalized */ init_dynamics(); /* user specific tasks */ initUserTasks(); /* create simulation windows */ if (!createWindows()) return FALSE; /* reset motor_servo variables */ servo_enabled = 1; motor_servo_calls = 0; servo_time = 0; motor_servo_rate = SERVO_BASE_RATE; zero_integrator(); bzero((char *)&(joint_sim_state[1]),sizeof(SL_DJstate)*N_DOFS); for (i=1; i<=N_DOFS; ++i) { joint_sim_state[i].th = joint_default_state[i].th; joint_des_state[i].th = joint_default_state[i].th; } /* reset task_servo variables */ servo_enabled = 1; task_servo_calls = 0; task_servo_time = 0; task_servo_errors = 0; task_servo_rate = SERVO_BASE_RATE/(double) task_servo_ratio; setTaskByName(NO_TASK); setDefaultPosture(); changeCollectFreq(task_servo_rate); /* reset vision servo variables */ servo_enabled = 1; vision_servo_calls = 0; vision_servo_time = 0; vision_servo_errors = 0; vision_servo_rate = VISION_SERVO_RATE; /* initialize all vision variables to safe values */ init_vision_states(); init_learning(); /* reset the invdyn servo variables */ servo_enabled = 1; invdyn_servo_errors = 0; invdyn_lookup_data = 0; invdyn_learning_data = 0; /* initalize objects in the environment */ readObjects(); /* assign contact force mappings */ #include "LEKin_contact.h" initContacts(); return TRUE; }
/******************************************************************************* ******************************************************************************* \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; }