clodbuster.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
 * CVS: $Id: ClodBuster.cc,v 1.29 2006/08/27 15:23:20 dalai_at_sfnet Exp $
 * Comment: The model is accurate to physical dimensions and implements front steering 4wd  */

/// @addtogroup models
/// @{ 
/** @defgroup ClodBuster Clod Buster

@htmlinclude ClodBuster_view.html

The ClodBuster model simulates the UPenn ClodBuster robot (4 wheel
drive robot with Ackerman steering).


@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

- rawEncoder (bool, on/off)
  - Set to true to use raw encoders.
  - Default: false

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

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

@par Views

@htmlinclude ClodBuster_more_views.html

@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 "HingeJoint.hh"

#include "ClodBuster.hh"
#include <stdio.h>
#include <ode/ode.h>
#define sign(x) (x/fabs(x))
#define RMIN 1.000
const double RBIG=1e6; 
const double kSteer=1; 


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


//////////////////////////////////////////////////////////////////////////////
// Constructor
ClodBuster::ClodBuster( World *world )
  : Model( world )
{
  int i;

  this->encoders[0] = 0;
  this->encoders[1] = 0;

  for (i=0;i <4; i++)
  {
    this->bodyGeoms[i] = NULL;
    this->tires[i] = NULL;
    this->stubs[i] = NULL;
    this->wjoints[i] = NULL;
    this->sjoints[i] = NULL;
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
ClodBuster::~ClodBuster()
{
  int i;

  delete this->body;
  this->body = NULL;

  for (i=0;i <4; i++)
  {
    if (this->bodyGeoms[i])
      delete this->bodyGeoms[i];
    if (this->tires[i])
      delete this->tires[i];
    if (this->stubs[i])
      delete this->stubs[i];
    if (this->wjoints[i])
      delete this->wjoints[i];
    if (this->sjoints[i])
      delete this->sjoints[i];

    this->bodyGeoms[i] = NULL;
    this->tires[i] = NULL;
    this->stubs[i] = NULL;
    this->wjoints[i] = NULL;
    this->sjoints[i] = NULL;
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int ClodBuster::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.17;
  this->wheelDiam = 0.15;

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

  // Model id
  
  this->raw_encoder_position = node->GetBool( "rawEncoder", false );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int ClodBuster::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 ClodBuster::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  int i;
  double length, width, height;
  double mass;
  double wheelThick;
  Geom *geom;

  length = 0.35;
  width = 0.17;
  height = 0.1;//0.19
  mass = 3.0;//10.0;
  wheelThick = 0.10;//0.10
  
  // Create the main body of the robot
  this->body = new Body( this->world );

  //  this->bodyGeoms[0] = new BoxGeom( this->modelSpaceId, 0.9 * length, 
  //     0.9 * width, height);
  GzVector bodySize = GzVectorSet(length, width, 0.01);
  this->bodyGeoms[0] = new BoxGeom(this->body, this->modelSpaceId, bodySize.x, bodySize.y, bodySize.z);
  this->bodyGeoms[0]->SetRelativePosition( GzVectorSet(0, 0, 0) );
  this->bodyGeoms[0]->SetMass( mass );
  this->bodyGeoms[0]->SetColor( GzColor(0.5, 0.5, 0.5) );  
  //this->body->AttachGeom( this->bodyGeoms[0] ); 

  this->bodyGeoms[1] = new BoxGeom(this->body, this->modelSpaceId, 0.9*length, 0.9*width, 0.01 );
  this->bodyGeoms[1]->SetRelativePosition(GzVectorSet(0, 0, 0.5 * height));
  this->bodyGeoms[1]->SetMass( 0 );
  this->bodyGeoms[1]->SetColor( GzColor(0.5, 0.5, 0.5) );  
  //this->body->AttachGeom( this->bodyGeoms[1] );

  this->AddBody( this->body, true );
   
  this->bodyGeoms[1] = new BoxGeom(this->body, this->modelSpaceId, 0.9*length, 0.9*width, 0.01 );
  this->bodyGeoms[1]->SetRelativePosition(GzVectorSet(0, 0, 0.5 * height));
  this->bodyGeoms[1]->SetMass( 0 );
  this->bodyGeoms[1]->SetColor( GzColor(0.5, 0.5, 0.5) ); 

  // Create the tires
  for (i = 0; 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->tires[i], this->modelSpaceId, 0.5 * this->wheelDiam, 0.5 * wheelThick);

    geom->SetRelativePosition(GzVectorSet(0, 0, 0));
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(0.3, 0.3, 0.3) );
    //    this->tires[i]->AttachGeom( geom );

    geom = new BoxGeom(this->tires[i], this->modelSpaceId, 0.5 * this->wheelDiam, 
                        0.5 * this->wheelDiam, 0.02);
    geom->SetRelativePosition(GzVectorSet(0, 0, wheelThick / 2));
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(1.0, 1.0, 1.0) );
    //if(i==0||i==1)
     // geom->SetFriction1(0.1);
    //this->tires[i]->AttachGeom( geom );

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

  this->tires[0]->SetPosition(GzVectorSet(0.4*length, +0.5*width+.03, -0.1));
  this->tires[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->tires[1]->SetPosition(GzVectorSet(0.4*length, -0.5*width-.03, -0.1));
  this->tires[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  this->tires[2]->SetPosition(GzVectorSet(-0.4*length, +0.5*width+.03, -0.1));
  this->tires[2]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->tires[3]->SetPosition(GzVectorSet(-0.4*length, -0.5*width-.03, -0.1));
  this->tires[3]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  
  for (i = 0; i < 4; i++)
  {
    this->stubs[i] = new Body( this->world );

    geom = new SphereGeom(this->stubs[i], this->modelSpaceId, 0.01);

    geom->SetRelativePosition(GzVectorSet(0, 0, 0));