/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Model for Pioneer2 Gripper
 * Author: Carle Cote 
 *           (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 23 february 2004
 * CVS: $Id: Pioneer2Gripper.cc,v 1.11 2004/11/17 03:33:15 natepak Exp $
 *
 * Modification description :
 *
 * Author: Carle Cote 
 *           (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 21 march 2004 
 * Desc: Patch the shaking bug. This should be a temporary patch until using 
 *        ODE's physic  model for friction 
 *
 * Author: Carle Cote 
 *           (Laborius - Universite de Sherbrooke - Sherbrooke, Quebec, Canada)
 * Date: 26 may 2004 
 * Desc: Fix model with refactored core
 *          - Change collision detection mechanism
 *          - Change pressure pads to a no collision geoms with their own spaceID
 *          - Move pressure pads a little bit inside the grips to help detect gripped state 
 */

/** @addtogroup models */
/** @{ */
/** @defgroup Pioneer2Gripper


@htmlinclude Pioneer2Gripper_view.html


The Pioneer2Gripper model simulates the ActivMedia Pioneer2 Gripper. Every object in the world that can be picked up by a gripper must set the <canBeGrip> to true.

@par libgazebo interfaces

This model supports the @ref gripper interface.

@par Player drivers

Control of the gripper position is available through the %gz_gripper driver.


@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html


@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

@verbatim
<model:Pioneer2Gripper>
  <xyz>0 0 0</xyz>
</model:Pioneer2Gripper>
@endverbatim

@par Views

@htmlinclude Pioneer2Gripper_more_views.html

@par Authors

Carle Cote

*/
/** @} */


#include <assert.h>
#include <iostream>

#include "gazebo.h"
#include "World.hh"
#include "Body.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "RayGeom.hh"
#include "BoxGeom.hh"
#include "Pioneer2Gripper.hh"
#include "SliderJoint.hh"
#include "RayProximity.hh"

using namespace std;


//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("Pioneer2Gripper", Pioneer2Gripper);


//////////////////////////////////////////////////////////////////////////////
// Constructor
Pioneer2Gripper::Pioneer2Gripper( World *world ) : Model( world )
{  
  // Init grippable models variables
  this->grippableModels = NULL;
  this->grippedModel = NULL;
  this->grippableModelsCount = 0;
  this->grippableModelsMaxCount = 0;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
Pioneer2Gripper::~Pioneer2Gripper()
{

}

//////////////////////////////////////////////////////////////////////////////
// Load the model
int Pioneer2Gripper::Load( WorldFile *file, WorldFileNode *node )
{
  // Create the ODE objects
  if ( this->OdeLoad( file, node ) != 0 )
    return -1;

  return 0;
}

void Pioneer2Gripper::set_inner_beam (double height) {
  double beamheight = -0.1;
  GzVector initpoint = 
    GzVectorSet(this->gripLength/2 + this->fixedbarLength + this->basebarLength - 0.01, -(this->gripsOpenWidth/2) + 0.01, beamheight+height);
  GzVector finalpoint = 
    GzVectorSet(this->gripLength/2 + this->fixedbarLength + this->basebarLength - 0.01, (this->gripsOpenWidth/2) - 0.01, beamheight+height);
  this->innerBreakbeam->SetRay (0, initpoint, finalpoint);
}

void Pioneer2Gripper::set_outer_beam (double height) {
  double beamheight = -0.1;
  GzVector initpoint =
    GzVectorSet(this->gripLength / 2 + this->fixedbarLength + this->basebarLength + 0.01, -(this->gripsOpenWidth/2.0) + 0.01, beamheight+height);
  GzVector finalpoint =
    GzVectorSet(this->gripLength / 2 + this->fixedbarLength + this->basebarLength + 0.01, (this->gripsOpenWidth/2.0) - 0.01, beamheight+height);
  this->outerBreakbeam->SetRay (0, initpoint, finalpoint);
}



//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
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 = gripsOpenWidth + 2 * 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 = gripsOpenWidth / 2 - gripsCloseWidth / 2;
  this->liftMinPosLimit = 0;
  this->liftMaxPosLimit = liftHeight;
  
  this->gripMaxSpeed = 0.1;
  this->gripMaxForce = 1;
  this->liftMaxSpeed = 0.05;
  this->liftMaxForce = 25;
  
  this->ffTolerance = 0.0001;
  this->simFrictionTolerance = 0.0025;
  
  // Build ODE objects
  this->body = new Body( this->world );
  this->baseBarBody = new Body( this->world );
  this->leftGripperBody = new Body( this->world );
  this->rightGripperBody = new Body( this->world );
  
  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 ) );
  
  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->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) ) );
  
  
  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->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);

  set_inner_beam (0.0);
  set_outer_beam (0.0);


  // 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->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 );

  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->gripMinPosLimit );
  
  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->gripMinPosLimit );
  
  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 );  
  
  this->contactGroupID = dJointGroupCreate( 0 );
  
  // Initialize gripper state to fully open and lift down
  this->rightGripperHaveContact = false;
  this->leftGripperHaveContact = false;
  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();

  gripper_init_height = -1;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Pioneer2Gripper::Init( WorldFile *file, WorldFileNode *node )
{  
  // Initialize ODE objects
  if ( this->OdeInit( file, node ) != 0 )
    return -1;
  // Initialize external interface
  if ( this->IfaceInit() != 0 )
    return -1;
  
  //Found and initialize grippable models
  Model **models = this->world->GetModels();
  int numModels = this->world->GetNumModels();
  
  this->grippableModelsCount = 0;
  this->grippableModelsMaxCount = 0;
    
  for ( int i = 0; i < numModels; i++ )
  {
    if ( strcmp( models[i]->node->GetString( "canBeGrip", "no", 0 ), "yes") == 0 )
    {
      this->AddGrippableModel( models[i] );
    }
  }
  
  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Add a grippable model
void Pioneer2Gripper::AddGrippableModel( Model *model )
{
  // Re-size array as needed
  if ( this->grippableModelsCount >= this->grippableModelsMaxCount )
  {
    this->grippableModelsMaxCount += 10;
    this->grippableModels = ( Model** ) realloc( this->grippableModels, 
                                                 this->grippableModelsMaxCount * sizeof( this->grippableModels[0] ) );
    assert( this->grippableModels );
  }

  this->grippableModels[ this->grippableModelsCount ] = model;
  this->grippableModelsCount++;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize ODE
int Pioneer2Gripper::OdeInit( WorldFile *file, WorldFileNode *node )
{
  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize the external interface
int Pioneer2Gripper::IfaceInit()
{
  this->gripper_iface = gz_gripper_alloc();
  if ( gz_gripper_create( this->gripper_iface, this->world->gz_server, this->GetId(),
                          "Pioneer2Gripper", (int) this, (int) this->parent) != 0 )
    return -1;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Pioneer2Gripper::Fini()
{
  // Finalize external interface  
  this->IfaceFini();
    
  // Finalize ODE objects
  this->OdeFini();
  
  free( this->grippableModels );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize ODE
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;
}

//////////////////////////////////////////////////////////////////////////////
// Finalize the external interface
int Pioneer2Gripper::IfaceFini()
{
  gz_gripper_destroy( this->gripper_iface );
  gz_gripper_free( this->gripper_iface );
  this->gripper_iface = NULL;

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Pioneer2Gripper::IfaceGetCmd()
{
  
  // Receives command from external interface.
  
  // Commands are defined in playerclient.h in Player's client directory.
  // GRIPopen   1
  // GRIPclose  2
  // GRIPstop   3
  // LIFTup     4
  // LIFTdown   5
  // LIFTstop   6
  // GRIPstore  7
  // GRIPdeploy 8
  // GRIPhalt   15
  // GRIPpress  16 -- not supported
  // 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;
  }
  
  return;
}

//////////////////////////////////////////////////////////////////////////////
// Update external interface
void Pioneer2Gripper::IfacePutData()
{
  // 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;
}

//////////////////////////////////////////////////////////////////////////////
// Update the model
void Pioneer2Gripper::Update( double step )
{
  // Need to destroy created contacts if using ODE friction model  
  //dJointGroupEmpty( this->contactGroupID );
  
  // Get commands from the external interface
  this->IfaceGetCmd();
  
  // 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;

  if (gripper_init_height < 0.0) {
    gripper_init_height = leftGripperPos[2];
  }
  
  double leftGripPos = this->leftGripperJoint->GetPosition();
  double rightGripPos = this->rightGripperJoint->GetPosition();
  double liftPos = this->baseJoint->GetPosition();
  
  // Do the collision detection to evaluates if grips are in contact with grippable objects
  if(!this->isGripping) {
    // Check each grippable objects one by one with each grip
    for ( int i = 0; i < this->grippableModelsCount; i++ ) {
      
      //      std::cerr << "CHECK GRIPPABLE MODEL: " << i << std::endl;
      
      this->rightGripperHaveContact = false;
      this->leftGripperHaveContact = false; 
      
      this->contactFound = false;
      dSpaceCollide2( ( dGeomID ) ( this->leftGripSpaceId ),
		      ( dGeomID ) ( this->grippableModels[i]->modelSpaceId ),
		      this, 
		      &UpdateGripCallback );
      
      if(this->contactFound) {
        this->leftGripperHaveContact = true;      
	std::cerr << "LEFT CONTACT: " << i << std::endl;
      }
      
      this->contactFound = false;
      dSpaceCollide2( ( dGeomID ) ( this->rightGripSpaceId ),
		      ( dGeomID ) ( this->grippableModels[i]->modelSpaceId ),
		      this, 
		      &UpdateGripCallback );
  
      if(this->contactFound) {
        this->rightGripperHaveContact = true;       
	std::cerr << "RIGHT CONTACT: " << i << std::endl;
      }
                     
      if ( this->leftGripperHaveContact && this->rightGripperHaveContact ) {
        this->grippedModel = this->grippableModels[i];
        
        // Attach model's body to gripper body (emulating friction)      
	
        // 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; //Move the gripper (and the robot) just a bit
	//	this->SetPose(gripperPose);
	
        
        //Body* grabBody = this->grippedModel->GetBody();
        if ( this->grippedModel->parent == NULL ) {
	  //this->grippedModel->SetParent( this, this->GetBody() );
	  //this->grippedModel->Attach();

          // Store gripper's initial pos when grabbing
          gripInitialPos = leftGripperPosVector;
          
          // Store model's initial pos when grabbed
          grippedModelInitialPos = this->grippedModel->GetPose().pos;

	  // Store the relative position to be able to update x,y
	  dBodyGetPosRelPoint (this->GetBody()->GetBodyId (),
			       grippedModelInitialPos.x,
			       grippedModelInitialPos.y,
			       grippedModelInitialPos.z,
			       gripped_local);

          
	  cerr << "Set isGripping true: " << gripped_local[0] 
	       << "  " << gripped_local[1] << "  " 
	       << gripped_local[2] << endl;
          this->isGripping = true;   
          
          // Stop the gripper (emulates pressure detection)
	  this->StopGrip();
        }
        break;
      }
    }
    if (this->leftGripperHaveContact) {
      std::cerr << "LEFT CONTACT AFTER FOR LOOP" << std::endl;
    }
    if (this->rightGripperHaveContact) {
      std::cerr << "RIGHT CONTACT AFTER FOR LOOP" << std::endl;
    }
  }
  //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 );
  this->innerBreakbeam->Update();
  this->outerBreakbeam->Update();
  
  // Update rays status
  //  this->inner_beam_obstruct = this->innerBreakbeam->FoundContact();
  //  this->outer_beam_obstruct = this->outerBreakbeam->FoundContact();
  this->inner_beam_obstruct = (this->innerBreakbeam->GetRange(0) < 10.0);
  this->outer_beam_obstruct = (this->outerBreakbeam->GetRange(0) < 10.0);
  //  std::cerr << "RANGE: " << this->innerBreakbeam->GetRange(0) << " --- " 
  //	    << this->outerBreakbeam->GetRange(0) << std::endl;
   
  // Update grip state  
  switch ( this->gripperMode ) {
  case GRIPPER_OPEN_MODE :

    //      std::cerr << "GRIPPER_OPEN_MODE: " << std::endl;

    // 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 opeing grips
    if ( this->isGripping ) {
      std::cerr << "GRIPPER_OPEN_MODE: Detach gripped model" << std::endl;

      this->isGripping = false;
      //      this->grippedModel->Detach();

      // 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 ) {
      cerr << "LEFT GRIP NOT FULLY OPEN" << endl;
      this->leftGripperJoint->SetParam( dParamHiStop, 
					leftGripPos + this->ffTolerance );
    }
      
    if ( rightGripPos > this->gripMinPosLimit ) {
      cerr << "RIGHT GRIP NOT FULLY OPEN" << endl;
      this->rightGripperJoint->SetParam( dParamHiStop, 
					 rightGripPos + this->ffTolerance );
    }
      
    if ((leftGripPos <= this->gripMinPosLimit) && 
	(rightGripPos <= this->gripMinPosLimit)) {
      cerr << "GRIPS FULLY OPEN (StopGrip())" << endl;
      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;

    //      std::cerr << "GRIPPER_CLOSE_MODE: " << std::endl;
      
    // Check if grips are fully closed or in contact with grippable object. 
    // Update joint's stop limit to emulates one way slider joint
    if (this->leftGripperHaveContact) {
      cerr << "GRIPPER_CLOSE_MODE: LEFT GRIPPER HAVE CONTACT" << endl;
    }

    if ( this->leftGripperHaveContact ) {
      // Stop grip and move it away from the contact object (emulating friction)
      // If not moving it away, ODE's friction model is interfering
      std::cerr << "GRIPPER_CLOSE_MODE: LEFT MOVE AWAY" << std::endl;
      this->leftGripperJoint->SetParam( dParamVel, 0 );
      this->leftGripperJoint->SetParam( dParamFMax, 0 );
      this->leftGripperJoint->SetParam( dParamLoStop, 
					leftGripPos - simFrictionTolerance );
      this->leftGripperJoint->SetParam( dParamHiStop, 
					leftGripPos - simFrictionTolerance );
    } else {
      if ( !this->isGripping && leftGripPos < this->gripMaxPosLimit ) {
	std::cerr << "CLOSING UPDATE Left LoStop: " << leftGripPos << " - " 
		  << this->ffTolerance << std::endl;
	  
        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->rightGripperHaveContact ) {
      // Stop grip and move it away from the contact object (emulating friction)
      // If not moving it away, ODE's friction model is interfering
      std::cerr << "GRIPPER_CLOSE_MODE: RIGHT MOVE AWAY" << std::endl;
      this->rightGripperJoint->SetParam( dParamVel, 0 );
      this->rightGripperJoint->SetParam( dParamFMax, 0 );
      this->rightGripperJoint->SetParam( dParamLoStop, 
					 rightGripPos - simFrictionTolerance );
      this->rightGripperJoint->SetParam( dParamHiStop, 
					 rightGripPos - simFrictionTolerance );
    } else {
      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 ) {
      cerr << "BOTH PADDLES CLOSED" << endl;
      this->StopGrip();
      this->paddles_opened = false;
      this->paddles_closed = true;
    }
    break;
    
  case GRIPPER_STOP_MODE :
    //      std::cerr << "GRIPPER_STOP_MODE: " << std::endl;
    this->paddles_moving = false;
    break;
    
  default : 
    std::cerr << "UNKNOWN GRIPPER MODE: " << std::endl;
    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 gripped model position (emulating friction)
  if ( isGripping ) {
    // Is this correct? Visually, the models does'nt seem to be at the correct height...
    GzPose newGripModelPose = this->grippedModel->GetPose();
    newGripModelPose.pos.z = this->grippedModelInitialPos.z + 
      leftGripperPos[2] - this->gripInitialPos.z;
    dBodyGetRelPointPos (this->GetBody()->GetBodyId (),
			 gripped_local[0], gripped_local[1], 
			 gripped_local[2], gripped_world);
    newGripModelPose.pos.x = gripped_world[0];
    newGripModelPose.pos.y = gripped_world[1];
    this->grippedModel->SetPose( newGripModelPose );
    //    std::cerr << "Update gripped model pose: " 
    //	      << newGripModelPose.pos.x << " "
    //	      << newGripModelPose.pos.y << " "
    //	      << newGripModelPose.pos.z << " "
    //	      << std::endl;
  }

  // Update ray height

  double h = leftGripperPos[2] - this->gripper_init_height;
  //cerr << this->gripper_init_height << "  " << leftGripperPos[2] << " -- " <<
  //    h << endl;

  // Works for DX but to low for AT.
  //  set_inner_beam (h);
  //  set_outer_beam (h);
  

  // Update the mmap interface with the new data
  IfacePutData();
}

//////////////////////////////////////////////////////////////////////////////
// Callback for grip intersection test
void Pioneer2Gripper::UpdateGripCallback( void *data, dGeomID o1, dGeomID o2 )
{
  Pioneer2Gripper *self = ( Pioneer2Gripper* )data;

  
  dBodyID b1ID = dGeomGetBody( o1 );
  dBodyID b2ID = dGeomGetBody( o2 );
  
  // Do not test collision if objects connected
  if ( b1ID && b2ID && dAreConnected( b1ID, b2ID ) )
  {
    return;
  }
  
  // REMOVE dGeomID object;
  
  if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) 
  {
    // colliding a space with something
    dSpaceCollide2 (o1,o2,data,&UpdateGripCallback);
  }
  else
  {    
    //parameters used when using ODE's physics for friction
    dContact contact[4];
    for ( int c = 0; c < 4; c++ )
    {
      contact[c].surface.mode = dContactSoftERP | dContactSoftCFM | dContactBounce;
      contact[c].surface.mu = 0;
      contact[c].surface.soft_erp = 0;
      contact[c].surface.soft_cfm = 0;
      contact[c].surface.bounce = 0;
      contact[c].surface.slip1 = 0.0;
      contact[c].surface.slip2 = 0.0;
    }
    
    // Find collision contact points
    int n = dCollide( o1, o2, 4, &contact[0].geom, sizeof( dContact ) );
    
    // Update grip state
    if ( n > 0 )
    {
      self->contactFound = true;
    }
  }
}

//////////////////////////////////////////////////////////////////////////////
// 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 )
  {
    // Update grips force, velocity and stops param to be in open state (if no contact)
    if ( !this->leftGripperHaveContact )
    {
       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 );
    }
    
    if ( !this->rightGripperHaveContact )
    {
       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 ) )
  {
    // Update grips force, velocity and stops param to be in close state (if no contact)
    if ( !this->leftGripperHaveContact )
    {
       this->leftGripperJoint->SetParam( dParamFMax, this->gripMaxForce );
       this->leftGripperJoint->SetParam( dParamHiStop, this->gripMaxPosLimit );
       this->leftGripperJoint->SetParam( dParamLoStop,  this->leftGripperJoint->GetPosition() - this->ffTolerance );
       this->leftGripperJoint->SetParam( dParamVel, this->gripMaxSpeed );
    }
    
    if ( !this->rightGripperHaveContact )
    {
       this->rightGripperJoint->SetParam( dParamFMax, this->gripMaxForce );
       this->rightGripperJoint->SetParam( dParamHiStop, this->gripMaxPosLimit );
       this->rightGripperJoint->SetParam( dParamLoStop,  this->rightGripperJoint->GetPosition() - this->ffTolerance );
       this->rightGripperJoint->SetParam( dParamVel, gripMaxSpeed );
    }
    
    // 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;
  }
}

//////////////////////////////////////////////////////////////////////////////
// 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;
  }
}