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;
        }
    }
}
示例#2
0
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);
        }
    }
}