Revision: 8728
http://playerstage.svn.sourceforge.net/playerstage/?rev=8728&view=rev
Author: natepak
Date: 2010-05-28 23:08:03 +0000 (Fri, 28 May 2010)
Log Message:
-----------
Hacked ODEPhysics for a test case
Modified Paths:
--------------
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-05-28 21:22:39 UTC (rev 8727)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-05-28 23:08:03 UTC (rev 8728)
@@ -222,12 +222,91 @@
{
std::vector<ContactFeedback>::iterator iter;
std::vector<dJointFeedback>::iterator jiter;
-
+
+ ODEBody *leftBody =
(ODEBody*)World::Instance()->GetEntityByName("pioneer::left_wheel");
+ ODEBody *rightBody =
(ODEBody*)World::Instance()->GetEntityByName("pioneer::right_wheel");
+ ODEBody *castorBody =
(ODEBody*)World::Instance()->GetEntityByName("pioneer::castor_body");
+ ODEBody *planeBody =
(ODEBody*)World::Instance()->GetEntityByName("plane1_model::plane1_body");
+
+ if (leftBody && rightBody && planeBody && castorBody)
+ {
+ ODEGeom *leftGeom = (ODEGeom*)leftBody->GetGeom("left_wheel_geom");
+ ODEGeom *rightGeom = (ODEGeom*)rightBody->GetGeom("right_wheel_geom");
+ ODEGeom *castorGeom = (ODEGeom*)castorBody->GetGeom("castor_geom");
+ ODEGeom *planeGeom = (ODEGeom*)planeBody->GetGeom("plane1_geom");
+
+ dContactGeom geomLeft, geomRight, geomCastor;
+ geomLeft.pos[0] = leftBody->GetAbsPose().pos.x;
+ geomLeft.pos[1] = leftBody->GetAbsPose().pos.y;
+ geomLeft.pos[2] = 0;
+ geomLeft.normal[0] = 0;
+ geomLeft.normal[1] = 0;
+ geomLeft.normal[2] = 1;
+ geomLeft.depth = .001;
+ geomLeft.g1 = leftGeom->GetGeomId();
+ geomLeft.g2 = planeGeom->GetGeomId();
+
+ geomRight.pos[0] = rightBody->GetAbsPose().pos.x;
+ geomRight.pos[1] = rightBody->GetAbsPose().pos.y;
+ geomRight.pos[2] = 0;
+ geomRight.normal[0] = 0;
+ geomRight.normal[1] = 0;
+ geomRight.normal[2] = 1;
+ geomRight.depth = .001;
+ geomRight.g1 = rightGeom->GetGeomId();
+ geomRight.g2 = planeGeom->GetGeomId();
+
+ geomCastor.pos[0] = castorBody->GetAbsPose().pos.x;
+ geomCastor.pos[1] = castorBody->GetAbsPose().pos.y;
+ geomCastor.pos[2] = 0;
+ geomCastor.normal[0] = 0;
+ geomCastor.normal[1] = 0;
+ geomCastor.normal[2] = 1;
+ geomCastor.depth = .001;
+ geomCastor.g1 = castorGeom->GetGeomId();
+ geomCastor.g2 = planeGeom->GetGeomId();
+
+ dContact contact;
+ contact.geom = geomLeft;
+ contact.surface.mode = dContactSoftERP | dContactSoftCFM;
+ contact.surface.mu = 0;
+ contact.surface.mu2 = 0;
+ contact.surface.slip1 = 0.1;
+ contact.surface.slip2 = 0.1;
+ contact.surface.bounce = 0;
+ dJointID c = dJointCreateContact(this->worldId, this->contactGroup,
&contact);
+ dJointAttach (c, leftBody->GetODEId(), planeBody->GetODEId());
+
+ dContact contact2;
+ contact2.geom = geomRight;
+ contact2.surface.mode = dContactSoftERP | dContactSoftCFM;
+ contact2.surface.mu = 0;
+ contact2.surface.mu2 = 0;
+ contact2.surface.slip1 = 0.1;
+ contact2.surface.slip2 = 0.1;
+ contact2.surface.bounce = 0;
+ dJointID c2 = dJointCreateContact(this->worldId, this->contactGroup,
&contact2);
+ dJointAttach (c2, rightBody->GetODEId(), planeBody->GetODEId());
+
+ dContact contact3;
+ contact3.geom = geomCastor;
+ contact3.surface.mode = dContactSoftERP | dContactSoftCFM;
+ contact3.surface.mu = 0;
+ contact3.surface.mu2 = 0;
+ contact3.surface.slip1 = 0.1;
+ contact3.surface.slip2 = 0.1;
+ contact3.surface.bounce = 0;
+ dJointID c3 = dJointCreateContact(this->worldId, this->contactGroup,
&contact3);
+ dJointAttach (c3, castorBody->GetODEId(), planeBody->GetODEId());
+
+ }
+
+
// Do collision detection; this will add contacts to the contact group
this->LockMutex();
{
//DiagnosticTimer timer("ODEPhysics Collision Update");
- dSpaceCollide( this->spaceId, this, CollisionCallback );
+ //dSpaceCollide( this->spaceId, this, CollisionCallback );
}
this->UnlockMutex();
@@ -649,7 +728,7 @@
//contact.surface.mode = dContactSlip1 | dContactSlip2 |
// dContactSoftERP | dContactSoftCFM |
// dContactBounce | dContactMu2 |
dContactApprox1;
- contact.surface.mode = dContactSoftERP | dContactSoftCFM |
dContactApprox1;
+ contact.surface.mode = dContactSoftERP | dContactSoftCFM;// |
dContactApprox1;
// with dContactSoftERP | dContactSoftCFM the test_pr2_collision
overshoots the cup
// Compute the CFM and ERP by assuming the two bodies form a
@@ -660,7 +739,7 @@
contact.surface.soft_erp = h * kp / (h * kp + kd);
contact.surface.soft_cfm = 1.0 / (h * kp + kd);
- if (geom1->surface->enableFriction && geom2->surface->enableFriction)
+ /*if (geom1->surface->enableFriction && geom2->surface->enableFriction)
{
contact.surface.mu = std::min(geom1->surface->mu1,
geom2->surface->mu1);
@@ -673,11 +752,12 @@
}
else
{
+ */
contact.surface.mu = 0;
contact.surface.mu2 = 0;
contact.surface.slip1 = 0.1;
contact.surface.slip2 = 0.1;
- }
+ //}
contact.surface.bounce = 0;
/*contact.surface.bounce = std::min(geom1->surface->bounce,
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
