Revision: 8563
http://playerstage.svn.sourceforge.net/playerstage/?rev=8563&view=rev
Author: hsujohnhsu
Date: 2010-03-05 03:47:26 +0000 (Fri, 05 Mar 2010)
Log Message:
-----------
fixed model pose and world reset function.
Modified Paths:
--------------
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
code/gazebo/trunk/server/physics/ode/ODEBody.cc
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2010-03-03 19:30:44 UTC (rev 8562)
+++ code/gazebo/trunk/server/Entity.cc 2010-03-05 03:47:26 UTC (rev 8563)
@@ -262,9 +262,18 @@
Pose3d Entity::GetAbsPose() const
{
if (this->parent)
+ {
+ //std::cout << " GetAbsPose for model " << this->GetName()
+ // << " relative " << this->GetRelativePose()
+ // << " parent-abs " << this->parent->GetAbsPose() << std::endl;
return this->GetRelativePose() + this->parent->GetAbsPose();
+ }
else
+ {
+ //std::cout << " GetAbsPose for model " << this->GetName()
+ // << " relative " << this->GetRelativePose() << std::endl;
return this->GetRelativePose();
+ }
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2010-03-03 19:30:44 UTC (rev 8562)
+++ code/gazebo/trunk/server/Model.cc 2010-03-05 03:47:26 UTC (rev 8563)
@@ -389,6 +389,30 @@
*/
this->InitChild();
+
+ // this updates the relativePose of the Model Entity
+ // set model pointer of the canonical body so when body updates,
+ // one can callback to the model
+ if (!this->bodies.empty())
+ {
+ Body* canonicalBody;
+ if (this->bodies.find(this->canonicalBodyNameP->GetValue()) !=
this->bodies.end())
+ {
+ canonicalBody = this->bodies[this->canonicalBodyNameP->GetValue()];
+ canonicalBody->SetCanonicalModel(this);
+ // std::cout << "model " << this->GetName()
+ // << " canoncal body: " << canonicalBody->GetName() <<
std::endl;
+ }
+ else
+ {
+ //std::cout << "bad canonical body?" << std::endl;
+ }
+ }
+ else
+ {
+ //std::cout << "model " << this->GetName() << " has no bodies" <<
std::endl;
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -495,7 +519,36 @@
}
}
+void Model::OnPoseChange()
+{
+ /// \brief set the pose of the model, which is the pose of the canonical body
+ // this updates the relativePose of the Model Entity
+ if (!this->bodies.empty())
+ {
+ Body* canonicalBody;
+ if (this->bodies.find(this->canonicalBodyNameP->GetValue()) !=
this->bodies.end())
+ {
+ canonicalBody = this->bodies[this->canonicalBodyNameP->GetValue()];
+ this->SetAbsPose(canonicalBody->GetAbsPose(),false); // do not recurse
+ // std::cout << " Model OnPoseChange " << this->GetName()
+ // << " canoncal body: " << canonicalBody->GetName()
+ // << " pose " << this->GetRelativePose() << std::endl;
+ }
+ else
+ {
+ //std::cout << " has no canonical body" << std::endl;
+ }
+ }
+ else
+ {
+ //std::cout << " Model " << this->GetName() << " OnPoseChange has no
bodies" << std::endl;
+ }
+
+
+}
+
+
////////////////////////////////////////////////////////////////////////////////
/// Primarily used to update the graphics interfaces
void Model::GraphicsUpdate()
@@ -543,7 +596,7 @@
std::map<std::string, Controller* >::iterator citer;
Vector3 v(0,0,0);
-// this->SetPose(this->initPose);
+ this->SetRelativePose(this->initPose); // this has to be relative for
nested models to work
for (citer=this->controllers.begin(); citer!=this->controllers.end();
citer++)
{
@@ -1138,3 +1191,13 @@
biter->second->GetInterfaceNames(list);
}
}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Return Model Absolute Pose
+/// this is redundant as long as Model's relativePose is maintained
+/// which is done in Model::OnPoseChange and during Model initialization
+Pose3d Model::GetAbsPose()
+{
+ return this->bodies[**this->canonicalBodyNameP]->GetAbsPose();
+}
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2010-03-03 19:30:44 UTC (rev 8562)
+++ code/gazebo/trunk/server/Model.hh 2010-03-05 03:47:26 UTC (rev 8563)
@@ -183,6 +183,10 @@
/// \return Pointer to the body
public: Body *GetCanonicalBody();
+ /// \brief Called when the pose of the entity (or one of its parents) has
+ /// changed
+ public: virtual void OnPoseChange();
+
/// \brief Set the gravity mode of the model
public: void SetGravityMode( const bool &v );
@@ -254,6 +258,8 @@
/// \brief Light numbering variable to give a unique name to all light
entities
private: static uint lightNumber;
+ public: Pose3d GetAbsPose();
+
private: ParamT<std::string> *canonicalBodyNameP;
private: ParamT<Vector3> *xyzP;
private: ParamT<Quatern> *rpyP;
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2010-03-03 19:30:44 UTC (rev
8562)
+++ code/gazebo/trunk/server/physics/Body.cc 2010-03-05 03:47:26 UTC (rev
8563)
@@ -61,6 +61,9 @@
this->cgVisual = NULL;
+ // preset canonicalModel to null
+ this->canonicalModel = NULL;
+
Param::Begin(&this->parameters);
this->xyzP = new ParamT<Vector3>("xyz", Vector3(), 0);
this->xyzP->Callback( &Entity::SetRelativePosition, (Entity*)this );
@@ -136,6 +139,13 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// setting model point if this is the canonical body of a model
+void Body::SetCanonicalModel(Model* model)
+{
+ this->canonicalModel = model;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Load the body based on an XMLConfig node
void Body::Load(XMLConfigNode *node)
{
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2010-03-03 19:30:44 UTC (rev
8562)
+++ code/gazebo/trunk/server/physics/Body.hh 2010-03-05 03:47:26 UTC (rev
8563)
@@ -258,6 +258,13 @@
protected: ParamT<double> *ixzP;
protected: ParamT<double> *iyzP;
protected: Mass customMass;
+
+ /// \brief if this is the canonical :ody of a model, point back
+ protected: Model* canonicalModel;
+
+ /// \brief this is how you set canonical body from the model
+ public: void SetCanonicalModel(Model* model);
+
};
/// \}
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-03-03 19:30:44 UTC
(rev 8562)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-03-05 03:47:26 UTC
(rev 8563)
@@ -36,7 +36,7 @@
#include "GazeboError.hh"
#include "PhysicsEngine.hh"
#include "Mass.hh"
-
+#include "Model.hh"
#include "ODEBody.hh"
using namespace gazebo;
@@ -113,6 +113,11 @@
Pose3d pp = self->comEntity->GetRelativePose().GetInverse() + pose;
self->SetAbsPose(pp, false);
+
+ // if this is a canonical body, update model's relative pose
+ if (self->canonicalModel)
+ self->canonicalModel->OnPoseChange();
+
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
