Updating params and now using correct mass in state_control
Changing ROS_INFO