inverse dynamic for chains works, have to separate it into composition of traversals. Now it is in one function call. Need to compare KDL tree kinematics solver values with templated version. RNE dynamics does not do name based joint and segment matching, so I naturally get different results for force/torque computations.