Incomplete tip link list from KinematicGroup::getAllPossibleTipLinkFrames

[marip8]

The available tip links for the KinematicGroup currently only include the tip links of the IK solver and direct children of those tip links:

tesseract/tesseract_kinematics/core/src/kinematic_group.cpp

Lines 90 to 95 in 67a83c7

	for (const auto& tip_link : inv_kin_->getTipLinkNames())
	{
	inv_tip_links_map_[tip_link] = tip_link;
	for (const auto& child : scene_graph.getLinkChildrenNames(tip_link))
	inv_tip_links_map_[child] = tip_link;
	}

It seems like this list should also include any link that is attached via a fixed joint tree/chain to the tip link(s) defined by the IK solver. It seems like this ought to be relatively easy to implement using the graph search capability of the scene graph.

I've run into this issue several times before where the TCP frame you want to use for planning was added several fixed joints beyond the IK solver tip frame to correspond more directly to geometry in the robot. When a frame like this is used for planning, the error is a very unhelpful generic map::at exception. It would also be nice to maybe wrap and rethrow that exception with more detailed information

[Levi-Armstrong]

It seems like this list should also include any link that is attached via a fixed joint tree/chain to the tip link(s) defined by the IK solver. It seems like this ought to be relatively easy to implement using the graph search capability of the scene graph.

The call to scene_graph.getLinkChildrenNames should get all children not just the immediate child. Do you have an example where this does not work as expect?

[marip8]

I think this does in fact function as expected. In the use-case where I thought I encountered this issue, I wasn't overly familar with the URDF; it turns out we had specified a TCP link that was a "cousin" of the tip link (it could be reached -- through fixed joints -- by going up one link in the hierarchy and then down to the specified TCP link). Is it worth trying to support a use-case like this, maybe in the case that the tip link can't be found in the existing map? It seems like it could be done using SceneGraph::getShortestPath and checking that all joints are fixed

[marip8]

Per our discussion yesterday, maybe a better approach would be to walk backwards from the specified tip link until we hit an actuated joint, and run getLinkChildrenNames from the child link of that actuated joint instead. This would capture all "cousin" frames that could potentially be specified as TCP frames