RobotLocomotion / gcs-science-robotics

Motion Planning around Obstacles with Convex Optimization by Marcucci et al, 2023
BSD 3-Clause "New" or "Revised" License
150 stars 19 forks source link

Trouble with Regenerating IRIS Regions for the prm_comparison.ipynb #8

Open BuildingAtom opened 2 months ago

BuildingAtom commented 2 months ago

Hi! When attempting to reproduce the results without using the pre-generated regions, I'm unable to complete planning to run the demonstration. Specifically, the getGCSPath function errors out with ValueError: Source vertex is not connected. I am using the provided Docker image to run the example for this (but I do run into the same problem when building from scratch).

---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
Cell In[22], line 2
      1 execute_task = task_circle
----> 2 GCS_path, GCS_time = getGCSPath(execute_task, verbose=True)
      3 linear_gcs_traj = make_traj(GCS_path, speed=2)
      4 print(
      5     f"Linear GCS length: {round(get_traj_length(linear_gcs_traj), 3)}, and time: {round(GCS_time, 3)} s"
      6 )

Cell In[19], line 9, in getGCSPath(sequence, verbose)
      7 for start_pt, goal_pt in zip(sequence[:-1], sequence[1:]):
      8     gcs = LinearGCS(regions)
----> 9     gcs.addSourceTarget(start_pt, goal_pt)
     10     gcs.setRoundingStrategy(randomForwardPathSearch,
     11                             max_paths=10,
     12                             max_trials=100,
     13                             seed=SEED)
     14     gcs.setSolver(MosekSolver())

File /gcs-science-robotics/gcs/linear.py:58, in LinearGCS.addSourceTarget(self, source, target, edges)
     57 def addSourceTarget(self, source, target, edges=None):
---> 58     source_edges, target_edges = super().addSourceTarget(source, target, edges)
     60     for edge in source_edges:
     61         for jj in range(self.dimension):

File /gcs-science-robotics/gcs/base.py:97, in BaseGCS.addSourceTarget(self, source, target, edges)
     94     edges = self.findStartGoalEdges(source, target)
     96 if not (len(edges[0]) > 0):
---> 97     raise ValueError('Source vertex is not connected.')
     98 if not (len(edges[1]) > 0):
     99     raise ValueError('Target vertex is not connected.')

ValueError: Source vertex is not connected.

I've tried to narrow down the issue and it seems to be that the IRIS regions are being generated differently, which shows in the generated GCS graphs. Regenerating the PRM with the pre-generated IRIS regions seems to work as expected. The pre-generated IRIS region creates the following GCS graph: image

Generating new IRIS regions create the following GCS graph (which has more edges than the prior): image

I'm looking to try running this method with a different scene/environment, so I'd greatly appreciate any insight into reproducing the results with newly generated IRIS regions.Thanks!