MATLAB's analyticalInverseKinematics - Problems with iCubGazeboV2_6 URDF

Goodmorning everyone!

I am currently using MatLab for some data analysis involving inverse kinematics of iCub with the Robotic Toolbox. I am working with iCubGenova07, and has been suggested to me to use iCubGazeboV2_6 for hardware compatibility. The function which I used to base my analysis, is the analyticalInverseKinematics() function, and the code giving me problems is:

iCub = importrobot("..\icub-models\iCub\robots\iCubLisboa01\model.urdf");
aik = analyticalInverseKinematics(iCub);
opts = showdetails(aik);

The problem is that using a simplest model such as iCubLisboa01, the creation of the inverse kinematic solver works fine as you can see here:

--------------------
Robot: (53 bodies)

Index                     Base Name                  EE Body Name     Type                    Actions
-----                     ---------                  ------------     ----                    -------
    1                     root_link                  l_shoulder_3   RRRSSS   Use this kinematic group
    2                     base_link                  l_shoulder_3   RRRSSS   Use this kinematic group
    3                     root_link                   l_upper_arm   RRRSSS   Use this kinematic group
    4                     base_link                   l_upper_arm   RRRSSS   Use this kinematic group
    5                     root_link          l_upper_arm_dh_frame   RRRSSS   Use this kinematic group
    6                     base_link          l_upper_arm_dh_frame   RRRSSS   Use this kinematic group
    7                  l_shoulder_1                        l_hand   RRRSSS   Use this kinematic group
    8                  r_shoulder_1                        r_hand   RRRSSS   Use this kinematic group
    9                  l_shoulder_1                     l_gripper   RRRSSS   Use this kinematic group
   10                  l_shoulder_1               l_hand_dh_frame   RRRSSS   Use this kinematic group
   11                  r_shoulder_1                     r_gripper   RRRSSS   Use this kinematic group
   12                  r_shoulder_1               r_hand_dh_frame   RRRSSS   Use this kinematic group

Whilst using the Gazebo model mentioned above or all the other complex one (which contains also the skin reference systems) MatLab gives me back the following error.

--------------------
Robot: (209 bodies)

Index                        Base Name                     EE Body Name     Type                    Actions
-----                        ---------                     ------------     ----                    -------
No valid kinematic groups were found.

I think it could be due the high number of the reference systems in the new models, could it be? There is a way to solve this problem without changing function or without using the bindings of iDynTree library?

Thank you in advance!

Cool, I had no idea the analyticalInverseKinematics function existed. If the algorithm works fine for the same model, but without skin and sensors "fake" links, this seems a bug on MATLAB's side. Which version of MATLAB are you using? Did you tried to report this problem to MathWorks?

Furthermore, to which KinematicGroup are you interested? Perhaps you can try to use the analyticalInverseKinematics(robotRBT,'KinematicGroup',kinGroup) signature of the function, in which you specify explicitly the KinematicGroup (i.e. the pair BaseName and EndEffectorBodyName) that you want to use?

I am using Matlab 2021b, and no I did not try to report it to MathWorks. By the way another detail that could be interesting is that this function accept only kinematic chains which are no more long than 6 joints, so maybe this also is a problem for the complex models, but again should be a problem MATLAB side. Should I try ask them?

Furthermore, to which KinematicGroup are you interested?

I am interested into the one from shoulder_1 to hand_dh_frame, which for the simple models are KinematicGroups 10 and 12, whilst for the complex they do not exist for the function.

I select the desidered KinematicGroup in the following way:

% [example for the left arm]
iCub = importrobot("..\icub-models\iCub\robots\iCubLisboa01\model.urdf");
aik = analyticalInverseKinematics(iCub);
opts = showdetails(aik);
aik.KinematicGroup = opts(10).KinematicGroup;
generateIKFunction(aik,'iCubIK_LArm');

I do not select it directly in the definition of the analitycalInverseKinematic in order to use the same aik object to select the arm group needed.

Can't you try something like:

iCub = importrobot("..\icub-models\iCub\robots\iCubGazeboV2_6\model.urdf");
kinGroup = 
aik = analyticalInverseKinematics(iCub, 'KinematicGroup', struct("BaseName","shoulder_1","EndEffectoryBodyName","hand_dh_frame"));

or something similar?

I am using Matlab 2021b, and no I did not try to report it to MathWorks. By the way another detail that could be interesting is that this function accept only kinematic chains which are no more long than 6 joints, so maybe this also is a problem for the complex models, but again should be a problem MATLAB side. Should I try ask them?

