Switching [control modes]

[rlober]

@lornat75

I am writing a really basic module to move the left arm around usingICartesianControl, and follow the hand using IGazeControl. In my threadRelease() method I have the following:

virtual void threadRelease()
    {    
        icart->stopControl();
        igaze->stopControl();

        icart->restoreContext(startup_context_id_cart);
        igaze->restoreContext(startup_context_id_gaze);

        client.close();
        clientGaze.close();

        if (setToPosMode() )
        {
            bool done=false;
            while(!done)
            {
                pos_LeftArm->positionMove(command_LeftArm.data());
                pos_LeftArm->checkMotionDone(&done);
                Time::delay(0.1);
            }
            robotDeviceLeftArm.close();
        }

    }

Basically I just want the arm to return to its home position so I close the cartesian and gaze controllers and then try to run a position control command for the left arm. Unfortunately nothing happens and when I look at robotMotorGui, it shows that the major articulations are in velocity control. So my guess is that I am not properly switching control modes when I close the icart and igaze controllers. Here is my setToPosMode() function:

bool setToPosMode()
    {
        /*******************************************************/
        Property optLeftArm;
        optLeftArm.put("device", "remote_controlboard");
        optLeftArm.put("local", "/test/client");
        optLeftArm.put("remote", "/icubSim/left_arm");  

        if (!robotDeviceLeftArm.open(optLeftArm))
            return false;

        bool ok;
        ok = robotDeviceLeftArm.view(pos_LeftArm);
        ok = ok && robotDeviceLeftArm.view(encs_LeftArm);

        if (!ok) {
            printf("Problems acquiring interfaces\n");
            return false;
        }

        nj_LeftArm=0;
        pos_LeftArm->getAxes(&nj_LeftArm);

        encoders_LeftArm.resize(nj_LeftArm);
        tmp_LeftArm.resize(nj_LeftArm);
        command_LeftArm.resize(nj_LeftArm);

        int i;
        for (i = 0; i < nj_LeftArm; i++) {
             tmp_LeftArm[i] = 50.0;
        }
        pos_LeftArm->setRefAccelerations(tmp_LeftArm.data());

        for (i = 0; i < nj_LeftArm; i++) {
            tmp_LeftArm[i] = 10.0;
            pos_LeftArm->setRefSpeed(i, tmp_LeftArm[i]);
        }

        printf("waiting for encoders");
        while(!encs_LeftArm->getEncoders(command_LeftArm.data()))
        {
            Time::delay(0.01);
        }
        printf("\n;");
        command_LeftArm[0] = -25.0;
        command_LeftArm[1] = 20.0;
        command_LeftArm[2] = 0.0;
        command_LeftArm[3] = 50.0;
        command_LeftArm[4] = 0.0;
        command_LeftArm[5] = 0.0;
        command_LeftArm[6] = 0.0;

        return true;
    } 

I can post the whole file as it is contained within one .cpp but it is 500ish lines so I didn't want to post unless needed.

Thanks in advance!

[alecive]

Hi @rlober ! Since ~9 months ago or so, there has been a big change in the control modes management (more info here http://wiki.icub.org/wiki/Control_Modes ). The net result in this case is, you have to explicitly ask for a change in control mode before sending the position command. To this end, you have to instantiate a new interface for both the arms (namely, iControlMode2 http://wiki.icub.org/yarpdoc/classyarp_1_1dev_1_1IControlMode2.html), and set the control modes to VOCAB_CM_POSITION before asking for a positionMove.

[rlober]

Hey there @alecive, Thanks for the quick reply. Could you give me a quick example of how to use this? Normally I would dig into the docs and the class documentation but I am in a rush to get something done today so I need to take some shortcuts. From my brief look, I can't tell how I should construct these objects and I didn't see any static methods so I am a little confused on their exact usage.

Ryan

[traversaro]

Something along the lines of:

yarp::dev::IControlMode2 iMode;
robotDeviceLeftArm.view(iMode);
for (i = 0; i < nj_LeftArm; i++) {
      iMode->setControlMode(i,VOCAB_CM_POSITION);
}

[barbalberto]

if you have to set anything for a whole chain of the robot, I strongly suggest to use the "all joints" version of the function, like:

• setControlMode's'
• setRefSpeed's'

and so on... 'cause they are optimized. Also "group of joints" are available.

Other comment, change the lines:

pos_LeftArm->positionMove(command_LeftArm.data());
pos_LeftArm->checkMotionDone(&done);
Time::delay(0.1);

to

pos_LeftArm->positionMove(command_LeftArm.data());
Time::delay(0.1);
pos_LeftArm->checkMotionDone(&done);

because if you call checkMotionDone() right after a positionMove() command, you can get back a value not yet updated. If you have to wait, place it in between those 2 calls.

[rlober]

Great! Thanks everyone I got it working and I made the change to the delay call and everything works great now! Just for future reference I used:

yarp::dev::IControlMode2 *iMode;
robotDeviceLeftArm.view(iMode);
for (int i = 0; i < nj_LeftArm; i++) {
      iMode->setControlMode(i,VOCAB_CM_POSITION);
}

note: iMode should be a pointer *iMode.

Thanks again and big :+1: