About the orientation in cartesian space control

[Yujia-Zhang0913]

Hi,
I am running the code of cartesian space control and my code is like this:

Vector position_vector, orientation_vector;
 while (!icart->getPose(position_vector, orientation_vector))
     Time::yield();  // remember to release the quantum
 Vector position_vector1 = position_vector; 
 position_vector1[2]+=0.1;
 Vector orientation_vector1=orientation_vector;
 //Wait motion done
 icart->goToPoseSync(position_vector1, orientation_vector1);
 yInfo() << "Waiting motion done...";
 icart->waitMotionDone(0.04, 3.0); //waiting for 3 seconds max

However, I don't know why when I changed the pose to make icub move along x or y or z, the orientation of the hand is different. I think since I didn't change the orientation, it should be the same.

• move along z
  

• move along y
  

• move along x
  

And in the tutorialhttps://robotology.github.io/robotology-documentation/doc/html/icub_cartesian_interface.html, if I write code like this:

Vector oy(4);
 oy[0]=0.0; oy[1]=1.0; oy[2]=0.0; oy[3]=+M_PI/2.0;    
 Matrix Ry=yarp::math::axis2dcm(oy);        // from axis/angle to rotation matrix notation
 Matrix R0=yarp::math::axis2dcm(orientation_vector);
    
  Matrix R=R0*Ry;                            // compose the two rotations keeping the order
  Vector o=yarp::math::dcm2axis(R);          // from rotation matrix back to the axis/angle notation

Is this meaning that the hand will rotate of pi/2 around the y-axis of current orientation?
I have troubling in the orientation change. Really thanks.

[pattacini]

Hi @Yujia-Zhang0913

Try to enable the torso by means of the setDOF() service. Just go with a whole 10-DOF reaching. Find more on this at https://robotology.github.io/robotology-documentation/doc/html/icub_cartesian_interface.html#sec_cart_useinterface.

The iCub has a narrow dexterous space when using only the 7-DOF of the arm. Also, reaching in position has a higher priority wrt reaching in orientation. This has a particular impact when targets are barely reachable given the current settings. Don't know if this is the case for your experiments, but it's definitely worth giving it a try.

Is this meaning that the hand will rotate of pi/2 around the y-axis of current orientation?

Yep 👍🏻
For example, the shrug move 📽️ is implemented by this function, where the left orientation is derived from the right one with a similar rotation.

[Yujia-Zhang0913]

Yes! It works out when I enable the torso. Thanks a lot!

[pattacini]

Happy it worked 👍🏻