Fix calibration matrices order in HumanStateProvider

[lrapetti]

Changing the order of the call to the fixed rotation and relative rotation computation. This allows using the fixed base together with the fixed rotation fixing https://github.com/robotology/human-dynamics-estimation/issues/269.

I have already tested the usage of a fixed rotation for the base frame with few sensors. I will test it again in a more complex scenario before merging

[lrapetti]

Hi @kouroshD @RiccardoGrieco, if you can have a quick look at this PR we can merge it

[RiccardoGrieco]

@lrapetti we rebased the branch on top of the master in order to make a test for the upcoming demo.

[lrapetti]

@lrapetti we rebased the branch on top of the master in order to make a test for the upcoming demo.

ok, feel free to push it to the branch in case everything is fine, and then we can proceed merging

[kouroshD]

Changing the order of the call to the fixed rotation and relative rotation computation. This allows using the fixed base together with the fixed rotation fixing #269.

I have already tested the usage of a fixed rotation for the base frame with few sensors. I will test it again in a more complex scenario before merging

@lrapetti Can you please elaborate a bit more detailed here, it is not clear to me exactly the purpose of the PR. I see we are changing the place of applying a transformation, hence their order; but could not follow why. Thanks.

[lrapetti]

Sure, here is what happens.

Consider $^WR_{SA}$ a sensor measurement and $^WR{S_B}$ base sensor measurement. Consider also that we have a fixed calibration matrix for the base sensor $^{SB}R{L_B}$ (that transforms the base sensor measurements $S_B$ into the base link $L_B$), and for the other sensor $^{SA}R{L_A}$

Consider the case in which we want to use fixed-based IK (all the sensors measurement are given w.r.t. the base)

Before

This is what was heppening before this PR:

• We compute the relative transforms between the sensors and the base: $$ \^{W^{*}}R_{S_A}= ^{S_B}R_{S_A} = (^{W}R_{S_B})^{-1}\ ^WR_{S_A} $$ $$ ^{W^{*}}R_{S_B}=^{S_B}R_{S_B}=1 $$ As you can see in this case the new World ($W^{*}$) concide with the base sensor and not with the base link
• Then we apply the fixed transform $$ \^{W^{*}}R_{L_A} = ^{W^{*}}R_{S_A} \ ^{S_A}R_{L_A} $$ $$ \^{W^{*}}R_{L_B} = ^{W^{*}}R_{S_B} \ ^{S_B}R_{L_B} $$

After

After the PR, things are fixed by the fact that we use the base link as reference instead of the base frame

• we apply the fixed transform $$ ^{W}R_{L_A} = ^{W}R_{S_A} \ ^{S_A}R_{L_A} $$ $$ ^{W}R_{L_B} = ^{W}R_{S_B} \ ^{S_B}R_{L_B} $$
• We compute the relative transforms w.r.t. the base $$ ^{W^{*}}R_{S_A}= ^{LB}R{LA} = (^{W}R{LB})^{-1}\ ^WR{LA} $$ $$ ^{W^{*}}R{L_B}=^{LB}R{L_B}=1 $$

[kouroshD]

Thank you @lrapetti . Now it is clear, also after checking with more attention to the code. Just one point, partially relevant to this PR: In https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1144-L1152 , we are saying if we are fixed based do sth. Then in the fucntion https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1146 , in this line we are saying check for the floating base frame name https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1562 .

I am confused a bit with these naming! If you want to explain it here, feel free. The PR looks good to me.

[lrapetti]

Thanks @kouroshD!

[kouroshD]

Thank you @lrapetti . Did you read this comment by chance? https://github.com/robotology/human-dynamics-estimation/pull/270#issuecomment-983534188

[lrapetti]

Thank you @lrapetti . Did you read this comment by chance? #270 (comment)

Sorry @kouroshD! I missed that! I will check it now

[kouroshD]

Sorry @kouroshD! I missed that! I will check it now

Yes, I imagined. Don't worry, take your time.

[lrapetti]

In https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1144-L1152 , we are saying if we are fixed based do sth. Then in the fucntion https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1146 , in this line we are saying check for the floating base frame name https://github.com/robotology/human-dynamics-estimation/pull/270/files#diff-3daa57ee2a77e48985ef2075ff08d8b318f9e9c67fd813230473f4492cf19fd1L1562 .

Sorry @kouroshD, I didn't understand what is your doubt. Also, I think the last two links are not redirecting me correctly.

[kouroshD]

Sorry @lrapetti , my bad. Check here: https://github.com/robotology/human-dynamics-estimation/blob/ef6ac99ad54d39ae7091070fb15a2c73e1334e5e/devices/HumanStateProvider/HumanStateProvider.cpp#L1151-L1159 We are saying if useFixedBase then perform some actions, then inside that function we have in https://github.com/robotology/human-dynamics-estimation/blob/ef6ac99ad54d39ae7091070fb15a2c73e1334e5e/devices/HumanStateProvider/HumanStateProvider.cpp#L1564 check the floating base frame name. It was a bit confusing to me, so that was why I asked you.

[lrapetti]

@kouroshD I see your point. That is due to the fact that either if we are forcing the system to be in fixed-base condition or not, we will have to define a frame that is the base frame of the floating base system (see https://github.com/robotology/human-dynamics-estimation/blob/ef6ac99ad54d39ae7091070fb15a2c73e1334e5e/devices/HumanStateProvider/HumanStateProvider.cpp#L546). Maybe we could have used a less confusing name like simply baseFrame