/******************************************************************************* Automatic Calibration of Phantom Device - No character inputs *******************************************************************************/ void HHD_Auto_Calibration() { int supportedCalibrationStyles; HDErrorInfo error; hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles); if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET) { calibrationStyle = HD_CALIBRATION_ENCODER_RESET; ROS_INFO("HD_CALIBRATION_ENCODER_RESE.."); } if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL) { calibrationStyle = HD_CALIBRATION_INKWELL; ROS_INFO("HD_CALIBRATION_INKWELL.."); } if (supportedCalibrationStyles & HD_CALIBRATION_AUTO) { calibrationStyle = HD_CALIBRATION_AUTO; ROS_INFO("HD_CALIBRATION_AUTO.."); } if (calibrationStyle == HD_CALIBRATION_ENCODER_RESET) { do { hdUpdateCalibration(calibrationStyle); ROS_INFO("Calibrating.. (put stylus in well)"); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Reset encoders reset failed."); break; } } while (hdCheckCalibration() != HD_CALIBRATION_OK); ROS_INFO("Calibration complete."); } if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_MANUAL_INPUT) { ROS_INFO("Please place the device into the inkwell for calibration."); } }
HDCallbackCode HDCALLBACK HapticDevice::sScheduleOut(void *pUserData) { HapticSynchronizer * hs = (HapticSynchronizer *) pUserData; for(unsigned int i=0;i<NB_DEVICES_MAX;i++) { if(hs[i].m_data!=NULL) { hdMakeCurrentDevice(hs[i].m_data->m_id); hdBeginFrame(hs[i].m_data->m_id); hdGetDoublev(HD_CURRENT_POSITION, hs[i].m_data->m_position); hdGetDoublev(HD_CURRENT_FORCE, hs[i].m_data->m_force); hdGetDoublev(HD_CURRENT_TRANSFORM, hs[i].m_data->m_transform); hdGetIntegerv(HD_CURRENT_BUTTONS, &hs[i].m_data->m_buttons); hdGetDoublev(HD_CURRENT_VELOCITY, hs[i].m_data->m_velocity); hdEndFrame(hs[i].m_data->m_id); hs[i].m_free.m_position = hs[i].m_data->m_position; for(int j=0;j<16;j++) hs[i].m_free.m_transform[j] = hs[i].m_data->m_transform[j]; hs[i].m_free.m_buttons=hs[i].m_data->m_buttons; hs[i].m_free.m_force = hs[i].m_data->m_force; hs[i].m_free.m_velocity = hs[i].m_data->m_velocity; } } return HD_CALLBACK_DONE; }
/******************************************************************************* Checks the state of the gimbal button and gets the position of the device. *******************************************************************************/ HDCallbackCode HDCALLBACK updateDeviceCallback(void *pUserData) { int nButtons = 0; hdBeginFrame(hdGetCurrentDevice()); /* Retrieve the current button(s). */ hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons); /* In order to get the specific button 1 state, we use a bitmask to test for the HD_DEVICE_BUTTON_1 bit. */ gServoDeviceData.m_buttonState = (nButtons & HD_DEVICE_BUTTON_1) ? HD_TRUE : HD_FALSE; /* Get the current location of the device (HD_GET_CURRENT_POSITION) We declare a vector of three doubles since hdGetDoublev returns the information in a vector of size 3. */ hdGetDoublev(HD_CURRENT_POSITION, gServoDeviceData.m_devicePosition); /* Also check the error state of HDAPI. */ gServoDeviceData.m_error = hdGetError(); /* Copy the position into our device_data tructure. */ hdEndFrame(hdGetCurrentDevice()); return HD_CALLBACK_CONTINUE; }
// les réalisation des manipulations // retourne les valeurs possible HD_CALLBACK_DONE ou HD_CALLBACK_CONTINUE HDCallbackCode HDCALLBACK startManipulationCallBack(void *pUserData){ //cf touchBack hduVector3Dd v_position; // vector3d de type double hduVector3Dd v_force; // définition du vecteur force que l'on souhaite associer int button; // récupération des valeurs des boutons du bras haptic hdGetIntegerv(HD_CURRENT_BUTTONS,&button); g_button1 = (button & HD_DEVICE_BUTTON_1); g_button2 = (button & HD_DEVICE_BUTTON_2); g_button3 = (button & HD_DEVICE_BUTTON_3); if(g_button2) g_doExit = true; // si activation du bouton sortie ==> sortie. /* début de la manipulation */ hdBeginFrame(ghHD); hdGetDoublev(HD_CURRENT_POSITION, v_position); // récupération de la position du bras switch(g_selectMode){ case CAM: // si on est dans le mode camera { g_position_out.v[0] = (float)v_position[0]; g_position_out.v[1] = (float)v_position[1]; g_position_out.v[2] = (float)v_position[2]; break; } case CATCH: // si on est dans le mode "attraper un bloc" { /* déplacement de la main */ g_position_out.v[0] = (float)v_position[0]; g_position_out.v[1] = (float)v_position[1]; g_position_out.v[2] = (float)v_position[2]; /* ajout de la force de retour*/ hdSetDoublev(HD_CURRENT_FORCE, force_active); break; } } /* fin de la manipulation */ hdEndFrame(ghHD); return HD_CALLBACK_CONTINUE; }
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; }
void CybPhantom::checkCalibrationStatus(CalibrationState *pUserData) { int supportedCalibrationStyles; hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles); if(supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET) { pUserData->calibrationType = HD_CALIBRATION_ENCODER_RESET; } if(supportedCalibrationStyles & HD_CALIBRATION_INKWELL) { pUserData->calibrationType = HD_CALIBRATION_INKWELL; } if(supportedCalibrationStyles & HD_CALIBRATION_AUTO) { pUserData->calibrationType = HD_CALIBRATION_AUTO; } if(pUserData->calibrationStatus == HD_CALIBRATION_ENCODER_RESET){ printf("Calibration' type not implemented!"); exit(-1); } }
void phantomCalibrate() { int calibrationStyle; int supportedCalibrationStyles; HDErrorInfo error; hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles); if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET) { calibrationStyle = HD_CALIBRATION_ENCODER_RESET; printf("HD_CALIBRATION_ENCODER_RESE..\n\n"); } if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL) { calibrationStyle = HD_CALIBRATION_INKWELL; printf("HD_CALIBRATION_INKWELL..\n\n"); } if (supportedCalibrationStyles & HD_CALIBRATION_AUTO) { calibrationStyle = HD_CALIBRATION_AUTO; printf("HD_CALIBRATION_AUTO..\n\n"); } do { hdUpdateCalibration(calibrationStyle); printf("Calibrating.. (put stylus in well)\n"); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Reset encoders reset failed."); break; } } while (hdCheckCalibration() != HD_CALIBRATION_OK); printf("\n\nCalibration complete.\n"); }
HDCallbackCode HDCALLBACK touchScene(void *pUserData){ static const HDdouble stiffness = 0.05; hduVector3Dd position; hduVector3Dd initialPosition(0, 0, 0); hduVector3Dd force((double) g_force.x, (double) g_force.y, (double) g_force.z); int button; // Get Haptic Arm State hdGetIntegerv(HD_CURRENT_BUTTONS,&button); g_button1 = (button & HD_DEVICE_BUTTON_1); g_button2 = (button & HD_DEVICE_BUTTON_2); g_button3 = (button & HD_DEVICE_BUTTON_3); printf("\t%i\t%i %i %i\r",button,g_button1,g_button2,g_button3); if(g_button2) g_doExit = true; hdBeginFrame(ghHD); hdGetDoublev(HD_CURRENT_POSITION, position); switch(g_selecMode){ case MOVE: { hduVector3Dd tempForce = shakeBaby(); hduVecSubtract(force, initialPosition, position); hduVecScaleInPlace(force, stiffness); force += tempForce; hdSetDoublev(HD_CURRENT_FORCE, force); g_position_out.v[0] = (float)position[0]; g_position_out.v[1] = 0.f; g_position_out.v[2] = (float)position[2]; break; } case CAM: { hduVecSubtract(force, initialPosition, position); hduVecScaleInPlace(force, stiffness); hdSetDoublev(HD_CURRENT_FORCE, force); g_position_out.v[0] = (float)position[0]; g_position_out.v[1] = (float)position[1]; g_position_out.v[2] = 0.f; break; } case ARM : { g_position_out.v[0] = (float)position[0]; g_position_out.v[1] = (float)position[1]; g_position_out.v[2] = (float)position[2]; hduVecScaleInPlace(force, stiffness); hdSetDoublev(HD_CURRENT_FORCE, force); break; } } hdEndFrame(ghHD); return HD_CALLBACK_CONTINUE; }
//========================================================================== HDCallbackCode HDCALLBACK servoPhantomDevices(void* pUserData) { for (int i=0; i<PHANTOM_NUM_DEVICES_MAX; i++) { // for each activated phantom device if (phantomDevices[i].enabled) { // retrieve handle HHD hHD = phantomDevices[i].handle; // activate ith device hdMakeCurrentDevice(hHD); // start sending commands hdBeginFrame(hHD); // retrieve the position and orientation of the end-effector. double frame[16]; hdGetDoublev(HD_CURRENT_TRANSFORM, frame); // convert position from [mm] to [m] frame[12] = frame[12] * 0.001; frame[13] = frame[13] * 0.001; frame[14] = frame[14] * 0.001; phantomDevices[i].position[0] = frame[12]; phantomDevices[i].position[1] = frame[13]; phantomDevices[i].position[2] = frame[14]; phantomDevices[i].rotation[0] = frame[0]; phantomDevices[i].rotation[1] = frame[1]; phantomDevices[i].rotation[2] = frame[2]; phantomDevices[i].rotation[3] = frame[4]; phantomDevices[i].rotation[4] = frame[5]; phantomDevices[i].rotation[5] = frame[6]; phantomDevices[i].rotation[6] = frame[8]; phantomDevices[i].rotation[7] = frame[9]; phantomDevices[i].rotation[8] = frame[10]; // read linear velocity double vel[3]; hdGetDoublev(HD_CURRENT_VELOCITY, vel); // convert position from [mm] to [m] vel[0] = vel[0] * 0.001; vel[1] = vel[1] * 0.001; vel[2] = vel[2] * 0.001; phantomDevices[i].linearVelocity[0] = vel[0]; phantomDevices[i].linearVelocity[1] = vel[1]; phantomDevices[i].linearVelocity[2] = vel[2]; // read user buttons int buttons; hdGetIntegerv(HD_CURRENT_BUTTONS, &buttons); phantomDevices[i].button = buttons; // send force to end-effector double force[3]; force[0] = phantomDevices[i].force[0]; force[1] = phantomDevices[i].force[1]; force[2] = phantomDevices[i].force[2]; hdSetDoublev(HD_CURRENT_FORCE, force); // send torque to end-effector double torque[3]; torque[0] = phantomDevices[i].torque[0]; torque[1] = phantomDevices[i].torque[1]; torque[2] = phantomDevices[i].torque[2]; hdSetDoublev(HD_CURRENT_TORQUE, torque); // flush commands hdEndFrame(hHD); } } 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; }
/****************************************************************************** * Main scheduler callback for rendering the anchored spring force. *****************************************************************************/ HDCallbackCode HDCALLBACK AnchoredSpringForceCallback(void *pUserData) { static hduVector3Dd anchor; static HDboolean bRenderForce = (1==2); HDErrorInfo error; HDint nCurrentButtons, nLastButtons; hduVector3Dd position; hduVector3Dd force; force[0] = 0; force[1] = 0; force[2] = 0; hdBeginFrame(hdGetCurrentDevice()); hdGetDoublev(HD_CURRENT_POSITION, position); hdGetIntegerv(HD_CURRENT_BUTTONS, &nCurrentButtons); hdGetIntegerv(HD_LAST_BUTTONS, &nLastButtons); if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 && (nLastButtons & HD_DEVICE_BUTTON_1) == 0) { /* Detected button down */ memcpy(anchor, position, sizeof(hduVector3Dd)); bRenderForce = (1==1); } else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 && (nLastButtons & HD_DEVICE_BUTTON_1) != 0) { /* Detected button up */ bRenderForce = (1==2); /* Send zero force to the device, or else it will just continue rendering the last force sent */ hdSetDoublev(HD_CURRENT_FORCE, force); } if (bRenderForce) { /* Compute spring force as F = k * (anchor - pos), which will attract the device position towards the anchor position */ hduVecSubtract(force, anchor, position); hduVecScaleInPlace(force, gSpringStiffness); hdSetDoublev(HD_CURRENT_FORCE, force); } ////////////////////////////////////////////// //More stuff oliver has added: call to update position and force display// if (count>MAX_COUNT) { updateUser(position, force); count -= MAX_COUNT; } ////////////////////////////////////////////// ////////////////////////////////////////////// hdEndFrame(hdGetCurrentDevice()); /* Check if an error occurred while attempting to render the force */ if (HD_DEVICE_ERROR(error = hdGetError())) { if (hduIsForceError(&error)) { bRenderForce = (1==2); } else if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } count++; return HD_CALLBACK_CONTINUE; }