Revision: 8363
http://playerstage.svn.sourceforge.net/playerstage/?rev=8363&view=rev
Author: natepak
Date: 2009-11-03 22:43:17 +0000 (Tue, 03 Nov 2009)
Log Message:
-----------
Fixed up the contact sensor
Modified Paths:
--------------
code/gazebo/trunk/server/physics/ContactParams.hh
code/gazebo/trunk/server/physics/ode/ODEGeom.hh
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
code/gazebo/trunk/server/sensors/contact/ContactSensor.hh
Modified: code/gazebo/trunk/server/physics/ContactParams.hh
===================================================================
--- code/gazebo/trunk/server/physics/ContactParams.hh 2009-11-03 22:07:22 UTC
(rev 8362)
+++ code/gazebo/trunk/server/physics/ContactParams.hh 2009-11-03 22:43:17 UTC
(rev 8363)
@@ -80,7 +80,12 @@
public: double softCfm;
public: bool enableFriction;
-
+
+ public: Vector3 body1Force;
+ public: Vector3 body1Torque;
+ public: Vector3 body2Force;
+ public: Vector3 body2Torque;
+
public: boost::signal< void (Geom*, Geom*)> contactSignal;
};
}
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.hh 2009-11-03 22:07:22 UTC
(rev 8362)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.hh 2009-11-03 22:43:17 UTC
(rev 8363)
@@ -104,8 +104,6 @@
/// ID for the sub-geom
protected: dGeomID geomId;
-
- public: dJointID cID;
};
/// \}
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-03 22:07:22 UTC
(rev 8362)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-11-03 22:43:17 UTC
(rev 8363)
@@ -503,11 +503,17 @@
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;
+ 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]);
+ 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 );
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-03
22:07:22 UTC (rev 8362)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.cc 2009-11-03
22:43:17 UTC (rev 8363)
@@ -29,8 +29,6 @@
#include <sstream>
#include "Global.hh"
-//#include "World.hh"
-//#include "Controller.hh"
#include "GazeboError.hh"
#include "XMLConfig.hh"
@@ -52,11 +50,7 @@
: Sensor(body)
{
this->active = false;
-
this->typeName = "contact";
-
- this->contactCount = 0;
- this->contactStates = NULL;
}
@@ -65,26 +59,21 @@
ContactSensor::~ContactSensor()
{
std::vector< ParamT<std::string> *>::iterator iter;
+ std::vector<ContactState>::iterator citer;
- if (this->contactStates)
- delete [] this->contactStates;
-
- if (this->contactTimes)
- delete [] this->contactTimes;
-
for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++)
- {
delete *iter;
- }
this->geomNamesP.clear();
+
+ this->contacts.clear();
}
//////////////////////////////////////////////////////////////////////////////
/// Get the contact time
double ContactSensor::GetContactTime(unsigned int index) const
{
- if (index < this->contactCount)
- return this->contactTimes[ index ];
+ if (index < this->contacts.size())
+ return this->contacts[index].time;
return 0;
}
@@ -93,15 +82,15 @@
/// Return the number of contacts
unsigned int ContactSensor::GetContactCount() const
{
- return this->contactCount;
+ return this->contacts.size();
}
//////////////////////////////////////////////////////////////////////////////
/// Return the contact states
uint8_t ContactSensor::GetContactState(unsigned int index) const
{
- if (index < this->contactCount)
- return this->contactStates[index];
+ if (index < this->contacts.size())
+ return this->contacts[index].state;
return 0;
}
@@ -110,17 +99,41 @@
/// Return the contact geom name
std::string ContactSensor::GetContactGeomName(unsigned int index) const
{
- if (index < this->contactCount)
- return this->contactNames[index];
+ if (index < this->contacts.size())
+ return this->contacts[index].name;
return std::string("");
}
//////////////////////////////////////////////////////////////////////////////
+/// Return contact force on the first body
+Vector3 ContactSensor::GetContactBody1Force(unsigned int index) const
+{
+ Vector3 result;
+
+ if (index < this->contacts.size())
+ result = this->contacts[index].body1Force;
+
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Return contact force on the second body
+Vector3 ContactSensor::GetContactBody2Force(unsigned int index) const
+{
+ Vector3 result;
+
+ if (index < this->contacts.size())
+ result = this->contacts[index].body2Force;
+
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
/// Return the self geom name
std::string ContactSensor::GetGeomName(unsigned int index) const
{
- if (index < this->contactCount)
+ if (index < this->contacts.size())
return this->geomNamesP[index]->GetValue();
return std::string("");
@@ -130,7 +143,9 @@
/// Reset the contact states
void ContactSensor::ResetContactStates()
{
- memset(this->contactStates, 0, sizeof(uint8_t) * this->contactCount);
+ std::vector<ContactState>::iterator iter;
+ for (iter = this->contacts.begin(); iter != this->contacts.end(); iter++)
+ (*iter).state = 0;
}
//////////////////////////////////////////////////////////////////////////////
@@ -140,9 +155,7 @@
XMLConfigNode *geomNode = NULL;
if (this->body == NULL)
- {
gzthrow("Null body in the contact sensor");
- }
Param::Begin(&this->parameters);
geomNode = node->GetChild("geom");
@@ -154,16 +167,6 @@
geomNode = geomNode->GetNext("geom");
}
Param::End();
-
- this->contactCount = this->geomNamesP.size();
- this->contactTimes = new double[ this->contactCount ];
- this->contactStates = new uint8_t[ this->contactCount ];
- for (unsigned int i=0; i< this->contactCount; i++)
- this->contactNames.push_back("");
-
- memset(this->contactTimes,0, sizeof(double) * this->contactCount);
- memset(this->contactStates,0, sizeof(uint8_t) * this->contactCount);
- memset(this->contactTimes,0, sizeof(double) * this->contactCount);
}
//////////////////////////////////////////////////////////////////////////////
@@ -173,9 +176,7 @@
std::vector< ParamT<std::string> *>::iterator iter;
for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); iter++)
- {
stream << prefix << " " << *(iter) << "\n";
- }
}
//////////////////////////////////////////////////////////////////////////////
@@ -191,15 +192,10 @@
// Set the callback
if (geom)
- {
geom->contact->Callback( &ContactSensor::ContactCallback, this );
- }
else
- {
gzthrow("Unable to find geom[" + **(*iter) + "]");
- }
}
-
}
//////////////////////////////////////////////////////////////////////////////
@@ -218,30 +214,23 @@
/// Contact callback
void ContactSensor::ContactCallback(Geom *g1, Geom *g2)
{
-
// somehow here, extract contact information when user requests it
//
-
-
std::vector< ParamT<std::string> *>::iterator iter;
int i = 0;
-
for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end();
iter++, i++)
{
if ( **(*iter) == g1->GetName() || **(*iter) == g2->GetName() )
{
- if (i < GAZEBO_MAX_CONTACT_FB_DATA)
- {
- /* BULLET
- dJointSetFeedback(g1->cID, this->contactFeedbacks+i);
- */
- }
-
- this->contactStates[i] = 1;
- this->contactTimes[i] = Simulator::Instance()->GetRealTime();
- this->contactNames[i] = **(*iter)==g1->GetName()? g2->GetName() :
g1->GetName();
+ this->contacts[i].state = 1;
+ this->contacts[i].time = Simulator::Instance()->GetRealTime();
+ this->contacts[i].name = **(*iter)==g1->GetName()? g2->GetName() :
g1->GetName();
+ this->contacts[i].body1Force = g1->contact->body1Force;
+ this->contacts[i].body2Force = g1->contact->body2Force;
+ this->contacts[i].body1Torque = g1->contact->body1Torque;
+ this->contacts[i].body2Torque = g1->contact->body2Torque;
}
}
Modified: code/gazebo/trunk/server/sensors/contact/ContactSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2009-11-03
22:07:22 UTC (rev 8362)
+++ code/gazebo/trunk/server/sensors/contact/ContactSensor.hh 2009-11-03
22:43:17 UTC (rev 8363)
@@ -35,8 +35,6 @@
#include "Sensor.hh"
#include "Body.hh"
-#define GAZEBO_MAX_CONTACT_FB_DATA 10
-
namespace gazebo
{
@@ -75,11 +73,12 @@
/// \brief Return contact geometry name
public: std::string GetContactGeomName(unsigned int index) const;
- /* BULLET
- /// \brief Return contact feedback, f1,f2,t1,t2
- public: dJointFeedback GetContactFeedback(unsigned int index) const;
- */
+ /// \brief Return contact force on the first body
+ public: Vector3 GetContactBody1Force(unsigned int index) const;
+ /// \brief Return contact force on the second body
+ public: Vector3 GetContactBody2Force(unsigned int index) const;
+
/// \brief Return geometry name
public: std::string GetGeomName(unsigned int index) const;
@@ -108,12 +107,18 @@
/// Geom name parameter
private: std::vector< ParamT<std::string> *> geomNamesP;
- private: std::vector<std::string> geomNames;
- private: uint8_t *contactStates;
- private: double *contactTimes;
- private: unsigned int contactCount;
- private: std::vector<std::string> contactNames;
+ private: class ContactState
+ {
+ public: std::string name;
+ public: Vector3 body1Force;
+ public: Vector3 body2Force;
+ public: Vector3 body1Torque;
+ public: Vector3 body2Torque;
+ public: double time;
+ public: uint8_t state;
+ };
+ private: std::vector<ContactState> contacts;
};
/// \}
/// \}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
