void doubleTouchThread::handleGaze()
{
if (step == 0 || step == 1 || step == 4) {
Vector ang(3,0.0);
igaze -> lookAtAbsAngles(ang);
}
else
{
cntctPosWRF = locateContact(cntctSkin);
/*printf("cntctPosWRF: %s\n",cntctPosWRF.toString().c_str());*/
igaze -> lookAtFixationPoint(cntctPosWRF);
}
}
bool doubleTouchThread::detectContact(skinContactList *_sCL)
{
// Reset variables:
cntctPosLink.resize(3,0.0);
cntctPosWRF.resize(3,0.0);
cntctNormDir.resize(3,0.0);
cntctPressure = -1;
cntctLinkNum = -1;
cntctSkinPart = SKIN_PART_UNKNOWN;
// Search for a suitable contact:
for(skinContactList::iterator it=_sCL->begin(); it!=_sCL->end(); it++)
{
printMessage(3,"skinContact: %s\n",it->toString(3).c_str());
if( it -> getPressure() > 25 &&
norm(it-> getNormalDir()) != 0.0)
{
for (size_t i = 0; i < skinParts.size(); i++)
{
if (skinParts[i] == it -> getSkinPart())
{
cntctSkin = *it; // Store the skinContact for eventual future use
cntctPosLink = it -> getCoP(); // Get the position of the contact;
cntctLinkNum = it -> getLinkNumber(); // Retrieve the link number of the contact;
cntctNormDir = it -> getNormalDir(); // Normal direction of the contact
cntctPressure = it -> getPressure(); // Retrieve the pressure of the contact
cntctSkinPart = it -> getSkinPart();
if (selectTask())
{
cntctPosWRF = locateContact();
cntctH0 = findH0(cntctSkin);
return true;
}
}
}
}
}
return false;
}
void doubleTouchThread::detectContact(skinContactList *_sCL)
{
// Reset variables:
cntctPosLink.resize(3,0.0);
cntctPosWRF.resize(3,0.0);
cntctNormDir.resize(3,0.0);
cntctPressure = -1;
cntctLinkNum = -1;
cntctArm = "";
// Search for a suitable contact:
for(skinContactList::iterator it=_sCL->begin(); it!=_sCL->end(); it++) {
int skinPart = it -> getSkinPart(); // Retrieve the skinPart of the skinContact
if(skinPart == fal || skinPart == ual || skinPart == far || skinPart == uar)
{
// Store the skinContact for eventual future use
cntctSkin = *it;
// Get the position of the contact:
cntctPosLink = it -> getGeoCenter();
// Retrieve the link number in order to find the origin of the reference frame
cntctLinkNum = it -> getLinkNumber();
// Retrieve the normal direction of the contact
cntctNormDir = it -> getNormalDir();
// Retrieve the pressure of the contact
cntctPressure = it -> getPressure();
if(skinPart == fal || skinPart == ual)
{
cntctArm = "left_arm";
}
else if(skinPart == far || skinPart == uar)
{
cntctArm = "right_arm";
}
cntctPosWRF = locateContact(cntctSkin);
cntctH0 = findH0(cntctSkin);
break;
}
}
}
