fix properly transforming robot position into goal frame