Revision: 8471
http://playerstage.svn.sourceforge.net/playerstage/?rev=8471&view=rev
Author: natepak
Date: 2009-12-17 02:53:42 +0000 (Thu, 17 Dec 2009)
Log Message:
-----------
Added an extra entity to Body to handle the CoM offset properly
Modified Paths:
--------------
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Entity.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Pose3d.cc
code/gazebo/trunk/server/Quatern.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/Joint.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-16 01:35:07 UTC (rev 8470)
+++ code/gazebo/trunk/server/Entity.cc 2009-12-17 02:53:42 UTC (rev 8471)
@@ -263,22 +263,12 @@
Pose3d Entity::GetAbsPose() const
{
if (this->parent)
- return this->relativePose + this->parent->GetAbsPose();
+ return this->GetRelativePose() + this->parent->GetAbsPose();
else
- return this->relativePose;
+ return this->GetRelativePose();
}
////////////////////////////////////////////////////////////////////////////////
-/// 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
{
@@ -286,13 +276,11 @@
}
////////////////////////////////////////////////////////////////////////////////
-// Get the pose of the entity relative to its parent
-Pose3d Entity::GetCoMRelativePose() const
+/// Set the pose of the entity relative to its parent
+void Entity::SetRelativePose(const Pose3d &pose, bool notify)
{
- Vector3 rotateCOM = this->relativePose.rot.RotateVector(this->comOffset);
-
- return Pose3d(this->relativePose.pos + rotateCOM,
- this->relativePose.rot);
+ this->relativePose = pose;
+ this->PoseChange(notify);
}
////////////////////////////////////////////////////////////////////////////////
@@ -302,18 +290,10 @@
if (this->IsModel() || !this->parent)
return Pose3d();
- return this->relativePose + this->parent->GetModelRelativePose();
+ return this->GetRelativePose() + this->parent->GetModelRelativePose();
}
////////////////////////////////////////////////////////////////////////////////
-/// Set the pose of the entity relative to its parent
-void Entity::SetRelativePose(const Pose3d &pose, bool notify)
-{
- this->relativePose = pose;
- this->PoseChange(notify);
-}
-
-////////////////////////////////////////////////////////////////////////////////
// Set the abs pose of the entity
void Entity::SetAbsPose(const Pose3d &pose, bool notify)
{
@@ -328,16 +308,14 @@
/// Set the position of the entity relative to its parent
void Entity::SetRelativePosition(const Vector3 &pos)
{
- this->relativePose.pos = pos;
- this->PoseChange();
+ this->SetRelativePose( Pose3d( pos, this->GetRelativePose().rot), true );
}
////////////////////////////////////////////////////////////////////////////////
/// Set the rotation of the entity relative to its parent
void Entity::SetRelativeRotation(const Quatern &rot)
{
- this->relativePose.rot = rot;
- this->PoseChange();
+ this->SetRelativePose( Pose3d( this->GetRelativePose().pos, rot), true );
}
////////////////////////////////////////////////////////////////////////////////
@@ -348,9 +326,9 @@
{
if (Simulator::Instance()->GetState() == Simulator::RUN &&
!this->IsStatic())
- this->visualNode->SetDirty(true, this->GetCoMRelativePose());
+ this->visualNode->SetDirty(true, this->GetRelativePose());
else
- this->visualNode->SetPose(this->GetCoMRelativePose());
+ this->visualNode->SetPose(this->GetRelativePose());
}
if (notify)
Modified: code/gazebo/trunk/server/Entity.hh
===================================================================
--- code/gazebo/trunk/server/Entity.hh 2009-12-16 01:35:07 UTC (rev 8470)
+++ code/gazebo/trunk/server/Entity.hh 2009-12-17 02:53:42 UTC (rev 8471)
@@ -105,17 +105,9 @@
/// \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;
@@ -177,9 +169,6 @@
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-16 01:35:07 UTC (rev 8470)
+++ code/gazebo/trunk/server/Model.cc 2009-12-17 02:53:42 UTC (rev 8471)
@@ -984,7 +984,7 @@
gzthrow("Parent has no canonical body");
this->joint->Attach(myBody, pBody);
- this->joint->SetAnchor(0, myBody->GetCoMAbsPose().pos );
+ this->joint->SetAnchor(0, myBody->GetAbsPose().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/Pose3d.cc
===================================================================
--- code/gazebo/trunk/server/Pose3d.cc 2009-12-16 01:35:07 UTC (rev 8470)
+++ code/gazebo/trunk/server/Pose3d.cc 2009-12-17 02:53:42 UTC (rev 8471)
@@ -68,7 +68,7 @@
Pose3d Pose3d::GetInverse() const
{
Quatern inv = this->rot.GetInverse();
- return Pose3d( inv * this->pos, inv );
+ return Pose3d( inv * (this->pos*-1), inv );
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/Quatern.cc
===================================================================
--- code/gazebo/trunk/server/Quatern.cc 2009-12-16 01:35:07 UTC (rev 8470)
+++ code/gazebo/trunk/server/Quatern.cc 2009-12-17 02:53:42 UTC (rev 8471)
@@ -91,9 +91,11 @@
// Invert the quaternion
void Quatern::Invert()
{
- this->x = -this->x;
- this->y = -this->y;
- this->z = -this->z;
+ double norm =
this->u*this->u+this->x*this->x+this->y*this->y+this->z*this->z;
+ this->u = this->u/norm;
+ this->x = -this->x/norm;
+ this->y = -this->y/norm;
+ this->z = -this->z/norm;
}
////////////////////////////////////////////////////////////////////////////////
@@ -102,12 +104,15 @@
{
Quatern q;
- q.u = this->u;
- q.x = -this->x;
- q.y = -this->y;
- q.z = -this->z;
+ double norm =
this->u*this->u+this->x*this->x+this->y*this->y+this->z*this->z;
- //q.Normalize();
+ if (norm > 0.0)
+ {
+ q.u = this->u / norm;
+ q.x = -this->x / norm;
+ q.y = -this->y / norm;
+ q.z = -this->z / norm;
+ }
return q;
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-12-16 01:35:07 UTC (rev
8470)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-12-17 02:53:42 UTC (rev
8471)
@@ -54,6 +54,8 @@
Body::Body(Entity *parent)
: Entity(parent)
{
+ this->comEntity = new Entity(this);
+
this->physicsEngine = World::Instance()->GetPhysicsEngine();
this->cgVisual = NULL;
@@ -172,7 +174,7 @@
this->turnGravityOffP->Load(node);
this->SetRelativePose(Pose3d(**this->xyzP, **this->rpyP));
-
+
// 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->SetSelfCollide( **this->selfCollideP );
@@ -358,7 +360,7 @@
if (this->cgVisual == NULL)
this->cgVisual = OgreCreator::Instance()->CreateVisual(visname.str(),
- this->GetVisualNode());
+ this->comEntity->GetVisualNode());
else
this->cgVisual->DetachObjects();
@@ -453,7 +455,7 @@
if (!**this->customMassMatrixP)
{
Mass tmpMass = geom->GetMass();
- tmpMass.Rotate( this->GetRelativePose().rot );
+ //tmpMass.Rotate( this->GetRelativePose().rot );
this->mass += tmpMass;
}
}
@@ -470,13 +472,13 @@
Pose3d origPose, newPose;
std::map<std::string, Geom*>::iterator iter;
+ bodyPose = this->GetRelativePose();
+
if (**this->customMassMatrixP)
+ {
this->mass = this->customMass;
+ }
- 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++)
{
@@ -484,14 +486,12 @@
origPose = iter->second->GetRelativePose();
newPose = origPose;
- offset = bodyPose.rot.GetInverse().RotateVector(this->mass.GetCoG());
- newPose.pos -= offset;
+ newPose.pos -= this->mass.GetCoG();
iter->second->SetRelativePose(newPose, true);
}
- Pose3d p = this->GetRelativePose();
-
- this->SetRelativePose( p, true );
+ this->comEntity->SetRelativePose(Pose3d(this->mass.GetCoG(),Quatern()),true);
+ this->OnPoseChange();
}
@@ -540,13 +540,6 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Get the Center of Mass pose
-const Pose3d &Body::GetCoMPose() const
-{
- return this->comPose;
-}
-
-////////////////////////////////////////////////////////////////////////////////
/// Set the linear acceleration of the body
void Body::SetLinearAccel(const Vector3 &accel)
{
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2009-12-16 01:35:07 UTC (rev
8470)
+++ code/gazebo/trunk/server/physics/Body.hh 2009-12-17 02:53:42 UTC (rev
8471)
@@ -98,9 +98,6 @@
/// \brief Update the center of mass
public: virtual void UpdateCoM();
- /// \brief Get the Center of Mass pose
- public: const Pose3d &GetCoMPose() const;
-
/// \brief Set whether gravity affects this body
public: virtual void SetGravityMode(bool mode) = 0;
@@ -209,6 +206,8 @@
/// \brief Set to true to show the physics visualizations
public: void ShowPhysics(bool show);
+ public: Entity *GetCoMEntity() { return this->comEntity; }
+
/// List of geometries attached to this body
protected: std::map< std::string, Geom* > geoms;
@@ -224,9 +223,9 @@
/// model pose = body pose + initModelOffset
public: Pose3d initModelOffset;
- protected: Pose3d comPose;
+ // Helper entity for separating body pose from center-of-mass pose
+ protected: Entity *comEntity;
-
/// The pose of the body relative to the model. Can also think of this
/// as the body's pose offset.
protected: Pose3d relativePose;
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-12-16 01:35:07 UTC (rev
8470)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-12-17 02:53:42 UTC (rev
8471)
@@ -44,7 +44,7 @@
////////////////////////////////////////////////////////////////////////////////
// Constructor
Geom::Geom( Body *body )
- : Entity(body)
+ : Entity(body->GetCoMEntity())
{
this->physicsEngine = World::Instance()->GetPhysicsEngine();
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2009-12-16 01:35:07 UTC (rev
8470)
+++ code/gazebo/trunk/server/physics/Joint.cc 2009-12-17 02:53:42 UTC (rev
8471)
@@ -123,8 +123,7 @@
gzthrow("Couldn't Find Body[" + node->GetString("body2","",1));
// setting anchor relative to gazebo body frame origin
- this->anchorPos = this->anchorBody->GetAbsPose().pos +
**(this->anchorOffsetP);
- this->anchorPos -= this->anchorBody->GetMass().GetCoG();
+ this->anchorPos = (Pose3d(**(this->anchorOffsetP),Quatern()) +
this->anchorBody->GetAbsPose()).pos ;
this->Attach(this->body1, this->body2);
@@ -215,14 +214,9 @@
if (this->visual)
this->visual->SetVisible(World::Instance()->GetShowJoints());
- // setting anchor relative to gazebo body frame origin
- this->anchorPos = this->anchorBody->GetAbsPose().pos +
**(this->anchorOffsetP);
- this->anchorPos -= this->anchorBody->GetMass().GetCoG();
-
- //this->anchorPos = this->GetAnchor(0);
-
+ this->anchorPos = (Pose3d(**(this->anchorOffsetP),Quatern())+
this->anchorBody->GetAbsPose()).pos;
+
this->visual->SetPosition(this->anchorPos);
-
if (this->body1)
this->line1->SetPoint(1, this->body1->GetAbsPose().pos - this->anchorPos);
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-12-16 01:35:07 UTC
(rev 8470)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-12-17 02:53:42 UTC
(rev 8471)
@@ -108,10 +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;
+ Pose3d pp = self->comEntity->GetRelativePose().GetInverse() + pose;
- self->SetAbsPose(pose, false);
+ self->SetAbsPose(pp, false);
self->physicsEngine->UnlockMutex();
}
@@ -176,7 +175,7 @@
if (this->bodyId == NULL)
return;
- Pose3d pose = this->GetCoMAbsPose();
+ Pose3d pose = this->comEntity->GetAbsPose();
this->physicsEngine->LockMutex();
dBodySetPosition(this->bodyId, pose.pos.x, pose.pos.y, pose.pos.z);
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-12-16 01:35:07 UTC
(rev 8470)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-12-17 02:53:42 UTC
(rev 8471)
@@ -73,8 +73,7 @@
dQuaternion q;
// Transform into CoM relative Pose
- //localPose = newPose - this->body->GetCoMPose();
- localPose = this->GetCoMRelativePose();
+ localPose = this->GetRelativePose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -99,9 +98,8 @@
if (this->IsStatic() && this->geomId && this->placeable)
{
-
// Transform into global pose since a static geom does not have a body
- localPose = this->GetCoMAbsPose();
+ localPose = this->GetAbsPose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -115,8 +113,7 @@
else if (this->geomId && this->placeable)
{
// Transform into CoM relative Pose
- //localPose = newPose - this->body->GetCoMPose();
- localPose = this->GetCoMRelativePose();
+ localPose = this->GetRelativePose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -195,76 +192,6 @@
////////////////////////////////////////////////////////////////////////////////
-// Set the global pose relative to the body
-/*void ODEGeom::SetPose(const Pose3d &newPose, bool updateCoM)
-{
- this->physicsEngine->LockMutex();
-
- if (this->placeable && this->geomId)
- {
- Pose3d localPose;
- dQuaternion q;
-
- // Transform into CoM relative Pose
- localPose = newPose - this->body->GetCoMPose();
-
- q[0] = localPose.rot.u;
- q[1] = localPose.rot.x;
- q[2] = localPose.rot.y;
- q[3] = localPose.rot.z;
-
- // Set the pose of the encapsulated geom; this is always relative
- // to the CoM
- dGeomSetOffsetPosition(this->geomId, localPose.pos.x, localPose.pos.y,
localPose.pos.z);
- dGeomSetOffsetQuaternion(this->geomId, q);
-
- if (updateCoM)
- {
- this->body->UpdateCoM();
- }
- }
-
- this->physicsEngine->UnlockMutex();
-}*/
-
-////////////////////////////////////////////////////////////////////////////////
-// Return the pose of the geom relative to the body
-/*Pose3d ODEGeom::GetPose() const
-{
- this->physicsEngine->LockMutex();
-
- Pose3d pose;
-
- if (this->placeable && this->geomId)
- {
- const dReal *p;
- dQuaternion r;
-
- // Get the pose of the encapsulated geom; this is always relative to
- // the CoM
- p = dGeomGetPosition(this->geomId);
- dGeomGetQuaternion(this->geomId, r);
-
- pose.pos.x = p[0];
- pose.pos.y = p[1];
- pose.pos.z = p[2];
-
- pose.rot.u = r[0];
- pose.rot.x = r[1];
- pose.rot.y = r[2];
- pose.rot.z = r[3];
-
- // Transform into body relative pose
- pose += this->body->GetCoMPose();
- }
-
- this->physicsEngine->UnlockMutex();
-
- return pose;
-}*/
-
-
-////////////////////////////////////////////////////////////////////////////////
/// Set the category bits, used during collision detection
void ODEGeom::SetCategoryBits(unsigned int bits)
{
@@ -311,7 +238,7 @@
products = this->mass.GetProductsofInertia();
this->physicsEngine->LockMutex();
- pose = this->GetCoMAbsPose(); // get pose of the geometry
+ pose = this->GetAbsPose(); // 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.
------------------------------------------------------------------------------
This SF.Net email is sponsored by the Verizon Developer Community
Take advantage of Verizon's best-in-class app development support
A streamlined, 14 day to market process makes app distribution fast and easy
Join now and get one step closer to millions of Verizon customers
http://p.sf.net/sfu/verizon-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
