void doubleTouchThread::goToTaxel(string s="standard")
{
cntctH0 = findH0(cntctSkin);
if (s == "waypoint")
{
printMessage(0,"H0: \n%s\n",cntctH0.toString().c_str());
Matrix safetyPoint = eye(4);
safetyPoint(0,3) = 0.02; // let's move 2 cm from the cover
cntctH0 = cntctH0 * safetyPoint;
}
probl->limb.setH0(SE3inv(cntctH0));
testLimb.setH0(SE3inv(cntctH0));
Vector sol;
if (s == "waypoint")
{
probl->limb.setAng(g->joints);
slv->setInitialGuess(*g);
slv->solve(*s0);
s0->print();
sol = CTRL_RAD2DEG * s0->joints;
}
else
{
probl->limb.setAng(s0->joints);
slv->setInitialGuess(*s0);
slv->solve(*s1);
/* s1->print();*/
sol = CTRL_RAD2DEG * s1->joints;
}
iposR -> positionMove(0,sol[5]);
iposR -> positionMove(1,sol[6]);
iposR -> positionMove(2,sol[7]);
iposR -> positionMove(3,sol[8]);
iposR -> positionMove(4,sol[9]);
iposR -> positionMove(5,sol[10]);
iposR -> positionMove(6,sol[11]);
delay(3);
iposL -> positionMove(4,-sol[0]);
iposL -> positionMove(3,-sol[1]);
iposL -> positionMove(2,-sol[2]);
iposL -> positionMove(1,-sol[3]);
iposL -> positionMove(0,-sol[4]);
testLimb.setAng(s1->joints);
}
bool doubleTouchThread::testAchievement2(skinContactList *_sCL)
{
// Search for a suitable contact:
for(skinContactList::iterator it=_sCL->begin(); it!=_sCL->end(); it++)
{
if(cntctSkinPart == it -> getSkinPart())
{
iencsS->getEncoders(encsS->data());
Vector qS=encsS->subVector(0,12);
armS->setAng(qS*iCub::ctrl::CTRL_DEG2RAD);
iencsM->getEncoders(encsM->data());
Vector qM=encsM->subVector(0,12);
armM->setAng(qM*iCub::ctrl::CTRL_DEG2RAD);
Matrix cntctH0_final = findH0(*it);
/**
* WRITE ON FILE: ITERATOR, ABSOLUTE TIME, YARP TIMESTAMP
* ROBOT, COLOR, TASKTYPE, ENCODERS SLAVE, ENCODERS MASTER,
* TARGET TAXEL REFERENCE FRAME, FINAL TAXEL REFERENCE FRAME,
* INDEX HN
*/
printMessage(0,"SUCCESS!!!! Self Touch Accomplished! Iterator: %i\n",iter);
printMessage(1,"Encoders Slave: %s\n", qS.toString(3,3).c_str());
printMessage(1,"Encoders Master: %s\n", qM.toString(3,3).c_str());
printMessage(0,"Target Position: %s\n", cntctH0.subcol(0,3,3).toString(3,3).c_str());
printMessage(0,"Final Position: %s\n", cntctH0_final.subcol(0,3,3).toString(3,3).c_str());
ofstream outputfile;
outputfile.open (filename.c_str(),ios::app);
outputfile << iter << "\t" << fixed << Time::now() << "\t"
<< robot << "\t" << color << "\t" << curTaskType << "\t"
<< qS.toString(3,3) << "\t" << qM.toString(3,3) << "\t"
<< toVector(cntctH0).toString(3,3) << "\t"
<< toVector(cntctH0_final).toString(3,3);
outputfile << "\t" << toVector(slv->probl->limb.getHN()).toString(3,3);
outputfile << endl;
outputfile.close();
iter++;
return true;
}
}
return false;
}
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;
}
}
}
