MATLAB iCub models - Problems with kinematics

[AlessandroT99]

Hi everyone,

I am making some data analysis on position and force on iCub arms with MatLab.
To describe my problem I will analyze the left arm, but my consideration are equal for both of them, furthermore I only have data of the F/T sensor of the upper arm and the position of the hand in the root frame read from the cartesian solver.

I saved the force data from /icub/left_arm/state:o but I need them transformed into the left_hand_dh_frame and rotated as the root frame as in the /wholeBodyDynamics/involved_arm/cartesianEndEffectorWrench:o in order to compare data of the two arms.

To understand if my algorithm is working fine, I made additional test reading also the joints state and the force from the cartesianEndEffectorWrench:o, and I compared the read position from /icub/cartesianController/involved_arm/state:o with the position evaluated using the direct kinematics from the read joint and the .urdf iCub-model to generate the transformation matrix.
For all of this procedure I have used the robotic toolbox from MatLab.

In the following the snap of the code used:

iCub = importrobot("..\icub-models\iCub\robots\iCubNancy01\model.urdf");
evaluatedPosition = zeros(height(positionDataSet),3);
for i = 1:height(positionDataSet)
      % Generate the actual configuration from joints
      pose = homeConfiguration(iCub);
      newPose(16).JointPosition = armJoints(1); % Shoulder Pitch joint
      newPose(17).JointPosition = armJoints(2); % Shoulder Roll joint
      newPose(18).JointPosition = armJoints(3); % Shoulder Yaw joint
      newPose(19).JointPosition = armJoints(4); % Elbow joint
      newPose(20).JointPosition = armJoints(5); % Wrist Prosup joint
      newPose(21).JointPosition = armJoints(6); % Wrist Pitch joint
      newPose(22).JointPosition = armJoints(7); % Wrist Roll joint
      
      % Evaluate the transformation matrix from the hand frame to the root frame
      T = getTransform(iCub,pose,'l_hand_dh_frame','root_link');

      % Evaluate the position in the root frame
      tmpPos = T*[0,0,0,1]';
      evaluatedPosition(i,:) = tmpPos(1:3);
end

Where positionDataSet is the x-y-z position read from iCub and armJoints is an array containing the 7 joints in radiants involved in the movement, whilst all the other joints of iCub are all sets to home position.
Also I add that the robot model loaded is one of the few without the skin reference frames, but there is no other reason about this choice.

The problem

Finally I leave you with my issue, comparing the results with the dumped signal, I got an error both in module and in phase, and also I got it mean shifted, as you can see in the following plot.

I am wondering if the model could influence these results, I am working with iCubGenova07, which is not available in the model folder (where I can find it? Is there any equivalent?), or am I making something wrong which I cannot figure it out?

Thank you in advance!
Alessandro

[pattacini]

Hi @AlessandroT99

iCubNancy01 and iCubGenova07 have different HW versions hence different kinematics:

• iCubNancy01 is a v1.0 robot.
• iCubGenova07 is a v2.6 robot.

You can explore what this means in terms of arm kinematics by perusing the DH tables in our documentation.

Not only this, but one needs to also consider that the URDF may show slight differences with the DH tables, even when the models are aligned. This is due to the fact that DH tables (used by the Cartesian Controller) were manually extracted from the CAD, whereas the URDF models are generally exported automatically.

In the past, we commenced an activity to realign the DH tables with the CAD, although this task is now "on hold". That said, we're talking here about discrepancies smaller than those you reported in your graphs above, which are mainly caused by the different HW versions.

[pattacini]

I am working with iCubGenova07, which is not available in the model folder (where I can find it? Is there any equivalent?)

You can use this model:

• https://github.com/robotology/icub-models/tree/master/iCub/robots/iCubGazeboV2_6

[AlessandroT99]

Hi @pattacini
Thank you for you answer and sorry for my late response, but I needed some times to check some more details.
In fact after some trial with the model from you suggested I find two issues:

• The error was still similar with to the one you saw in my first question.
• The model with the skin reference system was too complex for the analyticalInverseKinematics function, which was not able to generate the inverseKinematic solver.

