Add secondary calibration

[lrapetti]

Adding secondary calibration as described in #158.

The PR still need to be cleaned, and the rpc interfaces changed accordingly to what described in #159 (unfortunately this is a duplicate of the same changes done in the feature branch, merging the branch will need to be handled correctly)

[S-Dafarra]

Thank you! This indeed proved to be very useful during the preparation of the RAI demo.

While using it we noticed that if we calibrate twice, the result is screwed up. I guess it may be worth resetting the previously computed calibration before setting a new one.

The second thing I was thinking, was to add the possibility of calibrating a subtree starting from a link. The idea is that by calling an RPC method like

calibrateSubTree chest_fake

the chest, the arms, and the head are calibrated all together. The idea would be to iteratively add to the calibration list the children of the starting link, and all the children of the children and so on. I am not quite sure iDynTree has a ready to use method for this, but I guess that the entry points may be the method getNeighbor from which you can get the links attached to another link, while isParentOf can test whether the link is parent or child.

[lrapetti]

While using it we noticed that if we calibrate twice, the result is screwed up. I guess it may be worth resetting the previously computed calibration before setting a new one.

fixed with 1bef99e

[lrapetti]

The PR still need to be cleaned, and the rpc interfaces changed accordingly to what described in #159

done in bd68c04

With ad42f04 general cleanup of the implementation.

[lrapetti]

I discovered that there is currently a problem when using the retargeting on iCub. In fact we are using fake link that are rigidly attached to the model, and those links are not present in the reduced model that is generated (with getReducedModel). Those links are just present as frames in the reduced model, however it is hard to use them since all the links of the original model can actually be found in the reduced model as frames.

e.g. this is the reduced model for the chain r_hand_fake -> head_fake

