model_gripper.cc
Go to the documentation of this file.
00001 
00002 //
00003 // File: model_gripper.cc
00004 // Authors: Richard Vaughan <vaughan@sfu.ca>
00005 //          Doug Blank
00006 // Date: 21 April 2005
00007 //
00008 // CVS info:
00009 //  $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_gripper.c,v $
00010 //  $Author: thjc $
00011 //  $Revision$
00012 //
00014 
00052 #include <sys/time.h>
00053 #include "stage.hh"
00054 #include "worldfile.hh"
00055 using namespace Stg;
00056 
00057 #include "option.hh"
00058 Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true, NULL );
00059 
00060 // TODO - simulate energy use when moving grippers
00061 
00062 ModelGripper::ModelGripper( World* world, 
00063                                                                          Model* parent,
00064                                                                          const std::string& type ) : 
00065   Model( world, parent, type ),  
00066   cfg(), // configured below
00067   cmd( CMD_NOOP )
00068 {
00069   // set up a gripper-specific config structure
00070   cfg.paddle_size.x = 0.66; // proportion of body length that is paddles
00071   cfg.paddle_size.y = 0.1; // proportion of body width that is paddles
00072   cfg.paddle_size.z = 0.4; // proportion of body height that is paddles
00073 
00074   cfg.paddles = PADDLE_OPEN;
00075   cfg.lift = LIFT_DOWN;
00076   cfg.paddle_position = 0.0;
00077   cfg.lift_position = 0.0;
00078   cfg.paddles_stalled = false;
00079   cfg.autosnatch = false;
00080   cfg.gripped = NULL;
00081   cfg.beam[0] = 0;
00082   cfg.beam[1] = 0;
00083   cfg.contact[0] = 0;
00084   cfg.contact[1] = 0;
00085 
00086   // place the break beam sensors at 1/4 and 3/4 the length of the paddle 
00087   cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x;
00088   cfg.break_beam_inset[1] = 1.0/4.0 * cfg.paddle_size.x;
00089   
00090   cfg.close_limit = 1.0;
00091   
00092   SetColor( Color(0.3, 0.3, 0.3, 1.0) );
00093 
00094   FixBlocks();
00095 
00096   // Update() is not reentrant
00097   thread_safe = false;
00098 
00099   // set default size
00100   SetGeom( Geom( Pose(0,0,0,0), Size( 0.2, 0.3, 0.2)));
00101   
00102   PositionPaddles();  
00103 
00104   RegisterOption( &showData );
00105 }
00106 
00107 ModelGripper::~ModelGripper()
00108 {
00109   /* do nothing */
00110 }
00111 
00112 
00113 void ModelGripper::Load()
00114 {
00115   cfg.autosnatch = wf->ReadInt( wf_entity, "autosnatch", cfg.autosnatch );
00116     
00117   // cfg.paddle_size.x = wf->ReadTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x );
00118   // cfg.paddle_size.y = wf->ReadTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y );
00119   // cfg.paddle_size.z = wf->ReadTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
00120 
00121   wf->ReadTuple( wf_entity, "paddle_size", 0, 3, "lll",
00122                  &cfg.paddle_size.x, 
00123                  &cfg.paddle_size.y, 
00124                  &cfg.paddle_size.z );
00125   
00126   //const char* paddles =  wf->ReadTupleString( wf_entity, "paddle_state", 0, NULL );
00127   //const char* lift =     wf->ReadTupleString( wf_entity, "paddle_state", 1, NULL );
00128   
00129   char* paddles = NULL;
00130   char* lift = NULL;
00131   wf->ReadTuple( wf_entity, "paddle_state", 0, 2, "ss", &paddles, &lift );
00132 
00133   if( paddles && strcmp( paddles, "closed" ) == 0 )
00134          {
00135                 cfg.paddle_position = 1.0;
00136                 cfg.paddles = PADDLE_CLOSED;
00137          }
00138 
00139   if( paddles && strcmp( paddles, "open" ) == 0 )
00140          {
00141                 cfg.paddle_position = 0.0;
00142                 cfg.paddles = PADDLE_OPEN;
00143          }
00144 
00145   if( lift && strcmp( lift, "up" ) == 0 )        
00146          {
00147                 cfg.lift_position = 1.0;
00148                 cfg.lift = LIFT_UP;
00149          }
00150 
00151   if( lift && strcmp( lift, "down" ) == 0 )      
00152          {
00153                 cfg.lift_position = 0.0;
00154                 cfg.lift = LIFT_DOWN;
00155          }
00156            
00157   FixBlocks();
00158   
00159   // do this at the end to ensure that the blocks are resize correctly
00160   Model::Load();        
00161 }
00162 
00163 void ModelGripper::Save()
00164 {
00165   Model::Save();
00166 
00167   // wf->WriteTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x );
00168   // wf->WriteTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y );
00169   // wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );    
00170 
00171   wf->WriteTuple( wf_entity, "paddle_size", 0, 3, "lll", 
00172                   cfg.paddle_size.x,
00173                   cfg.paddle_size.y,
00174                   cfg.paddle_size.z );    
00175 
00176   // wf->WriteTupleString( wf_entity, "paddle_state", 0, (cfg.paddles == PADDLE_CLOSED ) ? "closed" : "open" );
00177   // wf->WriteTupleString( wf_entity, "paddle_state", 1, (cfg.lift == LIFT_UP ) ? "up" : "down" );
00178   
00179   wf->WriteTuple( wf_entity, "paddle_state", 0, 2, "ss", 
00180                   (cfg.paddles == PADDLE_CLOSED ) ? "closed" : "open",
00181                   (cfg.lift == LIFT_UP ) ? "up" : "down" );
00182   
00183 }
00184 
00185 void ModelGripper::FixBlocks()
00186 {
00187   // get rid of the default cube
00188   ClearBlocks(); 
00189   
00190   // add three blocks that make the gripper
00191   // base
00192   AddBlockRect( 0, 0, 1.0-cfg.paddle_size.x, 1.0, 1.0 );
00193 
00194   // left (top) paddle
00195   paddle_left = AddBlockRect( 1.0-cfg.paddle_size.x, 0, cfg.paddle_size.x, cfg.paddle_size.y, cfg.paddle_size.z );
00196 
00197   // right (bottom) paddle
00198   paddle_right = AddBlockRect( 1.0-cfg.paddle_size.x, 1.0-cfg.paddle_size.y, cfg.paddle_size.x, cfg.paddle_size.y, cfg.paddle_size.z );
00199 
00200   PositionPaddles();
00201 }
00202 
00203 // Update the blocks that are the gripper's body
00204 void ModelGripper::PositionPaddles()
00205 {
00206         unsigned int layer = world->GetUpdateCount()%2;
00207   UnMap(layer);
00208 
00209   double paddle_center_pos = cfg.paddle_position * (0.5 - cfg.paddle_size.y );
00210   paddle_left->SetCenterY( paddle_center_pos + cfg.paddle_size.y/2.0 );
00211   paddle_right->SetCenterY( 1.0 - paddle_center_pos - cfg.paddle_size.y/2.0);
00212 
00213   double paddle_bottom = cfg.lift_position * (1.0 - cfg.paddle_size.z);
00214   double paddle_top = paddle_bottom +  cfg.paddle_size.z;
00215 
00216   paddle_left->SetZ( paddle_bottom, paddle_top );
00217   paddle_right->SetZ( paddle_bottom, paddle_top );
00218   
00219   Map(layer);
00220  }
00221 
00222 
00223 void ModelGripper::Update()
00224 {   
00225   float start_paddle_position = cfg.paddle_position;
00226   float start_lift_position = cfg.lift_position;
00227 
00228   switch( cmd )
00229     {
00230     case CMD_NOOP:
00231                 break;
00232       
00233     case CMD_CLOSE:     
00234       if( cfg.paddles != PADDLE_CLOSED )
00235                   {
00236                          //puts( "closing gripper paddles" );
00237                          cfg.paddles = PADDLE_CLOSING;
00238                   }
00239       break;
00240       
00241     case CMD_OPEN:
00242       if( cfg.paddles != PADDLE_OPEN )
00243                   {
00244                          //puts( "opening gripper paddles" );      
00245                          cfg.paddles = PADDLE_OPENING;
00246                   }
00247       break;
00248       
00249     case CMD_UP:
00250       if( cfg.lift != LIFT_UP )
00251                   {
00252                          //puts( "raising gripper lift" );      
00253                          cfg.lift = LIFT_UPPING;
00254                   }
00255       break;
00256       
00257     case CMD_DOWN:
00258       if( cfg.lift != LIFT_DOWN )
00259                   {
00260                          //puts( "lowering gripper lift" );      
00261                          cfg.lift = LIFT_DOWNING;
00262                   }      
00263       break;
00264       
00265     default:
00266       printf( "unknown gripper command %d\n",cmd );
00267     }
00268   
00269   //   // move the paddles 
00270   if( cfg.paddles == PADDLE_OPENING )// && !cfg.paddles_stalled  )
00271          {
00272                 cfg.paddle_position -= 0.05;
00273       
00274                 if( cfg.paddle_position < 0.0 ) // if we're fully open
00275                   {
00276                          cfg.paddle_position = 0.0;
00277                          cfg.paddles = PADDLE_OPEN; // change state
00278                   }
00279                 
00280                 
00281                 // drop the thing we're carrying
00282                 if( cfg.gripped &&
00283                          (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
00284                   {
00285                          // move it to the new location
00286                          cfg.gripped->SetParent( NULL );              
00287                          cfg.gripped->SetPose( this->GetGlobalPose() );
00288                          cfg.gripped = NULL;
00289                          
00290                          cfg.close_limit = 1.0;
00291                   }
00292          }
00293 
00294   else if( cfg.paddles == PADDLE_CLOSING ) //&& !cfg.paddles_stalled  )
00295          {
00296                 cfg.paddle_position += 0.05;
00297                 //printf( "paddle position %.2f\n", cfg.paddle_position );
00298       
00299                 if( cfg.paddle_position > cfg.close_limit ) // if we're fully closed
00300                   {
00301                          cfg.paddle_position = cfg.close_limit;
00302                          cfg.paddles = PADDLE_CLOSED; // change state
00303                   }
00304          }
00305   
00306   switch( cfg.lift )
00307          {
00308          case LIFT_DOWNING:
00309                 cfg.lift_position -= 0.05;
00310       
00311       if( cfg.lift_position < 0.0 ) // if we're fully down
00312                   {
00313                          cfg.lift_position = 0.0;
00314                          cfg.lift = LIFT_DOWN; // change state
00315                   }
00316           break;
00317          
00318          case LIFT_UPPING:
00319                 cfg.lift_position += 0.05;
00320       
00321                 if( cfg.lift_position > 1.0 ) // if we're fully open
00322                   {
00323                          cfg.lift_position = 1.0;
00324                          cfg.lift = LIFT_UP; // change state
00325                   }
00326                 break;  
00327 
00328          case LIFT_DOWN: // nothing to do for these cases
00329          case LIFT_UP:
00330          default:
00331                 break;
00332          }
00333   
00334   // if the paddles or lift have changed position
00335   if( start_paddle_position != cfg.paddle_position || 
00336                 start_lift_position != cfg.lift_position )
00337          // figure out where the paddles should be
00338          PositionPaddles();
00339   
00340 
00341   UpdateBreakBeams();
00342   UpdateContacts();
00343   
00344   Model::Update();
00345 }
00346 
00347 
00348         
00349 static bool gripper_raytrace_match( Model* hit, 
00350                                                                                                 Model* finder,
00351                                                                                                 const void* dummy )
00352 {
00353   (void)dummy; // avoid warning about unused var
00354 
00355   return( (hit != finder) && hit->vis.gripper_return );
00356   // can't use the normal relation check, because we may pick things
00357   // up and we must still see them.
00358 }
00359 
00360 void ModelGripper::UpdateBreakBeams() 
00361 {
00362   for( unsigned int index=0; index < 2; index++ )
00363          {  
00364                 Pose pz;
00365                 
00366                 // x location of break beam origin
00367                 double inset = cfg.break_beam_inset[index];
00368                 
00369                 pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
00370                 
00371                 // y location of break beam origin
00372                 pz.y = (1.0 - cfg.paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
00373                 
00374                 pz.z = 0.0; // TODO
00375                 
00376                 // break beam local heading
00377                 pz.a = -M_PI/2.0;
00378                 
00379                 // break beam max range
00380                 double bbr = 
00381                   (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 ));
00382                 
00383                 RaytraceResult sample = 
00384                   Raytrace( pz,  // ray origin
00385                                                 bbr, // range
00386                                                 gripper_raytrace_match, // match func
00387                                                 NULL, // match arg
00388                                                 true ); // ztest
00389                 
00390                 cfg.beam[index] = sample.mod;
00391          }
00392 
00393   // autosnatch grabs anything that breaks the inner beam
00394   if( cfg.autosnatch )
00395          {
00396                 if( cfg.beam[0] || cfg.beam[1] )
00397                   cmd = CMD_CLOSE;
00398                 else
00399                   cmd = CMD_OPEN;
00400          }
00401 }
00402 
00403 void ModelGripper::UpdateContacts()
00404 {
00405   cfg.paddles_stalled = false; // may be changed below
00406 
00407   Pose lpz, rpz;
00408   
00409   // x location of contact sensor origin  
00410   lpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
00411   rpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
00412 
00413 //   //double inset = beam ? cfg->inner_break_beam_inset : cfg->outer_break_beam_inset;
00414 //   //pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
00415 
00416   // y location of paddle beam origin
00417   
00418   lpz.y = (1.0 - cfg.paddle_position) * 
00419          ((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
00420   
00421   rpz.y = (1.0 - cfg.paddle_position) * 
00422          -((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
00423   
00424   lpz.z = 0.0; // todo
00425   rpz.z = 0.0;
00426 
00427   // paddle beam local heading
00428   lpz.a = 0.0;
00429   rpz.a = 0.0;
00430   
00431   // paddle beam max range
00432   double bbr = cfg.paddle_size.x * geom.size.x; 
00433   
00434   RaytraceResult leftsample = 
00435          Raytrace( lpz,  // ray origin
00436                                   bbr, // range
00437                                   gripper_raytrace_match, // match func
00438                                   NULL, // match arg
00439                                   true ); // ztest
00440   
00441   cfg.contact[0] = leftsample.mod;
00442   
00443   RaytraceResult rightsample = 
00444          Raytrace( rpz,  // ray origin
00445                                   bbr, // range
00446                                   gripper_raytrace_match, // match func
00447                                   NULL, // match arg
00448                                   true ); // ztest
00449 
00450   cfg.contact[1] = rightsample.mod;
00451   
00452   if( cfg.contact[0] || cfg.contact[1] )
00453          {
00454                 cfg.paddles_stalled = true;;
00455   
00456                 //   if( lhit && (lhit == rhit) )
00457                 //   {
00458                 //     //puts( "left and right hit same thing" );
00459                 
00460                 if(  cfg.paddles == PADDLE_CLOSING )
00461                   {
00462                          Model* hit = cfg.contact[0];
00463                          if( !hit )
00464                                 hit = cfg.contact[1];
00465                          
00466                          if( cfg.gripped == NULL ) // if we're not carrying something already
00467                                 {
00468                                   // get the global pose of the gripper for calculations of the gripped object position
00469                                   // and move it to be right between the paddles
00470                                   Geom hitgeom = hit->GetGeom();
00471                                   //Pose hitgpose = hit->GetGlobalPose();
00472                                   
00473                                   //       pose_t pose = {0,0,0};
00474                                   //       model_local_to_global( lhit, &pose );
00475                                   //       model_global_to_local( mod, &pose );
00476                                   
00477                                   //       // grab the model we hit - very simple grip model for now
00478                                   hit->SetParent( this );
00479                                   hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) );
00480                                   
00481                                   cfg.gripped = hit;
00482                                   
00483                                   //       // calculate how far closed we can get the paddles now
00484                                   double puckw = hitgeom.size.y;
00485                                   double gripperw = geom.size.y;              
00486                                   cfg.close_limit = std::max( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
00487                                 }
00488                   }
00489          }
00490 }
00491 
00492 
00493 void ModelGripper::DataVisualize( Camera* cam )
00494 {
00495   (void)cam; // avoid warning about unused var
00496 
00497   // only draw if someone is using the gripper
00498   if( subs < 1 )
00499          return;
00500   
00501   //if( ! showData )
00502   //return;
00503 
00504         // outline the sensor lights in black
00505         PushColor( 0,0,0,1.0 ); // black
00506         glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
00507         glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00508 
00509    // different x location for each beam
00510    double ibbx =  (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) - geom.size.x/2.0;
00511    double obbx =  (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) - geom.size.x/2.0;
00512         
00513         // common y position
00514         double invp = 1.0 - cfg.paddle_position;
00515    double bby = 
00516      invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
00517         
00518         //   // size of the paddle indicator lights
00519    double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
00520                 
00521         // paddle break beams
00522         Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
00523         Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
00524         Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
00525         Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
00526         
00527         // paddle contacts
00528         double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) - geom.size.x/2.0;
00529         double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y);
00530         double plen = cfg.paddle_size.x * geom.size.x;
00531         double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y;  
00532         
00533         Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth ); 
00534         Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
00535         
00536         // if the gripper detects anything, fill the lights in with yellow
00537         if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
00538           {
00539                  PushColor( 1,1,0,1.0 ); // yellow
00540                  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00541                  
00542                  if( cfg.contact[0] )
00543                         Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth ); 
00544                  
00545                  if( cfg.contact[1] )
00546                         Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth ); 
00547                  
00548                  if( cfg.beam[0] )
00549                         {
00550                           Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
00551                           Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );  
00552                         }
00553                  
00554                  if( cfg.beam[1] )                        
00555                         {
00556                           Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
00557                           Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );  
00558                         }
00559 
00560                  PopColor(); // yellow    
00561           }
00562                 
00563           PopColor(); // black
00564  }
00565 
00566 


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57