Revision: 8456
http://playerstage.svn.sourceforge.net/playerstage/?rev=8456&view=rev
Author: natepak
Date: 2009-12-14 17:20:51 +0000 (Mon, 14 Dec 2009)
Log Message:
-----------
Added in functions to get the center of mass pose
Modified Paths:
--------------
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Entity.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/ode/ODEBody.cc
code/gazebo/trunk/server/physics/ode/ODEGeom.cc
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2009-12-13 11:51:05 UTC (rev 8455)
+++ code/gazebo/trunk/server/Entity.cc 2009-12-14 17:20:51 UTC (rev 8456)
@@ -269,6 +269,16 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Get the absolute pose of the entity
+Pose3d Entity::GetCoMAbsPose() const
+{
+ if (this->parent)
+ return this->GetCoMRelativePose() + this->parent->GetCoMAbsPose();
+ else
+ return this->GetCoMRelativePose();
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Get the pose of the entity relative to its parent
Pose3d Entity::GetRelativePose() const
{
@@ -276,6 +286,16 @@
}
////////////////////////////////////////////////////////////////////////////////
+// Get the pose of the entity relative to its parent
+Pose3d Entity::GetCoMRelativePose() const
+{
+ Vector3 rotateCOM = this->relativePose.rot.RotateVector(this->comOffset);
+
+ return Pose3d(this->relativePose.pos + rotateCOM,
+ this->relativePose.rot);
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get the pose relative to the model this entity belongs to
Pose3d Entity::GetModelRelativePose() const
{
@@ -328,9 +348,9 @@
{
if (Simulator::Instance()->GetState() == Simulator::RUN &&
!this->IsStatic())
- this->visualNode->SetDirty(true, this->relativePose);
+ this->visualNode->SetDirty(true, this->GetCoMRelativePose());
else
- this->visualNode->SetPose(this->relativePose);
+ this->visualNode->SetPose(this->GetCoMRelativePose());
}
if (notify)
Modified: code/gazebo/trunk/server/Entity.hh
===================================================================
--- code/gazebo/trunk/server/Entity.hh 2009-12-13 11:51:05 UTC (rev 8455)
+++ code/gazebo/trunk/server/Entity.hh 2009-12-14 17:20:51 UTC (rev 8456)
@@ -105,9 +105,17 @@
/// \brief Get the absolute pose of the entity
public: virtual Pose3d GetAbsPose() const;
+ /// \brief Get the absolute pose of the entity. The result
+ /// is based on the center of mass of the bodies
+ public: virtual Pose3d GetCoMAbsPose() const;
+
/// \brief Get the pose of the entity relative to its parent
public: Pose3d GetRelativePose() const;
+ /// \brief Get the pose of the entity relative to its parent. The result
+ /// is based on the center of mass of the bodies
+ public: Pose3d GetCoMRelativePose() const;
+
/// \brief Get the pose relative to the model this entity belongs to
public: Pose3d GetModelRelativePose() const;
@@ -169,6 +177,9 @@
private: bool selected;
private: Pose3d relativePose;
+
+ // Center of Mass offset
+ protected: Vector3 comOffset;
};
/// \}
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-12-13 11:51:05 UTC (rev 8455)
+++ code/gazebo/trunk/server/Model.cc 2009-12-14 17:20:51 UTC (rev 8456)
@@ -984,7 +984,7 @@
gzthrow("Parent has no canonical body");
this->joint->Attach(myBody, pBody);
- this->joint->SetAnchor(0, myBody->GetAbsPose().pos );
+ this->joint->SetAnchor(0, myBody->GetCoMAbsPose().pos );
this->joint->SetAxis(0, Vector3(0,1,0) );
this->joint->SetHighStop(0,Angle(0));
this->joint->SetLowStop(0,Angle(0));
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-12-13 11:51:05 UTC (rev
8455)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-12-14 17:20:51 UTC (rev
8456)
@@ -475,6 +475,8 @@
bodyPose = this->GetRelativePose();
+ this->comOffset = this->mass.GetCoG();
+
// Translate all the geoms so that the CoG is at (0,0,0) in the body frame
for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++)
{
@@ -487,9 +489,7 @@
iter->second->SetRelativePose(newPose, true);
}
- // Apply the CoG offset to the body
Pose3d p = this->GetRelativePose();
- p.pos += this->mass.GetCoG();
this->SetRelativePose( p, true );
}
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-12-13 11:51:05 UTC
(rev 8455)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-12-14 17:20:51 UTC
(rev 8456)
@@ -108,6 +108,9 @@
pose.pos.Set(p[0], p[1], p[2]);
pose.rot.Set(r[0], r[1], r[2], r[3] );
+ Vector3 rotateCOM = pose.rot.RotateVector(self->comOffset);
+ pose.pos -= rotateCOM;
+
self->SetAbsPose(pose, false);
self->physicsEngine->UnlockMutex();
}
@@ -173,7 +176,7 @@
if (this->bodyId == NULL)
return;
- Pose3d pose = this->GetAbsPose();
+ Pose3d pose = this->GetCoMAbsPose();
this->physicsEngine->LockMutex();
dBodySetPosition(this->bodyId, pose.pos.x, pose.pos.y, pose.pos.z);
@@ -189,6 +192,7 @@
this->physicsEngine->UnlockMutex();
}
+////////////////////////////////////////////////////////////////////////////////
// Return the position of the body. in global CS
Vector3 ODEBody::GetPositionRate() const
{
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-12-13 11:51:05 UTC
(rev 8455)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-12-14 17:20:51 UTC
(rev 8456)
@@ -74,7 +74,7 @@
// Transform into CoM relative Pose
//localPose = newPose - this->body->GetCoMPose();
- localPose = this->GetRelativePose();
+ localPose = this->GetCoMRelativePose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -101,7 +101,7 @@
{
// Transform into global pose since a static geom does not have a body
- localPose = this->GetAbsPose();
+ localPose = this->GetCoMAbsPose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -109,14 +109,14 @@
q[3] = localPose.rot.z;
dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y,
- localPose.pos.z);
+ localPose.pos.z);
dGeomSetQuaternion(this->geomId, q);
}
else if (this->geomId && this->placeable)
{
// Transform into CoM relative Pose
//localPose = newPose - this->body->GetCoMPose();
- localPose = this->GetRelativePose();
+ localPose = this->GetCoMRelativePose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -311,7 +311,7 @@
products = this->mass.GetProductsofInertia();
this->physicsEngine->LockMutex();
- pose = this->GetAbsPose(); // get pose of the geometry
+ pose = this->GetCoMAbsPose(); // get pose of the geometry
q[0] = pose.rot.u;
q[1] = pose.rot.x;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Return on Information:
Google Enterprise Search pays you back
Get the facts.
http://p.sf.net/sfu/google-dev2dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
