Skip to content

Commit

Permalink
[skinDynLib] Add forceTorqueEstimateConfidence
Browse files Browse the repository at this point in the history 
Add a new integer attribute (that is correctly transmitted over the network)
that represents the confidence on the force/torque estimate trasmitted
with the skinContact . The actual meaning is implementation defined, but
the idea that more precise estimation (for example estimation using calibrated
skin) should fill this parameter with a greater integer w.r.t. rough estimations
of the contact force.
  • Loading branch information
@traversaro
traversaro committed Aug 14, 2017
1 parent 848c50f commit 2f370f6
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 14 deletions.
18 changes: 17 additions & 1 deletion src/libraries/skinDynLib/include/iCub/skinDynLib/skinContact.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,10 @@ class skinContact : public dynContact
unsigned int activeTaxels;
// list of active taxel ids
std::vector<unsigned int> taxelList;

// non-zero integer representing the confidence on the provided force/torque estimate provided (default: 0)
unsigned int forceTorqueEstimateConfidence;


public:
//~~~~~~~~~~~~~~~~~~~~~~
// CONSTRUCTORS
Expand Down Expand Up @@ -187,6 +190,12 @@ class skinContact : public dynContact
* @return the list of taxels' id's
*/
std::vector<unsigned int> getTaxelList() const{ return taxelList; }
/**
* Get a non-zero integer representing the confidence in the provided force/torque estimate.
* @return the forceTorqueEstimateConfidence
* @note the concrete meaning of this parameter is implementation defined.
*/
unsigned int getForceTorqueEstimateConfidence() const{ return forceTorqueEstimateConfidence; }


