Revision: 8366
http://playerstage.svn.sourceforge.net/playerstage/?rev=8366&view=rev
Author: natepak
Date: 2009-11-04 21:26:28 +0000 (Wed, 04 Nov 2009)
Log Message:
-----------
Fixed factory loading
Modified Paths:
--------------
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/ode/ODEBody.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2009-11-04 16:38:26 UTC (rev 8365)
+++ code/gazebo/trunk/server/Entity.cc 2009-11-04 21:26:28 UTC (rev 8366)
@@ -343,6 +343,7 @@
// Handle a change of pose
void Entity::PoseChange(bool notify)
{
+
this->visualNode->SetDirty(true, this->relativePose);
if (notify)
{
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-11-04 16:38:26 UTC (rev 8365)
+++ code/gazebo/trunk/server/Model.cc 2009-11-04 21:26:28 UTC (rev 8366)
@@ -203,9 +203,13 @@
this->SetRelativePose( Pose3d( **this->xyzP, **this->rpyP) );
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");
@@ -873,7 +877,7 @@
}
catch (GazeboError e)
{
- std::cerr << "Error Loading Controller[" << controllerName
+ gzerr(0) << "Error Loading Controller[" << controllerName
<< "]\n" << e << std::endl;
delete controller;
controller = NULL;
@@ -1142,14 +1146,13 @@
}
catch (GazeboError e)
{
- std::cerr << "Error Loading Joint[" << childNode->GetString("name",
std::string(), 0) << "]\n";
- std::cerr << e << std::endl;
+ gzerr(0) << "Error Loading Joint[" << childNode->GetString("name",
std::string(), 0) << "]\n";
+ gzerr(0) << e << std::endl;
childNode = childNode->GetNextByNSPrefix("joint");
continue;
}
childNode = childNode->GetNextByNSPrefix("joint");
}
-
}
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-11-04 16:38:26 UTC (rev 8365)
+++ code/gazebo/trunk/server/World.cc 2009-11-04 21:26:28 UTC (rev 8366)
@@ -175,8 +175,6 @@
// Load OpenAL audio
if (rootNode->GetChild("openal","audio"))
{
- //this->openAL = new OpenALAPI();
- //this->openAL->Load(rootNode->GetChild("openal", "audio"));
this->openAL = OpenAL::Instance();
this->openAL->Load(rootNode->GetChild("openal", "audio"));
}
@@ -445,6 +443,7 @@
// Load all the entities that have been queued
void World::ProcessEntitiesToLoad()
{
+
if (!this->toLoadEntities.empty())
{
// maybe try try_lock here instead
@@ -473,7 +472,6 @@
this->LoadEntities( xmlConfig->GetRootNode(), NULL, true);
delete xmlConfig;
}
-
this->toLoadEntities.clear();
}
}
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-11-04 16:38:26 UTC (rev
8365)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-11-04 21:26:28 UTC (rev
8366)
@@ -350,10 +350,8 @@
// global-inertial damping is implemented in ode svn trunk
if(this->GetId() && this->dampingFactorP->GetValue() > 0)
{
- this->physicsEngine->LockMutex();
this->SetLinearDamping(**this->dampingFactorP);
this->SetAngularDamping(**this->dampingFactorP);
- this->physicsEngine->UnlockMutex();
}
this->linearAccel.Set(0,0,0);
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-11-04 16:38:26 UTC
(rev 8365)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2009-11-04 21:26:28 UTC
(rev 8366)
@@ -114,7 +114,7 @@
//std::cout << "Body Move[" << pose << "]\n";
self->SetAbsPose(pose, false);
- self->physicsEngine->LockMutex();
+ self->physicsEngine->UnlockMutex();
}
////////////////////////////////////////////////////////////////////////////////
@@ -153,12 +153,12 @@
if ( this->bodyId && odeGeom->IsPlaceable())
{
- this->physicsEngine->LockMutex();
-
if (odeGeom->GetGeomId())
+ {
+ this->physicsEngine->LockMutex();
dGeomSetBody(odeGeom->GetGeomId(), this->bodyId);
-
- this->physicsEngine->UnlockMutex();
+ this->physicsEngine->UnlockMutex();
+ }
}
}
@@ -183,10 +183,8 @@
// Set the rotation of the ODE body
dBodySetQuaternion(this->bodyId, q);
this->physicsEngine->UnlockMutex();
-
}
-////////////////////////////////////////////////////////////////////////////////
// Return the position of the body. in global CS
Vector3 ODEBody::GetPositionRate() const
{
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-04 16:38:26 UTC
(rev 8365)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-04 21:26:28 UTC
(rev 8366)
@@ -90,8 +90,14 @@
this->contactMaxCorrectingVelP = new
ParamT<double>("contactMaxCorrectingVel", 10.0, 0);
this->contactSurfaceLayerP = new ParamT<double>("contactSurfaceLayer", 0.01,
0);
Param::End();
+
+ //this->contactFeedbacks.resize(100);
+
+ // Reset the contact pointer
+ //this->contactFeedbackIter = this->contactFeedbacks.begin();
}
+
////////////////////////////////////////////////////////////////////////////////
// Destructor
ODEPhysics::~ODEPhysics()
@@ -193,6 +199,32 @@
dSpaceCollide( this->spaceId, this, CollisionCallback );
this->UnlockMutex();
+ // Process all the contacts, get the feedback info, and call the geom
+ // callbacks
+ /*for (std::vector<ContactFeedback>::iterator iter =
+ this->contactFeedbacks.begin();
+ iter != this->contactFeedbackIter; iter++)
+ {
+
+ (*iter).geom1->contact->body1Force.Set(
+ (*iter).feedback.f1[0], (*iter).feedback.f1[1],
(*iter).feedback.f1[2]);
+ (*iter).geom2->contact->body2Force.Set(
+ (*iter).feedback.f2[0], (*iter).feedback.f2[1],
(*iter).feedback.f2[2]);
+
+ (*iter).geom1->contact->body1Torque.Set(
+ (*iter).feedback.t1[0], (*iter).feedback.t1[1],
(*iter).feedback.t1[2]);
+ (*iter).geom1->contact->body2Torque.Set(
+ (*iter).feedback.t2[0], (*iter).feedback.t2[1],
(*iter).feedback.t2[2]);
+
+ // Call the geom's contact callbacks
+ (*iter).geom1->contact->contactSignal( (*iter).geom1, (*iter).geom2 );
+ (*iter).geom2->contact->contactSignal( (*iter).geom2, (*iter).geom1 );
+ }
+
+ // Reset the contact pointer
+ this->contactFeedbackIter = this->contactFeedbacks.begin();
+ */
+
//usleep(1000000);
#ifdef TIMING
double tmpT2 = Simulator::Instance()->GetWallTime();
@@ -234,9 +266,10 @@
#endif
this->LockMutex();
+ this->UpdateCollision();
// Do collision detection; this will add contacts to the contact group
- dSpaceCollide( this->spaceId, this, CollisionCallback );
+ //dSpaceCollide( this->spaceId, this, CollisionCallback );
//usleep(1000000);
#ifdef TIMING
@@ -503,21 +536,18 @@
dJointID c = dJointCreateContact (self->worldId,
self->contactGroup, &contact);
- dJointFeedback *feedback = dJointGetFeedback(c);
- geom1->contact->body1Force.Set(feedback->f1[0],feedback->f1[1],
- feedback->f1[2]);
- geom2->contact->body2Force.Set(feedback->f2[0],feedback->f2[1],
- feedback->f2[2]);
+ /*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;
+ dJointSetFeedback(c, &(*self->contactFeedbackIter).feedback);
+ self->contactFeedbackIter++;
+ */
- geom1->contact->body1Torque.Set(feedback->t1[0],feedback->t1[1],
- feedback->t1[2]);
- geom1->contact->body2Torque.Set(feedback->t2[0],feedback->t2[1],
- feedback->t2[2]);
-
- // Call the geom's contact callbacks
- geom1->contact->contactSignal( geom1, geom2 );
- geom2->contact->contactSignal( geom2, geom1 );
-
dJointAttach (c,b1,b2);
}
}
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2009-11-04 16:38:26 UTC
(rev 8365)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2009-11-04 21:26:28 UTC
(rev 8366)
@@ -159,6 +159,16 @@
private: ParamT<double> *quickStepWP;
private: ParamT<double> *contactMaxCorrectingVelP;
private: ParamT<double> *contactSurfaceLayerP;
+
+ private: class ContactFeedback
+ {
+ public: dJointFeedback feedback;
+ public: Geom *geom1;
+ public: Geom *geom2;
+ };
+
+ private: std::vector<ContactFeedback> contactFeedbacks;
+ private: std::vector<ContactFeedback>::iterator contactFeedbackIter;
};
/** \}*/
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
