Created new utility helper function for sending to a goal pose