I would first try to check if the problem is present also in the latest MATLAB release. Do you have any specific reason for using an old MATLAB release?

Can't you try something like:

iCub = importrobot("..\icub-models\iCub\robots\iCubGazeboV2_6\model.urdf");
kinGroup = 
aik = analyticalInverseKinematics(iCub, 'KinematicGroup', struct("BaseName","shoulder_1","EndEffectoryBodyName","hand_dh_frame"));

This does not work in any way, I also checked the guide from MATLAB for the robotic toolbox, and it also says that should work, the strange fact is that it does not work for any model.

I update MATLAB, check again, and let you know is something will fix!

Also the same behavior is reported with MATLAB R2023b. So no kinematic group is found, and when I try to force it in creation of aik through

aik = analyticalInverseKinematics(iCub, 'KinematicGroup', struct("BaseName","l_shoulder_1","EndEffectoryBodyName","l_hand_dh_frame"));

I receive as error:

Error using analyticalInverseKinematics>@(s)validatestring(s,{'BaseName','EndEffectorBodyName'})
Expected input to match one of these values:

'BaseName', 'EndEffectorBodyName'

The input, 'EndEffectoryBodyName', did not match any of the valid values.

Error in analyticalInverseKinematics/validateAndSetKinematicGroup (line 496)
            cellfun(@(s)validatestring(s, {'BaseName', 'EndEffectorBodyName'}), fields, 'UniformOutput', false);

Error in analyticalInverseKinematics (line 197)
            obj.validateAndSetKinematicGroup(parameterValue(parser, 'KinematicGroup'));

Looking the documentation I think it is the problem of using this call with an .urdf model, but by the way the working code I sent initially for the simplest models, also in this version is not working for the complex one.

I think that the error message is just due a typo in my message, can you try EndEffectorBodyName instead of EndEffectoryBodyName ?

Yes the last error was fault of the typo, I did not recognize it. But unfortunately it does not solve the main issue, it generates the object aik with the KinematicGroup desidered, but when I try to generate the solver function:

generateIKFunction(aik,'iCubIK_try');

I get the error:

Error using robotics.manip.internal.error
Inverse kinematics cannot be generated for this kinematic group. Change the kinematic group or use the showdetails function to see valid options.

Error in analyticalInverseKinematics/generateIKFunction (line 338)
                robotics.manip.internal.error('analyticalinversekinematics:IKNotSupported');

In fact if I try to aik.showdetails() it returns me that 0 valid KinematicGroups are available.

Thanks for the check. At this point, I would try to write a script that quickly process the ..\icub-models\iCub\robots\iCubGazeboV2_6\model.urdf to remove the unnecessary frames, and see if that is indeed the problem, or if there is some other difference w.r.t. to iCubLisboa01 that is actually triggering this behaviour.

Using idyntree-model-info I can get the list of the frames:

