void CybPhantom::exitHL()
{
if(shapeIdListIscreated){
for(int i = 0; i < numHapticLayers; i++)
hlDeleteShapes(shapeId[i], 1);
delete [] properties;
}
hlMakeCurrent(NULL);
if(hapticContext != NULL)
{
hlDeleteContext(hapticContext);
}
if(hapticDevice != HD_INVALID_HANDLE)
{
hdDisableDevice(hapticDevice);
hapticDevice = HD_INVALID_HANDLE;
}
if(ambientPropertyIsEnable){
CybViscosity *viscosity = CybViscosity::getInstance();
viscosity->stopAmbientProperty();
delete viscosity;
}
}
HapticDevice::~HapticDevice(void)
{
if(m_updateDeviceCallback != HD_INVALID_HANDLE)
{
hdStopScheduler();
hdUnschedule(m_updateDeviceCallback);
}
for(unsigned int i=0;i<m_nbDevices;i++)
{
if(m_constraints[i] != NULL)
delete m_constraints[i];
if(m_hss[i].m_data!=NULL)
{
if (m_hss[i].m_data->m_id != HD_INVALID_HANDLE)
{
hdDisableDevice(m_hss[i].m_data->m_id);
m_hss[i].m_data->m_id = HD_INVALID_HANDLE;
}
}
}
printf(" Haptic Divice Done . \n");
}
void phantomClose()
{
fclose(data);
hdStopScheduler();
hdUnschedule(hPhantomMain);
hdDisableDevice(Device);
}
/******************************************************************************
This handler gets called when the process is exiting. Ensures that HDAPI is
properly shutdown
******************************************************************************/
void exitHandler()
{
hdStopScheduler();
hdUnschedule(gSchedulerCallback);
if (ghHD != HD_INVALID_HANDLE)
{
hdDisableDevice(ghHD);
ghHD = HD_INVALID_HANDLE;
}
}
/*******************************************************************************
* main function
Initializes the device, creates a callback to handles plane forces,
terminates upon key press.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
// Initialize the default haptic device.
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;
}
// Start the servo scheduler and enable forces.
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start the scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
// Schedule the frictionless plane callback, which will then run at
// servoloop rates and command forces if the user penetrates the plane.
HDCallbackCode hPlaneCallback = hdScheduleAsynchronous(
FrictionlessPlaneCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Plane example.\n");
printf("Move the device up and down to feel a plane along Y=0.\n");
printf("Push hard against the plane to popthrough to the other side.\n");
printf("Press any key to quit.\n\n");
while (!_kbhit())
{
if (!hdWaitForCompletion(hPlaneCallback, HD_WAIT_CHECK_STATUS))
{
fprintf(stderr, "\nThe main scheduler callback has exited\n");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
// Cleanup and shutdown the haptic device, cleanup all callbacks.
hdStopScheduler();
hdUnschedule(hPlaneCallback);
hdDisableDevice(hHD);
return 0;
}
bool BaseGeometryPatch::terminate()
{
/* For cleanup, unschedule callback and stop the scheduler. */
hdStopScheduler();
if(hHapticEffect != NULL)
hdUnschedule(hHapticEffect);
/* Disable the device. */
hdDisableDevice(hHD);
return true;
}
/******************************************************************************
main function
Initializes the device, creates a callback to handle sphere forces, terminates
upon key press.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
// Initialize the default haptic device.
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;
}
// Start the servo scheduler and enable forces.
hdEnable(HD_FORCE_OUTPUT);
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;
}
// Application loop - schedule our call to the main callback.
HDSchedulerHandle hSphereCallback = hdScheduleAsynchronous(
FrictionlessSphereCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Sphere example.\n");
printf("Move the device around to feel a frictionless sphere\n\n");
printf("Press any key to quit.\n\n");
while (!_kbhit())
{
if (!hdWaitForCompletion(hSphereCallback, HD_WAIT_CHECK_STATUS))
{
fprintf(stderr, "\nThe main scheduler callback has exited\n");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
// For cleanup, unschedule our callbacks and stop the servo loop.
hdStopScheduler();
hdUnschedule(hSphereCallback);
hdDisableDevice(hHD);
return 0;
}
/******************************************************************************
Main function.
******************************************************************************/
int main(int argc, char* argv[])
{
hHD = device.setup();
/* 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;
}
/*******************************************************************************
Cleanup
*******************************************************************************/
void exitHandler()
{
// free up the haptic rendering context
hlMakeCurrent(NULL);
if (hHLRC != NULL)
{
hlDeleteContext(hHLRC);
}
// free up the haptic device
if (hHD != HD_INVALID_HANDLE)
{
hdDisableDevice(hHD);
}
}
/******************************************************************************
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;
}
int main(int argc, char** argv) {
////////////////////////////////////////////////////////////////
// Init Phantom
////////////////////////////////////////////////////////////////
HDErrorInfo error;
HHD hHD;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError())) {
//hduPrintError(stderr, &error, "Failed to initialize haptic device");
ROS_ERROR("Failed to initialize haptic device"); //: %s", &error);
return -1;
}
ROS_INFO("Found %s.", hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError())) {
ROS_ERROR("Failed to start the scheduler"); //, &error);
return -1;
}
HHD_Auto_Calibration();
////////////////////////////////////////////////////////////////
// Init ROS
////////////////////////////////////////////////////////////////
ros::init(argc, argv, "omni_haptic_node");
OmniState state;
PhantomROS omni_ros;
omni_ros.init(&state);
hdScheduleAsynchronous(omni_state_callback, &state,
HD_MAX_SCHEDULER_PRIORITY);
////////////////////////////////////////////////////////////////
// Loop and publish
////////////////////////////////////////////////////////////////
pthread_t publish_thread;
pthread_create(&publish_thread, NULL, ros_publish, (void*) &omni_ros);
pthread_join(publish_thread, NULL);
ROS_INFO("Ending Session....");
hdStopScheduler();
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Main function.
Sets up the device, runs main application loop, cleans up when finished.
*******************************************************************************/
int main(int argc, char* argv[])
{
HDSchedulerHandle hUpdateHandle = 0;
HDErrorInfo error;
/* Initialize the device, must be done before attempting to call any hd
functions. */
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize the device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
/* Schedule the main scheduler callback that updates the device state. */
hUpdateHandle = hdScheduleAsynchronous(
updateDeviceCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
/* Start the servo loop scheduler. */
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start the scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
/* Run the application loop. */
mainLoop();
/* For cleanup, unschedule callbacks and stop the servo loop. */
hdStopScheduler();
hdUnschedule(hUpdateHandle);
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
This handler is called when the application is exiting. Deallocates any state
and cleans up.
*******************************************************************************/
void exitHandler()
{
// Remove event callbacks.
hlRemoveEventCallback(HL_EVENT_TOUCH, gSphereShapeId,
HL_CLIENT_THREAD, touchSphereCallback);
hlRemoveEventCallback(HL_EVENT_UNTOUCH, gSphereShapeId,
HL_CLIENT_THREAD, untouchSphereCallback);
hlRemoveEventCallback(HL_EVENT_TOUCH, gTorusShapeId,
HL_CLIENT_THREAD, touchTorusCallback);
hlRemoveEventCallback(HL_EVENT_UNTOUCH, gTorusShapeId,
HL_CLIENT_THREAD, untouchTorusCallback);
hlRemoveEventCallback(HL_EVENT_TOUCH, gTeapotShapeId,
HL_CLIENT_THREAD, touchTeapotCallback);
hlRemoveEventCallback(HL_EVENT_UNTOUCH, gTeapotShapeId,
HL_CLIENT_THREAD, untouchTeapotCallback);
hlRemoveEventCallback(HL_EVENT_CALIBRATION_UPDATE, HL_OBJECT_ANY,
HL_CLIENT_THREAD, calibrationCallback);
hlRemoveEventCallback(HL_EVENT_CALIBRATION_INPUT, HL_OBJECT_ANY,
HL_CLIENT_THREAD, calibrationCallback);
// Deallocate the shape ids we reserved in initHL.
hlDeleteShapes(gSphereShapeId, 1);
hlDeleteShapes(gTorusShapeId, 1);
hlDeleteShapes(gSphereShapeId, 1);
// Free up the haptic rendering context.
hlMakeCurrent(NULL);
if (hHLRC != NULL)
{
hlDeleteContext(hHLRC);
}
// Free up the haptic device.
if (hHD != HD_INVALID_HANDLE)
{
hdDisableDevice(hHD);
}
}
/*******************************************************************************
This handler is called when the application is exiting. Deallocates any state
and cleans up.
*******************************************************************************/
void exitHandler()
{
// Deallocate the sphere shape id we reserved in initHL.
if (objmodel)
{
hlDeleteShapes(gShapeId, 1);
hlDeleteShapes(gPointId, 1);
}
// Free up the haptic rendering context.
hlMakeCurrent(NULL);
if (ghHLRC != NULL)
{
hlDeleteContext(ghHLRC);
}
// Free up the haptic device.
if (ghHD != HD_INVALID_HANDLE)
{
hdDisableDevice(ghHD);
}
}
/*******************************************************************************
Main function.
*******************************************************************************/
int main(int argc, char *argv[])
{
HHD hHD;
HHLRC hHLRC;
HDErrorInfo error;
HLuint friction, spring;
HLerror frameError;
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;
}
hdMakeCurrentDevice(hHD);
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
hlDisable(HL_USE_GL_MODELVIEW);
spring = hlGenEffects(1);
/* Add a callback to handle button down in the collision thread. */
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_2BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, 0);
/* Start an ambient friction effect. */
friction = hlGenEffects(1);
hlBeginFrame();
hlEffectd(HL_EFFECT_PROPERTY_GAIN, 0.2);
hlEffectd(HL_EFFECT_PROPERTY_MAGNITUDE, 0.5);
hlStartEffect(HL_EFFECT_FRICTION, friction);
hlEndFrame();
printf("Move around to feel the ambient stick-slip friction.\n\n");
printf("Press and hold the primary stylus button to feel the spring effect.\n\n");
printf("Press the second stylus button to trigger an impulse.\n\n");
/* Run the main loop. */
while (!_kbhit())
{
hlBeginFrame();
/* Poll for events. Note that client thread event callbacks get
dispatched from within a frame here, so we can safely start/stop
effects from the event callback directly */
hlCheckEvents();
hlEndFrame();
/* Check for any errors. */
while (HL_ERROR(frameError = hlGetError()))
{
fprintf(stderr, "HL Error: %s\n", frameError.errorCode);
if (frameError.errorCode == HL_DEVICE_ERROR)
{
hduPrintError(stderr, &frameError.errorInfo,
"Error during haptic rendering\n");
}
}
}
/* Stop the friction effect. */
hlBeginFrame();
hlStopEffect(friction);
hlEndFrame();
hlDeleteEffects(friction, 1);
hlDeleteEffects(spring, 1);
hlDeleteContext(hHLRC);
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Main function.
*******************************************************************************/
int main(int argc, char *argv[])
{
HHD hHD;
HHLRC hHLRC;
HDErrorInfo error;
HLuint spring;
/* The code snippet provided to you by SensAble should be executed near
application startup. Once a deployment license has been validated,
it will remain in effect until the application is shutdown.
Be sure to place the HD deployment license code before the call
to hdInitDevice, and the HL deployment license code before the call
to hlCreateContext.
NOTE THAT THE FOLLOWING IS FOR DEMONSTRATION ONLY. THE LICENSES
ARE NOT VALID. */
hdDeploymentLicense(
"ABC Software, Inc.",
"HapticsGold",
"F1312D97ECCC754D5BE4BEE7E831BC27ACF809E9B850D9576F1A856AF70DD3A879B4D3DC7F922BDB2C639DA4A565CA5FC598D8AF34EA010B13A8C232B78F22C");
hlDeploymentLicense(
"ABC Software, Inc.",
"HapticsGold",
"A653A1EEAFF7B87C952754672FDB1AA16A9035ADE1CFCA6394FE869BAFECE0B7A32251502DDF220B7BA27979695041AE59DCEA007605027D471801F4BF26C24");
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;
}
hdMakeCurrentDevice(hHD);
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
hlDisable(HL_USE_GL_MODELVIEW);
spring = hlGenEffects(1);
// Add a callback to handle button down in the collision thread.
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
buttonCB, &spring);
printf("Press and hold the primary stylus button to feel the spring effect.\n");
printf("Press any key to quit.\n\n");
// Run the main loop.
while (!_kbhit())
{
hlBeginFrame();
// Poll for events. Note that client thread event callbacks get
// dispatched from within a frame here, so we can safely start/stop
// effects from the event callback directly.
hlCheckEvents();
hlEndFrame();
}
hlDeleteEffects(spring, 1);
hlDeleteContext(hHLRC);
hdDisableDevice(hHD);
return 0;
}
void Phantom::Close() {
hdStopScheduler();
hdUnschedule(_hPhantomMain);
hdDisableDevice(_Device);
}
/*******************************************************************************
Main function.
Initializes the device, starts the schedule, creates a schedule callback
to handle gravity well forces, waits for the user to press a button, exits
the application.
*******************************************************************************/
int main(int argc, char* argv[])
{
int KeyInfo;
HDErrorInfo error;
HDSchedulerHandle hGravityWell;
/* Initialize the device, must be done before attempting to call any hd
functions. Passing in HD_DEFAULT_DEVICE causes the default device to be
initialized. */
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
hIOMutex = CreateMutex(NULL, FALSE, NULL);
kill = 0;
recording = 0;
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("Hello Haptic Device!\n");
printf("Found device model: %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
/* Schedule the main callback that will render forces to the device. */
hGravityWell = hdScheduleAsynchronous(
gravityWellCallback, 0,
HD_MAX_SCHEDULER_PRIORITY);
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
_beginthread( recordingLoop );
/* Check for errors and abort if so. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
return -1;
}
/* Wait until the user presses a key. Meanwhile, the scheduler
runs and applies forces to the device. */
printf("Press R to record data\n");
printf("Press Q to quit.\n\n");
count = 0;
//while (!_kbhit())
do
{
KeyInfo = _getch();
if ( tolower( KeyInfo ) == 'q')
{
kill = 1;
}
else
{
if ( tolower( KeyInfo ) == 'r' )
{
if ( recording == 0) {
printf("Data recording ON\n");
recording = 1;
}
else
{
printf("Data recording OFF\n");
recording = 0;
}
}
}
/* Periodically check if the gravity well callback has exited. */
if (!hdWaitForCompletion(hGravityWell, HD_WAIT_CHECK_STATUS))
{
kill = 1;
fprintf(stderr, "Press any key to quit.\n");
getch();
break;
}
// printf("\n%d", count);
} while (!kill);
/* For cleanup, unschedule callback and stop the scheduler. */
hdStopScheduler();
hdUnschedule(hGravityWell);
/* Disable the device. */
hdDisableDevice(hHD);
return 0;
}
/******************************************************************************
Collects statistics about the update rate of the haptic device.
******************************************************************************/
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;
}
HDstring model = hdGetString(HD_DEVICE_MODEL_TYPE);
std::cout << "Initialized: " << model << std::endl;
HDSchedulerHandle hServoCallback = hdScheduleAsynchronous(
ServoSchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to schedule servoloop callback" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdSetSchedulerRate(TARGET_SERVOLOOP_RATE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to set servoloop rate" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdDisable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to start servoloop" << std::endl;
hdDisableDevice(hHD);
return -1;
}
std::cout << "Printing servoloop rate stats. All numbers are in Hz units" << std::endl;
std::cout << std::endl;
char fileName[256];
sprintf(fileName, "%s Rate Stats.txt", model);
std::ofstream fout(fileName, std::ios::out | std::ios::app);
for (int nRuns = 0; nRuns < 10 && !_kbhit(); nRuns++)
{
Sleep(1000);
hdScheduleSynchronous(CopyUpdateRateStats, 0, HD_MIN_SCHEDULER_PRIORITY);
// Prints some stats about the rate as well as log it to file.
PrintUpdateRateStats(std::cout);
PrintUpdateRateStats(fout);
if (!hdWaitForCompletion(hServoCallback, HD_WAIT_CHECK_STATUS))
{
std::cerr << "Error occurred during main loop" << std::endl;
std::cerr << "Press any key to quit." << std::endl;
getch();
break;
}
}
hdStopScheduler();
hdUnschedule(hServoCallback);
hdDisableDevice(hHD);
return 0;
}