Model: 
  Links: 
    [0] root_link
    [1] r_shoulder_1
    [2] neck_1
    [3] neck_2
    [4] head
    [5] r_shoulder_2
    [6] r_shoulder_3
    [7] r_elbow_1
    [8] r_forearm
    [9] r_wrist_1
    [10] r_hand
  Frames: 
    [11] base_link --> root_link
    [12] root_link_ems_acc_eb5 --> root_link
    [13] root_link_ems_gyro_eb5 --> root_link
    [14] torso_1 --> root_link
    [15] r_hip_1 --> root_link
    [16] l_hip_1 --> root_link
    [17] root_link_fake --> root_link
    [18] Pelvis_accelerometer --> root_link
    [19] Pelvis_gyro --> root_link
    [20] l_hip_2 --> root_link
    [21] l_hip_3 --> root_link
    [22] l_leg_ft_sensor --> root_link
    [23] l_upper_leg --> root_link
    [24] l_upper_leg_back_contact --> root_link
    [25] l_upper_leg_dh_frame --> root_link
    [26] l_upper_leg_ems_acc_eb6 --> root_link
    [27] l_upper_leg_ems_acc_eb10 --> root_link
    [28] l_upper_leg_mtb_acc_10b1 --> root_link
    [29] l_upper_leg_mtb_acc_10b2 --> root_link
    [30] l_upper_leg_mtb_acc_10b3 --> root_link
    [31] l_upper_leg_mtb_acc_10b4 --> root_link
    [32] l_upper_leg_mtb_acc_10b5 --> root_link
    [33] l_upper_leg_mtb_acc_10b6 --> root_link
    [34] l_upper_leg_mtb_acc_10b7 --> root_link
    [35] l_upper_leg_ems_gyro_eb6 --> root_link
    [36] l_upper_leg_ems_gyro_eb10 --> root_link
    [37] l_lower_leg --> root_link
    [38] l_lower_leg_ems_acc_eb7 --> root_link
    [39] l_lower_leg_mtb_acc_10b8 --> root_link
    [40] l_lower_leg_mtb_acc_10b9 --> root_link
    [41] l_lower_leg_mtb_acc_10b10 --> root_link
    [42] l_lower_leg_mtb_acc_10b11 --> root_link
    [43] l_lower_leg_ems_gyro_eb7 --> root_link
    [44] l_upper_leg_fake --> root_link
    [45] LeftUpperLeg_accelerometer --> root_link
    [46] LeftUpperLeg_gyro --> root_link
    [47] l_ankle_1 --> root_link
    [48] l_lower_leg_fake --> root_link
    [49] LeftLowerLeg_accelerometer --> root_link
    [50] LeftLowerLeg_gyro --> root_link
    [51] l_ankle_2 --> root_link
    [52] l_foot --> root_link
    [53] l_sole --> root_link
    [54] l_sole_skin_0 --> root_link
    [55] l_sole_skin_1 --> root_link
    [56] l_sole_skin_2 --> root_link
    [57] l_sole_skin_3 --> root_link
    [58] l_sole_skin_4 --> root_link
    [59] l_sole_skin_8 --> root_link
    [60] l_sole_skin_9 --> root_link
    [61] l_sole_skin_10 --> root_link
    [62] l_sole_skin_11 --> root_link
    [63] l_sole_skin_12 --> root_link
    [64] l_sole_skin_13 --> root_link
    [65] l_sole_skin_14 --> root_link
    [66] l_sole_skin_15 --> root_link
    [67] l_sole_skin_17 --> root_link
    [68] l_sole_skin_18 --> root_link
    [69] l_sole_skin_19 --> root_link
    [70] l_sole_skin_20 --> root_link
    [71] l_sole_skin_21 --> root_link
    [72] l_sole_skin_22 --> root_link
    [73] l_sole_skin_24 --> root_link
    [74] l_sole_skin_25 --> root_link
    [75] l_sole_skin_26 --> root_link
    [76] l_sole_skin_27 --> root_link
    [77] l_sole_skin_28 --> root_link
    [78] l_sole_skin_29 --> root_link
    [79] l_foot_dh_frame --> root_link
    [80] l_foot_ft_sensor --> root_link
    [81] l_foot_mtb_acc_10b12 --> root_link
    [82] l_foot_mtb_acc_10b13 --> root_link
    [83] l_foot_fake --> root_link
    [84] LeftFoot_accelerometer --> root_link
    [85] LeftFoot_gyro --> root_link
    [86] r_hip_2 --> root_link
    [87] r_hip_3 --> root_link
    [88] r_leg_ft_sensor --> root_link
    [89] r_upper_leg --> root_link
    [90] r_upper_leg_back_contact --> root_link
    [91] r_upper_leg_dh_frame --> root_link
    [92] r_upper_leg_ems_acc_eb8 --> root_link
    [93] r_upper_leg_ems_acc_eb11 --> root_link
    [94] r_upper_leg_mtb_acc_11b1 --> root_link
    [95] r_upper_leg_mtb_acc_11b2 --> root_link
    [96] r_upper_leg_mtb_acc_11b3 --> root_link
    [97] r_upper_leg_mtb_acc_11b4 --> root_link
    [98] r_upper_leg_mtb_acc_11b5 --> root_link
    [99] r_upper_leg_mtb_acc_11b6 --> root_link
    [100] r_upper_leg_mtb_acc_11b7 --> root_link
    [101] r_upper_leg_ems_gyro_eb8 --> root_link
    [102] r_upper_leg_ems_gyro_eb11 --> root_link
    [103] r_lower_leg --> root_link
    [104] r_lower_leg_skin_0 --> root_link
    [105] r_lower_leg_skin_3 --> root_link
    [106] r_lower_leg_skin_4 --> root_link
    [107] r_lower_leg_skin_5 --> root_link
    [108] r_lower_leg_skin_6 --> root_link
    [109] r_lower_leg_skin_9 --> root_link
    [110] r_lower_leg_skin_10 --> root_link
    [111] r_lower_leg_skin_11 --> root_link
    [112] r_lower_leg_skin_14 --> root_link
    [113] r_lower_leg_skin_15 --> root_link
    [114] r_lower_leg_skin_16 --> root_link
    [115] r_lower_leg_skin_17 --> root_link
    [116] r_lower_leg_skin_19 --> root_link
    [117] r_lower_leg_skin_20 --> root_link
    [118] r_lower_leg_skin_21 --> root_link
    [119] r_lower_leg_skin_28 --> root_link
    [120] r_lower_leg_skin_29 --> root_link
    [121] r_lower_leg_skin_31 --> root_link
    [122] r_lower_leg_skin_32 --> root_link
    [123] r_lower_leg_skin_35 --> root_link
    [124] r_lower_leg_skin_36 --> root_link
    [125] r_lower_leg_skin_37 --> root_link
    [126] r_lower_leg_skin_38 --> root_link
    [127] r_lower_leg_skin_41 --> root_link
    [128] r_lower_leg_skin_42 --> root_link
    [129] r_lower_leg_skin_43 --> root_link
    [130] r_lower_leg_skin_46 --> root_link
    [131] r_lower_leg_skin_47 --> root_link
    [132] r_lower_leg_skin_49 --> root_link
    [133] r_lower_leg_skin_50 --> root_link
    [134] r_lower_leg_skin_51 --> root_link
    [135] r_lower_leg_skin_52 --> root_link
    [136] r_lower_leg_skin_53 --> root_link
    [137] r_lower_leg_skin_54 --> root_link
    [138] r_lower_leg_skin_55 --> root_link
    [139] r_lower_leg_skin_56 --> root_link
    [140] r_lower_leg_skin_60 --> root_link
    [141] r_lower_leg_skin_61 --> root_link
    [142] r_lower_leg_ems_acc_eb9 --> root_link
    [143] r_lower_leg_mtb_acc_11b8 --> root_link
    [144] r_lower_leg_mtb_acc_11b9 --> root_link
    [145] r_lower_leg_mtb_acc_11b10 --> root_link
    [146] r_lower_leg_mtb_acc_11b11 --> root_link
    [147] r_lower_leg_ems_gyro_eb9 --> root_link
    [148] r_upper_leg_fake --> root_link
    [149] RightUpperLeg_accelerometer --> root_link
    [150] RightUpperLeg_gyro --> root_link
    [151] r_ankle_1 --> root_link
    [152] r_lower_leg_fake --> root_link
    [153] RightLowerLeg_accelerometer --> root_link
    [154] RightLowerLeg_gyro --> root_link
    [155] r_ankle_2 --> root_link
    [156] r_foot --> root_link
    [157] r_sole --> root_link
    [158] r_sole_skin_0 --> root_link
    [159] r_sole_skin_1 --> root_link
    [160] r_sole_skin_2 --> root_link
    [161] r_sole_skin_3 --> root_link
    [162] r_sole_skin_4 --> root_link
    [163] r_sole_skin_8 --> root_link
    [164] r_sole_skin_9 --> root_link
    [165] r_sole_skin_10 --> root_link
    [166] r_sole_skin_11 --> root_link
    [167] r_sole_skin_12 --> root_link
    [168] r_sole_skin_13 --> root_link
    [169] r_sole_skin_14 --> root_link
    [170] r_sole_skin_15 --> root_link
    [171] r_sole_skin_17 --> root_link
    [172] r_sole_skin_18 --> root_link
    [173] r_sole_skin_19 --> root_link
    [174] r_sole_skin_20 --> root_link
    [175] r_sole_skin_21 --> root_link
    [176] r_sole_skin_22 --> root_link
    [177] r_sole_skin_24 --> root_link
    [178] r_sole_skin_25 --> root_link
    [179] r_sole_skin_26 --> root_link
    [180] r_sole_skin_27 --> root_link
    [181] r_sole_skin_28 --> root_link
    [182] r_sole_skin_29 --> root_link
    [183] r_foot_dh_frame --> root_link
    [184] r_foot_ft_sensor --> root_link
    [185] r_foot_mtb_acc_11b12 --> root_link
    [186] r_foot_mtb_acc_11b13 --> root_link
    [187] r_foot_fake --> root_link
    [188] RightFoot_accelerometer --> root_link
    [189] RightFoot_gyro --> root_link
    [190] torso_2 --> root_link
    [191] chest --> root_link
    [192] chest_ems_acc_eb1 --> root_link
    [193] chest_ems_acc_eb2 --> root_link
    [194] chest_ems_acc_eb3 --> root_link
    [195] chest_ems_acc_eb4 --> root_link
    [196] chest_mtb_acc_0b7 --> root_link
    [197] chest_mtb_acc_0b8 --> root_link
    [198] chest_mtb_acc_0b9 --> root_link
    [199] chest_mtb_acc_0b10 --> root_link
    [200] chest_ems_gyro_eb1 --> root_link
    [201] chest_ems_gyro_eb2 --> root_link
    [202] chest_ems_gyro_eb3 --> root_link
    [203] chest_ems_gyro_eb4 --> root_link
    [204] l_shoulder_1 --> root_link
    [205] chest_fake --> root_link
    [206] T8_accelerometer --> root_link
    [207] T8_gyro --> root_link
    [208] l_shoulder_2 --> root_link
    [209] l_shoulder_3 --> root_link
    [210] l_upper_arm --> root_link
    [211] l_arm_ft_sensor --> root_link
    [212] l_upper_arm_mtb_acc_1b10 --> root_link
    [213] l_upper_arm_mtb_acc_1b11 --> root_link
    [214] l_upper_arm_mtb_acc_1b12 --> root_link
    [215] l_upper_arm_mtb_acc_1b13 --> root_link
    [216] l_elbow_1 --> root_link
    [217] l_upper_arm_fake --> root_link
    [218] LeftUpperArm_accelerometer --> root_link
    [219] LeftUpperArm_gyro --> root_link
    [220] l_forearm --> root_link
    [221] l_forearm_skin_0 --> root_link
    [222] l_forearm_skin_1 --> root_link
    [223] l_forearm_skin_2 --> root_link
    [224] l_forearm_skin_3 --> root_link
    [225] l_forearm_skin_4 --> root_link
    [226] l_forearm_skin_5 --> root_link
    [227] l_forearm_skin_6 --> root_link
    [228] l_forearm_skin_7 --> root_link
    [229] l_forearm_skin_8 --> root_link
    [230] l_forearm_skin_9 --> root_link
    [231] l_forearm_skin_10 --> root_link
    [232] l_forearm_skin_11 --> root_link
    [233] l_forearm_skin_12 --> root_link
    [234] l_forearm_skin_13 --> root_link
    [235] l_forearm_skin_14 --> root_link
    [236] l_forearm_skin_15 --> root_link
    [237] l_forearm_skin_16 --> root_link
    [238] l_forearm_skin_17 --> root_link
    [239] l_forearm_skin_19 --> root_link
    [240] l_forearm_skin_22 --> root_link
    [241] l_forearm_skin_24 --> root_link
    [242] l_forearm_skin_25 --> root_link
    [243] l_forearm_skin_28 --> root_link
    [244] l_forearm_skin_29 --> root_link
    [245] l_forearm_dh_frame --> root_link
    [246] l_forearm_mtb_acc_1b7 --> root_link
    [247] l_forearm_mtb_acc_1b8 --> root_link
    [248] l_forearm_mtb_acc_1b9 --> root_link
    [249] l_wrist_1 --> root_link
    [250] l_forearm_fake --> root_link
    [251] LeftForeArm_accelerometer --> root_link
    [252] LeftForeArm_gyro --> root_link
    [253] l_hand --> root_link
    [254] l_hand_dh_frame --> root_link
    [255] l_hand_fake --> root_link
    [256] LeftHand_accelerometer --> root_link
    [257] LeftHand_gyro --> root_link
    [258] imu_frame --> head
    [259] head_imu_acc_1x1 --> head
    [260] Head_gyro --> head
    [261] head_fake --> head
    [262] Head_accelerometer --> head
    [263] r_upper_arm --> r_shoulder_3
    [264] r_arm_ft_sensor --> r_shoulder_3
    [265] r_upper_arm_mtb_acc_2b10 --> r_shoulder_3
    [266] r_upper_arm_mtb_acc_2b11 --> r_shoulder_3
    [267] r_upper_arm_mtb_acc_2b12 --> r_shoulder_3
    [268] r_upper_arm_mtb_acc_2b13 --> r_shoulder_3
    [269] r_upper_arm_fake --> r_shoulder_3
    [270] RightUpperArm_accelerometer --> r_shoulder_3
    [271] RightUpperArm_gyro --> r_shoulder_3
    [272] r_forearm_skin_0 --> r_forearm
    [273] r_forearm_skin_1 --> r_forearm
    [274] r_forearm_skin_2 --> r_forearm
    [275] r_forearm_skin_3 --> r_forearm
    [276] r_forearm_skin_4 --> r_forearm
    [277] r_forearm_skin_5 --> r_forearm
    [278] r_forearm_skin_6 --> r_forearm
    [279] r_forearm_skin_7 --> r_forearm
    [280] r_forearm_skin_8 --> r_forearm
    [281] r_forearm_skin_9 --> r_forearm
    [282] r_forearm_skin_10 --> r_forearm
    [283] r_forearm_skin_11 --> r_forearm
    [284] r_forearm_skin_12 --> r_forearm
    [285] r_forearm_skin_13 --> r_forearm
    [286] r_forearm_skin_14 --> r_forearm
    [287] r_forearm_skin_15 --> r_forearm
    [288] r_forearm_skin_16 --> r_forearm
    [289] r_forearm_skin_17 --> r_forearm
    [290] r_forearm_skin_19 --> r_forearm
    [291] r_forearm_skin_22 --> r_forearm
    [292] r_forearm_skin_24 --> r_forearm
    [293] r_forearm_skin_25 --> r_forearm
    [294] r_forearm_skin_28 --> r_forearm
    [295] r_forearm_skin_29 --> r_forearm
    [296] r_forearm_dh_frame --> r_forearm
    [297] r_forearm_mtb_acc_2b7 --> r_forearm
    [298] r_forearm_mtb_acc_2b8 --> r_forearm
    [299] r_forearm_mtb_acc_2b9 --> r_forearm
    [300] r_forearm_fake --> r_forearm
    [301] RightForeArm_accelerometer --> r_forearm
    [302] RightForeArm_gyro --> r_forearm
    [303] r_hand_dh_frame --> r_hand
    [304] r_hand_fake --> r_hand
    [305] RightHand_accelerometer --> r_hand
    [306] RightHand_gyro --> r_hand
 Joints: 
    [0] r_wrist_yaw (dofs: 1) : r_wrist_1<-->r_hand
    [1] r_wrist_pitch (dofs: 1) : r_forearm<-->r_wrist_1
    [2] r_wrist_prosup (dofs: 1) : r_elbow_1<-->r_forearm
    [3] r_elbow (dofs: 1) : r_shoulder_3<-->r_elbow_1
    [4] r_shoulder_yaw (dofs: 1) : r_shoulder_2<-->r_shoulder_3
    [5] r_shoulder_roll (dofs: 1) : r_shoulder_1<-->r_shoulder_2
    [6] r_shoulder_pitch (dofs: 1) : root_link<-->r_shoulder_1
    [7] neck_pitch (dofs: 1) : root_link<-->neck_1
    [8] neck_roll (dofs: 1) : neck_1<-->neck_2
    [9] neck_yaw (dofs: 1) : neck_2<-->head

