void haptic_close()
{
if(!haptic_ok)
return;
dhdClose(DHD);
haptic_ok = false;
//free(buf);
buf = NULL;
return;
}
//===========================================================================
int cDeltaDevice::close()
{
// check if the system has been opened previously
if (!m_systemReady) return (-1);
// yes, the device is open so let's close it
int result = dhdClose(m_deviceID);
// update status
m_systemReady = false;
return (result);
}
void Omega3::AcquisitionTask::run(){
while(1){
//! qDebug()<< "try to open the first available device";
if (drdOpen () < 0) {
//qDebug()<<"no device available...";
//dhdSleep (2.0);
omegaDetected = false;
}
else{
omegaDetected = true;
}
if(omegaDetected){
break;
}
sleep(1);
}
done = 0;
double p[DHD_MAX_DOF];
double r[3][3];
double v[DHD_MAX_DOF];
double f[DHD_MAX_DOF];
double normf, normt;
double t0 = dhdGetTime ();
double t1 = t0;
bool base = false;
bool wrist = false;
bool grip = false;
//! Eigen objects (mapped to the arrays above)
Map<Vector3d> position(&p[0], 3);
Map<Vector3d> force (&f[0], 3);
Map<Vector3d> torque (&f[3], 3);
Map<Vector3d> velpos (&v[0], 3);
Map<Vector3d> velrot (&v[3], 3);
Matrix3d center;
Matrix3d rotation;
//! center of workspace
center.setIdentity (); // rotation (matrix)
double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0, // translation
0.0, 0.0, 0.0, // rotation (joint angles)
0.0 }; // gripper
//! print out device identifier
if (!drdIsSupported ()) {
dhdSleep (2.0);
drdClose ();
}
//! perform auto-initialization
if (!drdIsInitialized () && drdAutoInit () < 0) {
qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
dhdSleep (2.0);
}
else if (drdStart () < 0) {
printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
dhdSleep (2.0);
}
//! move to center
drdMoveTo (nullPose);
// request a null force (only gravity compensation will be applied)
// this will only apply to unregulated axis
for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
drdSetForceAndTorqueAndGripperForce (f);
// disable all axis regulation (but leave regulation thread running)
drdRegulatePos (base);
drdRegulateRot (wrist);
drdRegulateGrip (grip);
//! save current pos
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
long long cpt = 0;
// loop while the button is not pushed
while (!done) {
// synchronize with regulation thread
drdWaitForTick ();
// get position/orientation/gripper and update Eigen rotation matrix
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
if(cpt%31==0){
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
}
else{
cpt += 1;
}
// get position/orientation/gripper velocity
drdGetVelocity (v);
// compute base centering force
force = - KP * position;
// compute wrist centering torque
AngleAxisd deltaRotation (rotation.transpose() * center);
torque = rotation * (KR * deltaRotation.angle() * deltaRotation.axis());
// compute gripper centering force
f[6] = - KG * (p[6] - 0.015);
// scale force to a pre-defined ceiling
if ((normf = force.norm()) > MAXF) force *= MAXF/normf;
// scale torque to a pre-defined ceiling
if ((normt = torque.norm()) > MAXT) torque *= MAXT/normt;
// scale gripper force to a pre-defined ceiling
if (f[6] > MAXG) f[6] = MAXG;
if (f[6] < -MAXG) f[6] = -MAXG;
// add damping
force -= KVP * velpos;
torque -= KWR * velrot;
f[6] -= KVG * v[6];
// apply centering force with damping
if (drdSetForceAndTorqueAndGripperForce (f, -1) < DHD_NO_ERROR) {
printf ("error: cannot set force (%s)\n", dhdErrorGetLastStr ());
done = 1;
}
// local loop refresh rate
//count++;
// display refresh rate and position at 10Hz
t1 = drdGetTime ();
if ((t1-t0) > REFRESH_INTERVAL) {
// retrieve/compute information to display
// double freq = (double)count/(t1-t0)*1e-3;
// count = 0;
// t0 = t1;
// qDebug()<<freq<<cpt;
if (dhdGetButtonMask()) {
qDebug()<<"stop";
dhdClose ();
done = 1;
}
}
//qDebug()<<cpt<<t1;
//dhdSleep (0.1);
}
}
int
main (int argc,
char **argv)
{
int i;
int done = 0;
int enc[DHD_MAX_ENC];
int encCount;
// message
int major, minor, release, revision;
dhdGetAPIVersion (&major, &minor, &release, &revision);
printf ("Force Dimension - Encoder Reading Example %d.%d.%d.%d\n", major, minor, release, revision);
printf ("(C) 2010 Force Dimension\n");
printf ("All Rights Reserved.\n\n");
// open the first available device
if (dhdOpen () < 0) {
printf ("error: cannot open device (%s)\n", dhdErrorGetLastStr());
return -1;
}
// identify device
printf ("%s device detected\n\n", dhdGetSystemName());
// identify number of encoders to report based on device type
switch (dhdGetSystemType ()) {
case DHD_DEVICE_3DOF:
case DHD_DEVICE_3DOF_USB:
case DHD_DEVICE_OMEGA:
case DHD_DEVICE_OMEGA3:
default:
encCount = 3;
break;
case DHD_DEVICE_6DOF:
case DHD_DEVICE_6DOF_500:
case DHD_DEVICE_6DOF_USB:
case DHD_DEVICE_OMEGA33:
case DHD_DEVICE_OMEGA33_LEFT:
encCount = 6;
break;
case DHD_DEVICE_OMEGA331:
case DHD_DEVICE_OMEGA331_LEFT:
encCount = 7;
break;
case DHD_DEVICE_CONTROLLER:
case DHD_DEVICE_CONTROLLER_HR:
encCount = 8;
break;
}
// display instructions
printf ("press BUTTON or 'q' to quit\n\n");
// configure device
dhdEnableExpertMode();
// loop while the button is not pushed
while (!done) {
// read all available encoders
if (dhdGetEnc (enc) < 0) {
printf ("error: cannot read encoders (%s)\n", dhdErrorGetLastStr ());
done = 1;
}
// print out encoders according to system type
for (i=0; i<encCount; i++) {
printf ("%06d ", enc[i]);
}
printf (" \r");
// limit to kHz and check for exit condition
dhdSleep (0.001);
if (dhdGetButton(0)) done = 1;
if (dhdKbHit()) {
if (dhdKbGet() == 'q') done = 1;
}
}
// close the connection
dhdClose ();
// happily exit
printf ("\ndone.\n");
return 0;
}
