I have a graph of convex sets where I am solving for the optimal path through multiple regions. I am working on improving the costs between each vertex (polygon-shaped regions). The cost to travel along the edge from vertex u to vertex v should be the distance from u to v multiplied by a scaling factor for traveling through that vectex. (The longer the edge travels through the vertex, the more effect that vertex will have on the cost)
i.e. cost_of_edge_from_u_to_v = distance_from_u_to_v * (factor_while_traveling_through_u * distance_from_start_point_to_end_of_u + factor_while_traveling_through_v * distance_from_start_of_v_to_end_point)
I am adding a cost to the edge as a drake::symbolic::expression. I would like to compute the intersection point of the path edge from u to v with one of the outer edges of the polygon, v. Using this intersection point, I will be able to compute the ratio of time in polygon u and in polygon v which will allow me to use the equation mentioned above to get a upper bound for the cost. Currently, I am running into issues of dividing by the determinate which can be zero at some points when solving for the intersection point.
An example of this could be a vehicle traveling from one region to another where the ground of the first region is muddy and the second is made of concrete. While the distance might be the same in region 1 and region 2 that the vehicle has to travel through, the cost will be much higher for traveling through the mud.
How should I use drake to create a continuous solution through convex sets while including specific edge costs (depending on the selected vertex)?
I think there are better ways to formulate this.
Let's assume, for the sake of discussion, that the polygons represent paths in ℜ³. Then it would be natural to create a GCS region for each of your regions in ℜ⁶ = ℜ³ x ℜ³. In words, each time you visit a vertex you pick 2 points in the original space. Then you can add a vertex cost to each region that charges a region-specific-scalar times the (Euclidean?) distance between the two points. And you can add an edge constraint saying that the second point in u == the first point in v.
This is effectively the simplest version of the transcription that we used in the first GCS motion planning paper.