(test227) traversaro@IITICUBLAP257:~/miniforge3/envs/test227/share/iCub/robots/iCubGazeboV2_7$ idyntree-model-info -m model.urdf  -p
Model:
  Links:
    [0] root_link
    [1] torso_1
    [2] r_hip_1
    [3] l_hip_1
    [4] l_hip_2
    [5] l_hip_3
    [6] l_upper_leg
    [7] l_lower_leg
    [8] l_ankle_1
    [9] l_ankle_2
    [10] l_foot
    [11] r_hip_2
    [12] r_hip_3
    [13] r_upper_leg
    [14] r_lower_leg
    [15] r_ankle_1
    [16] r_ankle_2
    [17] r_foot
    [18] torso_2
    [19] chest
    [20] r_shoulder_1
    [21] neck_1
    [22] l_shoulder_1
    [23] l_shoulder_2
    [24] l_shoulder_3
    [25] l_upper_arm
    [26] l_elbow_1
    [27] l_forearm
    [28] l_wrist_1
    [29] l_hand
    [30] neck_2
    [31] head
    [32] r_shoulder_2
    [33] r_shoulder_3
    [34] r_upper_arm
    [35] r_elbow_1
    [36] r_forearm
    [37] r_wrist_1
    [38] r_hand
  Frames:
    [39] root_link_ems_gyro_eb5 --> root_link
    [40] base_link --> root_link
    [41] root_link_imu_frame --> root_link
    [42] l_upper_leg_ft_gyro_3b12 --> l_hip_2
    [43] l_leg_ft --> l_hip_3
    [44] l_upper_leg_ems_gyro_eb6 --> l_upper_leg
    [45] l_upper_leg_ems_gyro_eb10 --> l_upper_leg
    [46] l_upper_leg_back_contact --> l_upper_leg
    [47] l_upper_leg_dh_frame --> l_upper_leg
    [48] l_lower_leg_ems_gyro_eb7 --> l_lower_leg
    [49] l_foot_ft_gyro_3b13 --> l_ankle_2
    [50] l_foot_ft --> l_foot
    [51] l_sole --> l_foot
    [52] l_sole_skin_0 --> l_foot
    [53] l_sole_skin_1 --> l_foot
    [54] l_sole_skin_2 --> l_foot
    [55] l_sole_skin_3 --> l_foot
    [56] l_sole_skin_4 --> l_foot
    [57] l_sole_skin_8 --> l_foot
    [58] l_sole_skin_9 --> l_foot
    [59] l_sole_skin_10 --> l_foot
    [60] l_sole_skin_11 --> l_foot
    [61] l_sole_skin_12 --> l_foot
    [62] l_sole_skin_13 --> l_foot
    [63] l_sole_skin_14 --> l_foot
    [64] l_sole_skin_15 --> l_foot
    [65] l_sole_skin_17 --> l_foot
    [66] l_sole_skin_18 --> l_foot
    [67] l_sole_skin_19 --> l_foot
    [68] l_sole_skin_20 --> l_foot
    [69] l_sole_skin_21 --> l_foot
    [70] l_sole_skin_22 --> l_foot
    [71] l_sole_skin_24 --> l_foot
    [72] l_sole_skin_25 --> l_foot
    [73] l_sole_skin_26 --> l_foot
    [74] l_sole_skin_27 --> l_foot
    [75] l_sole_skin_28 --> l_foot
    [76] l_sole_skin_29 --> l_foot
    [77] l_foot_dh_frame --> l_foot
    [78] r_upper_leg_ft_gyro_3b11 --> r_hip_2
    [79] r_leg_ft --> r_hip_3
    [80] r_upper_leg_ems_gyro_eb8 --> r_upper_leg
    [81] r_upper_leg_ems_gyro_eb11 --> r_upper_leg
    [82] r_upper_leg_back_contact --> r_upper_leg
    [83] r_upper_leg_dh_frame --> r_upper_leg
    [84] r_lower_leg_ems_gyro_eb9 --> r_lower_leg
    [85] r_lower_leg_skin_0 --> r_lower_leg
    [86] r_lower_leg_skin_3 --> r_lower_leg
    [87] r_lower_leg_skin_4 --> r_lower_leg
    [88] r_lower_leg_skin_5 --> r_lower_leg
    [89] r_lower_leg_skin_6 --> r_lower_leg
    [90] r_lower_leg_skin_9 --> r_lower_leg
    [91] r_lower_leg_skin_10 --> r_lower_leg
    [92] r_lower_leg_skin_11 --> r_lower_leg
    [93] r_lower_leg_skin_14 --> r_lower_leg
    [94] r_lower_leg_skin_15 --> r_lower_leg
    [95] r_lower_leg_skin_16 --> r_lower_leg
    [96] r_lower_leg_skin_17 --> r_lower_leg
    [97] r_lower_leg_skin_19 --> r_lower_leg
    [98] r_lower_leg_skin_20 --> r_lower_leg
    [99] r_lower_leg_skin_21 --> r_lower_leg
    [100] r_lower_leg_skin_28 --> r_lower_leg
    [101] r_lower_leg_skin_29 --> r_lower_leg
    [102] r_lower_leg_skin_31 --> r_lower_leg
    [103] r_lower_leg_skin_32 --> r_lower_leg
    [104] r_lower_leg_skin_35 --> r_lower_leg
    [105] r_lower_leg_skin_36 --> r_lower_leg
    [106] r_lower_leg_skin_37 --> r_lower_leg
    [107] r_lower_leg_skin_38 --> r_lower_leg
    [108] r_lower_leg_skin_41 --> r_lower_leg
    [109] r_lower_leg_skin_42 --> r_lower_leg
    [110] r_lower_leg_skin_43 --> r_lower_leg
    [111] r_lower_leg_skin_46 --> r_lower_leg
    [112] r_lower_leg_skin_47 --> r_lower_leg
    [113] r_lower_leg_skin_49 --> r_lower_leg
    [114] r_lower_leg_skin_50 --> r_lower_leg
    [115] r_lower_leg_skin_51 --> r_lower_leg
    [116] r_lower_leg_skin_52 --> r_lower_leg
    [117] r_lower_leg_skin_53 --> r_lower_leg
    [118] r_lower_leg_skin_54 --> r_lower_leg
    [119] r_lower_leg_skin_55 --> r_lower_leg
    [120] r_lower_leg_skin_56 --> r_lower_leg
    [121] r_lower_leg_skin_60 --> r_lower_leg
    [122] r_lower_leg_skin_61 --> r_lower_leg
    [123] r_foot_ft_gyro_3b14 --> r_ankle_2
    [124] r_foot_ft --> r_foot
    [125] r_sole --> r_foot
    [126] r_sole_skin_0 --> r_foot
    [127] r_sole_skin_1 --> r_foot
    [128] r_sole_skin_2 --> r_foot
    [129] r_sole_skin_3 --> r_foot
    [130] r_sole_skin_4 --> r_foot
    [131] r_sole_skin_8 --> r_foot
    [132] r_sole_skin_9 --> r_foot
    [133] r_sole_skin_10 --> r_foot
    [134] r_sole_skin_11 --> r_foot
    [135] r_sole_skin_12 --> r_foot
    [136] r_sole_skin_13 --> r_foot
    [137] r_sole_skin_14 --> r_foot
    [138] r_sole_skin_15 --> r_foot
    [139] r_sole_skin_17 --> r_foot
    [140] r_sole_skin_18 --> r_foot
    [141] r_sole_skin_19 --> r_foot
    [142] r_sole_skin_20 --> r_foot
    [143] r_sole_skin_21 --> r_foot
    [144] r_sole_skin_22 --> r_foot
    [145] r_sole_skin_24 --> r_foot
    [146] r_sole_skin_25 --> r_foot
    [147] r_sole_skin_26 --> r_foot
    [148] r_sole_skin_27 --> r_foot
    [149] r_sole_skin_28 --> r_foot
    [150] r_sole_skin_29 --> r_foot
    [151] r_foot_dh_frame --> r_foot
    [152] chest_ems_gyro_eb1 --> chest
    [153] chest_ems_gyro_eb2 --> chest
    [154] chest_ems_gyro_eb3 --> chest
    [155] chest_ems_gyro_eb4 --> chest
    [156] l_arm_ft --> l_shoulder_3
    [157] l_forearm_skin_0 --> l_forearm
    [158] l_forearm_skin_1 --> l_forearm
    [159] l_forearm_skin_2 --> l_forearm
    [160] l_forearm_skin_3 --> l_forearm
    [161] l_forearm_skin_4 --> l_forearm
    [162] l_forearm_skin_5 --> l_forearm
    [163] l_forearm_skin_6 --> l_forearm
    [164] l_forearm_skin_7 --> l_forearm
    [165] l_forearm_skin_8 --> l_forearm
    [166] l_forearm_skin_9 --> l_forearm
    [167] l_forearm_skin_10 --> l_forearm
    [168] l_forearm_skin_11 --> l_forearm
    [169] l_forearm_skin_12 --> l_forearm
    [170] l_forearm_skin_13 --> l_forearm
    [171] l_forearm_skin_14 --> l_forearm
    [172] l_forearm_skin_15 --> l_forearm
    [173] l_forearm_skin_16 --> l_forearm
    [174] l_forearm_skin_17 --> l_forearm
    [175] l_forearm_skin_19 --> l_forearm
    [176] l_forearm_skin_22 --> l_forearm
    [177] l_forearm_skin_24 --> l_forearm
    [178] l_forearm_skin_25 --> l_forearm
    [179] l_forearm_skin_28 --> l_forearm
    [180] l_forearm_skin_29 --> l_forearm
    [181] l_forearm_dh_frame --> l_forearm
    [182] l_hand_dh_frame --> l_hand
    [183] head_imu_0 --> head
    [184] r_arm_ft --> r_shoulder_3
    [185] r_forearm_skin_0 --> r_forearm
    [186] r_forearm_skin_1 --> r_forearm
    [187] r_forearm_skin_2 --> r_forearm
    [188] r_forearm_skin_3 --> r_forearm
    [189] r_forearm_skin_4 --> r_forearm
    [190] r_forearm_skin_5 --> r_forearm
    [191] r_forearm_skin_6 --> r_forearm
    [192] r_forearm_skin_7 --> r_forearm
    [193] r_forearm_skin_8 --> r_forearm
    [194] r_forearm_skin_9 --> r_forearm
    [195] r_forearm_skin_10 --> r_forearm
    [196] r_forearm_skin_11 --> r_forearm
    [197] r_forearm_skin_12 --> r_forearm
    [198] r_forearm_skin_13 --> r_forearm
    [199] r_forearm_skin_14 --> r_forearm
    [200] r_forearm_skin_15 --> r_forearm
    [201] r_forearm_skin_16 --> r_forearm
    [202] r_forearm_skin_17 --> r_forearm
    [203] r_forearm_skin_19 --> r_forearm
    [204] r_forearm_skin_22 --> r_forearm
    [205] r_forearm_skin_24 --> r_forearm
    [206] r_forearm_skin_25 --> r_forearm
    [207] r_forearm_skin_28 --> r_forearm
    [208] r_forearm_skin_29 --> r_forearm
    [209] r_forearm_dh_frame --> r_forearm
    [210] r_hand_dh_frame --> r_hand
    [211] root_link_ems_acc_eb5 --> root_link
    [212] chest_ems_acc_eb1 --> chest
    [213] chest_ems_acc_eb2 --> chest
    [214] chest_ems_acc_eb3 --> chest
    [215] chest_ems_acc_eb4 --> chest
    [216] chest_mtb_acc_0b7 --> chest
    [217] chest_mtb_acc_0b8 --> chest
    [218] chest_mtb_acc_0b9 --> chest
    [219] chest_mtb_acc_0b10 --> chest
    [220] r_upper_arm_mtb_acc_2b10 --> r_upper_arm
    [221] r_upper_arm_mtb_acc_2b11 --> r_upper_arm
    [222] r_upper_arm_mtb_acc_2b12 --> r_upper_arm
    [223] r_upper_arm_mtb_acc_2b13 --> r_upper_arm
    [224] r_forearm_mtb_acc_2b7 --> r_forearm
    [225] r_forearm_mtb_acc_2b8 --> r_forearm
    [226] r_forearm_mtb_acc_2b9 --> r_forearm
    [227] l_upper_arm_mtb_acc_1b10 --> l_upper_arm
    [228] l_upper_arm_mtb_acc_1b11 --> l_upper_arm
    [229] l_upper_arm_mtb_acc_1b12 --> l_upper_arm
    [230] l_upper_arm_mtb_acc_1b13 --> l_upper_arm
    [231] l_forearm_mtb_acc_1b7 --> l_forearm
    [232] l_forearm_mtb_acc_1b8 --> l_forearm
    [233] l_forearm_mtb_acc_1b9 --> l_forearm
    [234] r_upper_leg_ems_acc_eb8 --> r_upper_leg
    [235] r_upper_leg_ems_acc_eb11 --> r_upper_leg
    [236] r_upper_leg_mtb_acc_11b1 --> r_upper_leg
    [237] r_upper_leg_mtb_acc_11b2 --> r_upper_leg
    [238] r_upper_leg_mtb_acc_11b3 --> r_upper_leg
    [239] r_upper_leg_mtb_acc_11b4 --> r_upper_leg
    [240] r_upper_leg_mtb_acc_11b5 --> r_upper_leg
    [241] r_upper_leg_mtb_acc_11b6 --> r_upper_leg
    [242] r_upper_leg_mtb_acc_11b7 --> r_upper_leg
    [243] r_lower_leg_ems_acc_eb9 --> r_lower_leg
    [244] r_lower_leg_mtb_acc_11b8 --> r_lower_leg
    [245] r_lower_leg_mtb_acc_11b9 --> r_lower_leg
    [246] r_lower_leg_mtb_acc_11b10 --> r_lower_leg
    [247] r_lower_leg_mtb_acc_11b11 --> r_lower_leg
    [248] r_foot_mtb_acc_11b12 --> r_foot
    [249] r_foot_mtb_acc_11b13 --> r_foot
    [250] l_upper_leg_ems_acc_eb6 --> l_upper_leg
    [251] l_upper_leg_ems_acc_eb10 --> l_upper_leg
    [252] l_upper_leg_mtb_acc_10b1 --> l_upper_leg
    [253] l_upper_leg_mtb_acc_10b2 --> l_upper_leg
    [254] l_upper_leg_mtb_acc_10b3 --> l_upper_leg
    [255] l_upper_leg_mtb_acc_10b4 --> l_upper_leg
    [256] l_upper_leg_mtb_acc_10b5 --> l_upper_leg
    [257] l_upper_leg_mtb_acc_10b6 --> l_upper_leg
    [258] l_upper_leg_mtb_acc_10b7 --> l_upper_leg
    [259] l_lower_leg_ems_acc_eb7 --> l_lower_leg
    [260] l_lower_leg_mtb_acc_10b8 --> l_lower_leg
    [261] l_lower_leg_mtb_acc_10b9 --> l_lower_leg
    [262] l_lower_leg_mtb_acc_10b10 --> l_lower_leg
    [263] l_lower_leg_mtb_acc_10b11 --> l_lower_leg
    [264] l_foot_mtb_acc_10b12 --> l_foot
    [265] l_foot_mtb_acc_10b13 --> l_foot
    [266] r_upper_leg_ft_acc_3b11 --> r_hip_2
    [267] r_foot_ft_acc_3b14 --> r_ankle_2
    [268] l_upper_leg_ft_acc_3b12 --> l_hip_2
    [269] l_foot_ft_acc_3b13 --> l_ankle_2
  Joints:
    [0] torso_pitch (dofs: 1) : root_link<-->torso_1
    [1] r_hip_pitch (dofs: 1) : root_link<-->r_hip_1
    [2] l_hip_pitch (dofs: 1) : root_link<-->l_hip_1
    [3] l_hip_roll (dofs: 1) : l_hip_1<-->l_hip_2
    [4] l_hip_yaw (dofs: 1) : l_hip_3<-->l_upper_leg
    [5] l_knee (dofs: 1) : l_upper_leg<-->l_lower_leg
    [6] l_ankle_pitch (dofs: 1) : l_lower_leg<-->l_ankle_1
    [7] l_ankle_roll (dofs: 1) : l_ankle_1<-->l_ankle_2
    [8] r_hip_roll (dofs: 1) : r_hip_1<-->r_hip_2
    [9] r_hip_yaw (dofs: 1) : r_hip_3<-->r_upper_leg
    [10] r_knee (dofs: 1) : r_upper_leg<-->r_lower_leg
    [11] r_ankle_pitch (dofs: 1) : r_lower_leg<-->r_ankle_1
    [12] r_ankle_roll (dofs: 1) : r_ankle_1<-->r_ankle_2
    [13] torso_roll (dofs: 1) : torso_1<-->torso_2
    [14] torso_yaw (dofs: 1) : torso_2<-->chest
    [15] r_shoulder_pitch (dofs: 1) : chest<-->r_shoulder_1
    [16] neck_pitch (dofs: 1) : chest<-->neck_1
    [17] l_shoulder_pitch (dofs: 1) : chest<-->l_shoulder_1
    [18] l_shoulder_roll (dofs: 1) : l_shoulder_1<-->l_shoulder_2
    [19] l_shoulder_yaw (dofs: 1) : l_shoulder_2<-->l_shoulder_3
    [20] l_elbow (dofs: 1) : l_upper_arm<-->l_elbow_1
    [21] l_wrist_prosup (dofs: 1) : l_elbow_1<-->l_forearm
    [22] l_wrist_pitch (dofs: 1) : l_forearm<-->l_wrist_1
    [23] l_wrist_yaw (dofs: 1) : l_wrist_1<-->l_hand
    [24] neck_roll (dofs: 1) : neck_1<-->neck_2
    [25] neck_yaw (dofs: 1) : neck_2<-->head
    [26] r_shoulder_roll (dofs: 1) : r_shoulder_1<-->r_shoulder_2
    [27] r_shoulder_yaw (dofs: 1) : r_shoulder_2<-->r_shoulder_3
    [28] r_elbow (dofs: 1) : r_upper_arm<-->r_elbow_1
    [29] r_wrist_prosup (dofs: 1) : r_elbow_1<-->r_forearm
    [30] r_wrist_pitch (dofs: 1) : r_forearm<-->r_wrist_1
    [31] r_wrist_yaw (dofs: 1) : r_wrist_1<-->r_hand
    [32] l_leg_ft_sensor (dofs: 0) : l_hip_2<-->l_hip_3
    [33] l_foot_ft_sensor (dofs: 0) : l_ankle_2<-->l_foot
    [34] r_leg_ft_sensor (dofs: 0) : r_hip_2<-->r_hip_3
    [35] r_foot_ft_sensor (dofs: 0) : r_ankle_2<-->r_foot
    [36] l_arm_ft_sensor (dofs: 0) : l_shoulder_3<-->l_upper_arm
    [37] r_arm_ft_sensor (dofs: 0) : r_shoulder_3<-->r_upper_arm

