Documentation and made function public

Made ``printTransform`` function public so that other MoveIt code can easily use this helper