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

/// @addtogroup models 
/// @{
/** @defgroup Atrv

@htmlinclude Atrv_view.html

The Atrv model simulates an ActivMedia Atrv 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:Atrv>
  <xyz>0 0 0</xyz>
</model:Atrv>
@endverbatim

@par Views

@htmlinclude Atrv_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"

// TODO: Fix the PrismGeom before including this model
#include "PrismGeom.hh"
#include "WheelGeom.hh"
#include "HingeJoint.hh"
#include "RayProximity/RayProximity.hh"

#include "Atrv.hh"

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


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


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


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Atrv::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.50;
  this->wheelDiam = 0.40;

  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 Atrv::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  int i;
  double mass;
  double wheelSep, wheelDiam, wheelThick;
  Geom *geom;

  mass = 10.0;

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

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

  // Bottom chassis
  double extractVector[3] = {0., 0.45, 0.};
  double vertices[18] = {0.34, -0.225, 0.12,
			 -0.34, -0.225, 0.12,
			 -0.42, -0.225, 0.20,
			 -0.42, -0.225, 0.38,
			 0.42, -0.225, 0.38,
			 0.42, -0.225, 0.20};
  
  geom = new PrismGeom( this->chassis, this->modelSpaceId, 0, extractVector, 6, vertices );
  geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
  geom->SetMass( mass );
  geom->SetColor( GzColor(1, 0, 0) );
 
  this->AddBody( this->chassis, true );

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

    geom = new WheelGeom( this->wheels[i], this->modelSpaceId, wheelDiam/2, wheelThick/2);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 1.0 );
    geom->SetColor( GzColor(0.3, 0.3, 0.3) );

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

  this->wheels[0]->SetPosition(GzVectorSet(0.25, +(0.5 * wheelSep + wheelThick / 2), -0.055));
  this->wheels[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->wheels[1]->SetPosition(GzVectorSet(0.25, -(0.5 * wheelSep + wheelThick / 2), -0.055));
  this->wheels[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  this->wheels[2]->SetPosition(GzVectorSet(-0.25, +(0.5 * wheelSep + wheelThick / 2), -0.055));
  this->wheels[2]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  this->wheels[3]->SetPosition(GzVectorSet(-0.25, -(0.5 * wheelSep + wheelThick / 2), -0.055));
  this->wheels[3]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));

  // Attach the wheels to the chassis
  for (i = 0; i < 4; i++)
  {
    this->wheelJoints[i] = new HingeJoint( this->world );
    this->wheelJoints[i]->Attach( this->wheels[i], this->chassis );

    GzVector 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 );
  }

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load the sonar
int Atrv::LoadSonar( WorldFile *file, WorldFileNode *node )
{
  int i;
  GzVector a, b;
  
  this->sonarMaxRange = 5.0;

  // Dimensions
  a.x = +0.41;
  a.y = +0.175;
  a.z = +( 0.425 - 0.195 );

  b.x = +0.33;
  b.y = +0.215;
  b.z = +( 0.425 - 0.195 );
  
  pos[0]  = GzVectorSet( -a.x, +a.y, a.z );
  pos[1]  = GzVectorSet( -b.x, +b.y, a.z );
  pos[2]  = GzVectorSet( -b.x, -b.y, a.z );
  pos[3]  = GzVectorSet( -a.x, -a.y, a.z );
  pos[4]  = GzVectorSet( +a.x, 0.07, a.z );
  pos[5]  = GzVectorSet( +a.x, 0.115, a.z );
  pos[6]  = GzVectorSet( +a.x, 0.17, a.z );
  pos[7]  = GzVectorSet( +b.x, +b.y, a.z );
  pos[8]  = GzVectorSet( +a.x, -0.07, a.z );
  pos[9]  = GzVectorSet( +a.x, -0.115, a.z );
  pos[10] = GzVectorSet( +a.x, -0.17, a.z );
  pos[11] = GzVectorSet( +b.x, -b.y, a.z );
  
  dir[0] = 180;
  dir[1] = 90;
  dir[2] = -90;
  dir[3] = 180;
  dir[4] = 0;
  dir[5] = 15;
  dir[6] = 30;
  dir[7] = 90;
  dir[8] = 0;
  dir[9] = -15;
  dir[10] = -30;
  dir[11] = -90;


  this->sonarSensor = new RayProximity(this->world, this->chassis, 12);
  for (i = 0; i < 12; i++)
  {
    a = pos[i];
    b = GzVectorSet(this->sonarMaxRange * cos(dir[i] * M_PI / 180),
                    this->sonarMaxRange * sin(dir[i] * M_PI / 180), 0.0);
    b = GzVectorAdd(a, b); 
    this->sonarSensor->SetRay(i, a, b);
  }
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Atrv::Init( WorldFile *file, WorldFileNode *node )
{
  // Create position interface
  this->position_iface = gz_position_alloc();
  assert(this->position_iface);

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

  // Create power inteface
  this->power_iface = gz_power_alloc();
  if (gz_power_create(this->power_iface, this->world->gz_server, this->GetId(),
                      "Atrv", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  // Create sonar inteface
  this->sonar_iface = gz_sonar_alloc();
  if (gz_sonar_create(this->sonar_iface, this->world->gz_server, this->GetId(),
                       "Atrv", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;

  // Reset odometric pose
  this->odomPose[0] = 0.0;
  this->odomPose[1] = 0.0;
  this->odomPose[2] = 0.0;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Atrv::Fini()
{
  // Close sonar interface
  gz_sonar_destroy( this->sonar_iface );
  gz_sonar_free( this->sonar_iface );
  this->sonar_iface = NULL;

  // Close power interface
  gz_power_destroy( this->power_iface );
  gz_power_free( this->power_iface );
  this->power_iface = NULL;

  // Close position interface
  gz_position_destroy( this->position_iface );
  gz_position_free( this->position_iface );
  this->position_iface = NULL;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void Atrv::Update( double step )
{
  // Do nothing if paused
  if (step == 0)
    return;
  
  // Update the odometry (do this always for better accuracy)
  this->UpdateOdometry( step );

  // Otherwise, update periodically
  if (this->world->GetSimTime() - this->updateTime > this->updatePeriod)
  {
    this->updateTime = this->world->GetSimTime();

    // Get commands from the external interface
    this->GetPositionCmd();

    this->wheelJoints[0]->SetParam( dParamVel, 
                                    this->wheelSpeed[1] / this->wheelDiam * 2 );
    this->wheelJoints[1]->SetParam( dParamVel, 
                                    this->wheelSpeed[0] / this->wheelDiam * 2 );
    this->wheelJoints[2]->SetParam( dParamVel, 
                                    this->wheelSpeed[1] / this->wheelDiam * 2 );
    this->wheelJoints[3]->SetParam( dParamVel, 
                                    this->wheelSpeed[0] / this->wheelDiam * 2 );

    this->wheelJoints[0]->SetParam( dParamFMax, 10.0 );
    this->wheelJoints[1]->SetParam( dParamFMax, 10.0 );
    this->wheelJoints[2]->SetParam( dParamFMax, 10.0 );
    this->wheelJoints[3]->SetParam( dParamFMax, 10.0 );
  
    // Update the sonar sensor
    this->sonarSensor->Update();

    // Update the interface
    this->PutPositionData();
    this->PutPowerData();
    this->PutSonarData();
  }
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry
void Atrv::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 / 2 * 
    (wheelJoints[0]->GetAngleRate() + wheelJoints[2]->GetAngleRate()) / 2;
  d2 = step * wd / 2 * 
    (wheelJoints[1]->GetAngleRate() + wheelJoints[3]->GetAngleRate()) / 2;

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

  // Compute odometric instantaneous velocity
  this->odomVel[0] = dr / step;
  this->odomVel[1] = 0.0;
  this->odomVel[2] = da / step;

  // Update the power discharge; this is probably completely bogus
  this->batteryLevel -= this->batteryCurve[0] * step;
  this->batteryLevel -= this->batteryCurve[1] * d1 * d1;
  this->batteryLevel -= this->batteryCurve[1] * d2 * d2;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Atrv::GetPositionCmd()
{
  double vr, va;

  gz_position_lock(this->position_iface, 1);
  vr = this->position_iface->data->cmd_vel_pos[0];
  va = this->position_iface->data->cmd_vel_rot[2];
  gz_position_unlock(this->position_iface);
  
  this->wheelSpeed[0] = vr + va * this->wheelSep / 2;
  this->wheelSpeed[1] = vr - va * this->wheelSep / 2;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void Atrv::PutPositionData()
{
  gz_position_lock(this->position_iface, 1);
    
  // Data timestamp
  this->position_iface->data->time = this->world->GetSimTime();

  this->position_iface->data->pos[0] = this->odomPose[0];
  this->position_iface->data->pos[1] = this->odomPose[1];
  this->position_iface->data->rot[2] = this->odomPose[2];

  this->position_iface->data->vel_pos[0] = this->odomVel[0];
  this->position_iface->data->vel_pos[1] = this->odomVel[1];
  this->position_iface->data->vel_rot[2] = this->odomVel[2];

  gz_position_unlock(this->position_iface);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the power interface
void Atrv::PutPowerData()
{
  gz_power_lock(this->power_iface, 1);
    
  this->power_iface->data->time = this->world->GetSimTime();
  this->power_iface->data->levels[0] = this->batteryLevel;

  gz_power_unlock(this->power_iface);
    
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the sonar interface
void Atrv::PutSonarData()
{
  int i;
  double r;

  gz_sonar_lock(this->sonar_iface, 1);
    
  // Data timestamp
  this->sonar_iface->data->time = this->world->GetSimTime();

  // Sonar count valid
  this->sonar_iface->data->sonar_count = 12;
  
  // Sonar's data
  for (i = 0; i < 12; i++)
  {
    r = Min(this->sonarSensor->GetRange(i), this->sonarMaxRange);

    this->sonar_iface->data->sonar_pos[i][0] = pos[i].x;
    this->sonar_iface->data->sonar_pos[i][1] = pos[i].y;
    this->sonar_iface->data->sonar_pos[i][2] = pos[i].z;

    this->sonar_iface->data->sonar_rot[i][0] = 0;
    this->sonar_iface->data->sonar_rot[i][1] = 0;
    this->sonar_iface->data->sonar_rot[i][2] = dir[i] * M_PI / 180;

    this->sonar_iface->data->sonar_ranges[i] = r;
  }

  gz_sonar_unlock(this->sonar_iface);
    
  return;
}