Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 551 lines (424 sloc) 14.446 kb
754ffa5 added missing file
rtv authored
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
4efd764 cleaning up gripper and adding visualization
rtv authored
30 paddle_size [ 0.66 0.1 0.4 ]
31 paddle_state [ "open" "down" ]
32 autosnatch 0
754ffa5 added missing file
rtv authored
33
34 # model properties
4efd764 cleaning up gripper and adding visualization
rtv authored
35 size [ 0.2 0.3 0.2 ]
754ffa5 added missing file
rtv authored
36 )
37 @endverbatim
38
39 @par Notes
40
41 @par Details
42
4efd764 cleaning up gripper and adding visualization
rtv authored
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"
754ffa5 added missing file
rtv authored
49 */
50
51
52 #include <sys/time.h>
53 #include "stage.hh"
54 #include "worldfile.hh"
55 using namespace Stg;
56
4efd764 cleaning up gripper and adding visualization
rtv authored
57 #include "option.hh"
58 Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true, NULL );
59
754ffa5 added missing file
rtv authored
60 // TODO - simulate energy use when moving grippers
61
62 ModelGripper::ModelGripper( World* world,
5a63ae0 removed toby's over-specific models and player interfaces. simplified mo...
rtv authored
63 Model* parent,
64 const std::string& type ) :
65 Model( world, parent, type ),
66 cfg(), // configured below
67 cmd( CMD_NOOP )
754ffa5 added missing file
rtv authored
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;
4efd764 cleaning up gripper and adding visualization
rtv authored
80 cfg.gripped = NULL;
4cba3e6 cleaned up vis options and dialog
rtv authored
81 cfg.beam[0] = 0;
82 cfg.beam[1] = 0;
83 cfg.contact[0] = 0;
84 cfg.contact[1] = 0;
754ffa5 added missing file
rtv authored
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
4ccc5cd replaced stg_color_t with Color class and added static named constructor...
rtv authored
92 SetColor( Color(0.3, 0.3, 0.3, 1.0) );
754ffa5 added missing file
rtv authored
93
94 FixBlocks();
95
5b95f28 fixed thread pool implementation
rtv authored
96 // Update() is not reentrant
97 thread_safe = false;
98
70cc772 moved all model-specific types into their class scope - may break Toby's...
rtv authored
99 // set default size
100 SetGeom( Geom( Pose(0,0,0,0), Size( 0.2, 0.3, 0.2)));
101
754ffa5 added missing file
rtv authored
102 PositionPaddles();
4efd764 cleaning up gripper and adding visualization
rtv authored
103
4cba3e6 cleaned up vis options and dialog
rtv authored
104 RegisterOption( &showData );
754ffa5 added missing file
rtv authored
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
4efd764 cleaning up gripper and adding visualization
rtv authored
121 const char* paddles = wf->ReadTupleString( wf_entity, "paddle_state", 0, NULL );
122 const char* lift = wf->ReadTupleString( wf_entity, "paddle_state", 1, NULL );
754ffa5 added missing file
rtv authored
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 );
4efd764 cleaning up gripper and adding visualization
rtv authored
160 wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
754ffa5 added missing file
rtv authored
161
4efd764 cleaning up gripper and adding visualization
rtv authored
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" );
754ffa5 added missing file
rtv authored
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 {
5b95f28 fixed thread pool implementation
rtv authored
204 //return;
205
754ffa5 added missing file
rtv authored
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
1fdc189 fixed gripper bug reported by Pante a
rtv authored
258 if( cfg.paddles == PADDLE_OPENING )// && !cfg.paddles_stalled )
754ffa5 added missing file
rtv authored
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
4efd764 cleaning up gripper and adding visualization
rtv authored
268
269 // drop the thing we're carrying
270 if( cfg.gripped &&
754ffa5 added missing file
rtv authored
271 (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
272 {
273 // move it to the new location
4efd764 cleaning up gripper and adding visualization
rtv authored
274 cfg.gripped->SetParent( NULL );
275 cfg.gripped->SetPose( this->GetGlobalPose() );
276 cfg.gripped = NULL;
754ffa5 added missing file
rtv authored
277
278 cfg.close_limit = 1.0;
279 }
280 }
281
1fdc189 fixed gripper bug reported by Pante a
rtv authored
282 else if( cfg.paddles == PADDLE_CLOSING ) //&& !cfg.paddles_stalled )
754ffa5 added missing file
rtv authored
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
4efd764 cleaning up gripper and adding visualization
rtv authored
452 if( cfg.gripped == NULL ) // if we're not carrying something already
754ffa5 added missing file
rtv authored
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
4efd764 cleaning up gripper and adding visualization
rtv authored
467 cfg.gripped = hit;
754ffa5 added missing file
rtv authored
468
469 // // calculate how far closed we can get the paddles now
470 double puckw = hitgeom.size.y;
471 double gripperw = geom.size.y;
910541e removed glib2 dependency (after many years of good service. Thanks GLib....
rtv authored
472 cfg.close_limit = std::max( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
754ffa5 added missing file
rtv authored
473 }
474 }
475 }
476 }
477
478
4efd764 cleaning up gripper and adding visualization
rtv authored
479 void ModelGripper::DataVisualize( Camera* cam )
480 {
481 // only draw if someone is using the gripper
482 if( subs < 1 )
483 return;
754ffa5 added missing file
rtv authored
484
4cba3e6 cleaned up vis options and dialog
rtv authored
485 //if( ! showData )
486 //return;
754ffa5 added missing file
rtv authored
487
4efd764 cleaning up gripper and adding visualization
rtv authored
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 );
754ffa5 added missing file
rtv authored
492
4efd764 cleaning up gripper and adding visualization
rtv authored
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 }
754ffa5 added missing file
rtv authored
549
550
Something went wrong with that request. Please try again.