Added a few more functions for working with Joints & Segments in a KDL Chain