#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>