humanoid_control: /home/laradmin/lar/bases/humanoid_control/src/Humanoid

00001 /**************************************************************************************************
00002 Software License Agreement (BSD License)
00003 Copyright (c) 2011-2014, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00004 All rights reserved.
00005 Redistribution and use in source and binary forms, with or without modification, are permitted
00006 provided that the following conditions are met:
00007 *Redistributions of source code must retain the above copyright notice, this list of
00008 conditions and the following disclaimer.
00009 *Redistributions in binary form must reproduce the above copyright notice, this list of
00010 conditions and the following disclaimer in the documentation and/or other materials provided
00011 with the distribution.
00012 *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00013 endorse or promote products derived from this software without specific prior written permission.
00014 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00015 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00018 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00019 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00020 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00021 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00022 ***************************************************************************************************/
00028 #include <ros/ros.h>
00029 #include <std_msgs/String.h>
00030 #include <sstream>
00031 #include <unistd.h>
00032 #include <iostream>
00033 #include <cmath>  
00034 #include <sys/types.h>
00035 #include <unistd.h>
00036 #include <stdlib.h>
00037 #include <humanoid_control/servohumanoid.h>
00038 #include <hitec5980sg/hitec5980sg.h>
00039 #include <phantom_filter/Phantom.h>
00040 #include <humanoid_control/Humanoid.h>
00041 
00042 #include <pressure_cells/Cop.h>
00043 using namespace ros;
00044 using namespace std;
00045 
00046 ServoHumanoid *g_human;
00047 
00048 ros::Publisher pub_state;
00049 
00050 phantom_filter::Phantom g_PState;
00051 humanoid_control::Humanoid g_RState;
00052 pressure_cells::Cop   g_Cop;
00053 int firstHome=0; //NEW
00054 int calibration=0;
00055 const double g_ZZ=THIGH_LENGTH+LEG_LENGTH;
00056 short int firsttme=1;
00057 
00058 //const short int id_list[12]={11,12,13,15,16,21,22,23,25,26,31,32};
00060                                         //0 ,1, 2   3  4  5  6  7  8  9 10 11  
00061 const short int id_list[12]={11,21,12,22,13,32,15,25,16,26,31,23};//Troca entre servo 23 e 32       // bottom to top servo's command   
00062 
00063 void PhantomFilterCallBk ( const phantom_filter::Phantom &phantom_state )
00064 {
00065                 double joint_angle, current_POS;
00066                 double  z_scale = 0.5, x_scale = 5, y_scale=0.8;
00067 
00068         switch(calibration){
00069           case 1:
00070         
00071 
00072                 g_PState=phantom_state;
00073 
00074                 if(g_human->RoboState.op_mode==1) //mode x y active
00075                 {
00076                         /*********************Retrieve info from position 1**************************/
00077                         current_POS=g_PState.Position[1];
00078                         if(current_POS>0.0) current_POS=current_POS-3;
00079                         if(current_POS<0.0) current_POS=current_POS+3;
00080 
00081                         joint_angle = acos ((g_ZZ + current_POS * z_scale)/ g_ZZ) * 180.0 / PI;
00082 
00083                         if(isnan(joint_angle)) joint_angle=0;
00084                         if(joint_angle>20.) joint_angle=20.0;
00085 
00086                         g_human->RoboState.joint_wanted[2]=joint_angle;
00087                         g_human->RoboState.joint_wanted[3]=joint_angle;
00088                         g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00089                         g_human->RoboState.joint_wanted[5]=joint_angle*2.;
00090                         g_human->RoboState.joint_wanted[8]=joint_angle;
00091                         g_human->RoboState.joint_wanted[9]=joint_angle;
00092 
00093 
00094                         //cout<<"Ankle angle = "<<joint_angle<<endl;
00095 
00096                         /*********************Retrieve info from position 0**************************/
00097                         current_POS=g_PState.Position[0];
00098                         if(current_POS>0.) current_POS=current_POS-3;
00099                         if(current_POS<0.0) current_POS=current_POS+3;
00100 
00101                         joint_angle = atan2(current_POS * x_scale, g_ZZ) * 180.0 / PI; // alt 25 anterior 30
00102 
00103                         if(isnan(joint_angle)) joint_angle=0;
00104                         if(joint_angle>25.) joint_angle=25.0;
00105                         if(joint_angle<-25.) joint_angle=-25.0;
00106 
00107                         g_human->RoboState.joint_wanted[0]=-joint_angle;
00108                         g_human->RoboState.joint_wanted[1]=joint_angle;
00109                         g_human->RoboState.joint_wanted[6]=joint_angle;
00110                         g_human->RoboState.joint_wanted[7]=-joint_angle;
00111 
00112                         cout<<"Ankle angle = "<<joint_angle<<endl;
00113 
00114                 }
00115 
00116                 else if(g_human->RoboState.op_mode==2) // Mode z active
00117                 {
00118                         if (firsttme==1)
00119                         {
00120                                 cout<<"First Time"<<endl;
00121                 
00122                                 g_human->RoboState.joint_wanted[2]=20.;
00123                                 g_human->RoboState.joint_wanted[3]=20.;
00124                                 g_human->RoboState.joint_wanted[4]=40.;
00125                                 g_human->RoboState.joint_wanted[5]=40.;
00126                                 g_human->RoboState.joint_wanted[8]=20.;
00127                                 g_human->RoboState.joint_wanted[9]=20.;
00128                 
00129                                 firsttme=0;
00130                         }
00131                         else
00132                         {
00133                                 current_POS=-g_PState.Position[2];
00134                                 if(current_POS>0.0) current_POS=current_POS-3;
00135                                 if(current_POS<0.0) current_POS=current_POS+3;
00136                                 joint_angle = asin((current_POS * y_scale)/THIGH_LENGTH) * 180.0 / PI + 20.;
00137                 
00138                                 //if(isnan(joint_angle)) joint_angle=0;
00139                                 if(joint_angle>0.) g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00140                                 if(joint_angle<0.) g_human->RoboState.joint_wanted[4]=joint_angle/2.;
00141                 
00142                                 g_human->RoboState.joint_wanted[2]=20.;
00143                                 g_human->RoboState.joint_wanted[3]=20.;
00144                                 //g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00145                                 g_human->RoboState.joint_wanted[5]=g_human->RoboState.joint_wanted[4];
00146                                 g_human->RoboState.joint_wanted[8]=joint_angle;
00147                                 g_human->RoboState.joint_wanted[9]=joint_angle;
00148                 
00149                                 cout<<"Hip angle = "<<joint_angle<<endl;
00150                 
00151                         }
00152                 }
00153 
00154                 else if(g_human->RoboState.op_mode==3) // mode x y z active
00155                 {
00156                         /*********************Retrieve info from position 1**************************/
00157                         current_POS=g_PState.Position[1];
00158                         if(current_POS>0.0) current_POS=current_POS-3; //Compensar dead-zone
00159                         if(current_POS<0.0) current_POS=current_POS+3;
00160                 
00161                         joint_angle = acos ((g_ZZ + current_POS * z_scale)/ g_ZZ) * 180.0 / PI;
00162 
00163                         if(isnan(joint_angle)) joint_angle=0;
00164                         if(joint_angle>20.) joint_angle=20.0;
00165 
00166                         g_human->RoboState.joint_wanted[2]=joint_angle;
00167                         g_human->RoboState.joint_wanted[3]=joint_angle;
00168                         g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00169                         g_human->RoboState.joint_wanted[5]=joint_angle*2.;
00170                         g_human->RoboState.joint_wanted[8]=joint_angle;
00171                         g_human->RoboState.joint_wanted[9]=joint_angle;
00172 
00173                         /*********************Retrieve info from position 0**************************/
00174                         current_POS=g_PState.Position[0];
00175                         if(current_POS>0.0) current_POS=current_POS-3;//Compensar dead-zone
00176                         if(current_POS<0.0) current_POS=current_POS+3;
00177                         joint_angle = atan2(current_POS * x_scale, g_ZZ) * 180.0 / PI; // alt 25 anterior 30
00178                 
00179                         if(isnan(joint_angle)) joint_angle=0;
00180                         if(joint_angle>25.) joint_angle=25.0;
00181                         if(joint_angle<-25.) joint_angle=-25.0;
00182                 
00183                         g_human->RoboState.joint_wanted[0]=-joint_angle;
00184                         g_human->RoboState.joint_wanted[1]=joint_angle;
00185                         g_human->RoboState.joint_wanted[6]=joint_angle;
00186                         g_human->RoboState.joint_wanted[7]=-joint_angle;
00187 
00188                         /*********************Retrieve info from position 0**************************/
00189                         current_POS=-g_PState.Position[2];
00190                         if(current_POS>0.0) current_POS=current_POS-3;//Compensar dead-zone
00191                         if(current_POS<0.0) current_POS=current_POS+3;   
00192                 
00193                         joint_angle = asin((current_POS * y_scale)/THIGH_LENGTH) * 180.0 / PI + 20.;
00194 
00195                         //if(isnan(joint_angle)) joint_angle=0;
00196                         if(joint_angle>0.) g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00197                         if(joint_angle<0.) g_human->RoboState.joint_wanted[4]=joint_angle/2.;
00198                         g_human->RoboState.joint_wanted[5]=g_human->RoboState.joint_wanted[4];
00199 
00200                 }
00201                         //Modo Simples (Emilio) para testes
00202                 else if(g_human->RoboState.op_mode==4)
00203                 {
00204                         current_POS=g_PState.Position[1];
00205 
00206                         joint_angle = acos ((g_ZZ + current_POS * z_scale)/ g_ZZ) * 180.0 / PI;
00207         
00208                         if(isnan(joint_angle)) joint_angle=0;
00209                         if(joint_angle>20.) joint_angle=20.0;
00210 
00211                         g_human->RoboState.joint_wanted[2]=joint_angle;
00212                         g_human->RoboState.joint_wanted[3]=joint_angle;
00213                         g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00214                         g_human->RoboState.joint_wanted[5]=joint_angle*2.;
00215                         g_human->RoboState.joint_wanted[8]=joint_angle;
00216                         g_human->RoboState.joint_wanted[9]=joint_angle;
00217 
00218                         cout<<"Ankle angle = "<<joint_angle<<endl;
00219                 }
00220 
00221                         //UMA Perna
00222                 else if(g_human->RoboState.op_mode==5)
00223                 {
00224                         current_POS=g_PState.Position[1];
00225 
00226                         joint_angle = acos ((g_ZZ + current_POS * z_scale)/ g_ZZ) * 180.0 / PI;
00227 
00228                         if(isnan(joint_angle)) joint_angle=0;
00229                         if(joint_angle>20.) joint_angle=20.0;
00230 
00231                         g_human->RoboState.joint_wanted[2]=joint_angle;
00232                         g_human->RoboState.joint_wanted[3]=joint_angle;
00233                         g_human->RoboState.joint_wanted[4]=joint_angle*2.;
00234                         g_human->RoboState.joint_wanted[5]=joint_angle*2.;
00235                         
00236 
00237                         /*ª**************Anca**************************************/
00238                     current_POS=-g_PState.Position[2]*2;
00239                         if(current_POS>0.0) current_POS=current_POS-3;//Compensar dead-zone
00240                         if(current_POS<0.0) current_POS=current_POS+3;   
00241                 
00242                         joint_angle = asin((current_POS * y_scale)/THIGH_LENGTH) * 180.0 / PI + 20.;
00243                         
00244                         g_human->RoboState.joint_wanted[8]=joint_angle;
00245                         g_human->RoboState.joint_wanted[9]=joint_angle;
00246                         
00247                 }
00248                 
00249                  else if(g_human->RoboState.op_mode==6)
00250                 {
00251                         current_POS=g_PState.Position[0];
00252         
00253                         joint_angle = atan2(current_POS * x_scale, g_ZZ) * 180.0 / PI;
00254         
00255                         if(isnan(joint_angle)) joint_angle=0;
00256                         if(joint_angle>30.) joint_angle=30.0;
00257                         if(joint_angle<-30.) joint_angle=-30.0;
00258         
00259                         g_human->RoboState.joint_wanted[0]=-joint_angle;
00260                         g_human->RoboState.joint_wanted[1]=joint_angle;
00261                         g_human->RoboState.joint_wanted[6]=joint_angle;
00262                         g_human->RoboState.joint_wanted[7]=-joint_angle;
00263         
00264                         cout<<"Ankle angle = "<<joint_angle<<endl;
00265                 }
00266                 //
00267                 else if(g_human->RoboState.op_mode==0)
00268                 {
00269                         for (int i = 0; i < 13; i++) //Servo alterado 
00270                         {
00271                                 g_human->RoboState.joint_wanted[i]=0;
00272                         }
00273                 }
00274                 break;
00275 
00276         case 0:
00277                 break;
00278 
00279         }
00280 }
00281 
00282 
00283 
00284 void PhantomCopCallBk ( const pressure_cells::Cop &cop)//NEW
00285 {
00286         short unsigned int resp=65535;
00287 
00288         switch(calibration){
00289                 case 1:
00290                          break;
00291                 case 0:
00292                         cout<<"Entrei"<<endl;
00293                         g_Cop=cop;
00294                         if(firstHome==0){
00295                                 g_human->HomePosition();
00296                                 firstHome=1;
00297                         }
00298                         else if(firstHome==1)
00299                         {
00300                         if(g_Cop.copy>0.075)
00301                         {
00302                                 g_human->RoboState.joint_wanted[2]=g_human->RoboState.joint_now[2]-0.5;
00303                                 g_human->RoboState.joint_wanted[3]=g_human->RoboState.joint_now[3]-0.5;
00304                                 g_human->RoboState.joint_wanted[4]=g_human->RoboState.joint_now[4]-1;
00305                                 g_human->RoboState.joint_wanted[5]=g_human->RoboState.joint_now[5]-1;
00306                                 g_human->RoboState.joint_wanted[8]=g_human->RoboState.joint_now[8]-0.5;
00307                                 g_human->RoboState.joint_wanted[9]=g_human->RoboState.joint_now[9]-0.5;
00308                         }
00309                         else if(g_Cop.copy<0.065)
00310                         {
00311                                 g_human->RoboState.joint_wanted[2]=g_human->RoboState.joint_now[2]+0.5;
00312                                 g_human->RoboState.joint_wanted[3]=g_human->RoboState.joint_now[3]+0.5;
00313                                 g_human->RoboState.joint_wanted[4]=g_human->RoboState.joint_now[4]+1;
00314                                 g_human->RoboState.joint_wanted[5]=g_human->RoboState.joint_now[5]+1;
00315                                 g_human->RoboState.joint_wanted[8]=g_human->RoboState.joint_now[8]+0.5;
00316                                 g_human->RoboState.joint_wanted[9]=g_human->RoboState.joint_now[9]+0.5;
00317                         }
00318                         if(g_Cop.copx>0.005)
00319                         {
00320                                 g_human->RoboState.joint_wanted[0]=g_human->RoboState.joint_now[0]+0.5;
00321                                 g_human->RoboState.joint_wanted[1]=g_human->RoboState.joint_now[1]-0.5;
00322                                 g_human->RoboState.joint_wanted[6]=g_human->RoboState.joint_now[6]-0.5;
00323                                 g_human->RoboState.joint_wanted[7]=g_human->RoboState.joint_now[7]+0.5;
00324                         }
00325                         else if(g_Cop.copx<-0.005)
00326                         {
00327                                 g_human->RoboState.joint_wanted[0]=g_human->RoboState.joint_now[0]-0.5;
00328                                 g_human->RoboState.joint_wanted[1]=g_human->RoboState.joint_now[1]+0.5;
00329                                 g_human->RoboState.joint_wanted[6]=g_human->RoboState.joint_now[6]+0.5;
00330                                 g_human->RoboState.joint_wanted[7]=g_human->RoboState.joint_now[7]-0.5;
00331                         }
00332                         }
00333                         for (uint i = 0; i <12 ; i++)
00334                         {
00335                                 resp = g_human->MoveJoint(id_list[i], g_human->RoboState.joint_wanted[i]);
00336                         
00337                 
00338                                 resp = g_human->SetJointSpeed(id_list[i], g_human->RoboState.speed_wanted[i]);
00339                                 g_human->RoboState.joint_now[i]=g_human->ConvertServoValueByID(id_list[i], resp);
00340                         }
00341 
00342                         for (int i = 0; i < 13 ; i++)
00343                         {
00344                                 g_RState.joint_now[i]=g_human->RoboState.joint_wanted[i];
00345                                 g_RState.speed_wanted[i]=g_human->RoboState.speed_wanted[i];
00346                         }
00347 
00348                         // publish HumanState
00349                         pub_state.publish( g_RState );
00350 
00351                         
00352                         cout<<"Calibration done? (1-Sim ; 0-Nao ) : "<<endl; //Operation mode:
00353                         cin>>calibration;
00354                         break;
00355         }       
00356 }
00361 int main(int argc, char** argv)
00362 {
00363         short unsigned int resp=65535;
00364 
00365         // ROS
00366         ros::init( argc, argv, "humanoid_node" );
00367 
00368         ros::NodeHandle n("~");
00369 
00370         pub_state= n.advertise< humanoid_control::Humanoid >( "/humanoid_state", 1000 );
00371 
00372         ros::Subscriber sub_force = n.subscribe ( "/phantom_filter", 100, PhantomFilterCallBk );
00373 
00374         ros::Subscriber sub_cop = n.subscribe ( "/cop_left_right", 100, PhantomCopCallBk ); //NEW
00375 
00376         int op_mode = 0;
00377         int op_mode_default = 5;
00378         n.param("op_mode", op_mode, op_mode_default );
00379 
00380         g_human = (ServoHumanoid*) new ServoHumanoid("/dev/ttyUSB0");
00381 
00382         if(op_mode==0)
00383         {
00384                 ROS_INFO("Operation mode of humanoid robot is 0");
00385                 ROS_INFO("STOP MODE");
00386 
00387                 g_human->RoboState.op_mode=op_mode;
00388                 g_human->RoboState.joint_wanted[0]=0.;
00389                 g_human->RoboState.joint_wanted[1]=0.;
00390                 g_human->RoboState.joint_wanted[2]=10.;
00391                 g_human->RoboState.joint_wanted[3]=10.;
00392                 g_human->RoboState.joint_wanted[4]=20.;
00393                 g_human->RoboState.joint_wanted[5]=20.;
00394                 g_human->RoboState.joint_wanted[6]=0.;
00395                 g_human->RoboState.joint_wanted[7]=0.;
00396                 g_human->RoboState.joint_wanted[8]=15.;
00397                 g_human->RoboState.joint_wanted[9]=15.;
00398                 g_human->RoboState.joint_wanted[10]=0.;
00399                 g_human->RoboState.joint_wanted[11]=0.;
00400         
00401         }else if(op_mode==1)
00402         {
00403                 ROS_INFO("Operation mode of humanoid robot is 1");
00404                 ROS_INFO("BOUNCY MODE");
00405                 g_human->RoboState.op_mode=op_mode;
00406         }
00407         else if(op_mode==2)
00408         {
00409                 ROS_INFO("Operation mode of humanoid robot is 2");
00410                 ROS_INFO("HUMPING MODE");
00411                 g_human->RoboState.op_mode=op_mode;
00412         }
00413 
00414         else if(op_mode==3)
00415         {
00416                 ROS_INFO("Operation mode of humanoid robot is 3");
00417                 ROS_INFO("3D MODE");
00418                 g_human->RoboState.op_mode=op_mode;
00419         }
00420         else if(op_mode==4)
00421         {
00422                 ROS_INFO("Operation mode of humanoid robot is 4");
00423                 ROS_INFO("Emilio test");
00424                 g_human->RoboState.op_mode=op_mode;
00425         }
00426 
00427         else if(op_mode==5)
00428         {
00429                 ROS_INFO("Operation mode of humanoid robot is 5");
00430                 ROS_INFO("Exp Down up");
00431                 g_human->RoboState.op_mode=op_mode;
00432         }
00433 
00434         for (int i = 0; i < 12; i++)
00435         {
00436                 g_human->RoboState.speed_wanted[i]=15;
00437         }
00438         //g_human->HomePosition();
00439 
00440 
00441         g_RState.speed_g_static=g_human->RoboState.speed_g_static;
00442 
00443         while(ros::ok())
00444         {   
00445                 cout<<"Publicado"<<endl;
00446                 for (uint i = 0; i <12 ; i++)
00447                 {
00448                         if(  (g_human->RoboState.joint_now[i] > (g_human->RoboState.joint_wanted[i] + 1 )) || (g_human->RoboState.joint_now[i] < (g_human->RoboState.joint_wanted[i] - 1)))
00449                         {
00450                                 resp = g_human->MoveJoint(id_list[i], g_human->RoboState.joint_wanted[i]);
00451                         
00452                                 //cout<<"Robot needs to change position in joint "<<id_list[i]<<" : position now "<<g_human->RoboState.joint_now[i]<<"   position wanted "<<g_human->RoboState.joint_wanted[i]<<endl;
00453                         }
00454                 
00455                         resp = g_human->SetJointSpeed(id_list[i], g_human->RoboState.speed_wanted[i]);
00456                         g_human->RoboState.joint_now[i]=g_human->ConvertServoValueByID(id_list[i], resp);
00457                         
00458                 }
00459 
00460                 for (int i = 0; i < 13 ; i++)
00461                 {
00462                         g_RState.joint_now[i]=g_human->RoboState.joint_wanted[i];
00463                         g_RState.speed_wanted[i]=g_human->RoboState.speed_wanted[i];
00464                 }
00465                 //cout<< g_RState.speed_wanted[8]<<std::endl;
00466                 g_RState.header.stamp=ros::Time::now();
00467                 // publish HumanState
00468                 pub_state.publish( g_RState );
00469 
00470 
00471                 ros::spinOnce (  );
00472         }
00473 
00474         delete g_human;
00475 
00476 
00477         return 0;
00478 }
/home/laradmin/lar/bases/humanoid_control/src/Humanoid_control.cpp
