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