#include <boost/foreach.hpp>
#include "joint_tracker/MultiJointTracker.h"
#include <geometry_msgs/Vector3.h>
#include <cmath>
#include <wrappers/matrix/matrix_wrapper.h>
#include "omip_msgs/RigidBodyTwistWithCovMsg.h"
#include "omip_common/OMIPUtils.h"