#include <string>
#include <vector>
#include <limits>
#include <ros/ros.h>
#include <cob_twist_controller/cob_twist_controller.h>
#include <kdl_conversions/kdl_msg.h>
#include <tf/transform_datatypes.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <cob_srvs/SetString.h>
#include <Eigen/Dense>