Refactored functions, new robot_model intialization