Merge "Fix bug in calculating path in ArcMotion"