Revision: 8390
http://playerstage.svn.sourceforge.net/playerstage/?rev=8390&view=rev
Author: natepak
Date: 2009-11-11 21:50:55 +0000 (Wed, 11 Nov 2009)
Log Message:
-----------
More updates to trimesh collision handling
Modified Paths:
--------------
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc
code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
code/gazebo/trunk/worlds/trimesh.world
code/gazebo/trunk/worlds/willowgarage.world
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-11 19:06:27 UTC
(rev 8389)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-11 21:50:55 UTC
(rev 8390)
@@ -94,7 +94,7 @@
this->contactSurfaceLayerP = new ParamT<double>("contactSurfaceLayer", 0.01,
0);
Param::End();
- this->contactFeedbacks.resize(100);
+ this->contactFeedbacks.resize(1000);
// Reset the contact pointer
this->contactFeedbackIter = this->contactFeedbacks.begin();
@@ -209,6 +209,12 @@
iter != this->contactFeedbackIter; iter++)
{
+ if ((*iter).geom1 == NULL)
+ printf("Geom1 is null\n");
+
+ if ((*iter).geom2 == NULL)
+ printf("Geom2 is null\n");
+
(*iter).geom1->contact->body1Force.Set(
(*iter).feedback.f1[0], (*iter).feedback.f1[1],
(*iter).feedback.f1[2]);
(*iter).geom2->contact->body2Force.Set(
@@ -450,10 +456,13 @@
else
geom2 = (ODEGeom*) dGeomGetData(o2);
- //std::cout << "Geom1[" << geom1->GetName() << "] Geom2[" <<
geom2->GetName() << "]\n";
+ int numContacts = 5;
- numc = dCollide(o1,o2,64, contactGeoms, sizeof(contactGeoms[0]));
+ if (geom1->GetType() == Shape::TRIMESH && geom2->GetType()==Shape::TRIMESH)
+ numContacts = 64;
+ numc = dCollide(o1,o2,numContacts, contactGeoms, sizeof(contactGeoms[0]));
+
if (numc != 0)
{
for (i=0; i<numc; i++)
@@ -506,7 +515,9 @@
self->contactGroup, &contact);
if (self->contactFeedbackIter == self->contactFeedbacks.end())
- self->contactFeedbacks.resize( self->contactFeedbacks.size() + 100);
+ {
+ self->contactFeedbacks.resize( self->contactFeedbacks.size() + 500);
+ }
(*self->contactFeedbackIter).geom1 = geom1;
(*self->contactFeedbackIter).geom2 = geom2;
@@ -518,118 +529,3 @@
}
}
}
-
-////////////////////////////////////////////////////////////////////////////////
-/*void ODEPhysics::CollisionCallback( void *data, dGeomID o1, dGeomID o2)
-{
- int i,n;
- ODEPhysics *self;
- Geom *geom1, *geom2;
- dContactGeom contactGeoms[10];
- dContact contactInfo;
- dJointID joint;
- int num;
-
- self = (ODEPhysics*) data;
-
- // Maximum number of contacts
- num = sizeof(contactGeoms) / sizeof(contactGeoms[0]);
-
- // If either geom is a space...
- if (dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ))
- {
- // If the spaces/geoms belong to different spaces, collide them
- if (dGeomGetSpace(o1) != dGeomGetSpace(o2))
- dSpaceCollide2( o1, o2, self, &CollisionCallback );
-
- // If the spaces belong the world space, collide them
- else if (dGeomGetSpace(o1) == self->spaceId || dGeomGetSpace(o2) ==
self->spaceId)
- dSpaceCollide2( o1, o2, self, &CollisionCallback );
- }
- else
- {
- // There should be no geoms in the world space
- assert(dGeomGetSpace(o1) != self->spaceId);
- assert(dGeomGetSpace(o2) != self->spaceId);
-
- // We should never test two geoms in the same space
- assert(dGeomGetSpace(o1) != dGeomGetSpace(o2));
-
- // Get pointers to the underlying geoms
- geom1 = NULL;
- if (dGeomGetClass(o1) == dGeomTransformClass)
- geom1 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o1));
- else
- geom1 = (Geom*) dGeomGetData(o1);
-
- geom2 = NULL;
- if (dGeomGetClass(o2) == dGeomTransformClass)
- geom2 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o2));
- else
- geom2 = (Geom*) dGeomGetData(o2);
-
- //std::cout << "Geom1[" << geom1->GetName() << "] Geom2[" <<
geom2->GetName() << "]\n";
-
- assert(geom1 && geom2);
-
- if (geom1->IsStatic() && geom2->IsStatic())
- printf("Geoms are static\n");
-
- // Detect collisions betweed geoms
- n = dCollide(o1, o2, num, contactGeoms, sizeof(contactGeoms[0]));
-
- printf("Num COllisions[%d]\n",n);
- for (i=0; i < n; i++)
- {
- dBodyID body1 = dGeomGetBody(contactGeoms[i].g1);
- dBodyID body2 = dGeomGetBody(contactGeoms[i].g2);
- printf("Bodies[%d %d]\n",body1, body2);
-
- // Dont add contact joints between already connected bodies.
- // Sometimes the body is unspecified; should probably figure out
- // what this means
- if (body1 && body2)
- if (dAreConnectedExcluding(body1, body2, dJointTypeContact))
- continue;
-
- contactInfo.geom = contactGeoms[i];
- contactInfo.surface.mode = 0;
-
- // Compute the CFM and ERP by assuming the two bodies form a
- // spring-damper system.
- double h, kp, kd;
- h = self->stepTime;
- kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp);
- kd = geom1->contact->kd + geom2->contact->kd;
- contactInfo.surface.mode |= dContactSoftERP | dContactSoftCFM;
- contactInfo.surface.soft_erp = h * kp / (h * kp + kd);
- contactInfo.surface.soft_cfm = 1 / (h * kp + kd);
-
- // Compute friction effects; this is standard Coulomb friction
- contactInfo.surface.mode |= dContactApprox1;
- contactInfo.surface.mu = std::min(geom1->contact->mu1,
geom2->contact->mu1);
- contactInfo.surface.mu2 = 0;
- contactInfo.surface.bounce = 0.1;
- contactInfo.surface.bounce_vel = 0.1;
-
-
- // contactInfo.surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
-
- //contactInfo.surface.soft_erp = 0.8;
- //contactInfo.surface.soft_cfm = 0.01;
- //contactInfo.surface.slip1 = 0.0;
- //contactInfo.surface.slip2 = 0.0;
- //contactInfo.surface.mu = 1;
-
- // Compute slipping effects
- //contactInfo.surface.slip1 = (geom1->contact->slip1 +
geom2->contact->slip1)/2.0;
- //contactInfo.surface.slip2 = (geom1->contact->slip2 +
geom2->contact->slip2)/2.0;
-
- std::cout << "Geom1[" << geom1->GetName() << "] Geom2[" <<
geom2->GetName() << "]\n";
- // Construct a contact joint between the two bodies
- joint = dJointCreateContact(self->worldId, self->contactGroup,
&contactInfo);
- dJointAttach(joint, body1, body2);
- }
- }
-}*/
-
Modified: code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc 2009-11-11
19:06:27 UTC (rev 8389)
+++ code/gazebo/trunk/server/physics/ode/ODETrimeshShape.cc 2009-11-11
21:50:55 UTC (rev 8390)
@@ -96,6 +96,7 @@
PhysicsEngine *physics = World::Instance()->GetPhysicsEngine();
TrimeshShape::Load(node);
+
/*if (this->mesh->GetSubMeshCount() > 1)
{
printf("ODETrimesh submesh count >1\n");
@@ -106,16 +107,14 @@
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)
{
printf("ODETrimesh invalid mesh\n");
return;
- }*/
+ }
/// This will hold the vertex data of the triangle mesh
odeData = dGeomTriMeshDataCreate();
@@ -125,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++)
{
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-11
19:06:27 UTC (rev 8389)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-11
21:50:55 UTC (rev 8390)
@@ -217,13 +217,15 @@
// somehow here, extract contact information when user requests it
//
std::vector< ParamT<std::string> *>::iterator iter;
- int i = 0;
+ unsigned int i = 0;
- for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end();
- iter++, i++)
+ for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++)
{
if ( **(*iter) == g1->GetName() || **(*iter) == g2->GetName() )
{
+ if (i >= this->contacts.size())
+ this->contacts.resize( this->contacts.size() + 10);
+
this->contacts[i].state = 1;
this->contacts[i].time = Simulator::Instance()->GetRealTime();
this->contacts[i].name = **(*iter)==g1->GetName()? g2->GetName() :
g1->GetName();
@@ -231,6 +233,7 @@
this->contacts[i].body2Force = g1->contact->body2Force;
this->contacts[i].body1Torque = g1->contact->body1Torque;
this->contacts[i].body2Torque = g1->contact->body2Torque;
+ i++;
}
}
Modified: code/gazebo/trunk/worlds/trimesh.world
===================================================================
--- code/gazebo/trunk/worlds/trimesh.world 2009-11-11 19:06:27 UTC (rev
8389)
+++ code/gazebo/trunk/worlds/trimesh.world 2009-11-11 21:50:55 UTC (rev
8390)
@@ -17,12 +17,12 @@
<verbosity>1</verbosity>
<physics:ode>
- <stepTime>0.001</stepTime>
+ <stepTime>0.0001</stepTime>
<gravity>0 0 -9.80665</gravity>
<cfm>10e-8</cfm>
<erp>0.3</erp>
<quickStep>true</quickStep>
- <quickStepIters>50</quickStepIters>
+ <quickStepIters>20</quickStepIters>
<quickStepW>1.3</quickStepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.001</contactSurfaceLayer>
@@ -54,12 +54,33 @@
<normal>0 0 1</normal>
<size>2000 2000</size>
<segments>10 10</segments>
- <uvTile>100 100</uvTile>
+ <uvTile>1000 1000</uvTile>
<material>Gazebo/GrayGrid</material>
</geom:plane>
</body:plane>
</model:physical>
+ <model:physical name="wood_pallet1">
+ <xyz>0 0 2.5</xyz>
+ <rpy>90 0 0</rpy>
+ <static>false</static>
+ <body:trimesh name="pallet_body">
+ <geom:trimesh name="pallet_geom">
+ <mesh>WoodPallet.mesh</mesh>
+ <scale>.2 .2 .2</scale>
+ <mass>0.1</mass>
+
+ <visual>
+ <scale>.2 .2 .2</scale>
+ <rpy>0 0 0</rpy>
+ <mesh>WoodPallet.mesh</mesh>
+ <material>Gazebo/WoodPallet</material>
+ </visual>
+ </geom:trimesh>
+ </body:trimesh>
+ </model:physical>
+
+
<model:physical name="wood_pallet">
<xyz>0 0 1.5</xyz>
<rpy>90 0 0</rpy>
@@ -68,7 +89,7 @@
<geom:trimesh name="pallet_geom">
<mesh>WoodPallet.mesh</mesh>
<scale>.2 .2 .2</scale>
- <mass>1.0</mass>
+ <mass>0.1</mass>
<visual>
<scale>.2 .2 .2</scale>
@@ -88,7 +109,7 @@
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
<size>0.2</size>
- <mass>1.0</mass>
+ <mass>0.1</mass>
<mu1>109999.0</mu1>
Modified: code/gazebo/trunk/worlds/willowgarage.world
===================================================================
--- code/gazebo/trunk/worlds/willowgarage.world 2009-11-11 19:06:27 UTC (rev
8389)
+++ code/gazebo/trunk/worlds/willowgarage.world 2009-11-11 21:50:55 UTC (rev
8390)
@@ -72,7 +72,7 @@
</model:physical>
<model:physical name="wg_model">
- <xyz>0 0 0.0</xyz>
+ <xyz>-8 -4 0.0</xyz>
<static>true</static>
<body:trimesh name="wg_body">
@@ -89,7 +89,7 @@
</model:physical>
<model:physical name="pr2_model">
- <xyz>8 4 0</xyz>
+ <xyz>0 0 0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<include embedded="true">
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
