It would be good to have a method for setting goals as a pose (final translation and rotation) of an object, should we create a different path?