Menu
▾
▴
[Playerstage-commit] code/gazebo/server/models/Pioneer2Gripper Pioneer2Gripper.cc, 1.21, 1.22 Pioneer2Gripper.hh, 1.7, 1.8
|
From: Nathan K. <na...@us...> - 2006-09-25 17:48:30
|
Update of /cvsroot/playerstage/code/gazebo/server/models/Pioneer2Gripper In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30582 Modified Files: Pioneer2Gripper.cc Pioneer2Gripper.hh Log Message: updates to the gipper Index: Pioneer2Gripper.cc =================================================================== RCS file: /cvsroot/playerstage/code/gazebo/server/models/Pioneer2Gripper/Pioneer2Gripper.cc,v retrieving revision 1.21 retrieving revision 1.22 diff -C2 -d -r1.21 -r1.22 *** Pioneer2Gripper.cc 22 Feb 2006 15:29:24 -0000 1.21 --- Pioneer2Gripper.cc 26 Sep 2006 00:48:27 -0000 1.22 *************** *** 117,135 **** Pioneer2Gripper::Pioneer2Gripper( World *world ) : Model( world ) { ! this->body = NULL; ! this->fixedBarGeom = NULL; ! this->leftGripperBody = NULL; ! this->rightGripperBody = NULL; ! this->baseBarBody = NULL; ! this->baseBarGeom = NULL; ! this->leftGripGeom = NULL; ! this->rightGripGeom = NULL; ! this->leftPressureSensorGeom = NULL; ! this->rightPressureSensorGeom = NULL; ! this->innerBreakbeam = NULL; ! this->outerBreakbeam = NULL; ! this->baseJoint = NULL; ! this->leftGripperJoint = NULL; ! this->rightGripperJoint = NULL; } --- 117,123 ---- Pioneer2Gripper::Pioneer2Gripper( World *world ) : Model( world ) { ! this->rightVel = -0.5; ! this->leftVel = 0.5; ! this->liftVel = 0.5; } *************** *** 139,189 **** Pioneer2Gripper::~Pioneer2Gripper() { - if (this->body) - delete this->body; - if (this->fixedBarGeom) - delete this->fixedBarGeom; - if (this->leftGripperBody) - delete this->leftGripperBody; - if (this->rightGripperBody) - delete this->rightGripperBody; - if (this->baseBarBody) - delete this->baseBarBody; - if (this->baseBarGeom) - delete this->baseBarGeom; - if (this->leftGripGeom) - delete this->leftGripGeom; - if (this->rightGripGeom) - delete this->rightGripGeom; - if (this->leftPressureSensorGeom) - delete this->leftPressureSensorGeom; - if (this->rightPressureSensorGeom) - delete this->rightPressureSensorGeom; - if (this->innerBreakbeam) - delete this->innerBreakbeam; - if (this->outerBreakbeam) - delete this->outerBreakbeam; - if (this->baseJoint) - delete this->baseJoint; - if (this->leftGripperJoint) - delete this->leftGripperJoint; - if (this->rightGripperJoint) - delete this->rightGripperJoint; - - this->body = NULL; - this->fixedBarGeom = NULL; - this->leftGripperBody = NULL; - this->rightGripperBody = NULL; - this->baseBarBody = NULL; - this->baseBarGeom = NULL; - this->leftGripGeom = NULL; - this->rightGripGeom = NULL; - this->leftPressureSensorGeom = NULL; - this->rightPressureSensorGeom = NULL; - this->innerBreakbeam = NULL; - this->outerBreakbeam = NULL; - this->baseJoint = NULL; - this->leftGripperJoint = NULL; - this->rightGripperJoint = NULL; - } --- 127,130 ---- *************** *** 203,370 **** int Pioneer2Gripper::OdeLoad( WorldFile *file, WorldFileNode *node ) { ! //initialize gripper's param ! this->liftHeight = 0.09; ! this->gripsOpenWidth = 0.215; ! this->gripsCloseWidth = 0.01; ! ! this->gripLength = 0.095; ! this->gripWidth = 0.02; ! this->gripHeight = 0.03; ! this->gripMass = 0.1; ! ! this->basebarLength = 0.015; ! this->basebarWidth = this->gripsOpenWidth + 2 * this->gripWidth; ! this->basebarHeight = 0.0525; ! this->basebarMass = 0.2; ! this->basebarZPos = -0.07875; ! ! this->fixedbarLength = 0.02667; ! this->fixedbarWidth = 0.22; ! this->fixedbarHeight = 0.105; ! this->fixedbarMass = 0.2; ! ! this->gripMinPosLimit = 0; ! this->gripMaxPosLimit = this->gripsOpenWidth*0.5 - this->gripWidth*0.5; ! this->liftMinPosLimit = 0.4; ! this->liftMaxPosLimit = this->liftHeight; ! ! this->gripMaxSpeed = 0.1; ! this->gripMaxForce = 10.0; ! this->liftMaxSpeed = 0.05; ! this->liftMaxForce = 25; ! ! this->ffTolerance = 0.0001; ! this->simFrictionTolerance = 0.0025; ! ! // Build ODE objects ! this->body = new Body( this->world ); ! ! // The bar that the two grippers attach too ! this->baseBarBody = new Body( this->world ); ! ! this->leftGripperBody = new Body( this->world ); ! this->rightGripperBody = new Body( this->world ); ! ! // TODO: Why have this??? ! this->leftGripSpaceId = dSimpleSpaceCreate( 0 ); ! this->rightGripSpaceId = dSimpleSpaceCreate( 0 ); ! ! this->fixedBarGeom = new BoxGeom( this->body, this->modelSpaceId, this->fixedbarLength, this->fixedbarWidth, this->fixedbarHeight ); ! this->fixedBarGeom->SetRelativePosition( ! GzVectorSet(this->fixedbarLength / 2, 0, 0) ); ! this->fixedBarGeom->SetMass( fixedbarMass ); ! this->fixedBarGeom->SetColor( GzColor( 0, 0, 0 ) ); ! ! this->baseBarBody->SetPosition( GzVectorSet( this->fixedbarLength + this->basebarLength / 2, 0, this->basebarZPos ) ); ! ! this->baseBarGeom = new BoxGeom( this->baseBarBody, this->modelSpaceId, this->basebarLength, this->basebarWidth, this->basebarHeight ); ! this->baseBarGeom->SetMass( this->basebarMass ); ! this->baseBarGeom->SetColor( GzColor( 0, 0, 0 ) ); ! ! ! // Setup the Left Gripper ! // ---------------------- ! this->leftGripGeom = new BoxGeom( this->leftGripperBody, this->modelSpaceId, this->gripLength, this->gripWidth, this->gripHeight); ! this->leftGripGeom->SetMass( gripMass / 4 ); ! this->leftGripGeom->SetColor( GzColor( 0, 0, 0 ) ); ! this->leftGripGeom->SetRelativePosition( GzVectorSet(0, 0, 0) ); ! this->leftGripGeom->SetFriction(dInfinity,dInfinity); ! ! // Left Pressure sensor ! this->leftPressureSensorGeom = new BoxGeom( this->leftGripperBody, ! this->leftGripSpaceId, this->gripLength - 2 * ( this->gripLength / 20 ), ! this->gripWidth / 10, this->gripHeight - 2 * this->gripHeight / 5 ); ! this->leftPressureSensorGeom->SetMass( gripMass / 4 ); ! this->leftPressureSensorGeom->SetColor( GzColor( 0.75, 0.75, 0.75 ) ); ! this->leftPressureSensorGeom->SetRelativePosition( GzVectorSet(0, this->gripWidth / 2 - ( this->gripWidth / 10 ) / 2 + 0.001, 0) ); ! ! ! this->leftGripperBody->SetPosition( GzVectorSet( this->gripLength / 2 + this->fixedbarLength + this->basebarLength , -( this->basebarWidth / 2 ) + this->gripWidth / 2 , this->basebarZPos - ( ( this->basebarHeight - this->gripHeight) / 2) ) ); ! ! ! // Setup the Right Gripper ! // ----------------------- ! this->rightGripGeom = new BoxGeom( this->rightGripperBody, ! this->modelSpaceId, this->gripLength, this->gripWidth, this->gripHeight ); ! this->rightGripGeom->SetMass( gripMass / 4 ); ! this->rightGripGeom->SetColor( GzColor( 0, 0, 0 ) ); ! this->rightGripGeom->SetRelativePosition( GzVectorSet(0, 0, 0) ); ! this->rightGripGeom->SetFriction(dInfinity, dInfinity); ! ! // Right Pressure Sensor ! this->rightPressureSensorGeom = new BoxGeom( this->rightGripperBody, ! this->rightGripSpaceId, this->gripLength - 2 * this->gripLength / 20, ! this->gripWidth / 10, this->gripHeight - 2 * this->gripHeight / 5 ); ! this->rightPressureSensorGeom->SetMass( gripMass / 4 ); ! this->rightPressureSensorGeom->SetColor( GzColor( 0.75, 0.75, 0.75 ) ); ! this->rightPressureSensorGeom->SetRelativePosition( GzVectorSet(0, -this->gripWidth / 2 + ( this->gripWidth / 10 ) / 2 - 0.001, 0)); - this->rightGripperBody->SetPosition(GzVectorSet( this->gripLength / 2 + this->fixedbarLength + this->basebarLength, ( this->basebarWidth / 2) - this->gripWidth / 2, this->basebarZPos - ( ( this->basebarHeight - this->gripHeight) / 2 ) ) ); - // Creates ray sensor for gripper's break beam - this->innerBreakbeam = new RayProximity(this->world, this->body, 1); - this->outerBreakbeam = new RayProximity(this->world, this->body, 1); ! // REMOVE this->innerRay = this->innerBreakbeam->GetRay(); ! // this->innerBreakbeam->SetPosition( ! // GzVectorSet(-(this->gripLength/2) + 0.01, -(this->gripWidth/2) + 0.001, 0)); ! // TODO this->innerBreakbeam->SetDirection( GzVectorSet(0, -M_PI/4, 0) ); - // REMOVE this->outerRay = this->outerBreakbeam->GetRay(); - // this->outerBreakbeam->SetPosition( - // GzVectorSet(this->gripLength / 2 - 0.01, -(gripWidth/2.0) + 0.001, 0) ); - // TODO this->outerBreakbeam->SetDirection( GzVectorSet(0, -M_PI/4, 0) ); - // This joint allows the gripper to lift objects - this->baseJoint = new SliderJoint( this->world->worldId ); - this->baseJoint->Attach( this->baseBarBody, this->body ); - this->baseJoint->SetAxis( 0, 0, 1 ); - this->baseJoint->SetParam( dParamLoStop, 0 ); - this->baseJoint->SetParam( dParamHiStop, this->liftHeight ); - this->baseJoint->SetParam( dParamFMax, this->liftMaxForce); - this->baseJoint->SetParam( dParamBounce, 0 ); - // Left gripper joint - this->leftGripperJoint = new SliderJoint( this->world->worldId ); - this->leftGripperJoint->Attach( this->leftGripperBody, this->baseBarBody ); - this->leftGripperJoint->SetAxis( 0, 1, 0 ); - this->leftGripperJoint->SetParam( dParamBounce, 0 ); - this->leftGripperJoint->SetParam( dParamLoStop, this->gripMinPosLimit ); - this->leftGripperJoint->SetParam( dParamHiStop, this->gripMaxPosLimit ); ! // Right gripper joint ! this->rightGripperJoint = new SliderJoint( this->world->worldId ); ! this->rightGripperJoint->Attach( this->rightGripperBody, this->baseBarBody ); ! this->rightGripperJoint->SetAxis( 0, -1, 0 ); ! this->rightGripperJoint->SetParam( dParamBounce, 0 ); ! this->rightGripperJoint->SetParam( dParamLoStop, this->gripMinPosLimit ); ! this->rightGripperJoint->SetParam( dParamHiStop, this->gripMaxPosLimit ); - this->leftGripperJoint->SetParam( dParamFMax, this->gripMaxForce ); - this->rightGripperJoint->SetParam( dParamFMax, this->gripMaxForce ); ! this->AddBody( this->body, true ); ! this->AddBody( this->baseBarBody ); ! this->AddBody( this->leftGripperBody ); ! this->AddBody( this->rightGripperBody ); - // Initialize gripper state to fully open and lift down - this->gripperMode = this->GRIPPER_STOP_MODE; - this->liftMode = this->LIFT_STOP_MODE; - this->lift_up = false; - this->lift_down = true; - this->lift_moving = false; - this->lift_error = false; - this->paddles_opened = true; - this->paddles_closed = false; - this->paddles_moving = false; - this->paddles_error = false; - this->isGripping = false; - this->StopGrip(); - this->StopLift(); return 0; } --- 144,232 ---- int Pioneer2Gripper::OdeLoad( WorldFile *file, WorldFileNode *node ) { ! Geom *geom = NULL; ! GzVector baseSize = GzVectorSet(0.05, 0.125, 0.13); ! GzVector horizBarSize = GzVectorSet(0.07, 0.315, 0.065); ! GzVector paddleSize = GzVectorSet(0.095, 0.025, 0.04); ! GzVector pos; + // Create the base mount for the gripper + this->base = new Body(this->world); + geom = new BoxGeom(this->base, this->modelSpaceId, baseSize.x, baseSize.y, baseSize.z); + geom->SetMass(0.10); + geom->SetColor(GzColor(0, 0, 0)); + this->AddBody(this->base, true); ! // Setup the horizontal bar. Moves vertically ! this->horizBar = new Body(this->world); ! geom = new BoxGeom(this->horizBar, this->modelSpaceId, horizBarSize.x, horizBarSize.y,horizBarSize.z); ! geom->SetMass(0.00); ! geom->SetColor(GzColor(0.5, 0.5 ,0.5)); ! this->AddBody(this->horizBar); ! pos = this->base->GetPosition(); ! pos.x += (baseSize.x * 0.5) + (horizBarSize.x * 0.5); ! pos.z -= baseSize.z * 0.5; ! this->horizBar->SetPosition(pos); + // Create the left paddle + this->leftPaddle = new Body(this->world); + geom = new BoxGeom(this->leftPaddle, this->modelSpaceId, paddleSize.x, paddleSize.y, paddleSize.z); + geom->SetMass(0.0); + geom->SetColor(GzColor(1, 0, 0)); + geom->SetFriction(dInfinity,dInfinity); + this->AddBody(this->leftPaddle); + pos = this->horizBar->GetPosition(); + pos.x += (horizBarSize.x * 0.5) + (paddleSize.x * 0.5); + pos.y -= (horizBarSize.y-0.08)*0.5; + this->leftPaddle->SetPosition(pos); + // Create the right paddle + this->rightPaddle = new Body(this->world); + geom = new BoxGeom(this->rightPaddle, this->modelSpaceId, paddleSize.x, paddleSize.y, paddleSize.z); + geom->SetMass(0.0); + geom->SetColor(GzColor(0, 0, 1)); + geom->SetFriction(dInfinity,dInfinity); + this->AddBody(this->rightPaddle); + pos = this->horizBar->GetPosition(); + pos.x += (horizBarSize.x * 0.5) + (paddleSize.x * 0.5); + pos.y += (horizBarSize.y-0.08)*0.5; + this->rightPaddle->SetPosition(pos); ! // Setup the slider joint for the horizontal bar ! this->horizBarSlider = new SliderJoint(this->world->worldId); ! this->horizBarSlider->Attach(this->horizBar, this->base); ! this->horizBarSlider->SetAxis(0,0,1); ! this->horizBarSlider->SetParam(dParamLoStop,0.0);//-baseSize.y*0.5); ! this->horizBarSlider->SetParam(dParamHiStop,baseSize.y*0.5); ! this->horizBarSlider->SetParam(dParamBounce,0.0); ! this->horizBarSlider->SetParam(dParamVel,0.0); ! this->horizBarSlider->SetParam(dParamFMax,50.0); ! // Create the slider for the right slider ! this->rightSlider = new SliderJoint(this->world->worldId); ! this->rightSlider->Attach(this->rightPaddle, this->horizBar); ! this->rightSlider->SetAxis(0,1,0); ! this->rightSlider->SetParam(dParamLoStop,-(horizBarSize.y-0.08-paddleSize.y)*0.5);//-baseSize.y*0.5); ! this->rightSlider->SetParam(dParamHiStop,0.0); ! this->rightSlider->SetParam(dParamBounce,0.0); ! this->rightSlider->SetParam(dParamVel,0.0); ! this->rightSlider->SetParam(dParamFMax,10.0); + // Create the slider for the left slider + this->leftSlider = new SliderJoint(this->world->worldId); + this->leftSlider->Attach(this->leftPaddle, this->horizBar); + this->leftSlider->SetAxis(0,1,0); + this->leftSlider->SetParam(dParamLoStop,0.0); + this->leftSlider->SetParam(dParamHiStop,(horizBarSize.y-0.08-paddleSize.y)*0.5); + this->leftSlider->SetParam(dParamBounce,0.0); + this->leftSlider->SetParam(dParamVel,0.0); + this->leftSlider->SetParam(dParamFMax,10.0); + return 0; } *************** *** 378,381 **** --- 240,244 ---- if ( this->OdeInit( file, node ) != 0 ) return -1; + // Initialize external interface if ( this->IfaceInit() != 0 ) *************** *** 396,401 **** int Pioneer2Gripper::IfaceInit() { ! this->gripper_iface = gz_gripper_alloc(); ! if (gz_gripper_create(this->gripper_iface, this->world->gz_server, this->GetId(), "Pioneer2Gripper", this->GetIntId(), this->GetParentIntId()) != 0) return -1; --- 259,265 ---- int Pioneer2Gripper::IfaceInit() { ! this->iface = gz_gripper_alloc(); ! ! if (gz_gripper_create(this->iface, this->world->gz_server, this->GetId(), "Pioneer2Gripper", this->GetIntId(), this->GetParentIntId()) != 0) return -1; *************** *** 422,441 **** int Pioneer2Gripper::OdeFini() { - delete body; - delete baseBarBody; - delete leftGripperBody; - delete rightGripperBody; - - delete fixedBarGeom; - delete baseBarGeom; - delete leftGripGeom; - delete rightGripGeom; - delete leftPressureSensorGeom; - delete rightPressureSensorGeom; - - delete leftGripperJoint; - delete rightGripperJoint; - delete baseJoint; - return 0; } --- 286,289 ---- *************** *** 445,451 **** int Pioneer2Gripper::IfaceFini() { ! gz_gripper_destroy( this->gripper_iface ); ! gz_gripper_free( this->gripper_iface ); ! this->gripper_iface = NULL; return 0; --- 293,299 ---- int Pioneer2Gripper::IfaceFini() { ! gz_gripper_destroy( this->iface ); ! gz_gripper_free( this->iface ); ! this->iface = NULL; return 0; *************** *** 472,517 **** // LIFTcarry 17 -- not supported ! switch ( this->gripper_iface->data->cmd ) { ! case 1 : ! this->OpenGrip(); break; ! ! case 2 : ! this->CloseGrip(); break; ! ! case 3 : ! this->StopGrip(); break; ! ! case 4 : this->LiftUp(); break; ! ! case 5 : this->LiftDown(); break; ! ! case 6 : ! this->StopLift(); break; ! ! case 7 : ! this->CloseGrip(); this->LiftUp(); break; ! ! case 8 : ! this->OpenGrip(); this->LiftDown(); break; ! ! case 15 : ! this->StopGrip(); ! this->StopLift(); ! break; ! ! default : break; } --- 320,354 ---- // LIFTcarry 17 -- not supported ! switch (this->iface->data->cmd) { ! case 1: ! this->GripOpen(); break; ! case 2: ! this->GripClose(); break; ! case 3: ! this->GripStop(); break; ! case 4: this->LiftUp(); break; ! case 5: this->LiftDown(); break; ! case 6: ! this->LiftStop(); break; ! case 7: ! this->GripClose(); this->LiftUp(); break; ! case 8: ! this->GripOpen(); this->LiftDown(); break; ! case 15: ! this->GripStop(); ! this->LiftStop(); break; } *************** *** 525,545 **** { // Data timestamp ! this->gripper_iface->data->time = this->world->GetSimTime(); // Gripper's status ! this->gripper_iface->data->lift_limit_reach = this->lift_limit_reach; ! this->gripper_iface->data->grip_limit_reach = this->grip_limit_reach; ! this->gripper_iface->data->outer_beam_obstruct = this->outer_beam_obstruct; ! this->gripper_iface->data->inner_beam_obstruct = this->inner_beam_obstruct; ! this->gripper_iface->data->left_paddle_open = this->left_paddle_open; ! this->gripper_iface->data->right_paddle_open = this->right_paddle_open; ! this->gripper_iface->data->paddles_opened = this->paddles_opened; ! this->gripper_iface->data->paddles_closed = this->paddles_closed; ! this->gripper_iface->data->paddles_moving = this->paddles_moving; ! this->gripper_iface->data->paddles_error = this->paddles_error; ! this->gripper_iface->data->lift_up = this->lift_up; ! this->gripper_iface->data->lift_down = this->lift_down; ! this->gripper_iface->data->lift_moving = this->lift_moving; ! this->gripper_iface->data->lift_error = this->lift_error; } --- 362,388 ---- { // Data timestamp ! this->iface->data->time = this->world->GetSimTime(); ! ! this->iface->data->left_paddle_open = this->leftPaddleState == OPEN; ! this->iface->data->right_paddle_open = this->rightPaddleState == OPEN; ! ! this->iface->data->paddles_opened = this->leftPaddleState == OPEN && ! this->rightPaddleState == CLOSED; ! this->iface->data->paddles_closed = this->leftPaddleState == CLOSED && ! this->leftPaddleState == CLOSED; ! ! this->iface->data->lift_up = this->liftState == UP; ! this->iface->data->lift_down = this->liftState == DOWN; // Gripper's status ! /*this->iface->data->lift_limit_reach = this->lift_limit_reach; ! this->iface->data->grip_limit_reach = this->grip_limit_reach; ! this->iface->data->outer_beam_obstruct = this->outer_beam_obstruct; ! this->iface->data->inner_beam_obstruct = this->inner_beam_obstruct; ! this->iface->data->paddles_moving = this->paddles_moving; ! this->iface->data->paddles_error = this->paddles_error; ! this->iface->data->lift_moving = this->lift_moving; ! this->iface->data->lift_error = this->lift_error; ! */ } *************** *** 552,895 **** // Create temp variables - GzVector leftGripperPosVector = this->leftGripperBody->GetPosition(); - GzVector rightGripperPosVector = this->rightGripperBody->GetPosition(); - - dVector3 leftGripperPos; - dVector3 rightGripperPos; - - leftGripperPos[0] = leftGripperPosVector.x; - leftGripperPos[1] = leftGripperPosVector.y; - leftGripperPos[2] = leftGripperPosVector.z; - rightGripperPos[0] = rightGripperPosVector.x; - rightGripperPos[1] = rightGripperPosVector.y; - rightGripperPos[2] = rightGripperPosVector.z; - - double leftGripPos = this->leftGripperJoint->GetPosition(); - double rightGripPos = this->rightGripperJoint->GetPosition(); - double liftPos = this->baseJoint->GetPosition(); - - //update ray length to let only object inside both grips to break the ray - double rayLength = dDISTANCE(leftGripperPos, rightGripperPos) - this->gripWidth; - if (rayLength < 0.001) - { - rayLength = 0.001; - } - - /* TODO - this->innerBreakbeam->SetRange( rayLength ); - this->outerBreakbeam->SetRange( rayLength ); - - // rays collision detection - this->innerBreakbeam->Update( step ); - this->outerBreakbeam->Update( step ); - - // Update rays status - this->inner_beam_obstruct = this->innerBreakbeam->FoundContact(); - this->outer_beam_obstruct = this->outerBreakbeam->FoundContact(); - */ - - // Update grip state - switch (this->gripperMode) - { - case GRIPPER_OPEN_MODE : - // Update joints limits to fully open gripper state - this->leftGripperJoint->SetParam( dParamLoStop, this->gripMinPosLimit ); - this->rightGripperJoint->SetParam( dParamLoStop, this->gripMinPosLimit ); - - this->paddles_opened = false; - this->paddles_closed = false; - this->paddles_moving = true; - - // Stop emulating grip friction when opening grips - if (this->isGripping) - { - this->isGripping = false; - - // Patch to avoid gripper basebar to touch the gripped model and have - // ode physic effects when emulating friction. This should get rid of - // the shaking bug. - GzPose gripperPose = this->GetPose(); - //gripperPose.pos.x += 0.002; //Reposition correctly the gripper (and the robot) - this->SetPose(gripperPose); - } - - // Check if grips are fully open. - // Update joint's stop limit to emulates one way slider joint - if (leftGripPos > this->gripMinPosLimit) - { - this->leftGripperJoint->SetParam(dParamHiStop, - leftGripPos + this->ffTolerance); - } - - if (rightGripPos > this->gripMinPosLimit) - { - this->rightGripperJoint->SetParam(dParamHiStop, - rightGripPos + this->ffTolerance ); - } - - if (leftGripPos <= this->gripMinPosLimit && - rightGripPos <= this->gripMinPosLimit ) - { - this->StopGrip(); - this->paddles_opened = true; - this->paddles_closed = false; - } - break; - - case GRIPPER_CLOSE_MODE : - // Update joints limits to fully closed gripper state - this->leftGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit); - this->rightGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit); - - this->paddles_opened = false; - this->paddles_closed = false; - this->paddles_moving = true; - - if (/*!this->isGripping &&*/ leftGripPos < this->gripMaxPosLimit) - { - this->leftGripperJoint->SetParam(dParamFMax, this->gripMaxForce); - this->leftGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit); - this->leftGripperJoint->SetParam(dParamLoStop, - leftGripPos - this->ffTolerance ); - this->leftGripperJoint->SetParam(dParamVel, this->gripMaxSpeed); - - } - - if (/*!this->isGripping &&*/ rightGripPos < this->gripMaxPosLimit) - { - this->rightGripperJoint->SetParam(dParamFMax, this->gripMaxForce); - this->rightGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit); - this->rightGripperJoint->SetParam(dParamLoStop, - rightGripPos - this->ffTolerance); - this->rightGripperJoint->SetParam(dParamVel, this->gripMaxSpeed); - } - - if (leftGripPos >= this->gripMaxPosLimit && - rightGripPos >= this->gripMaxPosLimit) - { - this->StopGrip(); - this->paddles_opened = false; - this->paddles_closed = true; - } - break; - - case GRIPPER_STOP_MODE : - this->paddles_moving = false; - break; - - default : - break; - } - - // Update lift state - switch (this->liftMode) - { - case LIFT_DOWN_MODE : - // Update joints limits for down lift state - this->baseJoint->SetParam(dParamLoStop, this->liftMinPosLimit); - - this->lift_up = false; - this->lift_down = false; - this->lift_moving = true; - - // Check if lift is completely down. - // Update joint's stop limit to emulates one way slider joint - if (liftPos > this->liftMinPosLimit) - { - this->baseJoint->SetParam(dParamHiStop, liftPos + this->ffTolerance); - } - else - { - this->lift_up = false; - this->lift_down = true; - this->StopLift(); - } - break; - - case LIFT_UP_MODE : - // Update joints limits for up lift state - this->baseJoint->SetParam(dParamHiStop, this->liftMaxPosLimit); - - this->lift_up = false; - this->lift_down = false; - this->lift_moving = true; - - // Check if lift is completely up. - // Update joint's stop limit to emulates one way slider joint - if (liftPos < this->liftMaxPosLimit) - { - this->baseJoint->SetParam(dParamLoStop, liftPos - this->ffTolerance); - } - else - { - this->lift_up = true; - this->lift_down = false; - this->StopLift(); - } - break; - - case LIFT_STOP_MODE : - this->lift_moving = false; - break; - - default : - break; - } - // Update the mmap interface with the new data ! IfacePutData(); ! } ! ! ! ////////////////////////////////////////////////////////////////////////////// ! // Open the gripper ! void Pioneer2Gripper::OpenGrip() ! { ! // Check if not already fully open ! if (this->leftGripperJoint->GetPosition() > ! this->liftMinPosLimit + this->ffTolerance || ! this->rightGripperJoint->GetPosition() > ! this->liftMinPosLimit + this->ffTolerance) ! { ! this->leftGripperJoint->SetParam(dParamFMax, this->gripMaxForce); ! this->leftGripperJoint->SetParam(dParamLoStop, this->gripMinPosLimit); ! this->leftGripperJoint->SetParam(dParamHiStop, ! this->leftGripperJoint->GetPosition() + this->ffTolerance ); ! this->leftGripperJoint->SetParam(dParamVel, -this->gripMaxSpeed); ! ! this->rightGripperJoint->SetParam(dParamFMax, this->gripMaxForce); ! this->rightGripperJoint->SetParam(dParamLoStop, gripMinPosLimit); ! this->rightGripperJoint->SetParam(dParamHiStop, ! this->rightGripperJoint->GetPosition() + this->ffTolerance); ! this->rightGripperJoint->SetParam(dParamVel, -this->gripMaxSpeed); ! ! // Change grip mode ! this->gripperMode = this->GRIPPER_OPEN_MODE; ! } } ! ////////////////////////////////////////////////////////////////////////////// ! // Close the gripper ! void Pioneer2Gripper::CloseGrip() { ! // Check if not already fully close ! if (/*!this->isGripping &&*/ (this->leftGripperJoint->GetPosition() < ! this->liftMaxPosLimit - this->ffTolerance || ! this->rightGripperJoint->GetPosition() < ! this->liftMaxPosLimit - this->ffTolerance)) ! { ! this->leftGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit); ! this->leftGripperJoint->SetParam(dParamLoStop, ! this->leftGripperJoint->GetPosition() - this->ffTolerance); ! this->leftGripperJoint->SetParam(dParamVel, this->gripMaxSpeed); ! ! this->rightGripperJoint->SetParam(dParamHiStop, this->gripMaxPosLimit ); ! this->rightGripperJoint->SetParam(dParamLoStop, ! this->rightGripperJoint->GetPosition() - this->ffTolerance); ! this->rightGripperJoint->SetParam(dParamVel, gripMaxSpeed); ! } ! ! this->leftGripperJoint->SetParam(dParamFMax, this->gripMaxForce); ! this->rightGripperJoint->SetParam(dParamFMax, this->gripMaxForce ); ! ! // this->StopGrip(); ! // Change grip mode ! this->gripperMode = this->GRIPPER_CLOSE_MODE; } ! ////////////////////////////////////////////////////////////////////////////// ! // Stop the gripper ! void Pioneer2Gripper::StopGrip() { ! // Check if not already stop ! if (this->gripperMode != this->GRIPPER_STOP_MODE) ! { ! // Update grips force, velocity and stops param to be in stop state ! this->leftGripperJoint->SetParam(dParamVel, 0); ! this->rightGripperJoint->SetParam(dParamVel, 0); ! this->leftGripperJoint->SetParam(dParamFMax, 0); ! this->rightGripperJoint->SetParam(dParamFMax, 0); ! ! this->leftGripperJoint->SetParam(dParamLoStop, ! this->leftGripperJoint->GetPosition() - simFrictionTolerance); ! this->leftGripperJoint->SetParam(dParamHiStop, ! this->leftGripperJoint->GetPosition() - simFrictionTolerance); ! this->rightGripperJoint->SetParam(dParamLoStop, ! this->rightGripperJoint->GetPosition() - simFrictionTolerance); ! this->rightGripperJoint->SetParam(dParamHiStop, ! this->rightGripperJoint->GetPosition() - simFrictionTolerance); ! // Change grip mode ! this->gripperMode = this->GRIPPER_STOP_MODE; ! } } ! ////////////////////////////////////////////////////////////////////////////// ! // Stop the lift ! void Pioneer2Gripper::StopLift() { ! // Check if not already stop ! if (this->liftMode != this->LIFT_STOP_MODE) ! { ! // Update lift force, velocity and stops param to be in stop state ! this->baseJoint->SetParam(dParamVel, 0); ! this->baseJoint->SetParam(dParamFMax, 0); ! ! ! if (this->baseJoint->GetPosition() > this->liftMaxPosLimit) ! { ! this->baseJoint->SetParam(dParamHiStop, liftMaxPosLimit); ! this->baseJoint->SetParam(dParamLoStop, liftMaxPosLimit); ! } ! else ! { ! this->baseJoint->SetParam(dParamHiStop, this->baseJoint->GetPosition()); ! this->baseJoint->SetParam(dParamLoStop, this->baseJoint->GetPosition()); ! } ! ! // Change lift mode ! this->liftMode = this->LIFT_STOP_MODE; ! } } - ////////////////////////////////////////////////////////////////////////////// - // Lift up void Pioneer2Gripper::LiftUp() { ! // Check if not already up ! if (this->baseJoint->GetPosition()<this->liftMaxPosLimit - this->ffTolerance) ! { ! // Update lift force, velocity and stops param to be in up state ! this->baseJoint->SetParam(dParamFMax, this->liftMaxForce); ! this->baseJoint->SetParam(dParamHiStop, this->liftMaxPosLimit); ! this->baseJoint->SetParam(dParamLoStop, this->baseJoint->GetPosition() - this->ffTolerance); ! this->baseJoint->SetParam(dParamVel, this->liftMaxSpeed); ! ! // Change lift mode ! this->liftMode = this->LIFT_UP_MODE; ! ! this->rightGripperJoint->SetParam( dParamLoStop, this->rightGripperJoint->GetPosition() ); ! this->leftGripperJoint->SetParam( dParamLoStop, this->leftGripperJoint->GetPosition() ); ! ! this->gripperMode = this->GRIPPER_STOP_MODE; ! } } - ////////////////////////////////////////////////////////////////////////////// - // Lift down void Pioneer2Gripper::LiftDown() { ! // Check if not already down ! if (this->baseJoint->GetPosition()>this->liftMinPosLimit + this->ffTolerance) ! { ! // Update lift force, velocity and stops param to be in down state ! this->baseJoint->SetParam(dParamFMax, this->liftMaxForce); ! this->baseJoint->SetParam(dParamLoStop, this->liftMinPosLimit); ! this->baseJoint->SetParam(dParamHiStop, this->baseJoint->GetPosition() + this->ffTolerance); ! this->baseJoint->SetParam(dParamVel, -(this->liftMaxSpeed)); ! // Change lift mode ! this->liftMode = this->LIFT_DOWN_MODE; ! } } --- 395,440 ---- // Create temp variables // Update the mmap interface with the new data ! this->IfacePutData(); } ! void Pioneer2Gripper::GripOpen() { ! this->rightSlider->SetParam(dParamVel, -this->rightVel); ! this->leftSlider->SetParam(dParamVel, -this->leftVel ); ! this->rightPaddleState = OPEN; ! this->leftPaddleState = OPEN; } ! void Pioneer2Gripper::GripClose() { ! this->rightSlider->SetParam(dParamVel, this->rightVel); ! this->leftSlider->SetParam(dParamVel, this->leftVel ); ! this->rightPaddleState = CLOSED; ! this->leftPaddleState = CLOSED; } ! void Pioneer2Gripper::GripStop() { ! this->rightSlider->SetParam(dParamVel, 0.0); ! this->leftSlider->SetParam(dParamVel, 0.0 ); } void Pioneer2Gripper::LiftUp() { ! this->horizBarSlider->SetParam(dParamVel, this->liftVel); ! this->liftState = UP; } void Pioneer2Gripper::LiftDown() { ! this->horizBarSlider->SetParam(dParamVel, -this->liftVel); ! this->liftState = DOWN; ! } ! void Pioneer2Gripper::LiftStop() ! { ! this->horizBarSlider->SetParam(dParamVel, 0.0); } Index: Pioneer2Gripper.hh =================================================================== RCS file: /cvsroot/playerstage/code/gazebo/server/models/Pioneer2Gripper/Pioneer2Gripper.hh,v retrieving revision 1.7 retrieving revision 1.8 diff -C2 -d -r1.7 -r1.8 *** Pioneer2Gripper.hh 13 Apr 2005 16:32:31 -0000 1.7 --- Pioneer2Gripper.hh 26 Sep 2006 00:48:27 -0000 1.8 *************** *** 86,89 **** --- 86,91 ---- class Pioneer2Gripper : public Model { + private: enum GripperState {OPEN,CLOSED,UP,DOWN,MOVING,STOPPED}; + // Constructor, destructor public: Pioneer2Gripper( World *world ); *************** *** 126,232 **** private: void IfacePutData(); ! // Grip command functions ! public: void OpenGrip(); ! public: void CloseGrip(); ! public: void StopGrip(); ! ! // Lift command functions ! public: void StopLift(); ! public: void LiftUp(); ! public: void LiftDown(); ! ! // adds model in the list of grippable models ! public: void AddGrippableModel( Model *model); ! ! // Gripper's mode ! public: enum GripperMode{ GRIPPER_STOP_MODE = 0, GRIPPER_OPEN_MODE, GRIPPER_CLOSE_MODE }; ! // Lift's mode ! public: enum LiftMode{ LIFT_STOP_MODE = 0, LIFT_UP_MODE, LIFT_DOWN_MODE }; ! ! // Gripper's parameters ! private: dReal gripMaxSpeed; ! private: dReal liftMaxSpeed; ! ! private: dReal fixedbarLength; ! private: dReal fixedbarWidth; ! private: dReal fixedbarHeight; ! private: dReal fixedbarMass; ! ! private: dReal basebarLength; ! private: dReal basebarWidth; ! private: dReal basebarHeight; ! private: dReal basebarZPos; // Use to set the gripper fully down position ! private: dReal basebarMass; ! ! private: dReal gripLength; ! private: dReal gripWidth; ! private: dReal gripHeight; ! private: dReal gripMass; ! private: dReal gripsOpenWidth; ! private: dReal gripsCloseWidth; ! private: dReal gripMinPosLimit; ! private: dReal gripMaxPosLimit; ! private: dReal gripMaxForce; ! ! private: dReal liftHeight; ! private: dReal liftMinPosLimit; ! private: dReal liftMaxPosLimit; ! private: dReal liftMaxForce; ! ! private: dReal ffTolerance; // used instead of FudgeFactor param to counter ODE bug with joints limit ! private: dReal simFrictionTolerance; // used to avoid gripped model contact with grips for emulating friction. ! // If not used, ODE's friction model generates unwanted forces. ! ! // These boolean variables indicate the state of the gripper ! //(same nomenclature found on Player side) ! private: bool lift_limit_reach; //Unused ! private: bool grip_limit_reach; //Unused ! private: bool outer_beam_obstruct; ! private: bool inner_beam_obstruct; ! private: bool left_paddle_open; //Unused ! private: bool right_paddle_open; //Unused ! private: bool paddles_opened; ! private: bool paddles_closed; ! private: bool paddles_moving; ! private: bool paddles_error; //Unused ! private: bool lift_up; ! private: bool lift_down; ! private: bool lift_moving; ! private: bool lift_error; //Unused - // data and state - private: unsigned char state; - private: unsigned char beams; ! private: GripperMode gripperMode; ! private: LiftMode liftMode; ! private: bool isGripping; ! // ODE objects ! private: Body *body; ! private: Body *baseBarBody; ! private: Body *leftGripperBody; ! private: Body *rightGripperBody; ! ! private: Geom *fixedBarGeom; ! private: Geom *baseBarGeom; ! private: Geom *leftGripGeom; ! private: Geom *rightGripGeom; ! private: Geom *leftPressureSensorGeom; ! private: Geom *rightPressureSensorGeom; ! private: RayProximity *innerBreakbeam; ! private: RayProximity *outerBreakbeam; - private: SliderJoint *leftGripperJoint; - private: SliderJoint *rightGripperJoint; - private: SliderJoint *baseJoint; - - private: dSpaceID leftGripSpaceId; - private: dSpaceID rightGripSpaceId; - // External interface ! private: gz_gripper_t *gripper_iface; }; --- 128,159 ---- private: void IfacePutData(); ! private: void GripOpen(); ! private: void GripClose(); ! private: void GripStop(); ! private: void LiftUp(); ! private: void LiftDown(); ! private: void LiftStop(); ! // ODE objects ! private: Body *base; ! private: Body *horizBar; ! private: Body *leftPaddle; ! private: Body *rightPaddle; ! private: SliderJoint *horizBarSlider; ! private: SliderJoint *rightSlider; ! private: SliderJoint *leftSlider; ! private: float liftVel; ! private: float rightVel; ! private: float leftVel; ! private: GripperState leftPaddleState; ! private: GripperState rightPaddleState; ! private: GripperState liftState; // External interface ! private: gz_gripper_t *iface; }; |