body.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 736 行 · 第 1/2 页

/*
 *  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: Base class for all bodies
 * Author: Nate Keonig, Andrew Howard
 * Date: 8 May 2003
 * CVS: $Id: Body.cc,v 1.37 2006/02/14 03:00:25 natepak Exp $
 */

#include <assert.h>
#include <ode/ode.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include "Geom.hh"
#include "Body.hh"
#include "World.hh"


//////////////////////////////////////////////////////////////////////////////
// Constructor
Body::Body( const World *world, const char *bodyName, bool dummy )
{
  if (bodyName)
    this->bodyName = strdup(bodyName);
  else
    this->bodyName = NULL;
  this->worldId = world->worldId;

  this->comPose = GzPoseSet(GzVectorZero(), GzQuaternIdent());
  this->dummyPose = GzPoseSet(GzVectorZero(), GzQuaternIdent());
  
  if (!dummy)
  {    
    this->bodyId = dBodyCreate( world->worldId );
    dBodyEnable(this->bodyId);
  }
  else
  {
    this->bodyId = NULL;
  }

  // List geoms associated with this body
  this->geomCount = 0;
  this->geomMaxCount = 0;
  this->geoms = NULL;

  // Initialize the mass matrix
  dMassSetZero( &this->mass );

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
Body::~Body()
{
  free(this->geoms);
  if (this->bodyId)
    dBodyDestroy( this->bodyId );
  if (this->bodyName)
    free(this->bodyName);
 
  return;
}

//////////////////////////////////////////////////////////////////////////////
// Attach a geometry to this body
void Body::AttachGeom( Geom *geom )
{
  // Attach the geom to the ODE body
  if (this->bodyId)
  {
    if (geom->placeableGeom)
      dGeomSetBody( geom->GetTransId(), this->bodyId );
  }

  // Expand list if necessary
  if (this->geomCount >= this->geomMaxCount)
  {
    this->geomMaxCount += 10;
    this->geoms = (Geom**) realloc(this->geoms, this->geomMaxCount * sizeof(this->geoms[0]));
    assert( this->geoms );
  }
  
  // Add to the list
  assert(this->geomCount < this->geomMaxCount);
  this->geoms[this->geomCount++] = geom;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Detach a geometry from this body
void Body::DetachGeom( Geom *geom )
{
  // TODO
  assert(0);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the CoM and mass matrix
/*
  What's going on here?  In ODE the CoM of a body corresponds to the
  origin of the body-fixed coordinate system.  In Gazebo, however, we
  want to have arbitrary body coordinate systems (i.e., CoM may be
  displaced from the body-fixed cs).  To get around this limitation in
  ODE, we have an extra fudge-factor (comPose), describing the pose of
  the CoM relative to Gazebo's body-fixed cs.  When using low-level
  ODE functions, one must use apply this factor appropriately.

  The UpdateCoM() function is used to compute this offset, based on
  the mass distribution of attached geoms.  This function also shifts
  the ODE-pose of the geoms, to keep everything in the same place in the
  Gazebo cs.  Simple, neh?

  TODO: messes up if you call it twice; should fix.
*/
void Body::UpdateCoM()
{
  int i;
  BaseGeom *geom;
  const dMass *mass;
  GzPose oldPose, newPose, pose;

  // Dummy bodies dont have mass
  if (!this->bodyId)
    return;
  
  // Construct the mass matrix by combining all the geoms
  dMassSetZero( &this->mass );
  for (i = 0; i < this->geomCount; i++)
  {
    geom = this->geoms[i];
    mass = geom->GetBodyMassMatrix();
    if (geom->placeableGeom)
      dMassAdd( &this->mass, mass );
  }

  // Old pose for the CoM
  oldPose = this->comPose;

  // New pose for the CoM
  newPose.pos.x = this->mass.c[0];
  newPose.pos.y = this->mass.c[1];
  newPose.pos.z = this->mass.c[2];
  newPose.rot = GzQuaternIdent();

  // Fixup the poses of the geoms (they are attached to the CoM)
  for (i = 0; i < this->geomCount; i++)
  {
    geom = this->geoms[i];

    if (geom->placeableGeom)
    {
      this->comPose = oldPose;
      pose = geom->GetRelativePose();
      this->comPose = newPose;
      geom->SetRelativePose(pose, false);
    }
  }

  // Fixup the pose of the CoM (ODE body)
  this->comPose = oldPose;
  pose = this->GetPose();
    this->comPose = newPose;
  this->SetPose(pose);
  
  // Settle on the new CoM pose
  this->comPose = newPose;

  // TODO: remove offsets from mass matrix?

  // Set the mass matrix
  dBodySetMass( this->bodyId, &this->mass );

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Enable/disable the body (so it doesnt fall, for example)
void Body::Enable(bool enable)
{
  if (!this->bodyId)
    return;
    
  if (enable)
    dBodyEnable(this->bodyId);
  else
    dBodyDisable(this->bodyId);
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the CoM body pose (global cs)
GzPose Body::GetCoMPose( void ) const
{
  const dReal *p, *r;
  GzPose pose;

  if (!this->bodyId)
    return this->dummyPose;
  
  // Get the CoM pose (global cs)
  p = dBodyGetPosition( this->bodyId );
  r = dBodyGetQuaternion( this->bodyId );
  pose.pos = GzVectorSet(p[0], p[1], p[2]);
  pose.rot = GzQuaternSet(r[0], r[1], r[2], r[3]);

  return pose;
}

//////////////////////////////////////////////////////////////////////////////
// Get the body pose (global cs)
GzPose Body::GetPose( void ) const
{
  const dReal *p, *r;
  GzPose pose;

  if (!this->bodyId)
  {
    pose = this->dummyPose;
    pose.rot = GzQuaternNormal(pose.rot);
    return pose;
  }
  
  // Get the CoM pose (global cs)
  p = dBodyGetPosition( this->bodyId );
  r = dBodyGetQuaternion( this->bodyId );
  pose.pos = GzVectorSet(p[0], p[1], p[2]);
  pose.rot = GzQuaternSet(r[0], r[1], r[2], r[3]);

  // Compute the body pose
  pose = GzCoordPoseSolve(this->comPose, pose);
  
  return pose;
}


//////////////////////////////////////////////////////////////////////////////
// Set the body pose (global cs)
void Body::SetPose( GzPose pose )
{
  dQuaternion q;
      
  if (!this->bodyId)
  {
    pose.rot = GzQuaternNormal(pose.rot);
    this->dummyPose = pose;
    return;
  }
  
  // Compute pose of CoM
  pose = GzCoordPoseAdd(this->comPose, pose);

  // Set the CoM pose
  dBodySetPosition( this->bodyId, pose.pos.x, pose.pos.y, pose.pos.z );
  q[0] = pose.rot.u;
  q[1] = pose.rot.x;
  q[2] = pose.rot.y;
  q[3] = pose.rot.z;
  dBodySetQuaternion( this->bodyId, q);
  
  return;
}

//////////////////////////////////////////////////////////////////////////////
// Get the bodies global position
GzVector Body::GetPosition( void ) const
{
  return this->GetPose().pos;
}


//////////////////////////////////////////////////////////////////////////////
// Get the body rotation (global cs)
GzQuatern Body::GetRotation( void ) const
{
  return this->GetPose().rot;
}




//////////////////////////////////////////////////////////////////////////////
// Set the body position (global cs)
void Body::SetPosition( GzVector pos )
{
  GzPose pose;
  
  pose = this->GetPose();
  pose.pos = pos;
  this->SetPose(pose);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Set the body rotation (global cs)
void Body::SetRotation( GzQuatern rot )
{
  GzPose pose;
  
  pose = this->GetPose();
  pose.rot = rot;
  this->SetPose(pose);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the body linear velocity (global cs)
GzVector Body::GetLinearVel( void ) const
{
  if (!this->bodyId)
  {
    return GzVectorZero();
  }
  else
  {
    const dReal *v = dBodyGetLinearVel( this->bodyId );
    return GzVectorSet( (double)(v[0]), (double)(v[1]), (double)(v[2]) );
  }
}


//////////////////////////////////////////////////////////////////////////////
// Get the body angular velocity (global cs)
GzQuatern Body::GetAngularVel( void ) const
{
  if (!this->bodyId)
  {
    return GzQuaternIdent();
  }
  else
  {
    const dReal *v;
    GzQuatern vel;

    // Get angular velocity in global frame
    v = dBodyGetAngularVel( this->bodyId );