Put pose into geometry message through posemath