Fix compile error with orocos_kdl. Fixes #370