Added subcriptions for initial and goal positions