/******************************************************************************
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;
}
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::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;
}
/*******************************************************************************
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;
}
/*******************************************************************************
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;
}
// les réalisation des manipulations
// retourne les valeurs possible HD_CALLBACK_DONE ou HD_CALLBACK_CONTINUE
HDCallbackCode HDCALLBACK startManipulationCallBack(void *pUserData){ //cf touchBack
hduVector3Dd v_position;
// vector3d de type double
hduVector3Dd v_force;
// définition du vecteur force que l'on souhaite associer
int button;
// récupération des valeurs des boutons du bras haptic
hdGetIntegerv(HD_CURRENT_BUTTONS,&button);
g_button1 = (button & HD_DEVICE_BUTTON_1);
g_button2 = (button & HD_DEVICE_BUTTON_2);
g_button3 = (button & HD_DEVICE_BUTTON_3);
if(g_button2) g_doExit = true;
// si activation du bouton sortie ==> sortie.
/* début de la manipulation */
hdBeginFrame(ghHD);
hdGetDoublev(HD_CURRENT_POSITION, v_position);
// récupération de la position du bras
switch(g_selectMode){
case CAM:
// si on est dans le mode camera
{
g_position_out.v[0] = (float)v_position[0];
g_position_out.v[1] = (float)v_position[1];
g_position_out.v[2] = (float)v_position[2];
break;
}
case CATCH:
// si on est dans le mode "attraper un bloc"
{
/* déplacement de la main */
g_position_out.v[0] = (float)v_position[0];
g_position_out.v[1] = (float)v_position[1];
g_position_out.v[2] = (float)v_position[2];
/* ajout de la force de retour*/
hdSetDoublev(HD_CURRENT_FORCE, force_active);
break;
}
}
/* fin de la manipulation */
hdEndFrame(ghHD);
return HD_CALLBACK_CONTINUE;
}
HDCallbackCode HDCALLBACK omni_state_callback(void *pUserData) {
OmniState *omni_state = static_cast<OmniState *>(pUserData);
if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE) {
ROS_DEBUG("Updating calibration...");
hdUpdateCalibration(calibrationStyle);
}
hdBeginFrame(hdGetCurrentDevice());
//Get angles, set forces
hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, omni_state->rot);
hdGetDoublev(HD_CURRENT_POSITION, omni_state->position);
hdGetDoublev(HD_CURRENT_JOINT_ANGLES, omni_state->joints);
hduVector3Dd vel_buff(0, 0, 0);
vel_buff = (omni_state->position * 3 - 4 * omni_state->pos_hist1
+ omni_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif
omni_state->velocity = (.2196 * (vel_buff + omni_state->inp_vel3)
+ .6588 * (omni_state->inp_vel1 + omni_state->inp_vel2)) / 1000.0
- (-2.7488 * omni_state->out_vel1 + 2.5282 * omni_state->out_vel2
- 0.7776 * omni_state->out_vel3); //cutoff freq of 20 Hz
omni_state->pos_hist2 = omni_state->pos_hist1;
omni_state->pos_hist1 = omni_state->position;
omni_state->inp_vel3 = omni_state->inp_vel2;
omni_state->inp_vel2 = omni_state->inp_vel1;
omni_state->inp_vel1 = vel_buff;
omni_state->out_vel3 = omni_state->out_vel2;
omni_state->out_vel2 = omni_state->out_vel1;
omni_state->out_vel1 = omni_state->velocity;
if (omni_state->lock == true) {
omni_state->force = 0.04 * (omni_state->lock_pos - omni_state->position)
- 0.001 * omni_state->velocity;
}
hdSetDoublev(HD_CURRENT_FORCE, omni_state->force);
//Get buttons
int nButtons = 0;
hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
omni_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0;
omni_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0;
hdEndFrame(hdGetCurrentDevice());
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError())) {
hduPrintError(stderr, &error, "Error during main scheduler callback");
if (hduIsSchedulerError(&error))
return HD_CALLBACK_DONE;
}
float t[7] = { 0., omni_state->joints[0], omni_state->joints[1],
omni_state->joints[2] - omni_state->joints[1], omni_state->rot[0],
omni_state->rot[1], omni_state->rot[2] };
for (int i = 0; i < 7; i++)
omni_state->thetas[i] = t[i];
return HD_CALLBACK_CONTINUE;
}
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;
}
/*******************************************************************************
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 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;
}
/******************************************************************************
* 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;
}
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 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 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 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;
}
