peoplebot.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 PeopleBot
 * Author: Konstantinos Karantzis
 */

/// @addtogroup models 
/// @{
/** @defgroup PeopleBot PeopleBot

@htmlinclude PeopleBot_view.html

The PeopleBot model simulates an ActivMedia People Bot mobile robot base with 16 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:PeopleBot>
  <xyz>0 0 0</xyz>
</model:PeopleBot>
@endverbatim

@par Views

@htmlinclude PeopleBot_more_views.html

@par Authors

Konstantinos Karantzis

*/
/// @} 


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

#include "PeopleBot.hh"


//////////////////////////////////////////////////////////////////////////////
// Register this model as static
GZ_REGISTER_STATIC("PeopleBot", PeopleBot);


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


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


//////////////////////////////////////////////////////////////////////////////
// Load the model
int PeopleBot::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.35;
  this->wheelDiam = 0.19;

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

  // Create the ODE objects
  if (this->LoadODE(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 PeopleBot::LoadODE( WorldFile *file, WorldFileNode *node )
{
  int i;
  Geom *geom;
  GzVector a;


  double mass = 5.0;
  double casterMass = 6.0;

  GzVector bodySize = GzVectorSet(0.380, 0.270, 0.170);
  GzVector bodyPos = GzVectorSet(-0.050, 0, -0.170/2);

  GzVector topSize = GzVectorSet(0.450, 0.380, 0.005);
  GzVector topPos = GzVectorSet(-0.050, 0, 0);

  GzVector riserSize = GzVectorSet(0.135, 0.06, 0.875); //used for both sliders
  GzVector riserPos = GzVectorSet(-0.1, +0.5 * bodySize.y -  0.5 * riserSize.y, 0.5 * riserSize.z);

  GzVector topPlateSize = GzVectorSet(0.33, 0.27, 0.05);
  GzVector topPlatePos = GzVectorSet(-0.05, 0, 0.9);

  GzVector wheelSize = GzVectorSet(this->wheelDiam, this->wheelDiam, 0.05);
  GzVector wheelPos = GzVectorSet(0, this->wheelSep/2, -0.145);

  GzVector casterSize = GzVectorSet(0.08, 0.08, 0.08);
  GzVector casterPos = GzVectorSet(-0.220, 0, wheelPos.z - wheelSize.x/2 + casterSize.x/2);
  
  // Create the main chassis of the robot
  this->chassis = new Body( this->world );

  geom = new BoxGeom(this->chassis, this->modelSpaceId, bodySize.x, bodySize.y, bodySize.z);
  geom->SetRelativePosition( GzVectorSet(bodyPos.x, bodyPos.y, bodyPos.z) );
  geom->SetMass( mass );
  geom->SetColor( GzColor(1, 0, 0) );  

  geom = new BoxGeom(this->chassis, this->modelSpaceId, topSize.x, topSize.y, topSize.z);
  geom->SetRelativePosition( GzVectorSet(topPos.x, topPos.y, topPos.z) );
  geom->SetMass( 0 );
  geom->SetColor( GzColor(0, 0, 0) );  

  geom = new BoxGeom(this->chassis, this->modelSpaceId, riserSize.x, riserSize.y, riserSize.z);
  geom->SetRelativePosition(GzVectorSet(riserPos.x, riserPos.y, riserPos.z));
  geom->SetMass( 0 );
  geom->SetColor( GzColor(1.0, 1.0, 1.0) );

  geom = new BoxGeom(this->chassis, this->modelSpaceId, riserSize.x, riserSize.y, riserSize.z);
  geom->SetRelativePosition(GzVectorSet(riserPos.x, - riserPos.y, riserPos.z));
  geom->SetMass( 0 );
  geom->SetColor( GzColor(1.0, 1.0, 1.0) );

  geom = new BoxGeom(this->chassis, this->modelSpaceId, topPlateSize.x, topPlateSize.y, topPlateSize.z);
  geom->SetRelativePosition(GzVectorSet(topPlatePos.x, topPlatePos.y, topPlatePos.z));
  geom->SetMass( 0 );
  geom->SetColor( GzColor(1, 0, 0) );

  this->AddBody( this->chassis, true );

  // Create the wheels
  for (i = 0; i < 2; i++)
  {
    this->wheels[i] = new Body( this->world );

    geom = new WheelGeom(this->wheels[i], this->modelSpaceId, 0.5 * wheelSize.x, wheelSize.z);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );
    this->AddBody( this->wheels[i] );
  }

  // Set the wheel positions
  this->wheels[0]->SetPosition(GzVectorSet(wheelPos.x, +wheelPos.y, wheelPos.z));
  this->wheels[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->wheels[1]->SetPosition(GzVectorSet(wheelPos.x, -wheelPos.y, wheelPos.z));
  this->wheels[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Attach the wheels to the chassis at their current positions
  for (i = 0; i < 2; i++)
  {
    this->wheelJoints[i] = new HingeJoint( this->world );
    this->wheelJoints[i]->Attach( this->wheels[i], this->chassis );
    a = wheels[i]->GetPosition();
    this->wheelJoints[i]->SetAnchor( a );
    this->wheelJoints[i]->SetAxis( GzVectorSet(0, 1, 0) );
    this->wheelJoints[i]->SetParam( dParamSuspensionERP, 0.4 );
    this->wheelJoints[i]->SetParam( dParamSuspensionCFM, 0.8 );
  }

  // Create the castor wheel
  this->castorWheel = new Body(this->world);
  geom = new SphereGeom(this->castorWheel, this->modelSpaceId, casterSize.x/2);
  geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
  geom->SetMass(casterMass);
  geom->SetColor(GzColor(0, 0, 0));

  this->castorWheel->SetPosition(casterPos);
  this->castorWheel->SetRotation(GzQuaternFromEuler(M_PI / 2, 0, 0));
  this->AddBody(this->castorWheel);

  // Attach the castor to the chassis
  this->castorJoint = new BallJoint(this->world);
  this->castorJoint->Attach(this->castorWheel, this->chassis);
  a = this->castorWheel->GetPosition();
  this->castorJoint->SetAnchor( a );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load the sonar
int PeopleBot::LoadSonar( WorldFile *file, WorldFileNode *node )
{
  int i;
  GzVector a, b;
  double posZ = -0.04;
  
  this->sonarMaxRange = 5.0;
  
  pos[0] = GzVectorSet( 0.14103, 0.135, posZ );