/****************************************************************************** Adds data for one tick. Return true if done ******************************************************************************/ HDboolean hduRecorder::addData() { /* Get the data and fill one packet. */ static hduRecorderPacket data; hdGetDoublev(HD_CURRENT_FORCE,data.m_force); hdGetDoublev(HD_CURRENT_POSITION,data.m_position); hdGetDoublev(HD_CURRENT_VELOCITY,data.m_velocity); data.m_userData = 0; if (m_pUserCallback) { char *userData = (*m_pUserCallback)(m_pUserCallbackData); data.m_userData = userData; } /* Either add the data to storage, or stop getting data if the recorder has received the amount specified. */ if (m_index < m_numData) { m_data[m_index] = data; m_index++; return false; } else { return true; } }
/****************************************************************************** hduMapWorkspaceModel Determines a mapping for the haptic device's workspace so that it optimally fits within the viewing volume provided and is aligned with the view. Parameters: modelMatrix A pointer to a 4x4 modelview matrix stored in column-major order. projMatrix A pointer to a 4x4 projection matrix stored in column-major order. wsViewMatrix A pointer to a 4x4 matrix used for storing the resultant column-major order workspaceview transform. ******************************************************************************/ void hduMapWorkspaceModel(const HDdouble *modelMatrix, const HDdouble *projMatrix, HDdouble *wsModelMatrix) { hduMatrix worldTeye(modelMatrix); hduMatrix eyeTworld = worldTeye; bool bNoError = eyeTworld.invert(); assert(bNoError); HDdouble aUsableWorkspace[6]; HDdouble aMaxWorkspace[6]; hdGetDoublev(HD_USABLE_WORKSPACE_DIMENSIONS, aUsableWorkspace); hdGetDoublev(HD_MAX_WORKSPACE_DIMENSIONS, aMaxWorkspace); HDdouble aWorkspaceBounds[6]; for (int i = 0; i < 6; i++) { aWorkspaceBounds[i] = (aUsableWorkspace[i] + aMaxWorkspace[i]) / 2.0; } /* Scale based on the entire extent of the view volume */ HDdouble zNear = 0.0; HDdouble zFar = 1.0; hduMatrix workspaceTeye; hduMapWorkspace(projMatrix, aWorkspaceBounds, zNear, zFar, workspaceTeye); /* Now apply the eyeTworld transform to the workspace transform (i.e. workspaceTeye * eyeTworld). */ hduMatrix workspaceTworld = workspaceTeye; workspaceTworld.multRight(eyeTworld); memcpy(wsModelMatrix, workspaceTworld, 16 * sizeof(HDdouble)); }
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; }
HDCallbackCode HDCALLBACK CybPhantom::getStateCB(void *pUserData) { HapticState *copyState; copyState = (HapticState *) pUserData; if(copyState->flagState == 0) { hdGetDoublev(HD_CURRENT_FORCE, copyState->force); } else if(copyState->flagState == 1) { hdGetDoublev(HD_CURRENT_TORQUE, copyState->torque); } else if(copyState->flagState == 2) { hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, copyState->gimbalAngle); } else if(copyState->flagState == 3) { hdGetDoublev(HD_CURRENT_JOINT_ANGLES, copyState->jointAngle); } else if(copyState->flagState == 4) { hdGetDoublev(HD_CURRENT_POSITION, copyState->pos); } else { //cout<<"FlagState error: Invalid flag!"<<endl; return HD_CALLBACK_DONE; } return HD_CALLBACK_DONE; }
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; }
/******************************************************************************* Client callback used by the graphics main loop function. Use this callback synchronously. Gets data, in a thread safe manner, that is constantly being modified by the haptics thread. *******************************************************************************/ HDCallbackCode HDCALLBACK DeviceStateCallback(void *pUserData) { DeviceDisplayState *pDisplayState = static_cast<DeviceDisplayState *>(pUserData); hdGetDoublev(HD_CURRENT_POSITION, pDisplayState->position); hdGetDoublev(HD_CURRENT_FORCE, pDisplayState->force); // execute this only once. return HD_CALLBACK_DONE; }
/****************************************************************************** The main servo loop scheduler callback. Samples the haptic frame rate. ******************************************************************************/ HDCallbackCode HDCALLBACK ServoSchedulerCallback(void *pUserData) { hdBeginFrame(hdGetCurrentDevice()); HDdouble rate; hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &rate); hdEndFrame(hdGetCurrentDevice()); gUpdateRateServo.sample(rate); HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { std::cerr << std::endl; std::cerr << error << std::endl; if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } return HD_CALLBACK_CONTINUE; }
/******************************************************************************* 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; }
/******************************************************************************* Given the position of the two charges in space, calculates the (modified) coulomb force. *******************************************************************************/ hduVector3Dd forceField(hduVector3Dd Pos1, hduVector3Dd Pos2, HDdouble Multiplier, HLdouble Radius) { hduVector3Dd diffVec = Pos2 - Pos1 ;//Find the difference in position double dist = 0.0; hduVector3Dd forceVec(0,0,0); HDdouble nominalMaxContinuousForce; hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &nominalMaxContinuousForce);//Find the max continuous for that the device is capable of dist = diffVec.magnitude(); if(dist < Radius*2.0) //Spring force (when the model and cursor are within a 'sphere of influence' { diffVec.normalize(); forceVec = (Multiplier) * diffVec * dist /(4.0 * Radius * Radius); static int i=0; } else //Inverse square attraction { forceVec = Multiplier * diffVec/(dist*dist); } for(int i=0;i<3;i++)//Limit force calculated to Max continuouis. This is very important because force values exceeding this value can damage the device motors. { if(forceVec[i]>nominalMaxContinuousForce) forceVec[i] = nominalMaxContinuousForce; if(forceVec[i]<-nominalMaxContinuousForce) forceVec[i] = -nominalMaxContinuousForce; } return forceVec; }
/******************************************************************************* Main callback that calculates and sets the force. *******************************************************************************/ HDCallbackCode HDCALLBACK CoulombCallback(void *data) { HHD hHD = hdGetCurrentDevice(); hdBeginFrame(hHD); hduVector3Dd pos; hdGetDoublev(HD_CURRENT_POSITION,pos); hduVector3Dd forceVec; forceVec = forceField(pos); hdSetDoublev(HD_CURRENT_FORCE, forceVec); hdEndFrame(hHD); HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error during scheduler callback"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } return HD_CALLBACK_CONTINUE; }
HDCallbackCode HDCALLBACK CybPhantom::setForce(void *pUserData) { //cout << "setForce()" << endl; hduVector3Dd position;//buffer para a recepção de dados do socket hduVector3Dd force; HDdouble forceClamp; static const HDdouble kAnchorStiffness = 0.1; hdBeginFrame(hdGetCurrentDevice()); /*Começa o frame.*/ hdGetDoublev(HD_CURRENT_POSITION, position); //cout << anchorPosition[0] << " " << anchorPosition[1] << " " << anchorPosition[2] << endl; hduVecSubtract(force, anchor, position); hduVecScaleInPlace(force, kAnchorStiffness); //hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &forceClamp); /*if (hduVecMagnitude(force) > forceClamp) { hduVecNormalizeInPlace(force); hduVecScaleInPlace(force, forceClamp); }*/ //cout << force[0] << " " << force[1] << " " << force[2] << endl; hdSetDoublev(HD_CURRENT_FORCE, force); hdEndFrame(hdGetCurrentDevice()); return HD_CALLBACK_CONTINUE; }
/******************************************************************************* Haptic sphere callback. The sphere is oriented at 0,0,0 with radius 40, and provides a repelling force if the device attempts to penetrate through it. *******************************************************************************/ HDCallbackCode HDCALLBACK FrictionlessSphereCallback(void *data) { const double sphereRadius = 10.0; const hduVector3Dd spherePosition(0,50,0); // Stiffness, i.e. k value, of the sphere. Higher stiffness results // in a harder surface. const double sphereStiffness = .01; hdBeginFrame(hdGetCurrentDevice()); // Get the position of the device. hduVector3Dd position; hdGetDoublev(HD_CURRENT_POSITION, position); // Find the distance between the device and the center of the // sphere. double distance = (position-spherePosition).magnitude(); // If the user is within the sphere -- i.e. if the distance from the user to // the center of the sphere is less than the sphere radius -- then the user // is penetrating the sphere and a force should be commanded to repel him // towards the surface. if (distance > sphereRadius) { // Calculate the penetration distance. double penetrationDistance = sphereRadius-distance; // Create a unit vector in the direction of the force, this will always // be outward from the center of the sphere through the user's // position. hduVector3Dd forceDirection = (position-spherePosition)/distance; // Use F=kx to create a force vector that is away from the center of // the sphere and proportional to the penetration distance, and scsaled // by the object stiffness. // Hooke's law explicitly: double k = sphereStiffness; hduVector3Dd x = penetrationDistance*forceDirection; hduVector3Dd f = k*x; hdSetDoublev(HD_CURRENT_FORCE, f); } 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; } } 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; }
static void changeAleaForce(){ static HDdouble timer = 0; static const hduVector3Dd direction(0, 1, 0); HDdouble instRate; hduVector3Dd force; hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate); timer += 1.0 / instRate; hduVecScale(force, direction, 300 * sin(timer)); }
/****************************************************************************** Main function. ******************************************************************************/ int main(int argc, char* argv[]) { HDErrorInfo error; printf("Starting application\n"); atexit(exitHandler); // Initialize the device. This needs to be called before any other // actions on the device are performed. ghHD = hdInitDevice(HD_DEFAULT_DEVICE); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Failed to initialize haptic device"); fprintf(stderr, "\nPress any key to quit.\n"); getchar(); exit(-1); } printf("Found device %s\n",hdGetString(HD_DEVICE_MODEL_TYPE)); hdEnable(HD_FORCE_OUTPUT); hdEnable(HD_MAX_FORCE_CLAMPING); hdStartScheduler(); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Failed to start scheduler"); fprintf(stderr, "\nPress any key to quit.\n"); getchar(); exit(-1); } initGlut(argc, argv); // Get the workspace dimensions. HDdouble maxWorkspace[6]; hdGetDoublev(HD_MAX_WORKSPACE_DIMENSIONS, maxWorkspace); // Low/left/back point of device workspace. hduVector3Dd LLB(maxWorkspace[0], maxWorkspace[1], maxWorkspace[2]); // Top/right/front point of device workspace. hduVector3Dd TRF(maxWorkspace[3], maxWorkspace[4], maxWorkspace[5]); initGraphics(LLB, TRF); // Application loop. CoulombForceField(); printf("Done\n"); return 0; }
/****************************************************************************** Main function. ******************************************************************************/ int main(int argc, char* argv[]) { HDErrorInfo error; HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Failed to initialize haptic device"); fprintf(stderr, "\nPress any key to quit.\n"); getch(); return -1; } printf("Anchored Spring Force Example\n"); /* Schedule the haptic callback function for continuously monitoring the button state and rendering the anchored spring force. */ gCallbackHandle = hdScheduleAsynchronous( AnchoredSpringForceCallback, 0, HD_MAX_SCHEDULER_PRIORITY); hdEnable(HD_FORCE_OUTPUT); /* Query the max closed loop control stiffness that the device can handle. Using a value beyond this limit will likely cause the device to buzz. */ hdGetDoublev(HD_NOMINAL_MAX_STIFFNESS, &gMaxStiffness); /* Start the haptic rendering loop. */ hdStartScheduler(); if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Failed to start scheduler"); fprintf(stderr, "\nPress any key to quit.\n"); getch(); return -1; } /* Start the main application loop. */ mainLoop(); /* Cleanup by stopping the haptics loop, unscheduling the asynchronous callback, disabling the device. */ hdStopScheduler(); hdUnschedule(gCallbackHandle); hdDisableDevice(hHD); return 0; }
/*************************************************************************************** Servo loop thread callback. Computes a force effect. This callback defines the motion of the purple skull and calculates the force based on the "real-time" Proxy position in Device space. ****************************************************************************************/ void HLCALLBACK computeForceCB(HDdouble force[3], HLcache *cache, void *userdata) { DataTransportClass *localdataObject = (DataTransportClass *) userdata;//Typecast the pointer passed in appropriately hduVector3Dd skullPositionDS;//Position of the skull (Moving sphere) in Device Space. hduVector3Dd proxyPosition;//Position of the proxy in device space HDdouble instRate = 0.0; HDdouble deltaT = 0.0; static float counter = 0.0; float degInRad = 0.0; static int counter1 = 0; // Get the time delta since the last update. hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate); deltaT = 1.0 / instRate; counter+=deltaT; degInRad = counter*20*3.14159/180; hduVector3Dd ModelPos = localdataObject->Model->getTranslation(); localdataObject->Model->setTranslation(-ModelPos); localdataObject->Model->setTranslation(cos(degInRad)*64.0, sin(degInRad)*64.0,5.0);//Move the skull aroubnd in a circle WorldToDevice.multVecMatrix(localdataObject->Model->getTranslation(),skullPositionDS);//Convert the position of the sphere from world space to device space hlCacheGetDoublev(cache, HL_PROXY_POSITION, proxyPosition);//Get the position of the proxy in Device Coordinates (All HL commands in the servo loop callback fetch values in device coordinates) forceVec = forceField(proxyPosition, skullPositionDS, 40.0, 5.0);//Calculate the force counter1++; if(counter1>2000)//Make the force start after 2 seconds of program start. This is because the servo loop thread executes before the graphics thread. //Hence global variables set in the graphics thread will not be valid for sometime in the begining og the program { force[0] = forceVec[0]; force[1] = forceVec[1]; force[2] = forceVec[2]; counter1 = 2001; } else { force[0] = 0.0; force[1] = 0.0; force[2] = 0.0; } }
hduVector3Dd shakeBaby(){ static const hduVector3Dd direction(0, 1, 0); hduVector3Dd force; //current force applied to the arm HDdouble instRate; static HDdouble timer = 0; gVibrationFreq = (HDint) g_speed * 10; gVibrationFreq = gVibrationFreq < 0 ? 0 : gVibrationFreq; gVibrationFreq = gVibrationFreq > 500 ? 500 : gVibrationFreq; /* Use the reciprocal of the instantaneous rate as a timer. */ hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate); timer += 1.0 / instRate; /* Apply a sinusoidal force in the direction of motion. */ hduVecScale(force, direction, gVibrationAmplitude * sin(timer * gVibrationFreq)); return force; }
int _tmain(int argc, _TCHAR* argv[]) { ghHD = HD_INVALID_HANDLE; gSchedulerCallback = HD_INVALID_HANDLE; bool returnValue = true; int devideID = 0; g_doExit = false; //Initialize Haptic device HDErrorInfo error; // Initialize the device. This needs to be called before any actions on the // device. ghHD = hdInitDevice(HD_DEFAULT_DEVICE); if (HD_DEVICE_ERROR(error = hdGetError())) returnValue = false; hdEnable(HD_FORCE_OUTPUT); hdEnable(HD_MAX_FORCE_CLAMPING); // Initialize amplitude for move vribration hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &gVibrationAmplitude); gVibrationAmplitude *= 0.75; gSchedulerCallback = hdScheduleAsynchronous(touchScene, 0, HD_MAX_SCHEDULER_PRIORITY); hdStartScheduler(); if (HD_DEVICE_ERROR(error = hdGetError())) returnValue = false; atexit(ExitHandler); while(!g_doExit){ //printf("%i %i %i\n",g_button1,g_button2,g_button3); } return 0; }
//========================================================================== int hdPhantomGetWorkspaceRadius(const int a_deviceID, double *a_workspaceRadius) { // check id if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); } // check if servo started if (!servoStarted) { hdPhantomStartServo(); } // check if enabled if (!phantomDevices[a_deviceID].enabled) { return (-1); } // retrieve handle HHD hHD = phantomDevices[a_deviceID].handle; // activate ith device hdMakeCurrentDevice(hHD); // read workspace of device double size[6]; hdGetDoublev(HD_USABLE_WORKSPACE_DIMENSIONS, size); double sizeX = size[3] - size[0]; double sizeY = size[4] - size[1]; double sizeZ = size[5] - size[2]; double radius = 0.5 * sqrt(sizeX * sizeX + sizeY * sizeY + sizeZ * sizeZ); // convert value to [m] phantomDevices[a_deviceID].workspaceRadius = 0.001 * radius; // return estimated workspace radius *a_workspaceRadius = phantomDevices[a_deviceID].workspaceRadius; // success return (0); }
//========================================================================== 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 HapticDevice::aSchedule(void *pUserData) { HapticSynchronizer * hs = (HapticSynchronizer *) pUserData; for(unsigned int i=0;i<NB_DEVICES_MAX;i++) { if(hs[i].m_data!=NULL) { HapticData * data = hs[i].m_data; hduVector3Dd force( 0, 0, 0 ); hdBeginFrame(hs[i].m_data->m_id); HDdouble forceClamp; hduVector3Dd distance = data->m_realPosition - data->m_position; force = data->m_force; /* if we are nearly at the center don't recalculate anything, since the center is a singular point. */ if(data->m_nbCollision == 1) { if(distance.magnitude() > EPSILON && data->m_ready) { /* force is calculated from F=kx; k is the stiffness and x is the vector magnitude and direction. In this case, k = STIFFNESS, and x is the penetration vector from the actual position to the desired position. */ force = STIFFNESS*distance; force *= BALL_MASS; force[0] = 0; } } // Check if we need to clamp the force. hdGetDoublev(HD_NOMINAL_MAX_FORCE, &forceClamp); if (hduVecMagnitude(force) > forceClamp) { hduVecNormalizeInPlace(force); hduVecScaleInPlace(force, forceClamp); } hdSetDoublev(HD_CURRENT_FORCE, force); hdEndFrame(data->m_id); } } HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { // hduPrintError(stderr, &error, "Error during scheduler callback"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } return HD_CALLBACK_CONTINUE; }
HDCallbackCode BaseGeometryPatch::patchCalc(){ HDErrorInfo error; hduVector3Dd forceVec; hduVector3Dd targForceVec; hduVector3Dd loopForceVec; hduVector3Dd patchMinusDeviceVec; hduVector3Dd sensorVec; double sensorRadius = 1.0; if(simToPhantom == NULL){ return HD_CALLBACK_CONTINUE; } HHD hHD = hdGetCurrentDevice(); // reset all the variables that need to accumulate for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){ phantomToSim->forceMagnitude[sensi] = 0; phantomToSim->forceVec[sensi][0] = 0; phantomToSim->forceVec[sensi][1] = 0; phantomToSim->forceVec[sensi][2] = 0; } // Begin haptics frame. ( In general, all state-related haptics calls // should be made within a frame. ) hdBeginFrame(hHD); /* Get the current devicePos of the device. */ hdGetDoublev(HD_CURRENT_POSITION, devicePos); hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, deviceAngle); hdGetDoublev(HD_CURRENT_TRANSFORM, transformMat); forceVec[0] = 0; forceVec[1] = 0; forceVec[2] = 0; if(simToPhantom->numTargets > SimToPhantom::MAX_TARGETS) simToPhantom->numTargets = SimToPhantom::MAX_TARGETS; // clear all the data structures if(phantomToSim != NULL){ for(int i = 0; i < PhantomToSim::NUM_SENSORS; ++i){ phantomToSim->forceMagnitude[i] = 0; phantomToSim->forceVec[i][0] = 0; phantomToSim->forceVec[i][1] = 0; phantomToSim->forceVec[i][2] = 0; for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){ phantomToSim->targForceMagnitude[j][i] = 0; phantomToSim->targForceVec[j][i][0] = 0; phantomToSim->targForceVec[j][i][1] = 0; phantomToSim->targForceVec[j][i][2] = 0; } } for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){ phantomToSim->targForcesMagnitude[j] = 0; } } /***/ //printf("BaseGeometryPatch::patchCalc() numtargets = %d\n", simToPhantom->numTargets); for(int targi = 0; targi < simToPhantom->numTargets; ++targi){ patchPos[0] = simToPhantom->targetX[targi]; patchPos[1] = simToPhantom->targetY[targi]; patchPos[2] = simToPhantom->targetZ[targi]; patchRadius = simToPhantom->targetRadius[targi]; targForceVec[0] = 0; targForceVec[1] = 0; targForceVec[2] = 0; for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){ loopForceVec[0] = 0; loopForceVec[1] = 0; loopForceVec[2] = 0; sensorVec[0] = simToPhantom->sensorX[sensi] + devicePos[0]; sensorVec[1] = simToPhantom->sensorY[sensi] + devicePos[1]; sensorVec[2] = simToPhantom->sensorZ[sensi] + devicePos[2]; sensorRadius = simToPhantom->sensorRadius[sensi]; // patchMinusDeviceVec = patchPos-devicePos < // Create a vector from the device devicePos towards the sphere's center. //hduVecSubtract(patchMinusDeviceVec, patchPos, devicePos); hduVecSubtract(patchMinusDeviceVec, patchPos, sensorVec); hduVector3Dd dirVec; hduVecNormalize(dirVec, patchMinusDeviceVec); // If the device position is within the sphere's surface // center, apply a spring forceVec towards the surface. The forceVec // calculation differs from a traditional gravitational body in that the // closer the device is to the center, the less forceVec the well exerts; // the device behaves as if a spring were connected between itself and // the well's center. * double penetrationDist = patchMinusDeviceVec.magnitude() - (patchRadius+sensorRadius); if(penetrationDist < 0) { // > F = k * x < // F: forceVec in Newtons (N) // k: Stiffness of the well (N/mm) // x: Vector from the device endpoint devicePos to the center // of the well. hduVecScale(dirVec, dirVec, penetrationDist); hduVecScale(loopForceVec, dirVec, stiffnessK); /*** for(int i = 0; i < 3; ++i) if(loopForceVec[i] < 0) loopForceVec[i] *= 0.5; printf("targForceVec[%d][%d] = %.2f, %.2f, %.2f\r", targi, sensi, loopForceVec[0], loopForceVec[1], loopForceVec[2]); /****/ } if(phantomToSim != NULL){ phantomToSim->forceMagnitude[sensi] += hduVecMagnitude(loopForceVec); phantomToSim->forceVec[sensi][0] += loopForceVec[0]; phantomToSim->forceVec[sensi][1] += loopForceVec[1]; phantomToSim->forceVec[sensi][2] += loopForceVec[2]; phantomToSim->targForceMagnitude[targi][sensi] = hduVecMagnitude(loopForceVec); phantomToSim->targForceVec[targi][sensi][0] = loopForceVec[0]; phantomToSim->targForceVec[targi][sensi][1] = loopForceVec[1]; phantomToSim->targForceVec[targi][sensi][2] = loopForceVec[2]; } hduVecAdd(targForceVec, targForceVec, loopForceVec); hduVecAdd(forceVec, forceVec, loopForceVec); } if(phantomToSim != NULL){ phantomToSim->targForcesMagnitude[targi] = hduVecMagnitude(targForceVec); } } if(phantomToSim != NULL){ for(int i = 0; i < 16; ++i){ phantomToSim->matrix[i] = transformMat[i]; } } // divide the forceVec the number of times that a force was added? /* Send the forceVec to the device. */ if(simToPhantom->enabled){ hdSetDoublev(HD_CURRENT_FORCE, forceVec); } /* End haptics frame. */ hdEndFrame(hHD); /* Check for errors and abort the callback if a scheduler error is detected. */ if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "BaseGeometryPatch.calcPatch():\n"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } /* Signify that the callback should continue running, i.e. that it will be called again the next scheduler tick. */ 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 OmniForceCallback(void *pUserData/*, Omni device*/) { static hduVector3Dd anchor; static HDboolean bRenderForce = FALSE; 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 = TRUE; } else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 && (nLastButtons & HD_DEVICE_BUTTON_1) != 0) { * Detected button up * bRenderForce = FALSE; * 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); }*/ if (count>MAX_COUNT) { device.updateUser(); 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 = FALSE; } else if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } count++; return HD_CALLBACK_CONTINUE; }
/******************************************************************************* Servo callback. Called every servo loop tick. Simulates a gravity well, which sucks the device towards its center whenever the device is within a certain range. *******************************************************************************/ HDCallbackCode HDCALLBACK gravityWellCallback(void *data) { //const HDdouble kStiffness = 0.075; /* N/mm */ const HDdouble kStiffness = 0.045; /* N/mm */ const HDdouble kGravityWellInfluence = 400; /* mm */ /* This is the position of the gravity well in cartesian (i.e. x,y,z) space. */ static const hduVector3Dd wellPos = {0,0,0}; hduVector3Dd wellPos2 = {0.0, 0.0, 0.0}; HDErrorInfo error; hduVector3Dd position; hduVector3Dd force; hduVector3Dd positionTwell; HHD hHD = hdGetCurrentDevice(); /* Begin haptics frame. ( In general, all state-related haptics calls should be made within a frame. ) */ WaitForSingleObject( hIOMutex, INFINITE ); hdBeginFrame(hHD); /* Get the current position of the device. */ hdGetDoublev(HD_CURRENT_POSITION, position); memset(force, 0, sizeof(hduVector3Dd)); /* > positionTwell = wellPos-position < Create a vector from the device position towards the gravity well's center. */ count++; //wellPos2[0] = (count%5000)/100; wellPos2[1] = 20*sin((count)*6.28/360); hduVecSubtract(positionTwell, wellPos2, position); /* If the device position is within some distance of the gravity well's center, apply a spring force towards gravity well's center. The force calculation differs from a traditional gravitational body in that the closer the device is to the center, the less force the well exerts; the device behaves as if a spring were connected between itself and the well's center. */ if (hduVecMagnitude(positionTwell) < kGravityWellInfluence) { /* > F = k * x < F: Force in Newtons (N) k: Stiffness of the well (N/mm) x: Vector from the device endpoint position to the center of the well. */ hduVecScale(force, positionTwell, kStiffness); } /* Send the force to the device. */ hdSetDoublev(HD_CURRENT_FORCE, force); /* Get data for logging */ hdGetDoublev(HD_CURRENT_POSITION, global_position); hdGetDoublev(HD_CURRENT_FORCE, global_force); hdGetDoublev(HD_CURRENT_JOINT_ANGLES, global_joint_angles); hdGetDoublev(HD_CURRENT_JOINT_TORQUE, global_joint_torque); /* End haptics frame. */ hdEndFrame(hHD); ReleaseMutex( hIOMutex ); /* Check for errors and abort the callback if a scheduler error is detected. */ if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error detected while rendering gravity well\n"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } /* Signify that the callback should continue running, i.e. that it will be called again the next scheduler tick. */ return HD_CALLBACK_CONTINUE; }
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 hdEndCB(void *data) { static const HDdouble kRampUpRate = 0.0001; static const HDdouble kImpulseLimit = 0.001; hdSetDoublev(HD_SOFTWARE_FORCE_IMPULSE_LIMIT,&kImpulseLimit); hdSetDoublev(HD_FORCE_RAMPING_RATE, &kRampUpRate ); hdGetDoublev(HD_CURRENT_FORCE, force); //Create custom haptic layers. There are two layers here //at DOP = 0.1 and DOP = 0.35 . if (touchedHole && force[2]>=0.0) { if (probeDop > 0.0 && probeDop < 0.0125) force[2] = 0.25; else if (probeDop > 0.0 && probeDop < 0.025) force[2] = 0.5; else if (probeDop > 0.025 && probeDop < 0.05) force[2] = 0.75; else if (probeDop > 0.05 && probeDop < 0.075) force[2] = 1.0; else if (probeDop > 0.075 && probeDop < 0.09) force[2] = 1.5; else if (probeDop > 0.09 && probeDop < 0.1) force[2] = 0.0; else if (probeDop > 0.1 && probeDop < 0.2) force[2] = 0.25; else if (probeDop > 0.2 && probeDop < 0.225) force[2] = 0.5; else if (probeDop > 0.225 && probeDop < 0.25) force[2] = 0.75; else if (probeDop > 0.25 && probeDop < 0.275) force[2] = 0.0; else if (probeDop > 0.275 && probeDop < 0.3) force[2] = 0.0; else if (probeDop > 0.3 && probeDop < 0.35) force[2] = 0.25; else if (probeDop > 0.35 && probeDop < 0.4) force[2] = 0.5; else if (probeDop > 0.4) force[2] = 0.5; hdSetDoublev(HD_CURRENT_FORCE, force*forceScaler); } // calling hdEndFrame decrements the frame counter. // when the beginFrame counter reaches 0, forces are rendered. // Note that HL makes calls to hdBeginFrame & hdEndFrame internally hdEndFrame(hdGetCurrentDevice()); HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error in hdEndCB\n"); } return HD_CALLBACK_CONTINUE; }
HDCallbackCode HDCALLBACK phantom_callback(void *pUserData) { double Acc=0; double Mss = 0.0001; Xprv = X; hdBeginFrame(hdGetCurrentDevice()); hdGetDoublev(HD_CURRENT_POSITION,posistion); if (start) { X0=posistion[0]; start = false; hdEndFrame(Device); return HD_CALLBACK_CONTINUE; } X = posistion[0]-X0; V = X - Xprv; /* * Velocity Command channel */ // Calculate Positive Energy if (Fs*Xm>0) { mst_Xm_P += Fs*Xm*Scale; } else { // Do nothing } //ROS_INFO("Xm channel Energy: %5f",mst_Xm_P); /* * Velocity feedback channel */ Fve = -FveGain*(Xm*Scale-Vs); if (Fve*Vs>0) { mst_Vs_N -=Fve*Vs; } else { //Do nothing } //ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Vs_N,Vs,Fve); //PC #if (PC_Fve_On) { if (mst_Vs_N+sla_Vs_P<0) { mst_Vs_N +=Fve*Vs; // backward 1 step // Modify Vs if (Fve*Fve>0) { Vs = (mst_Vs_N+sla_Vs_P)/Fve; } else { Vs = 0; } // update Fv = -FveGain*(Xm*Scale-Vs); mst_Vs_N -=Fv*Vs; ROS_INFO("PC"); } } #endif /* * Obstacle force */ if (Fe*Vm>0) { mst_Fe_N-=Fe*Vm; //fe channel output energy } else { } // ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe); // fprintf(mstEnergy,"%.3f \n",mstE_N_fe); // fprintf(slvEnergy,"%.3f \n",slaE_P_fe); // ROS_INFO("Master fe Energy: %5f ",mstE_N_fe); #if PC_Fe_On if (mst_Fe_N+sla_Fe_P<0) //output+input <0 { mst_Fe_N+=Fe*Vm; //backward 1 step // Modify Fe if (Vm*Vm > 0.0) Fe = (mst_Fe_N+sla_Fe_P)/Vm; else Fe = 0; ROS_INFO("PC"); // Update mst_Fe_N-=Fe*Vm; } #endif /* * Velocity Based force */ if (Fv*Vm>0) { mst_Fv_N-=Fv*Vm; //fe channel output energy } else { } // ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe); // fprintf(mstEnergy,"%.3f \n",mstE_N_fe); // fprintf(slvEnergy,"%.3f \n",slaE_P_fe); // ROS_INFO("Master fe Energy: %5f ",mstE_N_fe); #if PC_Fv_On if (mst_Fv_N+sla_Fv_P<0) //output+input <0 { mst_Fv_N+=Fv*Vm; //backward 1 step // Modify Fe if (Vm*Vm > 0.0) Fv = (mst_Fv_N+sla_Fv_P)/Vm; else Fv = 0; // Update mst_Fv_N-=Fv*Vm; ROS_INFO("PC"); } #endif /* * Combination */ #if !FeOn Fe = 0; #endif #if !FveOn Fve = 0; #endif #if !FvOn Fv = 0; #endif #if (PC_Fe_On||PC_Fve_On||PC_Fv_On) Fm = Fe + Fve + Fv; Acc = ( -1*K_Master*( Xm - X ) + Fm )/Mss; Vm += Acc*0.001; Xm += Vm*0.001; Fm = K_Master*(Xm - X); #else Xm = X; Vm = V; Fm = Fe +Fve+Fv; #endif // ROS_INFO("Obstacle Feedback Force: %5f",Fe); // ROS_INFO("Vsd: %5f",Xm*Scale); ROS_INFO("Force: %5f",Fm); force[2] = 0; force[1] = 0; force[0] = Fm; Saturation(force); hdSetDoublev(HD_CURRENT_FORCE, force); hdEndFrame(hdGetCurrentDevice()); fprintf(data,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",Fm,Fve,Fe,Xm*Scale,Vs,mst_Fe_N,sla_Fe_P,mst_Vs_N,sla_Vs_P,mst_Fv_N,sla_Fv_P,Fv); MasToSlaDelay(); return HD_CALLBACK_CONTINUE; }
/******************************************************************************* Haptic plane callback. The plane is oriented along Y=0 and provides a repelling force if the device attempts to penetrates through it. *******************************************************************************/ HDCallbackCode HDCALLBACK FrictionlessPlaneCallback(void *data) { // Stiffnes, i.e. k value, of the plane. Higher stiffness results // in a harder surface. const double planeStiffness = .25; // Amount of force the user needs to apply in order to pop through // the plane. const double popthroughForceThreshold = 5.0; // Plane direction changes whenever the user applies sufficient // force to popthrough it. // 1 means the plane is facing +Y. // -1 means the plane is facing -Y. static int directionFlag = 1; hdBeginFrame(hdGetCurrentDevice()); // Get the position of the device. hduVector3Dd position; hdGetDoublev(HD_CURRENT_POSITION, position); // If the user has penetrated the plane, set the device force to // repel the user in the direction of the surface normal of the plane. // Penetration occurs if the plane is facing in +Y and the user's Y position // is negative, or vice versa. if ((position[1] <= 0 && directionFlag > 0) || (position[1] > 0) && (directionFlag < 0)) { // Create a force vector repelling the user from the plane proportional // to the penetration distance, using F=kx where k is the plane // stiffness and x is the penetration vector. Since the plane is // oriented at the Y=0, the force direction is always either directly // upward or downward, i.e. either (0,1,0) or (0,-1,0). double penetrationDistance = fabs(position[1]); hduVector3Dd forceDirection(0,directionFlag,0); // Hooke's law explicitly: double k = planeStiffness; hduVector3Dd x = penetrationDistance*forceDirection; hduVector3Dd f = k*x; // If the user applies sufficient force, pop through the plane // by reversing its direction. Otherwise, apply the repel // force. if (f.magnitude() > popthroughForceThreshold) { f.set(0.0,0.0,0.0); directionFlag = -directionFlag; } hdSetDoublev(HD_CURRENT_FORCE, f); } hdEndFrame(hdGetCurrentDevice()); // In case of error, terminate the callback. HDErrorInfo error; if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error detected during main scheduler callback\n"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } return HD_CALLBACK_CONTINUE; }