KinematicSimulator.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "KinematicSimulator.h"
00020 
00021 //#define COLLISION_TESTING
00022 
00023 int cartesian_dim = 3;
00024 int joint_angle_dim = 0;
00025 
00026 
00027 KinematicSimulator::KinematicSimulator(){}
00028 
00029 
00030 KinematicSimulator::KinematicSimulator(int argc, char **argv):
00031   GLUTBaseApplication(argc, argv){
00032   strcpy(tree_file,argv[1]);
00033   strcpy(shape_file,argv[2]);
00034   m_bProprioActive = false;
00035   proprio_arm.open("/simulator/arm:i");
00036   proprio_head.open("/simulator/head:i");
00037   targetIn.open("/simulator/target:i");
00038   targetOut.open("/simulator/target:o");
00039   cmdPort.open("/simulator/cmd:o");
00040   bodySchema.open("/simulator/body_schema:i");
00041   target_info=1;
00042   body=NULL;
00043   angles=NULL;
00044   arm = NULL;
00045   head = NULL;
00046   head_size=0;
00047   arm_size = 0;
00048   mapping = 0;
00049 
00050 #ifdef TWO_BODIES
00051   body_data = NULL;
00052 #endif
00053 
00054 }
00055 
00056 KinematicSimulator::~KinematicSimulator(){
00057     if(arm) delete arm;
00058     if(head) delete head;
00059     if(body)    delete body;
00060     if(angles) delete[] angles;
00061 #ifdef TWO_BODIES
00062     if(arm) delete arm2;
00063     if(head) delete head2;
00064     if(body)    delete body2;
00065     if(body_data) delete[] body_data;
00066 #endif
00067 }
00068 
00069 
00070 
00071 void  KinematicSimulator::InitOpenGL(){
00072   GetCamera()->Load("Camera.cfg");
00073 
00074   glEnable(GL_DEPTH_TEST);
00075 
00076   GLfloat LightAmbient[]= { 0.3f, 0.3f, 0.3f, 0.0f };   
00077   GLfloat LightDiffuse[]= { 0.4f, 0.8f, 0.3f, 1.0f };   
00078   GLfloat LightPosition[]= { -10.0f, 0.0f, 10.0f, 0.0f };       
00079   glLightfv(GL_LIGHT1, GL_AMBIENT, LightAmbient);
00080   glLightfv(GL_LIGHT1, GL_DIFFUSE, LightDiffuse);       
00081   glLightfv(GL_LIGHT1, GL_POSITION,LightPosition);
00082   glEnable(GL_LIGHT1);
00083   //glEnable(GL_LIGHTING);
00084 
00085   glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
00086   glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT);
00087   glEnable(GL_COLOR_MATERIAL);
00088   
00089   glEnable(GL_BLEND);
00090   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00091 
00092 
00093   glEnable(GL_LINE_SMOOTH);
00094   glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
00095   glShadeModel (GL_SMOOTH);
00096 
00097   glClearColor(0.2,0.4,0.0,1.0);
00098 
00099 
00100   m_Camera[m_CurrentCamera]->Move(-200,-400,0,0,-20);
00101 }
00102 
00103 
00104 void KinematicSimulator::ClosePorts(){
00105   proprio_arm.close();
00106   proprio_head.close();
00107   targetIn.close();
00108   targetOut.close();
00109   cmdPort.close();
00110   bodySchema.close();
00111 }
00112 
00113 void KinematicSimulator::InputSpecialKey(int key, int x, int y){
00114     cart_vec_t dtar;
00115     switch(key){
00116     case GLUT_KEY_UP:
00117         dtar.GetArray()[2] = -20;
00118         break;
00119     case GLUT_KEY_DOWN:
00120         dtar.GetArray()[2] = 20;
00121         break;
00122     case GLUT_KEY_LEFT:
00123         dtar.GetArray()[0] = -20;
00124         break;
00125     case GLUT_KEY_RIGHT:
00126         dtar.GetArray()[0] = 20;
00127         break;
00128     case GLUT_KEY_PAGE_DOWN:
00129         dtar.GetArray()[1] = -20;
00130         break;
00131     case GLUT_KEY_PAGE_UP:
00132         dtar.GetArray()[1] = 20;
00133         break;
00134     }
00135     tarpos+=dtar;
00136     tarpos.Print();
00137 }
00138 
00139 
00140 void KinematicSimulator::InputNormalKey(unsigned char key,int x, int y){
00141     switch(key){
00142   case 27:
00143       cmdPort.SendQuit();
00144       ClosePorts();  
00145       Exit();
00146       break;
00147   case 'p':
00148       //      ListenToRobot();
00149       for(int i=0;i<body_size;i++){
00150           cout<<angles[i]*rad2deg<<" ";
00151       }
00152       cout<<endl;
00153       break;
00154   case 'r':
00155       body->GetArticulatedTree()->RandomAngle();
00156       cout<<"----- collisions ---------"<<endl;
00157       body->CheckAllCollisions(NULL);
00158       break;
00159   case 's':
00160       tarpos.Print();
00161       targetOut.SendPosition(tarpos.GetArray());
00162       break;
00163   default:
00164       cout<<"key not understood"<<endl;
00165   }
00166 }
00167   
00168 void KinematicSimulator::InitBody(){
00169   body = new EmbodiedKinematicTree(tree_file,shape_file);
00170   body_size = body->GetArticulatedTree()->GetSize();
00171   if(body->LoadChain("r_sfe","r_hand",0)){
00172       cout<<"found arm chain"<<endl;
00173       arm = body->GetChain(0);
00174       joint_angle_dim=arm_size = arm->GetNbJoints();// no last link
00175   }
00176   else{
00177       return;
00178   }
00179   if(body->LoadChain("neck_tilt","eyes",1)>0){
00180       cout<<"found head chain "<< endl;
00181       head = body->GetChain(1);
00182       head_size = head->GetNbJoints();
00183       cout<<"head size "<<head->GetNbJoints()<<endl;
00184   }
00185 
00186 #ifdef TWO_BODIES
00187   //  display_trans.Zero();
00188   display_trans.GetArray()[0] = 800;
00189   body2 = new EmbodiedKinematicTree(tree_file,shape_file);
00190   body2->GetArticulatedTree()->GetJoint()->SetTranslation(display_trans.GetArray());
00191   if(body2->LoadChain("r_sfe","r_hand",0)){
00192       cout<<"found arm chain"<<endl;
00193       arm2 = body2->GetChain(0);
00194   }
00195   if(body2->LoadChain("neck_tilt","eyes",1)>0){
00196       cout<<"found head chain "<< endl;
00197       head2 = body2->GetChain(1);
00198   }
00199   body_data_size = body->GetTreeSize()*6;
00200   body_data = new float[body_data_size];
00201 #endif
00202 
00203 
00204 
00205   cout<<"size "<<body_size<<endl; 
00206   angles = new float[body_size];
00207   arm_angles = angles;
00208   if(head){
00209        head_angles = angles+arm_size;
00210   }
00211 }
00212 
00213 void KinematicSimulator::Init(){
00214    InitOpenGL();
00215    InitBody();
00216    //   body->GetArticulatedTree()->PrintList();
00217 }
00218 
00219 
00220 void KinematicSimulator::RenderTarget(){
00221  
00222   switch(target_info){
00223   case 1:{ 
00224     glPushMatrix();
00225     glTranslatef(tarpos.GetArray()[0],tarpos.GetArray()[1],tarpos.GetArray()[2]);
00226     //glTranslatef(tarpos[0],tarpos[1],tarpos[2]);
00227    //  glTranslatef(-200,0,0);
00228     glColor3f(1.0,0,1.0);
00229     glutSolidSphere(40,15,15);
00230     glPopMatrix();
00231     break;
00232   }
00233   case 2:{
00234     Parallelipiped par;
00235     par.SetPosition(tarpos.GetArray());
00236     par.SetSize(5,-50,30);
00237     par.SetOrientation(tarpos.GetArray()+3);
00238     par.Render();
00239     break;
00240   }
00241   }
00242 }
00243 
00244 void KinematicSimulator::Render(){
00245  RenderTarget(); 
00246  body->UpdateShape();
00247  body->Render();
00248 #ifdef TWO_BODIES
00249  tarpos += display_trans;
00250  RenderTarget();
00251  tarpos -= display_trans;
00252  body2->UpdateShape();
00253  body2->Render();
00254 #endif
00255 
00256  glutPostRedisplay();
00257 }
00258 
00259 
00260 void KinematicSimulator::ListenToRobot(){
00261   proprio_arm.open();
00262   proprio_head.open();
00263 }
00264 
00265 void KinematicSimulator::Rearrange(float *a){
00266   float b[29];
00267   memcpy(b,a,29*sizeof(float));
00268   a[0] = 0; 
00269   a[1] = b[21]; 
00270   a[2] = b[22]; 
00271   a[3] = b[23]; 
00272   a[4] = b[6]; 
00273   a[5] = b[7]; 
00274   a[6] = b[8]; 
00275   a[7] = b[9]; 
00276   a[8] = 0; 
00277   a[9] = b[16]; 
00278   a[10] = b[17]; 
00279   a[11] = b[18]; 
00280   a[12] = b[19];
00281   a[13] = 0; 
00282   a[14] = b[20]; 
00283   a[15] = b[0]; 
00284   a[16] = b[1]; 
00285   a[17] = b[2]; 
00286   a[18] = b[3]; 
00287   a[19] = b[4]; 
00288   a[20] = b[5]; 
00289   a[21] = 0; 
00290   a[22] = b[10]; 
00291   a[23] = b[11]; 
00292   a[24] = b[12]; 
00293   a[25] = b[13]; 
00294   a[26] = b[14]; 
00295   a[27] = b[15];
00296   a[28] = 0;
00297 }
00298 
00299 
00300 void KinematicSimulator::OnIdle(){
00301 
00302   int ok = 0;
00303 //   if(proprio.IsOpen()){
00304 //     if(!m_bProprioActive){
00305 //       //      targetIn.ConnectFrom("/target/out");
00306 //       m_bProprioActive =(proprio.getInputCount>0)
00307 //      proprio.ConnectFrom("/proprio") && targetIn.ConnectFrom("/target/out") ;
00308 //     }
00309 //   }
00310 #ifdef TWO_BODIES
00311   if(bodySchema.ReadBodySchema(body_data,body_data_size)){
00312       cout<<"got schema"<<endl;
00313       body2->GetArticulatedTree()->Deserialize(body_data,body_data_size);
00314       body2->GetArticulatedTree()->GetJoint()->SetTranslation(display_trans.GetArray());
00315   }
00316 
00317 #endif
00318   m_bProprioActive = proprio_arm.getInputCount()>0 || proprio_head.getInputCount()>0 ;
00319   if(m_bProprioActive){
00320       //     cout<<"active"<<endl;
00321       ok = targetIn.ReadPositionAndOrientation(tarpos.GetArray(),tarpos.GetArray()+3);
00322       if(ok){
00323           target_info = ok>4?2:1;
00324       }
00325       if(ok)cout<<tarpos<<endl;
00326       if(mapping){
00327           ok = proprio_arm.ReadWithMapping(arm_angles,arm_size);
00328       }
00329       else{
00330           ok = proprio_arm.ReadPosition(arm_angles,arm_size);
00331       }
00332            //   cout<<angles[0]<<" "<<angles[1]<<" "<<angles[2]<<" "<<endl;
00333       //    Rearrange(angles);
00334       //     cout<<"ok "<<ok<<endl;
00335       if(ok){
00336           for(int i=0;i<arm_size;i++){
00337               arm_angles[i] *= deg2rad;
00338           }
00339           arm->SetAngles(arm_angles);
00340 #ifdef TWO_BODIES
00341           arm2->SetAngles(arm_angles);
00342 #endif
00343       }
00344       else{
00345           if(!proprio_arm.getInputCount()){
00346               m_bProprioActive=false;
00347           }
00348       }
00349       if(head){
00350           ok = proprio_head.ReadPosition(head_angles,head_size);
00351           if(ok){
00352               for(int i=0;i<head_size;i++){
00353                   //             cout<<" "<<head_angles[i];
00354                   head_angles[i] *= deg2rad;
00355               }
00356               //              cout<<endl;
00357               head->SetAngles(head_angles);
00358 #ifdef TWO_BODIES
00359               head2->SetAngles(head_angles);
00360 #endif
00361 
00362           }
00363       }
00364     }
00365 
00366       //Exit();      
00367 }
00368 
00369 
00370 
00371 #define SIMULATOR_APP_MAIN
00372 
00373 #ifdef SIMULATOR_APP_MAIN
00374 
00375 void usage(){
00376   cout<<"usage: simulator <struct file> <shape file> [--nomapping]"<<endl;
00377 }
00378 
00379 int main(int argc, char *argv[]){
00380 
00381   if(argc != 3 && argc != 4 ){
00382     cout<<argc<<endl;
00383     usage();
00384     return 0;
00385   }
00386   KinematicSimulator app(argc,argv);
00387   app.Init();
00388   app.Run();
00389   return 0;
00390 }
00391 
00392 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:26 2010 for Body_Schema_Learning by  doxygen 1.6.1