Graph of Convex Sets

This notebook walks through the way the semantic digital twin 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 digital twin 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_digital_twin.world_description.geometry import Box, Scale, Color
from semantic_digital_twin.world_description.shape_collection import ShapeCollection, BoundingBoxCollection
from semantic_digital_twin.world_description.world_entity import Body
from semantic_digital_twin.datastructures.prefixed_name import PrefixedName
from semantic_digital_twin.spatial_types import HomogeneousTransformationMatrix
from semantic_digital_twin.world import World

box_world = World()

with box_world.modify_world():
    box = Body(name=PrefixedName("box"), collision=ShapeCollection([Box(scale=Scale(0.5, 0.5, 0.5),
                                                        color=Color(1., 1., 1., 1.),
                                                        origin=HomogeneousTransformationMatrix.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_digital_twin.world_description.graph_of_convex_sets.boxes import VolumetricGraphOfBoundingBoxes
from semantic_digital_twin.world_description.geometry import VolumetricBoundingBox

search_space = BoundingBoxCollection([VolumetricBoundingBox(min_x=-1, max_x=1,
                           min_y=-1, max_y=1,
                           min_z=0.1, max_z=0.2, origin=HomogeneousTransformationMatrix(reference_frame=box_world.root))], box_world.root)
                           
graph_of_bounding_boxes = VolumetricGraphOfBoundingBoxes.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(graph_of_bounding_boxes.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.

graph_of_bounding_boxes.draw()

Let’s use graph theory to find a path!

from semantic_digital_twin.spatial_types import Point3

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

A potential path is [(Scalar(-0.75), Scalar(0)), (Scalar(0.25), Scalar(0.625)), (Scalar(0.75), Scalar(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 importlib.resources import files
from pathlib import Path
import semantic_digital_twin
from semantic_digital_twin.adapters.urdf import URDFParser

apartment = os.path.realpath(os.path.join(Path(files("semantic_digital_twin")).parent.parent, "resources", "urdf", "kitchen.urdf"))

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

search_space = BoundingBoxCollection([VolumetricBoundingBox(min_x=-2, max_x=2,
                           min_y=-2, max_y=2,
                           min_z=0., max_z=2, origin=HomogeneousTransformationMatrix(reference_frame=world.root))], world.root)
graph_of_bounding_boxes = VolumetricGraphOfBoundingBoxes.free_space_from_world(world, search_space=search_space)

Scalar element defined multiple times: limit

---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
Cell In[6], line 10
      6 
      7 apartment = os.path.realpath(os.path.join(Path(files("semantic_digital_twin")).parent.parent, "resources", "urdf", "kitchen.urdf"))
      8 
      9 apartment_parser = URDFParser.from_file(apartment)
---> 10 world = apartment_parser.parse()
     11 
     12 search_space = BoundingBoxCollection([VolumetricBoundingBox(min_x=-2, max_x=2,
     13                            min_y=-2, max_y=2,

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/urdf.py:194, in URDFParser.parse(self)
    192 world = World()
    193 world.name = self.prefix
--> 194 with world.modify_world():
    195     world.add_kinematic_structure_entity(root)
    196     main_joints = []

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:314, in WorldModelUpdateContextManager.__exit__(self, exc_type, exc_val, exc_tb)
    312 try:
    313     if exc_type is None:
--> 314         self.world._notify_model_change(
    315             publish_changes=self.publish_changes
    316         )
    317         run_pending_publications = True
    318 finally:

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:2129, in World._notify_model_change(self, publish_changes, **kwargs)
   2123 def _notify_model_change(self, publish_changes: bool = True, **kwargs) -> None:
   2124     """
   2125     Notifies the system of a model change and updates the necessary states, caches,
   2126     and forward kinematics expressions while also triggering registered callbacks
   2127     for model changes.
   2128     """
-> 2129     self._model_manager.update_model_version_and_notify_callbacks(
   2130         publish_changes=publish_changes, **kwargs
   2131     )
   2132     self.notify_state_change(
   2133         publish_changes=publish_changes, force_republish=True, **kwargs
   2134     )
   2136     for callback in list(self.state.state_change_callbacks):

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:523, in WorldModelManager.update_model_version_and_notify_callbacks(self, **kwargs)
    521 self.version += 1
    522 for callback in list(self.model_change_callbacks):
--> 523     callback.notify_model_change(**kwargs)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/callbacks/callback.py:171, in ModelChangeCallback.notify_model_change(self, **kwargs)
    169 def notify_model_change(self, **kwargs):
    170     if not self._is_paused:
--> 171         self.on_model_change(**kwargs)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/collision_detector.py:142, in CollisionDetectorModelUpdater.on_model_change(self, **kwargs)
    140 if self._world.is_empty():
    141     return
--> 142 self.collision_detector.sync_world_model()
    143 self.compile_collision_fks()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:390, in BulletCollisionDetector.sync_world_model(self)
    388     return
    389 for body in self._world.bodies_with_collision:
--> 390     self.add_body(body)
    391 self._ordered_bullet_objects = list(self.body_to_bullet_object.values())

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:405, in BulletCollisionDetector.add_body(self, body)
    404 def add_body(self, body: Body):
--> 405     o = create_shape_from_body(body=body, mesh_decomposer=self.mesh_decomposer)
    406     self.kineverse_world.add_collision_object(o)
    407     self.body_to_bullet_object[body] = o

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:226, in create_shape_from_body(body, mesh_decomposer)
    224 shapes = []
    225 for collision_id, geometry in enumerate(body.collision):
--> 226     shape = create_shape_from_geometry(
    227         geometry=geometry, mesh_decomposer=mesh_decomposer
    228     )
    229     link_T_geometry = bullet.Transform.from_np(geometry.origin.to_np())
    230     shapes.append((link_T_geometry, shape))

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:199, in create_shape_from_geometry(geometry, mesh_decomposer)
    194     shape = create_cylinder_shape(
    195         diameter=geometry.width, height=geometry.height
    196     )
    198 case Mesh():
--> 199     shape = load_convex_mesh_shape(
    200         mesh=geometry,
    201         single_shape=False,
    202         scale=geometry.scale,
    203         mesh_decomposer=mesh_decomposer,
    204     )
    206 case _:
    207     raise NotImplementedError()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:266, in load_convex_mesh_shape(mesh, single_shape, scale, mesh_decomposer)
    256 """
    257 Loads a convex mesh shape from a mesh.
    258 
   (...)    263 :return: the bullet convex shape.
    264 """
    265 if not mesh.mesh.is_convex and mesh_decomposer is not None:
--> 266     obj_pkg_filename = convert_to_decomposed_obj_and_save_in_tmp(
    267         mesh=mesh, mesh_decomposer=mesh_decomposer
    268     )
    269 else:
    270     obj_pkg_filename = str(mesh.local_file)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:313, in convert_to_decomposed_obj_and_save_in_tmp(mesh, mesh_decomposer, cache_dir, log_path)
    311 if not trimesh_obj.is_convex and mesh_decomposer is not None:
    312     with suppress_stdout_stderr():
--> 313         mesh_decomposer.apply_to_mesh_and_save(mesh, obj_file_name)
    314     logging.info(f'Saved convex decomposition to "{obj_file_name}".')
    315 else:

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/pipeline/mesh_decomposition/vhacd.py:132, in VHACDMeshDecomposer.apply_to_mesh_and_save(self, mesh, output_path)
    131 def apply_to_mesh_and_save(self, mesh: Mesh, output_path: str) -> str:
--> 132     parts = self.apply_to_mesh(mesh)
    133     trimesh.Scene([p.mesh for p in parts]).export(output_path, file_type="obj")
    134     return output_path

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/pipeline/mesh_decomposition/vhacd.py:113, in VHACDMeshDecomposer.apply_to_mesh(self, mesh)
    112 def apply_to_mesh(self, mesh: Mesh) -> List[Mesh]:
--> 113     decomposed = mesh.mesh.convex_decomposition(
    114         maxConvexHulls=self.max_convex_hulls,
    115         resolution=self.resolution,
    116         minimumVolumePercentErrorAllowed=self.minimum_volume_percent_error_allowed,
    117         maxRecursionDepth=self.max_recursion_depth,
    118         shrinkWrap=self.shrink_wrap,
    119         fillMode=self.fill_mode.value,
    120         maxNumVerticesPerCH=self.max_vertices_per_convex_hull,
    121         asyncACD=self.asynchronous,
    122         minEdgeLength=self.min_edge_length,
    123         findBestPlane=self.find_best_plane,
    124     )
    125     new_geometry = [
    126         Mesh.from_trimesh(mesh=decomposed_part, origin=mesh.origin)
    127         for decomposed_part in decomposed
    128     ]
    129     return new_geometry

File /opt/ros/cram-env/lib/python3.12/site-packages/trimesh/base.py:3048, in Trimesh.convex_decomposition(self, **kwargs)
   3035 def convex_decomposition(self, **kwargs) -> list["Trimesh"]:
   3036     """
   3037     Compute an approximate convex decomposition of a mesh
   3038     using `pip install pyVHACD`.
   (...)   3044     **kwargs : VHACD keyword arguments
   3045     """
   3046     return [
   3047         Trimesh(**kwargs)
-> 3048         for kwargs in decomposition.convex_decomposition(self, **kwargs)
   3049     ]

File /opt/ros/cram-env/lib/python3.12/site-packages/trimesh/decomposition.py:48, in convex_decomposition(mesh, **kwargs)
     37 # the faces are triangulated in a (len(face), ...vertex-index)
     38 # for vtkPolyData
     39 # i.e. so if shaped to four columns the first column is all 3
     40 faces = (
     41     np.column_stack((np.ones(len(mesh.faces), dtype=np.int64) * 3, mesh.faces))
     42     .ravel()
     43     .astype(np.uint32)
     44 )
     46 return [
     47     {"vertices": v, "faces": f}
---> 48     for v, f in compute_vhacd(mesh.vertices, faces, **kwargs)
     49 ]

KeyboardInterrupt: 

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 = graph_of_bounding_boxes.plot_occupied_space()
fig.add_traces(occupied_traces, rows=[1 for _ in occupied_traces], cols=[1 for _ in occupied_traces])
free_traces = graph_of_bounding_boxes.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!

graph_of_bounding_boxes.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, reference_frame=world.root)
goal = Point3(0.75, 0, 1.15, reference_frame=world.root)
path = graph_of_bounding_boxes.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