ReferentialSkeletons do not include BodyNodes with zero-dof parent Joints

[mkoval]

I expected Branch::create(node) to return a branch that includes the sub-tree rooted at node, including node itself. This is consistent with my interpretation of the Doxygen comment:

Branch is a specialized type of Linkage that represents a complete subtree of a Skeleton.

The Branch will start at a specific BodyNode and will include every BodyNode that descends from it, all the way to the leaves.

However, this is not currently the case - the Branch does not include node. I am not sure if I am misinterpreting the documentation (as in #437 😄) or if this is a bug.

[mxgrey]

It should definitely include the starting node. I'll look into why that might fail.

[mkoval]

Actually, the issue is more subtle than I first thought. Branch seems to skip all BodyNodes that have a parent FixedJoint.

I have a kinematic structure that looks like this:

wam6 -[ revolute ]-> wam7 -[ fixed ]-> hand_base -[ revolute ]-> ...fingers...

The contents of Branch::create(wam7) is:

- /right/wam7
- /right/finger0_0
- /right/finger0_1
- /right/finger0_2
- /right/finger1_0
- /right/finger1_1
- /right/finger1_2
- /right/finger2_1
- /right/finger2_2

The contents of Branch::create(hand_base) is:

- /right/finger0_0
- /right/finger0_1
- /right/finger0_2
- /right/finger1_0
- /right/finger1_1
- /right/finger1_2
- /right/finger2_1
- /right/finger2_2

Note that hand_base is missing in both outputs.

[mxgrey]

That's a less surprising bug. I thought this situation was being handled, but I'm not shocked that it isn't. I'll fix it and write a regression test.

[mkoval]

In fact, I think this issue applies to all ReferentialSkeletons. I'm now seeing similar behavior in Chain because it stops at the first FixedJoint it encounters. For example, Chain::create(Chain::Criteria(wam1, finger2_2)) stops at wam7 instead of including hand_base and the finger BodyNodes.

[Edit] Confirmed. I can't even manually add a BodyNode that is a child of a FixedJoint to a Group using addBodyNode.

[mxgrey]

Interestingly, it turns out this issue is directly related to #548. The coupling between DOFs and BodyNodes is so strong in the ReferentialSkeleton that a BodyNode with no DOFs will get left out of the ReferentialSkeleton. This is obviously not okay. I'll be fixing this issue simultaneously with the feature request of #548.

[mxgrey]

I believe this issue should be resolved as of #557. Let me know if that is not the case.