segwayrmp.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 SegwayRMP
 * Author: Andrew Howard, Marin Kobilarov
 * Date: 8 May 2003
 * CVS: $Id: SegwayRMP.cc,v 1.39 2006/02/22 15:29:24 natepak Exp $
 */

/// @addtogroup models
/// @{ 
/** @defgroup SegwayRMP Segway RMP

@htmlinclude SegwayRMP_view.html

The SegwayRMP model simulates an ActivMedia SegwayRMP mobile robot
base.  Note that the sonars are not simulated.


@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:SegwayRMP>
  <xyz>0 0 0</xyz>
</model:SegwayRMP>
@endverbatim

@par Views

@htmlinclude SegwayRMP_more_views.html

@par Authors

Andrew Howard, Marin Kobilarov

*/
/// @}

#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 "SliderJoint.hh"
#include "HingeJoint.hh"

#include "SegwayRMP.hh"


/* System Gains */

#define RMP_CTRL_LQR_K1     7.0
#define RMP_CTRL_LQR_K2     4.0
#define RMP_CTRL_LQR_K3    41.0
#define RMP_CTRL_LQR_K4    15.0

#define RMP_CTRL_LIN_KP   -11.0
#define RMP_CTRL_LIN_KI     0.0
#define RMP_CTRL_LIN_KD     3.5

#define RMP_CTRL_ANG_KP     2.0
#define RMP_CTRL_ANG_KI     0.02
#define RMP_CTRL_ANG_KD     0.1


// uncomment this to get plottable data in the file specified
// #define RUN_LOG "run.log"


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


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

  this->angVelPID = NULL;
  this->linVelPID = NULL;
  this->desState = NULL;
  this->curState = NULL;

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

  this->base = NULL;
  this->top = NULL;
  this->topJoint = NULL;

  this->odomPose[0] = this->odomPose[1] = this->odomPose[2] = 0.0;
  this->odomOrient[0] = this->odomOrient[1] = 0.0;
  
  return;
}


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

  if (this->angVelPID)
    delete this->angVelPID;
  if (this->linVelPID)
    delete this->linVelPID;
  if (this->desState)
    delete this->desState;
  if (this->curState)
    delete this->curState;

  if (this->base)
    delete this->base;
  if (this->top)
    delete this->top;
  if (this->topJoint)
    delete this->topJoint;

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

    this->tires[i] = NULL;
    this->tireJoints[i] = NULL;
  }

  this->base = NULL;
  this->top = NULL;
  this->topJoint = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int SegwayRMP::Load( WorldFile *file, WorldFileNode *node )
{
  // Create the ODE objects
  if (this->OdeLoad(file, node) != 0)
    return -1;

  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int SegwayRMP::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 SegwayRMP::OdeLoad( WorldFile *file, WorldFileNode *node )
{

  int i;

  double baseMass = 2.0;
  double baseSx = 0.35;
  double baseSy = 0.60;
  double baseSz = 0.10;

  double topMass = 10.0;
  double topSx = 0.60;
  double topSy = 0.80;
  double topSz = 0.04;
  double topHeight = 0.70;

  double riserMass = 2.0;
  double riserSx = 0.30;
  double riserSy = 0.04;
  double riserSz = 0.70;
  double riserHeight = 0.35;

  double tireMass = .3;
  double tireDiam = 0.50;
  double tireThick = 0.05;
  double axelHeight = 0.05;

  double oz = 0.182;

  BoxGeom *boxGeom;
  //SphereGeom *sphGeom;
  WheelGeom *cylGeom;

  // Constants used for odometry
  this->wheelSep = baseSy + tireThick;
  this->wheelDiam = tireDiam;

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

  // Make the base
  boxGeom = new BoxGeom( this->base, this->modelSpaceId, baseSx, baseSy, baseSz );
  boxGeom->SetRelativePosition( GzVectorSet(0, 0, oz) );
  boxGeom->SetMass( baseMass );
  boxGeom->SetColor( GzColor(0.7, 0.7, 0.8) );

  // Make the risers
  boxGeom = new BoxGeom( this->base, this->modelSpaceId, riserSx, riserSy, riserSz );
  boxGeom->SetRelativePosition(GzVectorSet(0,+0.5 * baseSy - riserSy, riserHeight + oz));
  boxGeom->SetMass( riserMass );
  boxGeom->SetColor( GzColor(1.0, 1.0, 1.0) );

  boxGeom = new BoxGeom( this->base, this->modelSpaceId, riserSx, riserSy, riserSz );
  boxGeom->SetRelativePosition(GzVectorSet(0,-0.5 * baseSy + riserSy, riserHeight + oz));
  boxGeom->SetMass( riserMass );
  boxGeom->SetColor( GzColor(1.0, 1.0, 1.0) );

  // Make the top of the robot
  this->top = new Body( this->world, "topPlate" );
  this->top->SetPosition( GzVectorSet( 0, 0, topHeight + oz ) );

  // Make the top plate
  boxGeom = new BoxGeom( this->top, this->modelSpaceId, topSx, topSy, topSz );
  boxGeom->SetMass( topMass );
  boxGeom->SetColor( GzColor(0.1, 0.1, 0.1) );

  // Attach the top to the base
  this->topJoint = new SliderJoint( this->world->worldId );
  this->topJoint->Attach( this->top, this->base );
  this->topJoint->SetAxis( 0, 0, 1 );
  this->topJoint->SetParam( dParamLoStop, -0.001 );
  this->topJoint->SetParam( dParamHiStop, +0.001 );

  // Create the tires.  Some of the geoms are purely decorative.
  for (i = 0; i < 2; i++)
  {
    this->tires[i] = new Body( this->world );
    cylGeom = new WheelGeom( this->tires[i], this->modelSpaceId, 0.5 * tireDiam, tireThick );
    cylGeom->SetMass( tireMass );
    cylGeom->SetColor( GzColor(0.1, 0.1, 0.1) );
    //cylGeom->SetFriction2(5);       cylGeom->SetFriction1(5);
  }

  this->tires[0]->SetPosition( GzVectorSet( 0.00, -0.5 * baseSy - 0.5 *