Added other QPs and some temp codes for ROS and C++