/*****************************************************************************
******************************************************************************
Function Name : run_tt_task
Date : Dec 2013
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int run_mpc_task(void) {
int i,j;
static int firsttime = TRUE;
// time for moving from desired hitting state to desired resting state
static double returnToRestTime = 1.0;
// time to wait for simulation
static double timeMaxWait = 6.0;
// the time it takes to reach from initial state (start) to hitting point
// = predicted time it takes the ball from current (est.) position to the hitting point
static double hitTime;
// solely for simulation
static double timeSimInit;
// reset kalman filter with this variable
static int initKF;
// we have this new variable in optim task
static double landTime;
static Vector ballLand;
// desired arm configuration, can be used for hitting state
static SL_DJstate target_joint_state[N_DOFS+1];
/* thread optimization variables */
static nlopt_thread_data *optim_data;
if (firsttime) {
firsttime = FALSE;
ballLand = my_vector(1,N_CART);
set_des_land_param(ballLand, &landTime);
timeSimInit = get_time();
initKF = TRUE;
resetSim = FALSE;
bzero((char *)&(target_joint_state[1]), N_DOFS * sizeof(target_joint_state[1]));
optim_data = (nlopt_thread_data *)malloc(sizeof(nlopt_thread_data));
}
// estimate ball state with a filter
filter_ball_state(&initKF,&resetSim);
// lookup + initialize optimization and get the hitting parameters
calc_optim_param(ballLand, landTime, target_joint_state, optim_data, &hitTime);
// generate movement or calculate next desired step
calc_next_state(*optim_data, returnToRestTime, target_joint_state, &hitTime);
// check safety and calculate inv dyn
control_arm();
// resetting ball in simulation mode
if(simulation) {
if ((((get_time() - timeSimInit) > timeMaxWait * 1e6) || resetSim) && !moving) {
reset_sim_ball();
resetSim = FALSE;
initKF = TRUE;
timeSimInit = get_time();
}
}
display_sim_ball();
return TRUE;
}
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;
}
