//==========================================================================
void init()
{
if (!initPhantomLIB)
{
initPhantomLIB = true;
for (int i=0; i<PHANTOM_NUM_DEVICES_MAX; i++)
{
// init button data
phantomDevices[i].button = 0;
// init position data
phantomDevices[i].position[0] = 0.0;
phantomDevices[i].position[1] = 0.0;
phantomDevices[i].position[2] = 0.0;
// init rotation data
phantomDevices[i].rotation[0] = 1.0;
phantomDevices[i].rotation[1] = 0.0;
phantomDevices[i].rotation[2] = 0.0;
phantomDevices[i].rotation[3] = 0.0;
phantomDevices[i].rotation[4] = 1.0;
phantomDevices[i].rotation[5] = 0.0;
phantomDevices[i].rotation[6] = 0.0;
phantomDevices[i].rotation[7] = 0.0;
phantomDevices[i].rotation[8] = 1.0;
// init force data
phantomDevices[i].force[0] = 0.0;
phantomDevices[i].force[1] = 0.0;
phantomDevices[i].force[2] = 0.0;
// init torque data
phantomDevices[i].torque[0] = 0.0;
phantomDevices[i].torque[1] = 0.0;
phantomDevices[i].torque[2] = 0.0;
// init enable/disable data
phantomDevices[i].enabled = false;
// init phantom api initialized
phantomDevices[i].initialized = false;
}
//------------------------------------------------------------------
// INITIALIZE DEVICES
//------------------------------------------------------------------
HDErrorInfo error;
numPhantomDevices = 0;
// search for a first device
HHD hHD0 = hdInitDevice(HD_DEFAULT_DEVICE);
// check if device is available
if (HD_DEVICE_ERROR(error = hdGetError()))
{
return;
}
// enable forces
hdMakeCurrentDevice(hHD0);
hdEnable(HD_FORCE_OUTPUT);
// add device to list
phantomDevices[0].handle = hHD0;
phantomDevices[0].enabled = true;
numPhantomDevices++;
// search for a possible second device
HHD hHD1 = hdInitDevice("Phantom2");
// check if device is available
if (HD_DEVICE_ERROR(error = hdGetError()))
{
return;
}
// enable forces
hdMakeCurrentDevice(hHD1);
hdEnable(HD_FORCE_OUTPUT);
// add device to list
phantomDevices[1].handle = hHD1;
phantomDevices[1].enabled = true;
numPhantomDevices++;
}
return;
}
/*******************************************************************************
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;
}
/******************************************************************************
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;
}
/******************************************************************************
* 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;
}
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;
}
int main(int argc, char** argv)
{
////////////////////////////////////////////////////////////////
// Init ROS
////////////////////////////////////////////////////////////////
ros::init(argc, argv, "phantom_node");
PhantomState state;
PhantomROS phantom_ros;
////////////////////////////////////////////////////////////////
// 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);
// hdEnable(HD_MAX_FORCE_CLAMPING);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
ROS_ERROR("Failed to start the scheduler");
//, &error);
return -1;
}
if(phantom_ros.init(&state))
{
hdStopScheduler();
hdDisableDevice(hHD);
return -1;
}
if(phantom_ros.calibrate_)
{
HHD_Auto_Calibration();
}
hdScheduleAsynchronous(phantom_state_callback, &state, HD_MAX_SCHEDULER_PRIORITY);
////////////////////////////////////////////////////////////////
// Loop and publish
////////////////////////////////////////////////////////////////
pthread_t publish_thread;
pthread_create(&publish_thread, NULL, ros_publish, (void*)&phantom_ros);
pthread_join(publish_thread, NULL);
ROS_INFO("Ending Session...");
hdStopScheduler();
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Initialize the HDAPI. This involves initing a device configuration, enabling
forces, and scheduling a haptic thread callback for servicing the device.
*******************************************************************************/
void initHL()
{
HDErrorInfo error;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "Press any key to exit");
getchar();
exit(-1);
}
hHLRC = hlCreateContext(hHD);
hlMakeCurrent(hHLRC);
// Enable optimization of the viewing parameters when rendering
// geometry for OpenHaptics.
hlEnable(HL_HAPTIC_CAMERA_VIEW);
// Generate id's for the three shapes.
gSphereShapeId = hlGenShapes(1);
gTorusShapeId = hlGenShapes(1);
gTeapotShapeId = hlGenShapes(1);
// Setup event callbacks.
hlAddEventCallback(HL_EVENT_TOUCH, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&touchShapeCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&untouchShapeCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gSphereShapeId, HL_CLIENT_THREAD,
&touchSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gSphereShapeId, HL_CLIENT_THREAD,
&untouchSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gTorusShapeId, HL_CLIENT_THREAD,
&touchTorusCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gTorusShapeId, HL_CLIENT_THREAD,
&untouchTorusCallback, NULL);
hlAddEventCallback(HL_EVENT_TOUCH, gTeapotShapeId, HL_CLIENT_THREAD,
&touchTeapotCallback, NULL);
hlAddEventCallback(HL_EVENT_UNTOUCH, gTeapotShapeId, HL_CLIENT_THREAD,
&untouchTeapotCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button1DownCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button1UpCallback, NULL);
hlAddEventCallback(HL_EVENT_2BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button2DownCallback, NULL);
hlAddEventCallback(HL_EVENT_2BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button2UpCallback, NULL);
hlAddEventCallback(HL_EVENT_3BUTTONDOWN, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button3DownCallback, NULL);
hlAddEventCallback(HL_EVENT_3BUTTONUP, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&button3UpCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONDOWN, gSphereShapeId, HL_CLIENT_THREAD,
&button1DownSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_1BUTTONUP, gSphereShapeId, HL_CLIENT_THREAD,
&button1UpSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_MOTION, HL_OBJECT_ANY, HL_CLIENT_THREAD,
&motionCallback, NULL);
hlAddEventCallback(HL_EVENT_MOTION, gSphereShapeId, HL_CLIENT_THREAD,
&motionOnSphereCallback, NULL);
hlAddEventCallback(HL_EVENT_CALIBRATION_UPDATE, HL_OBJECT_ANY,
HL_CLIENT_THREAD, &calibrationCallback, NULL);
hlAddEventCallback(HL_EVENT_CALIBRATION_INPUT, HL_OBJECT_ANY,
HL_CLIENT_THREAD, &calibrationCallback, NULL);
}
/*******************************************************************************
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;
}
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;
}
/*******************************************************************************
This routine allows the device to provide information about the current
location of the stylus, and contains a mechanism for terminating the
application.
Pressing the button causes the application to display the current location
of the device.
Holding the button down for N iterations causes the application to exit.
*******************************************************************************/
void mainLoop(void)
{
static const int kTerminateCount = 1000;
int buttonHoldCount = 0;
/* Instantiate the structure used to capture data from the device. */
DeviceData currentData;
DeviceData prevData;
/* Perform a synchronous call to copy the most current device state. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData, HD_MIN_SCHEDULER_PRIORITY);
memcpy(&prevData, ¤tData, sizeof(DeviceData));
printHelp();
/* Run the main loop until the gimbal button is held. */
while (1)
{
/* Perform a synchronous call to copy the most current device state.
This synchronous scheduler call ensures that the device state
is obtained in a thread-safe manner. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData,
HD_MIN_SCHEDULER_PRIORITY);
/* If the user depresses the gimbal button, display the current
location information. */
if (currentData.m_buttonState && !prevData.m_buttonState)
{
fprintf(stdout, "Current position: (%g, %g, %g)\n",
currentData.m_devicePosition[0],
currentData.m_devicePosition[1],
currentData.m_devicePosition[2]);
}
else if (currentData.m_buttonState && prevData.m_buttonState)
{
/* Keep track of how long the user has been pressing the button.
If this exceeds N ticks, then terminate the application. */
buttonHoldCount++;
if (buttonHoldCount > kTerminateCount)
{
/* Quit, since the user held the button longer than
the terminate count. */
break;
}
}
else if (!currentData.m_buttonState && prevData.m_buttonState)
{
/* Reset the button hold count, since the user stopped holding
down the stylus button. */
buttonHoldCount = 0;
}
/* Check if an error occurred. */
if (HD_DEVICE_ERROR(currentData.m_error))
{
hduPrintError(stderr, ¤tData.m_error, "Device error detected");
if (hduIsSchedulerError(¤tData.m_error))
{
/* Quit, since communication with the device was disrupted. */
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
/* Store off the current data for the next loop. */
memcpy(&prevData, ¤tData, sizeof(DeviceData));
}
}
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;
}
/*******************************************************************************
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;
}
/*******************************************************************************
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;
}
/*******************************************************************************
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;
}
