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