bandit.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 of the Bandit Humanooid
 * Author: Nate Koenig 
 * Date: 31 Aug 2005
 * CVS info: $Id: Bandit.cc,v 1.3 2006/02/22 14:58:57 natepak Exp $
 */

/// @addtogroup models 
/// @{ 
/** @defgroup Bandit Bandit

@htmlinclude Bandit_view.html

The Bandit humanoid model simulates a simple humanoid torso.

@par libgazebo interfaces

- Joint information is available through the @ref joint interface.

@par Player drivers

There are currently no player drivers

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

@par Views

@htmlinclude Bandit_more_views.html

@par Authors

Nate Koenig

*/
/// @} 


#include <unistd.h>
#include <string.h>
#include <stdio.h>

#include "Body.hh"
#include "BoxGeom.hh"
#include "SphereGeom.hh"
#include "HingeJoint.hh"
#include "UniversalJoint.hh"
#include "ModelFactory.hh"
#include "World.hh"

#include "Bandit.hh"


/////////////////////////////////////////////////////////////////////////////
// Register the model
GZ_REGISTER_STATIC("Bandit", Bandit)


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


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


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Bandit::Load( WorldFile *file, WorldFileNode *node )
{
  GzVector anchor;
  GzVector torsoSize;
  GzVector shoulderSize;
  GzVector upperArmSize;
  GzVector elbowSize;
  GzVector lowerArmSize;
  GzVector handSize;
  double headRadius;
  Geom *geom;

  // Note: This model is created as if it were lying on its back.

  // Size of the torso
  torsoSize.x = 0.165;
  torsoSize.y = 0.18;
  torsoSize.z = 0.055;

  // Size of the shoulder piece
  shoulderSize.x = 0.05;
  shoulderSize.y = 0.05;
  shoulderSize.z = 0.05;

  // Size of the upper arm
  upperArmSize.x = 0.06;
  upperArmSize.y = 0.05;
  upperArmSize.z = 0.05;

  // Size of the elbow
  elbowSize.x = 0.055;
  elbowSize.y = 0.05;
  elbowSize.z = 0.05;

  // Size of the lower arm
  lowerArmSize.x = 0.075;
  lowerArmSize.y = 0.05;
  lowerArmSize.z = 0.05;

  // Size of the hand
  handSize.x = 0.09;
  handSize.y = 0.05;
  handSize.z = 0.05;

  // Radius of the head
  headRadius = 0.08;

  // Create the torso
  this->torso = new Body( this->world );
  this->torso->SetPosition(GzVectorSet(0.00, 0.00, torsoSize.z * 0.5));
  this->torso->SetAngularVel(GzVectorSet(0,0,0));
  this->torso->SetLinearVel(GzVectorSet(0,0,0));
  this->AddBody( this->torso, true );

  geom = new BoxGeom( this->torso, this->modelSpaceId, torsoSize.x, torsoSize.y,
      torsoSize.z);
  geom->SetRelativePosition(GzVectorSet(0, 0, torsoSize.z * 0.5) );
  geom->SetMass( 0.5 );
  geom->SetColor( GzColor(0.8, 0.8, 0.8) );


  // Create the left shoulder
  this->leftShoulder = new Body(this->world);
  anchor = this->torso->GetPosition();
  anchor.x += torsoSize.x*0.5-shoulderSize.x*0.5;
  anchor.y += torsoSize.y*0.5+shoulderSize.y*0.5 + 0.01;
  this->leftShoulder->SetPosition(anchor);
  this->leftShoulder->SetAngularVel(GzVectorSet(0,0,0));
  this->leftShoulder->SetLinearVel(GzVectorSet(0,0,0));
  this->leftShoulder->SetGravityMode(false);
  this->AddBody(this->leftShoulder, false);

  geom = new BoxGeom(this->leftShoulder, this->modelSpaceId, shoulderSize.x, shoulderSize.y, shoulderSize.z);
  geom->SetColor( GzColor(0, 0, 1) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, shoulderSize.z * 0.5) );


  // Create the left upper arm
  this->leftUpperArm = new Body(this->world);
  anchor = this->leftShoulder->GetPosition();
  anchor.x -= shoulderSize.x*0.5 + upperArmSize.x*0.5 + 0.005;
  this->leftUpperArm->SetPosition(anchor);
  this->leftUpperArm->SetAngularVel(GzVectorSet(0,0,0));
  this->leftUpperArm->SetLinearVel(GzVectorSet(0,0,0));
  this->leftUpperArm->SetGravityMode(false);
  this->AddBody(this->leftUpperArm, false);

  geom = new BoxGeom(this->leftUpperArm, this->modelSpaceId, upperArmSize.x, upperArmSize.y, upperArmSize.z);
  geom->SetColor( GzColor(1, 0, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, upperArmSize.z * 0.5) );

  // Create the left elbow
  this->leftElbow = new Body(this->world);
  anchor = this->leftUpperArm->GetPosition();
  anchor.x -= upperArmSize.x*0.5 + elbowSize.x*0.5 + 0.005;
  this->leftElbow->SetPosition(anchor);
  this->leftElbow->SetAngularVel(GzVectorSet(0,0,0));
  this->leftElbow->SetLinearVel(GzVectorSet(0,0,0));
  this->leftElbow->SetGravityMode(false);
  this->AddBody(this->leftElbow, false);
 
  geom = new BoxGeom(this->leftElbow, this->modelSpaceId, elbowSize.x, elbowSize.y, elbowSize.z);
  geom->SetColor( GzColor(0, 1, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0, 0.0, elbowSize.z*0.5) );

  // Create the left lower arm
  this->leftLowerArm = new Body(this->world);
  anchor = this->leftElbow->GetPosition();
  anchor.x -= elbowSize.x*0.5 + lowerArmSize.x*0.5 + 0.005;
  this->leftLowerArm->SetPosition(anchor);
  this->leftLowerArm->SetAngularVel(GzVectorSet(0,0,0));
  this->leftLowerArm->SetLinearVel(GzVectorSet(0,0,0));
  this->leftLowerArm->SetGravityMode(false);
  this->AddBody(this->leftLowerArm, false);
 
  geom = new BoxGeom(this->leftLowerArm, this->modelSpaceId, lowerArmSize.x, lowerArmSize.y, lowerArmSize.z);
  geom->SetColor( GzColor(0, 1, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0, 0.0, lowerArmSize.z*0.5) );
 

  // Create the left hand
  this->leftHand = new Body(this->world);
  anchor = this->leftLowerArm->GetPosition();
  anchor.x -= elbowSize.x*0.5 + handSize.x*0.5 + 0.005;
  this->leftHand->SetPosition(anchor);
  this->leftHand->SetAngularVel(GzVectorSet(0,0,0));
  this->leftHand->SetLinearVel(GzVectorSet(0,0,0));
  this->leftHand->SetGravityMode(false);
  this->AddBody(this->leftHand, false);

  geom = new BoxGeom(this->leftHand, this->modelSpaceId, handSize.x, handSize.y, handSize.z);
  geom->SetColor( GzColor(1, 0, 1) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, handSize.z * 0.5));


  // Left should joint
  this->joints[L_SHOULDER] = new HingeJoint(this->world);
  this->joints[L_SHOULDER]->Attach(this->torso, this->leftShoulder);
  anchor = this->leftShoulder->GetCoMPose().pos;
  this->joints[L_SHOULDER]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[L_SHOULDER])->SetAxis(GzVectorSet(0,1,0));
  this->joints[L_SHOULDER]->SetParam( dParamBounce, 0);
  this->joints[L_SHOULDER]->SetParam( dParamLoStop, DTOR(-45));
  this->joints[L_SHOULDER]->SetParam( dParamHiStop, DTOR(135.0) );
  this->joints[L_SHOULDER]->SetParam( dParamFMax, 10.1 );

  // Left upper arm joint
  this->joints[L_UPPERARM] = new HingeJoint( this->world );
  this->joints[L_UPPERARM]->Attach(this->leftShoulder, this->leftUpperArm);
  anchor = this->leftUpperArm->GetCoMPose().pos;
  this->joints[L_UPPERARM]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[L_UPPERARM])->SetAxis(GzVectorSet(1,0,0));
  this->joints[L_UPPERARM]->SetParam( dParamBounce, 0);
  this->joints[L_UPPERARM]->SetParam( dParamLoStop, DTOR(-90));
  this->joints[L_UPPERARM]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[L_UPPERARM]->SetParam( dParamFMax, 10.1 );
 
  // Left elbow joint
  this->joints[L_ELBOW] = new HingeJoint( this->world );
  this->joints[L_ELBOW]->Attach(this->leftUpperArm, this->leftElbow);
  anchor = this->leftElbow->GetCoMPose().pos;
  anchor.x += elbowSize.x * 0.5;
  this->joints[L_ELBOW]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[L_ELBOW])->SetAxis(GzVectorSet(0,1,0));
  this->joints[L_ELBOW]->SetParam( dParamBounce, 0);
  this->joints[L_ELBOW]->SetParam( dParamLoStop, DTOR(0.0));
  this->joints[L_ELBOW]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[L_ELBOW]->SetParam( dParamFMax, 1.1 );

  // Left lower arm joint
  this->joints[L_LOWERARM] = new HingeJoint( this->world );
  this->joints[L_LOWERARM]->Attach(this->leftElbow, this->leftLowerArm);
  anchor = this->leftLowerArm->GetCoMPose().pos;
  this->joints[L_LOWERARM]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[L_LOWERARM])->SetAxis(GzVectorSet(1,0,0));
  this->joints[L_LOWERARM]->SetParam( dParamBounce, 0);
  this->joints[L_LOWERARM]->SetParam( dParamLoStop, DTOR(-90.0));
  this->joints[L_LOWERARM]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[L_LOWERARM]->SetParam( dParamFMax, 1.1 );

  // Left wrist joint
  this->joints[L_WRIST] = new HingeJoint( this->world );
  this->joints[L_WRIST]->Attach( this->leftLowerArm, this->leftHand);
  anchor = this->leftHand->GetCoMPose().pos;
  anchor.x += handSize.x * 0.5;
  this->joints[L_WRIST]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[L_WRIST])->SetAxis(GzVectorSet(0,1,0));
  this->joints[L_WRIST]->SetParam( dParamLoStop, DTOR(0));
  this->joints[L_WRIST]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[L_WRIST]->SetParam( dParamFMax, 1.1 );



  // Create the right shoulder
  this->rightShoulder = new Body(this->world);
  anchor = this->torso->GetPosition();
  anchor.x += torsoSize.x*0.5-shoulderSize.x*0.5;
  anchor.y -= torsoSize.y*0.5+shoulderSize.y*0.5 + 0.01;
  this->rightShoulder->SetPosition(anchor);
  this->rightShoulder->SetAngularVel(GzVectorSet(0,0,0));
  this->rightShoulder->SetLinearVel(GzVectorSet(0,0,0));
  this->rightShoulder->SetGravityMode(false);
  this->AddBody(this->rightShoulder, false);

  geom = new BoxGeom(this->rightShoulder, this->modelSpaceId, shoulderSize.x, shoulderSize.y, shoulderSize.z);
  geom->SetColor( GzColor(0, 0, 1) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, shoulderSize.z * 0.5) );


  // Create the right upper arm
  this->rightUpperArm = new Body(this->world);
  anchor = this->rightShoulder->GetPosition();