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