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; } } }