Revision: 7493
http://playerstage.svn.sourceforge.net/playerstage/?rev=7493&view=rev
Author: hsujohnhsu
Date: 2009-03-17 00:34:59 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
added callbacks to get body rates.
added XML initialization for custom inertial matrices for bodies.
Modified Paths:
--------------
code/gazebo/branches/ogre-1.4.9/server/physics/Body.cc
code/gazebo/branches/ogre-1.4.9/server/physics/Body.hh
Modified: code/gazebo/branches/ogre-1.4.9/server/physics/Body.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/physics/Body.cc 2009-03-17
00:23:10 UTC (rev 7492)
+++ code/gazebo/branches/ogre-1.4.9/server/physics/Body.cc 2009-03-17
00:34:59 UTC (rev 7493)
@@ -44,7 +44,15 @@
#include "PlaneGeom.hh"
#include "Geom.hh"
#include "Body.hh"
+#include "Simulator.hh"
+#include "World.hh"
+#include "ODEPhysics.hh"
+#include "PhysicsEngine.hh"
+#ifdef TIMING
+#include "Simulator.hh"// for timing
+#endif
+
using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
@@ -70,6 +78,22 @@
this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0);
this->rpyP->Callback( &Body::SetRotation, this );
+ this->turnGravityOffP = new ParamT<bool>("turnGravityOff", false, 0); //
option to turn gravity off for individual body
+ this->selfCollideP = new ParamT<bool>("selfCollide", false, 0); // option
to make body collide with bodies of the same parent
+
+ // User can specify mass/inertia property for the body
+ this->customMassMatrixP = new ParamT<bool>("massMatrix",false,0);
+ this->cxP = new ParamT<double>("cx",0.0,0);
+ this->cyP = new ParamT<double>("cy",0.0,0);
+ this->czP = new ParamT<double>("cz",0.0,0);
+ this->bodyMassP = new ParamT<double>("mass",0.001,0);
+ this->ixxP = new ParamT<double>("ixx",1e-6,0);
+ this->iyyP = new ParamT<double>("iyy",1e-6,0);
+ this->izzP = new ParamT<double>("izz",1e-6,0);
+ this->ixyP = new ParamT<double>("ixy",0.0,0);
+ this->ixzP = new ParamT<double>("ixz",0.0,0);
+ this->iyzP = new ParamT<double>("iyz",0.0,0);
+
Param::End();
}
@@ -95,24 +119,74 @@
delete this->xyzP;
delete this->rpyP;
+ delete this->turnGravityOffP;
+ delete this->selfCollideP;
+ delete this->customMassMatrixP;
+ delete this->cxP ;
+ delete this->cyP ;
+ delete this->czP ;
+ delete this->bodyMassP;
+ delete this->ixxP;
+ delete this->iyyP;
+ delete this->izzP;
+ delete this->ixyP;
+ delete this->ixzP;
+ delete this->iyzP;
}
////////////////////////////////////////////////////////////////////////////////
// Load the body based on an XMLConfig node
void Body::Load(XMLConfigNode *node)
{
+
+ // before loading child geometry, we have to figure out of selfCollide is
true
+ // and modify parent class Entity so this body has its own spaceId
+ this->selfCollideP->Load(node);
+ if (this->selfCollideP->GetValue())
+ {
+ std::cout << "setting self collide: " << this->nameP->GetValue() <<
std::endl;
+ ODEPhysics* pe =
dynamic_cast<ODEPhysics*>(World::Instance()->GetPhysicsEngine());
+ this->spaceId = dSimpleSpaceCreate( pe->spaceId);
+ }
+
+ // option to enter full mass matrix
+ // load custom inertia matrix for the body
+ this->customMassMatrixP->Load(node);
+ this->cxP ->Load(node);
+ this->cyP ->Load(node);
+ this->czP ->Load(node);
+ this->bodyMassP->Load(node);
+ this->ixxP->Load(node);
+ this->iyyP->Load(node);
+ this->izzP->Load(node);
+ this->ixyP->Load(node);
+ this->ixzP->Load(node);
+ this->iyzP->Load(node);
+
+ this->customMassMatrix = this->customMassMatrixP->GetValue();
+ this->cx = this->cxP ->GetValue();
+ this->cy = this->cyP ->GetValue();
+ this->cz = this->czP ->GetValue();
+ this->bodyMass = this->bodyMassP->GetValue();
+ this->ixx = this->ixxP->GetValue();
+ this->iyy = this->iyyP->GetValue();
+ this->izz = this->izzP->GetValue();
+ this->ixy = this->ixyP->GetValue();
+ this->ixz = this->ixzP->GetValue();
+ this->iyz = this->iyzP->GetValue();
+
XMLConfigNode *childNode;
this->nameP->Load(node);
this->xyzP->Load(node);
this->rpyP->Load(node);
+ this->turnGravityOffP->Load(node);
Pose3d initPose;
initPose.pos = **(this->xyzP);
initPose.rot = **(this->rpyP);
-
childNode = node->GetChildByNSPrefix("geom");
// Load the geometries
@@ -123,6 +197,21 @@
childNode = childNode->GetNextByNSPrefix("geom");
}
+ /// Attach mesh for CG visualization
+ /// Add a renderable visual for CG, make visible in Update()
+ if (Simulator::Instance()->GetRenderEngineEnabled())
+ {
+ if (this->mass.mass > 0.0)
+ {
+ this->cg_visual = new OgreVisual(this->GetModel()->GetVisualNode());
+ this->cg_visual->AttachMesh("body_cg");
+ this->cg_visual->SetVisible(false);
+ this->cg_visual->SetMaterial("Gazebo/Red");
+ }
+ }
+
+
+
childNode = node->GetChildByNSPrefix("sensor");
// Load the sensors
@@ -134,8 +223,9 @@
}
// If no geoms are attached, then don't let gravity affect the body.
- if (this->geoms.size()==0)
+ if (this->geoms.size()==0 || this->turnGravityOffP->GetValue())
{
+ std::cout << "setting gravity to zero for: " << this->nameP->GetValue() <<
std::endl;
this->SetGravityMode(false);
}
@@ -197,6 +287,13 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Return Self-Collision Setting
+bool Body::GetSelfCollide()
+{
+ return this->selfCollideP->GetValue();
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Initialize the body
void Body::Init()
{
@@ -218,25 +315,68 @@
std::vector< Sensor* >::iterator sensorIter;
std::map< std::string, Geom* >::iterator geomIter;
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
this->UpdatePose();
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " Body Name (" << this->nameP->GetValue() << ")" <<
std::endl;
+ std::cout << " UpdatePose dt (" << tmpT2-tmpT1 << ")" <<
std::endl;
+#endif
+
if (!this->IsStatic())
{
// Set the pose of the scene node
- this->visualNode->SetPose(this->pose);
+ if (Simulator::Instance()->GetRenderEngineEnabled())
+ {
+ this->visualNode->SetPose(this->pose);
+
+ if (this->mass.mass > 0.0)
+ {
+ // Visualization of Body CG
+ if (this->cg_visual)
+ {
+ this->cg_visual->SetVisible(true);
+
+ // set visual pose of CG (CoM)
+ // comPose is only good for initial pose, to represent dynamically
changing CoM, rotate by pose.rot
+ this->cg_visual->SetPose(this->pose +
this->comPose.RotatePositionAboutOrigin(this->pose.rot.GetInverse()));
+ }
+ }
+ }
}
+#ifdef TIMING
+ double tmpT3 = Simulator::Instance()->GetWallTime();
+ std::cout << " Static SetPose dt (" << tmpT3-tmpT2 << ")" <<
std::endl;
+#endif
+
for (geomIter=this->geoms.begin();
geomIter!=this->geoms.end(); geomIter++)
{
geomIter->second->Update();
}
+#ifdef TIMING
+ double tmpT4 = Simulator::Instance()->GetWallTime();
+ std::cout << " Geom Update DT (" << tmpT4-tmpT3 << ")" <<
std::endl;
+#endif
+
for (sensorIter=this->sensors.begin();
sensorIter!=this->sensors.end(); sensorIter++)
{
(*sensorIter)->Update();
}
+
+#ifdef TIMING
+ double tmpT5 = Simulator::Instance()->GetWallTime();
+ std::cout << " ALL Sensors Update DT (" << tmpT5-tmpT4 << ")"
<< std::endl;
+ std::cout << " Body::Update Total DT (" << tmpT5-tmpT1 << ")"
<< std::endl;
+#endif
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -268,7 +408,12 @@
void Body::SetPose(const Pose3d &pose)
{
- this->pose = pose;
+ // check if pose is NaN, if so, constraint solver likely blew up
+ if (std::isnan(pose.pos.x) || std::isnan(pose.pos.y) ||
std::isnan(pose.pos.z) ||
+ std::isnan(pose.rot.u) || std::isnan(pose.rot.x) ||
std::isnan(pose.rot.y) || std::isnan(pose.rot.z))
+ std::cout << "trying to SetPose() for Body(" << this->GetName() << ") with
some NaN's (" << pose << ")" << std::endl;
+ else
+ this->pose = pose;
if (this->IsStatic())
{
@@ -329,7 +474,8 @@
dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z);
// Set the position of the scene node
- this->visualNode->SetPosition(pos);
+ if (Simulator::Instance()->GetRenderEngineEnabled())
+ this->visualNode->SetPosition(pos);
}
////////////////////////////////////////////////////////////////////////////////
@@ -350,7 +496,8 @@
}
// Set the orientation of the scene node
- this->visualNode->SetRotation(rot);
+ if (Simulator::Instance()->GetRenderEngineEnabled())
+ this->visualNode->SetRotation(rot);
}
////////////////////////////////////////////////////////////////////////////////
@@ -368,6 +515,14 @@
pos.x = p[0];
pos.y = p[1];
pos.z = p[2];
+
+ // check for NaN
+ if (std::isnan(pos.x) || std::isnan(pos.y) || std::isnan(pos.z))
+ {
+ std::cout << "Your simulation has exploded, position of body(" <<
this->GetName() << ") has NaN(" << pos << ")" << std::endl;
+ //pos = this->pose.pos;
+ assert(0);
+ }
}
else
{
@@ -394,6 +549,14 @@
rot.x = r[1];
rot.y = r[2];
rot.z = r[3];
+
+ // check for NaN
+ if (std::isnan(rot.u) || std::isnan(rot.x) || std::isnan(rot.y) ||
std::isnan(rot.z))
+ {
+ std::cout << "Your simulation has exploded, rotation of body(" <<
this->GetName() << ") has NaN(" << rot << ")" << std::endl;
+ //rot = this->pose.rot;
+ assert(0);
+ }
}
else
{
@@ -404,6 +567,88 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Return the position of the body. in global CS
+Vector3 Body::GetPositionRate() const
+{
+ Vector3 vel;
+
+ if (this->bodyId)
+ {
+ const dReal *v;
+
+ v = dBodyGetLinearVel(this->bodyId);
+
+ vel.x = v[0];
+ vel.y = v[1];
+ vel.z = v[2];
+ }
+ else
+ {
+ vel.x = 0;
+ vel.y = 0;
+ vel.z = 0;
+ }
+
+ return vel;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Return the rotation
+Quatern Body::GetRotationRate() const
+{
+ Quatern velQ;
+ Vector3 vel;
+
+ if (this->bodyId)
+ {
+ const dReal *v;
+
+ v = dBodyGetAngularVel(this->bodyId);
+ vel.x = v[0];
+ vel.y = v[1];
+ vel.z = v[2];
+
+ velQ.SetFromEuler(vel);
+ }
+ else
+ {
+ vel.x = 0;
+ vel.y = 0;
+ vel.z = 0;
+ velQ.SetFromEuler(vel);
+ }
+
+ return velQ;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Return the rotation
+Vector3 Body::GetEulerRate() const
+{
+ Vector3 vel;
+
+ if (this->bodyId)
+ {
+ const dReal *v;
+
+ v = dBodyGetAngularVel(this->bodyId);
+ vel.x = v[0];
+ vel.y = v[1];
+ vel.z = v[2];
+
+ }
+ else
+ {
+ vel.x = 0;
+ vel.y = 0;
+ vel.z = 0;
+ }
+
+ return vel;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Return the ID of this body
dBodyID Body::GetId() const
{
@@ -447,7 +692,7 @@
}
else if (node->GetName() == "map")
{
- //this->SetStatic(true);
+ this->SetStatic(true);
geom = new MapGeom(this);
}
else
@@ -505,13 +750,103 @@
*/
void Body::UpdateCoM()
{
+ if (!this->bodyId)
+ return;
+
+ // user can specify custom mass matrix or alternatively, UpdateCoM will
calculate CoM for
+ // combined mass of all children geometries.
+ if (this->customMassMatrix)
+ {
+ // Old pose for the CoM
+ Pose3d oldPose, newPose, tmpPose;
+
+ // oldPose is the last comPose
+ // newPose is mass CoM
+ oldPose = this->comPose;
+
+ std::cout << " in UpdateCoM, name: " << this->GetName() << std::endl;
+ std::cout << " in UpdateCoM, comPose or oldPose: " << this->comPose <<
std::endl;
+
+ // New pose for the CoM
+ newPose.pos.x = this->cx;
+ newPose.pos.y = this->cy;
+ newPose.pos.z = this->cz;
+
+ std::map< std::string, Geom* >::iterator giter;
+ // Fixup the poses of the geoms (they are attached to the CoM)
+ for (giter = this->geoms.begin(); giter != this->geoms.end(); giter++)
+ {
+ if (giter->second->IsPlaceable())
+ {
+ // FOR GEOMS:
+ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+ tmpPose = giter->second->GetPose();
+
+ // get pose with comPose set to newPose
+ this->comPose = newPose;
+ giter->second->SetPose(tmpPose, false);
+ }
+ }
+
+ // FOR BODY: Fixup the pose of the CoM (ODE body)
+ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+ tmpPose = this->GetPose();
+ // get pose with comPose set to newPose
+ this->comPose = newPose;
+ this->SetPose(tmpPose);
+
+ // Settle on the new CoM pose
+ this->comPose = newPose;
+
+
+
+ // comPose is zero in this case, we'll keep cx, cy, cz
+ this->comPose.Reset();
+
+ this->comPose.pos.x = this->cx;
+ this->comPose.pos.y = this->cy;
+ this->comPose.pos.z = this->cz;
+
+ // setup this->mass as well
+ dMassSetParameters(&this->mass, this->bodyMass,
+ this->cx, this->cy, this->cz,
+ //0,0,0,
+ this->ixx,this->iyy,this->izz,
+ this->ixy,this->ixz,this->iyz);
+
+ dMassTranslate( &this->mass, -this->cx, -this->cy, -this->cz);
+
+ // dMatrix3 rot;
+ // dMassRotate(&this->mass, rot);
+
+ // Set the mass matrix
+ if (this->mass.mass > 0)
+ dBodySetMass( this->bodyId, &this->mass );
+
+ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
+ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
+ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
+ // std::cout << " I[0] " << this->mass.I[0] << std::endl;
+ // std::cout << " I[1] " << this->mass.I[1] << std::endl;
+ // std::cout << " I[2] " << this->mass.I[2] << std::endl;
+ // std::cout << " I[3] " << this->mass.I[3] << std::endl;
+ // std::cout << " I[4] " << this->mass.I[4] << std::endl;
+ // std::cout << " I[5] " << this->mass.I[5] << std::endl;
+ // std::cout << " I[6] " << this->mass.I[6] << std::endl;
+ // std::cout << " I[7] " << this->mass.I[7] << std::endl;
+ // std::cout << " I[8] " << this->mass.I[8] << std::endl;
+
+ }
+ else
+ {
+
+ // original gazebo subroutine that gathers mass from all geoms and sums into
one single mass matrix
+
const dMass *lmass;
- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
- if (!this->bodyId)
- return;
-
// Construct the mass matrix by combining all the geoms
dMassSetZero( &this->mass );
@@ -525,6 +860,10 @@
}
// Old pose for the CoM
+ Pose3d oldPose, newPose, tmpPose;
+
+ // oldPose is the last comPose
+ // newPose is mass CoM
oldPose = this->comPose;
if (std::isnan(this->mass.c[0]))
@@ -546,19 +885,24 @@
{
if (giter->second->IsPlaceable())
{
+ // FOR GEOMS:
+ // get pose with comPose set to oldPose
this->comPose = oldPose;
- pose = giter->second->GetPose();
+ tmpPose = giter->second->GetPose();
+
+ // get pose with comPose set to newPose
this->comPose = newPose;
- giter->second->SetPose(pose, false);
+ giter->second->SetPose(tmpPose, false);
}
}
-
- // Fixup the pose of the CoM (ODE body)
+ // FOR BODY: Fixup the pose of the CoM (ODE body)
+ // get pose with comPose set to oldPose
this->comPose = oldPose;
- pose = this->GetPose();
+ tmpPose = this->GetPose();
+ // get pose with comPose set to newPose
this->comPose = newPose;
- this->SetPose(pose);
+ this->SetPose(tmpPose);
// Settle on the new CoM pose
@@ -570,6 +914,8 @@
// Set the mass matrix
if (this->mass.mass > 0)
dBodySetMass( this->bodyId, &this->mass );
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/branches/ogre-1.4.9/server/physics/Body.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/physics/Body.hh 2009-03-17
00:23:10 UTC (rev 7492)
+++ code/gazebo/branches/ogre-1.4.9/server/physics/Body.hh 2009-03-17
00:34:59 UTC (rev 7493)
@@ -101,6 +101,18 @@
/// \return Rotation quaternion
public: Quatern GetRotation() const;
+ /// \brief Return the velocity of the body
+ /// \return Velocity vector
+ public: Vector3 GetPositionRate() const;
+
+ /// \brief Return the rotation rates
+ /// \return Rotation Rate quaternion
+ public: Quatern GetRotationRate() const;
+
+ /// \brief Return the rotation rates
+ /// \return Rotation Rate Euler Angles RPY
+ public: Vector3 GetEulerRate() const;
+
/// \brief Return the ID of this body
/// \return ODE body id
public: dBodyID GetId() const;
@@ -117,6 +129,10 @@
/// \brief Set whether gravity affects this body
public: void SetGravityMode(bool mode);
+ /// \brief Get Self-Collision Flag, if this is true, this body will collide
+ // with other bodies even if they share the same parent.
+ public: bool GetSelfCollide();
+
/// \brief Set the linear velocity of the body
public: void SetLinearVel(const Vector3 &vel);
@@ -187,6 +203,26 @@
private: ParamT<Vector3> *xyzP;
private: ParamT<Quatern> *rpyP;
+
+ private: ParamT<bool> *turnGravityOffP;
+ private: ParamT<bool> *selfCollideP;
+
+ private: OgreVisual *cg_visual;
+
+ /// User specified Mass Matrix
+ protected: ParamT<bool> *customMassMatrixP;
+ protected: ParamT<double> *cxP ;
+ protected: ParamT<double> *cyP ;
+ protected: ParamT<double> *czP ;
+ protected: ParamT<double> *bodyMassP;
+ protected: ParamT<double> *ixxP;
+ protected: ParamT<double> *iyyP;
+ protected: ParamT<double> *izzP;
+ protected: ParamT<double> *ixyP;
+ protected: ParamT<double> *ixzP;
+ protected: ParamT<double> *iyzP;
+ protected: bool customMassMatrix;
+ protected: double cx,cy,cz,bodyMass,ixx,iyy,izz,ixy,ixz,iyz;
};
/// \}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
