Revision: 8431
http://playerstage.svn.sourceforge.net/playerstage/?rev=8431&view=rev
Author: natepak
Date: 2009-11-21 23:43:09 +0000 (Sat, 21 Nov 2009)
Log Message:
-----------
more updates to the contact feedback system
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Contact.cc
code/gazebo/trunk/server/physics/Contact.hh
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
code/gazebo/trunk/server/sensors/contact/ContactSensor.hh
Modified: code/gazebo/trunk/server/physics/Contact.cc
===================================================================
--- code/gazebo/trunk/server/physics/Contact.cc 2009-11-21 21:45:42 UTC (rev
8430)
+++ code/gazebo/trunk/server/physics/Contact.cc 2009-11-21 23:43:09 UTC (rev
8431)
@@ -63,9 +63,29 @@
{
this->geom1 = contact.geom1;
this->geom2 = contact.geom2;
- this->forces = contact.forces;
- this->pos = contact.pos;
- this->normal = contact.normal;
- this->depth = contact.depth;
+
+ this->forces.clear();
+ this->positions.clear();
+ this->normals.clear();
+
+ std::copy(contact.forces.begin(), contact.forces.end(),
+ std::back_inserter(this->forces));
+ std::copy(contact.positions.begin(), contact.positions.end(),
+ std::back_inserter(this->positions));
+ std::copy(contact.normals.begin(), contact.normals.end(),
+ std::back_inserter(this->normals));
+ std::copy(contact.depths.begin(), contact.depths.end(),
+ std::back_inserter(this->depths));
+
return *this;
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Reset
+void Contact::Reset()
+{
+ this->depths.clear();
+ this->positions.clear();
+ this->normals.clear();
+ this->forces.clear();
+}
Modified: code/gazebo/trunk/server/physics/Contact.hh
===================================================================
--- code/gazebo/trunk/server/physics/Contact.hh 2009-11-21 21:45:42 UTC (rev
8430)
+++ code/gazebo/trunk/server/physics/Contact.hh 2009-11-21 23:43:09 UTC (rev
8431)
@@ -27,13 +27,17 @@
#ifndef CONTACT_HH
#define CONTACT_HH
+#include <vector>
+
#include "Vector3.hh"
#include "JointFeedback.hh"
namespace gazebo
{
class Geom;
-
+
+ /// \brief A contact between two geoms. Each contact can consist of
+ /// a numnber of contact points
class Contact
{
/// \brief Constructor
@@ -50,16 +54,19 @@
/// \brief Operator =
public: const Contact &operator=(const Contact &contact);
+
+ /// \brief Reset
+ public: void Reset();
public: Geom *geom1;
public: Geom *geom2;
- public: JointFeedback forces;
+ public: std::vector<JointFeedback> forces;
- public: Vector3 pos;
- public: Vector3 normal;
+ public: std::vector<Vector3> positions;
+ public: std::vector<Vector3> normals;
- public: double depth;
+ public: std::vector<double> depths;
};
}
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-21 21:45:42 UTC
(rev 8430)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-21 23:43:09 UTC
(rev 8431)
@@ -195,6 +195,9 @@
// Update the ODE collisions, create joints
void ODEPhysics::UpdateCollision()
{
+ std::vector<ContactFeedback>::iterator iter;
+ std::vector<dJointFeedback>::iterator jiter;
+
#ifdef TIMING
double tmpT1 = Simulator::Instance()->GetWallTime();
#endif
@@ -206,26 +209,31 @@
// 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++)
+ for (iter = this->contactFeedbacks.begin();
+ iter != this->contactFeedbackIter; iter++)
{
-
if ((*iter).contact.geom1 == NULL)
gzerr(0) << "collision update Geom1 is null\n";
if ((*iter).contact.geom2 == NULL)
gzerr(0) << "Collision update Geom2 is null\n";
- (*iter).contact.forces.body1Force.Set( (*iter).feedback.f1[0],
- (*iter).feedback.f1[1], (*iter).feedback.f1[2]);
- (*iter).contact.forces.body2Force.Set( (*iter).feedback.f2[0],
- (*iter).feedback.f2[1], (*iter).feedback.f2[2]);
- (*iter).contact.forces.body1Torque.Set((*iter).feedback.t1[0],
- (*iter).feedback.t1[1], (*iter).feedback.t1[2]);
- (*iter).contact.forces.body2Torque.Set((*iter).feedback.t2[0],
- (*iter).feedback.t2[1], (*iter).feedback.t2[2]);
+ (*iter).contact.forces.clear();
+ // Copy all the joint forces to the contact
+ for (jiter = (*iter).feedbacks.begin(); jiter != (*iter).feedbacks.end();
+ jiter++)
+ {
+ JointFeedback feedback;
+ feedback.body1Force.Set( (*jiter).f1[0], (*jiter).f1[1], (*jiter).f1[2]
);
+ feedback.body2Force.Set( (*jiter).f2[0], (*jiter).f2[1], (*jiter).f2[2]
);
+
+ feedback.body1Torque.Set((*jiter).t1[0], (*jiter).t1[1], (*jiter).t1[2]);
+ feedback.body2Torque.Set((*jiter).t2[0], (*jiter).t2[1], (*jiter).t2[2]);
+
+ (*iter).contact.forces.push_back(feedback);
+ }
+
// Add the contact to each geom
(*iter).contact.geom1->AddContact( (*iter).contact );
(*iter).contact.geom2->AddContact( (*iter).contact );
@@ -473,6 +481,10 @@
if (numc != 0)
{
+ (*self->contactFeedbackIter).contact.geom1 = geom1;
+ (*self->contactFeedbackIter).contact.geom2 = geom2;
+ (*self->contactFeedbackIter).feedbacks.resize(numc);
+
for (i=0; i<numc; i++)
{
double h, kp, kd;
@@ -522,26 +534,24 @@
dJointID c = dJointCreateContact (self->worldId,
self->contactGroup, &contact);
- if (self->contactFeedbackIter == self->contactFeedbacks.end())
- {
- self->contactFeedbacks.resize( self->contactFeedbacks.size() + 500);
- }
-
// Store the contact info
- (*self->contactFeedbackIter).contact.geom1 = geom1;
- (*self->contactFeedbackIter).contact.geom2 = geom2;
- (*self->contactFeedbackIter).contact.depth = contact.geom.depth;
- (*self->contactFeedbackIter).contact.pos.x = contact.geom.pos[0];
- (*self->contactFeedbackIter).contact.pos.y = contact.geom.pos[1];
- (*self->contactFeedbackIter).contact.pos.z = contact.geom.pos[2];
- (*self->contactFeedbackIter).contact.normal.x = contact.geom.normal[0];
- (*self->contactFeedbackIter).contact.normal.y = contact.geom.normal[1];
- (*self->contactFeedbackIter).contact.normal.z = contact.geom.normal[2];
- dJointSetFeedback(c, &(*self->contactFeedbackIter).feedback);
- self->contactFeedbackIter++;
+ (*self->contactFeedbackIter).contact.depths.push_back(
+ contact.geom.depth);
+ (*self->contactFeedbackIter).contact.positions.push_back(
+ Vector3(contact.geom.pos[0], contact.geom.pos[1],
+ contact.geom.pos[2]) );
+ (*self->contactFeedbackIter).contact.normals.push_back(
+ Vector3(contact.geom.normal[0], contact.geom.normal[1],
+ contact.geom.normal[2]) );
+ dJointSetFeedback(c, &((*self->contactFeedbackIter).feedbacks[i]));
+
dJointAttach (c,b1,b2);
}
+
+ self->contactFeedbackIter++;
+ if (self->contactFeedbackIter == self->contactFeedbacks.end())
+ self->contactFeedbacks.resize( self->contactFeedbacks.size() + 500);
}
}
}
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2009-11-21 21:45:42 UTC
(rev 8430)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2009-11-21 23:43:09 UTC
(rev 8431)
@@ -162,8 +162,8 @@
private: class ContactFeedback
{
- public: dJointFeedback feedback;
public: Contact contact;
+ public: std::vector<dJointFeedback> feedbacks;
};
private: std::vector<ContactFeedback> contactFeedbacks;
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-21
21:45:42 UTC (rev 8430)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-21
23:43:09 UTC (rev 8431)
@@ -145,7 +145,7 @@
//////////////////////////////////////////////////////////////////////////////
/// Get a contact for a geom by index
-Contact ContactSensor::GetContact(unsigned int geom, unsigned int index) const
+Contact ContactSensor::GetGeomContact(unsigned int geom, unsigned int index)
const
{
if (geom < this->geoms.size())
return this->geoms[geom]->GetContact( index );
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2009-11-21
21:45:42 UTC (rev 8430)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2009-11-21
23:43:09 UTC (rev 8431)
@@ -80,11 +80,14 @@
/// \brief Get the number of geoms that the sensor is observing
public: unsigned int GetGeomCount() const;
+ /// \brief Get a geom
+ public: Geom *GetGeom(unsigned int index);
+
/// \brief Return the number of contacts for an observed geom
public: unsigned int GetGeomContactCount(unsigned int geomIndex) const;
/// \brief Get a contact for a geom by index
- public: Contact GetContact(unsigned int geom, unsigned int index) const;
+ public: Contact GetGeomContact(unsigned int geom, unsigned int index)
const;
/// Geom name parameter
private: std::vector< ParamT<std::string> *> geomNamesP;
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
