avatarheli.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 the USC AVATAR helicopter
 * Author: Andrew Howard
 * Date: 8 Feb 2004
 * CVS: $Id: AvatarHeli.cc,v 1.19 2006/02/22 14:58:57 natepak Exp $
 */

/// @addtogroup models
/// @{
/** @defgroup AvatarHeli Avatar Helicopter


@htmlinclude AvatarHeli_view.html

The AvatarHeli model simulates the USC AVATAR helicopter, a Bergen
Industrial Twin RC helicopter
(http://www.bergenrc.com/IndustrialTwin.asp).


@par libgazebo interfaces

The AvatarHeli currently does not support any interfaces. 


@par Player drivers

No player drivers are available.

@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

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

- skinFile (string, filename)
  - Specify an 3D geometry file to create an arbitrary appearance.
  - Default: empty

- skinXyz (float tuple, meters)
  - Skin pose relative to the model
  - Default: 0 0 0

- skinRpy (float tuple, degrees)
  - Skin rotation relative to the model in Euler angles: roll, pitch, yaw
  - Default: 0.0 0.0 0.0

- skinScale (float tuple, meters)
  - Scale factor for skin file.
  - Default: 1.0 1.0 1.0

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html

@par Example

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

@par Views

@htmlinclude AvatarHeli_more_views.html

@par Authors

Srikanth Saripalli


*/
/// @}

#if HAVE_CONFIG_H
#include <config.h>
#endif

#include <assert.h>

#include "gazebo.h"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "Error.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "SphereGeom.hh"
#include "CylinderGeom.hh"
#include "BallJoint.hh"

#include "AvatarHeli.hh"


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


