MoveGroup has improper Chain parsing from URDF
pbeeson opened this issue · comments
If your URDF has a link A that is a parent to multiple joints, and that link is part of a requested chain, then MoveGroup GetActiveJoints() (and getCurrentJointValues(), etc.) returns a list that includes all the joints attached to link, even if they are not in the chain. That is, the MoveIt! URDF parser doesn't properly handle chains that span the root nodes of a URDF Tree (or similar) despite the fact that KDL's parser can handle this just fine.
| -----> Z
|
A ---> B --> C ---> E
|
|------> F --> G
If you request chain from G to E, you get the joint between A and Z (assuming it's not fixed) in your MoveGroup data structure.
Furthermore MoveIt! complains that this isn't a valid chain for KDL to handle, depite the fact that KDL's tree/chain parser handles this properly.
This shows an example where the base link is not a direct ancestor (parent of a parent of a parent....) of the tip link, though they have a common ancestor. Not only does MoveIt! say that this isn't a chain that can be used with the IK plugins, but the MoveGroup does create a datastructure that include all joints hanging off the common ancestor, even if they are not needed to connect the base link to the tip link.