Fix WaypointEdge class

* For n waypoints, the class contains n states and n+1 edges.
* The constraints of the WaypointEdge are not used.