Revision: 7501
http://playerstage.svn.sourceforge.net/playerstage/?rev=7501&view=rev
Author: hsujohnhsu
Date: 2009-03-17 01:04:14 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
* option to use quick step in ODE.
* make spaceId public so we can arbitrarily assign collision from geoms even
for the same model.
* debug comments for timing and threading.
* assign Geom's cID upon contact. This only allows single contact for now,
need to make it more general.
Modified Paths:
--------------
code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.cc
code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.hh
Modified: code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.cc
2009-03-17 01:00:38 UTC (rev 7500)
+++ code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.cc
2009-03-17 01:04:14 UTC (rev 7501)
@@ -44,6 +44,10 @@
#include "XMLConfig.hh"
#include "ODEPhysics.hh"
+#ifdef TIMING
+#include "Simulator.hh"// for timing
+#endif
+
using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
@@ -70,6 +74,9 @@
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
this->globalERPP = new ParamT<double>("erp", 0.2, 0);
+ this->quickStepP = new ParamT<bool>("quickStep", false, 0);
+ this->quickStepItersP = new ParamT<int>("quickStepIters", 20, 0);
+ this->quickStepWP = new ParamT<double>("quickStepW", 1.3, 0); ///
over_relaxation value for SOR
Param::End();
}
@@ -88,6 +95,7 @@
delete this->globalCFMP;
delete this->globalERPP;
+ delete this->quickStepP;
}
////////////////////////////////////////////////////////////////////////////////
@@ -103,6 +111,9 @@
this->updateRateP->Load(cnode);
this->globalCFMP->Load(cnode);
this->globalERPP->Load(cnode);
+ this->quickStepP->Load(cnode);
+ this->quickStepItersP->Load(cnode);
+ this->quickStepWP->Load(cnode);
}
////////////////////////////////////////////////////////////////////////////////
@@ -115,6 +126,7 @@
stream << prefix << " " << *(this->updateRateP) << "\n";
stream << prefix << " " << *(this->globalCFMP) << "\n";
stream << prefix << " " << *(this->globalERPP) << "\n";
+ stream << prefix << " " << *(this->quickStepP) << "\n";
stream << prefix << "</physics:ode>\n";
}
@@ -126,25 +138,88 @@
dWorldSetGravity(this->worldId, g.x, g.y, g.z);
dWorldSetCFM(this->worldId, this->globalCFMP->GetValue());
dWorldSetERP(this->worldId, this->globalERPP->GetValue());
+ dWorldSetQuickStepNumIterations(this->worldId,
this->quickStepItersP->GetValue() );
+ dWorldSetQuickStepW(this->worldId, this->quickStepWP->GetValue() );
+}
+////////////////////////////////////////////////////////////////////////////////
+// Update the ODE collisions, create joints
+void ODEPhysics::UpdateCollision()
+{
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
+ // Do collision detection; this will add contacts to the contact group
+ dSpaceCollide( this->spaceId, this, CollisionCallback );
+
+ //usleep(1000000);
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
+#endif
}
////////////////////////////////////////////////////////////////////////////////
// Update the ODE engine
-void ODEPhysics::Update()
+// void ODEPhysics::UpdatePhysics()
+// {
+// #ifdef TIMING
+// double tmpT1 = Simulator::Instance()->GetWallTime();
+// #endif
+//
+// // Update the dynamical model
+// if (this->quickStepP->GetValue())
+// dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
+// else
+// dWorldStep( this->worldId, this->stepTimeP->GetValue() );
+//
+// #ifdef TIMING
+// double tmpT3 = Simulator::Instance()->GetWallTime();
+// std::cout << " ODE step DT (" << tmpT3-tmpT1 << ")" << std::endl;
+// //std::cout << " Physics Total DT (" << tmpT3-tmpT1 << ")" << std::endl;
+// #endif
+//
+// // Very important to clear out the contact group
+// dJointGroupEmpty( this->contactGroup );
+//
+// }
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the ODE engine
+void ODEPhysics::UpdatePhysics()
{
+#ifdef TIMING
+ double tmpT1 = Simulator::Instance()->GetWallTime();
+#endif
+
// Do collision detection; this will add contacts to the contact group
dSpaceCollide( this->spaceId, this, CollisionCallback );
+ //usleep(1000000);
+#ifdef TIMING
+ double tmpT2 = Simulator::Instance()->GetWallTime();
+ std::cout << " Collision DT (" << tmpT2-tmpT1 << ")" << std::endl;
+#endif
+
// Update the dynamical model
- dWorldStep( this->worldId, this->stepTimeP->GetValue() );
- //dWorldQuickStep(this->worldId, this->stepTime);
+ if (this->quickStepP->GetValue())
+ dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
+ else
+ dWorldStep( this->worldId, this->stepTimeP->GetValue() );
+#ifdef TIMING
+ double tmpT3 = Simulator::Instance()->GetWallTime();
+ std::cout << " ODE step DT (" << tmpT3-tmpT2 << ")" << std::endl;
+ //std::cout << " Physics Total DT (" << tmpT3-tmpT1 << ")" << std::endl;
+#endif
+
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
}
+
////////////////////////////////////////////////////////////////////////////////
// Finilize the ODE engine
void ODEPhysics::Fini()
@@ -263,18 +338,19 @@
continue;
contact.geom = contactGeoms[i];
- contact.surface.mode = dContactSlip1 | dContactSlip2 |
- dContactSoftERP | dContactSoftCFM |
- dContactBounce | dContactMu2 | dContactApprox1;
+ //contact.surface.mode = dContactSlip1 | dContactSlip2 |
+ // dContactSoftERP | dContactSoftCFM |
+ // dContactBounce | dContactMu2 |
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
// spring-damper system.
h = self->stepTimeP->GetValue();
- kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp);
+ kp = 1.0 / (1.0 / geom1->contact->kp + 1.0 / geom2->contact->kp);
kd = geom1->contact->kd + geom2->contact->kd;
contact.surface.soft_erp = h * kp / (h * kp + kd);
- contact.surface.soft_cfm = 1 / (h * kp + kd);
+ contact.surface.soft_cfm = 1.0 / (h * kp + kd);
contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2);
@@ -290,6 +366,11 @@
dJointID c = dJointCreateContact (self->worldId,
self->contactGroup, &contact);
+ // save "dJointID *c" in Geom, so we can do a
+ // dJointFeedback *jft = dJointGetFeedback( c[i] ) later
+ geom1->cID = c;
+ geom2->cID = c;
+
// Call the geom's contact callbacks
geom1->contact->contactSignal( geom1, geom2 );
geom2->contact->contactSignal( geom2, geom1 );
Modified: code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.hh
2009-03-17 01:00:38 UTC (rev 7500)
+++ code/gazebo/branches/ogre-1.4.9/server/physics/ode/ODEPhysics.hh
2009-03-17 01:04:14 UTC (rev 7501)
@@ -32,6 +32,9 @@
#include "Param.hh"
#include "PhysicsEngine.hh"
+#include <boost/thread/thread.hpp>
+#include <boost/thread/mutex.hpp>
+
namespace gazebo
{
class SphereGeom;
@@ -98,8 +101,11 @@
/// \brief Initialize the ODE engine
public: virtual void Init();
+ /// \brief Update the ODE collision
+ public: virtual void UpdateCollision();
+
/// \brief Update the ODE engine
- public: virtual void Update();
+ public: virtual void UpdatePhysics();
/// \brief Finilize the ODE engine
public: virtual void Fini();
@@ -126,13 +132,16 @@
private: dWorldID worldId;
/// \brief Top-level space for all sub-spaces/geoms
- private: dSpaceID spaceId;
+ public: dSpaceID spaceId;
/// \brief Collision attributes
private: dJointGroupID contactGroup;
private: ParamT<double> *globalCFMP;
private: ParamT<double> *globalERPP;
+ private: ParamT<bool> *quickStepP;
+ private: ParamT<int> *quickStepItersP;
+ private: ParamT<double> *quickStepWP;
};
/** \}*/
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
