phua_haptic: hd_hl_apis_callbacks.cpp Source File

Go to the documentation of this file.
00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00032 #include <phua_haptic/hd_hl_apis_callbacks.h>
00033 
00034 using namespace std;
00035 
00036 extern DeviceData RawDeviceData;
00037 /*~~~~~~~~~~~~~~~~~ 
00038 ||   CALLBACKS    ||
00039 ~~~~~~~~~~~~~~~~~~*/
00040 
00041 HDCallbackCode HDCALLBACK updateDeviceCallback(void *pUserData)
00042 {
00043   DeviceData *RawDeviceData=(DeviceData*)pUserData;
00044   
00045   int nButtons = 0;
00046   
00047   static HDdouble T[16];
00048   static hduMatrix M;
00049   
00050   hdBeginFrame(hdGetCurrentDevice());
00051   
00052   /* Retrieve the current button(s). */
00053   hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
00054   
00055   /* In order to get the specific button 1 state, we use a bitmask to test for the HD_DEVICE_BUTTON_1 bit. */
00056   RawDeviceData->m_button1State = (nButtons & HD_DEVICE_BUTTON_1) ? HD_TRUE : HD_FALSE;
00057   
00058   /* In order to get the specific button 2 state, we use a bitmask to test for the HD_DEVICE_BUTTON_2 bit. */
00059   RawDeviceData->m_button2State = (nButtons & HD_DEVICE_BUTTON_2) ? HD_TRUE : HD_FALSE;
00060   
00061   /* Get the current location of the device.*/
00062   hdGetDoublev(HD_CURRENT_POSITION, RawDeviceData->m_devicePosition); /*milimeters*/
00063   
00064   /*Get joint angles*/
00065   hdGetDoublev(HD_CURRENT_JOINT_ANGLES, RawDeviceData->jointAngles); /*rad*/
00066   
00067   /*Get gimbal angles*/
00068   hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, RawDeviceData->gimbalAngles); /*rad*/
00069   
00070   // Get motor temperatures [0...1]
00071   hdGetDoublev(HD_MOTOR_TEMPERATURE, RawDeviceData->motor_temperature);
00072   
00073   // Get transform
00074   hdGetDoublev(HD_CURRENT_TRANSFORM, T);
00075   M = hduMatrix(T);
00076   M.transpose();
00077   RawDeviceData->end_effector_transform = M;
00078   
00079   /* Also check the error state of HDAPI. */
00080   RawDeviceData->m_error = hdGetError();
00081   
00082   /* Copy the position into our device_data structure. */
00083   hdEndFrame(hdGetCurrentDevice());
00084   
00085   return HD_CALLBACK_CONTINUE;    
00086 }
00087 
00088 HDCallbackCode HDCALLBACK updateDeviceParametersCallback(void *pUserData)
00089 {
00090   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00091   
00092   // read max stifness value from device
00093   hdGetDoublev(HD_NOMINAL_MAX_STIFFNESS, &(RobotVars->phantom_data.max_stiffness));
00094   
00095   // read max damping value from device
00096   hdGetDoublev(HD_NOMINAL_MAX_DAMPING, &(RobotVars->phantom_data.max_damping));
00097   
00098   // read max force value from device
00099   hdGetDoublev(HD_NOMINAL_MAX_FORCE, &(RobotVars->phantom_data.max_force)); //N
00100   
00101   // read max continuous force value from device
00102   hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &(RobotVars->phantom_data.max_continuous_force)); //N
00103 
00104   return HD_CALLBACK_DONE;    
00105 }
00106 
00107 HDCallbackCode HDCALLBACK copyDeviceDataCallback(void *pUserData)
00108 {
00109   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00110   static double rotVals[4][4];
00111 #if ROS_VERSION_MINIMUM(1, 8, 0)   //At least FUERTE, V. Santos, 27-Mai-2013,23:34
00112   static tf::Quaternion q;
00113   static tf::Matrix3x3 mat;
00114 #else
00115   static btQuaternion q;
00116   static btMatrix3x3 mat;
00117 #endif
00118   
00119   if(!RobotVars->parameters.exit_status)
00120   {
00121     //alternate button 1 clicked state
00122     if((bool)RawDeviceData.m_button1State==TRUE && (bool)RobotVars->phantom_data.m_button1State==FALSE)
00123     {
00124       RobotVars->phantom_data.m_button1Clicked=!RobotVars->phantom_data.m_button1Clicked;
00125     }
00126     
00127     //alternate button 2 clicked state
00128     if((bool)RawDeviceData.m_button2State==TRUE && (bool)RobotVars->phantom_data.m_button2State==FALSE)
00129     {
00130       RobotVars->phantom_data.m_button2Clicked=!RobotVars->phantom_data.m_button2Clicked;
00131     }
00132     
00133     //check button 1
00134     if(RawDeviceData.m_button1State!=RobotVars->phantom_data.m_button1State)
00135     {
00136       RobotVars->phantom_data.m_button1State=RawDeviceData.m_button1State;
00137     }
00138     //check button 2
00139     if(RawDeviceData.m_button2State!=RobotVars->phantom_data.m_button2State)
00140     {
00141       RobotVars->phantom_data.m_button2State=RawDeviceData.m_button2State;
00142     }
00143     
00144     //check position
00145     if(RawDeviceData.m_devicePosition[0]!=RobotVars->phantom_data.m_devicePosition[0] ||
00146       RawDeviceData.m_devicePosition[1]!=RobotVars->phantom_data.m_devicePosition[1] ||
00147       RawDeviceData.m_devicePosition[2]!=RobotVars->phantom_data.m_devicePosition[2])
00148     {
00149       //update position
00150       if(RobotVars->parameters.coordinate_resolution==1.)
00151       {
00152         // resolution by milimeters
00153         RobotVars->phantom_data.m_devicePosition[0]=round(RawDeviceData.m_devicePosition[2]);
00154         RobotVars->phantom_data.m_devicePosition[1]=round(RawDeviceData.m_devicePosition[0]);
00155         RobotVars->phantom_data.m_devicePosition[2]=round(RawDeviceData.m_devicePosition[1]);
00156       }
00157       else
00158       {
00159         // all other resolutions 
00160         RobotVars->phantom_data.m_devicePosition[0]=RetrievePrecisionFromDouble(RawDeviceData.m_devicePosition[2],RobotVars->parameters.coordinate_resolution);
00161         RobotVars->phantom_data.m_devicePosition[1]=RetrievePrecisionFromDouble(RawDeviceData.m_devicePosition[0],RobotVars->parameters.coordinate_resolution);
00162         RobotVars->phantom_data.m_devicePosition[2]=RetrievePrecisionFromDouble(RawDeviceData.m_devicePosition[1],RobotVars->parameters.coordinate_resolution);
00163       }
00164     }
00165     if(RobotVars->parameters.cntrl_pos_back_front<0.)
00166     {
00167       RobotVars->phantom_data.m_devicePosition[0] = -RobotVars->phantom_data.m_devicePosition[0];
00168       RobotVars->phantom_data.m_devicePosition[1] = -RobotVars->phantom_data.m_devicePosition[1];
00169     }
00170     
00171     
00172     //check joint angles
00173     if(RawDeviceData.jointAngles[0]!=RobotVars->phantom_data.jointAngles[0] || RawDeviceData.jointAngles[1]!=RobotVars->phantom_data.jointAngles[1] ||RawDeviceData.jointAngles[2]!=RobotVars->phantom_data.jointAngles[2])
00174     {
00175       //RobotVars->phantom_data.jointAngles=RawDeviceData.jointAngles * (180./arma::datum::pi);
00176       //V. Santos, 27-Mai-2013,23:38 replace previous by next
00177       RobotVars->phantom_data.jointAngles=RawDeviceData.jointAngles * (180./M_PI);
00178     }
00179     
00180     //check gimbal angles
00181     if(RawDeviceData.gimbalAngles[0]!=RobotVars->phantom_data.gimbalAngles[0] || RawDeviceData.gimbalAngles[1]!=RobotVars->phantom_data.gimbalAngles[1] ||RawDeviceData.gimbalAngles[2]!=RobotVars->phantom_data.gimbalAngles[2])
00182     {
00183       //RobotVars->phantom_data.gimbalAngles = RawDeviceData.gimbalAngles * (180./arma::datum::pi);
00184       //V. Santos, 27-Mai-2013,23:38 replace previous by next
00185       RobotVars->phantom_data.gimbalAngles = RawDeviceData.gimbalAngles * (180./M_PI);
00186     }
00187     
00188     if(RawDeviceData.motor_temperature[0] != RobotVars->phantom_data.motor_temperature[0] ||
00189       RawDeviceData.motor_temperature[1] != RobotVars->phantom_data.motor_temperature[1] ||
00190       RawDeviceData.motor_temperature[2] != RobotVars->phantom_data.motor_temperature[2])
00191     {
00192       // motor temperatures
00193       memcpy(&(RobotVars->phantom_data.motor_temperature),&(RawDeviceData.motor_temperature),sizeof(RawDeviceData.motor_temperature));
00194     }
00195     
00196     // Get transform matrixes
00197    
00198     RobotVars->phantom_data.phantom_to_base_point.setOrigin( tf::Vector3( RawDeviceData.m_devicePosition[0], RawDeviceData.m_devicePosition[1], RawDeviceData.m_devicePosition[2]) );
00199 
00200     RawDeviceData.end_effector_transform.get(rotVals);
00201 
00202 #if ROS_VERSION_MINIMUM(1, 8, 0)   //At least FUERTE, V. Santos, 27-Mai-2013,23:34
00203     mat = tf::Matrix3x3(rotVals[0][0], rotVals[0][1], rotVals[0][2],
00204                       rotVals[1][0], rotVals[1][1], rotVals[1][2],
00205                       rotVals[2][0], rotVals[2][1], rotVals[2][2]);
00206 #else
00207     mat = btMatrix3x3(rotVals[0][0], rotVals[0][1], rotVals[0][2],
00208                       rotVals[1][0], rotVals[1][1], rotVals[1][2],
00209                       rotVals[2][0], rotVals[2][1], rotVals[2][2]);
00210 
00211 #endif
00212     mat.getRotation(q);
00213 
00214     RobotVars->phantom_data.phantom_to_base_point.setRotation( q );
00215     
00216     RobotVars->phantom_data.base_point_to_pen_tip.setOrigin( tf::Vector3( 0,0,-40));
00217 
00218 #if ROS_VERSION_MINIMUM(1, 8, 0)   //At least FUERTE, V. Santos, 27-Mai-2013,23:34
00219     mat = tf::Matrix3x3(1, 0,0,
00220                       0,1,0,
00221                       0,0,1);
00222 #else
00223     mat = btMatrix3x3(1, 0,0,
00224                       0,1,0,
00225                       0,0,1);
00226 #endif
00227     mat.getRotation(q);
00228     RobotVars->phantom_data.base_point_to_pen_tip.setRotation( q );
00229     // end of transform matrixes
00230     
00231     /* Copy error state of HDAPI. */
00232     RobotVars->phantom_data.m_error = RawDeviceData.m_error;
00233   }
00234   
00235   return HD_CALLBACK_DONE;
00236 }
00237 
00238 HDCallbackCode HDCALLBACK forcefeedbackCallback(void *pUserData)
00239 {
00240   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00241   static timespec loop_rate_start,loop_rate_stop;
00242   long double time_in_seconds;
00243   static int counter=0;
00244   static hduVector3Dd TOTAL_FORCE_1=hduVector3Dd(0.,0.,0.), TOTAL_FORCE_2=hduVector3Dd(0.,0.,0.);
00245   static bool first_run = FALSE;
00246 //   static float phantom_pen_apparent_mass = 0.045 + 0.01;
00247   static hduVector3Dd GRAVITY_COMPENSATION_FORCE = TransformWorldCoordinatesToPHaNToMCoordinates(hduVector3Dd(0,0,(PHANToM_TOOLTIP_MASS * 9.81)));
00248   
00249   // variable for error handling
00250   HDErrorInfo error;
00251   
00252   // ****** START OF DEMO 1: WORKSPACE LIMITS ******
00253   if(RobotVars->haptics_data.chosen_demo == 1 && RobotVars->haptic_loop_start)
00254   {
00255     //compute force for distance to workspace limits
00256     TOTAL_FORCE_1 = CalculateWorkspaceDemoForce(RobotVars);
00257     
00258     //apply forces
00259     if(TOTAL_FORCE_1.magnitude() > 0.)
00260     {
00261       hlBeginFrame();
00262       hdSetDoublev(HD_CURRENT_FORCE, TOTAL_FORCE_1);
00263       hlEndFrame();
00264       RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(TOTAL_FORCE_1); //NEWTONS
00265     }
00266     else
00267     {
00268       //gravity compensation
00269       hlBeginFrame();
00270       hdSetDoublev(HD_CURRENT_FORCE, GRAVITY_COMPENSATION_FORCE);
00271       hlEndFrame();
00272       RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(GRAVITY_COMPENSATION_FORCE);
00273     }
00274     //error handling
00275     if (HD_DEVICE_ERROR(error = hdGetError()))
00276     {
00277         hduPrintError(stderr, &error, "Error during force feedback callback\n");
00278 
00279         if (hduIsSchedulerError(&error))
00280         {
00281             return HD_CALLBACK_DONE;
00282         }        
00283     }
00284     
00285     //calculate computational frequency
00286     if(counter<100)
00287       counter++;
00288     else
00289     {
00290       clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00291       time_in_seconds =
00292       ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00293       ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00294       RobotVars->parameters.haptics_rate = round((long double)counter / (time_in_seconds));
00295       clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00296       counter = 0;
00297     }
00298   }// ****** END OF DEMO 1: WORKSPACE LIMITS ******
00299   // ****** START OF DEMO 2: DRAWING ******
00300   else if(RobotVars->haptics_data.chosen_demo == 2 && RobotVars->haptic_loop_start)
00301   {
00302     //compute force for distance to workspace limits
00303     TOTAL_FORCE_1 = CalculateWorkspaceDemoForce(RobotVars);
00304 //     TOTAL_FORCE_1 = 2. * TOTAL_FORCE_1 / 3.;
00305     
00306     if(RobotVars->haptics_data.demo_2_point_1.magnitude() > 0. &&
00307       RobotVars->haptics_data.demo_2_point_2.magnitude() > 0. &&
00308       RobotVars->haptics_data.demo_2_point_3.magnitude() > 0. &&
00309       first_run == FALSE)
00310     {
00311       std::cout<<"Building Plane...";
00312       Demo2_BuildPlane(RobotVars);
00313       std::cout<<"...DONE!"<<std::endl;
00314       first_run = TRUE;
00315     }
00316     else if(RobotVars->haptics_data.demo_2_point_1.magnitude() > 0. &&
00317       RobotVars->haptics_data.demo_2_point_2.magnitude() > 0. &&
00318       RobotVars->haptics_data.demo_2_point_3.magnitude() > 0.&&
00319       first_run == TRUE)
00320     {
00321       //compute force for distance to plane
00322       TOTAL_FORCE_2 = CalculatePlaneDrawingDemoForce(RobotVars);
00323     }
00324     else
00325     {
00326       //keep waiting for points
00327       if(RobotVars->phantom_data.m_button2Clicked)
00328       {
00329         // pen tip
00330         // hduVector3Dd joystick_position = RobotVars->phantom_data.m_PenTipPosition;
00331         // wrist point
00332         hduVector3Dd joystick_position = RobotVars->phantom_data.m_devicePosition;
00333         //point tests
00334         if(RobotVars->haptics_data.demo_2_point_1.magnitude() == 0. &&
00335           RobotVars->haptics_data.demo_2_point_2.magnitude() == 0. && 
00336           RobotVars->haptics_data.demo_2_point_3.magnitude() == 0.)
00337         {
00338           RobotVars->haptics_data.demo_2_point_1 = joystick_position;
00339           cout<<"Setting Point 1 to: "<<RobotVars->haptics_data.demo_2_point_1<<endl;
00340           RobotVars->phantom_data.m_button2Clicked=!RobotVars->phantom_data.m_button2Clicked;
00341         }
00342         if(RobotVars->haptics_data.demo_2_point_1.magnitude() != 0. &&
00343           RobotVars->haptics_data.demo_2_point_2.magnitude() == 0. && 
00344           RobotVars->haptics_data.demo_2_point_3.magnitude() == 0. &&
00345           RobotVars->haptics_data.demo_2_point_1 != joystick_position)
00346         {
00347           RobotVars->haptics_data.demo_2_point_2 = joystick_position;
00348           cout<<"Setting Point 2 to: "<<RobotVars->haptics_data.demo_2_point_2<<endl;
00349           RobotVars->phantom_data.m_button2Clicked=!RobotVars->phantom_data.m_button2Clicked;
00350         }
00351         if(RobotVars->haptics_data.demo_2_point_1.magnitude() != 0. &&
00352           RobotVars->haptics_data.demo_2_point_2.magnitude() != 0. && 
00353           RobotVars->haptics_data.demo_2_point_3.magnitude() == 0. &&
00354           RobotVars->haptics_data.demo_2_point_1 != joystick_position &&
00355           RobotVars->haptics_data.demo_2_point_2 != joystick_position)
00356         {
00357           RobotVars->haptics_data.demo_2_point_3 = joystick_position;
00358           cout<<"Setting Point 3 to: "<<RobotVars->haptics_data.demo_2_point_3<<endl;
00359           RobotVars->phantom_data.m_button2Clicked=!RobotVars->phantom_data.m_button2Clicked;
00360         }
00361       }
00362       first_run = FALSE;
00363     }
00364     
00365 //     cout<<"workspace:"<<TOTAL_FORCE_1<<endl;
00366 //     cout<<"demo 2:"<<TOTAL_FORCE_2<<endl;
00367     
00368     //apply forces
00369     if(TOTAL_FORCE_1.magnitude() > 0. || TOTAL_FORCE_2.magnitude() > 0.)
00370     {
00371       hlBeginFrame();
00372       hdSetDoublev(HD_CURRENT_FORCE, TOTAL_FORCE_1+TOTAL_FORCE_2);
00373       hlEndFrame();
00374       RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(TOTAL_FORCE_1+TOTAL_FORCE_2); //NEWTONS
00375     }
00376     else
00377     {
00378       //gravity compensation
00379       hlBeginFrame();
00380       hdSetDoublev(HD_CURRENT_FORCE, GRAVITY_COMPENSATION_FORCE);
00381       hlEndFrame();
00382       RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(GRAVITY_COMPENSATION_FORCE);
00383     }
00384     //error handling
00385     if (HD_DEVICE_ERROR(error = hdGetError()))
00386     {
00387         hduPrintError(stderr, &error, "Error during force feedback callback\n");
00388 
00389         if (hduIsSchedulerError(&error))
00390         {
00391             return HD_CALLBACK_DONE;
00392         }        
00393     }
00394     
00395     //calculate computational frequency
00396     if(counter<100)
00397       counter++;
00398     else
00399     {
00400       clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00401       time_in_seconds =
00402       ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00403       ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00404       RobotVars->parameters.haptics_rate = round((long double)counter / (time_in_seconds));
00405       clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00406       counter = 0;
00407     }
00408     
00409     
00410   }// ****** END OF DEMO 2: DRAWING ******
00411   // ****** START OF DEMO 3: USER PATH ******
00412   else if(RobotVars->haptics_data.chosen_demo == 3 && RobotVars->haptic_loop_start)
00413   {
00414     //gravity compensation
00415     hlBeginFrame();
00416     hdSetDoublev(HD_CURRENT_FORCE, GRAVITY_COMPENSATION_FORCE);
00417     hlEndFrame();
00418     RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(GRAVITY_COMPENSATION_FORCE);
00419     //calculate computational frequency
00420     if(counter<100)
00421       counter++;
00422     else
00423     {
00424       clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00425       time_in_seconds =
00426       ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00427       ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00428       RobotVars->parameters.haptics_rate = round((long double)counter / (time_in_seconds));
00429       clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00430       counter = 0;
00431     }
00432   }// ****** END OF DEMO 3: USER PATH ******
00433   // ****** START OF DEMO 4: BALANCING ******
00434   else if(RobotVars->haptics_data.chosen_demo == 4 && RobotVars->haptic_loop_start)
00435   {
00436     //compute force for distance to workspace limits
00437 //     TOTAL_FORCE_1 = CalculateWorkspaceDemoForce(RobotVars);
00438     hduVector3Dd COP_force = Leg_COP_Monitoring(RobotVars);
00439     
00440     if(COP_force.magnitude() > 0.0 || TOTAL_FORCE_1.magnitude() > 0.0)
00441     {
00442       hlBeginFrame();
00443       hdSetDoublev(HD_CURRENT_FORCE, TransformWorldCoordinatesToPHaNToMCoordinates(COP_force) + TOTAL_FORCE_1);
00444       hlEndFrame();
00445       RobotVars->haptics_data.applied_force = COP_force + TransformPHANToMCoordinatesToWorldCoordinates(TOTAL_FORCE_1);
00446     }
00447     else
00448     {
00449       //gravity compensation
00450       hlBeginFrame();
00451       hdSetDoublev(HD_CURRENT_FORCE, GRAVITY_COMPENSATION_FORCE);
00452       hlEndFrame();
00453       RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(GRAVITY_COMPENSATION_FORCE);
00454     }
00455     //calculate computational frequency
00456     if(counter<100)
00457       counter++;
00458     else
00459     {
00460       clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00461       time_in_seconds =
00462       ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00463       ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00464       RobotVars->parameters.haptics_rate = round((long double)counter / (time_in_seconds));
00465       clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00466       counter = 0;
00467     }
00468   }// ****** END OF DEMO 4: BALANCING ******
00469   else if(RobotVars->haptics_data.chosen_demo == 0 && RobotVars->haptic_loop_start)
00470   {
00471     //gravity compensation
00472     hlBeginFrame();
00473     hdSetDoublev(HD_CURRENT_FORCE, GRAVITY_COMPENSATION_FORCE);
00474     hlEndFrame();
00475     RobotVars->haptics_data.applied_force = TransformPHANToMCoordinatesToWorldCoordinates(GRAVITY_COMPENSATION_FORCE);
00476     
00477     //calculate computational frequency
00478     if(counter<100)
00479       counter++;
00480     else
00481     {
00482       clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00483       time_in_seconds =
00484       ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00485       ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00486       RobotVars->parameters.haptics_rate = round((long double)counter / (time_in_seconds));
00487       clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00488       counter = 0;
00489     }
00490   }
00491   else
00492   {
00493     RobotVars->parameters.haptics_rate = 0.;
00494     counter = 0;
00495     if(RobotVars->haptics_data.demo_2_point_1.magnitude() != 0.)
00496     {
00497     RobotVars->haptics_data.demo_2_point_1 = hduVector3Dd(0,0,0);
00498     RobotVars->haptics_data.demo_2_point_2 = hduVector3Dd(0,0,0);
00499     RobotVars->haptics_data.demo_2_point_3 = hduVector3Dd(0,0,0);
00500     TOTAL_FORCE_1=hduVector3Dd(0.,0.,0.);
00501     TOTAL_FORCE_2=hduVector3Dd(0.,0.,0.);
00502     first_run = FALSE;
00503     }
00504     RobotVars->haptics_data.applied_force = hduVector3Dd(0,0,0);
00505   }
00506   
00507   
00508   if(!RobotVars->parameters.exit_status)
00509   {
00510     // this assures the callback is run at every servo loop tick
00511     return HD_CALLBACK_CONTINUE;
00512   }
00513   else
00514   {
00515     return HD_CALLBACK_DONE;
00516   }
00517 }
00518 
00519 HDCallbackCode HDCALLBACK CalibrationCallback(void *pUserData)
00520 {
00521   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00522 
00523   if(RobotVars->phantom_data.need_update && RobotVars->phantom_data.phantom_on)
00524   {
00525     int calibrationStyle= HD_CALIBRATION_INKWELL;
00526 
00527     hdBeginFrame(hdGetCurrentDevice());
00528     HDenum status = hdCheckCalibration();
00529     hdEndFrame(hdGetCurrentDevice());
00530     
00531     if (status == HD_CALIBRATION_OK)
00532     {
00533       printf("PHANToM calibration OK! Will not update.\n");
00534     }
00535     else
00536     {
00537       hdUpdateCalibration(calibrationStyle);
00538       printf("PHANToM successfully calibrated!\n");
00539     }
00540     RobotVars->phantom_data.need_update=FALSE;
00541   } 
00542   // this assures the callback is run at every servo loop tick
00543   return HD_CALLBACK_CONTINUE;
00544 }