phua_haptic: /home/laradmin/lar/bases/phua_haptic/src/gtk

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 <gtk_aux.h>
00033 
00034 using namespace std;
00035 
00036 void UpdateLabels(shared_vars_t*RobotVars)
00037 {
00038   string text;
00039   if(RobotVars->parameters.selected_notebook_page==0)
00040   {
00041     if(RobotVars->parameters.kinematic_model>=1 && RobotVars->parameters.kinematic_model<=3)
00042     {
00043       // -> XYZRPY left
00044       text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.X_arm_end_left)) +" [mm]";
00045       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_left,text.c_str());
00046       
00047       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.Y_arm_end_left)) +" [mm]";
00048       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_left,text.c_str());
00049       
00050       text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.Z_arm_end_left)) +" [mm]";
00051       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_left,text.c_str());
00052       
00053       text="R: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.ROLL_arm_end_left)) +" [deg]";
00054       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_left,text.c_str());
00055       
00056       text="P: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.PITCH_arm_end_left)) +" [deg]";
00057       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_left,text.c_str());
00058       
00059       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.YAW_arm_end_left)) +" [deg]";
00060       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_left,text.c_str());
00061       
00062       // -> XYZRPY right
00063       text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.X_arm_end_right)) +" [mm]";
00064       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_right,text.c_str());
00065       
00066       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.Y_arm_end_right)) +" [mm]";
00067       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_right,text.c_str());
00068       
00069       text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.Z_arm_end_right)) +" [mm]";
00070       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_right,text.c_str());
00071       
00072       text="R: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.ROLL_arm_end_right)) +" [deg]";
00073       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_right,text.c_str());
00074       
00075       text="P: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.PITCH_arm_end_right)) +" [deg]";
00076       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_right,text.c_str());
00077       
00078       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.YAW_arm_end_right)) +" [deg]";
00079       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_right,text.c_str());
00080     }
00081     else if(RobotVars->parameters.kinematic_model==4)
00082     {
00083       // -> XYZRPY left
00084       text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_left[0])) +" [mm]";
00085       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_left,text.c_str());
00086       
00087       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_left[1])) +" [mm]";
00088       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_left,text.c_str());
00089       
00090       text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_left[2])) +" [mm]";
00091       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_left,text.c_str());
00092       
00093       text="R: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_left[0])) +" [deg]";
00094       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_left,text.c_str());
00095       
00096       text="P: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_left[1])) +" [deg]";
00097       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_left,text.c_str());
00098       
00099       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_left[2])) +" [deg]";
00100       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_left,text.c_str());
00101       
00102       // -> XYZRPY right
00103       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_right,"X: ---- [mm]");
00104       
00105       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_right,"Y: ---- [mm]");
00106       
00107       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_right,"Z: ---- [mm]");
00108       
00109       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_right,"R: ---- [deg]");
00110       
00111       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_right,"P: ---- [deg]");
00112       
00113       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_right,"Y: ---- [deg]");
00114     }
00115     else if(RobotVars->parameters.kinematic_model==5)
00116     {
00117       // -> XYZRPY left
00118       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_left,"X: ---- [mm]");
00119       
00120       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_left,"Y: ---- [mm]");
00121       
00122       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_left,"Z: ---- [mm]");
00123       
00124       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_left,"R: ---- [deg]");
00125       
00126       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_left,"P: ---- [deg]");
00127       
00128       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_left,"Y: ---- [deg]");
00129       
00130       // -> XYZRPY right
00131       text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_right[0])) +" [mm]";
00132       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_right,text.c_str());
00133       
00134       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_right[1])) +" [mm]";
00135       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_right,text.c_str());
00136       
00137       text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_pos_right[2])) +" [mm]";
00138       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_right,text.c_str());
00139       
00140       text="R: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_right[0])) +" [deg]";
00141       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_right,text.c_str());
00142       
00143       text="P: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_right[1])) +" [deg]";
00144       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_right,text.c_str());
00145       
00146       text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.detached_leg_rpy_right[2])) +" [deg]";
00147       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_right,text.c_str());
00148     }
00149     else
00150     {
00151       // -> XYZRPY left
00152       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_left,"X: ---- [mm]");
00153       
00154       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_left,"Y: ---- [mm]");
00155       
00156       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_left,"Z: ---- [mm]");
00157       
00158       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_left,"R: ---- [deg]");
00159       
00160       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_left,"P: ---- [deg]");
00161       
00162       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_left,"Y: ---- [deg]");
00163       
00164       // -> XYZRPY right
00165       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.x_label_right,"X: ---- [mm]");
00166       
00167       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.y_label_right,"Y: ---- [mm]");
00168       
00169       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.z_label_right,"Z: ---- [mm]");
00170       
00171       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.roll_label_right,"R: ---- [deg]");
00172       
00173       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.pitch_label_right,"P: ---- [deg]");
00174       
00175       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.yaw_label_right,"Y: ---- [deg]");
00176     }
00177     
00178     // -> head
00179     text="Head Tilt: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.HeadTilt)) +" [deg]";
00180     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.head_tilt_label,text.c_str());
00181     
00182     // -> arms
00183     text="Right Shoulder Flexion:           " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightShoulderFlexion)) + " [deg]\nRight Shoulder Abduction:  "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightShoulderAbduction)) +" [deg]\nRight Elbow Flexion:         "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightElbowFlexion)) +" [deg]";
00184     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.right_arm_label,text.c_str());
00185     text="Left Shoulder Flexion:          " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftShoulderFlexion)) + " [deg]\nLeft Shoulder Abduction:  "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftShoulderAbduction)) +" [deg]\nLeft Elbow Flexion:              "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftElbowFlexion)) +" [deg]";
00186     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.left_arm_label,text.c_str());
00187     
00188     // -> torso
00189     text="Torso Rotation:            " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.TorsoRotation)) + " [deg]\nTorso Flexion:                     "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.TorsoFlexion)) +" [deg]\nTorso Lateral Flexion:  " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.TorsoLateralFlexion)) +" [deg]";
00190     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.torso_label,text.c_str());
00191     
00192     //-> legs
00193     text="Right Hip Flexion:          " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightHipFlexion)) + " [deg]\nRight Hip Abduction:     " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightHipAbduction)) +" [deg]\nRight Knee Flexion:      " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightKneeFlexion)) + " [deg]\nRight Ankle Flexion:     " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightAnkleFlexion)) + " [deg]\nRight Ankle Inversion:  " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.RightAnkleInversion)) + " [deg]";
00194     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.right_leg_label,text.c_str());
00195     text="Left Hip Flexion:         " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftHipFlexion)) + " [deg]\nLeft Hip Abduction:     "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftHipAbduction)) +" [deg]\nLeft Knee Flexion:      " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftKneeFlexion)) + " [deg]\nLeft Ankle Flexion:     " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftAnkleFlexion)) + " [deg]\nLeft Ankle Inversion:  " + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->robot_kin_data.LeftAnkleInversion)) + " [deg]";
00196     gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.left_leg_label,text.c_str());
00197   }
00198   //joystick
00199   if(RobotVars->phantom_data.phantom_on)
00200   {
00201     if(RobotVars->parameters.selected_notebook_page==1)
00202     {
00203       //position
00204       if(RobotVars->parameters.coordinate_resolution>0.01)
00205       {
00206         text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[0])) +" [mm]";
00207         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_x_label,text.c_str());
00208         text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[1])) +" [mm]";
00209         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_y_label,text.c_str());
00210         text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[2])) +" [mm]";
00211         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_z_label,text.c_str());
00212       }
00213       else if(RobotVars->parameters.coordinate_resolution==0.01)
00214       {
00215         text="X: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[0])) +" [mm]";
00216         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_x_label,text.c_str());
00217         text="Y: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[1])) +" [mm]";
00218         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_y_label,text.c_str());
00219         text="Z: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[2])) +" [mm]";
00220         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_z_label,text.c_str());
00221       }
00222       else
00223       {
00224         text="X: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[0])) +" [mm]";
00225         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_x_label,text.c_str());
00226         text="Y: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[1])) +" [mm]";
00227         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_y_label,text.c_str());
00228         text="Z: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_devicePosition[2])) +" [mm]";
00229         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_z_label,text.c_str());
00230       }
00231       
00232       //pen tip position
00233       if(RobotVars->parameters.coordinate_resolution>0.01)
00234       {
00235         text="X: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[0])) +" [mm]";
00236         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_x_label,text.c_str());
00237         text="Y: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[1])) +" [mm]";
00238         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_y_label,text.c_str());
00239         text="Z: "+ str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[2])) +" [mm]";
00240         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_z_label,text.c_str());
00241       }
00242       else if(RobotVars->parameters.coordinate_resolution==0.01)
00243       {
00244         text="X: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[0])) +" [mm]";
00245         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_x_label,text.c_str());
00246         text="Y: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[1])) +" [mm]";
00247         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_y_label,text.c_str());
00248         text="Z: "+ str(boost::format("%3.2f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[2])) +" [mm]";
00249         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_z_label,text.c_str());
00250       }
00251       else
00252       {
00253         text="X: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[0])) +" [mm]";
00254         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_x_label,text.c_str());
00255         text="Y: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[1])) +" [mm]";
00256         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_y_label,text.c_str());
00257         text="Z: "+ str(boost::format("%3.3f") % AvoidMinusZero(RobotVars->phantom_data.m_PenTipPosition[2])) +" [mm]";
00258         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_pen_tip_z_label,text.c_str());
00259       }
00260       
00261       //joint angles
00262       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.jointAngles[0]));
00263       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_jnt1_label),
00264                             g_markup_printf_escaped ("\u03B8<sub>1</sub>: %s<sup>o</sup>", text.c_str()));
00265       
00266       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.jointAngles[1]));
00267       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_jnt2_label),
00268                             g_markup_printf_escaped ("\u03B8<sub>2</sub>: %s<sup>o</sup>", text.c_str()));
00269       
00270       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.jointAngles[2]));
00271       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_jnt3_label),
00272                             g_markup_printf_escaped ("\u03B8<sub>3</sub>: %s<sup>o</sup>", text.c_str()));
00273     
00274       //gimbal angles
00275       text=str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.gimbalAngles[0]));
00276       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_gbl1_label),
00277                             g_markup_printf_escaped ("\u03B8<sub>4</sub>: %s<sup>o</sup>", text.c_str()));
00278       
00279       text=str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.gimbalAngles[1]));
00280       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_gbl2_label),
00281                             g_markup_printf_escaped ("\u03B8<sub>5</sub>: %s<sup>o</sup>", text.c_str()));
00282       
00283       text=str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.gimbalAngles[2]));
00284       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.phantom_gbl3_label),
00285                             g_markup_printf_escaped ("\u03B8<sub>6</sub>: %s<sup>o</sup>", text.c_str()));
00286       
00287       //motor temperatures
00288       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.motor_temperature[0]));
00289       if(RobotVars->phantom_data.motor_temperature[0]<(1./3.))
00290       {
00291         //green
00292         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_1_temp), g_markup_printf_escaped ("Motor 1: <span foreground=\"green\"><b>%s</b></span> ", text.c_str()));
00293       }
00294       else if(RobotVars->phantom_data.motor_temperature[0]<(2./3.) && RobotVars->phantom_data.motor_temperature[0]>=(1./3.))
00295       {
00296         //yellow
00297         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_1_temp), g_markup_printf_escaped ("Motor 1: <span foreground=\"gold\"><b>%s</b></span> ", text.c_str()));
00298       }
00299       else
00300       {
00301         //red
00302         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_1_temp), g_markup_printf_escaped ("Motor 1: <span foreground=\"red\"><b>%s</b></span> ", text.c_str()));
00303       }
00304       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.motor_temperature[1]));
00305       if(RobotVars->phantom_data.motor_temperature[1]<(1./3.))
00306       {
00307         //green
00308         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_2_temp), g_markup_printf_escaped ("Motor 2: <span foreground=\"green\"><b>%s</b></span> ", text.c_str()));
00309       }
00310       else if(RobotVars->phantom_data.motor_temperature[1]<(2./3.) && RobotVars->phantom_data.motor_temperature[1]>=(1./3.))
00311       {
00312         //yellow
00313         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_2_temp), g_markup_printf_escaped ("Motor 2: <span foreground=\"gold\"><b>%s</b></span> ", text.c_str()));
00314       }
00315       else
00316       {
00317         //red
00318         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_2_temp), g_markup_printf_escaped ("Motor 2: <span foreground=\"red\"><b>%s</b></span> ", text.c_str()));
00319       }
00320       text = str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.motor_temperature[2]));
00321       if(RobotVars->phantom_data.motor_temperature[2]<(1./3.))
00322       {
00323         //green
00324         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_3_temp), g_markup_printf_escaped ("Motor 3: <span foreground=\"green\"><b>%s</b></span> ", text.c_str()));
00325       }
00326       else if(RobotVars->phantom_data.motor_temperature[2]<(2./3.) && RobotVars->phantom_data.motor_temperature[2]>=(1./3.))
00327       {
00328         //yellow
00329         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_3_temp), g_markup_printf_escaped ("Motor 3: <span foreground=\"gold\"><b>%s</b></span> ", text.c_str()));
00330       }
00331       else
00332       {
00333         //red
00334         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.motor_3_temp), g_markup_printf_escaped ("Motor 3: <span foreground=\"red\"><b>%s</b></span> ", text.c_str()));
00335       }
00336       
00337       //phantom speed
00338       text = "Magnitude: " +
00339       str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.average_speed.magnitude())) +
00340       " [mm/s] (" + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.average_speed[0])) +
00341       "," + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.average_speed[1])) +
00342       "," + str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->phantom_data.average_speed[2])) + ")";
00343       
00344       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.phantom_speed_label, text.c_str());
00345     } //end of notebook page == 1
00346     else if(RobotVars->parameters.selected_notebook_page==0)
00347     {
00348       if(RobotVars->haptics_data.chosen_demo == 1)
00349       {
00350         gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.workspace_zone_label, "Workspace Demonstration ONLINE");
00351       }
00352       else if(RobotVars->haptics_data.chosen_demo == 2)
00353       {
00354         hduVector3Dd normal = RobotVars->haptics_data.demo_2_Plane.normal();
00355         text = "Plane navigation: At " +
00356         str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->haptics_data.demo2_distance_to_plane)) +
00357         " mm to plane surface\nPlane equation: (" +
00358         str(boost::format("%3.1f") % AvoidMinusZero(normal[0])) +
00359         ").X + (" +
00360         str(boost::format("%3.1f") % AvoidMinusZero(normal[1])) +
00361         ").Y + (" +
00362         str(boost::format("%3.1f") % AvoidMinusZero(normal[2])) +
00363         ").Z + (" +
00364         str(boost::format("%3.1f") % AvoidMinusZero(RobotVars->haptics_data.demo_2_Plane.d())) + 
00365         ") = 0";
00366         gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.drawing_demo_label), g_markup_printf_escaped ("%s", text.c_str()));
00367       }
00368       else if(RobotVars->haptics_data.chosen_demo == 4)
00369       {
00370         if(RobotVars->parameters.kinematic_model == 4)
00371         {
00372           text = "Balancing Support Leg:\nCOG = X:" + 
00373           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_left[0])) +
00374           " [mm] Y:" + 
00375           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_left[1])) +
00376           " [mm] Z:" + 
00377           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_left[2])) +
00378           " [mm]";
00379           gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.leg_balancing_demo_label), g_markup_printf_escaped ("%s", text.c_str()));
00380         }
00381         else if(RobotVars->parameters.kinematic_model == 5)
00382         {
00383           text = "Balancing Support Leg:\nCOG = X:" + 
00384           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_right[0])) +
00385           " [mm] Y:" + 
00386           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_right[1])) +
00387           " [mm] Z:" + 
00388           str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->robot_kin_data.COG_detached_leg_right[2])) +
00389           " [mm]";
00390           gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.leg_balancing_demo_label), g_markup_printf_escaped ("%s", text.c_str()));
00391         }
00392       }
00393       else
00394       {
00395         
00396       }
00397       
00398       // force magnitude label
00399       text = str(boost::format("%3.1f") % AvoidMinusZero((double)RobotVars->haptics_data.applied_force.magnitude()));
00400       gtk_label_set_markup (GTK_LABEL (RobotVars->updt_labels.force_magnitude_label), g_markup_printf_escaped ("<u>Force magnitude</u>: <b>%s</b> [N] (sent to the device)", text.c_str()));
00401       
00402     }//end of notebook page == 0
00403     else
00404     {
00405       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.workspace_zone_label, "Workspace location: [Start haptic loop to update]");
00406       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.force_magnitude_label, "Force magnitude: --- [N] (sent to the device)");
00407       gtk_label_set_text((GtkLabel *)RobotVars->updt_labels.drawing_demo_label, "Plane navigation: -------------\nPlane equation: ax + by + cz + d = 0");
00408     }
00409   }
00410   return;
00411 }
00412 
00413 void UpdateForceBars(shared_vars_t*RobotVars)
00414 {
00415   // X force bars
00416   if(RobotVars->haptics_data.applied_force[0] < 0.)
00417   {
00418     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_x_force_bar, 0.);
00419     if(RobotVars->haptics_data.applied_force[0] < -RobotVars->phantom_data.max_force)
00420       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_x_force_bar, 1.);
00421     else
00422       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_x_force_bar, RobotVars->haptics_data.applied_force[0] / (-RobotVars->phantom_data.max_force));
00423   }
00424   else
00425   {
00426     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_x_force_bar, 0.);
00427     if(RobotVars->haptics_data.applied_force[0] > RobotVars->phantom_data.max_force)
00428       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_x_force_bar, 1.);
00429     else
00430       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_x_force_bar, RobotVars->haptics_data.applied_force[0] / RobotVars->phantom_data.max_force);
00431   }
00432   // Y force bars
00433   if(RobotVars->haptics_data.applied_force[1] < 0.)
00434   {
00435     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_y_force_bar, 0.);
00436     if(RobotVars->haptics_data.applied_force[1] < -RobotVars->phantom_data.max_force)
00437       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_y_force_bar, 1.);
00438     else
00439       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_y_force_bar, RobotVars->haptics_data.applied_force[1] / (-RobotVars->phantom_data.max_force));
00440   }
00441   else
00442   {
00443     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_y_force_bar, 0.);
00444     if(RobotVars->haptics_data.applied_force[1] > RobotVars->phantom_data.max_force)
00445       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_y_force_bar, 1.);
00446     else
00447       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_y_force_bar, RobotVars->haptics_data.applied_force[1] / RobotVars->phantom_data.max_force);
00448   }
00449   // Z force bars
00450   if(RobotVars->haptics_data.applied_force[2] < 0.)
00451   {
00452     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_z_force_bar, 0.);
00453     if(RobotVars->haptics_data.applied_force[2] < -RobotVars->phantom_data.max_force)
00454       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_z_force_bar, 1.);
00455     else
00456       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_z_force_bar, RobotVars->haptics_data.applied_force[2] / (-RobotVars->phantom_data.max_force));
00457   }
00458   else
00459   {
00460     gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.neg_z_force_bar, 0.);
00461     if(RobotVars->haptics_data.applied_force[2] > RobotVars->phantom_data.max_force)
00462       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_z_force_bar, 1.);
00463     else
00464       gtk_progress_bar_set_fraction((GtkProgressBar *)RobotVars->updt_labels.pos_z_force_bar, RobotVars->haptics_data.applied_force[2] / RobotVars->phantom_data.max_force);
00465   }
00466 }
00467 
00468 void UpdateStatusBar(shared_vars_t*RobotVars)
00469 {
00470   string text;
00471 
00472     // throw statusbar messages
00473     
00474   if(RobotVars->servo->IsActive() && !RobotVars->phantom_data.phantom_on)
00475   {
00476     gtk_statusbar_push(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),
00477                        gtk_statusbar_get_context_id(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),"statusbar_info"),
00478                        ":: Hitec servomotor COMM active :: PHANToM OMNI inactive ::");
00479   }
00480   else if(RobotVars->servo->IsActive() && RobotVars->phantom_data.phantom_on)
00481   {
00482     text=":: Hitec servomotor COMM active :: PHANToM OMNI active :: Control Frequency: " + str(boost::format("%d") % AvoidMinusZero(RobotVars->parameters.control_rate)) + " [Hz] :: Haptic Rendering Frequency: " + str(boost::format("%d") % AvoidMinusZero(RobotVars->parameters.haptics_rate)) + " [Hz] ::";
00483     
00484     gtk_statusbar_push(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),
00485                        gtk_statusbar_get_context_id(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),"statusbar_info"),
00486                        text.c_str());
00487   }
00488   else if(!RobotVars->servo->IsActive() && RobotVars->phantom_data.phantom_on)
00489   {
00490     gtk_statusbar_push(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),
00491                        gtk_statusbar_get_context_id(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),"statusbar_info"),
00492                        ":: Hitec servomotor COMM inactive :: PHANToM OMNI active ::");
00493   }
00494   else
00495   {
00496     gtk_statusbar_push(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),
00497                        gtk_statusbar_get_context_id(GTK_STATUSBAR(RobotVars->updt_labels.status_bar),"statusbar_info"),
00498                        ":: Communications inactive ::");
00499   }
00500 }
/home/laradmin/lar/bases/phua_haptic/src/gtk_aux.cpp
