Skip to content
This repository
Newer
Older
100644 548 lines (424 sloc) 14.446 kb
754ffa58 » rtv
2009-02-13 added missing file
1 ///////////////////////////////////////////////////////////////////////////
2 //
3 // File: model_gripper.cc
4 // Authors: Richard Vaughan <vaughan@sfu.ca>
5 // Doug Blank
6 // Date: 21 April 2005
7 //
8 // CVS info:
9 // $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_gripper.c,v $
10 // $Author: thjc $
11 // $Revision: 1.26.8.1 $
12 //
13 ///////////////////////////////////////////////////////////////////////////
14
15 /**
16 @ingroup model
17 @defgroup model_gripper Gripper model
18
19 The ranger model simulates a simple two-fingered gripper with two
20 internal break-beams, similar to the the Pioneer gripper.
21
22 <h2>Worldfile properties</h2>
23
24 @par Summary and default values
25
26 @verbatim
27 gripper
28 (
29 # gripper properties
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
30 paddle_size [ 0.66 0.1 0.4 ]
31 paddle_state [ "open" "down" ]
32 autosnatch 0
754ffa58 » rtv
2009-02-13 added missing file
33
34 # model properties
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
35 size [ 0.2 0.3 0.2 ]
754ffa58 » rtv
2009-02-13 added missing file
36 )
37 @endverbatim
38
39 @par Notes
40
41 @par Details
42
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
43 - autosnatch < 0 or 1>\n
44 iff 1, gripper will close automatically when break beams are broken
45 - paddle_size [ <float x> <float y < float z> ]\n
46 Gripper paddle size as a proportion of the model's body size (0.0 to 1.0)
47 - paddle_state [ <string open/close> <string up/down> ]\n
48 Gripper arms state, either "open" or "closed", and lift state, either "up" or "down"
754ffa58 » rtv
2009-02-13 added missing file
49 */
50
51
52 #include <sys/time.h>
53 #include "stage.hh"
54 #include "worldfile.hh"
55 using namespace Stg;
56
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
57 #include "option.hh"
58 Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true, NULL );
59
754ffa58 » rtv
2009-02-13 added missing file
60 // TODO - simulate energy use when moving grippers
61
62 ModelGripper::ModelGripper( World* world,
5a63ae0b » rtv
2009-08-19 removed toby's over-specific models and player interfaces. simplified…
63 Model* parent,
64 const std::string& type ) :
65 Model( world, parent, type ),
66 cfg(), // configured below
67 cmd( CMD_NOOP )
754ffa58 » rtv
2009-02-13 added missing file
68 {
69 // set up a gripper-specific config structure
70 cfg.paddle_size.x = 0.66; // proportion of body length that is paddles
71 cfg.paddle_size.y = 0.1; // proportion of body width that is paddles
72 cfg.paddle_size.z = 0.4; // proportion of body height that is paddles
73
74 cfg.paddles = PADDLE_OPEN;
75 cfg.lift = LIFT_DOWN;
76 cfg.paddle_position = 0.0;
77 cfg.lift_position = 0.0;
78 cfg.paddles_stalled = false;
79 cfg.autosnatch = false;
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
80 cfg.gripped = NULL;
4cba3e6b » rtv
2009-02-17 cleaned up vis options and dialog
81 cfg.beam[0] = 0;
82 cfg.beam[1] = 0;
83 cfg.contact[0] = 0;
84 cfg.contact[1] = 0;
754ffa58 » rtv
2009-02-13 added missing file
85
86 // place the break beam sensors at 1/4 and 3/4 the length of the paddle
87 cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x;
88 cfg.break_beam_inset[1] = 1.0/4.0 * cfg.paddle_size.x;
89
90 cfg.close_limit = 1.0;
91
4ccc5cd7 » rtv
2009-07-03 replaced stg_color_t with Color class and added static named construc…
92 SetColor( Color(0.3, 0.3, 0.3, 1.0) );
754ffa58 » rtv
2009-02-13 added missing file
93
94 FixBlocks();
95
5b95f288 » rtv
2009-03-03 fixed thread pool implementation
96 // Update() is not reentrant
97 thread_safe = false;
98
70cc7724 » rtv
2009-06-23 moved all model-specific types into their class scope - may break Tob…
99 // set default size
100 SetGeom( Geom( Pose(0,0,0,0), Size( 0.2, 0.3, 0.2)));
101
754ffa58 » rtv
2009-02-13 added missing file
102 PositionPaddles();
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
103
4cba3e6b » rtv
2009-02-17 cleaned up vis options and dialog
104 RegisterOption( &showData );
754ffa58 » rtv
2009-02-13 added missing file
105 }
106
107 ModelGripper::~ModelGripper()
108 {
109 /* do nothing */
110 }
111
112
113 void ModelGripper::Load()
114 {
115 cfg.autosnatch = wf->ReadInt( wf_entity, "autosnatch", cfg.autosnatch );
116
117 cfg.paddle_size.x = wf->ReadTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x );
118 cfg.paddle_size.y = wf->ReadTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y );
119 cfg.paddle_size.z = wf->ReadTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
120
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
121 const char* paddles = wf->ReadTupleString( wf_entity, "paddle_state", 0, NULL );
122 const char* lift = wf->ReadTupleString( wf_entity, "paddle_state", 1, NULL );
754ffa58 » rtv
2009-02-13 added missing file
123
124 if( paddles && strcmp( paddles, "closed" ) == 0 )
125 {
126 cfg.paddle_position = 1.0;
127 cfg.paddles = PADDLE_CLOSED;
128 }
129
130 if( paddles && strcmp( paddles, "open" ) == 0 )
131 {
132 cfg.paddle_position = 0.0;
133 cfg.paddles = PADDLE_OPEN;
134 }
135
136 if( lift && strcmp( lift, "up" ) == 0 )
137 {
138 cfg.lift_position = 1.0;
139 cfg.lift = LIFT_UP;
140 }
141
142 if( lift && strcmp( lift, "down" ) == 0 )
143 {
144 cfg.lift_position = 0.0;
145 cfg.lift = LIFT_DOWN;
146 }
147
148 FixBlocks();
149
150 // do this at the end to ensure that the blocks are resize correctly
151 Model::Load();
152 }
153
154 void ModelGripper::Save()
155 {
156 Model::Save();
157
158 wf->WriteTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x );
159 wf->WriteTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y );
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
160 wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
754ffa58 » rtv
2009-02-13 added missing file
161
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
162 wf->WriteTupleString( wf_entity, "paddle_state", 0, (cfg.paddles == PADDLE_CLOSED ) ? "closed" : "open" );
163 wf->WriteTupleString( wf_entity, "paddle_state", 1, (cfg.lift == LIFT_UP ) ? "up" : "down" );
754ffa58 » rtv
2009-02-13 added missing file
164 }
165
166 void ModelGripper::FixBlocks()
167 {
168 // get rid of the default cube
169 ClearBlocks();
170
171 // add three blocks that make the gripper
172 // base
173 AddBlockRect( 0, 0, 1.0-cfg.paddle_size.x, 1.0, 1.0 );
174
175 // left (top) paddle
176 paddle_left = AddBlockRect( 1.0-cfg.paddle_size.x, 0, cfg.paddle_size.x, cfg.paddle_size.y, cfg.paddle_size.z );
177
178 // right (bottom) paddle
179 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 );
180
181 PositionPaddles();
182 }
183
184 // Update the blocks that are the gripper's body
185 void ModelGripper::PositionPaddles()
186 {
187 UnMap();
188 double paddle_center_pos = cfg.paddle_position * (0.5 - cfg.paddle_size.y );
189 paddle_left->SetCenterY( paddle_center_pos + cfg.paddle_size.y/2.0 );
190 paddle_right->SetCenterY( 1.0 - paddle_center_pos - cfg.paddle_size.y/2.0);
191
192 double paddle_bottom = cfg.lift_position * (1.0 - cfg.paddle_size.z);
193 double paddle_top = paddle_bottom + cfg.paddle_size.z;
194
195 paddle_left->SetZ( paddle_bottom, paddle_top );
196 paddle_right->SetZ( paddle_bottom, paddle_top );
197
198 Map();
199 }
200
201
202 void ModelGripper::Update()
203 {
5b95f288 » rtv
2009-03-03 fixed thread pool implementation
204 //return;
205
754ffa58 » rtv
2009-02-13 added missing file
206 // no work to do if we're unsubscribed
207 if( subs < 1 )
208 {
209 Model::Update();
210 return;
211 }
212
213 float start_paddle_position = cfg.paddle_position;
214 float start_lift_position = cfg.lift_position;
215
216 switch( cmd )
217 {
218 case CMD_NOOP:
219 break;
220
221 case CMD_CLOSE:
222 if( cfg.paddles != PADDLE_CLOSED )
223 {
224 //puts( "closing gripper paddles" );
225 cfg.paddles = PADDLE_CLOSING;
226 }
227 break;
228
229 case CMD_OPEN:
230 if( cfg.paddles != PADDLE_OPEN )
231 {
232 //puts( "opening gripper paddles" );
233 cfg.paddles = PADDLE_OPENING;
234 }
235 break;
236
237 case CMD_UP:
238 if( cfg.lift != LIFT_UP )
239 {
240 //puts( "raising gripper lift" );
241 cfg.lift = LIFT_UPPING;
242 }
243 break;
244
245 case CMD_DOWN:
246 if( cfg.lift != LIFT_DOWN )
247 {
248 //puts( "lowering gripper lift" );
249 cfg.lift = LIFT_DOWNING;
250 }
251 break;
252
253 default:
254 printf( "unknown gripper command %d\n",cmd );
255 }
256
257 // // move the paddles
1fdc189b » rtv
2009-02-24 fixed gripper bug reported by Pante a
258 if( cfg.paddles == PADDLE_OPENING )// && !cfg.paddles_stalled )
754ffa58 » rtv
2009-02-13 added missing file
259 {
260 cfg.paddle_position -= 0.05;
261
262 if( cfg.paddle_position < 0.0 ) // if we're fully open
263 {
264 cfg.paddle_position = 0.0;
265 cfg.paddles = PADDLE_OPEN; // change state
266 }
267
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
268
269 // drop the thing we're carrying
270 if( cfg.gripped &&
754ffa58 » rtv
2009-02-13 added missing file
271 (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
272 {
273 // move it to the new location
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
274 cfg.gripped->SetParent( NULL );
275 cfg.gripped->SetPose( this->GetGlobalPose() );
276 cfg.gripped = NULL;
754ffa58 » rtv
2009-02-13 added missing file
277
278 cfg.close_limit = 1.0;
279 }
280 }
281
1fdc189b » rtv
2009-02-24 fixed gripper bug reported by Pante a
282 else if( cfg.paddles == PADDLE_CLOSING ) //&& !cfg.paddles_stalled )
754ffa58 » rtv
2009-02-13 added missing file
283 {
284 cfg.paddle_position += 0.05;
285 //printf( "paddle position %.2f\n", cfg.paddle_position );
286
287 if( cfg.paddle_position > cfg.close_limit ) // if we're fully closed
288 {
289 cfg.paddle_position = cfg.close_limit;
290 cfg.paddles = PADDLE_CLOSED; // change state
291 }
292 }
293
294 switch( cfg.lift )
295 {
296 case LIFT_DOWNING:
297 cfg.lift_position -= 0.05;
298
299 if( cfg.lift_position < 0.0 ) // if we're fully down
300 {
301 cfg.lift_position = 0.0;
302 cfg.lift = LIFT_DOWN; // change state
303 }
304 break;
305
306 case LIFT_UPPING:
307 cfg.lift_position += 0.05;
308
309 if( cfg.lift_position > 1.0 ) // if we're fully open
310 {
311 cfg.lift_position = 1.0;
312 cfg.lift = LIFT_UP; // change state
313 }
314 break;
315
316 case LIFT_DOWN: // nothing to do for these cases
317 case LIFT_UP:
318 default:
319 break;
320 }
321
322 // if the paddles or lift have changed position
323 if( start_paddle_position != cfg.paddle_position ||
324 start_lift_position != cfg.lift_position )
325 // figure out where the paddles should be
326 PositionPaddles();
327
328
329 UpdateBreakBeams();
330 UpdateContacts();
331
332 Model::Update();
333 }
334
335
336
337 static bool gripper_raytrace_match( Model* hit,
338 Model* finder,
339 const void* dummy )
340 {
341 return( (hit != finder) && hit->vis.gripper_return );
342 // can't use the normal relation check, because we may pick things
343 // up and we must still see them.
344 }
345
346 void ModelGripper::UpdateBreakBeams()
347 {
348 for( unsigned int index=0; index < 2; index++ )
349 {
350 Pose pz;
351
352 // x location of break beam origin
353 double inset = cfg.break_beam_inset[index];
354
355 pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
356
357 // y location of break beam origin
358 pz.y = (1.0 - cfg.paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
359
360 pz.z = 0.0; // TODO
361
362 // break beam local heading
363 pz.a = -M_PI/2.0;
364
365 // break beam max range
366 double bbr =
367 (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 ));
368
369 stg_raytrace_result_t sample =
370 Raytrace( pz, // ray origin
371 bbr, // range
372 gripper_raytrace_match, // match func
373 NULL, // match arg
374 true ); // ztest
375
376 cfg.beam[index] = sample.mod;
377 }
378
379 // autosnatch grabs anything that breaks the inner beam
380 if( cfg.autosnatch )
381 {
382 if( cfg.beam[0] || cfg.beam[1] )
383 cmd = CMD_CLOSE;
384 else
385 cmd = CMD_OPEN;
386 }
387 }
388
389 void ModelGripper::UpdateContacts()
390 {
391 cfg.paddles_stalled = false; // may be changed below
392
393 Pose lpz, rpz;
394
395 // x location of contact sensor origin
396 lpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
397 rpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
398
399 // //double inset = beam ? cfg->inner_break_beam_inset : cfg->outer_break_beam_inset;
400 // //pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
401
402 // y location of paddle beam origin
403
404 lpz.y = (1.0 - cfg.paddle_position) *
405 ((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
406
407 rpz.y = (1.0 - cfg.paddle_position) *
408 -((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
409
410 lpz.z = 0.0; // todo
411 rpz.z = 0.0;
412
413 // paddle beam local heading
414 lpz.a = 0.0;
415 rpz.a = 0.0;
416
417 // paddle beam max range
418 double bbr = cfg.paddle_size.x * geom.size.x;
419
420 stg_raytrace_result_t leftsample =
421 Raytrace( lpz, // ray origin
422 bbr, // range
423 gripper_raytrace_match, // match func
424 NULL, // match arg
425 true ); // ztest
426
427 cfg.contact[0] = leftsample.mod;
428
429 stg_raytrace_result_t rightsample =
430 Raytrace( rpz, // ray origin
431 bbr, // range
432 gripper_raytrace_match, // match func
433 NULL, // match arg
434 true ); // ztest
435
436 cfg.contact[1] = rightsample.mod;
437
438 if( cfg.contact[0] || cfg.contact[1] )
439 {
440 cfg.paddles_stalled = true;;
441
442 // if( lhit && (lhit == rhit) )
443 // {
444 // //puts( "left and right hit same thing" );
445
446 if( cfg.paddles == PADDLE_CLOSING )
447 {
448 Model* hit = cfg.contact[0];
449 if( !hit )
450 hit = cfg.contact[1];
451
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
452 if( cfg.gripped == NULL ) // if we're not carrying something already
754ffa58 » rtv
2009-02-13 added missing file
453 {
454 // get the global pose of the gripper for calculations of the gripped object position
455 // and move it to be right between the paddles
456 Geom hitgeom = hit->GetGeom();
457 //Pose hitgpose = hit->GetGlobalPose();
458
459 // stg_pose_t pose = {0,0,0};
460 // stg_model_local_to_global( lhit, &pose );
461 // stg_model_global_to_local( mod, &pose );
462
463 // // grab the model we hit - very simple grip model for now
464 hit->SetParent( this );
465 hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) );
466
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
467 cfg.gripped = hit;
754ffa58 » rtv
2009-02-13 added missing file
468
469 // // calculate how far closed we can get the paddles now
470 double puckw = hitgeom.size.y;
471 double gripperw = geom.size.y;
910541e2 » rtv
2009-07-22 removed glib2 dependency (after many years of good service. Thanks GL…
472 cfg.close_limit = std::max( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
754ffa58 » rtv
2009-02-13 added missing file
473 }
474 }
475 }
476 }
477
478
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
479 void ModelGripper::DataVisualize( Camera* cam )
480 {
481 // only draw if someone is using the gripper
482 if( subs < 1 )
483 return;
754ffa58 » rtv
2009-02-13 added missing file
484
4cba3e6b » rtv
2009-02-17 cleaned up vis options and dialog
485 //if( ! showData )
486 //return;
754ffa58 » rtv
2009-02-13 added missing file
487
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
488 // outline the sensor lights in black
489 PushColor( 0,0,0,1.0 ); // black
490 glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
491 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
754ffa58 » rtv
2009-02-13 added missing file
492
4efd7641 » rtv
2009-02-15 cleaning up gripper and adding visualization
493 // different x location for each beam
494 double ibbx = (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) - geom.size.x/2.0;
495 double obbx = (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) - geom.size.x/2.0;
496
497 // common y position
498 double invp = 1.0 - cfg.paddle_position;
499 double bby =
500 invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
501
502 // // size of the paddle indicator lights
503 double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
504
505 // paddle break beams
506 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
507 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
508 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
509 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
510
511 // paddle contacts
512 double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) - geom.size.x/2.0;
513 double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y);
514 double plen = cfg.paddle_size.x * geom.size.x;
515 double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y;
516
517 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
518 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
519
520 // if the gripper detects anything, fill the lights in with yellow
521 if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
522 {
523 PushColor( 1,1,0,1.0 ); // yellow
524 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
525
526 if( cfg.contact[0] )
527 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
528
529 if( cfg.contact[1] )
530 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
531
532 if( cfg.beam[0] )
533 {
534 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
535 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
536 }
537
538 if( cfg.beam[1] )
539 {
540 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
541 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
542 }
543
544 PopColor(); // yellow
545 }
546
547 PopColor(); // black
548 }
754ffa58 » rtv
2009-02-13 added missing file
549
550
Something went wrong with that request. Please try again.