Revision: 8395
http://playerstage.svn.sourceforge.net/playerstage/?rev=8395&view=rev
Author: natepak
Date: 2009-11-12 17:36:32 +0000 (Thu, 12 Nov 2009)
Log Message:
-----------
Fixed a pose problem with joints and static bodies. Added in support for
heightmaps
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/physics/CMakeLists.txt
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/HeightmapShape.cc
code/gazebo/trunk/server/physics/HeightmapShape.hh
code/gazebo/trunk/server/physics/HingeJoint.hh
code/gazebo/trunk/server/physics/Joint.cc
code/gazebo/trunk/server/physics/ode/CMakeLists.txt
code/gazebo/trunk/server/physics/ode/ODEBody.cc
code/gazebo/trunk/server/physics/ode/ODEGeom.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/worlds/pioneer2dx.world
code/gazebo/trunk/worlds/terrain.world
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-11-12 15:28:37 UTC (rev 8394)
+++ code/gazebo/trunk/server/Model.cc 2009-11-12 17:36:32 UTC (rev 8395)
@@ -194,30 +194,27 @@
this->SetStatic( **(this->staticP) );
- // Set the relative pose of the model
- this->SetRelativePose( Pose3d( **this->xyzP, **this->rpyP) );
-
// Get the position and orientation of the model (relative to parent)
pose.Reset();
pose.pos = **this->xyzP;
pose.rot = **this->rpyP;
- // Record the model's initial pose (for reseting)
- this->SetInitPose(pose);
+ if (this->IsStatic())
+ this->SetRelativePose( pose );
if (this->type == "physical")
- {
this->LoadPhysical(node);
- }
else if (this->type == "renderable")
- {
this->LoadRenderable(node);
- }
else if (this->type != "empty")
- {
gzthrow("Invalid model type[" + this->type + "]\n");
- }
+ // Set the relative pose of the model
+ this->SetRelativePose( pose );
+
+ // Record the model's initial pose (for reseting)
+ this->SetInitPose(pose);
+
// Load controllers
childNode = node->GetChildByNSPrefix("controller");
while (childNode)
Modified: code/gazebo/trunk/server/physics/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/physics/CMakeLists.txt 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/CMakeLists.txt 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -24,7 +24,7 @@
MultiRayShape.cc
TrimeshShape.cc
MapShape.cc
- #HeightmapGeom.cc
+ HeightmapShape.cc
)
set (headers BallJoint.hh
@@ -47,7 +47,7 @@
MultiRayShape.hh
SphereShape.hh
TrimeshShape.hh
- #HeightmapShape.hh
+ HeightmapShape.hh
)
#add_library(gazebo_physics STATIC ${sources})
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-11-12 15:28:37 UTC (rev
8394)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-11-12 17:36:32 UTC (rev
8395)
@@ -109,9 +109,7 @@
this->laserFiducialIdP->Load(node);
this->laserRetroP->Load(node);
- // TODO: This should probably be true....but "true" breaks trimesh postions.
this->SetRelativePose( Pose3d( **this->xyzP, **this->rpyP ) );
- //this->SetPose(Pose3d( **this->xyzP, **this->rpyP ), false);
this->mass.SetMass( **this->massP );
Modified: code/gazebo/trunk/server/physics/HeightmapShape.cc
===================================================================
--- code/gazebo/trunk/server/physics/HeightmapShape.cc 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/HeightmapShape.cc 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -18,7 +18,7 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
-/* Desc: Heightmap geometry
+/* Desc: Heightmap shape
* Author: Nate Keonig, Andrew Howard
* Date: 8 May 2003
* CVS: $Id$
@@ -33,16 +33,16 @@
#include "OgreHeightmap.hh"
#include "GazeboError.hh"
#include "Body.hh"
-#include "HeightmapGeom.hh"
+#include "HeightmapShape.hh"
using namespace gazebo;
//////////////////////////////////////////////////////////////////////////////
// Constructor
-HeightmapGeom::HeightmapGeom(Body *body)
- : Geom(body)
+HeightmapShape::HeightmapShape(Geom *parent)
+ : Shape(parent)
{
- this->type = Geom::HEIGHTMAP;
+ this->type = Shape::HEIGHTMAP;
Param::Begin(&this->parameters);
this->imageFilenameP = new ParamT<std::string>("image","",1);
@@ -58,7 +58,7 @@
//////////////////////////////////////////////////////////////////////////////
// Destructor
-HeightmapGeom::~HeightmapGeom()
+HeightmapShape::~HeightmapShape()
{
delete this->imageFilenameP;
delete this->worldTextureP;
@@ -71,50 +71,14 @@
//////////////////////////////////////////////////////////////////////////////
/// Update function.
-void HeightmapGeom::UpdateChild()
+void HeightmapShape::Update()
{
}
////////////////////////////////////////////////////////////////////////////////
-// Create a lookup table of the terrain's height
-void HeightmapGeom::FillHeightMap()
-{
- unsigned int x,y;
- float h;
-
- // Resize the vector to match the size of the vertices
- this->heights.resize(this->odeVertSize*this->odeVertSize);
-
- // Iterate over all the verices
- for (y=0; y<this->odeVertSize; y++)
- {
- for (x=0; x<this->odeVertSize; x++)
- {
- // Find the height at a vertex
- h = this->ogreHeightmap->GetHeightAt(Vector2<float>(x*this->odeScale.x,
y*this->odeScale.y));
-
- // Store the height for future use
- this->heights[y*this->odeVertSize + x] = h;
- }
- }
-}
-
-////////////////////////////////////////////////////////////////////////////////
-// Called by ODE to get the height at a vertex
-dReal HeightmapGeom::GetHeightCallback(void *data, int x, int y)
-{
- HeightmapGeom *geom = (HeightmapGeom*)(data);
-
- // Return the height at a specific vertex
- return geom->heights[y * geom->odeVertSize + x];
-}
-
-////////////////////////////////////////////////////////////////////////////////
/// Load the heightmap
-void HeightmapGeom::LoadChild(XMLConfigNode *node)
+void HeightmapShape::Load(XMLConfigNode *node)
{
- Image img;
-
this->imageFilenameP->Load(node);
this->worldTextureP->Load(node);
this->detailTextureP->Load(node);
@@ -122,83 +86,18 @@
this->offsetP->Load(node);
// Use the image to get the size of the heightmap
- img.Load( (**this->imageFilenameP) );
+ this->img.Load( (**this->imageFilenameP) );
// Width and height must be the same
- if (img.GetWidth() != img.GetHeight())
- {
+ if (this->img.GetWidth() != this->img.GetHeight())
gzthrow("Heightmap image must be square\n");
- }
this->terrainSize = (**this->sizeP);
-
- // sampling size along image width and height
- this->odeVertSize = img.GetWidth() * 4;
- this->odeScale = this->terrainSize / this->odeVertSize;
-
-
- /*std::ostringstream stream;
- std::cout << "ODE Scale[" << this->odeScale << "]\n";
- std::cout << "Terrain Image[" << this->imageFilenameP->GetValue() << "]
Size[" << this->terrainSize << "]\n";
- printf("Terrain Size[%f %f %f]\n", this->terrainSize.x, this->terrainSize.y,
this->terrainSize.z);
- */
-
- // Step 1: Create the Ogre height map: Performs a ray scene query
- this->ogreHeightmap->Load( (**this->imageFilenameP), (**this->worldTextureP),
- (**this->detailTextureP), this->terrainSize );
-
- // Step 2: Construct the heightmap lookup table, using the ogre ray scene
- // query functionality
- this->FillHeightMap();
-
- // Step 3: Create the ODE heightfield geom
- this->odeData = dGeomHeightfieldDataCreate();
-
- // Step 4: Setup a callback method for ODE
- dGeomHeightfieldDataBuildCallback(
- this->odeData,
- this,
- HeightmapGeom::GetHeightCallback,
- this->terrainSize.x, // in meters
- this->terrainSize.y, // in meters
- this->odeVertSize, // width sampling size
- this->odeVertSize, // depth sampling size (along height of image)
- 1.0, // vertical (z-axis) scaling
- 0.0, // vertical (z-axis) offset
- 0.1, // vertical thickness for closing the height map mesh
- 0 // wrap mode
- );
-
- // Step 5: Restrict the bounds of the AABB to improve efficiency
- dGeomHeightfieldDataSetBounds( this->odeData, 0, this->terrainSize.z);
-
- this->geomId = dCreateHeightfield( this->spaceId, this->odeData, 1);
-
- this->SetGeom(this->geomId, false);
-
- this->SetStatic(true);
-
- //Rotate so Z is up, not Y (which is the default orientation)
- Quatern quat;
- Pose3d pose = this->GetPose();
-
- quat.SetFromEuler(Vector3(DTOR(90),0,0));
-
- pose.rot = pose.rot * quat;
- this->body->SetPose(pose);
-
- dQuaternion q;
- q[0] = pose.rot.u;
- q[1] = pose.rot.x;
- q[2] = pose.rot.y;
- q[3] = pose.rot.z;
-
- dGeomSetQuaternion(this->geomId, q);
}
////////////////////////////////////////////////////////////////////////////////
/// Save child parameters
-void HeightmapGeom::SaveChild(std::string &prefix, std::ostream &stream)
+void HeightmapShape::Save(std::string &prefix, std::ostream &stream)
{
stream << prefix << *(this->imageFilenameP) << "\n";
stream << prefix << *(this->worldTextureP) << "\n";
Modified: code/gazebo/trunk/server/physics/HeightmapShape.hh
===================================================================
--- code/gazebo/trunk/server/physics/HeightmapShape.hh 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/HeightmapShape.hh 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -18,16 +18,17 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
-/* Desc: Heightmap geometry
+/* Desc: Heightmap shape
* Author: Nate Keonig, Andrew Howard
* Date: 8 May 2003
* CVS: $Id$
*/
-#ifndef HEIGHTMAPGEOM_HH
-#define HEIGHTMAPGEOM_HH
+#ifndef HEIGHTMAPSHAPE_HH
+#define HEIGHTMAPSHAPE_HH
#include "Vector2.hh"
+#include "Image.hh"
#include "Geom.hh"
namespace gazebo
@@ -74,45 +75,35 @@
/// \brief Height map geom
- class HeightmapGeom : public Geom
+ class HeightmapShape : public Shape
{
/// \brief Constructor
- public: HeightmapGeom(Body *body);
+ public: HeightmapShape(Geom *parent);
/// \brief Destructor
- public: virtual ~HeightmapGeom();
+ public: virtual ~HeightmapShape();
/// \brief Update function
- public: void UpdateChild();
+ public: void Update();
/// \brief Load the heightmap
- protected: virtual void LoadChild(XMLConfigNode *node);
+ protected: virtual void Load(XMLConfigNode *node);
/// \brief Save child parameters
- protected: void SaveChild(std::string &prefix, std::ostream &stream);
+ protected: void Save(std::string &prefix, std::ostream &stream);
- /// Create a lookup table of the terrain's height
- private: void FillHeightMap();
+ protected: Vector3 terrainSize;
- /// \brief Called by ODE to get the height at a vertex
- private: static dReal GetHeightCallback(void *data, int x, int y);
+ protected: std::vector<double> heights;
- private: dHeightfieldDataID odeData;
+ protected: Image img;
+ protected: ParamT<std::string> *imageFilenameP;
+ protected: ParamT<std::string> *worldTextureP;
+ protected: ParamT<std::string> *detailTextureP;
+ protected: ParamT<Vector3> *sizeP;
+ protected: ParamT<Vector3> *offsetP;
- private: Vector3 terrainSize;
-
- private: unsigned int odeVertSize;
- private: Vector3 odeScale;
-
- private: std::vector<double> heights;
-
- private: ParamT<std::string> *imageFilenameP;
- private: ParamT<std::string> *worldTextureP;
- private: ParamT<std::string> *detailTextureP;
- private: ParamT<Vector3> *sizeP;
- private: ParamT<Vector3> *offsetP;
-
- private: OgreHeightmap *ogreHeightmap;
+ protected: OgreHeightmap *ogreHeightmap;
};
/// \}
Modified: code/gazebo/trunk/server/physics/HingeJoint.hh
===================================================================
--- code/gazebo/trunk/server/physics/HingeJoint.hh 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/HingeJoint.hh 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -123,7 +123,13 @@
this->SetLowStop(0,this->loStopP->GetValue());
this->SetHighStop(0, this->hiStopP->GetValue());
- this->SetAxis(0, **(this->axisP));
+ Vector3 a = **this->axisP;
+ //std::cout << "Axis[" << a << "]\n";
+ //std::cout << "Abs Rot[" <<
this->anchorBody->GetAbsPose().rot << "]\n";
+ //a = this->anchorBody->GetAbsPose().rot.RotateVector(a);
+ //a.Normalize();
+ //std::cout << "Axis Rot[" << a << "]\n";
+ this->SetAxis(0, a);
}
/// \brief Save a joint to a stream in XML format
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2009-11-12 15:28:37 UTC (rev
8394)
+++ code/gazebo/trunk/server/physics/Joint.cc 2009-11-12 17:36:32 UTC (rev
8395)
@@ -122,7 +122,6 @@
if (!this->body2 && this->body2NameP->GetValue() != std::string("world"))
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();
Modified: code/gazebo/trunk/server/physics/ode/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/physics/ode/CMakeLists.txt 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/ode/CMakeLists.txt 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -12,6 +12,7 @@
ODETrimeshShape.cc
ODERayShape.cc
ODEMultiRayShape.cc
+ ODEHeightmapShape.cc
)
ADD_LIBRARY(gazebo_physics_ode SHARED ${sources})
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -175,6 +175,7 @@
Pose3d pose = this->GetAbsPose();
this->physicsEngine->LockMutex();
+
dBodySetPosition(this->bodyId, pose.pos.x, pose.pos.y, pose.pos.z);
dQuaternion q;
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -99,6 +99,7 @@
if (this->IsStatic() && this->geomId && this->placeable)
{
+
// Transform into global pose since a static geom does not have a body
localPose = this->GetAbsPose();
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-12 15:28:37 UTC
(rev 8394)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-12 17:36:32 UTC
(rev 8395)
@@ -50,6 +50,7 @@
#include "ODEPlaneShape.hh"
#include "ODETrimeshShape.hh"
#include "ODEMultiRayShape.hh"
+#include "ODEHeightmapShape.hh"
#include "MapShape.hh"
#include "ODEPhysics.hh"
@@ -341,6 +342,9 @@
case Shape::TRIMESH:
shape = new ODETrimeshShape(geom);
break;
+ case Shape::HEIGHTMAP:
+ shape = new ODEHeightmapShape(geom);
+ break;
case Shape::MAP:
shape = new MapShape(geom);
break;
Modified: code/gazebo/trunk/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/trunk/worlds/pioneer2dx.world 2009-11-12 15:28:37 UTC (rev
8394)
+++ code/gazebo/trunk/worlds/pioneer2dx.world 2009-11-12 17:36:32 UTC (rev
8395)
@@ -102,7 +102,7 @@
-->
<model:physical name="pioneer2dx_model1">
- <xyz>0 0 .145</xyz>
+ <xyz>0 0 1.145</xyz>
<rpy>0.0 0.0 90.0</rpy>
<model:physical name="laser">
@@ -124,8 +124,8 @@
</model:physical>
<model:physical name="pioneer2dx_model2">
- <xyz>0 -1 .145</xyz>
- <rpy>0.0 0.0 -90.0</rpy>
+ <xyz>-0.5 -1 1.145</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
<model:physical name="laser">
<xyz>0.15 0 0.18</xyz>
@@ -145,6 +145,40 @@
</include>
</model:physical>
+
+ <model:physical name="ramp_model">
+ <xyz>0 2 0.0725</xyz>
+ <rpy>-20 0 0</rpy>
+ <static>true</static>
+ <body:box name="ramp_body">
+ <geom:box name="ramp_geom">
+ <size>2 5 .1</size>
+ <visual>
+ <scale>2 5 .1</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Grey</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="ramp_model2">
+ <xyz>1.5 -1.2 0.0725</xyz>
+ <rpy>0 20 0</rpy>
+ <static>true</static>
+ <body:box name="ramp2_body">
+ <geom:box name="ramp2_geom">
+ <size>5 2 .1</size>
+ <visual>
+ <scale>5 2 .1</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Grey</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
<model:physical name="box1_model">
<xyz>1 1.5 0.5</xyz>
<canonicalBody>box1_body</canonicalBody>
Modified: code/gazebo/trunk/worlds/terrain.world
===================================================================
--- code/gazebo/trunk/worlds/terrain.world 2009-11-12 15:28:37 UTC (rev
8394)
+++ code/gazebo/trunk/worlds/terrain.world 2009-11-12 17:36:32 UTC (rev
8395)
@@ -17,7 +17,7 @@
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.02</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.80665</gravity>
<cfm>10e-5</cfm>
<erp>0.3</erp>
@@ -27,6 +27,14 @@
<type>fltk</type>
<size>800 600</size>
<pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>-1.5 5.9 15</xyz>
+ <rpy>0 0 -45</rpy>
+ </camera>
+ </row>
+ </frames>
</rendering:gui>
<rendering:ogre>
@@ -38,7 +46,7 @@
</rendering:ogre>
<model:physical name="sphere1_model">
- <xyz>197 130 4.0</xyz>
+ <xyz>00 0 20.0</xyz>
<static>false</static>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