Clearly we can not detect the links to calibrate neither reading the links in the reduced model, or the frames. So for the moment this option is useless using models with "fake links".

[lrapetti]

So for the moment this option is useless using models with "fake links".

The positive side is that actually this options makes the fake links almost useless. In fact, we may just use the regular links of the robot and perform a whole-body calibration once at the beginning so that we have the calibration matrix between the robot and the human link orientation.

[S-Dafarra]

I discovered that there is currently a problem when using the retargeting on iCub. In fact we are using fake link that are rigidly attached to the model, and those links are not present in the reduced model that is generated (with getReducedModel). Those links are just present as frames in the reduced model, however it is hard to use them since all the links of the original model can actually be found in the reduced model as frames.

Sorry, I did not understand 🤔

The reduced model seems right. Initially, I found weird that root_link was there, but it makes sense: the chest and the rest of the robot are merged into a single body. The name of the body corresponds to the one with the lowest index amongst those to be merged.

[lrapetti]

The reduced model seems right.

I am not saying it is wrong, but it is not exactly what we were looking for. It doesn't have any information regarding the link (fake links) that are present along the chain and we want to calibrate.

[S-Dafarra]

Ah ok, I see!

[lrapetti]

The problem described above for the chain calibration remains open, however I think we can procede merging the PR because the feature can still be useful in different cases (e.g. @kouroshD has been using it lately I think)

