Move to home position after motion with both arms

[jimmybaraglia]

Hi,

I am using the ICartesianControl to set the hands of the robot at certain position. Once the motion is done, I want the robot to come back to its initial resting position. The torso is activated for both cartesianCtrl:

enableBody(cartesianCtrlRight, dofRight);
enableBody(cartesianCtrlLeft, dofLeft);
bool actionRepertoire::enableBody(ICartesianControl* cartesianCtrl, yarp::sig::Vector* dof){
    Vector newDof(3);

    newDof[0]=1; // torso pitch: 1 => enable
    newDof[1]=0; // torso roll: 0 => disable
    newDof[2]=1; // torso yaw: 1 => enable

    return cartesianCtrl->setDOF(newDof,*dof);
}

To go back to home(resting) position, I save at start the position and orientation of each arms:

cartesianCtrlRight->getPose(home_pose_right, home_rot_right);
cartesianCtrlLeft->getPose(home_pose_left, home_rot_left);

Then, after moving to the desired position (with the right_arm and/or the left_arm), I try to go back to the home position:

cartesianCtrlRight->goToPose(home_pose_right, home_rot_right);
waitUntilTimeOut(4, cartesianCtrlRight);
cartesianCtrlLeft->goToPose(home_pose_left, home_rot_left);
waitUntilTimeOut(4, cartesianCtrlLeft);

However, this method does not work so well as the calculated inverse kinematic for each home position set the torso to different positions.

My question is: is there any simple way to tell iCub to come back to its initial home(resting) position?

Thank you in advance, Jimmy Baraglia

[pattacini]

Hi Jimmy,

Hope you're doing well 😄

Take a look at this snippet. It basically steers the robot back to a given pose but first specifies that the torso angles are required to be always equal to 0.0 [deg] (or any other suitable home configuration).

This way, you'll bypass the IK service and at the same time ask the Cartesian controller to account for the torso as well. Once done, the normal behavior gets restored by means of context switching.

[jimmybaraglia]

Hi Ugo,

I am doing good. How about you?

It worked like a charm, thank you for the link to the code. I updated my code as follows:

    void actionRepertoire::go_home(ICartesianControl *ctrl, Vector home_pos, Vector home_rot){    
            //Create & store the contect
            int context;
            ctrl->storeContext(&context);

            //Set new DoF for contect
            Vector dof;
            ctrl->getDOF(dof); dof=1.0;
            ctrl->setDOF(dof,dof);

            //Set limits 
            ctrl->setLimits(0,0.0,0.0);
            ctrl->setLimits(1,0.0,0.0);
            ctrl->setLimits(2,0.0,0.0);

            //Motion time = 3 seconds
            ctrl->setTrajTime(3);

            //Go to pose
            ctrl->goToPose(home_pos, home_rot);
            ctrl->waitMotionDone(0.1,4);

            //Restore the previous context
            ctrl->restoreContext(context);
            ctrl->deleteContext(context);
    }

Cheers, Jimmy Baraglia

[pattacini]

I'm the same old 😉 Glad to know it helped.