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).create_world()

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).create_world()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/urdf.py:238, in URDFParser.create_world(self)
    236 world = World()
    237 world.name = self.prefix
--> 238 with world.modify_world():
    239     world.add_kinematic_structure_entity(root)
    240     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:2109, in World._notify_model_change(self, publish_changes, **kwargs)
   2103 def _notify_model_change(self, publish_changes: bool = True, **kwargs) -> None:
   2104     """
   2105     Notifies the system of a model change and updates the necessary states, caches,
   2106     and forward kinematics expressions while also triggering registered callbacks
   2107     for model changes.
   2108     """
-> 2109     self._model_manager.update_model_version_and_notify_callbacks(
   2110         publish_changes=publish_changes, **kwargs
   2111     )
   2112     self.notify_state_change(
   2113         publish_changes=publish_changes, force_republish=True, **kwargs
   2114     )
   2116     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:521, in WorldModelManager.update_model_version_and_notify_callbacks(self, **kwargs)
    519 self.version += 1
    520 for callback in list(self.model_change_callbacks):
--> 521     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:170, in ModelChangeCallback.notify_model_change(self, **kwargs)
    168 def notify_model_change(self, **kwargs):
    169     if not self._is_paused:
--> 170         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:452, in BulletCollisionDetector.sync_world_model(self)
    450     return
    451 for body in self._world.bodies_with_collision:
--> 452     self.add_body(body)
    453 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:467, in BulletCollisionDetector.add_body(self, body)
    466 def add_body(self, body: Body):
--> 467     o = create_shape_from_body(body=body, mesh_decomposer=self.mesh_decomposer)
    468     self.kineverse_world.add_collision_object(o)
    469     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:239, in create_shape_from_body(body, mesh_decomposer)
    237 shapes = []
    238 for collision_id, geometry in enumerate(body.collision):
--> 239     shape = create_shape_from_geometry(
    240         geometry=geometry, mesh_decomposer=mesh_decomposer
    241     )
    242     link_T_geometry = bullet.Transform.from_np(geometry.origin.to_np())
    243     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:212, in create_shape_from_geometry(geometry, mesh_decomposer)
    207     shape = create_cylinder_shape(
    208         diameter=geometry.width, height=geometry.height
    209     )
    211 case Mesh():
--> 212     shape = load_convex_mesh_shape(
    213         mesh=geometry,
    214         single_shape=False,
    215         scale=geometry.scale,
    216         mesh_decomposer=mesh_decomposer,
    217     )
    219 case _:
    220     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:279, in load_convex_mesh_shape(mesh, single_shape, scale, mesh_decomposer)
    269 """
    270 Loads a convex mesh shape from a mesh.
    271 
   (...)    276 :return: the bullet convex shape.
    277 """
    278 if not mesh.mesh.is_convex and mesh_decomposer is not None:
--> 279     obj_pkg_filename = convert_to_decomposed_obj_and_save_in_tmp(
    280         mesh=mesh, mesh_decomposer=mesh_decomposer
    281     )
    282 elif mesh.local_file.suffix.removeprefix(".").lower() in BulletMeshFileFormat:
    283     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:331, in convert_to_decomposed_obj_and_save_in_tmp(mesh, mesh_decomposer, cache_dir, log_path)
    328         with suppress_stdout_stderr():
    329             mesh_decomposer.apply_to_mesh_and_save(mesh, unfinished_file_name)
--> 331 return save_in_cache(
    332     obj_file_name=str(cache_dir / f"{trimesh_quantized_hash(trimesh_obj)}.obj"),
    333     write=write,
    334 )

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:380, in save_in_cache(obj_file_name, write)
    376 with tempfile.NamedTemporaryFile(
    377     dir=os.path.dirname(obj_file_name), suffix=".obj", delete=False
    378 ) as unfinished_file:
    379     pass
--> 380 write(unfinished_file.name)
    381 logging.info(f'Saved obj to "{obj_file_name}".')
    382 os.replace(unfinished_file.name, obj_file_name)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:329, in convert_to_decomposed_obj_and_save_in_tmp.<locals>.write(unfinished_file_name)
    327 if not trimesh_obj.is_convex and mesh_decomposer is not None:
    328     with suppress_stdout_stderr():
--> 329         mesh_decomposer.apply_to_mesh_and_save(mesh, unfinished_file_name)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/pipeline/mesh_decomposition/vhacd.py:131, in VHACDMeshDecomposer.apply_to_mesh_and_save(self, mesh, output_path)
    130 def apply_to_mesh_and_save(self, mesh: Mesh, output_path: str) -> str:
--> 131     parts = self.apply_to_mesh(mesh)
    132     trimesh.Scene([p.mesh for p in parts]).export(output_path, file_type="obj")
    133     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:112, in VHACDMeshDecomposer.apply_to_mesh(self, mesh)
    111 def apply_to_mesh(self, mesh: Mesh) -> list[Mesh]:
--> 112     decomposed = mesh.mesh.convex_decomposition(
    113         maxConvexHulls=self.max_convex_hulls,
    114         resolution=self.resolution,
    115         minimumVolumePercentErrorAllowed=self.minimum_volume_percent_error_allowed,
    116         maxRecursionDepth=self.max_recursion_depth,
    117         shrinkWrap=self.shrink_wrap,
    118         fillMode=self.fill_mode.value,
    119         maxNumVerticesPerCH=self.max_vertices_per_convex_hull,
    120         asyncACD=self.asynchronous,
    121         minEdgeLength=self.min_edge_length,
    122         findBestPlane=self.find_best_plane,
    123     )
    124     new_geometry = [
    125         Mesh.from_trimesh(mesh=decomposed_part, origin=mesh.origin)
    126         for decomposed_part in decomposed
    127     ]
    128     return new_geometry

File /opt/ros/cram-env/lib/python3.12/site-packages/trimesh/base.py:3075, in Trimesh.convex_decomposition(self, **kwargs)
   3062 def convex_decomposition(self, **kwargs) -> list["Trimesh"]:
   3063     """
   3064     Compute an approximate convex decomposition of a mesh
   3065     using `pip install pyVHACD`.
   (...)   3071     **kwargs : VHACD keyword arguments
   3072     """
   3073     return [
   3074         Trimesh(**kwargs)
-> 3075         for kwargs in decomposition.convex_decomposition(self, **kwargs)
   3076     ]

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.