[lrapetti]

Codacy review is still failing (https://app.codacy.com/manual/diegoferigo/human-dynamics-estimation/pullRequest?prid=4567113) claming that the devel branch is not enabled

However in the setting it seems to be active on the devel branch

@diegoferigo Since this is something we didn't succede to address the problem for long time (#142 (comment), #137 (comment), #133 (comment), #121 (comment)), at this point I would remove the check for the time being.

[diegoferigo]

@lrapetti Are you sure your codacy points to robotology and not your fork? I tried to enable devel from my account, can you check if it's ok now?

[lrapetti]

@lrapetti Are you sure your codacy points to robotology and not your fork? I tried to enable devel from my account, can you check if it's ok now?

@diegoferigo still not running, please try if you can launch it manually.

[diegoferigo]

@diegoferigo still not running, please try if you can launch it manually.

@lrapetti probably we had to wait a bit more. I just checked and this PR has been successfully analysed.

[lrapetti]

@diegoferigo still not running, please try if you can launch it manually.

@lrapetti probably we had to wait a bit more. I just checked and this PR has been successfully analysed.

no I'm quite sure it was not running when triggered manually, it started again triggered by new changes.

[lrapetti]

I have squashed the commits, waiting for the checks before merging.

[lrapetti]

@diegoferigo I would ignore the issue raised by Codacy (https://app.codacy.com/manual/diegoferigo/human-dynamics-estimation/pullRequest?prid=4567113) but I don't have the rights, if you agree could you please do that so we got green flag ✔️?

[diegoferigo]

Done, triggered a new analysis, waiting the update of the check.

[lrapetti]

Done, triggered a new analysis, waiting the update of the check.

Still, it is showing as Not up to standards but I don't see any new issue in the logger

[traversaro]

Done, triggered a new analysis, waiting the update of the check.

Still, it is showing as Not up to standards but I don't see any new issue in the logger

I am afraid the issues are in "New duplicates".

However, what about removing Codacy check, or at least making it optional to merge a PR? In my experience the amount of false positives and hiccups of Codacy it non-negligible.

[lrapetti]

However, what about removing Codacy check, or at least making it optional to merge a PR? In my experience the amount of false positives and hiccups of Codacy it non-negligible.

I agree on making it optional, if this means that we get the green flag even when its failing.

[diegoferigo]

I just set only Travis as required check, let's try again :)

[diegoferigo]

What I'm not sure is if the check mark of the PR will become green even though Codacy is not a required check

[lrapetti]

What I'm not sure is if the check mark of the PR will become green even though Codacy is not a required check

Unfortunately seems it is not like that. Anyway I will procede merging since this PR has been ready for a while. In general I think having the green flag helps a lot when searching for deprecation points, I don't know if it would make sense indeed to remove Codacy.