At a first glance, removing all the frames that contain skin or mtb in their name should drastically reduced the number of frames. If your supposition is right, at this point the IK should start to work, otherwise the problem is something else. Anyhow, this URDF is a perfectly valid URDF according to the URDF spec, so if the problem is present in the latest MATLAB, I think it make sense to report the issue to MathWorks.

At this point, I would try to write a script that quickly process the ..\icub-models\iCub\robots\iCubGazeboV2_6\model.urdf to remove the unnecessary frames, and see if that is indeed the problem, or if there is some other difference w.r.t. to iCubLisboa01 that is actually triggering this behaviour.

A possible starting point (I do not know if it runs, but it is a step in the right direction) could be https://chat.openai.com/share/662bf250-de48-4d16-a8a0-5ffef47fde56 .

Thank you @traversaro for all your suggestion, I come back to you in order to check what I've done.

It seems that i get a valid result in the modification of the urdf file, using the following code:

from lxml import etree
import os

def remove_links_with_skin(urdf_path, destination):
    try:
        # Print the file path for debugging
        print("File path:", urdf_path)

        # Parse the URDF file
        tree = etree.parse(urdf_path)
        root = tree.getroot()

        # Find and remove link tags with name containing "skin"
        for link in root.findall(".//link"):
            link_name = link.get('name')

            if 'skin' in link_name:
                # Remove the link tag
                root.remove(link)

                # Remove the corresponding joint tag if it exists
                joint_name = link_name + "_fixed_joint"
                joint = root.find(".//joint[@name='{}']".format(joint_name))
                if joint is not None:
                    root.remove(joint)

        # Save the modified URDF
        tree.write(destination, pretty_print=True)

        print("Modification completed successfully.")

    except Exception as e:
        print("An error occurred:", e)

