Revision: 7496
http://playerstage.svn.sourceforge.net/playerstage/?rev=7496&view=rev
Author: hsujohnhsu
Date: 2009-03-17 00:48:50 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
update contact sensors to return contact force and torques.
Modified Paths:
--------------
code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.cc
code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.hh
Modified:
code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.cc
2009-03-17 00:44:57 UTC (rev 7495)
+++ code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.cc
2009-03-17 00:48:50 UTC (rev 7496)
@@ -107,6 +107,36 @@
}
//////////////////////////////////////////////////////////////////////////////
+/// Return the contact geom name
+std::string ContactSensor::GetContactGeomName(unsigned int index) const
+{
+ if (index < this->contactCount)
+ return this->contactNames[index];
+
+ return std::string("");
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Return the contact feedback forces and torques
+dJointFeedback ContactSensor::GetContactFeedback(unsigned int index) const
+{
+ if (index < this->contactCount)
+ return this->contactFeedbacks[index];
+ return dJointFeedback();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+/// Return the self geom name
+std::string ContactSensor::GetGeomName(unsigned int index) const
+{
+ if (index < this->contactCount)
+ return this->geomNamesP[index]->GetValue();
+
+ return std::string("");
+}
+
+//////////////////////////////////////////////////////////////////////////////
/// Reset the contact states
void ContactSensor::ResetContactStates()
{
@@ -138,9 +168,13 @@
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->contactStates,0, sizeof(double) * this->contactCount);
+ //memset(this->contactFeedbacks,0, sizeof(dJointFeedback) *
this->contactCount);
+ //this->contactFeedbacks = new dJointFeedback[this->contactCount];
}
//////////////////////////////////////////////////////////////////////////////
@@ -195,6 +229,11 @@
/// 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;
@@ -204,9 +243,29 @@
{
if ( **(*iter) == g1->GetName() || **(*iter) == g2->GetName() )
{
+ if (i < GAZEBO_MAX_CONTACT_FB_DATA)
+ {
+ dJointSetFeedback(g1->cID, this->contactFeedbacks+i);
+
+ // std::cout << "contact id:" << i
+ // << " geom1:" << g1->GetName()
+ // << " geom2:" << g2->GetName();
+ // std::cout << " contact joint ID:" << g1->cID
+ // << "
f1:"<<this->contactFeedbacks[i].f1[0]<<","<<this->contactFeedbacks[i].f1[1]<<","<<this->contactFeedbacks[i].f1[2]<<","<<this->contactFeedbacks[i].f1[3]
+ // << "
t1:"<<this->contactFeedbacks[i].t1[0]<<","<<this->contactFeedbacks[i].t1[1]<<","<<this->contactFeedbacks[i].t1[2]<<","<<this->contactFeedbacks[i].t1[3]
+ // << " f1_magnitude:" <<
dLENGTH(this->contactFeedbacks[i].f1)
+ // << "
f2:"<<this->contactFeedbacks[i].f2[0]<<","<<this->contactFeedbacks[i].f2[1]<<","<<this->contactFeedbacks[i].f2[2]<<","<<this->contactFeedbacks[i].f2[3]
+ // << "
t2:"<<this->contactFeedbacks[i].t2[0]<<","<<this->contactFeedbacks[i].t2[1]<<","<<this->contactFeedbacks[i].t2[2]<<","<<this->contactFeedbacks[i].t2[3]
+ // << " f2_magnitude:" <<
dLENGTH(this->contactFeedbacks[i].f2)
+ // << std::endl;
+ }
+
this->contactStates[i] = 1;
this->contactTimes[i] = Simulator::Instance()->GetRealTime();
+ this->contactNames[i] = **(*iter)==g1->GetName()? g2->GetName() :
g1->GetName();
}
}
}
+
+
Modified:
code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.hh
2009-03-17 00:44:57 UTC (rev 7495)
+++ code/gazebo/branches/ogre-1.4.9/server/sensors/contact/ContactSensor.hh
2009-03-17 00:48:50 UTC (rev 7496)
@@ -33,6 +33,8 @@
#include "Sensor.hh"
#include "Body.hh"
+#define GAZEBO_MAX_CONTACT_FB_DATA 10
+
namespace gazebo
{
@@ -68,6 +70,15 @@
/// \brief Return a contact state
public: uint8_t GetContactState(unsigned int index) const;
+ /// \brief Return contact geometry name
+ public: std::string GetContactGeomName(unsigned int index) const;
+
+ /// \brief Return contact feedback, f1,f2,t1,t2
+ public: dJointFeedback GetContactFeedback(unsigned int index) const;
+
+ /// \brief Return geometry name
+ public: std::string GetGeomName(unsigned int index) const;
+
/// \brief Reset the contact states
public: void ResetContactStates();
@@ -93,9 +104,14 @@
/// 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;
+ /// \brief a place for storing joint feedbacks, including contact joints
+ private: dJointFeedback contactFeedbacks[GAZEBO_MAX_CONTACT_FB_DATA];
+
};
/// \}
/// \}
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
