wheelchair.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  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 a ClodBuster
 * Author: Pranav Srivastava
 * Date: 4 Sep 2003
 * Comment: The model is accurate to physical dimensions but the steering needs to be altered . As of right now, it mirrors the steering for the Pioneer2AT platform.
 * CVS: $Id: WheelChair.cc,v 1.17 2006/08/27 15:23:20 dalai_at_sfnet Exp $
 */

/// @addtogroup models
/// @{ 
/** @defgroup WheelChair Wheelchair

The WheelChair model simulates a robotic wheelchair. 

@par libgazebo interfaces

This model supports the @ref position interface.

@par Player drivers

Position information is available through the %gz_position 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:Pioneer2AT>
  <xyz>0 0 0</xyz>
</model:Pioneer2AT>
@endverbatim


@par Authors

Pranav Srivastava

*/
/// @}


#include <assert.h>
#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "SphereGeom.hh"
#include "CylinderGeom.hh"
#include "WheelGeom.hh"
#include "Hinge2Joint.hh"

#include "WheelChair.hh"
#include <stdio.h>


//////////////////////////////////////////////////////////////////////////////
// Register this class
GZ_REGISTER_STATIC("WheelChair", WheelChair);


//////////////////////////////////////////////////////////////////////////////
// Constructor
WheelChair::WheelChair( World *world )
    : Model( world )
{
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
WheelChair::~WheelChair()
{
  int i;
  if (this->body)
    delete this->body;

  for (i=0; i<8; i++)
  {
    if (this->bodyGeoms[i])
      delete this->bodyGeoms[i];
    this->bodyGeoms[i] = NULL;
  }

  for (i=0; i<2; i++)
  {
    if (this->tires[i])
      delete this->tires[i];
    this->tires[i] = NULL;
  }

  for (i=0; i<4; i++)
  {
    if (this->joints[i])
      delete this->joints[i];
    this->joints[i] = NULL;
  }

  this->body = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int WheelChair::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.5334;


  // Create the ODE objects
  if (this->OdeLoad(file, node) != 0)
    return -1;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int WheelChair::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;
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int WheelChair::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  int i;
  double SeatLength, SeatBackWidth,BackLength;
  double SeatMass;
  double WheelThick,FWheelDiam,BWheelDiam;
  Geom *geom;

  SeatLength=0.4826;
  SeatBackWidth=0.3683;
  BackLength=0.5842;
  SeatMass = 1.5;
  WheelThick = 0.0508;
  FWheelDiam=0.10;

  BWheelDiam=0.52;
  wheelDiam=BWheelDiam;
  // Create the main body of the robot
  this->body = new Body( this->world );

  this->bodyGeoms[0] = new BoxGeom( this->body, this->modelSpaceId, SeatLength,SeatBackWidth,0.07);
  this->bodyGeoms[0]->SetRelativePosition( GzVectorSet(0, 0, 0) );
  this->bodyGeoms[0]->SetMass( SeatMass );
  this->bodyGeoms[0]->SetColor( GzColor(0, 1, 0.75) );  

  this->bodyGeoms[1] = new BoxGeom( this->body, this->modelSpaceId,0.02, SeatBackWidth,BackLength);
  //this->bodyGeoms[0]->SetRelativeRotation(0,1.0,0,M_PI/2);

  this->bodyGeoms[1]->SetRelativePosition( GzVectorSet(-0.5*SeatLength, 0,BackLength/2.0));// 0.1+BackLength/2.0 );
  this->bodyGeoms[1]->SetMass( 0.5 );
  this->bodyGeoms[1]->SetColor( GzColor(0, 0, 0) );  

  this->AddBody( this->body, true );
  /*
     this->bodyGeoms[4] = new BoxGeom( this->modelSpaceId,SeatLength,0.03,0.02);
  //this->bodyGeoms[0]->SetRelativeRotation(0,1.0,0,M_PI/2);

  this->bodyGeoms[4]->SetRelativePosition(0,-SeatBackWidth*0.5+0.01,BackLength/2.0);// 0.1+BackLength/2.0 );
  this->bodyGeoms[4]->SetMass( 0 );
  this->bodyGeoms[4]->SetColor( GzColor(0, 0, 0) );  
  this->body->AttachGeom( this->bodyGeoms[4] );

  this->AddBody( this->body);

  this->bodyGeoms[5] = new BoxGeom( this->modelSpaceId,SeatLength,0.03,0.02);
  // this->bodyGeoms[2]->SetRotation(0,1.0,0,M_PI/2);
  this->bodyGeoms[5]->SetRelativePosition(0,SeatBackWidth*0.5-0.01,BackLength/2.0);

  this->bodyGeoms[5]->SetMass( 0.0 );
  this->bodyGeoms[5]->SetColor( GzColor(0, 0, 0) );  
  this->body->AttachGeom( this->bodyGeoms[5] );

  this->AddBody( this->body );
  */



  /*
     this->bodyGeoms[3] = new BoxGeom( this->modelSpaceId,0.02, SeatBackWidth,BackLength);
  //this->bodyGeoms[0]->SetRelativeRotation(0,1.0,0,M_PI/2);

  this->bodyGeoms[3]->SetRelativePosition(-0.5*SeatLength, 0,-BackLength/2.0);// 0.1+BackLength/2.0 );
  this->bodyGeoms[3]->SetMass( 0 );
  this->bodyGeoms[3]->SetColor( GzColor(0, 0, 0) );  
  this->body->AttachGeom( this->bodyGeoms[3] );

  this->AddBody( this->body);

  this->bodyGeoms[2] = new BoxGeom( this->modelSpaceId,SeatLength+0.0508, SeatBackWidth,0.02);
  // this->bodyGeoms[2]->SetRotation(0,1.0,0,M_PI/2);

  this->bodyGeoms[2]->SetRelativePosition(-0.0254, 0,-0.36);// 0.1+BackLength/2.0 );

  this->bodyGeoms[2]->SetMass( 0.0 );
  this->bodyGeoms[2]->SetColor( GzColor(0, 0, 0) );  
  this->body->AttachGeom( this->bodyGeoms[2] );

  this->AddBody( this->body );

*/

  // Create the tires
  // front two ..then the back two wheels.
  for (i = 0; i < 2; i++)
  {
    this->tires[i] = new Body( this->world );

    //this->tireGeoms[i][0] = new SphereGeom(this->modelSpaceId, 0.5 * this->wheelDiam);
    geom = new WheelGeom( this->body, this->modelSpaceId, 0.5 * FWheelDiam, WheelThick/2.0);

    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass(0.2 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );
    //geom->SetFriction1(0.001);
    //geom->SetFriction2(0.0);

    geom = new BoxGeom( this->body, this->modelSpaceId, 0.5 * FWheelDiam, 
        0.5 * FWheelDiam, 0.005);
    geom->SetRelativePosition( GzVectorSet(0, 0, WheelThick / 4.0) );
    geom->SetMass( 0.01 );
    geom->SetColor( GzColor(0.5, 1.0, 1.0) );

    this->AddBody( this->tires[i] );
  }

  for (i = 2; i < 4; i++)
  {
    this->tires[i] = new Body( this->world );

    //this->tireGeoms[i][0] = new SphereGeom(this->modelSpaceId, 0.5 * this->wheelDiam);
    geom = new WheelGeom( this->body, this->modelSpaceId, 0.5 * BWheelDiam, 
        WheelThick);

    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 1.5 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );

    geom = new BoxGeom( this->body, this->modelSpaceId, 0.5*BWheelDiam, 
        0.5*BWheelDiam, 0.001);
    geom->SetRelativePosition( GzVectorSet(0, 0, WheelThick / 2.0) );
    geom->SetMass( 0.2 );
    geom->SetColor( GzColor(0.5, 1.0, 1.0) );

    this->AddBody( this->tires[i] );
  }


  //FRONTWHEELS
  this->tires[0]->SetPosition(GzVectorSet((0.5*SeatLength/2+0.0508), +0.5*(SeatBackWidth+.05)+0.03, -0.508));
  this->tires[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->tires[1]->SetPosition(GzVectorSet((0.5*SeatLength+0.0508), -0.5*(SeatBackWidth+0.0508)-.03, -0.508));
  this->tires[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  // BACKWHEELS 
  this->tires[2]->SetPosition(GzVectorSet(-0.5*SeatLength, +0.5*(SeatBackWidth+0.0508)+.03, -0.3));
  this->tires[2]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->tires[3]->SetPosition(GzVectorSet(-0.5*(SeatLength), -0.5*(SeatBackWidth+0.0508)-.03, -0.3));
  this->tires[3]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Attach the tires to the body
  for (i = 0; i < 4; i++)
  {
    this->joints[i] = new Hinge2Joint( this->world );

    this->joints[i]->Attach(this->tires[i], this->body );

    GzVector a = tires[i]->GetPosition();
    this->joints[i]->SetAnchor( GzVectorSet(a.x, a.y, a.z) );
    this->joints[i]->SetAxis1( GzVectorSet(0,1,0));
    this->joints[i]->SetAxis2( GzVectorSet(0,1,0) );


    //   this->joints[i]->SetParam( dParamSuspensionERP, 0.2 ); // 0.4
    //  this->joints[i]->SetParam( dParamSuspensionCFM, 0.8 ); //0.8

    this->joints[i]->SetParam( dParamLoStop,-M_PI/2);
    this->joints[i]->SetParam( dParamHiStop,+M_PI/2);
  }

  this->body->SetFiniteRotationMode(1);  

  return 0;
}


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


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int WheelChair::Fini()
{
  // Finalize external interface  
  this->IfaceFini();

  // Finalize ODE objects
  this->OdeFini();

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize ODE
int WheelChair::OdeFini()
{
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void WheelChair::Update( double step )
{
  // Get commands from the external interface
  this->IfaceGetCmd();
 
  double vleft = this->wheelSpeed[0] + this->wheelSpeed[1] * this->wheelSep;

  double vright = this->wheelSpeed[0] - this->wheelSpeed[1] * this->wheelSep;

  // The Front Wheels arent powered.
  //  this->joints[0]->SetParam( dParamVel2,vright / this->wheelDiam * 2 );
  // this->joints[1]->SetParam( dParamVel2,vleft / this->wheelDiam * 2 );


  this->joints[2]->SetParam( dParamVel2, 
                             vright / this->wheelDiam * 2 );
  
  this->joints[3]->SetParam( dParamVel2, 
                             vleft / this->wheelDiam * 2 ); 
 

  // printf("angle rates: axis1 %f %f",this->joints[0]->GetAngle1Rate(),this->joints[2]->GetAngle1Rate());
 
  // printf(" axis2 %f %f\n",this->joints[0]->GetAngle2Rate(),this->joints[2]->GetAngle2Rate());


  this->joints[0]->SetParam( dParamFMax2, 10.1 ); //1.1
  this->joints[1]->SetParam( dParamFMax2, 10.1 ); // 1.1
  this->joints[2]->SetParam( dParamFMax2, 10.1 ); // 1.1
  this->joints[3]->SetParam( dParamFMax2, 10.1 ); // 1.1
  
  // Update the odometry
  this->UpdateOdometry( step );

  // Update the interface
  this->IfacePutData();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry

void WheelChair::UpdateOdometry( double step )
{
  double wd, ws;
  double d1, d2;
  double dr, da;

  wd = this->wheelDiam;
  ws = this->wheelSep;

  // Average distance travelled by left and right wheels
  d1 = step * wd * (joints[0]->GetAngle2Rate() + joints[2]->GetAngle2Rate()) / 2;
  d2 = step * wd * (joints[1]->GetAngle2Rate() + joints[3]->GetAngle2Rate()) / 2;

  dr = (d1 + d2) / 2;
  da = (d1-d2)/2*ws;  
  // Compute odometric pose
  /* this->odomPose[0] += dr * cos( this->odomPose[2] );
  this->odomPose[1] += dr * sin( this->odomPose[2] );
  this->odomPose[2] += da;
  */
  //GzVector pos;

  //pos=this->body->GetRelPointPos(0,0,0);
  /* const double *p0,*p1;
  p0=this->body->GetRelPointPos(0,0,0);
  p1=this->body->GetRelPointPos(1,0,0);
  */
  
  //GzQuatern angles=this->body->GetRotation();
  //double r,p,y;
  //GzQuaternToEuler(&r,&p,&y,angles);
  //if(y<0)
  //  y=y+2*M_PI;

  //this->odomPose[0] =pos.x;
  //this->odomPose[1] =pos.y;
  //this->odomPose[2] =y;
 
  return;
}


// Initialize the external interface
int WheelChair::IfaceInit()
{
  this->iface = gz_position_alloc();
  assert(this->iface);

  if (gz_position_create(this->iface, this->world->gz_server, this->GetId(),
        "WheelChair",this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;
  
  return 0;
}


// Finalize the external interface
int WheelChair::IfaceFini()
{
  gz_position_destroy( this->iface );
  gz_position_free( this->iface );
  this->iface = NULL;
  
  return 0;
}


// Get commands from the external interface
void WheelChair::IfaceGetCmd()
{
  /*double vr, va;

  vr = this->iface->data->cmd_vel[0];
  va = this->iface->data->cmd_vel[2];

  //printf("vr=%f va=%f \n",vr,va);  
  this->wheelSpeed[0] = vr*4.0 ;
  this->wheelSpeed[1] = va;
  */
  
  return;
}

// Update the data in the erinterface
void WheelChair::IfacePutData()
{
  // Data timestamp
  /*this->iface->data->time = this->world->GetSimTime();

  this->iface->data->odom_pose[0] = ( this->odomPose[0]);
  this->iface->data->odom_pose[1] = ( this->odomPose[1]);
  this->iface->data->odom_pose[2] = ( this->odomPose[2]);
  // printf("CLOD DUMP %f,%f,%f\n",this->odomPose[0],this->odomPose[1],this->odomPose[2]);

  */
  return;
}