Implement tree merging

[hdamron17]

This is the first key difference between our project and TF2 or pytransform3d. When attempting to link two frames which both already have parents but are part of different trees, we should take the node with fewest ancestors and reverse the links so it is the root of its tree; then we can connect the two frames with that node as child. The implementation would go here.