I thought then that this error could be generated from a different cause. I made some test on the robot and I caught a strange behavior, only after the power-on of iCub and of the cartesianSolver module, I started to dump the data from the /icub/cartesianController/involved_arm/state:o and I saw that when the robot receives for the first time the command positionMove() from <yarp/dev/IPositionControl.h> there is a single significant jump in the position signal, which is not constant, and this jump is very close to the mean shift which you can see in the error graph in my first question.
Notice that the mean error increase with the activation of the compliance.

The error is then solved a little bit, but it is still present as you can see in the following example.

Where the first image is referred to the situation without the mean correction:

And the second image is the corrected signal:

Notice that this results are evaluated again with iCubNancy01 model, and I have also to underline that the error that I am looking for seems not to be always present, look for example this other test with the same conditions:

It got the mean shift, but the highest standard deviation error is way lower than the example discussed above, I would say that could be considered almost correct in the deviation standard field.

My question now are, is it possible that the model iCubGazeboV2_6 from you suggested solve out this last problem? (because in that case I think that I should need to remove from it all the skin details in order to let the analyticalInverseKinematic() create correctly the solver).

Do you think there could be another reason? Or maybe simply this error is just normal and acceptable with iCub?

[pattacini]

Hi @AlessandroT99

The behavior you reported can be particularly tricky to debug as the Cartesian Control component involves several services. Results may be also dependent on the exact operations you've been performing for your checks.

Therefore, I would rather focus first on the kinematics per se.

In https://github.com/pattacini/robotology-community-667, I made available a simple C++ executable that can compute the forward kinematics of the iCub arm reading the joints' values from a text file.

You may consider using this small program as your new baseline to compare against.

---

Notice that the mean error increase with the activation of the compliance.

When you enable compliance, the error between the joint setpoints and their final actual values increases because of how compliance works. This obviously is reflected in the end-effector static mapping.

That said, if you use the joint feedback rather than the joint setpoints in your checks, you shouldn't experiemce any difference between compliance on and off modes.

[AlessandroT99]

Hi @pattacini,

I really appreciated your help! Unfortunately I had to wait to give you an answer because I did not had the chance yet to test the code that you sent me, but I bring news.
I dig deeper into the problem in order to be able to understand better the overall behavior, and check all the other aspect that you underlined.

To briefly summarize my whole problem:

• I have a dataset of data taken from the F/T sensor of each arm output port, but I have not the joint during this acquisition.
• I aimed to transform the force signal into the RF of the hand, and into the orientation of the original frame as the /wholeBodyDynamics/involved_arm/cartesianEndEffectorWrench:o.
• Also the F/T sensor of the right arm was not working properly, generating a shifting in all the axis which seemed to be non systematic.

After lot of analysis, using the AnalyticiKin solver of MatLab I almost reached a desidered solution with an admissible error.

Due to these problems as said before I had not yet had the chance to test your executable, but I will test it as soon as possible!

Up to know I have got one last question before considering the main problem resolved. The robot is controlled in the joint space (with the compliance active) in order to reach two position in space (called A and B), but the trajectory done from A to B is evaluated online from the cartesian solver each time the arm needs to move from one position to the other. The feedback joint mentioned by you are the one that I can read from the /icub/involved_arm/state:o is it correct?
Because if it is correct, the joints data that I used to elaborate the output of the previous answer were already taken from that port, so maybe am I missing something different?

[AlessandroT99]

Hi!

After a call with @pattacini in order to analyze deeply the problem, we manage to figure it out.
To briefly summarize it, I was mainly making a wrong comparison between the signal of the force made up from the inverse kinematics from the raw sensor signal with the signal from /wholeBodyDynamics/involved_arm/cartesianEndEffectorWrench:o which removes the force shift created by the arm weight.
Also the raw signal which I am analyzing has been collected from a not properly working F/T sensor and without the initial calibration command calib all of the WBD to help evaluate the weight drift, so the overall error could not be totally removed.

The remaining issue to solve is to use the proper .urdf model, which MatLab does not accept, and the reason will try to be evaluated in another issue here.

So up to know I thank again @pattacini and close this discussion as solved!