00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00061
00062 ModelGripper::ModelGripper( World* world,
00063 Model* parent,
00064 const std::string& type ) :
00065 Model( world, parent, type ),
00066 cfg(),
00067 cmd( CMD_NOOP )
00068 {
00069
00070 cfg.paddle_size.x = 0.66;
00071 cfg.paddle_size.y = 0.1;
00072 cfg.paddle_size.z = 0.4;
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
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
00097 thread_safe = false;
00098
00099
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
00110 }
00111
00112
00113 void ModelGripper::Load()
00114 {
00115 cfg.autosnatch = wf->ReadInt( wf_entity, "autosnatch", cfg.autosnatch );
00116
00117
00118
00119
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
00127
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
00160 Model::Load();
00161 }
00162
00163 void ModelGripper::Save()
00164 {
00165 Model::Save();
00166
00167
00168
00169
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
00177
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
00188 ClearBlocks();
00189
00190
00191
00192 AddBlockRect( 0, 0, 1.0-cfg.paddle_size.x, 1.0, 1.0 );
00193
00194
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
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
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
00237 cfg.paddles = PADDLE_CLOSING;
00238 }
00239 break;
00240
00241 case CMD_OPEN:
00242 if( cfg.paddles != PADDLE_OPEN )
00243 {
00244
00245 cfg.paddles = PADDLE_OPENING;
00246 }
00247 break;
00248
00249 case CMD_UP:
00250 if( cfg.lift != LIFT_UP )
00251 {
00252
00253 cfg.lift = LIFT_UPPING;
00254 }
00255 break;
00256
00257 case CMD_DOWN:
00258 if( cfg.lift != LIFT_DOWN )
00259 {
00260
00261 cfg.lift = LIFT_DOWNING;
00262 }
00263 break;
00264
00265 default:
00266 printf( "unknown gripper command %d\n",cmd );
00267 }
00268
00269
00270 if( cfg.paddles == PADDLE_OPENING )
00271 {
00272 cfg.paddle_position -= 0.05;
00273
00274 if( cfg.paddle_position < 0.0 )
00275 {
00276 cfg.paddle_position = 0.0;
00277 cfg.paddles = PADDLE_OPEN;
00278 }
00279
00280
00281
00282 if( cfg.gripped &&
00283 (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
00284 {
00285
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 )
00295 {
00296 cfg.paddle_position += 0.05;
00297
00298
00299 if( cfg.paddle_position > cfg.close_limit )
00300 {
00301 cfg.paddle_position = cfg.close_limit;
00302 cfg.paddles = PADDLE_CLOSED;
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 )
00312 {
00313 cfg.lift_position = 0.0;
00314 cfg.lift = LIFT_DOWN;
00315 }
00316 break;
00317
00318 case LIFT_UPPING:
00319 cfg.lift_position += 0.05;
00320
00321 if( cfg.lift_position > 1.0 )
00322 {
00323 cfg.lift_position = 1.0;
00324 cfg.lift = LIFT_UP;
00325 }
00326 break;
00327
00328 case LIFT_DOWN:
00329 case LIFT_UP:
00330 default:
00331 break;
00332 }
00333
00334
00335 if( start_paddle_position != cfg.paddle_position ||
00336 start_lift_position != cfg.lift_position )
00337
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;
00354
00355 return( (hit != finder) && hit->vis.gripper_return );
00356
00357
00358 }
00359
00360 void ModelGripper::UpdateBreakBeams()
00361 {
00362 for( unsigned int index=0; index < 2; index++ )
00363 {
00364 Pose pz;
00365
00366
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
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;
00375
00376
00377 pz.a = -M_PI/2.0;
00378
00379
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,
00385 bbr,
00386 gripper_raytrace_match,
00387 NULL,
00388 true );
00389
00390 cfg.beam[index] = sample.mod;
00391 }
00392
00393
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;
00406
00407 Pose lpz, rpz;
00408
00409
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
00414
00415
00416
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;
00425 rpz.z = 0.0;
00426
00427
00428 lpz.a = 0.0;
00429 rpz.a = 0.0;
00430
00431
00432 double bbr = cfg.paddle_size.x * geom.size.x;
00433
00434 RaytraceResult leftsample =
00435 Raytrace( lpz,
00436 bbr,
00437 gripper_raytrace_match,
00438 NULL,
00439 true );
00440
00441 cfg.contact[0] = leftsample.mod;
00442
00443 RaytraceResult rightsample =
00444 Raytrace( rpz,
00445 bbr,
00446 gripper_raytrace_match,
00447 NULL,
00448 true );
00449
00450 cfg.contact[1] = rightsample.mod;
00451
00452 if( cfg.contact[0] || cfg.contact[1] )
00453 {
00454 cfg.paddles_stalled = true;;
00455
00456
00457
00458
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 )
00467 {
00468
00469
00470 Geom hitgeom = hit->GetGeom();
00471
00472
00473
00474
00475
00476
00477
00478 hit->SetParent( this );
00479 hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) );
00480
00481 cfg.gripped = hit;
00482
00483
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;
00496
00497
00498 if( subs < 1 )
00499 return;
00500
00501
00502
00503
00504
00505 PushColor( 0,0,0,1.0 );
00506 glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
00507 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00508
00509
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
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
00519 double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
00520
00521
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
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
00537 if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
00538 {
00539 PushColor( 1,1,0,1.0 );
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();
00561 }
00562
00563 PopColor();
00564 }
00565
00566