phua_haptic: /home/laradmin/lar/bases/phua_haptic/src/main.cpp Source 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 <main.h>
00033 
00034 using namespace std;
00035 
00036 /*~~~~~~~~~~~~~~~~~~~~~~~~ 
00037 ||      global vars      ||
00038 ~~~~~~~~~~~~~~~~~~~~~~~~~*/
00040 DeviceData RawDeviceData;
00041 
00042 /*~~~~~~~~~~~~~~~~~ 
00043 ||      main      ||
00044 ~~~~~~~~~~~~~~~~~~*/
00055 int main(int argc, char *argv[])
00056 {
00057   shared_vars_t RobotVars;
00058   memset(&RobotVars, 0, sizeof(RobotVars));
00059   
00060   
00061   HDSchedulerHandle hUpdateHandle = NULL;
00062   HDSchedulerHandle hForceHandle = NULL;
00063   HDSchedulerHandle hCalibrationHandle = NULL;
00064   HDErrorInfo error;
00065 
00066   //check for inline inputs
00067   //must be path to USB port
00068   if(argc>2){
00069     cout<<"********************************************************************"<<endl;
00070     cout<<"    Program execution stopped. Please insert only one argument."<<endl;
00071     cout<<"********************************************************************"<<endl;
00072     return EXIT_FAILURE;
00073   }
00074   else if(argc==1){
00075     //if no user argument inserted the program attempts standard USB port
00076     cout<<"***********************************************************************"<<endl;
00077     cout<<" No argument inserted. Using standard path to USB port [/dev/ttyUSB0]."<<endl;
00078     cout<<"***********************************************************************"<<endl;
00079     RobotVars.servo=(hitec_5980SG*) new hitec_5980SG("/dev/ttyUSB0");
00080     //if port not open
00081     if(!RobotVars.servo->IsActive())
00082     {
00083       cout<<"***********************************************************************"<<endl;
00084       return EXIT_FAILURE;
00085     }
00086     /* Initialize the device, must be done before attempting to call any hd functions. */
00087     RobotVars.phantom_data.hHD = hdInitDevice(HD_DEFAULT_DEVICE);
00088     /*check for device error*/
00089     if (HD_DEVICE_ERROR(error = hdGetError()))
00090     {
00091       hduPrintError(stderr, &error, "Failed to initialize the device");
00092       RobotVars.phantom_data.phantom_on=FALSE;
00093       return EXIT_FAILURE;
00094     }
00095     else
00096     {
00097       RobotVars.phantom_data.phantom_on=TRUE;
00098     }
00099   }
00100   else{
00101     // check for other arguments 
00102     string input_str (argv[1]);
00103     string dbg_str ("--debug");
00104     string hlp_str ("--help");
00105     string dir_str ("/");
00106     // check for ´--help´
00107     //check if user placed "--" in the beggining
00108     if(input_str==hlp_str)
00109     {
00110       print_help();
00111       return EXIT_SUCCESS;
00112     }
00113     else if(input_str==dbg_str)
00114     {
00115       //check for ´debug´
00116       // debug argument allows for program execution to run without USB/FireWire connection
00117       RobotVars.servo=(hitec_5980SG*) new hitec_5980SG("/dev/ttyUSB0"); //no matter if comm not sucessful
00118       /* Initialize the device, must be done before attempting to call any hd functions. */
00119       RobotVars.phantom_data.hHD = hdInitDevice(HD_DEFAULT_DEVICE);
00120       /*check for device error*/
00121       if (HD_DEVICE_ERROR(error = hdGetError()))
00122       {
00123         RobotVars.phantom_data.phantom_on=FALSE;
00124       }
00125       else
00126       {
00127         RobotVars.phantom_data.phantom_on=TRUE;
00128       }
00129     }
00130     else
00131     {
00132       //separate gibberish from unix paths
00133       if(input_str.find(dir_str)==0)
00134       {
00135         //try to open port
00136         cout<<"*********************************************************"<<endl;
00137         cout<<"   Using inserted path to USB port ["<<argv[1]<<"]."<<endl;
00138         cout<<"*********************************************************"<<endl;
00139         RobotVars.servo=(hitec_5980SG*) new hitec_5980SG(argv[1]);
00140         if(!RobotVars.servo->IsActive())
00141         {
00142           cout<<"*********************************************************"<<endl;
00143           return EXIT_FAILURE;
00144         }
00145         /* Initialize the device, must be done before attempting to call any hd functions. */
00146         RobotVars.phantom_data.hHD = hdInitDevice(HD_DEFAULT_DEVICE);
00147         /*check for device error*/
00148         if (HD_DEVICE_ERROR(error = hdGetError()))
00149         {
00150           hduPrintError(stderr, &error, "Failed to initialize the device");
00151           RobotVars.phantom_data.phantom_on=FALSE;
00152           return EXIT_FAILURE;           
00153         }
00154         else
00155         {
00156           RobotVars.phantom_data.phantom_on=TRUE;
00157         }
00158       }
00159       else
00160       {
00161         cout<<"******************************************************************"<<endl;
00162         cout<<" Invalid program syntax call. Use ´--help´ for more information"<<endl;
00163         cout<<"******************************************************************"<<endl;
00164         return EXIT_FAILURE;
00165       }
00166     }
00167   }
00168   
00169   if(RobotVars.phantom_data.phantom_on)
00170   {
00171     // start haptic context
00172     RobotVars.phantom_data.hHLRC = hlCreateContext(RobotVars.phantom_data.hHD);
00173     hlMakeCurrent(RobotVars.phantom_data.hHLRC);
00174     hlDisable(HL_USE_GL_MODELVIEW);// VERY IMPORTANT!!! IF NOT DISABLED PROGRAM SEGFAULTS DUE TO NO REAL USE OF OPENGL PIXEL BUFFER
00175     
00176     //variable that is used so store phantom information on the fly
00177     memset(&RawDeviceData, 0, sizeof(DeviceData));
00178     // Schedule the main scheduler callback that updates the device state and the haptics callback
00179     hUpdateHandle = hdScheduleAsynchronous(updateDeviceCallback, &RawDeviceData, HD_MAX_SCHEDULER_PRIORITY);
00180     hForceHandle = hdScheduleAsynchronous(forcefeedbackCallback, &RobotVars, HD_DEFAULT_SCHEDULER_PRIORITY);
00181     hCalibrationHandle = hdScheduleAsynchronous(CalibrationCallback, &RobotVars, HD_MIN_SCHEDULER_PRIORITY);
00182 
00183     // Start the servo loop scheduler.
00184     hdStartScheduler();
00185     //check for errors
00186     if (HD_DEVICE_ERROR(error = hdGetError()))
00187     {
00188         hduPrintError(stderr, &error, "Failed to start the scheduler");
00189         return EXIT_FAILURE;           
00190     }
00191     // run the callback to update device parameters
00192     hdScheduleSynchronous(updateDeviceParametersCallback,&RobotVars,HD_DEFAULT_SCHEDULER_PRIORITY);
00193   }
00194   //clean screen
00195   ScreenClear();
00196   //greeting
00197   print_greeting(&RobotVars);
00198   
00199   if(RobotVars.phantom_data.phantom_on)
00200     cout<<"* OpenHaptics scheduler callbacks running."<<endl;
00201   
00202   //init humanoid class
00203   RobotVars.humanoid_f=(humanoid*) new humanoid();
00204   
00205   //parameter initialization
00206   RobotVars.parameters.exit_status=FALSE;
00207   RobotVars.parameters.coordinate_resolution=1.;
00208   RobotVars.parameters.pos_coord_scale = 2.;
00209   RobotVars.parameters.manual_speed_control=TRUE;
00210   RobotVars.parameters.automatic_speed_control=FALSE;
00211   RobotVars.parameters.robot_home_position_base_speed = 15; // [ 1...255 ]
00212   RobotVars.parameters.automatic_speed_control_freq = 100.; //Hz
00213   
00214   RobotVars.update_labels=FALSE;
00215   RobotVars.parameters.graphics_refresh_rate = 22.; //HZ
00216   
00217   RobotVars.phantom_data.need_update=FALSE;
00218   
00219   RobotVars.haptics_data.chosen_demo = 0;
00220   RobotVars.haptics_data.demo_2_point_1 = hduVector3Dd(0,0,0);
00221   RobotVars.haptics_data.demo_2_point_2 = hduVector3Dd(0,0,0);
00222   RobotVars.haptics_data.demo_2_point_3 = hduVector3Dd(0,0,0);
00223   RobotVars.haptics_data.demo_2_Plane = hduPlaned(hduVector3Dd(0,0,0),0.0);
00224   RobotVars.haptics_data.demo_user_path_point_storing = FALSE;
00225   RobotVars.haptics_data.demo_user_path_is_run_once = TRUE;
00226   RobotVars.haptics_data.demo_user_path_run_start = FALSE;
00227   
00228   cout<<"* Robot parameters initialized."<<endl;
00229   
00230   //ROS INIT ###########################################################
00231   ros::init(argc, argv, "phua"); // name of the system
00232   ros::NodeHandle n;
00233 //   ros::Rate r(1);
00234   ros::Publisher marker_pub = n.advertise<visualization_msgs::MarkerArray>("workspace_sphere", 1);
00235   
00236   
00237   visualization_msgs::MarkerArray markersMsg;
00238   //   #################################################################
00239   static tf::TransformBroadcaster broadcaster;
00240   static tf::TransformListener listener;
00241   
00242   //define phantom pre transform
00243 #if ROS_VERSION_MINIMUM(1, 8, 0)   //At least FUERTE, V. Santos, 27-Mai-2013
00244   tf::Quaternion q;
00245   tf::Matrix3x3 mat;
00246   mat = tf::Matrix3x3(0, 0, 1,
00247                     1,0,0,
00248                     0, 1, 0); //rotz(90)*rotx(90)
00249 #else
00250   btQuaternion q;
00251   btMatrix3x3 mat;
00252   mat = btMatrix3x3(0, 0, 1,
00253                     1,0,0,
00254                     0, 1, 0); //rotz(90)*rotx(90)
00255 #endif
00256 
00257   RobotVars.phantom_data.world_to_phantom.setOrigin( tf::Vector3( 0,0,0 ));
00258 
00259   mat.getRotation(q);
00260   RobotVars.phantom_data.world_to_phantom.setRotation( q );
00261   
00262   cout<<"* ROS components initialized."<<endl;
00263   
00264   // mutex
00265   pthread_mutex_init(&(RobotVars.mutex_gtk),NULL);
00266   //create thread for handling gtk interface
00267   pthread_t thread_gtk;
00268   //run interface
00269   pthread_create( &thread_gtk, NULL, interface_init, &RobotVars);
00270   
00271   //vars used for calculations and checks
00272   short unsigned int prev_speed = 0;
00273   hduVector3Dd prev_position;
00274   double arm_curr_pos[3];
00275   memset(&arm_curr_pos, 0., sizeof(arm_curr_pos));
00276   double detached_leg_curr_pos[3];
00277   memset(&detached_leg_curr_pos, 0., sizeof(detached_leg_curr_pos));
00278   double hip_abduction_angle;
00279   hduVector3Dd pos_diff;
00280   hduVector3Dd instant_diff;
00281   hduVector3Dd last_position;
00282   double *avg_speed=new double[3];
00283   timespec tf,interval_time_i,loop_rate_start,loop_rate_stop;
00284   long double time_in_seconds;
00285   long double interval;
00286   int k_tick=0;
00287   int counter = 0;
00288 
00289   
00290   if(RobotVars.phantom_data.phantom_on)
00291   {
00292     cout<<"* Main loop started."<<endl;
00293     // cycle that updates device data using parameter set
00294     bool exit=RobotVars.parameters.exit_status;
00295     while(!exit)
00296     {
00297       /* Perform a synchronous call to update the most current device state.*/
00298       hdScheduleSynchronous(copyDeviceDataCallback,&RobotVars,HD_DEFAULT_SCHEDULER_PRIORITY);
00299       
00300       prev_position = RobotVars.phantom_data.m_devicePosition;
00301       
00302       //calculate pen tip coordinates/velocity
00303       ROS_CalculatePHANToMPenFrame(&broadcaster,&listener, &RobotVars);
00304       
00305       ROS_UpdateMarkers(markersMsg.markers, &RobotVars );
00306       marker_pub.publish(markersMsg);
00307       
00308       //keep setting current time
00309       clock_gettime(CLOCK_REALTIME, &interval_time_i);
00310       
00311       if(RobotVars.haptic_loop_start)
00312       {
00313         //entering here means the loop will start so vars and checks must begin
00314         UpdateJointDataByID(1000, 0., &RobotVars);
00315         //detect kinematic model
00316         if(RobotVars.parameters.kinematic_model==1)
00317         {
00318           cout<<" :: Controlling Right Arm ::"<<endl;
00319           //retrive current arm end pos;
00320           arm_curr_pos[0] = RobotVars.robot_kin_data.X_arm_end_right;
00321           arm_curr_pos[1] = RobotVars.robot_kin_data.Y_arm_end_right;
00322           arm_curr_pos[2] = RobotVars.robot_kin_data.Z_arm_end_right;
00323           //Determine haptic spheres center position
00324           RobotVars.haptics_data.arm_main_spheresPosition = (hduVector3Dd)RobotVars.phantom_data.m_devicePosition -
00325                                                             ((hduVector3Dd)arm_curr_pos / (double)RobotVars.parameters.pos_coord_scale);
00326                                                             
00327           RobotVars.haptics_data.arm_back_spherePosition = RobotVars.haptics_data.arm_main_spheresPosition
00328           + 
00329           (
00330           hduVector3Dd(-RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*sin(DegToRad(45.0)),
00331                        0.0,
00332                        -RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*cos(DegToRad(45.0)))
00333           /
00334           (double)RobotVars.parameters.pos_coord_scale
00335           );
00336         }
00337         else if(RobotVars.parameters.kinematic_model==2)
00338         {
00339           cout<<" :: Controlling Left Arm ::"<<endl;
00340           //retrive current arm end pos;
00341           arm_curr_pos[0] = RobotVars.robot_kin_data.X_arm_end_left;
00342           arm_curr_pos[1] = RobotVars.robot_kin_data.Y_arm_end_left;
00343           arm_curr_pos[2] = RobotVars.robot_kin_data.Z_arm_end_left;
00344           //Determine haptic spheres center position
00345           RobotVars.haptics_data.arm_main_spheresPosition = (hduVector3Dd)RobotVars.phantom_data.m_devicePosition -
00346           ((hduVector3Dd)arm_curr_pos / (double)RobotVars.parameters.pos_coord_scale);
00347           
00348           RobotVars.haptics_data.arm_back_spherePosition = RobotVars.haptics_data.arm_main_spheresPosition
00349           + 
00350           (
00351           hduVector3Dd(-RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*sin(DegToRad(45.0)),
00352                        0.0,
00353                        -RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*cos(DegToRad(45.0)))
00354                        /
00355                        (double)RobotVars.parameters.pos_coord_scale
00356                        );
00357         }
00358         else if(RobotVars.parameters.kinematic_model==3)
00359         {
00360           cout<<" :: Controlling Both Arms ::"<<endl;
00361           //retrive current arm end pos;
00362           arm_curr_pos[0] = RobotVars.robot_kin_data.X_arm_end_left;
00363           arm_curr_pos[1] = RobotVars.robot_kin_data.Y_arm_end_left;
00364           arm_curr_pos[2] = RobotVars.robot_kin_data.Z_arm_end_left;
00365           //Determine haptic spheres center position
00366           RobotVars.haptics_data.arm_main_spheresPosition = (hduVector3Dd)RobotVars.phantom_data.m_devicePosition -
00367           ((hduVector3Dd)arm_curr_pos / (double)RobotVars.parameters.pos_coord_scale);
00368           
00369           RobotVars.haptics_data.arm_back_spherePosition = RobotVars.haptics_data.arm_main_spheresPosition
00370           + 
00371           (
00372           hduVector3Dd(-RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*sin(DegToRad(45.0)),
00373                        0.0,
00374                        -RobotVars.humanoid_f->GetRobotDimension(ARM_LENGTH)*cos(DegToRad(45.0)))
00375                        /
00376                        (double)RobotVars.parameters.pos_coord_scale
00377                        );
00378         }
00379         else if(RobotVars.parameters.kinematic_model==4)
00380         {
00381           cout<<" :: Controlling Left Detached Leg ::"<<endl;
00382           memcpy(&detached_leg_curr_pos, &RobotVars.robot_kin_data.detached_leg_pos_left, sizeof(RobotVars.robot_kin_data.detached_leg_pos_left));
00383           //calc centers
00384           RobotVars.haptics_data.leg_main_spheresPosition = (hduVector3Dd)RobotVars.phantom_data.m_devicePosition -
00385           (hduVector3Dd(RobotVars.robot_kin_data.detached_leg_pos_left[0], RobotVars.robot_kin_data.detached_leg_pos_left[1], RobotVars.robot_kin_data.detached_leg_pos_left[2]) / (double)RobotVars.parameters.pos_coord_scale);
00386           RobotVars.haptics_data.leg_back_spherePosition = RobotVars.haptics_data.leg_main_spheresPosition
00387                                                           + 
00388                                                           (
00389                                                           hduVector3Dd(-RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*cos(DegToRad(40.0)),
00390                                                                         0.0,
00391                                                                         RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*sin(DegToRad(40.0)))
00392                                                                         /
00393                                                                         (double)RobotVars.parameters.pos_coord_scale
00394                                                           );
00395           RobotVars.haptics_data.leg_front_spherePosition = RobotVars.haptics_data.leg_main_spheresPosition
00396                                                           + 
00397                                                           (
00398                                                           hduVector3Dd(RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*cos(DegToRad(20.0)),
00399                                                                       0.0,
00400                                                                       RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*sin(DegToRad(20.0)))
00401                                                                       /
00402                                                                       (double)RobotVars.parameters.pos_coord_scale
00403                                                           );
00404         }
00405         else if(RobotVars.parameters.kinematic_model==5)
00406         {
00407           cout<<" :: Controlling Right Detached Leg ::"<<endl;
00408           memcpy(&detached_leg_curr_pos, &RobotVars.robot_kin_data.detached_leg_pos_right, sizeof(RobotVars.robot_kin_data.detached_leg_pos_right));
00409           //calc centers
00410           RobotVars.haptics_data.leg_main_spheresPosition = (hduVector3Dd)RobotVars.phantom_data.m_devicePosition -
00411           (hduVector3Dd(RobotVars.robot_kin_data.detached_leg_pos_right[0], RobotVars.robot_kin_data.detached_leg_pos_right[1], RobotVars.robot_kin_data.detached_leg_pos_right[2]-RobotVars.humanoid_f->GetRobotDimension(ANKLE_HEIGHT)) / (double)RobotVars.parameters.pos_coord_scale);
00412           RobotVars.haptics_data.leg_back_spherePosition = RobotVars.haptics_data.leg_main_spheresPosition
00413           + 
00414           (
00415           hduVector3Dd(-RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*cos(DegToRad(40.0)),
00416                        0.0,
00417                        RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*sin(DegToRad(40.0)))
00418                        /
00419                        (double)RobotVars.parameters.pos_coord_scale
00420                        );
00421           RobotVars.haptics_data.leg_front_spherePosition = RobotVars.haptics_data.leg_main_spheresPosition
00422                        + 
00423                        (
00424                        hduVector3Dd(RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*cos(DegToRad(20.0)),
00425                                     0.0,
00426                                     RobotVars.humanoid_f->GetRobotDimension(LEG_LENGTH)*sin(DegToRad(20.0)))
00427                                     /
00428                                     (double)RobotVars.parameters.pos_coord_scale
00429                                     );
00430         }
00431         else
00432         {
00433           //does not do anything if no kinematic model selected
00434           continue;
00435         }
00436         
00437         
00438         if(RobotVars.parameters.manual_speed_control)// ***************** MANUAL SPEED CONTROL MODE ***************** 
00439           cout<<"-> Running on Manual Servo Speed."<<endl;
00440         else// ***************** AUTOMATIC SPEED CONTROL MODE ***************** 
00441           cout<<"-> Running on Automatic Servo Speed."<<endl;
00442         
00443         //get instant phantom position
00444         //get coordinate difference from start point
00445         memcpy(&last_position, &RobotVars.phantom_data.m_devicePosition, sizeof(RobotVars.phantom_data.m_devicePosition));
00446         
00447         clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00448         
00449         if(!hdIsEnabled(HD_FORCE_OUTPUT))
00450           hdEnable(HD_FORCE_OUTPUT);
00451         
00452         //CONTROL LOOPS RUN HERE!!!
00453         while(RobotVars.haptic_loop_start && !exit)
00454         {
00455           /* Perform a synchronous call to update the most current device state.*/
00456           hdScheduleSynchronous(copyDeviceDataCallback,&RobotVars,HD_DEFAULT_SCHEDULER_PRIORITY);
00457           
00458           //calculate pen tip coordinates
00459           ROS_CalculatePHANToMPenFrame(&broadcaster,&listener, &RobotVars);
00460           
00461           ROS_UpdateMarkers(markersMsg.markers, &RobotVars );
00462           marker_pub.publish(markersMsg);
00463           
00464           // ***************** POSITION CONTROL *****************
00465           //get movement increment
00466           instant_diff = RobotVars.parameters.pos_coord_scale * (RobotVars.phantom_data.m_devicePosition-last_position);
00467           
00468           // redefine servo speed if necessary
00469           if(RobotVars.parameters.manual_speed_control && RobotVars.parameters.manual_speed!=prev_speed)
00470           {
00471             if(RobotVars.parameters.kinematic_model==1)
00472             {
00473               cout<<"-> Setting New Arm Servomotor Speed:"<<RobotVars.parameters.manual_speed<<"."<<endl;
00474               SetArmSpeed(RIGHT, RobotVars.parameters.manual_speed, &RobotVars);
00475             }
00476             else if(RobotVars.parameters.kinematic_model==2)
00477             {
00478               cout<<"-> Setting New Arm Servomotor Speed:"<<RobotVars.parameters.manual_speed<<"."<<endl;
00479               SetArmSpeed(LEFT, RobotVars.parameters.manual_speed, &RobotVars);
00480             }
00481             else if(RobotVars.parameters.kinematic_model==3)
00482             {
00483               cout<<"-> Setting New Arms Servomotors Speed:"<<RobotVars.parameters.manual_speed<<"."<<endl;
00484               SetArmSpeed(RIGHT, RobotVars.parameters.manual_speed, &RobotVars);
00485               SetArmSpeed(LEFT, RobotVars.parameters.manual_speed, &RobotVars);
00486             }
00487             else if(RobotVars.parameters.kinematic_model==4)
00488             {
00489               cout<<"-> Setting New Leg Servomotor Speed:"<<RobotVars.parameters.manual_speed<<"."<<endl;
00490               SetLegSpeed(LEFT, RobotVars.parameters.manual_speed, &RobotVars);
00491             }
00492             else if(RobotVars.parameters.kinematic_model==5)
00493             {
00494               cout<<"-> Setting New Leg Servomotor Speed:"<<RobotVars.parameters.manual_speed<<"."<<endl;
00495               SetLegSpeed(RIGHT, RobotVars.parameters.manual_speed, &RobotVars);
00496             }
00497             prev_speed=RobotVars.parameters.manual_speed;
00498           }
00499           
00500           //robot instructions will only proceed if position has changed over resolution or running user path
00501           if((fabs(instant_diff[0]) >= (RobotVars.parameters.coordinate_resolution) || 
00502             fabs(instant_diff[1]) >= (RobotVars.parameters.coordinate_resolution) || 
00503             fabs(instant_diff[2]) >= (RobotVars.parameters.coordinate_resolution) )
00504             ||
00505             RobotVars.haptics_data.demo_user_path_run_start)
00506           {
00507             //get coordinate difference from start point
00508             pos_diff = RobotVars.parameters.pos_coord_scale * (RobotVars.phantom_data.m_devicePosition - prev_position);
00509             //choose manual or automatic speed control
00510             if(RobotVars.parameters.manual_speed_control)
00511             {
00512               // ***************** MANUAL SPEED CONTROL MODE ***************** 
00513               
00514               //move corresponding limbs
00515               if(RobotVars.parameters.kinematic_model==1 && !RobotVars.haptics_data.demo_user_path_run_start)
00516               {
00517                 
00518                 MoveArmToCartesianPosition(RetrievePrecisionFromDouble((arm_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00519                                            RetrievePrecisionFromDouble((arm_curr_pos[1] - pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00520                                            RetrievePrecisionFromDouble((arm_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00521                                            RIGHT,
00522                                            &RobotVars);
00523               }
00524               else if(RobotVars.parameters.kinematic_model==2 && !RobotVars.haptics_data.demo_user_path_run_start)
00525               {
00526                 MoveArmToCartesianPosition(RetrievePrecisionFromDouble((arm_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00527                                            RetrievePrecisionFromDouble((arm_curr_pos[1] + pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00528                                            RetrievePrecisionFromDouble((arm_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00529                                            LEFT,
00530                                            &RobotVars);
00531               }
00532               else if(RobotVars.parameters.kinematic_model==3 && !RobotVars.haptics_data.demo_user_path_run_start)
00533               {
00534                 MoveArmToCartesianPosition(RetrievePrecisionFromDouble((arm_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00535                                            RetrievePrecisionFromDouble((arm_curr_pos[1] + pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00536                                            RetrievePrecisionFromDouble((arm_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00537                                            RIGHT,
00538                                            &RobotVars);
00539                 MoveArmToCartesianPosition(RetrievePrecisionFromDouble((arm_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00540                                            RetrievePrecisionFromDouble((arm_curr_pos[1] + pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00541                                            RetrievePrecisionFromDouble((arm_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00542                                            LEFT,
00543                                            &RobotVars);
00544               }
00545               else if(RobotVars.parameters.kinematic_model==4 && !RobotVars.haptics_data.demo_user_path_run_start)
00546               {
00547                 hip_abduction_angle = (RobotVars.phantom_data.gimbalAngles[2]+180.) * RobotVars.parameters.pos_coord_scale;
00548                   MoveDetachedLegToCartesianPosition(RetrievePrecisionFromDouble((detached_leg_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00549                                                   RetrievePrecisionFromDouble((detached_leg_curr_pos[1] + pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00550                                                   RetrievePrecisionFromDouble((detached_leg_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00551                                                   hip_abduction_angle,
00552                                                   LEFT,
00553                                                   &RobotVars);
00554               }
00555               else if(RobotVars.parameters.kinematic_model==5 && !RobotVars.haptics_data.demo_user_path_run_start)
00556               {
00557                 hip_abduction_angle = (RobotVars.phantom_data.gimbalAngles[2]+180.) * RobotVars.parameters.pos_coord_scale;
00558                   MoveDetachedLegToCartesianPosition(RetrievePrecisionFromDouble((detached_leg_curr_pos[0] + pos_diff[0]),RobotVars.parameters.coordinate_resolution),
00559                                                   RetrievePrecisionFromDouble((detached_leg_curr_pos[1] - pos_diff[1]),RobotVars.parameters.coordinate_resolution),
00560                                                   RetrievePrecisionFromDouble((detached_leg_curr_pos[2] + pos_diff[2]),RobotVars.parameters.coordinate_resolution),
00561                                                   hip_abduction_angle,
00562                                                   RIGHT,
00563                                                   &RobotVars);
00564               }
00565               else if( RobotVars.parameters.kinematic_model > 0 && RobotVars.haptics_data.chosen_demo == 3 && RobotVars.haptics_data.demo_user_path_run_start)
00566               {
00567                 PathFollowingExecute(&RobotVars);
00568               }
00569               else
00570               {
00571                 //does not do anything if no kinematic model selected | redundant
00572                 continue;
00573               }
00574               // ****************************************************************
00575             }
00576             else
00577             {
00578               // ***************** AUTOMATIC SPEED CONTROL MODE *****************
00579               clock_gettime(CLOCK_REALTIME, &tf);
00580               interval = ((long double)tf.tv_sec + (long double)tf.tv_nsec / (long double)1e9) - ((long double)interval_time_i.tv_sec + (long double)interval_time_i.tv_nsec / (long double)1e9);
00581               printf("Interval: %9.9Lf\n",interval);
00582               if(interval>=(1./RobotVars.parameters.automatic_speed_control_freq))
00583               {
00584                 cout<<"Speed Control!"<<endl;
00585                 
00586                 avg_speed[0] = RobotVars.phantom_data.average_speed[0];
00587                 avg_speed[1] = RobotVars.phantom_data.average_speed[1];
00588                 avg_speed[2] = RobotVars.phantom_data.average_speed[2];
00589                 
00590                 ArmsDifferencialSpeedControl(avg_speed, &RobotVars);
00591                 clock_gettime(CLOCK_REALTIME, &interval_time_i);
00592               }
00593               // ****************************************************************
00594             }
00595             //update direct kinematics at every iteration
00596             if(RobotVars.parameters.kinematic_model>=1 && RobotVars.parameters.kinematic_model<=3)
00597             {
00598               UpdateArmsDirKinData(&RobotVars);
00599             }
00600             else if(RobotVars.parameters.kinematic_model==4 || RobotVars.parameters.kinematic_model==5)
00601             {
00602               UpdateDetachedLegsDirKinData(&RobotVars);
00603             }
00604             else
00605             {
00606               //does not do anything if no kinematic model selected | redundant
00607               continue;
00608             }
00609             
00610             k_tick=0;
00611             
00612             //get coordinate difference from start point
00613             memcpy(&last_position, &RobotVars.phantom_data.m_devicePosition, sizeof(RobotVars.phantom_data.m_devicePosition));
00614           }
00615           else
00616           {
00617             //count ticks to stop robot
00618             k_tick++;
00619             if(k_tick==500)
00620             {
00621               UpdateJointDataByID(1000, 0., &RobotVars);
00622               if(RobotVars.parameters.kinematic_model>=1 && RobotVars.parameters.kinematic_model<=3)
00623               {
00624                 UpdateArmsDirKinData(&RobotVars);
00625               }
00626               else if(RobotVars.parameters.kinematic_model==4 || RobotVars.parameters.kinematic_model==5)
00627               {
00628                 UpdateDetachedLegsDirKinData(&RobotVars);
00629               }
00630               StopRobotMovement(&RobotVars);
00631             }
00632             else if(k_tick>2500)
00633               k_tick=500-1;
00634           }
00635           //update labels
00636           RobotVars.update_labels=TRUE;
00637           // ****************************************************
00638           exit=RobotVars.parameters.exit_status;
00639           //calculate control rate
00640           if(counter<100)
00641             counter++;
00642           else
00643           {
00644             clock_gettime(CLOCK_REALTIME, &loop_rate_stop);
00645             time_in_seconds =
00646             ((long double)loop_rate_stop.tv_sec + ((long double)loop_rate_stop.tv_nsec / (long double)1e9)) -
00647             ((long double)loop_rate_start.tv_sec + ((long double)loop_rate_start.tv_nsec / (long double)1e9));
00648 
00649             RobotVars.parameters.control_rate = round((long double)1. / (time_in_seconds/(long double)counter));
00650             counter = 0;
00651             clock_gettime(CLOCK_REALTIME, &loop_rate_start);
00652           }
00653         }
00654         StopRobotMovement(&RobotVars);
00655         cout<<" :: Terminating haptic loop!... ::"<<endl;
00656         RobotVars.parameters.control_rate = 0.;
00657         if(hdIsEnabled(HD_FORCE_OUTPUT))
00658           hdDisable(HD_FORCE_OUTPUT);
00659         if(RobotVars.haptics_data.chosen_demo == 2)
00660         {
00661           RobotVars.haptics_data.demo_2_point_1=hduVector3Dd(0,0,0);
00662           RobotVars.haptics_data.demo_2_point_2=hduVector3Dd(0,0,0);
00663           RobotVars.haptics_data.demo_2_point_3=hduVector3Dd(0,0,0);
00664         }
00665       } //end of haptic/control loop
00666       // update interface labels
00667       RobotVars.update_labels=TRUE;
00668       exit=RobotVars.parameters.exit_status;
00669     }
00670   }
00671   
00672   if(RobotVars.phantom_data.phantom_on || RobotVars.servo->IsActive())
00673   {
00674     cout<<"* Application termination initiated."<<endl<<"* Processing shutdown routines:"<<endl;
00675     cout<<"---> Main loop stopped."<<endl;
00676     cout<<"---> Waiting for GTK+ interface thread to close...";
00677   }
00678   
00679   // wait for interface to close
00680   pthread_join( thread_gtk, NULL);
00681   if(RobotVars.phantom_data.phantom_on || RobotVars.servo->IsActive())
00682     cout<<"DONE!"<<endl;
00683   else
00684   {
00685     cout<<"* Application termination initiated."<<endl<<"* Processing shutdown routines:"<<endl;
00686     cout<<"---> Main loop stopped."<<endl;
00687     cout<<"---> GTK+ interface closed."<<endl;
00688   }
00689   
00690   if(RobotVars.phantom_data.phantom_on)
00691   {
00692     cout<<"---> Cleaning OpenHaptics Toolkit variables:"<<endl;
00693     // For cleanup, unschedule callbacks and stop the servo loop.
00694     cout<<"     >> Deleting haptic context...";
00695     // hlDeleteContext(hlGetCurrentContext());
00696     // OK, SO THIS IS GETTING WEIRDER... NOW, CREATING AND DELETING HAPTIC CONTEXT IS GIVING SEGFAULT... TODO: TRY AND SOLVE THIS CRAP...
00697     hlMakeCurrent(NULL);
00698     cout<<"DONE!"<<endl;
00699     cout<<"     >> Stopping servo loop thread and scheduler...";
00700     hdUnschedule(hForceHandle);
00701     hdUnschedule(hUpdateHandle);
00702     hdStopScheduler();
00703     cout<<"DONE!"<<endl;
00704     cout<<"     >> Disabling PHANToM device...";
00705     hdDisableDevice(RobotVars.phantom_data.hHD);
00706     cout<<"DONE!"<<endl;
00707     
00708     //EXIT message
00709     cout<<"* Exiting now."<<endl<<">> [Cleanup errors may appear bellow this line...]"<<endl;
00710   }
00711   else
00712   {
00713     cout<<"* Exiting now."<<endl;
00714   }
00715   
00716   return EXIT_SUCCESS;
00717 }
00718 
00719 void ROS_CalculatePHANToMPenFrame(tf::TransformBroadcaster *broadcaster, tf::TransformListener *listener, void *pUserData)
00720 {
00721   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00722   
00723   static tf::StampedTransform world_to_pen_tip_transform;
00724   static timespec ts,tf;
00725   static long double time_in_seconds;
00726   static uint counter;
00727   static hduVector3Dd pos;
00728   double *avg_speed=new double[3];
00729   
00730   
00731   ros::Time time_ros;
00732   time_ros = ros::Time::now();
00733     
00734   broadcaster->sendTransform(tf::StampedTransform(RobotVars->phantom_data.world_to_phantom, time_ros, "/phantom_world_corrected", "/phantom_world"));
00735   
00736   broadcaster->sendTransform(tf::StampedTransform(RobotVars->phantom_data.phantom_to_base_point, time_ros, "/phantom_world", "/phantom_base_point"));
00737   
00738   broadcaster->sendTransform(tf::StampedTransform(RobotVars->phantom_data.base_point_to_pen_tip, time_ros, "/phantom_base_point", "/phantom_pen_tip"));
00739   
00740   try
00741   {
00742     listener->lookupTransform("/phantom_world_corrected", "/phantom_pen_tip", ros::Time(0),world_to_pen_tip_transform);
00743   }
00744   catch (tf::TransformException ex)
00745   {
00746     if(listener->waitForTransform("/phantom_world_corrected", "/phantom_pen_tip",ros::Time(0), ros::Duration(2.0)))
00747     {
00748       try
00749       {
00750         listener->lookupTransform("/phantom_world_corrected", "/phantom_pen_tip", ros::Time(0),world_to_pen_tip_transform);
00751       }
00752       catch (tf::TransformException ex)
00753       {
00754         ROS_ERROR("Joystick Transforms: Could not lookup transform after waiting 2 secs\n.%s",ex.what());
00755       }
00756     }
00757     else
00758     {
00759       ROS_ERROR("Joystick Transforms: Could not find valid transform after waiting 2 seconds\n.%s",ex.what());
00760     }
00761   }
00762   
00763   ros::spinOnce();
00764   
00765   RobotVars->phantom_data.m_PenTipPosition[0] = world_to_pen_tip_transform.getOrigin().x();
00766   RobotVars->phantom_data.m_PenTipPosition[1] = world_to_pen_tip_transform.getOrigin().y();
00767   RobotVars->phantom_data.m_PenTipPosition[2] = world_to_pen_tip_transform.getOrigin().z();
00768   
00769   if(counter>=15)
00770   {
00771   //calculate speed
00772   clock_gettime(CLOCK_REALTIME, &tf);
00773   time_in_seconds = ((long double)tf.tv_sec + ((long double)tf.tv_nsec / (long double)1e9)) - ((long double)ts.tv_sec + ((long double)ts.tv_nsec / (long double)1e9));
00774   
00775   
00776   CalculateAverageCartesianSpeed((RobotVars->phantom_data.m_PenTipPosition[0] - pos[0]),
00777                                 (RobotVars->phantom_data.m_PenTipPosition[1] - pos[1]),
00778                                 (RobotVars->phantom_data.m_PenTipPosition[2] - pos[2]),
00779                                 time_in_seconds,
00780                                 avg_speed);
00781   
00782   //update speed
00783   RobotVars->phantom_data.average_speed[0] = avg_speed[0];
00784   RobotVars->phantom_data.average_speed[1] = avg_speed[1];
00785   RobotVars->phantom_data.average_speed[2] = avg_speed[2];
00786   
00787   counter=0;
00788   pos = RobotVars->phantom_data.m_PenTipPosition;
00789   clock_gettime(CLOCK_REALTIME, &ts);
00790   }
00791   else
00792   {
00793     counter++;
00794   }
00795   delete avg_speed;
00796 }
00797 
00798 void ROS_UpdateMarkers(vector<visualization_msgs::Marker>& marker_vector, void *pUserData)
00799 {
00800   shared_vars_t*RobotVars=(shared_vars_t*)pUserData;
00801   string text;
00802   geometry_msgs::Point p;
00803   geometry_msgs::Point p2;
00804   //clear the whole vector
00805   marker_vector.clear();
00806   
00807   //TAGS
00808   //Base frame
00809   visualization_msgs::Marker marker_BASE_TAG;
00810   marker_BASE_TAG.header.frame_id = "/phantom_world_corrected";
00811   //marker info
00812   marker_BASE_TAG.header.stamp = ros::Time::now();
00813   marker_BASE_TAG.ns = "text_BASE_TAG";
00814   marker_BASE_TAG.action = visualization_msgs::Marker::ADD;
00815   //marker type
00816   marker_BASE_TAG.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00817   text = "/phantom_world";
00818   marker_BASE_TAG.text = text;
00819   marker_BASE_TAG.pose.position.x = 0.0;
00820   marker_BASE_TAG.pose.position.y = 0.0;
00821   marker_BASE_TAG.pose.position.z = 0.0;
00822   marker_BASE_TAG.scale.z = 10.0; 
00823   //marker color
00824   marker_BASE_TAG.color.r=0.5;
00825   marker_BASE_TAG.color.g=0.5;
00826   marker_BASE_TAG.color.b=0.5;
00827   marker_BASE_TAG.color.a=1;
00828   marker_BASE_TAG.id=0;
00829   marker_BASE_TAG.lifetime = ros::Duration();
00830   marker_vector.push_back(marker_BASE_TAG);
00831   //wirst frame
00832   visualization_msgs::Marker marker_WRIST_TAG;
00833   marker_WRIST_TAG.header.frame_id = "/phantom_base_point";
00834   //marker info
00835   marker_WRIST_TAG.header.stamp = ros::Time::now();
00836   marker_WRIST_TAG.ns = "text_wrist_TAG";
00837   marker_WRIST_TAG.action = visualization_msgs::Marker::ADD;
00838   //marker type
00839   marker_WRIST_TAG.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00840   text = "/phantom_wrist";
00841   marker_WRIST_TAG.text = text;
00842   marker_WRIST_TAG.pose.position = marker_BASE_TAG.pose.position;
00843   marker_WRIST_TAG.scale = marker_BASE_TAG.scale;
00844   //marker color
00845   marker_WRIST_TAG.color = marker_BASE_TAG.color;
00846   marker_WRIST_TAG.id=0;
00847   marker_WRIST_TAG.lifetime = ros::Duration();
00848   marker_vector.push_back(marker_WRIST_TAG);
00849   //Tip frame
00850   visualization_msgs::Marker marker_TIP_TAG;
00851   marker_TIP_TAG.header.frame_id = "/phantom_pen_tip";
00852   //marker info
00853   marker_TIP_TAG.header.stamp = ros::Time::now();
00854   marker_TIP_TAG.ns = "text_tip_TAG";
00855   marker_TIP_TAG.action = visualization_msgs::Marker::ADD;
00856   //marker type
00857   marker_TIP_TAG.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00858   text = "/phantom_tip";
00859   marker_TIP_TAG.text = text;
00860   marker_TIP_TAG.pose.position = marker_BASE_TAG.pose.position;
00861   marker_TIP_TAG.scale = marker_BASE_TAG.scale;
00862   //marker color
00863   marker_TIP_TAG.color = marker_BASE_TAG.color;
00864   marker_TIP_TAG.id=0;
00865   marker_TIP_TAG.lifetime = ros::Duration();
00866   marker_vector.push_back(marker_TIP_TAG);
00867   
00868   if(RobotVars->haptics_data.chosen_demo == 1 && RobotVars->haptic_loop_start)
00869   {
00870     if((RobotVars->parameters.kinematic_model>=4 && RobotVars->parameters.kinematic_model<=5) && RobotVars->parameters.manual_speed_control)
00871     {
00872       // |||||||||||||||||||||||||||||||||||||||||||||||||||||| LEG WORKSPACE DEMO CONSTRUCTIONS ||||||||||||||||||||||||||||||||||||||||||||||||||||||
00873       //OUTER SPHERE
00874       visualization_msgs::Marker marker_outer_sphere;
00875       marker_outer_sphere.header.frame_id = "/phantom_world_corrected";
00876       //marker info
00877       marker_outer_sphere.header.stamp = ros::Time::now();
00878       marker_outer_sphere.ns = "outer_sphere";
00879       marker_outer_sphere.action = visualization_msgs::Marker::ADD;
00880       //marker type
00881       marker_outer_sphere.type = visualization_msgs::Marker::SPHERE;
00882       //marker scale
00883       marker_outer_sphere.scale.x = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
00884       marker_outer_sphere.scale.y = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
00885       marker_outer_sphere.scale.z = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
00886       //marker pose
00887       marker_outer_sphere.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0];
00888       marker_outer_sphere.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1];
00889       marker_outer_sphere.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2];
00890       marker_outer_sphere.pose.orientation.x = 0.0;
00891       marker_outer_sphere.pose.orientation.y = 0.0;
00892       marker_outer_sphere.pose.orientation.z = 0.0;
00893       marker_outer_sphere.pose.orientation.w = 1.0;
00894       
00895       marker_outer_sphere.color.r=0;
00896       marker_outer_sphere.color.g=1;
00897       marker_outer_sphere.color.b=0;
00898       marker_outer_sphere.color.a=0.2;
00899       marker_outer_sphere.id=0;
00900       marker_outer_sphere.lifetime = ros::Duration();
00901       
00902       marker_vector.push_back(marker_outer_sphere);
00903       
00904       //INNER SPHERE
00905       visualization_msgs::Marker marker_inner_sphere;
00906       marker_inner_sphere.header.frame_id = "/phantom_world_corrected";
00907       //marker info
00908       marker_inner_sphere.header.stamp = ros::Time::now();
00909       marker_inner_sphere.ns = "inner_sphere";
00910       marker_inner_sphere.action = visualization_msgs::Marker::ADD;
00911       //marker type
00912       marker_inner_sphere.type = visualization_msgs::Marker::SPHERE;
00913       //marker scale
00914       marker_inner_sphere.scale.x = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
00915       marker_inner_sphere.scale.y = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
00916       marker_inner_sphere.scale.z = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
00917       //marker pose
00918       marker_inner_sphere.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0];
00919       marker_inner_sphere.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1];
00920       marker_inner_sphere.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2];
00921       marker_inner_sphere.pose.orientation.x = 0.0;
00922       marker_inner_sphere.pose.orientation.y = 0.0;
00923       marker_inner_sphere.pose.orientation.z = 0.0;
00924       marker_inner_sphere.pose.orientation.w = 1.0;
00925       //marker color
00926       marker_inner_sphere.color.r=0;
00927       marker_inner_sphere.color.g=0;
00928       marker_inner_sphere.color.b=1;
00929       marker_inner_sphere.color.a=0.2;
00930       marker_inner_sphere.id=0;
00931       marker_inner_sphere.lifetime = ros::Duration();
00932       marker_vector.push_back(marker_inner_sphere);
00933       
00934       
00935       //FORCE VECTOR
00936       visualization_msgs::Marker marker_force_vector;
00937       marker_force_vector.header.frame_id = "/phantom_world_corrected";
00938       //marker info
00939       marker_force_vector.header.stamp = ros::Time::now();
00940       marker_force_vector.ns = "force_vector";
00941       marker_force_vector.action = visualization_msgs::Marker::ADD;
00942       //marker type
00943       marker_force_vector.type = visualization_msgs::Marker::ARROW;
00944       //marker points
00945       marker_force_vector.points.clear();
00946       p.x = RobotVars->phantom_data.m_devicePosition[0];
00947       p.y = RobotVars->phantom_data.m_devicePosition[1];
00948       p.z = RobotVars->phantom_data.m_devicePosition[2];
00949       marker_force_vector.points.push_back(p);
00950       
00951       hduVector3Dd Fdir = RobotVars->haptics_data.applied_force;
00952       if(Fdir.magnitude() > 3.3)
00953       {
00954         Fdir.normalize();
00955         Fdir = Fdir * 3.3;
00956       }
00957       p2.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
00958       p2.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
00959       p2.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
00960       marker_force_vector.points.push_back(p2);
00961       //marker scale
00962       marker_force_vector.scale.x = Fdir.magnitude()*5; 
00963       marker_force_vector.scale.y = Fdir.magnitude()*6;
00964       marker_force_vector.scale.z = Fdir.magnitude()*7;
00965       //marker color
00966       marker_force_vector.color.r=0;
00967       marker_force_vector.color.g=0;
00968       marker_force_vector.color.b=0;
00969       marker_force_vector.color.a=1;
00970       marker_force_vector.id=0;
00971       marker_force_vector.lifetime = ros::Duration();
00972       marker_vector.push_back(marker_force_vector);
00973       
00974       //vector magnitude
00975       visualization_msgs::Marker marker_force_magnitude;
00976       marker_force_magnitude.header.frame_id = "/phantom_world_corrected";
00977       //marker info
00978       marker_force_magnitude.header.stamp = ros::Time::now();
00979       marker_force_magnitude.ns = "text_force";
00980       marker_force_magnitude.action = visualization_msgs::Marker::ADD;
00981       //marker type
00982       marker_force_magnitude.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00983       text = str(boost::format("%3.1f") % AvoidMinusZero(Fdir.magnitude())) + " N";
00984       marker_force_magnitude.text = text;
00985       marker_force_magnitude.pose.position.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
00986       marker_force_magnitude.pose.position.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
00987       marker_force_magnitude.pose.position.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
00988       marker_force_magnitude.scale.z = 10.0; 
00989       //marker color
00990       marker_force_magnitude.color = marker_force_vector.color;
00991       marker_force_magnitude.color.a=0.8;
00992       marker_force_magnitude.id=0;
00993       marker_force_magnitude.lifetime = ros::Duration();
00994       marker_vector.push_back(marker_force_magnitude);   
00995     }
00996     else if((RobotVars->parameters.kinematic_model>=1 && RobotVars->parameters.kinematic_model<=3) && RobotVars->parameters.manual_speed_control)
00997     {
00998       // |||||||||||||||||||||||||||||||||||||||||||||||||||||| ARM WORKSPACE DEMO CONSTRUCTIONS ||||||||||||||||||||||||||||||||||||||||||||||||||||||
00999       //OUTER SPHERE
01000       visualization_msgs::Marker marker_outer_sphere;
01001       marker_outer_sphere.header.frame_id = "/phantom_world_corrected";
01002       //marker info
01003       marker_outer_sphere.header.stamp = ros::Time::now();
01004       marker_outer_sphere.ns = "outer_sphere";
01005       marker_outer_sphere.action = visualization_msgs::Marker::ADD;
01006       //marker type
01007       marker_outer_sphere.type = visualization_msgs::Marker::SPHERE;
01008       //marker scale
01009       marker_outer_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01010       marker_outer_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01011       marker_outer_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01012       //marker pose
01013       marker_outer_sphere.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01014       marker_outer_sphere.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01015       marker_outer_sphere.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01016       marker_outer_sphere.pose.orientation.x = 0.0;
01017       marker_outer_sphere.pose.orientation.y = 0.0;
01018       marker_outer_sphere.pose.orientation.z = 0.0;
01019       marker_outer_sphere.pose.orientation.w = 1.0;
01020       
01021       marker_outer_sphere.color.r=0;
01022       marker_outer_sphere.color.g=1;
01023       marker_outer_sphere.color.b=0;
01024       marker_outer_sphere.color.a=0.2;
01025       marker_outer_sphere.id=0;
01026       marker_outer_sphere.lifetime = ros::Duration();
01027       
01028       marker_vector.push_back(marker_outer_sphere);
01029       
01030       //INNER SPHERE
01031       visualization_msgs::Marker marker_inner_sphere;
01032       marker_inner_sphere.header.frame_id = "/phantom_world_corrected";
01033       //marker info
01034       marker_inner_sphere.header.stamp = ros::Time::now();
01035       marker_inner_sphere.ns = "inner_sphere";
01036       marker_inner_sphere.action = visualization_msgs::Marker::ADD;
01037       //marker type
01038       marker_inner_sphere.type = visualization_msgs::Marker::SPHERE;
01039       //marker scale
01040       marker_inner_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01041       marker_inner_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01042       marker_inner_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01043       //marker pose
01044       marker_inner_sphere.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01045       marker_inner_sphere.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01046       marker_inner_sphere.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01047       marker_inner_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01048       //marker color
01049       marker_inner_sphere.color.r=0;
01050       marker_inner_sphere.color.g=0;
01051       marker_inner_sphere.color.b=1;
01052       marker_inner_sphere.color.a=1;
01053       marker_inner_sphere.id=0;
01054       marker_inner_sphere.lifetime = ros::Duration();
01055       marker_vector.push_back(marker_inner_sphere);
01056       
01057       //INNER SPHERE
01058       visualization_msgs::Marker marker_back_sphere;
01059       marker_back_sphere.header.frame_id = "/phantom_world_corrected";
01060       //marker info
01061       marker_back_sphere.header.stamp = ros::Time::now();
01062       marker_back_sphere.ns = "back_sphere";
01063       marker_back_sphere.action = visualization_msgs::Marker::ADD;
01064       //marker type
01065       marker_back_sphere.type = visualization_msgs::Marker::SPHERE;
01066       //marker scale
01067       marker_back_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01068       marker_back_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01069       marker_back_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01070       //marker pose
01071       marker_back_sphere.pose.position.x = RobotVars->haptics_data.arm_back_spherePosition[0];
01072       marker_back_sphere.pose.position.y = RobotVars->haptics_data.arm_back_spherePosition[1];
01073       marker_back_sphere.pose.position.z = RobotVars->haptics_data.arm_back_spherePosition[2];
01074       marker_back_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01075       //marker color
01076       marker_back_sphere.color.r=0;
01077       marker_back_sphere.color.g=0;
01078       marker_back_sphere.color.b=1;
01079       marker_back_sphere.color.a=1;
01080       marker_back_sphere.id=0;
01081       marker_back_sphere.lifetime = ros::Duration();
01082       marker_vector.push_back(marker_back_sphere);
01083       
01084       //PLANE
01085       visualization_msgs::Marker marker_plane;
01086       marker_plane.header.frame_id = "/phantom_world_corrected";
01087       //marker info
01088       marker_plane.header.stamp = ros::Time::now();
01089       marker_plane.ns = "plane";
01090       marker_plane.action = visualization_msgs::Marker::ADD;
01091       //marker type
01092       marker_plane.type = visualization_msgs::Marker::CUBE;
01093       //marker scale
01094       marker_plane.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01095       marker_plane.scale.y = 0.01; 
01096       marker_plane.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01097       //marker pose
01098       marker_plane.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01099       marker_plane.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01100       marker_plane.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01101       marker_plane.pose.orientation = marker_outer_sphere.pose.orientation;
01102       //marker color
01103       marker_plane.color.r=1;
01104       marker_plane.color.g=1;
01105       marker_plane.color.b=0;
01106       marker_plane.color.a=0.8;
01107       marker_plane.id=0;
01108       marker_plane.lifetime = ros::Duration();
01109       marker_vector.push_back(marker_plane);
01110       
01111       //FORCE VECTOR
01112       visualization_msgs::Marker marker_force_vector;
01113       marker_force_vector.header.frame_id = "/phantom_world_corrected";
01114       //marker info
01115       marker_force_vector.header.stamp = ros::Time::now();
01116       marker_force_vector.ns = "force_vector";
01117       marker_force_vector.action = visualization_msgs::Marker::ADD;
01118       //marker type
01119       marker_force_vector.type = visualization_msgs::Marker::ARROW;
01120       //marker points
01121       marker_force_vector.points.clear();
01122       p.x = RobotVars->phantom_data.m_devicePosition[0];
01123       p.y = RobotVars->phantom_data.m_devicePosition[1];
01124       p.z = RobotVars->phantom_data.m_devicePosition[2];
01125       marker_force_vector.points.push_back(p);
01126       
01127       hduVector3Dd Fdir = RobotVars->haptics_data.applied_force;
01128       if(Fdir.magnitude() > 3.3)
01129       {
01130         Fdir.normalize();
01131         Fdir = Fdir * 3.3;
01132       }
01133       p2.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01134       p2.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01135       p2.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01136       marker_force_vector.points.push_back(p2);
01137       //marker scale
01138       marker_force_vector.scale.x = Fdir.magnitude()*5; 
01139       marker_force_vector.scale.y = Fdir.magnitude()*6;
01140       marker_force_vector.scale.z = Fdir.magnitude()*7;
01141       //marker color
01142       marker_force_vector.color.r=0;
01143       marker_force_vector.color.g=0;
01144       marker_force_vector.color.b=0;
01145       marker_force_vector.color.a=1;
01146       marker_force_vector.id=0;
01147       marker_force_vector.lifetime = ros::Duration();
01148       marker_vector.push_back(marker_force_vector);
01149       
01150       //vector magnitude
01151       visualization_msgs::Marker marker_force_magnitude;
01152       marker_force_magnitude.header.frame_id = "/phantom_world_corrected";
01153       //marker info
01154       marker_force_magnitude.header.stamp = ros::Time::now();
01155       marker_force_magnitude.ns = "text_force";
01156       marker_force_magnitude.action = visualization_msgs::Marker::ADD;
01157       //marker type
01158       marker_force_magnitude.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01159       text = str(boost::format("%3.1f") % AvoidMinusZero(Fdir.magnitude())) + " N";
01160       marker_force_magnitude.text = text;
01161       marker_force_magnitude.pose.position.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01162       marker_force_magnitude.pose.position.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01163       marker_force_magnitude.pose.position.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01164       marker_force_magnitude.scale.z = 10.0; 
01165       //marker color
01166       marker_force_magnitude.color = marker_force_vector.color;
01167       marker_force_magnitude.color.a=0.8;
01168       marker_force_magnitude.id=0;
01169       marker_force_magnitude.lifetime = ros::Duration();
01170       marker_vector.push_back(marker_force_magnitude);
01171     }
01172   }
01173   else if(RobotVars->haptics_data.chosen_demo == 2 && RobotVars->haptic_loop_start)
01174   {
01175     if((RobotVars->parameters.kinematic_model>=1 && RobotVars->parameters.kinematic_model<=3) && RobotVars->parameters.manual_speed_control)
01176     {
01177       // |||||||||||||||||||||||||||||||||||||||||||||||||||||| PLANE DRAWING DEMO CONSTRUCTIONS ||||||||||||||||||||||||||||||||||||||||||||||||||||||
01178       
01179       // ******************************* workspace information *******************************
01180       //OUTER SPHERE
01181       visualization_msgs::Marker marker_outer_sphere;
01182       marker_outer_sphere.header.frame_id = "/phantom_world_corrected";
01183       //marker info
01184       marker_outer_sphere.header.stamp = ros::Time::now();
01185       marker_outer_sphere.ns = "outer_sphere";
01186       marker_outer_sphere.action = visualization_msgs::Marker::ADD;
01187       //marker type
01188       marker_outer_sphere.type = visualization_msgs::Marker::SPHERE;
01189       //marker scale
01190       marker_outer_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01191       marker_outer_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01192       marker_outer_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01193       //marker pose
01194       marker_outer_sphere.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01195       marker_outer_sphere.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01196       marker_outer_sphere.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01197       marker_outer_sphere.pose.orientation.x = 0.0;
01198       marker_outer_sphere.pose.orientation.y = 0.0;
01199       marker_outer_sphere.pose.orientation.z = 0.0;
01200       marker_outer_sphere.pose.orientation.w = 1.0;
01201       
01202       marker_outer_sphere.color.r=0;
01203       marker_outer_sphere.color.g=1;
01204       marker_outer_sphere.color.b=0;
01205       marker_outer_sphere.color.a=0.2;
01206       marker_outer_sphere.id=0;
01207       marker_outer_sphere.lifetime = ros::Duration();
01208       
01209       marker_vector.push_back(marker_outer_sphere);
01210       
01211       //INNER SPHERE
01212       visualization_msgs::Marker marker_inner_sphere;
01213       marker_inner_sphere.header.frame_id = "/phantom_world_corrected";
01214       //marker info
01215       marker_inner_sphere.header.stamp = ros::Time::now();
01216       marker_inner_sphere.ns = "inner_sphere";
01217       marker_inner_sphere.action = visualization_msgs::Marker::ADD;
01218       //marker type
01219       marker_inner_sphere.type = visualization_msgs::Marker::SPHERE;
01220       //marker scale
01221       marker_inner_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01222       marker_inner_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01223       marker_inner_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01224       //marker pose
01225       marker_inner_sphere.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01226       marker_inner_sphere.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01227       marker_inner_sphere.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01228       marker_inner_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01229       //marker color
01230       marker_inner_sphere.color.r=0;
01231       marker_inner_sphere.color.g=0;
01232       marker_inner_sphere.color.b=1;
01233       marker_inner_sphere.color.a=1;
01234       marker_inner_sphere.id=0;
01235       marker_inner_sphere.lifetime = ros::Duration();
01236       marker_vector.push_back(marker_inner_sphere);
01237       
01238       //INNER SPHERE
01239       visualization_msgs::Marker marker_back_sphere;
01240       marker_back_sphere.header.frame_id = "/phantom_world_corrected";
01241       //marker info
01242       marker_back_sphere.header.stamp = ros::Time::now();
01243       marker_back_sphere.ns = "back_sphere";
01244       marker_back_sphere.action = visualization_msgs::Marker::ADD;
01245       //marker type
01246       marker_back_sphere.type = visualization_msgs::Marker::SPHERE;
01247       //marker scale
01248       marker_back_sphere.scale.x = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01249       marker_back_sphere.scale.y = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01250       marker_back_sphere.scale.z = 2.0*RobotVars->haptics_data.arm_back_sphereRadius; 
01251       //marker pose
01252       marker_back_sphere.pose.position.x = RobotVars->haptics_data.arm_back_spherePosition[0];
01253       marker_back_sphere.pose.position.y = RobotVars->haptics_data.arm_back_spherePosition[1];
01254       marker_back_sphere.pose.position.z = RobotVars->haptics_data.arm_back_spherePosition[2];
01255       marker_back_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01256       //marker color
01257       marker_back_sphere.color.r=0;
01258       marker_back_sphere.color.g=0;
01259       marker_back_sphere.color.b=1;
01260       marker_back_sphere.color.a=1;
01261       marker_back_sphere.id=0;
01262       marker_back_sphere.lifetime = ros::Duration();
01263       marker_vector.push_back(marker_back_sphere);
01264       
01265       //PLANE
01266       visualization_msgs::Marker marker_plane;
01267       marker_plane.header.frame_id = "/phantom_world_corrected";
01268       //marker info
01269       marker_plane.header.stamp = ros::Time::now();
01270       marker_plane.ns = "plane";
01271       marker_plane.action = visualization_msgs::Marker::ADD;
01272       //marker type
01273       marker_plane.type = visualization_msgs::Marker::CUBE;
01274       //marker scale
01275       marker_plane.scale.x = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01276       marker_plane.scale.y = 0.01; 
01277       marker_plane.scale.z = 2.0*RobotVars->haptics_data.arm_main_sphere_max_Radius; 
01278       //marker pose
01279       marker_plane.pose.position.x = RobotVars->haptics_data.arm_main_spheresPosition[0];
01280       marker_plane.pose.position.y = RobotVars->haptics_data.arm_main_spheresPosition[1];
01281       marker_plane.pose.position.z = RobotVars->haptics_data.arm_main_spheresPosition[2];
01282       marker_plane.pose.orientation = marker_outer_sphere.pose.orientation;
01283       //marker color
01284       marker_plane.color.r=1;
01285       marker_plane.color.g=1;
01286       marker_plane.color.b=0;
01287       marker_plane.color.a=0.8;
01288       marker_plane.id=0;
01289       marker_plane.lifetime = ros::Duration();
01290       marker_vector.push_back(marker_plane);
01291       
01292       //FORCE VECTOR
01293       visualization_msgs::Marker marker_force_vector;
01294       marker_force_vector.header.frame_id = "/phantom_world_corrected";
01295       //marker info
01296       marker_force_vector.header.stamp = ros::Time::now();
01297       marker_force_vector.ns = "force_vector";
01298       marker_force_vector.action = visualization_msgs::Marker::ADD;
01299       //marker type
01300       marker_force_vector.type = visualization_msgs::Marker::ARROW;
01301       //marker points
01302       marker_force_vector.points.clear();
01303       p.x = RobotVars->phantom_data.m_devicePosition[0];
01304       p.y = RobotVars->phantom_data.m_devicePosition[1];
01305       p.z = RobotVars->phantom_data.m_devicePosition[2];
01306       marker_force_vector.points.push_back(p);
01307       
01308       hduVector3Dd Fdir = RobotVars->haptics_data.applied_force;
01309       if(Fdir.magnitude() > 3.3)
01310       {
01311         Fdir.normalize();
01312         Fdir = Fdir * 3.3;
01313       }
01314       p2.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01315       p2.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01316       p2.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01317       marker_force_vector.points.push_back(p2);
01318       //marker scale
01319       marker_force_vector.scale.x = Fdir.magnitude()*5; 
01320       marker_force_vector.scale.y = Fdir.magnitude()*6;
01321       marker_force_vector.scale.z = Fdir.magnitude()*7;
01322       //marker color
01323       marker_force_vector.color.r=0;
01324       marker_force_vector.color.g=0;
01325       marker_force_vector.color.b=0;
01326       marker_force_vector.color.a=1;
01327       marker_force_vector.id=0;
01328       marker_force_vector.lifetime = ros::Duration();
01329       marker_vector.push_back(marker_force_vector);
01330       
01331       //vector magnitude
01332       visualization_msgs::Marker marker_force_magnitude;
01333       marker_force_magnitude.header.frame_id = "/phantom_world_corrected";
01334       //marker info
01335       marker_force_magnitude.header.stamp = ros::Time::now();
01336       marker_force_magnitude.ns = "text_force";
01337       marker_force_magnitude.action = visualization_msgs::Marker::ADD;
01338       //marker type
01339       marker_force_magnitude.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01340       text = str(boost::format("%3.1f") % AvoidMinusZero(Fdir.magnitude())) + " N";
01341       marker_force_magnitude.text = text;
01342       marker_force_magnitude.pose.position.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01343       marker_force_magnitude.pose.position.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01344       marker_force_magnitude.pose.position.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01345       marker_force_magnitude.scale.z = 10.0; 
01346       //marker color
01347       marker_force_magnitude.color = marker_force_vector.color;
01348       marker_force_magnitude.color.a=0.8;
01349       marker_force_magnitude.id=0;
01350       marker_force_magnitude.lifetime = ros::Duration();
01351       marker_vector.push_back(marker_force_magnitude);
01352       // *************************************************************************************
01353       // PLANE AND POINTS
01354       //point1
01355       if(RobotVars->haptics_data.demo_2_point_1.magnitude() > 0.0)
01356       {
01357         visualization_msgs::Marker marker_point_sphere;
01358         marker_point_sphere.header.frame_id = "/phantom_world_corrected";
01359         //marker info
01360         marker_point_sphere.header.stamp = ros::Time::now();
01361         marker_point_sphere.ns = "point_1";
01362         marker_point_sphere.action = visualization_msgs::Marker::ADD;
01363         //marker type
01364         marker_point_sphere.type = visualization_msgs::Marker::SPHERE;
01365         //marker scale
01366         marker_point_sphere.scale.x = 0.1*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01367         marker_point_sphere.scale.y = marker_point_sphere.scale.x; 
01368         marker_point_sphere.scale.z = marker_point_sphere.scale.x; 
01369         //marker pose
01370         marker_point_sphere.pose.position.x = RobotVars->haptics_data.demo_2_point_1[0];
01371         marker_point_sphere.pose.position.y = RobotVars->haptics_data.demo_2_point_1[1];
01372         marker_point_sphere.pose.position.z = RobotVars->haptics_data.demo_2_point_1[2];
01373         marker_point_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01374         //marker color
01375         marker_point_sphere.color.r=153.0/255.0;
01376         marker_point_sphere.color.g=51.0/255.0;
01377         marker_point_sphere.color.b=51.0/255.0;
01378         marker_point_sphere.color.a=1;
01379         marker_point_sphere.id=0;
01380         marker_point_sphere.lifetime = ros::Duration();
01381         marker_vector.push_back(marker_point_sphere);
01382         //text
01383         visualization_msgs::Marker marker_point_text;
01384         marker_point_text.header.frame_id = "/phantom_world_corrected";
01385         //marker info
01386         marker_point_text.header.stamp = ros::Time::now();
01387         marker_point_text.ns = "point_1_text";
01388         marker_point_text.action = visualization_msgs::Marker::ADD;
01389         //marker type
01390         marker_point_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01391         text = "Point 1";
01392         marker_point_text.text = text;
01393         marker_point_text.pose.position = marker_point_sphere.pose.position;
01394         marker_point_text.scale.z = 7.0;
01395         //marker color
01396         marker_point_text.color = marker_point_sphere.color;
01397         marker_point_text.color.a=1;
01398         marker_point_text.id=0;
01399         marker_point_text.lifetime = ros::Duration();
01400         marker_vector.push_back(marker_point_text);
01401       }
01402       //point2
01403       if(RobotVars->haptics_data.demo_2_point_2.magnitude() > 0.0)
01404       {
01405         visualization_msgs::Marker marker_point2_sphere;
01406         marker_point2_sphere.header.frame_id = "/phantom_world_corrected";
01407         //marker info
01408         marker_point2_sphere.header.stamp = ros::Time::now();
01409         marker_point2_sphere.ns = "point_2";
01410         marker_point2_sphere.action = visualization_msgs::Marker::ADD;
01411         //marker type
01412         marker_point2_sphere.type = visualization_msgs::Marker::SPHERE;
01413         //marker scale
01414         marker_point2_sphere.scale.x = 0.1*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01415         marker_point2_sphere.scale.y = marker_point2_sphere.scale.x; 
01416         marker_point2_sphere.scale.z = marker_point2_sphere.scale.x; 
01417         //marker pose
01418         marker_point2_sphere.pose.position.x = RobotVars->haptics_data.demo_2_point_2[0];
01419         marker_point2_sphere.pose.position.y = RobotVars->haptics_data.demo_2_point_2[1];
01420         marker_point2_sphere.pose.position.z = RobotVars->haptics_data.demo_2_point_2[2];
01421         marker_point2_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01422         //marker color
01423         marker_point2_sphere.color.r=153.0/255.0;
01424         marker_point2_sphere.color.g=51.0/255.0;
01425         marker_point2_sphere.color.b=51.0/255.0;
01426         marker_point2_sphere.color.a=1;
01427         marker_point2_sphere.id=0;
01428         marker_point2_sphere.lifetime = ros::Duration();
01429         marker_vector.push_back(marker_point2_sphere);
01430         //text
01431         visualization_msgs::Marker marker_point2_text;
01432         marker_point2_text.header.frame_id = "/phantom_world_corrected";
01433         //marker info
01434         marker_point2_text.header.stamp = ros::Time::now();
01435         marker_point2_text.ns = "point_2_text";
01436         marker_point2_text.action = visualization_msgs::Marker::ADD;
01437         //marker type
01438         marker_point2_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01439         text = "Point 2";
01440         marker_point2_text.text = text;
01441         marker_point2_text.pose.position = marker_point2_sphere.pose.position;
01442         marker_point2_text.scale.z = 7.0;
01443         //marker color
01444         marker_point2_text.color = marker_point2_sphere.color;
01445         marker_point2_text.color.a=1;
01446         marker_point2_text.id=0;
01447         marker_point2_text.lifetime = ros::Duration();
01448         marker_vector.push_back(marker_point2_text);
01449       }
01450       //point3
01451       if(RobotVars->haptics_data.demo_2_point_3.magnitude() > 0.0)
01452       {
01453         visualization_msgs::Marker marker_point3_sphere;
01454         marker_point3_sphere.header.frame_id = "/phantom_world_corrected";
01455         //marker info
01456         marker_point3_sphere.header.stamp = ros::Time::now();
01457         marker_point3_sphere.ns = "point_3";
01458         marker_point3_sphere.action = visualization_msgs::Marker::ADD;
01459         //marker type
01460         marker_point3_sphere.type = visualization_msgs::Marker::SPHERE;
01461         //marker scale
01462         marker_point3_sphere.scale.x = 0.1*RobotVars->haptics_data.arm_main_sphere_min_Radius; 
01463         marker_point3_sphere.scale.y = marker_point3_sphere.scale.x; 
01464         marker_point3_sphere.scale.z = marker_point3_sphere.scale.x; 
01465         //marker pose
01466         marker_point3_sphere.pose.position.x = RobotVars->haptics_data.demo_2_point_3[0];
01467         marker_point3_sphere.pose.position.y = RobotVars->haptics_data.demo_2_point_3[1];
01468         marker_point3_sphere.pose.position.z = RobotVars->haptics_data.demo_2_point_3[2];
01469         marker_point3_sphere.pose.orientation = marker_outer_sphere.pose.orientation;
01470         //marker color
01471         marker_point3_sphere.color.r=153.0/255.0;
01472         marker_point3_sphere.color.g=51.0/255.0;
01473         marker_point3_sphere.color.b=51.0/255.0;
01474         marker_point3_sphere.color.a=1;
01475         marker_point3_sphere.id=0;
01476         marker_point3_sphere.lifetime = ros::Duration();
01477         marker_vector.push_back(marker_point3_sphere);
01478         //text
01479         visualization_msgs::Marker marker_point_text;
01480         marker_point_text.header.frame_id = "/phantom_world_corrected";
01481         //marker info
01482         marker_point_text.header.stamp = ros::Time::now();
01483         marker_point_text.ns = "point_3_text";
01484         marker_point_text.action = visualization_msgs::Marker::ADD;
01485         //marker type
01486         marker_point_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01487         text = "Point 3";
01488         marker_point_text.text = text;
01489         marker_point_text.pose.position = marker_point3_sphere.pose.position;
01490         marker_point_text.scale.z = 7.0;
01491         //marker color
01492         marker_point_text.color = marker_point3_sphere.color;
01493         marker_point_text.color.a=1;
01494         marker_point_text.id=0;
01495         marker_point_text.lifetime = ros::Duration();
01496         marker_vector.push_back(marker_point_text);
01497       }
01498       hduVector3Dd normalLocal = RobotVars->haptics_data.demo_2_Plane.normal();
01499       //plane
01500       if(normalLocal.magnitude() > 0.0)
01501       {
01502         hduVector3Dd a = normalLocal;
01503         hduVector3Dd n = RobotVars->haptics_data.demo_2_point_2 - ((RobotVars->haptics_data.demo_2_point_1 + RobotVars->haptics_data.demo_2_point_2 + RobotVars->haptics_data.demo_2_point_3) / 3.0);
01504         hduVector3Dd s = crossProduct(a,n);
01505         
01506         n.normalize();
01507         s.normalize();
01508         a.normalize();
01509 
01510 #if ROS_VERSION_MINIMUM(1, 8, 0)   //At least FUERTE, V. Santos, 27-Mai-2013
01511         tf::Quaternion plane_quat;
01512         tf::Matrix3x3 mat = tf::Matrix3x3(n[0], s[0], a[0],
01513                           n[1], s[1], a[1],
01514                           n[2], s[2], a[2]);
01515 #else
01516         btQuaternion plane_quat;
01517         btMatrix3x3 mat = btMatrix3x3(n[0], s[0], a[0],
01518                           n[1], s[1], a[1],
01519                           n[2], s[2], a[2]);
01520 #endif
01521         mat.getRotation(plane_quat);
01522 
01523         //PLANE
01524         visualization_msgs::Marker marker_plane;
01525         marker_plane.header.frame_id = "/phantom_world_corrected";
01526         //marker info
01527         marker_plane.header.stamp = ros::Time::now();
01528         marker_plane.ns = "drawing_plane";
01529         marker_plane.action = visualization_msgs::Marker::ADD;
01530         //marker type
01531         marker_plane.type = visualization_msgs::Marker::CUBE;
01532         //marker scale
01533         marker_plane.scale.x = 200;
01534         marker_plane.scale.y = 200;
01535         marker_plane.scale.z = 0.01;
01536         
01537         //marker pose
01538         marker_plane.pose.position.x = (RobotVars->haptics_data.demo_2_point_1[0] + RobotVars->haptics_data.demo_2_point_2[0] + RobotVars->haptics_data.demo_2_point_3[0]) / 3.0;
01539         marker_plane.pose.position.y = (RobotVars->haptics_data.demo_2_point_1[1] + RobotVars->haptics_data.demo_2_point_2[1] + RobotVars->haptics_data.demo_2_point_3[1]) / 3.0;
01540         marker_plane.pose.position.z = (RobotVars->haptics_data.demo_2_point_1[2] + RobotVars->haptics_data.demo_2_point_2[2] + RobotVars->haptics_data.demo_2_point_3[2]) / 3.0;
01541         
01542         tf::quaternionTFToMsg(plane_quat.normalize(),
01543                               marker_plane.pose.orientation);           
01544         
01545         //marker color
01546         marker_plane.color.r=0;
01547         marker_plane.color.g=245.0/255.0;
01548         marker_plane.color.b=255.0/255.0;
01549         marker_plane.color.a=0.8;
01550         marker_plane.id=0;
01551         marker_plane.lifetime = ros::Duration();
01552         marker_vector.push_back(marker_plane);
01553 /*      
01554         visualization_msgs::Marker marker_n_vector;
01555         marker_n_vector.header.frame_id = "/phantom_world_corrected";
01556         //marker info
01557         marker_n_vector.header.stamp = ros::Time::now();
01558         marker_n_vector.ns = "n_vector";
01559         marker_n_vector.action = visualization_msgs::Marker::ADD;
01560         //marker type
01561         marker_n_vector.type = visualization_msgs::Marker::ARROW;
01562         //marker points
01563         marker_n_vector.points.clear();
01564         p = marker_plane.pose.position;
01565         marker_n_vector.points.push_back(p);
01566         p2.x = p.x + normalLocal[0]/10.0;
01567         p2.y = p.y + normalLocal[1]/10.0;
01568         p2.z = p.z + normalLocal[2]/10.0;
01569         marker_n_vector.points.push_back(p2);
01570         //marker scale
01571         marker_n_vector.scale.x = 3; 
01572         marker_n_vector.scale.y = 4;
01573         marker_n_vector.scale.z = 3;
01574         //marker color
01575         marker_n_vector.color.r=0;
01576         marker_n_vector.color.g=0;
01577         marker_n_vector.color.b=0;
01578         marker_n_vector.color.a=1;
01579         marker_n_vector.id=0;
01580         marker_n_vector.lifetime = ros::Duration();
01581         marker_vector.push_back(marker_n_vector);*/
01582       }
01583     }
01584   }
01585   else if(RobotVars->haptics_data.chosen_demo == 4 && RobotVars->haptic_loop_start)
01586   {
01587     // |||||||||||||||||||||||||||||||||||||||||||||||||||||| LEG WORKSPACE DEMO CONSTRUCTIONS ||||||||||||||||||||||||||||||||||||||||||||||||||||||
01588     //OUTER SPHERE
01589     visualization_msgs::Marker marker_outer_sphere;
01590     marker_outer_sphere.header.frame_id = "/phantom_world_corrected";
01591     //marker info
01592     marker_outer_sphere.header.stamp = ros::Time::now();
01593     marker_outer_sphere.ns = "outer_sphere";
01594     marker_outer_sphere.action = visualization_msgs::Marker::ADD;
01595     //marker type
01596     marker_outer_sphere.type = visualization_msgs::Marker::SPHERE;
01597     //marker scale
01598     marker_outer_sphere.scale.x = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
01599     marker_outer_sphere.scale.y = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
01600     marker_outer_sphere.scale.z = 2.0*RobotVars->haptics_data.leg_main_sphere_max_Radius; 
01601     //marker pose
01602     marker_outer_sphere.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0];
01603     marker_outer_sphere.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1];
01604     marker_outer_sphere.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2];
01605     marker_outer_sphere.pose.orientation.x = 0.0;
01606     marker_outer_sphere.pose.orientation.y = 0.0;
01607     marker_outer_sphere.pose.orientation.z = 0.0;
01608     marker_outer_sphere.pose.orientation.w = 1.0;
01609     
01610     marker_outer_sphere.color.r=0;
01611     marker_outer_sphere.color.g=1;
01612     marker_outer_sphere.color.b=0;
01613     marker_outer_sphere.color.a=0.1;
01614     marker_outer_sphere.id=0;
01615     marker_outer_sphere.lifetime = ros::Duration();
01616     
01617     marker_vector.push_back(marker_outer_sphere);
01618     
01619     //INNER SPHERE
01620     visualization_msgs::Marker marker_inner_sphere;
01621     marker_inner_sphere.header.frame_id = "/phantom_world_corrected";
01622     //marker info
01623     marker_inner_sphere.header.stamp = ros::Time::now();
01624     marker_inner_sphere.ns = "inner_sphere";
01625     marker_inner_sphere.action = visualization_msgs::Marker::ADD;
01626     //marker type
01627     marker_inner_sphere.type = visualization_msgs::Marker::SPHERE;
01628     //marker scale
01629     marker_inner_sphere.scale.x = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
01630     marker_inner_sphere.scale.y = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
01631     marker_inner_sphere.scale.z = 2.0*RobotVars->haptics_data.leg_main_sphere_min_Radius; 
01632     //marker pose
01633     marker_inner_sphere.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0];
01634     marker_inner_sphere.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1];
01635     marker_inner_sphere.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2];
01636     marker_inner_sphere.pose.orientation.x = 0.0;
01637     marker_inner_sphere.pose.orientation.y = 0.0;
01638     marker_inner_sphere.pose.orientation.z = 0.0;
01639     marker_inner_sphere.pose.orientation.w = 1.0;
01640     //marker color
01641     marker_inner_sphere.color.r=0;
01642     marker_inner_sphere.color.g=0;
01643     marker_inner_sphere.color.b=1;
01644     marker_inner_sphere.color.a=0.15;
01645     marker_inner_sphere.id=0;
01646     marker_inner_sphere.lifetime = ros::Duration();
01647     marker_vector.push_back(marker_inner_sphere);
01648     
01649     // |||||||||||||||||||||||||||||||||||||||||||||||||||||| LEG BALANCING DEMO CONSTRUCTIONS ||||||||||||||||||||||||||||||||||||||||||||||||||||||
01650     //FORCE VECTOR
01651     visualization_msgs::Marker marker_force_vector;
01652     marker_force_vector.header.frame_id = "/phantom_world_corrected";
01653     //marker info
01654     marker_force_vector.header.stamp = ros::Time::now();
01655     marker_force_vector.ns = "force_vector";
01656     marker_force_vector.action = visualization_msgs::Marker::ADD;
01657     //marker type
01658     marker_force_vector.type = visualization_msgs::Marker::ARROW;
01659     //marker points
01660     marker_force_vector.points.clear();
01661     p.x = RobotVars->phantom_data.m_devicePosition[0];
01662     p.y = RobotVars->phantom_data.m_devicePosition[1];
01663     p.z = RobotVars->phantom_data.m_devicePosition[2];
01664     marker_force_vector.points.push_back(p);
01665     
01666     hduVector3Dd Fdir = RobotVars->haptics_data.applied_force;
01667     if(Fdir.magnitude() > 3.3)
01668     {
01669       Fdir.normalize();
01670       Fdir = Fdir * 3.3;
01671     }
01672     p2.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01673     p2.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01674     p2.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01675     marker_force_vector.points.push_back(p2);
01676     //marker scale
01677     marker_force_vector.scale.x = Fdir.magnitude()*5; 
01678     marker_force_vector.scale.y = Fdir.magnitude()*6;
01679     marker_force_vector.scale.z = Fdir.magnitude()*7;
01680     //marker color
01681     marker_force_vector.color.r=0;
01682     marker_force_vector.color.g=0;
01683     marker_force_vector.color.b=0;
01684     marker_force_vector.color.a=1;
01685     marker_force_vector.id=0;
01686     marker_force_vector.lifetime = ros::Duration();
01687     marker_vector.push_back(marker_force_vector);
01688     
01689     //vector magnitude
01690     visualization_msgs::Marker marker_force_magnitude;
01691     marker_force_magnitude.header.frame_id = "/phantom_world_corrected";
01692     //marker info
01693     marker_force_magnitude.header.stamp = ros::Time::now();
01694     marker_force_magnitude.ns = "text_force";
01695     marker_force_magnitude.action = visualization_msgs::Marker::ADD;
01696     //marker type
01697     marker_force_magnitude.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01698     text = str(boost::format("%3.1f") % AvoidMinusZero(Fdir.magnitude())) + " N";
01699     marker_force_magnitude.text = text;
01700     marker_force_magnitude.pose.position.x = RobotVars->phantom_data.m_devicePosition[0] + Fdir[0]*10;
01701     marker_force_magnitude.pose.position.y = RobotVars->phantom_data.m_devicePosition[1] + Fdir[1]*10;
01702     marker_force_magnitude.pose.position.z = RobotVars->phantom_data.m_devicePosition[2] + Fdir[2]*10;
01703     marker_force_magnitude.scale.z = 10.0; 
01704     //marker color
01705     marker_force_magnitude.color = marker_force_vector.color;
01706     marker_force_magnitude.color.a=0.9;
01707     marker_force_magnitude.id=0;
01708     marker_force_magnitude.lifetime = ros::Duration();
01709     marker_vector.push_back(marker_force_magnitude);
01710     
01711     
01712     //Center of Gravity
01713     visualization_msgs::Marker marker_COG;
01714     marker_COG.header.frame_id = "/phantom_world_corrected";
01715     //marker info
01716     marker_COG.header.stamp = ros::Time::now();
01717     marker_COG.ns = "COG_sphere";
01718     marker_COG.action = visualization_msgs::Marker::ADD;
01719     //marker type
01720     marker_COG.type = visualization_msgs::Marker::SPHERE;
01721     //marker scale
01722     marker_COG.scale.x = 10.0; 
01723     marker_COG.scale.y = marker_COG.scale.x; 
01724     marker_COG.scale.z = marker_COG.scale.x; 
01725     //marker pose
01726     if(RobotVars->parameters.kinematic_model==4)
01727     {  
01728       marker_COG.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0] + RobotVars->robot_kin_data.COG_detached_leg_left[0];
01729       marker_COG.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1] + RobotVars->robot_kin_data.COG_detached_leg_left[1];
01730       marker_COG.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2] + RobotVars->robot_kin_data.COG_detached_leg_left[2];
01731     }
01732     else
01733     {   
01734       marker_COG.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0] + RobotVars->robot_kin_data.COG_detached_leg_right[0];
01735       marker_COG.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1] + RobotVars->robot_kin_data.COG_detached_leg_right[1];
01736       marker_COG.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2] + RobotVars->robot_kin_data.COG_detached_leg_right[2];
01737     }
01738     marker_COG.pose.orientation.x = 0.0;
01739     marker_COG.pose.orientation.y = marker_COG.pose.orientation.x;
01740     marker_COG.pose.orientation.z = marker_COG.pose.orientation.x;
01741     marker_COG.pose.orientation.w = 1.0;
01742     //marker color
01743     marker_COG.color.r=1;
01744     marker_COG.color.g=1;
01745     marker_COG.color.b=0;
01746     marker_COG.color.a=0.9;
01747     marker_COG.id=0;
01748     marker_COG.lifetime = ros::Duration();
01749     marker_vector.push_back(marker_COG);
01750     
01751     //cog_tag
01752     visualization_msgs::Marker marker_cog_tag;
01753     marker_cog_tag.header.frame_id = "/phantom_world_corrected";
01754     //marker info
01755     marker_cog_tag.header.stamp = ros::Time::now();
01756     marker_cog_tag.ns = "cog_tag";
01757     marker_cog_tag.action = visualization_msgs::Marker::ADD;
01758     //marker type
01759     marker_cog_tag.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01760     text = "COG";
01761     marker_cog_tag.text = text;
01762     marker_cog_tag.pose.position = marker_COG.pose.position;
01763     marker_cog_tag.scale.z = 7.0; 
01764     //marker color
01765     marker_cog_tag.color.r = 0;
01766     marker_cog_tag.color.g = 0;
01767     marker_cog_tag.color.b = 0;
01768     marker_cog_tag.color.a=1.0;
01769     marker_cog_tag.id=0;
01770     marker_cog_tag.lifetime = ros::Duration();
01771     marker_vector.push_back(marker_cog_tag);
01772     
01773     //Foot AREA PLANE
01774     visualization_msgs::Marker marker_foot_area_plane;
01775     marker_foot_area_plane.header.frame_id = "/phantom_world_corrected";
01776     //marker info
01777     marker_foot_area_plane.header.stamp = ros::Time::now();
01778     marker_foot_area_plane.ns = "foot_area_plane";
01779     marker_foot_area_plane.action = visualization_msgs::Marker::ADD;
01780     //marker type
01781     marker_foot_area_plane.type = visualization_msgs::Marker::CUBE;
01782     //marker scale
01783     marker_foot_area_plane.scale.x = RobotVars->humanoid_f->GetRobotDimension(FOOT_LENGTH)/ (double)RobotVars->parameters.pos_coord_scale; 
01784     marker_foot_area_plane.scale.y = RobotVars->humanoid_f->GetRobotDimension(FOOT_WIDTH)/ (double)RobotVars->parameters.pos_coord_scale; 
01785     marker_foot_area_plane.scale.z = 0.01;
01786     //marker pose    
01787     marker_foot_area_plane.pose.position.x = RobotVars->haptics_data.leg_main_spheresPosition[0];
01788     marker_foot_area_plane.pose.position.y = RobotVars->haptics_data.leg_main_spheresPosition[1];
01789     marker_foot_area_plane.pose.position.z = RobotVars->haptics_data.leg_main_spheresPosition[2]- (RobotVars->humanoid_f->GetRobotDimension(ANKLE_HEIGHT)/ (double)RobotVars->parameters.pos_coord_scale);
01790     marker_foot_area_plane.pose.orientation = marker_COG.pose.orientation;
01791     //marker color
01792     marker_foot_area_plane.color.r=70.0/255.0;
01793     marker_foot_area_plane.color.g=130.0/255.0;
01794     marker_foot_area_plane.color.b=180.0/255.0;
01795     marker_foot_area_plane.color.a=0.75;
01796     marker_foot_area_plane.id=0;
01797     marker_foot_area_plane.lifetime = ros::Duration();
01798     marker_vector.push_back(marker_foot_area_plane);
01799     
01800     // plane tag
01801     visualization_msgs::Marker marker_plane_tag;
01802     marker_plane_tag.header.frame_id = "/phantom_world_corrected";
01803     //marker info
01804     marker_plane_tag.header.stamp = ros::Time::now();
01805     marker_plane_tag.ns = "foot_plane_tag";
01806     marker_plane_tag.action = visualization_msgs::Marker::ADD;
01807     //marker type
01808     marker_plane_tag.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01809     text = "Foot Area";
01810     marker_plane_tag.text = text;
01811     marker_plane_tag.pose.position.x = marker_foot_area_plane.pose.position.x + marker_foot_area_plane.scale.x/2.0;
01812     marker_plane_tag.pose.position.y = marker_foot_area_plane.pose.position.y + marker_foot_area_plane.scale.y/2.0;
01813     marker_plane_tag.pose.position.z = marker_foot_area_plane.pose.position.z;
01814     marker_plane_tag.scale.z = 5.0; 
01815     //marker color
01816     marker_plane_tag.color.r = 0;
01817     marker_plane_tag.color.g = 0;
01818     marker_plane_tag.color.b = 0;
01819     marker_plane_tag.color.a=1.0;
01820     marker_plane_tag.id=0;
01821     marker_plane_tag.lifetime = ros::Duration();
01822     marker_vector.push_back(marker_plane_tag);
01823     
01824     //Center of Pressure
01825     visualization_msgs::Marker marker_COP;
01826     marker_COP.header.frame_id = "/phantom_world_corrected";
01827     //marker info
01828     marker_COP.header.stamp = ros::Time::now();
01829     marker_COP.ns = "COP_sphere";
01830     marker_COP.action = visualization_msgs::Marker::ADD;
01831     //marker type
01832     marker_COP.type = visualization_msgs::Marker::SPHERE;
01833     //marker scale
01834     marker_COP.scale.x = marker_COG.scale.x; 
01835     marker_COP.scale.y = marker_COP.scale.x; 
01836     marker_COP.scale.z = 1; 
01837     //marker pose
01838     marker_COP.pose.position = marker_COG.pose.position;
01839     marker_COP.pose.position.z = marker_foot_area_plane.pose.position.z;
01840     marker_COP.pose.orientation = marker_COG.pose.orientation;
01841     //marker color
01842     marker_COP.color.r=178.0/255.0;
01843     marker_COP.color.g=34.0/255.0;
01844     marker_COP.color.b=34.0/255.0;
01845     marker_COP.color.a=1;
01846     marker_COP.id=0;
01847     marker_COP.lifetime = ros::Duration();
01848     marker_vector.push_back(marker_COP);
01849     
01850     //cop_tag
01851     visualization_msgs::Marker marker_cop_tag;
01852     marker_cop_tag.header.frame_id = "/phantom_world_corrected";
01853     //marker info
01854     marker_cop_tag.header.stamp = ros::Time::now();
01855     marker_cop_tag.ns = "cop_tag";
01856     marker_cop_tag.action = visualization_msgs::Marker::ADD;
01857     //marker type
01858     marker_cop_tag.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
01859     text = "COP";
01860     marker_cop_tag.text = text;
01861     marker_cop_tag.pose.position = marker_COP.pose.position;
01862     marker_cop_tag.scale.z = 10.0; 
01863     //marker color
01864     marker_cop_tag.color.r = 0;
01865     marker_cop_tag.color.g = 0;
01866     marker_cop_tag.color.b = 0;
01867     marker_cop_tag.color.a=1.0;
01868     marker_cop_tag.id=0;
01869     marker_cop_tag.lifetime = ros::Duration();
01870     marker_vector.push_back(marker_cop_tag);
01871   }
01872   else
01873   {
01874     
01875   }
01876 }