//==========================================================================
int hdPhantomSetForceAndTorque(const int a_deviceID,
const double *a_forceX,
const double *a_forceY,
const double *a_forceZ,
const double *a_torqueX,
const double *a_torqueY,
const double *a_torqueZ)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// set force
phantomDevices[a_deviceID].force[2] = *a_forceX;
phantomDevices[a_deviceID].force[0] = *a_forceY;
phantomDevices[a_deviceID].force[1] = *a_forceZ;
// set torque
phantomDevices[a_deviceID].torque[2] = *a_torqueX;
phantomDevices[a_deviceID].torque[0] = *a_torqueY;
phantomDevices[a_deviceID].torque[1] = *a_torqueZ;
// success
return (0);
}
FB::variant hapticAPI::startDevice(void){
if (!initialized){
if ( hdPhantomOpen(deviceID)!=-1) {
hdPhantomStartServo();
initialized = true;
return (FB::variant)initialized;
}
}
return initialized;
}
//==========================================================================
int hdPhantomGetButtons(int a_deviceID)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// return value
return (phantomDevices[a_deviceID].button);
}
//==========================================================================
int hdPhantomGetLinearVelocity(int a_deviceID,
double *a_velX,
double *a_velY,
double *a_velZ)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// get position
*a_velX = phantomDevices[a_deviceID].linearVelocity[2];
*a_velY = phantomDevices[a_deviceID].linearVelocity[0];
*a_velZ = phantomDevices[a_deviceID].linearVelocity[1];
// success
return (0);
}
//==========================================================================
int hdPhantomGetPosition(int a_deviceID,
double *a_posX,
double *a_posY,
double *a_posZ)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// get position
*a_posX = phantomDevices[a_deviceID].position[2];
*a_posY = phantomDevices[a_deviceID].position[0];
*a_posZ = phantomDevices[a_deviceID].position[1];
// success
return (0);
}
//==========================================================================
int hdPhantomGetType(int a_deviceID,
char* a_typeName)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// retrieve handle
HHD hHD = phantomDevices[a_deviceID].handle;
// activate ith device
hdMakeCurrentDevice(hHD);
// read device model
const char* typeName = hdGetString(HD_DEVICE_MODEL_TYPE);
strcpy(a_typeName, typeName);
}
//==========================================================================
int hdPhantomGetWorkspaceRadius(const int a_deviceID,
double *a_workspaceRadius)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// retrieve handle
HHD hHD = phantomDevices[a_deviceID].handle;
// activate ith device
hdMakeCurrentDevice(hHD);
// read workspace of device
double size[6];
hdGetDoublev(HD_USABLE_WORKSPACE_DIMENSIONS, size);
double sizeX = size[3] - size[0];
double sizeY = size[4] - size[1];
double sizeZ = size[5] - size[2];
double radius = 0.5 * sqrt(sizeX * sizeX +
sizeY * sizeY +
sizeZ * sizeZ);
// convert value to [m]
phantomDevices[a_deviceID].workspaceRadius = 0.001 * radius;
// return estimated workspace radius
*a_workspaceRadius = phantomDevices[a_deviceID].workspaceRadius;
// success
return (0);
}
//==========================================================================
int hdPhantomGetRotation(int a_deviceID,
double *a_rot00,
double *a_rot01,
double *a_rot02,
double *a_rot10,
double *a_rot11,
double *a_rot12,
double *a_rot20,
double *a_rot21,
double *a_rot22)
{
// check id
if ((a_deviceID < 0) || (a_deviceID >= numPhantomDevices)) { return (-1); }
// check if servo started
if (!servoStarted) { hdPhantomStartServo(); }
// check if enabled
if (!phantomDevices[a_deviceID].enabled) { return (-1); }
// return rotation matrix
/*
*a_rot00 = phantomDevices[a_deviceID].rotation[8];
*a_rot01 = phantomDevices[a_deviceID].rotation[2];
*a_rot02 = phantomDevices[a_deviceID].rotation[5];
*a_rot10 = phantomDevices[a_deviceID].rotation[6];
*a_rot11 = phantomDevices[a_deviceID].rotation[0];
*a_rot12 = phantomDevices[a_deviceID].rotation[3];
*a_rot20 = phantomDevices[a_deviceID].rotation[7];
*a_rot21 = phantomDevices[a_deviceID].rotation[1];
*a_rot22 = phantomDevices[a_deviceID].rotation[4];
*/
// read value from matrix and correct matrix to be orthogonal
// unfortunately there seems be some precision errors coming
// from the OpenHaptics library.
cVector3d v0, v1, v2;
v0.set( phantomDevices[a_deviceID].rotation[8],
phantomDevices[a_deviceID].rotation[6],
phantomDevices[a_deviceID].rotation[7]);
v1.set( phantomDevices[a_deviceID].rotation[2],
phantomDevices[a_deviceID].rotation[0],
phantomDevices[a_deviceID].rotation[1]);
v0.normalize();
v1.normalize();
v0.crossr(v1, v2);
v2.crossr(v0, v1);
*a_rot00 = v0.x;
*a_rot01 = v1.x;
*a_rot02 = v2.x;
*a_rot10 = v0.y;
*a_rot11 = v1.y;
*a_rot12 = v2.y;
*a_rot20 = v0.z;
*a_rot21 = v1.z;
*a_rot22 = v2.z;
// success
return (0);
}