/*
* Calculate the optimization parameters using an NLOPT nonlinear optimization algorithm
* in another thread
*
* The optimized parameters are: qf, qf_dot, T
* assuming T_return and q0 are fixed. First using a lookup to initialize a reasonable estimate.
*
* We activate MPC only if
* 1. new observations are available
* 2. only with a certain frequency
* 3. only if optimization thread is not active
* 4. only if new observations (e.g. filtered state) indicate the ball is infront of the robot
*
* TODO: can we increase the mpc_limit ?
* how do we make sure we do not try to hit not-legal balls?
*
*/
static void calc_optim_param(Vector ballLand, double landTime, SL_DJstate target_joint_state[N_DOFS+1], nlopt_thread_data *optim_data, double *hitTime) {
int i;
static pthread_t nlopt_thread;
static int rc;
static int firsttime = TRUE;
static double lastTime = 0.0;
static int FREQ_MPC = 20;
static double MPC_LIMIT_Y;
double currentTime = get_time();
int activateUpdate = (currentTime - lastTime) > (1e6) * (1.0/FREQ_MPC);
int passedRobot = ballPred.x[_Y_] > cart_state[1].x[_Y_];
if (firsttime) {
MPC_LIMIT_Y = dist_to_table/2;
firsttime = FALSE;
}
if (check_new_ball_obs(ballPred) && activateUpdate &&
(!passedRobot) && ballPred.xd[_Y_] > 1.0 &&
ballPred.x[_Y_] < MPC_LIMIT_Y) {
predict_ball_path(&ballPred);
calc_racket_strategy(ballLand, landTime);
trj_time = time_step;
lastTime = currentTime;
moving = TRUE;
// for good initialization, and to immediately start hitting
cp_lookup_param(optim_data,target_joint_state,hitTime);
for (i = 1; i <= N_DOFS; i++) {
current_joint_state[i].th = joint_state[i].th;
current_joint_state[i].thd = joint_state[i].thd;
}
// run optimization in another thread
if (!busy) {
printf("Predicted when ball_y : %f\n", ballPred.x[_Y_]);
if ((rc = pthread_create(&nlopt_thread, NULL, multi_thread_nlopt_optim, optim_data))) {
fprintf(stderr, "Error: pthread_create, rc: %d\n", rc);
freeze();
}
}
}
}
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;
}
