Add publication of waist pose as geometry_msgs::TransformStamped
Bug fix on the waist pose publication