From: <rt...@us...> - 2009-02-13 09:43:24
|
Revision: 7331 http://playerstage.svn.sourceforge.net/playerstage/?rev=7331&view=rev Author: rtv Date: 2009-02-13 09:43:20 +0000 (Fri, 13 Feb 2009) Log Message: ----------- basic gripper working - needs thorough testing Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 09:43:20 UTC (rev 7331) @@ -59,21 +59,8 @@ double charger_range; double charger_heading; nav_mode_t mode; + bool at_dest; - static int LaserUpdate( ModelLaser* mod, Robot* robot ); - static int PositionUpdate( ModelPosition* mod, Robot* robot ); - static int FiducialUpdate( ModelFiducial* mod, Robot* robot ); - static int BlobFinderUpdate( ModelBlobfinder* mod, Robot* robot ); - - void Dock(); - void Work(); - void UnDock(); - bool ObstacleAvoid(); - - // predicate that indicates if we need to charge - bool Hungry(); - bool Full(); - public: Robot( ModelPosition* pos, Model* source, @@ -94,7 +81,8 @@ charger_bearing(0), charger_range(0), charger_heading(0), - mode(MODE_WORK) + mode(MODE_WORK), + at_dest( false ) { // need at least these models to get any work done // (pos must be good, as we used it in the initialization list) @@ -104,381 +92,421 @@ // PositionUpdate() checks to see if we reached source or sink pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); + pos->Subscribe(); // LaserUpdate() controls the robot, by reading from laser and // writing to position laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this ); + laser->Subscribe(); - // trivial demos - - if( fiducial ) // optional - fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); - + fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); + fiducial->Subscribe(); + + //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate, this ); + gripper->Subscribe(); + if( blobfinder ) // optional - blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); - } -}; - - - -// Stage calls this when the model starts up -extern "C" int Init( Model* mod ) -{ - Robot* robot = new Robot( (ModelPosition*)mod, - mod->GetWorld()->GetModel( "source" ), - mod->GetWorld()->GetModel( "sink" ) ); - - return 0; //ok + { + blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); + blobfinder->Subscribe(); + } } - - -void Robot::Dock() -{ - // close the grippers so they can be pushed into the charger - ModelGripper::data_t gripper_data = gripper->GetData(); + void Dock() + { + // close the grippers so they can be pushed into the charger + ModelGripper::data_t gripper_data = gripper->GetData(); - if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) - gripper->CommandClose(); - else if( gripper_data.lift != ModelGripper::LIFT_UP ) - gripper->CommandUp(); + if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) + gripper->CommandClose(); + else if( gripper_data.lift != ModelGripper::LIFT_UP ) + gripper->CommandUp(); - if( charger_ahoy ) - { - double a_goal = normalize( charger_bearing ); + if( charger_ahoy ) + { + double a_goal = normalize( charger_bearing ); -// if( pos->Stalled() ) -// { -// puts( "stalled. stopping" ); -// pos->Stop(); -// } -// else + // if( pos->Stalled() ) + // { + // puts( "stalled. stopping" ); + // pos->Stop(); + // } + // else - if( charger_range > 0.5 ) - { - if( !ObstacleAvoid() ) - { - pos->SetXSpeed( cruisespeed ); - pos->SetTurnSpeed( a_goal ); - } - } - else - { - pos->SetTurnSpeed( a_goal ); - pos->SetXSpeed( 0.02 ); // creep towards it + if( charger_range > 0.5 ) + { + if( !ObstacleAvoid() ) + { + pos->SetXSpeed( cruisespeed ); + pos->SetTurnSpeed( a_goal ); + } + } + else + { + pos->SetTurnSpeed( a_goal ); + pos->SetXSpeed( 0.02 ); // creep towards it - if( charger_range < 0.08 ) // close enough - pos->Stop(); + if( charger_range < 0.08 ) // close enough + pos->Stop(); - if( pos->Stalled() ) // touching - pos->SetXSpeed( -0.01 ); // back off a bit + if( pos->Stalled() ) // touching + pos->SetXSpeed( -0.01 ); // back off a bit - } - } - else - { - //printf( "docking but can't see a charger\n" ); - pos->Stop(); - mode = MODE_WORK; // should get us back on track eventually - } + } + } + else + { + //printf( "docking but can't see a charger\n" ); + pos->Stop(); + mode = MODE_WORK; // should get us back on track eventually + } - // if the battery is charged, go back to work - if( Full() ) - { - //printf( "fully charged, now back to work\n" ); - mode = MODE_UNDOCK; - } -} + // if the battery is charged, go back to work + if( Full() ) + { + //printf( "fully charged, now back to work\n" ); + mode = MODE_UNDOCK; + } + } -void Robot::UnDock() -{ - const stg_meters_t gripper_distance = 0.2; - const stg_meters_t back_off_distance = 0.3; - const stg_meters_t back_off_speed = -0.05; + void UnDock() + { + const stg_meters_t gripper_distance = 0.2; + const stg_meters_t back_off_distance = 0.3; + const stg_meters_t back_off_speed = -0.05; - // back up a bit - if( charger_range < back_off_distance ) - pos->SetXSpeed( back_off_speed ); - else - pos->SetXSpeed( 0.0 ); + // back up a bit + if( charger_range < back_off_distance ) + pos->SetXSpeed( back_off_speed ); + else + pos->SetXSpeed( 0.0 ); - // once we have backed off a bit, open and lower the gripper - ModelGripper::data_t gripper_data = gripper->GetData(); - if( charger_range > gripper_distance ) - { - if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) - gripper->CommandOpen(); - else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) - gripper->CommandDown(); - } + // once we have backed off a bit, open and lower the gripper + ModelGripper::data_t gripper_data = gripper->GetData(); + if( charger_range > gripper_distance ) + { + if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) + gripper->CommandOpen(); + else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) + gripper->CommandDown(); + } - // if the gripper is down and open and we're away from the charger, undock is finished - if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && - gripper_data.lift == ModelGripper::LIFT_DOWN && - charger_range > back_off_distance ) - mode = MODE_WORK; -} + // if the gripper is down and open and we're away from the charger, undock is finished + if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && + gripper_data.lift == ModelGripper::LIFT_DOWN && + charger_range > back_off_distance ) + mode = MODE_WORK; + } -bool Robot::ObstacleAvoid() -{ - bool obstruction = false; - bool stop = false; + bool ObstacleAvoid() + { + bool obstruction = false; + bool stop = false; - // find the closest distance to the left and right and check if - // there's anything in front - double minleft = 1e6; - double minright = 1e6; + // find the closest distance to the left and right and check if + // there's anything in front + double minleft = 1e6; + double minright = 1e6; - // Get the data - uint32_t sample_count=0; - stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); + // Get the data + uint32_t sample_count=0; + stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); - for (uint32_t i = 0; i < sample_count; i++) - { - if( verbose ) printf( "%.3f ", scan[i].range ); + for (uint32_t i = 0; i < sample_count; i++) + { + if( verbose ) printf( "%.3f ", scan[i].range ); - if( (i > (sample_count/4)) - && (i < (sample_count - (sample_count/4))) - && scan[i].range < minfrontdistance) - { - if( verbose ) puts( " obstruction!" ); - obstruction = true; - } + if( (i > (sample_count/4)) + && (i < (sample_count - (sample_count/4))) + && scan[i].range < minfrontdistance) + { + if( verbose ) puts( " obstruction!" ); + obstruction = true; + } - if( scan[i].range < stopdist ) - { - if( verbose ) puts( " stopping!" ); - stop = true; - } + if( scan[i].range < stopdist ) + { + if( verbose ) puts( " stopping!" ); + stop = true; + } - if( i > sample_count/2 ) - minleft = MIN( minleft, scan[i].range ); - else - minright = MIN( minright, scan[i].range ); - } + if( i > sample_count/2 ) + minleft = MIN( minleft, scan[i].range ); + else + minright = MIN( minright, scan[i].range ); + } - if( verbose ) - { - puts( "" ); - printf( "minleft %.3f \n", minleft ); - printf( "minright %.3f\n ", minright ); - } + if( verbose ) + { + puts( "" ); + printf( "minleft %.3f \n", minleft ); + printf( "minright %.3f\n ", minright ); + } - if( obstruction || stop || (avoidcount>0) ) - { - if( verbose ) printf( "Avoid %d\n", avoidcount ); + if( obstruction || stop || (avoidcount>0) ) + { + if( verbose ) printf( "Avoid %d\n", avoidcount ); - pos->SetXSpeed( stop ? 0.0 : avoidspeed ); + pos->SetXSpeed( stop ? 0.0 : avoidspeed ); - /* once we start avoiding, select a turn direction and stick - with it for a few iterations */ - if( avoidcount < 1 ) - { - if( verbose ) puts( "Avoid START" ); - avoidcount = random() % avoidduration + avoidduration; + /* once we start avoiding, select a turn direction and stick + with it for a few iterations */ + if( avoidcount < 1 ) + { + if( verbose ) puts( "Avoid START" ); + avoidcount = random() % avoidduration + avoidduration; - if( minleft < minright ) - { - pos->SetTurnSpeed( -avoidturn ); - if( verbose ) printf( "turning right %.2f\n", -avoidturn ); - } - else - { - pos->SetTurnSpeed( +avoidturn ); - if( verbose ) printf( "turning left %2f\n", +avoidturn ); - } - } + if( minleft < minright ) + { + pos->SetTurnSpeed( -avoidturn ); + if( verbose ) printf( "turning right %.2f\n", -avoidturn ); + } + else + { + pos->SetTurnSpeed( +avoidturn ); + if( verbose ) printf( "turning left %2f\n", +avoidturn ); + } + } - avoidcount--; + avoidcount--; - return true; // busy avoding obstacles - } + return true; // busy avoding obstacles + } - return false; // didn't have to avoid anything -} + return false; // didn't have to avoid anything + } -void Robot::Work() -{ - if( ! ObstacleAvoid() ) - { - if( verbose ) puts( "Cruise" ); + void Work() + { + if( ! ObstacleAvoid() ) + { + if( verbose ) puts( "Cruise" ); - //avoidcount = 0; - pos->SetXSpeed( cruisespeed ); + ModelGripper::data_t gdata = gripper->GetData(); + + //avoidcount = 0; + pos->SetXSpeed( cruisespeed ); - Pose pose = pos->GetPose(); + Pose pose = pos->GetPose(); - int x = (pose.x + 8) / 4; - int y = (pose.y + 8) / 4; + int x = (pose.x + 8) / 4; + int y = (pose.y + 8) / 4; - // oh what an awful bug - 5 hours to track this down. When using - // this controller in a world larger than 8*8 meters, a_goal can - // sometimes be NAN. Causing trouble WAY upstream. - if( x > 3 ) x = 3; - if( y > 3 ) y = 3; - if( x < 0 ) x = 0; - if( y < 0 ) y = 0; + // oh what an awful bug - 5 hours to track this down. When using + // this controller in a world larger than 8*8 meters, a_goal can + // sometimes be NAN. Causing trouble WAY upstream. + if( x > 3 ) x = 3; + if( y > 3 ) y = 3; + if( x < 0 ) x = 0; + if( y < 0 ) y = 0; - double a_goal = - dtor( pos->GetFlagCount() ? have[y][x] : need[y][x] ); + double a_goal = + dtor( ( pos->GetFlagCount() || gdata.stack_count ) ? have[y][x] : need[y][x] ); - // if we are low on juice - find the direction to the recharger instead - if( Hungry() ) - { - //puts( "hungry - using refuel map" ); + // if we are low on juice - find the direction to the recharger instead + if( Hungry() ) + { + //puts( "hungry - using refuel map" ); - // use the refuel map - a_goal = dtor( refuel[y][x] ); + // use the refuel map + a_goal = dtor( refuel[y][x] ); - if( charger_ahoy ) // I see a charger while hungry! - mode = MODE_DOCK; - } + if( charger_ahoy ) // I see a charger while hungry! + mode = MODE_DOCK; + } + else + { + if( ! at_dest ) + { + if( gdata.beam[0] ) // inner break beam broken + gripper->CommandClose(); + } + } + + assert( ! isnan(a_goal ) ); + assert( ! isnan(pose.a ) ); + + double a_error = normalize( a_goal - pose.a ); - assert( ! isnan(a_goal ) ); - assert( ! isnan(pose.a ) ); + assert( ! isnan(a_error) ); - double a_error = normalize( a_goal - pose.a ); - - assert( ! isnan(a_error) ); - - pos->SetTurnSpeed( a_error ); - } -} + pos->SetTurnSpeed( a_error ); + } + } -// inspect the laser data and decide what to do -int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) -{ -// if( laser->power_pack && laser->power_pack->charging ) -// printf( "model %s power pack @%p is charging\n", -// laser->Token(), laser->power_pack ); + // inspect the laser data and decide what to do + static int LaserUpdate( ModelLaser* laser, Robot* robot ) + { + // if( laser->power_pack && laser->power_pack->charging ) + // printf( "model %s power pack @%p is charging\n", + // laser->Token(), laser->power_pack ); - if( laser->GetSamples(NULL) == NULL ) - return 0; + if( laser->GetSamples(NULL) == NULL ) + return 0; - switch( robot->mode ) - { - case MODE_DOCK: - //puts( "DOCK" ); - robot->Dock(); - break; + switch( robot->mode ) + { + case MODE_DOCK: + //puts( "DOCK" ); + robot->Dock(); + break; - case MODE_WORK: - //puts( "WORK" ); - robot->Work(); - break; + case MODE_WORK: + //puts( "WORK" ); + robot->Work(); + break; - case MODE_UNDOCK: - //puts( "UNDOCK" ); - robot->UnDock(); - break; + case MODE_UNDOCK: + //puts( "UNDOCK" ); + robot->UnDock(); + break; - default: - printf( "unrecognized mode %u\n", robot->mode ); - } + default: + printf( "unrecognized mode %u\n", robot->mode ); + } - //if( robot->charger_ahoy ) - //return 1; - //else + //if( robot->charger_ahoy ) + //return 1; + //else return 0; -} + } -bool Robot::Hungry() -{ - return( pos->FindPowerPack()->ProportionRemaining() < 0.25 ); -} + bool Hungry() + { + return( pos->FindPowerPack()->ProportionRemaining() < 0.25 ); + } -bool Robot::Full() -{ - return( pos->FindPowerPack()->ProportionRemaining() > 0.95 ); -} + bool Full() + { + return( pos->FindPowerPack()->ProportionRemaining() > 0.95 ); + } -int Robot::PositionUpdate( ModelPosition* pos, Robot* robot ) -{ - Pose pose = pos->GetPose(); + static int PositionUpdate( ModelPosition* pos, Robot* robot ) + { + Pose pose = pos->GetPose(); - //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", - // pose.x, pose.y, pose.z, pose.a ); + //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", + // pose.x, pose.y, pose.z, pose.a ); - //pose.z += 0.0001; - //robot->pos->SetPose( pose ); + //pose.z += 0.0001; + //robot->pos->SetPose( pose ); - if( pos->GetFlagCount() < payload && - hypot( -7-pose.x, -7-pose.y ) < 2.0 ) - { - if( ++robot->work_get > workduration ) - { - // protect source from concurrent access - robot->source->Lock(); + if( pos->GetFlagCount() < payload && + hypot( -7-pose.x, -7-pose.y ) < 2.0 ) + { + if( ++robot->work_get > workduration ) + { + // protect source from concurrent access + robot->source->Lock(); - // transfer a chunk from source to robot - pos->PushFlag( robot->source->PopFlag() ); - robot->source->Unlock(); + // transfer a chunk from source to robot + pos->PushFlag( robot->source->PopFlag() ); + robot->source->Unlock(); - robot->work_get = 0; - } - } + robot->work_get = 0; + } + } - if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) - { - if( ++robot->work_put > workduration ) - { - // protect sink from concurrent access - robot->sink->Lock(); + robot->at_dest = false; - //puts( "dropping" ); - // transfer a chunk between robot and goal - robot->sink->PushFlag( pos->PopFlag() ); - robot->sink->Unlock(); + if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) + { + robot->at_dest = true; - robot->work_put = 0; - } - } + if( ++robot->work_put > workduration ) + { + // protect sink from concurrent access + robot->sink->Lock(); + + //puts( "dropping" ); + // transfer a chunk between robot and goal + robot->sink->PushFlag( pos->PopFlag() ); + robot->sink->Unlock(); + + robot->work_put = 0; + + robot->gripper->CommandOpen(); + } + } - return 0; // run again -} + return 0; // run again + } -int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot ) -{ - robot->charger_ahoy = false; + static int FiducialUpdate( ModelFiducial* mod, Robot* robot ) + { + robot->charger_ahoy = false; - for( unsigned int i = 0; i < mod->fiducial_count; i++ ) - { - stg_fiducial_t* f = &mod->fiducials[i]; + for( unsigned int i = 0; i < mod->fiducial_count; i++ ) + { + stg_fiducial_t* f = &mod->fiducials[i]; - //printf( "fiducial %d is %d at %.2f m %.2f radians\n", - // i, f->id, f->range, f->bearing ); + //printf( "fiducial %d is %d at %.2f m %.2f radians\n", + // i, f->id, f->range, f->bearing ); - if( f->id == 2 ) // I see a charging station - { - // record that I've seen it and where it is - robot->charger_ahoy = true; - robot->charger_bearing = f->bearing; - robot->charger_range = f->range; - robot->charger_heading = f->geom.a; + if( f->id == 2 ) // I see a charging station + { + // record that I've seen it and where it is + robot->charger_ahoy = true; + robot->charger_bearing = f->bearing; + robot->charger_range = f->range; + robot->charger_heading = f->geom.a; - //printf( "charger at %.2f radians\n", robot->charger_bearing ); - break; - } - } + //printf( "charger at %.2f radians\n", robot->charger_bearing ); + break; + } + } - return 0; // run again -} + return 0; // run again + } -int Robot::BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot ) + static int BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot ) + { + unsigned int blob_count = 0; + stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count ); + + if( blobs && (blob_count>0) ) + { + printf( "%s sees %u blobs\n", blobmod->Token(), blob_count ); + } + + return 0; + } + + static int GripperUpdate( ModelGripper* grip, Robot* robot ) + { + ModelGripper::data_t gdata = grip->GetData(); + + printf( "BREAKBEAMS %s %s\n", + gdata.beam[0] ? gdata.beam[0]->Token() : "<null>", + gdata.beam[1] ? gdata.beam[1]->Token() : "<null>" ); + + printf( "CONTACTS %s %s\n", + gdata.contact[0] ? gdata.contact[0]->Token() : "<null>", + gdata.contact[1] ? gdata.contact[1]->Token() : "<null>"); + + + return 0; + } + + +}; + + +// Stage calls this when the model starts up +extern "C" int Init( Model* mod ) { - unsigned int blob_count = 0; - stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count ); + Robot* robot = new Robot( (ModelPosition*)mod, + mod->GetWorld()->GetModel( "source" ), + mod->GetWorld()->GetModel( "sink" ) ); + + return 0; //ok +} - if( blobs && (blob_count>0) ) - { - printf( "%s sees %u blobs\n", blobmod->Token(), blob_count ); - } - return 0; -} + Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 09:43:20 UTC (rev 7331) @@ -23,12 +23,6 @@ // and replace the list in the hash table g_hash_table_insert( callbacks, key, cb_list ); - - // if the callback was an update function, add this model to the - // world's list of models that need updating (if it's not there - // already) - //if( address == (void*)&update ) - //stg_world_start_updating_model( world, this ); } @@ -76,9 +70,7 @@ // return the number of callbacks now in the list. Useful for // detecting when the list is empty. - //return g_list_length( cb_list ); - - return 0; + return g_list_length( cb_list ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/libstage/stage.hh 2009-02-13 09:43:20 UTC (rev 7331) @@ -2091,13 +2091,13 @@ void AddUpdateCallback( stg_model_callback_t cb, void* user ) { AddCallback( &hooks.update, cb, user ); - Subscribe(); // if attaching a callback here, assume we want updates to happen + //Subscribe(); // if attaching a callback here, assume we want updates to happen } void RemoveUpdateCallback( stg_model_callback_t cb ) { RemoveCallback( &hooks.update, cb ); - Unsubscribe(); + //Unsubscribe(); } /** named-property interface @@ -2326,6 +2326,7 @@ CMD_DOWN }; + /** gripper configuration */ struct config_t @@ -2338,16 +2339,19 @@ double paddle_position; ///< 0.0 = full open, 1.0 full closed double lift_position; ///< 0.0 = full down, 1.0 full up - stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle - stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle bool paddles_stalled; // true iff some solid object stopped // the paddles closing or opening - GSList *grip_stack; ///< stack of items gripped + GList*grip_stack; ///< stack of items gripped int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically + + double break_beam_inset[2]; ///< distance from the end of the paddle + + Model* beam[2]; ///< points to a model detected by the beams + Model* contact[2]; ///< pointers to a model detected by the contacts }; @@ -2361,17 +2365,14 @@ double paddle_position; ///< 0.0 = full open, 1.0 full closed double lift_position; ///< 0.0 = full down, 1.0 full up - stg_bool_t inner_break_beam; ///< non-zero iff beam is broken - stg_bool_t outer_break_beam; ///< non-zero iff beam is broken - stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something - stg_bool_t paddles_stalled; // true iff some solid object stopped // the paddles closing or opening + Model* beam[2]; ///< points to a model detected by the beams + Model* contact[2]; ///< pointers to a model detected by the contacts + int stack_count; ///< number of objects in stack - - }; private: @@ -2380,6 +2381,8 @@ void FixBlocks(); void PositionPaddles(); + void UpdateBreakBeams(); + void UpdateContacts(); config_t cfg; cmd_t cmd; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/worlds/fasr.world 2009-02-13 09:43:20 UTC (rev 7331) @@ -22,9 +22,9 @@ ( size [ 902.000 856.000 ] - center [ 5.355 5.319 ] - rotate [ 30.000 26.000 ] - scale 126.761 + center [ 3.982 5.051 ] + rotate [ 0 0 ] + scale 168.655 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -79,12 +79,12 @@ fiducial_return 0 # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) - gripper( pose [0.25 0 -0.22 0] + gripper( pose [0.250 0 -0.220 0] take_watts 1000.0 fiducial( range 3 ) # paddles [ "closed" "up" ] obstacle_return 0 # cheating for simplicity - autosnatch 0 + # autosnatch 1 ) ) @@ -94,14 +94,14 @@ color "purple" # side blocks to restrict view angle - model( color "purple" size [0.100 0.050 0.400] pose [ 0 0.100 0 0 ] ) - model( color "purple" size [0.100 0.050 0.400] pose [ 0 -0.100 0 0 ] ) + model( color "purple" size [0.100 0.050 0.250] pose [ 0 0.100 0 0 ] ) + model( color "purple" size [0.100 0.050 0.250] pose [ 0 -0.100 0 0 ] ) # the charging block model( pose [ 0.010 0 0 0 ] color "yellow" - size [ 0.050 0.200 0.150 ] + size [ 0.050 0.200 0.100 ] joules -1 # provides infinite energy give_watts 1000 fiducial_return 2 # look for this in the fiducial sensor @@ -114,7 +114,46 @@ charge_station( pose [ 7.931 -3.367 0 0 ] ) charge_station( pose [ 7.931 -4.444 0 0 ] ) -autorob( pose [4.144 6.834 0 -98.076] joules 300000 ) +define puck model ( + size [0.120 0.120 0.100] + color "violet" + gripper_return 1 + obstacle_return 0 +) + +puck( pose [ 4.519 6.829 0 0 ] ) +puck( pose [ 4.088 7.018 0 0 ] ) +puck( pose [ 4.409 7.207 0 0 ] ) +puck( pose [ 3.589 7.324 0 0 ] ) +puck( pose [ 3.729 6.893 0 0 ] ) +puck( pose [ 3.714 7.683 0 0 ] ) +puck( pose [ 3.869 7.275 0 0 ] ) +puck( pose [ 4.039 6.719 0 0 ] ) +puck( pose [ 4.269 6.704 0 0 ] ) +puck( pose [ 3.752 7.101 0 0 ] ) +puck( pose [ 4.243 7.033 0 0 ] ) +puck( pose [ 4.666 7.052 0 0 ] ) +puck( pose [ 3.820 6.629 0 0 ] ) + +puck( pose [ 3.930 6.381 0 0 ] ) +puck( pose [ 4.560 6.269 0 0 ] ) +puck( pose [ 4.391 5.814 0 0 ] ) +puck( pose [ 3.475 5.926 0 0 ] ) +puck( pose [ 3.205 6.595 0 0 ] ) +puck( pose [ 3.549 6.393 0 0 ] ) +puck( pose [ 3.957 5.640 0 0 ] ) +puck( pose [ 3.825 5.781 0 0 ] ) +puck( pose [ 4.789 5.122 0 0 ] ) +puck( pose [ 4.167 5.850 0 0 ] ) +puck( pose [ 3.652 6.101 0 0 ] ) +puck( pose [ 3.957 5.966 0 0 ] ) +puck( pose [ 4.685 5.982 0 0 ] ) +puck( pose [ 4.274 6.152 0 0 ] ) +puck( pose [ 4.380 6.491 0 0 ] ) +puck( pose [ 4.870 6.449 0 0 ] ) +puck( pose [ 3.588 6.635 0 0 ] ) + +autorob( pose [5.462 7.216 0 -163.478] joules 300000 ) autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |