Revision: 7695
http://playerstage.svn.sourceforge.net/playerstage/?rev=7695&view=rev
Author: hsujohnhsu
Date: 2009-05-19 18:00:37 +0000 (Tue, 19 May 2009)
Log Message:
-----------
Body::pose in SetPose is set to pose relative to Gazebo Body Frame Origin (not
ODE CoM frame).
more updates on custom matrix stuff.
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Body.cc
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-05-19 17:58:30 UTC (rev
7694)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-05-19 18:00:37 UTC (rev
7695)
@@ -505,6 +505,7 @@
////////////////////////////////////////////////////////////////////////////////
// Set the pose of the body
+// SetPose(Gazebo Body Origin Pose)
void Body::SetPose(const Pose3d &_pose)
{
@@ -544,7 +545,7 @@
// Compute pose of CoM
localPose = this->comPose + _pose;
- this->pose = localPose;
+ this->pose = _pose;
this->SetPosition(localPose.pos);
this->SetRotation(localPose.rot);
}
@@ -866,6 +867,7 @@
*/
void Body::UpdateCoM()
{
+ std::cout << "calling UpdateCoM() " << this->GetName() << std::endl;
if (!this->bodyId)
return;
@@ -893,10 +895,12 @@
{
// FOR GEOMS:
// get pose with comPose set to oldPose
+ // pose of geom relative to old CoM location
this->comPose = oldPose;
tmpPose = giter->second->GetPose();
// get pose with comPose set to newPose
+ // using new CoM location, set relative pose of geom
this->comPose = newPose;
giter->second->SetPose(tmpPose, false);
}
@@ -904,11 +908,25 @@
// 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
+ //
+ std::cout << " name : " << this->GetName();
+ std::cout << " pose : " << this->GetPose();
+
+ // get pose of gazebo body origin given new comPose
this->comPose = newPose;
+ this->UpdatePose();
+ std::cout << " UpdatePose : " << this->GetPose();
+ tmpPose = this->GetPose();
+ std::cout << " tmpPose : " << tmpPose;
+
+
+ // set pose
+ this->comPose = oldPose;
+ std::cout << " oldPose : " << oldPose;
this->SetPose(tmpPose);
+ std::cout << " final pose : " << this->GetPose();
+ std::cout << std::endl;
// Settle on the new CoM pose
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Crystal Reports - New Free Runtime and 30 Day Trial
Check out the new simplified licensing option that enables
unlimited royalty-free distribution of the report engine
for externally facing server and web deployment.
http://p.sf.net/sfu/businessobjects
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
