blimp.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 PennBlimp
 * Author: Pranav Srivastava
 * Date: 14 Sep 2003
 * CVS: $Id: Blimp.cc,v 1.17 2006/08/27 15:23:20 dalai_at_sfnet Exp $
 */

/// @addtogroup models
/// @{ 
/** @defgroup Blimp Blimp

@htmlinclude Blimp_view.html

The Blimp model simulates a UPenn Blimp. 

@par libgazebo interfaces

This Blimp model supports the @ref position interface.

@par Player drivers

Basic motor control and odometry 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:Blimp>
  <xyz>0 0 0</xyz>
</model:Blimp>
@endverbatim

@par Views

@htmlinclude Blimp_more_views.html

@par Authors

Pranav Srivastava

*/
/// @} 


#include <math.h> // for sqrt in the velocity setting function.
#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 "CylinderGeom.hh"
#include "SliderJoint.hh"

#include "Blimp.hh"

#include <iostream>
using namespace std;


//////////////////////////////////////////////////////////////////////////////
// Register this class
GZ_REGISTER_STATIC("Blimp", Blimp);


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


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

  this->body = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int Blimp::Load( WorldFile *file, WorldFileNode *node )
{
  // this->wheelSep = 0.381; // maybe prop separation.

  // Create the ODE objects
  if (this->OdeLoad(file, node) != 0)
    return -1;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Blimp::Init( WorldFile *file, WorldFileNode *node )
{
  // Initialize ODE objects
  if (this->OdeInit(file, node) != 0)
    return -1;
  this->is3d=node->GetBool( "is3d", false );
  this->first_run=true;
  // Initialize external interface
  if (this->IfaceInit() != 0)
    return -1;
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Load ODE objects
int Blimp::OdeLoad( WorldFile *file, WorldFileNode *node )
{
  double radius,length;
  double gondolaLength,gondolaWidth;
  double mass; // mass of just the cylinder

  radius=2.0;
  length=5.0;
  gondolaLength=0.85;
  gondolaWidth=0.5;
  mass = 15.0; 
  total_mass=mass+4.0;

  // Create the main body of the robot
  this->body = new Body( this->world );
  
  this->body->SetGravityMode(0);
  this->bodyGeoms[0] = new CylinderGeom( this->body, this->modelSpaceId,radius,length);
  this->bodyGeoms[0]->SetRelativePosition( GzVectorSet(0, 0, 0) );
  this->bodyGeoms[0]->SetMass( mass );
  this->bodyGeoms[0]->SetColor( GzColor(1, 1, 1) );  
  this->bodyGeoms[0]->SetRelativeRotation( GzQuaternFromAxis(0,1.0,0,-M_PI/2) );

  //this->body->AttachGeom( this->bodyGeoms[0] );  

  this->bodyGeoms[1] = new BoxGeom( this->body, this->modelSpaceId,gondolaLength,gondolaWidth,0.2);
  this->bodyGeoms[1]->SetRelativePosition( GzVectorSet(0.0,0.0,-2.0) );
  this->bodyGeoms[1]->SetMass( 4 );
  this->bodyGeoms[1]->SetColor( GzColor(0, 0, 1) );  
  //this->body->AttachGeom( this->bodyGeoms[1] );
     
  this->bodyGeoms[2] = new SphereGeom( this->body, this->modelSpaceId,radius+0.01 );
  this->bodyGeoms[2]->SetRelativePosition( GzVectorSet(length/2,0,0) );
  this->bodyGeoms[2]->SetMass( 0 );
  this->bodyGeoms[2]->SetColor( GzColor(1,1,1));  
  //this->body->AttachGeom( this->bodyGeoms[2] );

  this->bodyGeoms[3] = new SphereGeom( this->body, this->modelSpaceId,radius+0.01 );
  this->bodyGeoms[3]->SetRelativePosition( GzVectorSet(-length/2,0,0) );
  this->bodyGeoms[3]->SetMass( 0 );
  this->bodyGeoms[3]->SetColor( GzColor(1,1,1) );  
  //this->body->AttachGeom( this->bodyGeoms[3] );

  this->body->SetPosition(GzVectorSet(0,0,15));
 
// Make the bogus counterweight
/*
  this->counter = new Body( this->world );
  this->counter->SetPosition( GzVectorSet( 2.0,0, 10.0 ) );
  sphGeom = new SphereGeom( this->modelSpaceId, 1.0 );
  sphGeom->SetCategoryBits( GZ_NONE_COLLIDE );
  sphGeom->SetCollideBits( GZ_NONE_COLLIDE );
  sphGeom->SetMass( mass/2.0 );
  sphGeom->SetDiffuseColor( GzColor(1.0, 0, 0) );
  this->counter->AttachGeom( sphGeom );
  
  // Attach the bogus counterweight to the base
  this->counterJoint = new SliderJoint( this->world->worldId );
  this->counterJoint->Attach( this->body, this->counter );
  this->counterJoint->SetAxis( 0, 0,1 );
  this->counterJoint->SetParam( dParamLoStop, -5.0 );
  this->counterJoint->SetParam( dParamHiStop, +5.0 );
 
 this->counter2 = new Body( this->world );
  this->counter2->SetPosition( GzVectorSet( -2.0,0, 10.0 ) );
  sphGeom = new SphereGeom( this->modelSpaceId, 1.0 );
  sphGeom->SetCategoryBits( GZ_NONE_COLLIDE );
  sphGeom->SetCollideBits( GZ_NONE_COLLIDE );
  sphGeom->SetMass( mass/2.0 );
  sphGeom->SetDiffuseColor( GzColor(1.0, 0, 0) );
  this->counter2->AttachGeom( sphGeom );
  
  // Attach the bogus counterweight to the base
  this->counterJoint2 = new SliderJoint( this->world->worldId );
  this->counterJoint2->Attach( this->body, this->counter2 );
  this->counterJoint2->SetAxis( 0,0,1 );
  this->counterJoint2->SetParam( dParamLoStop, -5.0 );
  this->counterJoint2->SetParam( dParamHiStop, +5.0 );
*/
 //  this->body->SetRotation(GzQuaternFromAxis(1,0,0, -M_PI/2));
 // this->body->AddForceAtRelPos(0,0,9.8*(mass+4),0.0,0.0,2.0);
   
  this->body->SetLinearVel(GzVectorSet(0,0,0));
  this->body->SetAngularVel(GzVectorSet(0,0,0));

  this->AddBody(this->body, true);
  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Initialize ODE
int Blimp::OdeInit( WorldFile *file, WorldFileNode *node )
{
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Blimp::Fini()
{
  // Finalize external interface  
  this->IfaceFini();

  // Finalize ODE objects
  this->OdeFini();

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize ODE
int Blimp::OdeFini()
{
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update model
void Blimp::Update( double step )
{
  if(first_run==true)
    {
      //  GetChildren();
       this->body->SetLinearVel(GzVectorSet(0,0,0));
       this->body->SetAngularVel(GzVectorSet(0,0,0));
      first_run=false;
    }

  // Get commands from the external interface
  //   if(is3d==false)
  this->IfaceGetCmd();
  //   else
  //     this->Iface3dGetCmd();
  
  
  // Update the odometry
  this->UpdateOdometry( step );
  
  // Update the interface
  //if(is3d==false)
  this->IfacePutData();
  //else
  //this->Iface3dPutData();
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry
// This is probably wrong
void Blimp::UpdateOdometry( double step )
{ // needs to be severely changed for the purposes of the 3D world
  // need to get 
  // double vr =this->Speed[0];
  //double va=this->Speed[1];
     
     //     GzVector torq= this->body->GetTorque();
     //     this->body->SetTorque(-torq.x,-torq.y,-torq.z);
     //this->body->SetAngularVel( GzVectorSet(0,0,0));
     //GzVector forc= this->body->GetForce();
     //this->body->SetForce(-forc.x,-forc.y,-forc.z);
     //   GzVector p1= this->body->GetRelPointPos(1,0,0);
     //   GzVector p0= this->body->GetRelPointPos(0,0,0);   
     //GzVector heading=GzVectorSub(p1,p0);
     
     // if(velocity_cmd==true)
     // this->body->SetLinearVel(vr*heading.x,vr*heading.y,0.0);

  //  this->body->SetAngularVel(wx,wy,0);
  // Compute odometric pose
  //  this->odomPose[0] += dr * cos( this->odomPose[2] );
  // this->odomPose[1] += dr * sin( this->odomPose[2] ); 
  GzVector gzpos = this->body->GetPosition();
  GzQuatern gzorient = this->body->GetRotation();
  GzVector euler = GzQuaternToEuler(gzorient);
  this->roll = euler.x;
  this->pitch = euler.y;
  this->yaw = euler.z;
  this->xpos=gzpos.x;
  this->ypos=gzpos.y;
  this->zpos=gzpos.z;
  
  //  this->body->SetLinearVel(GzVectorSet(xspeed,yspeed,zspeed));
  this->body->SetLinearVel(GzVectorSet(xspeed*cos(this->yaw),xspeed*sin(this->yaw),zspeed));
  //  this->body->SetAngularVel(GzVectorSet(0,0,yawspeed));
  // set commanded yaw rate and zero roll and pitch 
  
  //this->body->SetAngularVel(GzQuaternSet(0,-10*step*this->roll,-10*step*this->pitch,yawspeed));

  this->body->SetRotation(GzQuaternFromEuler(0.,.0,this->yaw));
  this->body->SetAngularVel(GzVectorSet(0,0,yawspeed));
  return;
}


// Initialize the external interface
int Blimp::IfaceInit()
{
     // if(this->is3d==false)
     //{
     this->iface = gz_position_alloc();
     assert(this->iface);
     
    if (gz_position_create(this->iface, this->world->gz_server, this->GetId(),
          "Blimp", this->GetIntId(), this->GetParentIntId()) != 0)
	  return -1;
     //}
     //else
     //{
     //     this->iface3d = gz_position3d_alloc();
     //     assert(this->iface3d);
     
     //      if (gz_position3d_create(this->iface3d, this->world->gz_server, this->GetId()) != 0)
     //	return -1;
     //    }
     return 0;
}


// Finalize the external interface
int Blimp::IfaceFini()
{
     //  if(is3d==false)
     //    {
     gz_position_destroy( this->iface );
     gz_position_free( this->iface );
     this->iface = NULL;
     //}
     // else
     //{
     // gz_position3d_destroy( this->iface3d );
     // gz_position3d_free( this->iface3d );
     // this->iface = NULL;
     // }
     return 0;
}


// Get commands from the external interface
void Blimp::IfaceGetCmd()
{
  double vr=0, va=0;

  vr = this->iface->data->cmd_vel_pos[0];
  va = this->iface->data->cmd_vel_rot[2];
  if (va>0.55)
       va = 0.55;
  else if (va<-.55)
       va = -.55;
  this->xspeed = vr;
  this->yawspeed = va;
  this->yspeed=0;
  this->zspeed=this->iface->data->cmd_vel_pos[2];;
  this->rollspeed=0;
  this->pitchspeed=0;
  //  printf("vel pos: %f %f %f, rot: %f %f %f\n",this->iface->data->cmd_vel_pos[0],this->iface->data->cmd_vel_pos[1],this->iface->data->cmd_vel_pos[2],this->iface->data->cmd_vel_rot[0],this->iface->data->cmd_vel_rot[1],this->iface->data->cmd_vel_rot[2]);
  return;
}

/*
  void Blimp::Iface3dGetCmd()
  {
  
  this->xspeed = this->iface3d->data->xspeed/1000;
  this->yspeed = this->iface3d->data->yspeed/1000;// converting to m/sec?
  this->zspeed = this->iface3d->data->zspeed/1000;
  this->yawspeed= this->iface3d->data->yawspeed;
  //  this->xspeed = 0;
  //  this->yspeed = 0;
  //  this->zspeed = 0;
  //  this->yawspeed= 0;
  this->rollspeed=0;
  this->pitchspeed=0;
  printf("Recd speeds: x= %f y=%f x=%f y=%f\n",xspeed,yspeed,zspeed,yawspeed);
  return;
  }
*/

/*
// Update the data in the erinterface
void Blimp::Iface3dPutData()
{
  // Data timestamp
  this->iface3d->data->time = this->world->GetSimTime();

  this->iface3d->data->xpos = this->xpos ;
  this->iface3d ->data->ypos = this->ypos;
  this->iface3d->data->zpos = this->zpos ; // conversion to mm??
  this->iface3d->data->roll=this->roll;
  this->iface3d->data->pitch=this->pitch;
  this->iface3d->data->yaw=this->yaw;
  return;
  }
*/

void Blimp::IfacePutData()
{
  // Data timestamp
  this->iface->data->time = this->world->GetSimTime();

  this->iface->data->pos[0] = this->xpos;
  this->iface->data->pos[1] = this->ypos;
  this->iface->data->pos[2] = this->zpos;
  this->iface->data->rot[0] = this->roll;
  this->iface->data->rot[1] = this->pitch;
  this->iface->data->rot[2] = this->yaw;

  GzVector vel = this->body->GetLinearVel();
  GzQuatern qvel =  this->body->GetAngularVel();
  this->iface->data->vel_pos[0] = vel.x;
  this->iface->data->vel_pos[1] = vel.y;
  this->iface->data->vel_pos[2] = vel.z;
  this->iface->data->vel_rot[0] = qvel.x*2;
  this->iface->data->vel_rot[1] = qvel.y*2;
  this->iface->data->vel_rot[2] = qvel.z*2;
  return;
}

/*
int Blimp::GetChildren()
{
  Model * models;

  {
    
  this->world->GetModels;


  return children;
}
*/