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