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;
}