//~~~~~~~~~~~~~~~~~~~~~~
Expand Down Expand Up @@ -228,6 +237,12 @@ class skinContact : public dynContact
* @return true if the operation succeeded, false otherwise
*/
void setTaxelList(const std::vector<unsigned int> &list);
/**
* Set a non-zero integer representing the confidence in the provided force/torque estimate.
* @param forceTorqueEstimateConfidence
* @note the concrete meaning of this parameter is implementation defined.
*/
void setForceTorqueEstimateConfidence(unsigned int forceTorqueEstimateConfidence);

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// SERIALIZATION methods
Expand All @@ -251,6 +266,7 @@ class skinContact : public dynContact
* - a list of N int, i.e. the active taxel ids
* - a double, i.e. the pressure
* - a list of 2 strings, containing linkName, frameName
* - a int, the forceTorqueEstimateConfidence
* @param connection connection to write to
* @return true iff a skinContact was written correctly
*/
Expand Down
32 changes: 23 additions & 9 deletions src/libraries/skinDynLib/src/skinContact.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,39 +21,39 @@ using namespace std;
// CONSTRUCTORS
//~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const dynContact &c)
:dynContact(c), skinPart(SKIN_PART_UNKNOWN), geoCenter(zeros(3)), pressure(0.0), activeTaxels(0), normalDir(zeros(3)) {}
:dynContact(c), skinPart(SKIN_PART_UNKNOWN), geoCenter(zeros(3)), pressure(0.0), activeTaxels(0), normalDir(zeros(3)), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
const yarp::sig::Vector &_geoCenter, unsigned int _activeTaxels, double _pressure)
:dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(zeros(3)){}
geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(zeros(3)), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
const yarp::sig::Vector &_geoCenter, unsigned int _activeTaxels, double _pressure, const Vector &_normalDir)
:dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(_normalDir){}
geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(_normalDir), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
const yarp::sig::Vector &_geoCenter, vector<unsigned int> _taxelList, double _pressure)
:dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(zeros(3)){}
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(zeros(3)), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
const yarp::sig::Vector &_geoCenter, vector<unsigned int> _taxelList, double _pressure, const Vector &_normalDir)
:dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir){}
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
const yarp::sig::Vector &_geoCenter, std::vector<unsigned int> _taxelList, double _pressure, const yarp::sig::Vector &_normalDir,
const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
:dynContact(_bodyPart,_linkNumber,_CoP,_Mu), skinPart(_skinPart),
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir){
geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir), forceTorqueEstimateConfidence(0) {
this->setForce(_F); //note that dynContact constructor sets Fmodule to 0; here setForce() overwrites the init with the proper force vector and sets
//also Fmodule and Fdir appropriately
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
skinContact::skinContact()
: dynContact(), skinPart(SKIN_PART_UNKNOWN), geoCenter(zeros(3)), pressure(0.0), activeTaxels(0), normalDir(zeros(3)) {}
: dynContact(), skinPart(SKIN_PART_UNKNOWN), geoCenter(zeros(3)), pressure(0.0), activeTaxels(0), normalDir(zeros(3)), forceTorqueEstimateConfidence(0) {}
//~~~~~~~~~~~~~~~~~~~~~~
// SET methods
//~~~~~~~~~~~~~~~~~~~~~~
Expand Down Expand Up @@ -91,6 +91,10 @@ void skinContact::setTaxelList(const vector<unsigned int> &list){
taxelList = list;
activeTaxels = list.size();
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void skinContact::setForceTorqueEstimateConfidence(unsigned int _forceTorqueEstimateConfidence){
forceTorqueEstimateConfidence = _forceTorqueEstimateConfidence;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// SERIALIZATION methods
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Expand All @@ -105,7 +109,7 @@ bool skinContact::write(ConnectionWriter& connection){
// - a list of N int, i.e. the active taxel ids
// - a double, i.e. the pressure
// - a list of 2 string, i.e. linkName, frameName

// - a int, the forceTorqueEstimateConfidence
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(8);
// list of 4 int, i.e. contactId, bodyPart, linkNumber, skinPart
Expand Down Expand Up @@ -147,6 +151,9 @@ bool skinContact::write(ConnectionWriter& connection){
connection.appendInt(2);
connection.appendString(linkName.c_str());
connection.appendString(frameName.c_str());
// - a int, the forceTorqueEstimateConfidence
connection.appendInt(BOTTLE_TAG_INT);
connection.appendInt(forceTorqueEstimateConfidence);

// if someone is foolish enough to connect in text mode,
// let them see something readable.
Expand All @@ -169,6 +176,7 @@ bool skinContact::read(ConnectionReader& connection){
// - a list of N int, i.e. the active taxel ids
// - a double, i.e. the pressure
// - a list of 2 string, i.e. linkName, frameName
// - a int, the forceTorqueEstimateConfidence
if(connection.expectInt() != BOTTLE_TAG_LIST || connection.expectInt() != 8)
return false;

Expand Down Expand Up @@ -224,6 +232,11 @@ bool skinContact::read(ConnectionReader& connection){
linkName = connection.expectText();
frameName = connection.expectText();

// - a int, the forceTorqueEstimateConfidence
if(connection.expectInt()!=BOTTLE_TAG_INT)
return false;
forceTorqueEstimateConfidence = connection.expectInt();

return !connection.isError();
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Expand Down Expand Up @@ -275,7 +288,8 @@ string skinContact::toString(int precision) const{
stringstream res;
res<< dynContact::toString(precision)<< ", Skin part: "<< SkinPart_s[skinPart]<< ", geometric center: "<<
geoCenter.toString(precision)<< ", normal direction: "<< normalDir.toString(precision)<<
", active taxels: "<< activeTaxels<< ", pressure: "<< pressure;
", active taxels: "<< activeTaxels<< ", pressure: "<< pressure << ", forceTorqueConfidence:"
<< forceTorqueEstimateConfidence;
return res.str();
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Expand Down
10 changes: 6 additions & 4 deletions src/libraries/skinDynLib/test/skinContactTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ void fillSkinContactWithArbitraryData(iCub::skinDynLib::skinContact & contact)
contact.setTaxelList(taxelList);
contact.setLinkName("l_upper_forearm");
contact.setFrameName("l_upper_forearm_dh_frame");
contact.setForceTorqueEstimateConfidence(5);
}

void checkSkinContactIsPreserved(iCub::skinDynLib::skinContact & contact,
Expand All @@ -39,6 +40,7 @@ void checkSkinContactIsPreserved(iCub::skinDynLib::skinContact & contact,
}
yAssert(contact.getLinkName() == contactCheck.getLinkName());
yAssert(contact.getFrameName() == contactCheck.getFrameName());
yAssert(contact.getForceTorqueEstimateConfidence() == contactCheck.getForceTorqueEstimateConfidence());
}

void checkAndBenchmarkSkinContactSerialization()
Expand All @@ -50,9 +52,9 @@ void checkAndBenchmarkSkinContactSerialization()
// Create a buffer to which write the skin contact
yarp::os::DummyConnector buffer;

// Run several times for a reliable benchmark
// Increase this parameter for a more reliable benchmark
size_t n = 20;
double codec_time = 0.0;
size_t n = 200000;
clock_t tic = clock();
contact.write(buffer.getWriter());
for (size_t i=0; i < n; i++)
Expand Down Expand Up @@ -97,9 +99,9 @@ void checkAndBenchmarkDynContactSerialization()
// Create a buffer to which write the skin contact
yarp::os::DummyConnector buffer;

// Run several times for a reliable benchmark
// Increase this parameter for a more reliable benchmark
size_t n = 20;
double codec_time = 0.0;
size_t n = 200;
clock_t tic = clock();
contact.write(buffer.getWriter());
for (size_t i=0; i < n; i++)
Expand Down

0 comments on commit 2f370f6

Please sign in to comment.