Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 548 lines (420 sloc) 14.445 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 $
314e392 fixed rotation of model_actuator, thanks to patch #2927791 from RAZOR
rtv authored
11 // $Revision$
754ffa5 added missing file
rtv authored
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 {
047355a @rtv threaded double-buffer with raytracing and moving happening in different...
authored
187 unsigned int layer = world->GetUpdateCount()%2;
188 UnMap(layer);
189
754ffa5 added missing file
rtv authored
190 double paddle_center_pos = cfg.paddle_position * (0.5 - cfg.paddle_size.y );
191 paddle_left->SetCenterY( paddle_center_pos + cfg.paddle_size.y/2.0 );
192 paddle_right->SetCenterY( 1.0 - paddle_center_pos - cfg.paddle_size.y/2.0);
193
194 double paddle_bottom = cfg.lift_position * (1.0 - cfg.paddle_size.z);
195 double paddle_top = paddle_bottom + cfg.paddle_size.z;
196
197 paddle_left->SetZ( paddle_bottom, paddle_top );
198 paddle_right->SetZ( paddle_bottom, paddle_top );
199
047355a @rtv threaded double-buffer with raytracing and moving happening in different...
authored
200 Map(layer);
754ffa5 added missing file
rtv authored
201 }
202
203
204 void ModelGripper::Update()
205 {
206 float start_paddle_position = cfg.paddle_position;
207 float start_lift_position = cfg.lift_position;
208
209 switch( cmd )
210 {
211 case CMD_NOOP:
212 break;
213
214 case CMD_CLOSE:
215 if( cfg.paddles != PADDLE_CLOSED )
216 {
217 //puts( "closing gripper paddles" );
218 cfg.paddles = PADDLE_CLOSING;
219 }
220 break;
221
222 case CMD_OPEN:
223 if( cfg.paddles != PADDLE_OPEN )
224 {
225 //puts( "opening gripper paddles" );
226 cfg.paddles = PADDLE_OPENING;
227 }
228 break;
229
230 case CMD_UP:
231 if( cfg.lift != LIFT_UP )
232 {
233 //puts( "raising gripper lift" );
234 cfg.lift = LIFT_UPPING;
235 }
236 break;
237
238 case CMD_DOWN:
239 if( cfg.lift != LIFT_DOWN )
240 {
241 //puts( "lowering gripper lift" );
242 cfg.lift = LIFT_DOWNING;
243 }
244 break;
245
246 default:
247 printf( "unknown gripper command %d\n",cmd );
248 }
249
250 // // move the paddles
1fdc189 fixed gripper bug reported by Pante a
rtv authored
251 if( cfg.paddles == PADDLE_OPENING )// && !cfg.paddles_stalled )
754ffa5 added missing file
rtv authored
252 {
253 cfg.paddle_position -= 0.05;
254
255 if( cfg.paddle_position < 0.0 ) // if we're fully open
256 {
257 cfg.paddle_position = 0.0;
258 cfg.paddles = PADDLE_OPEN; // change state
259 }
260
4efd764 cleaning up gripper and adding visualization
rtv authored
261
262 // drop the thing we're carrying
263 if( cfg.gripped &&
754ffa5 added missing file
rtv authored
264 (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
265 {
266 // move it to the new location
4efd764 cleaning up gripper and adding visualization
rtv authored
267 cfg.gripped->SetParent( NULL );
268 cfg.gripped->SetPose( this->GetGlobalPose() );
269 cfg.gripped = NULL;
754ffa5 added missing file
rtv authored
270
271 cfg.close_limit = 1.0;
272 }
273 }
274
1fdc189 fixed gripper bug reported by Pante a
rtv authored
275 else if( cfg.paddles == PADDLE_CLOSING ) //&& !cfg.paddles_stalled )
754ffa5 added missing file
rtv authored
276 {
277 cfg.paddle_position += 0.05;
278 //printf( "paddle position %.2f\n", cfg.paddle_position );
279
280 if( cfg.paddle_position > cfg.close_limit ) // if we're fully closed
281 {
282 cfg.paddle_position = cfg.close_limit;
283 cfg.paddles = PADDLE_CLOSED; // change state
284 }
285 }
286
287 switch( cfg.lift )
288 {
289 case LIFT_DOWNING:
290 cfg.lift_position -= 0.05;
291
292 if( cfg.lift_position < 0.0 ) // if we're fully down
293 {
294 cfg.lift_position = 0.0;
295 cfg.lift = LIFT_DOWN; // change state
296 }
297 break;
298
299 case LIFT_UPPING:
300 cfg.lift_position += 0.05;
301
302 if( cfg.lift_position > 1.0 ) // if we're fully open
303 {
304 cfg.lift_position = 1.0;
305 cfg.lift = LIFT_UP; // change state
306 }
307 break;
308
309 case LIFT_DOWN: // nothing to do for these cases
310 case LIFT_UP:
311 default:
312 break;
313 }
314
315 // if the paddles or lift have changed position
316 if( start_paddle_position != cfg.paddle_position ||
317 start_lift_position != cfg.lift_position )
318 // figure out where the paddles should be
319 PositionPaddles();
320
321
322 UpdateBreakBeams();
323 UpdateContacts();
324
325 Model::Update();
326 }
327
328
329
330 static bool gripper_raytrace_match( Model* hit,
331 Model* finder,
332 const void* dummy )
333 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
334 (void)dummy; // avoid warning about unused var
335
754ffa5 added missing file
rtv authored
336 return( (hit != finder) && hit->vis.gripper_return );
337 // can't use the normal relation check, because we may pick things
338 // up and we must still see them.
339 }
340
341 void ModelGripper::UpdateBreakBeams()
342 {
343 for( unsigned int index=0; index < 2; index++ )
344 {
345 Pose pz;
346
347 // x location of break beam origin
348 double inset = cfg.break_beam_inset[index];
349
350 pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
351
352 // y location of break beam origin
353 pz.y = (1.0 - cfg.paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
354
355 pz.z = 0.0; // TODO
356
357 // break beam local heading
358 pz.a = -M_PI/2.0;
359
360 // break beam max range
361 double bbr =
362 (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 ));
363
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
364 RaytraceResult sample =
754ffa5 added missing file
rtv authored
365 Raytrace( pz, // ray origin
366 bbr, // range
367 gripper_raytrace_match, // match func
368 NULL, // match arg
369 true ); // ztest
370
371 cfg.beam[index] = sample.mod;
372 }
373
374 // autosnatch grabs anything that breaks the inner beam
375 if( cfg.autosnatch )
376 {
377 if( cfg.beam[0] || cfg.beam[1] )
378 cmd = CMD_CLOSE;
379 else
380 cmd = CMD_OPEN;
381 }
382 }
383
384 void ModelGripper::UpdateContacts()
385 {
386 cfg.paddles_stalled = false; // may be changed below
387
388 Pose lpz, rpz;
389
390 // x location of contact sensor origin
391 lpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
392 rpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
393
394 // //double inset = beam ? cfg->inner_break_beam_inset : cfg->outer_break_beam_inset;
395 // //pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
396
397 // y location of paddle beam origin
398
399 lpz.y = (1.0 - cfg.paddle_position) *
400 ((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
401
402 rpz.y = (1.0 - cfg.paddle_position) *
403 -((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
404
405 lpz.z = 0.0; // todo
406 rpz.z = 0.0;
407
408 // paddle beam local heading
409 lpz.a = 0.0;
410 rpz.a = 0.0;
411
412 // paddle beam max range
413 double bbr = cfg.paddle_size.x * geom.size.x;
414
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
415 RaytraceResult leftsample =
754ffa5 added missing file
rtv authored
416 Raytrace( lpz, // ray origin
417 bbr, // range
418 gripper_raytrace_match, // match func
419 NULL, // match arg
420 true ); // ztest
421
422 cfg.contact[0] = leftsample.mod;
423
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
424 RaytraceResult rightsample =
754ffa5 added missing file
rtv authored
425 Raytrace( rpz, // ray origin
426 bbr, // range
427 gripper_raytrace_match, // match func
428 NULL, // match arg
429 true ); // ztest
430
431 cfg.contact[1] = rightsample.mod;
432
433 if( cfg.contact[0] || cfg.contact[1] )
434 {
435 cfg.paddles_stalled = true;;
436
437 // if( lhit && (lhit == rhit) )
438 // {
439 // //puts( "left and right hit same thing" );
440
441 if( cfg.paddles == PADDLE_CLOSING )
442 {
443 Model* hit = cfg.contact[0];
444 if( !hit )
445 hit = cfg.contact[1];
446
4efd764 cleaning up gripper and adding visualization
rtv authored
447 if( cfg.gripped == NULL ) // if we're not carrying something already
754ffa5 added missing file
rtv authored
448 {
449 // get the global pose of the gripper for calculations of the gripped object position
450 // and move it to be right between the paddles
451 Geom hitgeom = hit->GetGeom();
452 //Pose hitgpose = hit->GetGlobalPose();
453
e45b70c combined laser and ranger functionality into ranger - still buggy
Richard Vaughan authored
454 // pose_t pose = {0,0,0};
455 // model_local_to_global( lhit, &pose );
456 // model_global_to_local( mod, &pose );
754ffa5 added missing file
rtv authored
457
458 // // grab the model we hit - very simple grip model for now
459 hit->SetParent( this );
460 hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) );
461
4efd764 cleaning up gripper and adding visualization
rtv authored
462 cfg.gripped = hit;
754ffa5 added missing file
rtv authored
463
464 // // calculate how far closed we can get the paddles now
465 double puckw = hitgeom.size.y;
466 double gripperw = geom.size.y;
910541e removed glib2 dependency (after many years of good service. Thanks GLib....
rtv authored
467 cfg.close_limit = std::max( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
754ffa5 added missing file
rtv authored
468 }
469 }
470 }
471 }
472
473
4efd764 cleaning up gripper and adding visualization
rtv authored
474 void ModelGripper::DataVisualize( Camera* cam )
475 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
476 (void)cam; // avoid warning about unused var
477
4efd764 cleaning up gripper and adding visualization
rtv authored
478 // only draw if someone is using the gripper
479 if( subs < 1 )
480 return;
754ffa5 added missing file
rtv authored
481
4cba3e6 cleaned up vis options and dialog
rtv authored
482 //if( ! showData )
483 //return;
754ffa5 added missing file
rtv authored
484
4efd764 cleaning up gripper and adding visualization
rtv authored
485 // outline the sensor lights in black
486 PushColor( 0,0,0,1.0 ); // black
487 glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
488 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
754ffa5 added missing file
rtv authored
489
4efd764 cleaning up gripper and adding visualization
rtv authored
490 // different x location for each beam
491 double ibbx = (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) - geom.size.x/2.0;
492 double obbx = (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) - geom.size.x/2.0;
493
494 // common y position
495 double invp = 1.0 - cfg.paddle_position;
496 double bby =
497 invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
498
499 // // size of the paddle indicator lights
500 double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
501
502 // paddle break beams
503 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
504 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
505 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
506 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
507
508 // paddle contacts
509 double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) - geom.size.x/2.0;
510 double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y);
511 double plen = cfg.paddle_size.x * geom.size.x;
512 double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y;
513
514 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
515 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
516
517 // if the gripper detects anything, fill the lights in with yellow
518 if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
519 {
520 PushColor( 1,1,0,1.0 ); // yellow
521 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
522
523 if( cfg.contact[0] )
524 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
525
526 if( cfg.contact[1] )
527 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
528
529 if( cfg.beam[0] )
530 {
531 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
532 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
533 }
534
535 if( cfg.beam[1] )
536 {
537 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
538 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
539 }
540
541 PopColor(); // yellow
542 }
543
544 PopColor(); // black
545 }
754ffa5 added missing file
rtv authored
546
547
Something went wrong with that request. Please try again.