HDCallbackCode HDCALLBACK omni_state_callback(void *pUserData) { OmniState *omni_state = static_cast<OmniState *>(pUserData); if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE) { ROS_DEBUG("Updating calibration..."); hdUpdateCalibration(calibrationStyle); } hdBeginFrame(hdGetCurrentDevice()); //Get angles, set forces hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, omni_state->rot); hdGetDoublev(HD_CURRENT_POSITION, omni_state->position); hdGetDoublev(HD_CURRENT_JOINT_ANGLES, omni_state->joints); hduVector3Dd vel_buff(0, 0, 0); vel_buff = (omni_state->position * 3 - 4 * omni_state->pos_hist1 + omni_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif omni_state->velocity = (.2196 * (vel_buff + omni_state->inp_vel3) + .6588 * (omni_state->inp_vel1 + omni_state->inp_vel2)) / 1000.0 - (-2.7488 * omni_state->out_vel1 + 2.5282 * omni_state->out_vel2 - 0.7776 * omni_state->out_vel3); //cutoff freq of 20 Hz omni_state->pos_hist2 = omni_state->pos_hist1; omni_state->pos_hist1 = omni_state->position; omni_state->inp_vel3 = omni_state->inp_vel2; omni_state->inp_vel2 = omni_state->inp_vel1; omni_state->inp_vel1 = vel_buff; omni_state->out_vel3 = omni_state->out_vel2; omni_state->out_vel2 = omni_state->out_vel1; omni_state->out_vel1 = omni_state->velocity; if (omni_state->lock == true) { omni_state->force = 0.04 * (omni_state->lock_pos - omni_state->position) - 0.001 * omni_state->velocity; } hdSetDoublev(HD_CURRENT_FORCE, omni_state->force); //Get buttons int nButtons = 0; hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons); omni_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0; omni_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0; hdEndFrame(hdGetCurrentDevice()); HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error during main scheduler callback"); if (hduIsSchedulerError(&error)) return HD_CALLBACK_DONE; } float t[7] = { 0., omni_state->joints[0], omni_state->joints[1], omni_state->joints[2] - omni_state->joints[1], omni_state->rot[0], omni_state->rot[1], omni_state->rot[2] }; for (int i = 0; i < 7; i++) omni_state->thetas[i] = t[i]; return HD_CALLBACK_CONTINUE; }
HDCallbackCode HDCALLBACK phantom_state_callback(void *pUserData) { static bool lock_flag = true; PhantomState *phantom_state = static_cast<PhantomState *>(pUserData); hdBeginFrame(hdGetCurrentDevice()); //Get angles, set forces hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, phantom_state->rot); hdGetDoublev(HD_CURRENT_POSITION, phantom_state->position); hdGetDoublev(HD_CURRENT_JOINT_ANGLES, phantom_state->joints); hdGetDoublev(HD_CURRENT_TRANSFORM, phantom_state->hd_cur_transform); hduVector3Dd vel_buff(0, 0, 0); vel_buff = (phantom_state->position * 3 - 4 * phantom_state->pos_hist1 + phantom_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif // phantom_state->velocity = 0.0985*(vel_buff+phantom_state->inp_vel3)+0.2956*(phantom_state->inp_vel1+phantom_state->inp_vel2)-(-0.5772*phantom_state->out_vel1+0.4218*phantom_state->out_vel2 - 0.0563*phantom_state->out_vel3); //cutoff freq of 200 Hz phantom_state->velocity = (.2196 * (vel_buff + phantom_state->inp_vel3) + .6588 * (phantom_state->inp_vel1 + phantom_state->inp_vel2)) / 1000.0 - (-2.7488 * phantom_state->out_vel1 + 2.5282 * phantom_state->out_vel2 - 0.7776 * phantom_state->out_vel3); //cutoff freq of 20 Hz phantom_state->pos_hist2 = phantom_state->pos_hist1; phantom_state->pos_hist1 = phantom_state->position; phantom_state->inp_vel3 = phantom_state->inp_vel2; phantom_state->inp_vel2 = phantom_state->inp_vel1; phantom_state->inp_vel1 = vel_buff; phantom_state->out_vel3 = phantom_state->out_vel2; phantom_state->out_vel2 = phantom_state->out_vel1; phantom_state->out_vel1 = phantom_state->velocity; // printf("position x, y, z: %f %f %f \node_", phantom_state->position[0], phantom_state->position[1], phantom_state->position[2]); // printf("velocity x, y, z, time: %f %f %f \node_", phantom_state->velocity[0], phantom_state->velocity[1],phantom_state->velocity[2]); if (phantom_state->lock == true) { lock_flag = true; phantom_state->force = 0.04 * (phantom_state->lock_pos - phantom_state->position) - 0.001 * phantom_state->velocity; } else { if(lock_flag == true) { phantom_state->force.set(0.0, 0.0, 0.0); lock_flag = false; } } // Set force hdSetDoublev(HD_CURRENT_FORCE, phantom_state->force); // Set torque hdSetDoublev(HD_CURRENT_TORQUE, phantom_state->torque); //Get buttons int nButtons = 0; hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons); phantom_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0; phantom_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0; hdEndFrame(hdGetCurrentDevice()); HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error during main scheduler callback\n"); if (hduIsSchedulerError(&error)) return HD_CALLBACK_DONE; } float t[7] = {0., phantom_state->joints[0], phantom_state->joints[1], phantom_state->joints[2] - phantom_state->joints[1], phantom_state->rot[0], phantom_state->rot[1], phantom_state->rot[2]}; for (int i = 0; i < 7; i++) phantom_state->thetas[i] = t[i]; return HD_CALLBACK_CONTINUE; }