Add support for writing systems in Python (#2081)

Creates a C++ system that is able to load a python module and pass along the System interface calls, such as Configure, PreUpdate, etc. The convention is pretty simple. The python module exposes a get_system function which returns an instance of a class that implements the desired System interface functions.

---------

Signed-off-by: Addisu Z. Taddese <[email protected]>

.github/ci/packages.apt

@@ -30,6 +30,8 @@ libxi-dev
 libxmu-dev
 python3-distutils
 python3-gz-math7
+python3-gz-msgs10
+python3-gz-transport13
 python3-pybind11
 python3-pytest
 python3-sdformat14

CMakeLists.txt

@@ -198,16 +198,15 @@ set(Protobuf_IMPORT_DIRS ${gz-msgs10_INCLUDE_DIRS})
 if (SKIP_PYBIND11)
   message(STATUS "SKIP_PYBIND11 set - disabling python bindings")
 else()
-  find_package(PythonLibs QUIET)
-  if (NOT PYTHONLIBS_FOUND)
+  find_package(Python3 QUIET COMPONENTS Interpreter Development)
+  if (NOT Python3_FOUND)
     GZ_BUILD_WARNING("Python is missing: Python interfaces are disabled.")
     message (STATUS "Searching for Python - not found.")
   else()
     message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.")
 
     set(PYBIND11_PYTHON_VERSION 3)
-    find_package(Python3 QUIET COMPONENTS Interpreter Development)
-    find_package(pybind11 2.2 QUIET)
+    find_package(pybind11 2.9 CONFIG QUIET)
 
     if (pybind11_FOUND)
       message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.")

examples/scripts/python_api/systems/test_system.py

@@ -0,0 +1,45 @@
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from gz.math7 import Vector3d
+from gz.sim8 import Model, Link
+import random
+
+
+class TestSystem(object):
+    def __init__(self):
+        self.id = random.randint(1, 100)
+
+    def configure(self, entity, sdf, ecm, event_mgr):
+        self.model = Model(entity)
+        self.link = Link(self.model.canonical_link(ecm))
+        print("Configured on", entity)
+        print("sdf name:", sdf.get_name())
+        self.force = sdf.get_double("force")
+        print(f"Applying {self.force} N on link {self.link.name(ecm)}")
+
+    def pre_update(self, info, ecm):
+        if info.paused:
+            return
+
+        if info.iterations % 3000 == 0:
+            print(f"{self.id} {info.real_time} pre_update")
+
+            self.link.add_world_force(
+                ecm, Vector3d(0, 0, self.force),
+                Vector3d(random.random(), random.random(), 0))
+
+
+def get_system():
+    return TestSystem()

examples/worlds/python_system_loader.sdf

@@ -0,0 +1,106 @@
+<?xml version="1.0" ?>
+<!--
+  Gazebo python system loader example
+
+  Before running this, add `examples/scripts/python_api/systems` to your
+  GZ_SIM_SYSTEM_PLUGIN_PATH
+
+```
+  export GZ_SIM_SYSTEM_PLUGIN_PATH=path_to_gazebo/examples/scripts/python_api/systems
+```
+
+If you're building with Colcon, you'll also need to put `install/lib/python` in
+your PYTHONPATH
+
+```
+  export PYTHONPATH=path_to_colcon_workspace/install/lib/python:$PYTHONPATH
+```
+-->
+<sdf version="1.6">
+  <world name="python_system">
+
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="box">
+      <pose>0 0 0.5 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>0.16666</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.16666</iyy>
+            <iyz>0</iyz>
+            <izz>0.16666</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+      <plugin filename="gz-sim-python-system-loader-system" name="gz::sim::systems::PythonSystemLoader">
+        <module_name>test_system</module_name>
+        <force>3000</force>
+      </plugin>
+    </model>
+
+  </world>
+</sdf>

include/gz/sim/EventManager.hh

@@ -56,6 +56,9 @@ namespace gz
       /// \brief Constructor
       public: EventManager() = default;
 
+      public: EventManager(const EventManager &) = delete;
+      public: EventManager &operator=(const EventManager &) = delete;
+
       /// \brief Destructor
       public: ~EventManager() = default;
 

python/CMakeLists.txt

@@ -63,6 +63,8 @@ target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE
   gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
 )
 
+set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES CXX_VISIBILITY_PRESET "hidden")
+
 target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE
   BINDINGS_MODULE_NAME=${BINDINGS_MODULE_NAME})
 
