b21r.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 B21R
 * Author: Andrew Howard
 * Date: 8 May 2003
 * CVS: $Id: B21R.cc,v 1.4 2006/04/05 14:51:57 natepak Exp $
 */

/// @addtogroup models
/// @{
/** @defgroup B21R

@htmlinclude B21R_view.html

The B21R model simulates an B21R mobile robot base with 48 sonars.

@par libgazebo interfaces

- Position information is available through the @ref position interface.
- Power information is available through the @ref power interface.
- Sonar information is available through the @ref sonar interface.


@par Player drivers

- Position information is available through the %gz_position driver.
- Power information is available through the %gz_power driver.
- Sonar information is available through the %gz_sonar driver.

@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- updateRate (float, Hz)
  - Updates per second
  - Default: 10

- batteryLevel (float, volts)
  - Initial battery level
  - Default: 12.4

- batteryCurve (float)
  - Discharge curve: about one hour quiescent
  - Default: 2/3600  2/1e4


@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

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

@par Views

@htmlinclude B21R_more_views.html

@par Authors

Andrew Howard, Nate Koenig

*/
/// @}

#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 "PrismGeom.hh"
#include "WheelGeom.hh"
#include "HingeJoint.hh"
#include "BallJoint.hh"
#include "RayProximity/RayProximity.hh"

#include "B21R.hh"
#include "B21RConst.hh"

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


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


//////////////////////////////////////////////////////////////////////////////
// Destructor
B21R::~B21R()
{
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int B21R::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.46;
  this->wheelDiam = 0.10;

  this->updatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6);
  this->updateTime = -updatePeriod;

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

  // Create the sonar sensor
  if (this->LoadSonar(file, node) != 0)
    return -1;

  // Initial battery level
  this->batteryLevel = node->GetDouble("batteryLevel", 12.4);

  // Discharge curve: about one hour quiescent
  this->batteryCurve[0] = node->GetTupleDouble("batteryCurve", 0, 2 / 3600.0);
  this->batteryCurve[1] = node->GetTupleDouble("batteryCurve", 1, 2 / 1e4);  

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int B21R::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  int i;
  double mass;
  double radius, heigth;
  double wheelSep, wheelDiam, wheelThick;
  Geom *geom;

  mass = 50.0;
  radius = 0.65 / 2;
  heigth = 1.10;

  wheelSep = this->wheelSep;
  wheelDiam = this->wheelDiam;
  wheelThick = 0.01;

  
  // Create the main chassis of the robot
  this->chassis = new Body( this->world );

  // Bottom chassis
  geom = new CylinderGeom( this->chassis, this->modelSpaceId, radius, 0.2*heigth);
  geom->SetRelativePosition( GzVectorSet(0, 0, -0.4*heigth) );
  geom->SetMass( mass );
  geom->SetColor( GzColor(1, 0, 0) );
 
  this->AddBody( this->chassis, true );

  // Upper chassis
   geom = new CylinderGeom( this->chassis, this->modelSpaceId, radius, 0.8*heigth);
   geom->SetRelativePosition( GzVectorSet(0, 0, 0.1*heigth) );
   geom->SetMass( 0.0 );
   geom->SetColor( GzColor(1, 0, 0) );
 
  // Create the wheels
  for (i = 0; i < 3; i += 2)
  {
    this->wheels[i+1] = new Body( this->world );
    
    geom = new WheelGeom( this->wheels[i+1], this->modelSpaceId, wheelDiam/2, wheelThick/2);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 1 );
    geom->SetColor( GzColor(0.3, 0.3, 0.3) );

    this->AddBody( this->wheels[i+1] );

    this->wheels[i] = new Body( this->world );
    
    geom = new SphereGeom( this->wheels[i], this->modelSpaceId, wheelDiam/2);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 1 );
    geom->SetColor( GzColor(0.3, 0.3, 0.3) );

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

  this->wheels[0]->SetPosition(GzVectorSet(wheelSep/2, 0, -heigth/2 - wheelDiam/2 -0.01));
  //  this->wheels[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  this->wheels[1]->SetPosition(GzVectorSet(0.0, wheelSep/2, -heigth/2 - wheelDiam/2 -0.01));
  this->wheels[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->wheels[2]->SetPosition(GzVectorSet(-wheelSep/2, 0, -heigth/2 - wheelDiam/2 -0.01));
  //  this->wheels[2]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  this->wheels[3]->SetPosition(GzVectorSet(0.0, -wheelSep/2, -heigth/2 - wheelDiam/2 -0.01));
  this->wheels[3]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Attach the wheels to the chassis
  double erp, cmf;
  erp = 0.8;
  cmf = 0.0;
  
  {
    this->wheelJoints[1] = new HingeJoint( this->world );
    this->wheelJoints[1]->Attach( this->wheels[1], this->chassis );

    GzVector a = wheels[1]->GetPosition();    
    this->wheelJoints[1]->SetAnchor( a );
    this->wheelJoints[1]->SetAxis( GzVectorSet(0, 1, 0) );
    this->wheelJoints[1]->SetParam( dParamSuspensionERP, erp );
    this->wheelJoints[1]->SetParam( dParamSuspensionCFM, cmf );
  }
  {
    this->wheelJoints[3] = new HingeJoint( this->world );
    this->wheelJoints[3]->Attach( this->wheels[3], this->chassis );

    GzVector a = wheels[3]->GetPosition();    
    this->wheelJoints[3]->SetAnchor( a );
    this->wheelJoints[3]->SetAxis( GzVectorSet(0, 1, 0) );
    this->wheelJoints[3]->SetParam( dParamSuspensionERP, erp );
    this->wheelJoints[3]->SetParam( dParamSuspensionCFM, cmf );
  }
  {
    this->wheel2Joints[0] = new BallJoint( this->world->worldId );
    this->wheel2Joints[0]->Attach( this->wheels[0], this->chassis );
  
    GzVector a = wheels[0]->GetPosition();    
    this->wheel2Joints[0]->SetAnchor( a );
    //    this->wheel2Joints[0]->SetAxis1( GzVectorSet(0, 0, 1) );
    //    this->wheel2Joints[0]->SetAxis2( GzVectorSet(0, 1, 0) );
    this->wheel2Joints[0]->SetParam( dParamSuspensionERP, erp );
    this->wheel2Joints[0]->SetParam( dParamSuspensionCFM, cmf );
  }
  {
    this->wheel2Joints[2] = new BallJoint( this->world->worldId );
    this->wheel2Joints[2]->Attach( this->wheels[2], this->chassis );

    GzVector a = wheels[2]->GetPosition();    
    this->wheel2Joints[2]->SetAnchor( a );
    //    this->wheel2Joints[2]->SetAxis1( 0, 0, 1 );
    //    this->wheel2Joints[2]->SetAxis2( 0, 1, 0 );
    this->wheel2Joints[2]->SetParam( dParamSuspensionERP, erp );
    this->wheel2Joints[2]->SetParam( dParamSuspensionCFM, cmf );
  }