Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 549 lines (424 sloc) 14.378 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 <math.h>
54 #include "stage.hh"
55 #include "worldfile.hh"
56 using namespace Stg;
57
4efd764 cleaning up gripper and adding visualization
rtv authored
58 #include "option.hh"
59 Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true, NULL );
60
754ffa5 added missing file
rtv authored
61 // TODO - simulate energy use when moving grippers
62
63 ModelGripper::ModelGripper( World* world,
64 Model* parent )
65 : Model( world, parent, MODEL_TYPE_GRIPPER ),
66 cfg(), // configured below
67 cmd( CMD_NOOP )
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;
754ffa5 added missing file
rtv authored
81
82 // place the break beam sensors at 1/4 and 3/4 the length of the paddle
83 cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x;
84 cfg.break_beam_inset[1] = 1.0/4.0 * cfg.paddle_size.x;
85
86 cfg.close_limit = 1.0;
87
88 SetColor( stg_color_pack( 0.3, 0.3, 0.3, 0 ));
89
90 FixBlocks();
91
92 // default size
93 Geom geom;
94 geom.pose.x = 0.0;
95 geom.pose.y = 0.0;
96 geom.pose.a = 0.0;
97 geom.size.x = 0.2;
98 geom.size.y = 0.3;
99 geom.size.z = 0.2;
100 SetGeom( geom );
101
102 PositionPaddles();
4efd764 cleaning up gripper and adding visualization
rtv authored
103
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 {
204 // no work to do if we're unsubscribed
205 if( subs < 1 )
206 {
207 Model::Update();
208 return;
209 }
210
211 float start_paddle_position = cfg.paddle_position;
212 float start_lift_position = cfg.lift_position;
213
214 switch( cmd )
215 {
216 case CMD_NOOP:
217 break;
218
219 case CMD_CLOSE:
220 if( cfg.paddles != PADDLE_CLOSED )
221 {
222 //puts( "closing gripper paddles" );
223 cfg.paddles = PADDLE_CLOSING;
224 }
225 break;
226
227 case CMD_OPEN:
228 if( cfg.paddles != PADDLE_OPEN )
229 {
230 //puts( "opening gripper paddles" );
231 cfg.paddles = PADDLE_OPENING;
232 }
233 break;
234
235 case CMD_UP:
236 if( cfg.lift != LIFT_UP )
237 {
238 //puts( "raising gripper lift" );
239 cfg.lift = LIFT_UPPING;
240 }
241 break;
242
243 case CMD_DOWN:
244 if( cfg.lift != LIFT_DOWN )
245 {
246 //puts( "lowering gripper lift" );
247 cfg.lift = LIFT_DOWNING;
248 }
249 break;
250
251 default:
252 printf( "unknown gripper command %d\n",cmd );
253 }
254
255 // // move the paddles
256 if( cfg.paddles == PADDLE_OPENING && !cfg.paddles_stalled )
257 {
258 cfg.paddle_position -= 0.05;
259
260 if( cfg.paddle_position < 0.0 ) // if we're fully open
261 {
262 cfg.paddle_position = 0.0;
263 cfg.paddles = PADDLE_OPEN; // change state
264 }
265
4efd764 cleaning up gripper and adding visualization
rtv authored
266
267 // drop the thing we're carrying
268 if( cfg.gripped &&
754ffa5 added missing file
rtv authored
269 (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit ))
270 {
271 // move it to the new location
4efd764 cleaning up gripper and adding visualization
rtv authored
272 cfg.gripped->SetParent( NULL );
273 cfg.gripped->SetPose( this->GetGlobalPose() );
274 cfg.gripped = NULL;
754ffa5 added missing file
rtv authored
275
276 cfg.close_limit = 1.0;
277 }
278 }
279
280 else if( cfg.paddles == PADDLE_CLOSING && !cfg.paddles_stalled )
281 {
282 cfg.paddle_position += 0.05;
283 //printf( "paddle position %.2f\n", cfg.paddle_position );
284
285 if( cfg.paddle_position > cfg.close_limit ) // if we're fully closed
286 {
287 cfg.paddle_position = cfg.close_limit;
288 cfg.paddles = PADDLE_CLOSED; // change state
289 }
290 }
291
292 switch( cfg.lift )
293 {
294 case LIFT_DOWNING:
295 cfg.lift_position -= 0.05;
296
297 if( cfg.lift_position < 0.0 ) // if we're fully down
298 {
299 cfg.lift_position = 0.0;
300 cfg.lift = LIFT_DOWN; // change state
301 }
302 break;
303
304 case LIFT_UPPING:
305 cfg.lift_position += 0.05;
306
307 if( cfg.lift_position > 1.0 ) // if we're fully open
308 {
309 cfg.lift_position = 1.0;
310 cfg.lift = LIFT_UP; // change state
311 }
312 break;
313
314 case LIFT_DOWN: // nothing to do for these cases
315 case LIFT_UP:
316 default:
317 break;
318 }
319
320 // if the paddles or lift have changed position
321 if( start_paddle_position != cfg.paddle_position ||
322 start_lift_position != cfg.lift_position )
323 // figure out where the paddles should be
324 PositionPaddles();
325
326
327 UpdateBreakBeams();
328 UpdateContacts();
329
330 Model::Update();
331 }
332
333
334
335 static bool gripper_raytrace_match( Model* hit,
336 Model* finder,
337 const void* dummy )
338 {
339 return( (hit != finder) && hit->vis.gripper_return );
340 // can't use the normal relation check, because we may pick things
341 // up and we must still see them.
342 }
343
344 void ModelGripper::UpdateBreakBeams()
345 {
346 for( unsigned int index=0; index < 2; index++ )
347 {
348 Pose pz;
349
350 // x location of break beam origin
351 double inset = cfg.break_beam_inset[index];
352
353 pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
354
355 // y location of break beam origin
356 pz.y = (1.0 - cfg.paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
357
358 pz.z = 0.0; // TODO
359
360 // break beam local heading
361 pz.a = -M_PI/2.0;
362
363 // break beam max range
364 double bbr =
365 (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 ));
366
367 stg_raytrace_result_t sample =
368 Raytrace( pz, // ray origin
369 bbr, // range
370 gripper_raytrace_match, // match func
371 NULL, // match arg
372 true ); // ztest
373
374 cfg.beam[index] = sample.mod;
375 }
376
377 // autosnatch grabs anything that breaks the inner beam
378 if( cfg.autosnatch )
379 {
380 if( cfg.beam[0] || cfg.beam[1] )
381 cmd = CMD_CLOSE;
382 else
383 cmd = CMD_OPEN;
384 }
385 }
386
387 void ModelGripper::UpdateContacts()
388 {
389 cfg.paddles_stalled = false; // may be changed below
390
391 Pose lpz, rpz;
392
393 // x location of contact sensor origin
394 lpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
395 rpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ;
396
397 // //double inset = beam ? cfg->inner_break_beam_inset : cfg->outer_break_beam_inset;
398 // //pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0;
399
400 // y location of paddle beam origin
401
402 lpz.y = (1.0 - cfg.paddle_position) *
403 ((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
404
405 rpz.y = (1.0 - cfg.paddle_position) *
406 -((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y));
407
408 lpz.z = 0.0; // todo
409 rpz.z = 0.0;
410
411 // paddle beam local heading
412 lpz.a = 0.0;
413 rpz.a = 0.0;
414
415 // paddle beam max range
416 double bbr = cfg.paddle_size.x * geom.size.x;
417
418 stg_raytrace_result_t leftsample =
419 Raytrace( lpz, // ray origin
420 bbr, // range
421 gripper_raytrace_match, // match func
422 NULL, // match arg
423 true ); // ztest
424
425 cfg.contact[0] = leftsample.mod;
426
427 stg_raytrace_result_t rightsample =
428 Raytrace( rpz, // ray origin
429 bbr, // range
430 gripper_raytrace_match, // match func
431 NULL, // match arg
432 true ); // ztest
433
434 cfg.contact[1] = rightsample.mod;
435
436 if( cfg.contact[0] || cfg.contact[1] )
437 {
438 cfg.paddles_stalled = true;;
439
440 // if( lhit && (lhit == rhit) )
441 // {
442 // //puts( "left and right hit same thing" );
443
444 if( cfg.paddles == PADDLE_CLOSING )
445 {
446 Model* hit = cfg.contact[0];
447 if( !hit )
448 hit = cfg.contact[1];
449
4efd764 cleaning up gripper and adding visualization
rtv authored
450 if( cfg.gripped == NULL ) // if we're not carrying something already
754ffa5 added missing file
rtv authored
451 {
452 // get the global pose of the gripper for calculations of the gripped object position
453 // and move it to be right between the paddles
454 Geom hitgeom = hit->GetGeom();
455 //Pose hitgpose = hit->GetGlobalPose();
456
457 // stg_pose_t pose = {0,0,0};
458 // stg_model_local_to_global( lhit, &pose );
459 // stg_model_global_to_local( mod, &pose );
460
461 // // grab the model we hit - very simple grip model for now
462 hit->SetParent( this );
463 hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) );
464
4efd764 cleaning up gripper and adding visualization
rtv authored
465 cfg.gripped = hit;
754ffa5 added missing file
rtv authored
466
467 // // calculate how far closed we can get the paddles now
468 double puckw = hitgeom.size.y;
469 double gripperw = geom.size.y;
470 cfg.close_limit = MAX( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
471 }
472 }
473 }
474 }
475
476
4efd764 cleaning up gripper and adding visualization
rtv authored
477 void ModelGripper::DataVisualize( Camera* cam )
478 {
479 // only draw if someone is using the gripper
480 if( subs < 1 )
481 return;
754ffa5 added missing file
rtv authored
482
4efd764 cleaning up gripper and adding visualization
rtv authored
483 if( ! showData )
484 return;
754ffa5 added missing file
rtv authored
485
4efd764 cleaning up gripper and adding visualization
rtv authored
486 // outline the sensor lights in black
487 PushColor( 0,0,0,1.0 ); // black
488 glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
489 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
754ffa5 added missing file
rtv authored
490
4efd764 cleaning up gripper and adding visualization
rtv authored
491 // different x location for each beam
492 double ibbx = (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) - geom.size.x/2.0;
493 double obbx = (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) - geom.size.x/2.0;
494
495 // common y position
496 double invp = 1.0 - cfg.paddle_position;
497 double bby =
498 invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
499
500 // // size of the paddle indicator lights
501 double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
502
503 // paddle break beams
504 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
505 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
506 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
507 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
508
509 // paddle contacts
510 double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) - geom.size.x/2.0;
511 double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y);
512 double plen = cfg.paddle_size.x * geom.size.x;
513 double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y;
514
515 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
516 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
517
518 // if the gripper detects anything, fill the lights in with yellow
519 if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
520 {
521 PushColor( 1,1,0,1.0 ); // yellow
522 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
523
524 if( cfg.contact[0] )
525 Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
526
527 if( cfg.contact[1] )
528 Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
529
530 if( cfg.beam[0] )
531 {
532 Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
533 Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
534 }
535
536 if( cfg.beam[1] )
537 {
538 Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
539 Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
540 }
541
542 PopColor(); // yellow
543 }
544
545 PopColor(); // black
546 }
754ffa5 added missing file
rtv authored
547
548
Something went wrong with that request. Please try again.