shrimp.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 Shrimp
 * Author: Stijn Opheide, Jef Marien, Koen Jans
 * Date: 8 May 2003
 * CVS: $Id: Shrimp.cc,v 1.14 2006/08/27 15:23:20 dalai_at_sfnet Exp $
 */


/// @addtogroup models
/// @{
/** @defgroup Shrimp Shrimp

@htmlinclude Shrimp_view.html

The Shrimp model simulates the BlueBotics Shrimp robot. This model is currently under development.


@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

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


@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

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

@par Views

@htmlinclude Shrimp_view.html

@par Authors

Stijn Opheide, Jef Marien, Koen Jans

*/
/// @}


#include <assert.h>
#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "CylinderGeom.hh"
#include "WheelGeom.hh"
#include "HingeJoint.hh"

#include "SideFork.hh"
#include "Shrimp.hh"
//#include <iostream>

using namespace std;


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


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


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


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Shrimp::Load( WorldFile *file, WorldFileNode *node )
{
  this->wheelSep = 0.381;
  this->wheelDiam = 0.116;

  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;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Shrimp::Init( WorldFile *file, WorldFileNode *node )
{
  // Initialize external interface
  this->iface = gz_position_alloc();
  assert(this->iface);
  if (gz_position_create(this->iface, this->world->gz_server, this->GetId(),
                         "Shrimp", 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;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int Shrimp::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  Geom *geom;

  double bodylength = 0.267;
  double bodywidth = 0.250;
  double bodyheight = 0.068;
  double bodymass = 0.1;
  double barMass = 0.05;
  double barWidth = 0.018;
  double wheelMass = 0.5;

  //double wheelSep = this->wheelSep;
  double wheelDiam = this->wheelDiam;
  double wheelThick = 0.02;

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

  geom = new BoxGeom(this->chassis, this->modelSpaceId, bodylength, bodywidth, bodyheight);
  geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
  geom->SetMass( bodymass );
  geom->SetColor( GzColor(1, 0, 0) );

  //add the back wheel
  geom = new BoxGeom(this->chassis, this->modelSpaceId, 0.18632498, barWidth, barWidth);
  geom->SetRelativePosition( GzVectorSet(0.1705, 0.0, -0.0515) );
  geom->SetRelativeRotation( GzQuaternFromAxis(0,1,0,1.1623809) );
  geom->SetMass( barMass );
  geom->SetColor( GzColor(1, 0, 0) );

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

  geom = new WheelGeom( this->backWheel, modelSpaceId, 0.5 * wheelDiam,  wheelThick);
  geom->SetRelativePosition( GzVectorSet(0,0,wheelThick/2) );
  geom->SetMass( wheelMass );
  geom->SetColor( GzColor(0.1, 0.1, 0.1) );

  this->AddBody( this->backWheel );

  this->backWheel->SetPosition(GzVectorSet( 0.2075, 0, -0.186));
  this->backWheel->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));

  HingeJoint *backJoint = new HingeJoint( this->world );
  backJoint->Attach(this->chassis,this->backWheel);
  GzVector bwp = this->backWheel->GetPosition();
  backJoint->SetAnchor( bwp);
  backJoint->SetAxis(GzVectorSet(0,1,0));

  //construct the left fork

  leftFork = new ShrimpSideFork(this);
  //position the fork right
  leftFork->Translate(-0.0355,-0.125,0.034);
  //add the bodies to this model
  this->AddBody(leftFork->GetTopBar());
  this->AddBody(leftFork->GetBottomBar());
  this->AddBody(leftFork->GetFrontBar());
  this->AddBody(leftFork->GetBackBar());
  //join the top and bottom bar to the chassis
  HingeJoint *leftForkTopJoint, *leftForkBottomJoint;

  leftForkTopJoint = new HingeJoint(this->world);
  leftForkTopJoint->Attach(leftFork->GetTopBar(),chassis);
  leftForkTopJoint->SetAnchor(GzVectorSet(-0.0355,-0.125,0.034-barWidth/2));
  leftForkTopJoint->SetAxis(GzVectorSet(0,1,0));

  leftForkBottomJoint = new HingeJoint(this->world);
  leftForkBottomJoint->Attach(leftFork->GetBottomBar(),chassis);
  leftForkBottomJoint->SetAnchor(GzVectorSet(-0.0355,0.125,-0.034 + barWidth/2));
  leftForkBottomJoint->SetAxis(GzVectorSet(0,1,0));

  //construct the right fork
  rightFork = new ShrimpSideFork(this);
  //position the fork right
  rightFork->Translate(-0.0355,0.125,0.034);
  //add the bodies to this model
  this->AddBody(rightFork->GetTopBar());
  this->AddBody(rightFork->GetBottomBar());
  this->AddBody(rightFork->GetFrontBar());
  this->AddBody(rightFork->GetBackBar());
  //join the top and bottom bar to the chassis
  HingeJoint *rightForkTopJoint, *rightForkBottomJoint;

  rightForkTopJoint = new HingeJoint(this->world);
  rightForkTopJoint->Attach(rightFork->GetTopBar(),chassis);
  rightForkTopJoint->SetAnchor(GzVectorSet(-0.0355,-0.225-barWidth/2,0.025));
  rightForkTopJoint->SetAxis(GzVectorSet(0,1,0));

  rightForkBottomJoint = new HingeJoint(this->world);
  rightForkBottomJoint->Attach(rightFork->GetBottomBar(),chassis);
  rightForkBottomJoint->SetAnchor(GzVectorSet(-0.0355,-0.225-barWidth/2,-0.025));
  rightForkBottomJoint->SetAxis(GzVectorSet(0,1,0));

  //attach wheels

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

    geom = new WheelGeom(this->wheels[i], this->modelSpaceId, 0.5 * wheelDiam,  wheelThick);
    geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );

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

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

  //the 0th wheel is front right
  this->wheels[0]->SetPosition(GzVectorSet(-bodylength/2 + barWidth/2, bodywidth/2 + barWidth, bodyheight/2 - 0.171));
  this->wheels[0]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  //the 1st wheel is front left
  this->wheels[1]->SetPosition(GzVectorSet(-bodylength/2 + barWidth/2, -bodywidth/2 - barWidth, bodyheight/2 - 0.171));
  this->wheels[1]->SetRotation(GzQuaternFromAxis(1, 0, 0, +M_PI / 2));
  //the 2nd wheel is back right
  this->wheels[2]->SetPosition(GzVectorSet(-bodylength/2 + 0.192 - barWidth/2, bodywidth/2 + barWidth, bodyheight/2 - 0.171));
  this->wheels[2]->SetRotation(GzQuaternFromAxis(1, 0, 0, -M_PI / 2));
  //the 3th wheel is back left
  this->wheels[3]->SetPosition(GzVectorSet(-bodylength/2 + 0.192 - barWidth/2, -bodywidth/2 - barWidth, bodyheight/2 - 0.171));
  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[0]->Attach( this->wheels[0], rightFork->GetFrontBar() );
  this->wheelJoints[1]->Attach( this->wheels[1], leftFork->GetFrontBar() );
  this->wheelJoints[2]->Attach( this->wheels[2], rightFork->GetBackBar() );
  this->wheelJoints[3]->Attach( this->wheels[3], leftFork->GetBackBar() );

  for (i = 0; i < 4; i++)
  {
    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 );
  }


  // **************************
  // **     FRONTFORK	     **
  // **************************

  // zie tekening
  double b = 0.040;