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 #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_{S_A}$ a sensor measurement and $^WR_{S_B}$ base sensor measurement. Consider also that we have a fixed calibration matrix for the base sensor $^{S_B}R_{L_B}$ (that transforms the base sensor measurements $S_B$ into the base link $L_B$), and for the other sensor $^{S_A}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}= ^{L_B}R_{L_A} = (^{W}R_{L_B})^{-1}\ ^WR_{L_A}
  $$
  $$
  ^{W^{*}}R_{L_B}=^{L_B}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? #270 (comment)

[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:

human-dynamics-estimation/devices/HumanStateProvider/HumanStateProvider.cpp

Lines 1151 to 1159 in ef6ac99

	if (pImpl->useFixedBase)
	{
	if (!pImpl->computeRelativeTransformForInputData(pImpl->linkTransformMatricesRaw)) {
	yError() << LogPrefix << "Failed to compute relative link transforms";
	askToStop();
	return;
	}
	}

We are saying if useFixedBase then perform some actions, then inside that function we have in

human-dynamics-estimation/devices/HumanStateProvider/HumanStateProvider.cpp

Line 1564 in ef6ac99

if (transforms.find(floatingBaseFrame) != transforms.end())

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

human-dynamics-estimation/devices/HumanStateProvider/HumanStateProvider.cpp

Line 546 in ef6ac99

pImpl->floatingBaseFrame = baseFrameName;

). Maybe we could have used a less confusing name like simply baseFrame