void doubleTouchThread::goToRest()
{
Vector rest = CTRL_RAD2DEG * g->joints;
iposL -> positionMove(4,-rest[0]);
iposL -> positionMove(3,-rest[1]);
iposL -> positionMove(2,-rest[2]);
iposL -> positionMove(1,-rest[3]);
iposL -> positionMove(0,-rest[4]);
delay(3);
iposR -> positionMove(0,rest[5]);
iposR -> positionMove(1,rest[6]);
iposR -> positionMove(2,rest[7]);
iposR -> positionMove(3,rest[8]);
iposR -> positionMove(4,rest[9]);
iposR -> positionMove(5,rest[10]);
iposR -> positionMove(6,rest[11]);
while(!checkMotionDone()) Time::delay(1);
}
void doubleTouchThread::run()
{
skinContactList *skinContacts = skinPort -> read(false);
sendOutput();
if (checkMotionDone())
{
switch (step)
{
case 0:
steerArmsHome();
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step++;
break;
case 1:
if(skinContacts)
{
printMessage(2,"Waiting for contact..\n");
if (detectContact(skinContacts))
{
yInfo("[doubleTouch] CONTACT!!! skinPart: %s Link: %i Position: %s NormDir: %s",
SkinPart_s[cntctSkinPart].c_str(), cntctLinkNum,cntctPosLink.toString(3,3).c_str(),
cntctNormDir.toString(3,3).c_str());
printMessage(1,"Switching to impedance position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_COMPLIANT);
imodeS -> setInteractionMode(3,VOCAB_IM_COMPLIANT);
step++;
}
}
break;
case 2:
solveIK();
yInfo("[doubleTouch] Going to taxel... Desired EE: %s\n",(sol->ee).toString(3,3).c_str());
printMessage(1,"Desired joint configuration: %s\n",(sol->joints*iCub::ctrl::CTRL_RAD2DEG).toString(3,3).c_str());
step++;
recFlag = 1;
break;
case 3:
configureHands();
if (record != 0)
{
Time::delay(2.0);
}
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelMaster();
}
else
{
goToTaxelSlave();
}
step++;
break;
case 4:
Time::delay(2.0);
step++;
break;
case 5:
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelSlave();
}
else
{
goToTaxelMaster();
}
step++;
break;
case 6:
recFlag = 0;
bool flag;
if (record == 0)
{
Time::delay(3.0);
flag=1;
if (flag == 1)
{
testAchievement();
step += 2;
}
}
else
{
testAchievement();
printMessage(0,"Waiting for the event to go back.\n");
step++;
}
break;
case 7:
if(skinContacts)
{
if (record == 1)
{
if (testAchievement2(skinContacts))
step++;
else if (robot == "icub" && exitFromDeadlock(skinContacts))
step++;
}
else if (record == 2)
{
if (exitFromDeadlock(skinContacts))
step++;
else
testAchievement2(skinContacts);
}
}
break;
case 8:
if (!dontgoback)
{
printMessage(0,"Going to rest...\n");
clearTask();
steerArmsHomeMasterSlave();
step++;
}
break;
case 9:
printMessage(1,"Switching to position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_STIFF);
imodeS -> setInteractionMode(3,VOCAB_IM_STIFF);
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step = 1;
break;
default:
yError("[doubleTouch] doubleTouchThread should never be here!!!\nStep: %d",step);
Time::delay(2.0);
break;
}
}
}
void doubleTouchThread::run() {
// printf("positionmove: %i\n",iposR -> positionMove(2, 84.406405));
skinContactList *skinContacts = cntctRdr -> read(false);
handleGaze();
if (checkMotionDone())
{
// cout << "step: " << step << endl;
if (step == 0) {
printMessage(1,"dblTchThrd: switching to impedance position mode..\n");
for (int i = 0; i < 5; ++i)
{
// ictrlR -> setImpedancePositionMode(i);
if (i!=4 && i!=1 && i!=0)
{
ictrlL -> setImpedancePositionMode(i);
}
}
printMessage(1,"Moving to rest...\n");
goToRest();
// move the thumbs close to the hand
iposR -> positionMove(9,90);
iposR -> positionMove(8,10);
iposL -> positionMove(9,90);
iposL -> positionMove(8,10);
step++;
printMessage(0,"*************\nWAITING FOR CONTACT...\n");
}
else if (step == 1)
{
if(skinContacts)
{
detectContact(skinContacts); // READ A CONTACT ON THE SKIN
if (cntctArm != "")
{
printMessage(0,"CONTACT!!! Arm: %s Position: %s NormDir: %s Pressure: %g ID: %i\n",cntctArm.c_str(),
cntctPosLink.toString().c_str(),cntctNormDir.toString().c_str(),cntctPressure,cntctSkin.getId());
step+=2;
}
}
}
else if (step == 2) {
printMessage(1,"Going to waypoint...\n");
goToTaxel("waypoint");
delay(10);
step++;
}
else if (step == 3) {
goToTaxel();
printMessage(0,"Going to taxel...Desired EE: %s\n",(s1->ee).toString().c_str());
printMessage(0,"jnts=%s\n",(s1->joints*CTRL_RAD2DEG).toString().c_str());
delay(10);
testAchievement();
printMessage(0," Final EE %s\n", testLimb.EndEffPosition().toString().c_str());
printMessage(0,"jnts=%s\n",(testLimb.getAng()*CTRL_RAD2DEG).toString().c_str());
step++;
}
else if (step == 4) {
printMessage(1,"Going to rest...\n");
goToRest();
delay(5);
printMessage(0,"*************\nWAITING FOR CONTACT...\n");
step = 1;
}
else {
printMessage(0,"ERROR!!! doubleTouchThread should never be here!!!\nStep: %d",step);
delay(1);
}
}
}
