Revision: 8388
http://playerstage.svn.sourceforge.net/playerstage/?rev=8388&view=rev
Author: natepak
Date: 2009-11-11 18:17:36 +0000 (Wed, 11 Nov 2009)
Log Message:
-----------
Fixed collide and category bits
Modified Paths:
--------------
code/gazebo/trunk/server/MeshManager.cc
code/gazebo/trunk/server/MeshManager.hh
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Shape.cc
code/gazebo/trunk/server/physics/Shape.hh
code/gazebo/trunk/server/physics/TrimeshShape.cc
code/gazebo/trunk/server/physics/ode/ODEGeom.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc
code/gazebo/trunk/server/rendering/OgreAdaptor.cc
code/gazebo/trunk/server/rendering/OgreVisual.cc
code/gazebo/trunk/worlds/models/pr2.model
code/gazebo/trunk/worlds/willowgarage.world
Modified: code/gazebo/trunk/server/MeshManager.cc
===================================================================
--- code/gazebo/trunk/server/MeshManager.cc 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/MeshManager.cc 2009-11-11 18:17:36 UTC (rev
8388)
@@ -53,6 +53,9 @@
std::string extension;
+ if (this->HasMesh(filename))
+ return this->meshes[filename];
+
fullname = std::string("./")+filename;
if (stat(fullname.c_str(), &st) == 0)
{
@@ -119,6 +122,14 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Add a mesh to the manager
+void MeshManager::AddMesh(Mesh *mesh)
+{
+ if (!this->HasMesh(mesh->GetName()))
+ this->meshes[mesh->GetName()] = mesh;
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get a mesh by name
const Mesh *MeshManager::GetMesh(const std::string &name) const
{
Modified: code/gazebo/trunk/server/MeshManager.hh
===================================================================
--- code/gazebo/trunk/server/MeshManager.hh 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/MeshManager.hh 2009-11-11 18:17:36 UTC (rev
8388)
@@ -26,6 +26,9 @@
/// \brief Load a mesh from a file
public: const Mesh *Load(const std::string &filename);
+ /// \brief Add a mesh to the manager
+ public: void AddMesh(Mesh *mesh);
+
/// \brief Get a mesh by name
public: const Mesh *GetMesh(const std::string &name) const;
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/Simulator.cc 2009-11-11 18:17:36 UTC (rev
8388)
@@ -327,7 +327,7 @@
{
double currTime = 0;
double lastTime = 0;
- double freq = 80.0;
+ double freq = 40.0;
#ifdef TIMING
double tmpT1 = this->GetWallTime();
@@ -338,6 +338,8 @@
this->physicsThread = new boost::thread(
boost::bind(&Simulator::PhysicsLoop, this));
+ struct timespec timeSpec;
+
// Update the gui
while (!this->userQuit)
{
@@ -361,12 +363,19 @@
if (currTime - lastTime < 1/freq)
{
- usleep((1/freq - (currTime - lastTime)) * 1e6);
+ double sleepTime = (1/freq - (currTime - lastTime));
+ timeSpec.tv_sec = (int)(sleepTime);
+ timeSpec.tv_nsec = (sleepTime - timeSpec.tv_sec) *1e9;
+
+ nanosleep(&timeSpec, NULL);
}
}
else
{
- usleep((1/freq - currTime - lastTime) * 1e6);
+ double sleepTime = (1/freq - (currTime - lastTime));
+ timeSpec.tv_sec = (int)(sleepTime);
+ timeSpec.tv_nsec = (sleepTime - timeSpec.tv_sec) *1e9;
+ nanosleep(&timeSpec, NULL);
}
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-11-11 18:17:36 UTC (rev
8388)
@@ -493,9 +493,6 @@
bodyPose = this->GetRelativePose();
- if (this->GetName() == "torso_lift_link")
- std::cout << "Before Pose[" << this->GetAbsPose().pos << "]\n";
-
// 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++)
{
@@ -513,11 +510,6 @@
p.pos += this->mass.GetCoG();
this->SetRelativePose( p, true );
-
- if (this->GetName() == "torso_lift_link")
- std::cout << "After Pose[" << this->GetAbsPose().pos << "]\n";
-
-
}
Modified: code/gazebo/trunk/server/physics/Shape.cc
===================================================================
--- code/gazebo/trunk/server/physics/Shape.cc 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/physics/Shape.cc 2009-11-11 18:17:36 UTC (rev
8388)
@@ -1,3 +1,4 @@
+#include "World.hh"
#include "Geom.hh"
#include "Shape.hh"
@@ -11,6 +12,7 @@
: parent(p)
{
this->parent->SetShape(this);
+ this->physicsEngine = World::Instance()->GetPhysicsEngine();
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/physics/Shape.hh
===================================================================
--- code/gazebo/trunk/server/physics/Shape.hh 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/server/physics/Shape.hh 2009-11-11 18:17:36 UTC (rev
8388)
@@ -11,6 +11,7 @@
namespace gazebo
{
class Geom;
+ class PhysicsEngine;
/// \brief Base class for all shapes
class Shape : public Common
@@ -37,6 +38,7 @@
protected: Geom *parent;
protected: Type type;
+ protected: PhysicsEngine *physicsEngine;
};
}
Modified: code/gazebo/trunk/server/physics/TrimeshShape.cc
===================================================================
--- code/gazebo/trunk/server/physics/TrimeshShape.cc 2009-11-11 02:38:45 UTC
(rev 8387)
+++ code/gazebo/trunk/server/physics/TrimeshShape.cc 2009-11-11 18:17:36 UTC
(rev 8388)
@@ -24,8 +24,11 @@
* SVN: $Id$
*/
+#include "PhysicsEngine.hh"
#include "MeshManager.hh"
#include "Mesh.hh"
+#include "Mass.hh"
+#include "Geom.hh"
#include "TrimeshShape.hh"
#include "GazeboError.hh"
#include "OgreAdaptor.hh"
@@ -70,6 +73,56 @@
this->mesh = meshManager->Load( **this->meshNameP );
+ Mass mass = this->parent->GetMass();
+
+ if (this->mesh->GetSubMeshCount() > 1)
+ {
+ // Create a mesh for each of the submeshes.
+ for (unsigned int i=1; i < this->mesh->GetSubMeshCount(); i++)
+ {
+ SubMesh *subMesh = const_cast<SubMesh*>(mesh->GetSubMesh(i));
+
+ if (subMesh->GetVertexCount() < 3)
+ continue;
+
+ std::ostringstream newName;
+ newName << this->mesh->GetName() << "_" << i;
+
+ Mesh *newMesh = new Mesh();
+ newMesh->SetName( newName.str() );
+ newMesh->AddSubMesh( subMesh );
+
+ meshManager->AddMesh( newMesh );
+
+ std::ostringstream stream;
+
+ stream << "<gazebo:world
xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\";
xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\";>";
+
+ stream << "<geom:trimesh name='" << newName.str() << "_geom'>";
+ stream << " <mass>" <<
+ mass.GetAsDouble() / this->mesh->GetSubMeshCount() << "</mass>";
+ stream << " <xyz>0 0 0</xyz>";
+ stream << " <scale>" << **this->scaleP << "</scale>";
+ stream << " <mesh>" << newName.str() << "</mesh>";
+ stream << " <visual>";
+ stream << " <mesh>" << newName.str() << "</mesh>";
+ stream << " <scale>" << **this->scaleP << "</scale>";
+ stream << " </visual>";
+ stream << "</geom:trimesh>";
+ stream << "</gazebo:world>";
+
+ XMLConfig *config = new XMLConfig();
+ config->LoadString( stream.str() );
+
+ Geom *newGeom = this->physicsEngine->CreateGeom( "trimesh",
+ this->parent->GetBody() );
+
+ newGeom->Load( config->GetRootNode()->GetChild() );
+
+ delete config;
+ }
+ }
+
/*const Mesh *mesh = meshManager->Load( **this->meshNameP );
if (!mesh)
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-11-11 02:38:45 UTC
(rev 8387)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2009-11-11 18:17:36 UTC
(rev 8388)
@@ -151,12 +151,13 @@
// Set the encapsulated geometry object
void ODEGeom::SetGeom(dGeomID geomId, bool placeable)
{
+ // Must go first in this function
+ this->geomId = geomId;
+
Geom::SetGeom(placeable);
this->physicsEngine->LockMutex();
- this->geomId = geomId;
-
if ( dGeomGetSpace(this->geomId) == 0 )
{
dSpaceAdd(this->spaceId, this->geomId);
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-11 02:38:45 UTC
(rev 8387)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-11 18:17:36 UTC
(rev 8388)
@@ -248,6 +248,7 @@
#endif
this->LockMutex();
+
// Update the dynamical model
if (this->quickStepP->GetValue())
dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
@@ -423,11 +424,9 @@
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
-
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact))
return;
-
// Check if either are spaces
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
@@ -450,23 +449,15 @@
else
geom2 = (ODEGeom*) dGeomGetData(o2);
- numc = dCollide(o1,o2,64, contactGeoms, sizeof(contactGeoms[0]));
+ //std::cout << "Geom1[" << geom1->GetName() << "] Geom2[" <<
geom2->GetName() << "]\n";
+
+ numc = dCollide(o1,o2,10, contactGeoms, sizeof(contactGeoms[0]));
if (numc != 0)
{
for (i=0; i<numc; i++)
{
double h, kp, kd;
- if (0)
- std::cout << "dContactGeoms: "
- << " geom1: " << geom1->GetName()
- << " geom2: " << geom2->GetName()
- << " contact points: " << numc
- << " contact: " << i
- << " pos: " << contactGeoms[i].pos[0]<<","<<
contactGeoms[i].pos[1]<<","<< contactGeoms[i].pos[2]<<","<<
contactGeoms[i].pos[3]
- << " norm: " << contactGeoms[i].normal[0]<<","<<
contactGeoms[i].normal[1]<<","<< contactGeoms[i].normal[2]<<","<<
contactGeoms[i].normal[3]
- << " depth: " << contactGeoms[i].depth
- << std::endl;
// skip negative depth contacts
if(contactGeoms[i].depth < 0)
continue;
@@ -513,10 +504,7 @@
self->contactGroup, &contact);
if (self->contactFeedbackIter == self->contactFeedbacks.end())
- {
self->contactFeedbacks.resize( self->contactFeedbacks.size() + 100);
- fprintf(stderr, "Resize\n");
- }
(*self->contactFeedbackIter).geom1 = geom1;
(*self->contactFeedbackIter).geom2 = geom2;
Modified: code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc 2009-11-11
02:38:45 UTC (rev 8387)
+++ code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc 2009-11-11
18:17:36 UTC (rev 8388)
@@ -95,17 +95,26 @@
PhysicsEngine *physics = World::Instance()->GetPhysicsEngine();
TrimeshShape::Load(node);
+ /*if (this->mesh->GetSubMeshCount() > 1)
+ {
+ printf("ODETrimesh submesh count >1\n");
+ return;
+ }*/
mass = this->parent->GetMass();
+ unsigned int i =0;
+
//for (unsigned int i=0; i < this->mesh->GetSubMeshCount(); i++)
- {
+ //{
dTriMeshDataID odeData;
- /*const SubMesh *subMesh = mesh->GetSubMesh(i);
+ const SubMesh *subMesh = mesh->GetSubMesh(i);
if (subMesh->GetVertexCount() < 3)
- continue;
- */
+ {
+ printf("ODETrimesh invalid mesh\n");
+ return;
+ }
/// This will hold the vertex data of the triangle mesh
odeData = dGeomTriMeshDataCreate();
@@ -115,10 +124,10 @@
float *vertices = NULL;
unsigned int *indices = NULL;
- mesh->FillArrays(&vertices, &indices);
+ subMesh->FillArrays(&vertices, &indices);
- numIndices = mesh->GetIndexCount();
- numVertices = mesh->GetVertexCount();
+ numIndices = subMesh->GetIndexCount();
+ numVertices = subMesh->GetVertexCount();
for (unsigned int j=0; j < numVertices; j++)
{
@@ -132,14 +141,12 @@
(float*)vertices, 3*sizeof(float), numVertices,
(int*)indices, numIndices, 3*sizeof(int));
- pgeom->SetGeom( dCreateTriMesh( pgeom->GetSpaceId(), odeData,0,0,0 ), true
);
+ pgeom->SetSpaceId( dSimpleSpaceCreate(pgeom->GetSpaceId()) );
+ pgeom->SetGeom( dCreateTriMesh(pgeom->GetSpaceId(), odeData,0,0,0 ), true);
if (!pgeom->IsStatic())
- {
- double massV = mass.GetAsDouble()/this->mesh->GetSubMeshCount();
- dMassSetTrimeshTotal(&odeMass, massV , pgeom->GetGeomId());
- }
- }
+ dMassSetTrimeshTotal(&odeMass, mass.GetAsDouble(), pgeom->GetGeomId());
+ //}
physics->ConvertMass(&mass, &odeMass);
this->parent->SetMass(mass);
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2009-11-11 02:38:45 UTC
(rev 8387)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2009-11-11 18:17:36 UTC
(rev 8388)
@@ -459,7 +459,6 @@
(*iter)->Render();
}
-
// Must update the user camera's last.
for (iter = this->cameras.begin(); iter != this->cameras.end(); iter++)
{
Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreVisual.cc 2009-11-11 02:38:45 UTC
(rev 8387)
+++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2009-11-11 18:17:36 UTC
(rev 8388)
@@ -175,8 +175,6 @@
if (!MeshManager::Instance()->HasMesh(meshName))
MeshManager::Instance()->Load(meshName);
- std::cout << "Loading mesh[" << meshName << "]\n";
-
// Add the mesh into OGRE
OgreCreator::InsertMesh( MeshManager::Instance()->GetMesh(meshName) );
Modified: code/gazebo/trunk/worlds/models/pr2.model
===================================================================
--- code/gazebo/trunk/worlds/models/pr2.model 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/worlds/models/pr2.model 2009-11-11 18:17:36 UTC (rev
8388)
@@ -61,6 +61,8 @@
</geom:box>
<turnGravityOff>false</turnGravityOff>
<selfCollide>false</selfCollide>
+
+ <!--
<sensor:ray name="base_laser">
<rayCount>120</rayCount>
<rangeCount>640</rangeCount>
@@ -74,6 +76,7 @@
<resRange>0.01</resRange>
<updateRate>20.0</updateRate>
</sensor:ray>
+ -->
</body:box>
<joint:hinge name="base_laser_joint">
<body1>base_laser_link</body1>
@@ -2150,6 +2153,7 @@
</geom:box>
<turnGravityOff>false</turnGravityOff>
<selfCollide>false</selfCollide>
+ <!--
<sensor:ray name="laser_tilt">
<rayCount>120</rayCount>
<rangeCount>640</rangeCount>
@@ -2163,6 +2167,7 @@
<resRange>0.01</resRange>
<updateRate>40.0</updateRate>
</sensor:ray>
+ -->
</body:box>
<joint:hinge name="laser_tilt_joint">
<body1>laser_tilt_link</body1>
Modified: code/gazebo/trunk/worlds/willowgarage.world
===================================================================
--- code/gazebo/trunk/worlds/willowgarage.world 2009-11-11 02:38:45 UTC (rev
8387)
+++ code/gazebo/trunk/worlds/willowgarage.world 2009-11-11 18:17:36 UTC (rev
8388)
@@ -97,6 +97,7 @@
</include>
</model:physical>
+ <!--
<model:physical name="sphere1_model">
<xyz>10 10 5.5</xyz>
<rpy>0.0 0.0 0.0</rpy>
@@ -117,6 +118,7 @@
</geom:sphere>
</body:sphere>
</model:physical>
+ -->
<!-- White Point light -->
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
