restored changes from before pull to make knowrob_sim possible.