Graph of Convex Sets

Graph of Convex Sets#

This notebook walks through the way the semantic world navigates spaces that are challenging to navigate. Usually, navigation is done in spaces that are convex, meaning that every point is reachable via a straight line without collision. Unfortunately, the real world is not like this.

The semantic world internally represents the objects in the world using some implementation of a scene description format. These formats include collision information for every object. The collision information is often approximated using a set of boxes.

These collision boxes are converted to their algebraic representation using the random-events package. This allows the free space to be formulated as the complement of the belief state collision boxes. The complement itself is a finite collection of (possible infinitely big) boxes that do not intersect. These boxes, however, have surfaces that are adjacent. Representing this adjacency is done using a Graph of Convex Sets (GCS) where every node is a box, and every edge means that these boxes are adjacent. Navigating the free space is then possible using path finding algorithms on the graph.

You can read more about GCS here.

Let’s get hands on! First, we need to create a world that makes navigation non-trivial.

from semantic_world.geometry import Box, Scale, Color
from semantic_world.prefixed_name import PrefixedName
from semantic_world.spatial_types import TransformationMatrix
from semantic_world.world import World
from semantic_world.world_entity import Body

box_world = World()

with box_world.modify_world():
    box = Body(name=PrefixedName("box"), collision=[Box(scale=Scale(0.5, 0.5, 0.5),
                                                        color=Color(1., 1., 1., 1.),
                                                        origin=TransformationMatrix.from_xyz_rpy(0,0,0,0,0,0),) ])
    box_world.add_kinematic_structure_entity(box)
    

Next, we create a connectivity graph of the space so we can solve navigation problems. To visualize the result in a better way, we limit the search space to a finite set around the box. Furthermore, we constraint the robot to be unable to fly by constraining the z-axis. Otherwise, he would get the idea to go over the box, which is not a good idea.

from random_events.interval import SimpleInterval
from semantic_world.graph_of_convex_sets import GraphOfConvexSets
from semantic_world.geometry import BoundingBox

search_space = BoundingBox(min_x=-1, max_x=1,
                           min_y=-1, max_y=1,
                           min_z=0.1, max_z=0.2, reference_frame=box_world.root).as_collection()
                           
gcs = GraphOfConvexSets.free_space_from_world(box_world, search_space=search_space)

Let’s have a look at the free space constructed. We can see that it is a rectangular catwalk around the obstacle.

import plotly
plotly.offline.init_notebook_mode()
import plotly.graph_objects as go

fig = go.Figure(gcs.plot_free_space())
fig.show()

Looking at the connectivity graph, we can see that it is still possible to go from one side of the box to the other, just not directly. Intuitively, we can see that we just have to go around the obstacle.

gcs.draw()

../_images/5eb87496c7d481dc79a06e3195a995f7bb0d795ffadedc288420ea05268614f8.png

Let’s use graph theory to find a path!

from semantic_world.spatial_types import Point3

start = Point3(-0.75, 0, 0.15)
goal = Point3(0.75, 0, 0.15)
path = gcs.path_from_to(start, goal)
print("A potential path is", [(point.x, point.y) for point in path])

A potential path is [(SX(-0.75), SX(0)), (SX(-0.25), SX(-0.625)), (SX(0.25), SX(-0.625)), (SX(0.75), SX(0))]

This minimal example demonstrates a concept that can be applied to the entire belief state of the robot. Let’s load a more complex environment and look at the connectivity of it.

import os
from pathlib import Path
from semantic_world.adapters.urdf import URDFParser
root = next(
    parent
    for parent in [Path.cwd(), *Path.cwd().parents]
    if (parent / "pyproject.toml").exists()
)

apartment = os.path.realpath(os.path.join(root, "resources", "urdf", "kitchen.urdf"))

apartment_parser = URDFParser.from_file(apartment)
world = apartment_parser.parse()

search_space = BoundingBox(min_x=-2, max_x=2,
                           min_y=-2, max_y=2,
                           min_z=0., max_z=2, reference_frame=world.root).as_collection()
gcs = GraphOfConvexSets.free_space_from_world(world, search_space=search_space)

Scalar element defined multiple times: limit

---------------------------------------------------------------------------
ModuleNotFoundError                       Traceback (most recent call last)
File /opt/ros/semantic_world-venv/lib/python3.12/site-packages/semantic_world/adapters/urdf.py:361, in URDFParser.parse_file_path(self, file_path)
try:
--> 361     from ament_index_python.packages import get_package_share_directory
   package_path = get_package_share_directory(package_name)

