We need to figure out a better way to do this... def. not robot-generic. I guess we should load this data from yaml format or rosparam. Same for the custom environment, below...