void haptic_fire(unsigned f) { if(!haptic_ok) return; dhdSetForce(f*10,f*10,f*10,DHD); dhdSleep(0.05); dhdSetForce(0,0,0,DHD); return; }
void haptic_wait(float f) { if(!haptic_ok) return; if(f < 0) dhdWaitForReset(0,DHD); else dhdSleep(f); return; }
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; }