search

tesseract-robotics / tesseract

Motion Planning Environment
http://tesseract-docs.rtfd.io
Other
274 stars 89 forks source link

Incomplete tip link list from `KinematicGroup::getAllPossibleTipLinkFrames` #1057

Open marip8 opened 1 day ago

marip8 commented 1 day ago

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

https://github.com/tesseract-robotics/tesseract/blob/67a83c76d12a9bde43c9d96e4e747b558155bbc5/tesseract_kinematics/core/src/kinematic_group.cpp#L90-L95

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 commented 1 day ago

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 commented 1 day ago

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