Visualizing Worlds

This tutorial explains you how to visualize a world. There are two recommended ways of doing it. One light weight way through RVIZ2 and a more heavy weight way through simulation with multiverse. Let’s load a world first to get started.

import logging
import os

from importlib.resources import files
from pathlib import Path

from semantic_digital_twin.adapters.urdf import URDFParser 

logging.disable(logging.CRITICAL)
apartment = os.path.join(Path(files("semantic_digital_twin")).parent.parent, "resources", "urdf", "apartment.urdf")
world = URDFParser.from_file(apartment).parse()

Unknown tag "material" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]

---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
Cell In[1], line 11
      7 from semantic_digital_twin.adapters.urdf import URDFParser
      8 
      9 logging.disable(logging.CRITICAL)
     10 apartment = os.path.join(Path(files("semantic_digital_twin")).parent.parent, "resources", "urdf", "apartment.urdf")
---> 11 world = URDFParser.from_file(apartment).parse()

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

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:255, in WorldModelUpdateContextManager.__exit__(self, exc_type, exc_val, exc_tb)
    253 try:
    254     if exc_type is None:
--> 255         self.world._notify_model_change(
    256             publish_changes=self.publish_changes
    257         )
    258         run_pending_publications = True
    259 finally:

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:1792, in World._notify_model_change(self, publish_changes, **kwargs)
   1786 def _notify_model_change(self, publish_changes: bool = True, **kwargs) -> None:
   1787     """
   1788     Notifies the system of a model change and updates the necessary states, caches,
   1789     and forward kinematics expressions while also triggering registered callbacks
   1790     for model changes.
   1791     """
-> 1792     self._model_manager.update_model_version_and_notify_callbacks(
   1793         publish_changes=publish_changes, **kwargs
   1794     )
   1795     self.notify_state_change(publish_changes=publish_changes, **kwargs)
   1797     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:404, in WorldModelManager.update_model_version_and_notify_callbacks(self, **kwargs)
    402 self.version += 1
    403 for callback in list(self.model_change_callbacks):
--> 404     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:115, in ModelChangeCallback.notify_model_change(self, **kwargs)
    113 def notify_model_change(self, **kwargs):
    114     if not self._is_paused:
--> 115         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:337, in BulletCollisionDetector.sync_world_model(self)
    335     return
    336 for body in self._world.bodies_with_collision:
--> 337     self.add_body(body)
    338 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:352, in BulletCollisionDetector.add_body(self, body)
    351 def add_body(self, body: Body):
--> 352     o = create_shape_from_body(body=body, mesh_decomposer=self.mesh_decomposer)
    353     self.kineverse_world.add_collision_object(o)
    354     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:173, in create_shape_from_body(body, mesh_decomposer)
    171 shapes = []
    172 for collision_id, geometry in enumerate(body.collision):
--> 173     shape = create_shape_from_geometry(
    174         geometry=geometry, mesh_decomposer=mesh_decomposer
    175     )
    176     link_T_geometry = bullet.Transform.from_np(geometry.origin.to_np())
    177     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:147, in create_shape_from_geometry(geometry, mesh_decomposer)
    142     shape = create_cylinder_shape(
    143         diameter=geometry.width, height=geometry.height
    144     )
    146 case Mesh():
--> 147     shape = load_convex_mesh_shape(
    148         mesh=geometry,
    149         single_shape=False,
    150         scale=geometry.scale,
    151         mesh_decomposer=mesh_decomposer,
    152     )
    154 case _:
    155     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:213, in load_convex_mesh_shape(mesh, single_shape, scale, mesh_decomposer)
    203 """
    204 Loads a convex mesh shape from a mesh.
    205 
   (...)    210 :return: the bullet convex shape.
    211 """
    212 if not mesh.mesh.is_convex and mesh_decomposer is not None:
--> 213     obj_pkg_filename = convert_to_decomposed_obj_and_save_in_tmp(
    214         mesh=mesh, mesh_decomposer=mesh_decomposer
    215     )
    216 else:
    217     obj_pkg_filename = mesh.filename

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:260, in convert_to_decomposed_obj_and_save_in_tmp(mesh, mesh_decomposer, cache_dir, log_path)
    258 if not trimesh_obj.is_convex and mesh_decomposer is not None:
    259     with suppress_stdout_stderr():
--> 260         mesh_decomposer.apply_to_mesh_and_save(mesh, obj_file_name)
    261     logging.info(f'Saved convex decomposition to "{obj_file_name}".')
    262 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:3031, in Trimesh.convex_decomposition(self, **kwargs)
   3018 def convex_decomposition(self, **kwargs) -> list["Trimesh"]:
   3019     """
   3020     Compute an approximate convex decomposition of a mesh
   3021     using `pip install pyVHACD`.
   (...)   3027     **kwargs : VHACD keyword arguments
   3028     """
   3029     return [
   3030         Trimesh(**kwargs)
-> 3031         for kwargs in decomposition.convex_decomposition(self, **kwargs)
   3032     ]

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: 

For the RVIZ2 way, ROS2 and the TFPublisher is needed. A caveat of this approach is that you have to manage the lifecycle of a ROS2 node yourself. We recommend to put the spinning into sperate threads and just shutdown the thread when exiting the system.

from semantic_digital_twin.adapters.ros.tf_publisher import TFPublisher
from semantic_digital_twin.adapters.ros.visualization.viz_marker import VizMarkerPublisher
import threading
import rclpy
rclpy.init()

node = rclpy.create_node("semantic_digital_twin")
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
thread.start()

tf_publisher = TFPublisher(_world=world, node=node)
viz = VizMarkerPublisher(_world=world, node=node)

When you want to stop visualizing, you have to stop the visualizer and afterwards clean up ROS2.

node.destroy_node()
rclpy.shutdown()

The world can also be visualized directly through a running simulation. Although this approach is computationally heavier, it provides the important advantage of enabling interaction with the environment. In addition to visualization, the physics engine can be used to simulate dynamics, contacts, and collisions. Further details are provided in the physics simulators section.

If you have followed the guide until here, you have probably noticed that we have used the RayTracer to visualize the world a few times. This is a convenient way of visualizing a world inside a notebook, like in these guides, but it is not recommended for normal usage.