Revision: 7329 http://playerstage.svn.sourceforge.net/playerstage/?rev=7329&view=rev Author: rtv Date: 2009-02-13 07:10:11 +0000 (Fri, 13 Feb 2009)
Log Message: ----------- grippers partially implemented. they don't lift anything yet Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/typetable.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -50,6 +50,7 @@ ModelRanger* ranger; ModelFiducial* fiducial; ModelBlobfinder* blobfinder; + ModelGripper* gripper; Model *source, *sink; int avoidcount, randcount; int work_get, work_put; @@ -82,6 +83,7 @@ ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )), fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )), blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )), + gripper( (ModelGripper*)pos->GetUnusedModelOfType( MODEL_TYPE_GRIPPER )), source(source), sink(sink), avoidcount(0), @@ -133,6 +135,14 @@ void Robot::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( charger_ahoy ) { double a_goal = normalize( charger_bearing ); @@ -183,9 +193,30 @@ void Robot::UnDock() { - if( charger_range < 0.3 ) - pos->SetXSpeed( -0.05 ); + 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 ); + + // 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; } Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-13 07:10:11 UTC (rev 7329) @@ -4,6 +4,7 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs ancestor.cc + model_gripper.cc block.cc blockgroup.cc camera.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/block.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -61,6 +61,80 @@ g_ptr_array_free( candidate_cells, TRUE ); } +void Block::Translate( double x, double y ) +{ + for( unsigned int p=0; p<pt_count; p++) + { + pts[p].x += x; + pts[p].y += y; + } + + // force redraw + mod->blockgroup.BuildDisplayList( mod ); +} + + +double Block::CenterY() +{ + double min = billion; + double max = -billion; + + for( unsigned int p=0; p<pt_count; p++) + { + if( pts[p].y > max ) max = pts[p].y; + if( pts[p].y < min ) min = pts[p].y; + } + + // return the value half way between max and min + return( min + (max - min)/2.0 ); +} + +double Block::CenterX() +{ + double min = billion; + double max = -billion; + + for( unsigned int p=0; p<pt_count; p++) + { + if( pts[p].x > max ) max = pts[p].x; + if( pts[p].x < min ) min = pts[p].x; + } + + // return the value half way between maxx and min + return( min + (max - min)/2.0 ); +} + +void Block::SetCenter( double x, double y ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( x-CenterX(), y-CenterY() ); +} + +void Block::SetCenterY( double y ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( 0, y-CenterY() ); +} + +void Block::SetCenterX( double x ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( x-CenterX(), 0 ); +} + +void Block::SetZ( double min, double max ) +{ + local_z.min = min; + local_z.max = max; + + // force redraw + mod->blockgroup.BuildDisplayList( mod ); +} + + stg_color_t Block::GetColor() { return( inherit_color ? mod->color : color ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -374,11 +374,11 @@ } -void Model::AddBlockRect( stg_meters_t x, - stg_meters_t y, - stg_meters_t dx, - stg_meters_t dy, - stg_meters_t dz ) +Block* Model::AddBlockRect( stg_meters_t x, + stg_meters_t y, + stg_meters_t dx, + stg_meters_t dy, + stg_meters_t dz ) { UnMap(); @@ -392,11 +392,15 @@ pts[3].x = x; pts[3].y = y + dy; - blockgroup.AppendBlock( new Block( this, - pts, 4, - 0, dz, - color, - true ) ); + Block* newblock = new Block( this, + pts, 4, + 0, dz, + color, + true ); + + blockgroup.AppendBlock( newblock ); + + return newblock; } Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -262,7 +262,7 @@ if ( !showFiducialData ) return; -// // draw the FOV + // draw the FOV // GLUquadric* quadric = gluNewQuadric(); // PushColor( 0,0,0,0.2 ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model_laser.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -127,7 +127,6 @@ void ModelLaser::Load( void ) { - Model::Load(); sample_count = wf->ReadInt( wf_entity, "samples", sample_count ); range_min = wf->ReadLength( wf_entity, "range_min", range_min); range_max = wf->ReadLength( wf_entity, "range_max", range_max ); @@ -142,6 +141,8 @@ PRINT_WARN( "laser resolution set < 1. Forcing to 1" ); resolution = 1; } + + Model::Load(); } stg_laser_cfg_t ModelLaser::GetConfig() Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/stage.hh 2009-02-13 07:10:11 UTC (rev 7329) @@ -95,6 +95,7 @@ MODEL_TYPE_BLOBFINDER, MODEL_TYPE_BLINKENLIGHT, MODEL_TYPE_CAMERA, + MODEL_TYPE_GRIPPER, MODEL_TYPE_COUNT // must be the last entry, to count the number of // types } stg_model_type_t; @@ -1085,6 +1086,23 @@ void DrawSolid(); // draw the block in OpenGL as a solid single color void DrawFootPrint(); // draw the projection of the block onto the z=0 plane + /** Translate all points in the block by the indicated amounts */ + void Translate( double x, double y ); + + /** Return the center of the block on the X axis */ + double CenterX(); + /** Return the center of the block on the Y axis */ + double CenterY(); + + /** Set the center of the block on the X axis */ + void SetCenterX( double y ); + /** Set the center of the block on the Y axis */ + void SetCenterY( double y ); + /** Set the center of the block */ + void SetCenter( double x, double y); + + void SetZ( double min, double max ); + void RecordRendering( Cell* cell ) { g_ptr_array_add( rendered_cells, (gpointer)cell ); }; @@ -1154,11 +1172,12 @@ class BlockGroup { friend class Model; + friend class Block; private: int displaylist; + void BuildDisplayList( Model* mod ); - GList* blocks; uint32_t count; Size size; @@ -1921,9 +1940,9 @@ /** Add a block to this model centered at [x,y] with extent [dx, dy, dz] */ - void AddBlockRect( stg_meters_t x, stg_meters_t y, - stg_meters_t dx, stg_meters_t dy, - stg_meters_t dz ); + Block* AddBlockRect( stg_meters_t x, stg_meters_t y, + stg_meters_t dx, stg_meters_t dy, + stg_meters_t dz ); /** remove all blocks from this model, freeing their memory */ void ClearBlocks(); @@ -2277,88 +2296,132 @@ // Set the user-tweakable configuration of the laser void SetConfig( stg_laser_cfg_t cfg ); }; - + // \todo GRIPPER MODEL -------------------------------------------------------- + + + class ModelGripper : public Model + { + public: - // typedef enum { - // STG_GRIPPER_PADDLE_OPEN = 0, // default state - // STG_GRIPPER_PADDLE_CLOSED, - // STG_GRIPPER_PADDLE_OPENING, - // STG_GRIPPER_PADDLE_CLOSING, - // } stg_gripper_paddle_state_t; + enum paddle_state_t { + PADDLE_OPEN = 0, // default state + PADDLE_CLOSED, + PADDLE_OPENING, + PADDLE_CLOSING, + }; + + enum lift_state_t { + LIFT_DOWN = 0, // default state + LIFT_UP, + LIFT_UPPING, // verbed these to match the paddle state + LIFT_DOWNING, + }; + + enum cmd_t { + CMD_NOOP = 0, // default state + CMD_OPEN, + CMD_CLOSE, + CMD_UP, + CMD_DOWN + }; + + /** gripper configuration + */ + struct config_t + { + Size paddle_size; ///< paddle dimensions + + paddle_state_t paddles; + lift_state_t lift; + + 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 + 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 + }; + + + /** gripper data packet + */ + struct data_t + { + paddle_state_t paddles; + lift_state_t lift; - // typedef enum { - // STG_GRIPPER_LIFT_DOWN = 0, // default state - // STG_GRIPPER_LIFT_UP, - // STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state - // STG_GRIPPER_LIFT_DOWNING, - // } stg_gripper_lift_state_t; + double paddle_position; ///< 0.0 = full open, 1.0 full closed + double lift_position; ///< 0.0 = full down, 1.0 full up - // typedef enum { - // STG_GRIPPER_CMD_NOP = 0, // default state - // STG_GRIPPER_CMD_OPEN, - // STG_GRIPPER_CMD_CLOSE, - // STG_GRIPPER_CMD_UP, - // STG_GRIPPER_CMD_DOWN - // } stg_gripper_cmd_type_t; + stg_bool_t inner_break_beam; ///< non-zero iff beam is broken + stg_bool_t outer_break_beam; ///< non-zero iff beam is broken - // /** gripper configuration packet - // */ - // typedef struct - // { - // Size paddle_size; ///< paddle dimensions + stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something - // stg_gripper_paddle_state_t paddles; - // stg_gripper_lift_state_t lift; + stg_bool_t paddles_stalled; // true iff some solid object stopped + // the paddles closing or opening - // double paddle_position; ///< 0.0 = full open, 1.0 full closed - // double lift_position; ///< 0.0 = full down, 1.0 full up + int stack_count; ///< number of objects in stack - // 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 - // stg_bool_t paddles_stalled; // true iff some solid object stopped - // // the paddles closing or opening - // GSList *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. + private: + virtual void Update(); + virtual void DataVisualize( Camera* cam ); + + void FixBlocks(); + void PositionPaddles(); - // } stg_gripper_config_t; + config_t cfg; + cmd_t cmd; + + Block* paddle_left; + Block* paddle_right; - // /** gripper command packet - // */ - // typedef struct - // { - // stg_gripper_cmd_type_t cmd; - // int arg; - // } stg_gripper_cmd_t; + public: + static const char* typestr; + static const Size size; + // constructor + ModelGripper( World* world, + Model* parent ); + // destructor + virtual ~ModelGripper(); + + virtual void Load(); + virtual void Save(); - // /** gripper data packet - // */ - // typedef struct - // { - // stg_gripper_paddle_state_t paddles; - // stg_gripper_lift_state_t lift; + void SetConfig( config_t & newcfg ); - // double paddle_position; ///< 0.0 = full open, 1.0 full closed - // double lift_position; ///< 0.0 = full down, 1.0 full up + /** Returns the static state of the gripper */ + config_t GetConfig(){ return cfg; }; - // stg_bool_t inner_break_beam; ///< non-zero iff beam is broken - // stg_bool_t outer_break_beam; ///< non-zero iff beam is broken + /** Returns the dynamic state of the gripper */ + data_t GetData(); + + /** Set the current activity of the gripper. */ + void SetCommand( cmd_t cmd ) { this->cmd = cmd; } - // stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something + /** Command the gripper paddles to close. Wrapper for SetCommand( CMD_CLOSE ). */ + void CommandClose() { SetCommand( CMD_CLOSE ); } + /** Command the gripper paddles to open. Wrapper for SetCommand( CMD_OPEN ). */ + void CommandOpen() { SetCommand( CMD_OPEN ); } + /** Command the gripper lift to go up. Wrapper for SetCommand( CMD_UP ). */ + void CommandUp() { SetCommand( CMD_UP ); } + /** Command the gripper lift to go down. Wrapper for SetCommand( CMD_DOWN ). */ + void CommandDown() { SetCommand( CMD_DOWN ); } + }; - // stg_bool_t paddles_stalled; // true iff some solid object stopped - // // the paddles closing or opening - // int stack_count; ///< number of objects in stack - - - // } stg_gripper_data_t; - - // \todo BUMPER MODEL -------------------------------------------------------- // typedef struct Modified: code/stage/trunk/libstage/typetable.cc =================================================================== --- code/stage/trunk/libstage/typetable.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/typetable.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -28,7 +28,10 @@ static Model* CreateModelBlobfinder( World* world, Model* parent ) { return new ModelBlobfinder( world, parent ); } +static Model* CreateModelGripper( World* world, Model* parent ) +{ return new ModelGripper( world, parent ); } + void Stg::RegisterModels() { RegisterModel( MODEL_TYPE_PLAIN, "model", CreateModel ); @@ -39,6 +42,7 @@ RegisterModel( MODEL_TYPE_POSITION, "position", CreateModelPosition ); RegisterModel( MODEL_TYPE_BLOBFINDER, "blobfinder", CreateModelBlobfinder ); RegisterModel( MODEL_TYPE_BLINKENLIGHT, "blinkenlight", CreateModelBlinkenlight); + RegisterModel( MODEL_TYPE_GRIPPER, "gripper", CreateModelGripper); #if DEBUG // human-readable view of the table puts( "Stg::Typetable" ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/worlds/fasr.world 2009-02-13 07:10:11 UTC (rev 7329) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 20 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 @@ -22,9 +22,9 @@ ( size [ 902.000 856.000 ] - center [ 6.726 -2.592 ] - rotate [ 0 0 ] - scale 116.966 + center [ 5.355 5.319 ] + rotate [ 30.000 26.000 ] + scale 126.761 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -77,26 +77,36 @@ joules 100000 joules_capacity 400000 fiducial_return 0 - charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + + gripper( pose [0.25 0 -0.22 0] + take_watts 1000.0 + fiducial( range 3 ) + # paddles [ "closed" "up" ] + obstacle_return 0 # cheating for simplicity + autosnatch 0 + ) ) define charge_station model ( size [ 0.100 0.300 0.100 ] - color "purple" + 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.100 0.400] 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 ] + joules -1 # provides infinite energy + give_watts 1000 + fiducial_return 2 # look for this in the fiducial sensor + ) - model( - pose [ 0.010 0 0 0 ] - color "yellow" - size [ 0.050 0.100 0.100 ] - joules -1 # infinite storage - give_watts 1000 - fiducial_return 2 - ) - - model( color "purple" size [0.100 0.100 0.400] pose [ 0 -0.100 0 0 ] ) ) charge_station( pose [ 7.803 -1.332 0 34.377 ] ) @@ -104,28 +114,29 @@ 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] ) -autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.615 6.185 0 107.666] ) -autorob( pose [7.028 6.502 0 -128.279] ) -autorob( pose [5.750 4.137 0 -97.047] ) -autorob( pose [4.909 6.097 0 -44.366] ) -autorob( pose [6.898 4.775 0 -117.576] ) -autorob( pose [7.394 5.595 0 129.497] ) -autorob( pose [6.468 6.708 0 170.743] ) -autorob( pose [6.451 4.189 0 -61.453] ) +autorob( pose [4.144 6.834 0 -98.076] 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 ) +autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) +autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) +autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) +autorob( pose [7.394 5.595 0 129.497] joules 400000 ) +autorob( pose [6.468 6.708 0 170.743] joules 100000 ) +autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) -autorob( pose [5.246 6.813 0 -61.295] ) -autorob( pose [4.127 5.388 0 -147.713] ) -autorob( pose [5.020 4.213 0 -125.236] ) -autorob( pose [3.286 4.715 0 78.789] ) -autorob( pose [5.440 5.317 0 -26.545] ) -autorob( pose [7.641 6.998 0 163.239] ) -autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [5.471 7.446 0 77.301] ) -autorob( pose [7.122 4.175 0 -31.440] ) -autorob( pose [5.944 6.951 0 2.937] ) +autorob( pose [5.246 6.813 0 -61.295] joules 300000 ) +autorob( pose [4.127 5.388 0 -147.713] joules 400000 ) +autorob( pose [5.020 4.213 0 -125.236] joules 100000 ) +autorob( pose [3.286 4.715 0 78.789] joules 200000 ) +autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) +autorob( pose [7.641 6.998 0 163.239] joules 400000 ) +#autorob( pose [7.559 4.764 0 -139.066] ) +#autorob( pose [5.471 7.446 0 77.301] ) +#autorob( pose [7.122 4.175 0 -31.440] ) +#autorob( pose [5.944 6.951 0 2.937] ) + #autorob( pose [6.800 5.897 0 -103.060] ) #autorob( pose [6.405 5.291 0 -103.060] ) #autorob( pose [5.974 5.725 0 -103.060] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA -OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise -Strategies to boost innovation and cut costs with open source participation -Receive a $600 discount off the registration fee with the source code: SFAD http://p.sf.net/sfu/XcvMzF8H _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit