Revision: 8158
http://playerstage.svn.sourceforge.net/playerstage/?rev=8158&view=rev
Author: hsujohnhsu
Date: 2009-07-30 02:52:18 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
fix Model::Reset(): keep pose offset from canonical body to model frame, use
for updating model pose.
turn off some debugging outputs.
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-07-30 02:35:06 UTC (rev 8157)
+++ code/gazebo/trunk/server/Model.cc 2009-07-30 02:52:18 UTC (rev 8158)
@@ -239,6 +239,8 @@
if (this->canonicalBodyNameP->GetValue().empty())
{
+ /// FIXME: Model::pose is set to the pose of first body
+ /// seems like there should be a warning for users
this->canonicalBodyNameP->SetValue( this->bodies.begin()->first );
}
@@ -457,7 +459,20 @@
if (!this->canonicalBodyNameP->GetValue().empty())
{
+ /// model pose is the canonical body pose of the body + a transform from
body frame to model frame
+ /// the tranform is defined by initModelOffset in body frame,
+
+ /// recover the transform in inertial frame based on body pose
this->pose = this->bodies[this->canonicalBodyNameP->GetValue()]->GetPose();
+ Quatern body_rot = this->pose.rot;
+ Pose3d offset_transform =
this->bodies[this->canonicalBodyNameP->GetValue()]->initModelOffset;
+ Vector3 xyz_offset =
(offset_transform.RotatePositionAboutOrigin(body_rot.GetInverse())).pos;
+ Quatern q_offset = offset_transform.rot;
+
+ // apply transform to get model pose
+ this->pose.pos = this->pose.pos + xyz_offset;
+ this->pose.rot = this->pose.CoordRotationAdd(q_offset);
+
this->xyzP->SetValue(this->pose.pos);
this->rpyP->SetValue(this->pose.rot);
}
@@ -535,13 +550,13 @@
jiter->second->Reset();
}
- /*for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
+ for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
{
biter->second->SetLinearVel(v);
biter->second->SetAngularVel(v);
biter->second->SetForce(v);
biter->second->SetTorque(v);
- }*/
+ }
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-07-30 02:35:06 UTC (rev
8157)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-07-30 02:52:18 UTC (rev
8158)
@@ -208,6 +208,12 @@
initPose.pos = **(this->xyzP);
initPose.rot = **(this->rpyP);
+ // save transform from this Parent Model Frame to this Body Frame
+ // this is only used in setting Model pose from canonicalBody
+ // the true model pose given a canonical body is
+ // this body's pose - this body's offsetFromModelFrame
+ this->initModelOffset = initPose.CoordPoseSolve(Pose3d());
+
childNode = node->GetChildByNSPrefix("geom");
// Load the geometries
@@ -253,7 +259,6 @@
}
this->SetPose(initPose);
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -936,23 +941,23 @@
// get pose with comPose set to oldPose
// get pose with comPose set to newPose
//
- std::cout << " name : " << this->GetName();
- std::cout << " pose : " << this->GetPose();
+ //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();
+ //std::cout << " UpdatePose : " << this->GetPose();
tmpPose = this->GetPose();
- std::cout << " tmpPose : " << tmpPose;
+ //std::cout << " tmpPose : " << tmpPose;
// set pose
this->comPose = oldPose;
- std::cout << " oldPose : " << oldPose;
+ //std::cout << " oldPose : " << oldPose;
this->SetPose(tmpPose);
- std::cout << " final pose : " << this->GetPose();
- std::cout << std::endl;
+ //std::cout << " final pose : " << this->GetPose();
+ //std::cout << std::endl;
// Settle on the new CoM pose
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2009-07-30 02:35:06 UTC (rev
8157)
+++ code/gazebo/trunk/server/physics/Body.hh 2009-07-30 02:52:18 UTC (rev
8158)
@@ -235,6 +235,10 @@
private: bool isStatic;
+ /// Used by Model if this body is the canonical body
+ /// model pose = body pose + initModelOffset
+ public: Pose3d initModelOffset;
+
private: Pose3d comPose;
private: Pose3d staticPose;
private: Pose3d pose;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day
trial. Simplify your report design, integration and deployment - and focus on
what you do best, core application coding. Discover what's new with
Crystal Reports now. http://p.sf.net/sfu/bobj-july
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
