/*!*****************************************************************************
*******************************************************************************
\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;
}