//////////////////////////////////////////////////////////////////////////////
// Constructor
AvatarHeli::AvatarHeli( World *world )
    : Model( world )
{
  this->body = NULL;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
AvatarHeli::~AvatarHeli()
{
  if (this->body)
    delete this->body;

  this->body = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int AvatarHeli::Load( WorldFile *file, WorldFileNode *node )
{
  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;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int AvatarHeli::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  Geom *geom;
  
  // Model origin is the bottom of the heli (skids), coaxial with
  // rotor.

  GzVector skidSize = GzVectorSet(1.00, 0.02, 0.02);
  GzVector skidPos = GzVectorSet(0.0, 0.40, 0.025);
  
  GzVector boxSize = GzVectorSet(0.50, 0.25, 0.20);
  GzVector boxPos = GzVectorSet(0.0, 0, 0.30);
  double boxMass = 2.0;
  
  GzVector bodySize = GzVectorSet(0.50, 0.15, 0.20);
  GzVector bodyPos = GzVectorSet(0.15, 0, 0.50);
  double bodyMass = 4.0;

  GzVector tailSize = GzVectorSet(1.2, 0.02, 0.02);
  GzVector tailPos = GzVectorSet(-0.6, 0, 0.60);

  double rotorSize = 2.0;
  GzVector rotorPos = GzVectorSet(0, 0, 0.80);

  double tailRotorSize = 0.30;
  GzVector tailRotorPos = GzVectorSet(-1.2, 0, 0.60);
  
  this->body = new Body(this->world);
  this->AddBody(this->body, true);
  
  // Create heli body
  geom = new BoxGeom(this->body, this->modelSpaceId, 
                     bodySize.x, bodySize.y, bodySize.z);
  geom->SetMass(bodyMass);
  geom->SetRelativePosition(GzVectorSet(bodyPos.x, bodyPos.y, bodyPos.z));
  geom->SetColor(GzColor(0, 0, 0));
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  const char *skinFile;
  
  // Apply a skin to chassis
  skinFile = node->SearchFilename("skinFile", GAZEBO_SKINS_PATH, NULL);
  if (skinFile)
  {
    PRINT_MSG1(1, "loading skin file [%s]", skinFile);  
    if (geom->SetSkinFile(skinFile) != 0)
    {
      PRINT_ERR("unable to load skin file");
      return -1;
    }
  }

  // Skin geometry
  GzVector pos;
  GzQuatern rot;
  pos = node->GetPosition("skinXyz", GzVectorZero());
  rot = node->GetRotation("skinRpy", GzQuaternIdent());  
  geom->SetSkinPose(GzPoseSet(pos, rot));
  geom->SetSkinScale(node->GetPosition("skinScale", GzVectorSet(1, 1, 1)));  

  geom = new BoxGeom(this->body, this->modelSpaceId, 0.05, 0.05, rotorPos.z - bodyPos.z);
  geom->SetRelativePosition(GzVectorSet(rotorPos.x, rotorPos.y, (rotorPos.z + bodyPos.z) / 2));
  geom->SetMass(bodyMass);
  geom->SetColor(GzColor(0, 0, 0));
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  // Create the tail frame
  geom = new BoxGeom(this->body, this->modelSpaceId, tailSize.x, tailSize.y, tailSize.z);
  geom->SetRelativePosition(GzVectorSet(tailPos.x, tailPos.y, tailPos.z));
  geom->SetMass(0);
  geom->SetColor(GzColor(0, 0, 0));
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);
  
  // Create the electronics box
  geom = new BoxGeom(this->body,this->modelSpaceId, boxSize.x, boxSize.y, boxSize.z);
  geom->SetRelativePosition(GzVectorSet(boxPos.x, boxPos.y, boxPos.z));
  geom->SetMass(boxMass);
  geom->SetColor(GzColor(1, 1, 1));
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  // Create the landing gear
  geom = new BoxGeom(this->body,this->modelSpaceId, skidSize.x, skidSize.y, skidSize.z);
  geom->SetRelativePosition(GzVectorSet(skidPos.x, +skidPos.y, skidPos.z));
  geom->SetMass(0);
  geom->SetColor(GzColor(0.5, 0.5, 0.5));
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  geom = new BoxGeom(this->body,this->modelSpaceId, skidSize.x, skidSize.y, skidSize.z);
  geom->SetRelativePosition(GzVectorSet(skidPos.x, -skidPos.y, skidPos.z));
  geom->SetMass(0);
  geom->SetColor(GzColor(0.5, 0.5, 0.5));
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  // Create the main rotor
  geom = new CylinderGeom(this->body,this->modelSpaceId, rotorSize / 2, 0.001);
  geom->SetRelativePosition(GzVectorSet(rotorPos.x, rotorPos.y, rotorPos.z));
  geom->SetMass(0);
  geom->SetTransparency(true);
  geom->SetColor(GzColor(0.9, 0.9, 0.0, 0.5));
  geom->SetLaser(0);
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);

  // Create the tail rotor
  geom = new CylinderGeom(this->body,this->modelSpaceId, tailRotorSize / 2, 0.001);
  geom->SetRelativePosition(GzVectorSet(tailRotorPos.x, tailRotorPos.y, tailRotorPos.z));
  geom->SetRelativeRotation(GzQuaternFromAxis(1, 0, 0, M_PI / 2));
  geom->SetMass(0);
  geom->SetTransparency(true);  
  geom->SetColor(GzColor(0.9, 0.9, 0.0, 0.5));
  geom->SetLaser(0);
  geom->SetSkinNull(skinFile != NULL);
  //geom->SetCategoryBits(GZ_NONE_COLLIDE);
  //geom->SetCollideBits(GZ_NONE_COLLIDE);
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int AvatarHeli::Init( WorldFile *file, WorldFileNode *node )
{
  int i, j;
  Model *model;
  Body *body;
  
  // Create position interface
  this->position = gz_position_alloc();
  assert(this->position);
  if (gz_position_create(this->position, this->world->gz_server, this->GetId(),
                         "AvatarHeli", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;


  // Turn off gravity in heli and any connected model
  this->body->SetGravityMode(false);
  for (i = 0; i < this->world->GetNumModels(); i++)
  {
    model = this->world->GetModels()[i];
    if (model->parent == this)
    {
      for (j = 0; j < model->GetNumBodies(); j++)
      {
        body = model->GetBodies()[j];
        body->SetGravityMode(false);
      }
    }
  }

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int AvatarHeli::Fini()
{
  // Close position interface
  gz_position_destroy( this->position );
  gz_position_free( this->position );
  this->position = NULL;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void AvatarHeli::Update( double /*step*/ )
{
  GzPose pose;
  GzVector lv, av;
  
  // Otherwise, update periodically
  if (this->world->GetSimTime() - this->updateTime > this->updatePeriod)
  {
    this->updateTime = this->world->GetSimTime();

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

    // Rotate velocities into global cs
    pose = this->GetPose();
    lv = GzCoordPositionAdd(this->cmdLinVel, GzVectorSet(0, 0, 0), pose.rot);
    av = GzCoordPositionAdd(this->cmdAngVel, GzVectorSet(0, 0, 0), pose.rot);
        
    // Dumb controller
    this->body->SetLinearVel(lv);
    this->body->SetAngularVel(av);
  
    // Update the interface
    this->PositionPutData();
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void AvatarHeli::PositionGetCmd()
{
  gz_position_lock(this->position, 1);

  this->cmdLinVel = GzVectorSet(this->position->data->cmd_vel_pos[0],
                             this->position->data->cmd_vel_pos[1],
                             this->position->data->cmd_vel_pos[2]);

  this->cmdAngVel = GzVectorSet(this->position->data->cmd_vel_rot[0],
                                this->position->data->cmd_vel_rot[1],
                                this->position->data->cmd_vel_rot[2]);

  gz_position_unlock(this->position);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void AvatarHeli::PositionPutData()
{
  gz_position_lock(this->position, 1);

  this->position->data->time = this->world->GetSimTime();
  
  // TODO
  
  gz_position_unlock(this->position);
  gz_position_post(this->position);
  
  return;
}