# Example usage
urdf_file_path = 'model.urdf'
final_path = 'iCubGazeboV2_6_new'
# Uncomment the line below to create the directory
# os.system("mkdir " + final_path)
destination = final_path + "/model.urdf"
remove_links_with_skin(urdf_file_path, destination)

copying the initial model in the same folder of the .py file.

The problem is that I do not get any better results in MATLAB it ends with the same error letting me know that this time the number of bodies is way lower than before, as it should be.

A possible starting point (I do not know if it runs, but it is a step in the right direction) could be https://chat.openai.com/share/662bf250-de48-4d16-a8a0-5ffef47fde56 .

Also I notice that you asked for a .xml modifier, while I thought it should be a .urdf am I wrong?

Also I notice that you asked for a .xml modifier, while I thought it should be a .urdf am I wrong?

Sure, a URDF file is a XML file, I just tought it was safer to ask for modifying an XML, but also asking for modifying a URDF file may work.

The problem is that I do not get any better results in MATLAB it ends with the same error letting me know that this time the number of bodies is way lower than before, as it should be.

So I guess this is not the problem. As that URDF file as far as I know is a valid URDF file that is working in Gazebo, KDL, iDynTree, MuJoCo and Bullet, I guess probably the best thing to do at this point is to try to contact MathWorks. We have a few direct contact in MathWorks itself, I can put in contact with them (ping me on IIT's Teams), but in the meanwhile I would try to open a regular support request.

Sure, a URDF file is a XML file, I just tought it was safer to ask for modifying an XML, but also asking for modifying a URDF file may work.

I undestand, I did not know that!

I guess probably the best thing to do at this point is to try to contact MathWorks. We have a few direct contact in MathWorks itself, I can put in contact with them (ping me on IIT's Teams), but in the meanwhile I would try to open a regular support request.

Okay then I proceed to open the support request on MathWorks, in case of resolution I will post here the update!

Good morning everyone,

Finally, I come back to you with some updates! As said I talked with the MATLAB tech-support and we found the problems causing the errors I got.

I attach in the following their comment with the explanation:

Upon a thorough review of the situation, we have identified two distinct factors contributing to the difficulties with the model. The first and main issue is that this robot's joint axes do not intersect at a common point; they almost intersect, but they do not actually intersect. The second issue is that this robot has some other attributes that make it incompatible with our solver as it currently exists.

First aspect

With respect to the first issue, and this is the most critical issue, the axes in the provided wrist do not intersect at one point. This issue is broken down below using the subchain, i.e. the tree created from the kinematic group, and it involves some vector computations between adjacent frames. To clarify our findings, we use the "wrtN" notation to indicate that a quantity is computed with respect to the reference frame for body "N".

%% Load the robot, analytical IK, and set the correct kinematic group 
robot = importrobot('model.urdf'); 
aik = analyticalInverseKinematics(robot); 
aik.KinematicGroup.EndEffectorBodyName = 'l_hand_dh_frame'; 
aik.KinematicGroup.BaseName = 'l_shoulder_1';

%% Investigate why this isn't showing as a valid subchain 
aik.KinematicGroupType % This shows as RRRRRR, but for a wrist we would expect it to be RRRSSS 
% Let's take a look at the subchain and take an in-depth look at what the 
% analysis sees 
robot2 = struct(aik).SubChain; %Access an internal property 
show(robot2) % You can see here that the last three joints don't intersect

Indeed exploring their evaluation the results are:

simple_model_left_arm

for the simple model such as Lisboa01, while for complex ones with skin such as the Gazebo version discussed we obtain:

complex_model_left_arm

Also they provide a code to check the offsets mentioned and visible in the previous figure:

% You can see that the joints from bodies in indices 5,6,and 7 are expected 
% to meet at a point. Let's look at where that point should be. First, body 
% 7 has a joint axis that is along it's Y frame, and body 6's joint axis is 
% angled in the XZ-plane. We see that body 7 frame is also offset in the XZ 
% plane; it's clear that for the intersection to work, they have to be 
% offset the same amount 
Trans7wrt6 = tform2trvec(getTransform(robot2,robot2.homeConfiguration,robot2.BodyNames{7},robot2.BodyNames{6})); 
body6jointaxis = robot2.Bodies{6}.Joint.JointAxis; 
% We confirm that the joint axis intersects at a point by verifying that 
% it's parallel to the vector relating the two frames in the XZ plane 
isParallel = (all(abs(cross([Trans7wrt6(1) 0 Trans7wrt6(3)], body6jointaxis))<=sqrt(eps))); 
% So therefore these last two joints intersect at the point 
intersection67wrt6 = [Trans7wrt6(1) 0 Trans7wrt6(3)]; 
% Now the question is whether the body 6 joint axis goes through this same 
% point. We can solve in the same way: first, we look at the pose of frame 
% 5 (the origin of the joint 5 axis) with respect to frame 6 
Trans5wrt6 = tform2trvec(getTransform(robot2,robot2.homeConfiguration,robot2.BodyNames{5},robot2.BodyNames{6})); 
% Next we compute a vector between the known intersection point and the 
% joint origin. 
expJointAxiswrt6 = intersection67wrt6 - Trans5wrt6; 
% For comparison, we also need the joint axis for frame 5 in the frame 6 
% reference, though this is largely a formality since there is no 
% significant rotation between the two frames 
Rot5wrt6 = tform2rotm(getTransform(robot2,robot2.homeConfiguration,robot2.BodyNames{5},robot2.BodyNames{6})); 
body5jointaxiswrt6 = (Rot5wrt6*robot2.Bodies{5}.Joint.JointAxis(:))'; 
% Let's check if the actual and expected joint axes are parallel, again 
% using the cross product. If they are, the cross product will be zero 
cross(body5jointaxiswrt6/norm(body5jointaxiswrt6),expJointAxiswrt6/norm(expJointAxiswrt6)) 
% As can be easily seen, these are not parallel 
normExpJointAxis = expJointAxiswrt6/norm(expJointAxiswrt6);

In fact, executing it the results are:

[-0.0000 -0.0028 0.0001] for the complex model, which for the funct are considered significative
1.0e-15 * [-0.0000 0.0143 0.2318] for the simplest model, considered all 0.

Second aspect

Removing these offsets from the model shall be enough to solve the problem, but here there is the second relevant limitation on the solver: currently it only supports wrists where the last three axes fall along the primary frame axes (X,Y, Z). For this reason, so tech-support has then informed the development team so that they can consider addressing your request in future releases.

Conclusion

In the end, up to now it is not possible to use the complex models such as the Gazebo.urdf one with the analyticalInverseKinematics() function for two reason:

The little offset presents in the model - depending on the .urdf;
The wrists axes do not fall on the primary frame axis - depending on the funct. properties.

Proposed solution

In my case, due to time limitation, I will just consider the results obtained with the not properly correct hardware model and decide if use some of them or just discard them, whereas if someone else needs an inverse kinematic solver able to provide all the existing solutions in the current configuration shall write its own or use the more general multiconstraint inverse kinematics generalizedInverseKinematics function and change the initial pose trying to obtain different solutions for the same target position.

Hope this whole explanation will be useful! Anyway, thanks to @pattacini and @traversaro for the time and help provided!

Thanks a lot @AlessandroT99 , this explanation is indeed quite useful!

robotology / icub-models