@@ -84,17 +86,6 @@ target_link_libraries(${GZ_COMMON_BINDINGS_MODULE_NAME} PRIVATE
 configure_build_install_location(${BINDINGS_MODULE_NAME})
 configure_build_install_location(${GZ_COMMON_BINDINGS_MODULE_NAME})
 
-if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
-  # Workaround for Clang and pybind11 on Focal
-  # https://github.com/pybind/pybind11/issues/1604
-  # Resolved by newer versions of pybind11
-  if(${pybind11_VERSION} VERSION_LESS "2.4.4")
-    target_compile_options(common PRIVATE -fsized-deallocation)
-    target_compile_options(sim PRIVATE -fsized-deallocation)
-  endif()
-endif()
-
-
 if (BUILD_TESTING)
   set(python_tests
     actor_TEST

python/test/gz_test_deps/msgs.py

@@ -0,0 +1,3 @@
+import sys
+import gz.msgs10
+sys.modules["gz_test_deps.msgs"] = gz.msgs10

python/test/gz_test_deps/transport.py

@@ -0,0 +1 @@
+from gz.transport13 import *

python/test/link_TEST.py

@@ -18,7 +18,7 @@
 
 from gz_test_deps.common import set_verbosity
 from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity
-from gz_test_deps.math import Matrix3d, Vector3d
+from gz_test_deps.math import Matrix3d, Vector3d, Pose3d
 
 class TestModel(unittest.TestCase):
     post_iterations = 0
@@ -59,9 +59,8 @@ def on_pre_udpate_cb(_info, _ecm):
             # Visuals Test
             self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test'))
             self.assertEqual(1, link.visual_count(_ecm))
-            # World Pose Test
-            self.assertEqual(None, link.world_pose(_ecm))
-            self.assertEqual(None, link.world_inertial_pose(_ecm))
+            # World Inertial Pose Test.
+            self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm))
             # World Velocity Test
             self.assertEqual(None, link.world_linear_velocity(_ecm))
             self.assertEqual(None, link.world_angular_velocity(_ecm))

python/test/plugins/test_model_system.py

@@ -0,0 +1,74 @@
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+from os.path import dirname, realpath
+import threading
+
+# Add "../__file__" to sys.path to get gz_test_deps
+sys.path.append(dirname(dirname(realpath(__file__))))
+
+from gz_test_deps.sim import Model
+from gz_test_deps.transport import Node
+from gz_test_deps.msgs.pose_pb2 import Pose
+from gz_test_deps.msgs.clock_pb2 import Clock
+
+
+# Test system to be used with test/integration/python_system_loader.cc
+class TestModelSystem(object):
+    def __init__(self):
+        self.node = Node()
+        self.has_been_reset = False
+        self.lock = threading.Lock()
+        self.sim_time_from_clock = None
+
+    def configure(self, entity, sdf, ecm, event_mgr):
+        self.model = Model(entity)
+        if not self.model.valid(ecm):
+            raise RuntimeError(f"Model {entity} is invalid")
+
+        self.target_pose = sdf.get_pose("target_pose")
+        self.reset_pose = sdf.get_pose("reset_pose")
+        self.pub = self.node.advertise(f"{self.model.name(ecm)}/pose", Pose)
+        self.sub = self.node.subscribe(Clock, "/clock", self.clock_cb)
+
+    def pre_update(self, info, ecm):
+        if info.paused or self.has_been_reset:
+            return
+
+        self.model.set_world_pose_cmd(ecm, self.target_pose)
+
+    def post_update(self, info, ecm):
+        msg = Pose()
+        msg.position.x = self.target_pose.x()
+        msg.position.y = self.target_pose.y()
+        msg.position.z = self.target_pose.z()
+        with self.lock:
+            if self.sim_time_from_clock is not None:
+                stamp = msg.header.stamp
+                stamp.sec = self.sim_time_from_clock.sec
+                stamp.nsec = self.sim_time_from_clock.nsec
+        self.pub.publish(msg)
+
+    def reset(self, info, ecm):
+        self.model.set_world_pose_cmd(ecm, self.reset_pose)
+        self.has_been_reset = True
+
+    def clock_cb(self, msg):
+        with self.lock:
+            self.sim_time_from_clock = msg.sim
+
+
+def get_system():
+    return TestModelSystem()

src/CMakeLists.txt

@@ -214,6 +214,12 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
   PRIVATE
   gz-plugin${GZ_PLUGIN_VER}::loader
 )
+
+if (pybind11_FOUND)
+  target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE pybind11::embed)
+  target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC -DHAVE_PYBIND11)
+endif()
+
 if (UNIX AND NOT APPLE)
   target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
     PRIVATE stdc++fs)