Fixed not being able to traverse backward in the graph, causing the traversal to fail for such queries.
Removed the cost matrix 'S' from the query functions. We will need to use the A and B matrices separately in the file to compute the lqr locally to get the cost to go.
Updated the existing ntcg using files to use the proper direct collocation function.
Renamed some of the ntcg functions and folders for consistency.
Fixed not being able to traverse backward in the graph, causing the traversal to fail for such queries.
Removed the cost matrix 'S' from the query functions. We will need to use the A and B matrices separately in the file to compute the lqr locally to get the cost to go.
Updated the existing ntcg using files to use the proper direct collocation function.
Renamed some of the ntcg functions and folders for consistency.