#include <sstream>
#include <string>
#include <vector>
#include <limits>
#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/pointer_cast.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/jntarray.hpp>
#include "cob_twist_controller/constraints/constraint.h"
#include "cob_twist_controller/constraints/constraint_params.h"
#include "cob_twist_controller/damping_methods/damping.h"
#include "cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h"
#include <eigen_conversions/eigen_kdl.h>