ModuleNotFoundError: No module named 'ament_index_python'

During handling of the above exception, another exception occurred:

ParsingError                              Traceback (most recent call last)
Cell In[6], line 13
apartment = os.path.realpath(os.path.join(root, "resources", "urdf", "kitchen.urdf"))
apartment_parser = URDFParser.from_file(apartment)
---> 13 world = apartment_parser.parse()
search_space = BoundingBox(min_x=-2, max_x=2,
                          min_y=-2, max_y=2,
                          min_z=0., max_z=2, reference_frame=world.root).as_collection()
gcs = GraphOfConvexSets.free_space_from_world(world, search_space=search_space)

File /opt/ros/semantic_world-venv/lib/python3.12/site-packages/semantic_world/adapters/urdf.py:141, in URDFParser.parse(self)
def parse(self) -> World:
   prefix = self.parsed.name
   links = [
--> 141         self.parse_link(link, PrefixedName(link.name, prefix))
       for link in self.parsed.links
   ]
   root = [link for link in links if link.name.name == self.parsed.get_root()][0]
   world = World()

File /opt/ros/semantic_world-venv/lib/python3.12/site-packages/semantic_world/adapters/urdf.py:257, in URDFParser.parse_link(self, link, parent_frame)
"""
Parses a URDF link to a link object.
:param link: The URDF link to parse.
:param parent_frame: The parent frame of the link, used for transformations of collisions and visuals.
:return: The parsed link object.
"""
name = PrefixedName(prefix=self.prefix, name=link.name)
--> 257 visuals = self.parse_geometry(link.visuals, parent_frame)
collisions = self.parse_geometry(link.collisions, parent_frame)
return Body(name=name, visual=visuals, collision=collisions)

File /opt/ros/semantic_world-venv/lib/python3.12/site-packages/semantic_world/adapters/urdf.py:342, in URDFParser.parse_geometry(self, geometry, parent_frame)
       if geom.geometry.filename is None:
           raise ValueError("Mesh geometry must have a filename.")
       res.append(
           Mesh(
               origin=origin_transform,
--> 342                 filename=self.parse_file_path(geom.geometry.filename),
               scale=Scale(*(geom.geometry.scale or (1, 1, 1))),
           )
       )
return res

File /opt/ros/semantic_world-venv/lib/python3.12/site-packages/semantic_world/adapters/urdf.py:374, in URDFParser.parse_file_path(self, file_path)
               raise ParsingError(
                   msg=f"Package '{package_name}' not found in package resolver and "
                   f"ROS is not installed."
               )
       else:
--> 374             raise ParsingError(
               msg="No ROS install found while the URDF file contains references to "
               "ROS packages."
           )
   file_path = file_path.replace("package://" + package_name, package_path)
if "file://" in file_path:

ParsingError: No ROS install found while the URDF file contains references to ROS packages.

We can now see the algebraic representation of the occupied and free space. The free space is the complement of the occupied space.

from plotly.subplots import make_subplots

fig = make_subplots(rows=1, cols=2,  specs=[[{'type': 'surface'}, {'type': 'surface'}]], subplot_titles=["Occupied Space", "Free Space"])

occupied_traces = gcs.plot_occupied_space()
fig.add_traces(occupied_traces, rows=[1 for _ in occupied_traces], cols=[1 for _ in occupied_traces])
free_traces = gcs.plot_free_space()
fig.add_traces(free_traces, rows=[1 for _ in free_traces], cols=[2 for _ in free_traces])
fig.show()

Now let’s look at the connectivity of the entire world!

gcs.draw()

We can see that all spaces are somehow reachable from everywhere besides one isolated region! Amazing! This allows the accessing of locations using a sequence of local problems put together in an overarching trajectory! Finally, let’s find a way from here to there:

start = Point3(-0.75, 0, 1.15)
goal = Point3(0.75, 0, 1.15)
path = gcs.path_from_to(start, goal)
print("A potential path is", [(point.x, point.y, point.z) for point in path])

Known limitations and potential improvements are:

The connectivity graph currently calculates its edges by using an approximation to adjacent surfaces. This can be improved by an exact calculation.
The path is generated through the center points of the connection boxes. This is perhaps not optimal
The path is chosen by taking the shortest (meaning the least amount of edges) path. This is not necessarily the best path