First successes with my KR5 robot. Joint Constraints for goal doesn't really work though...