Python control mode is ready. ROS version is Andreas' work.