Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for writing systems in Python #2081

Merged
merged 23 commits into from
Aug 31, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .github/ci/packages.apt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 3 additions & 4 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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}.")
Expand Down
45 changes: 45 additions & 0 deletions examples/scripts/python_api/systems/test_system.py
Original file line number Diff line number Diff line change
@@ -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):
mjcarroll marked this conversation as resolved.
Show resolved Hide resolved
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()
106 changes: 106 additions & 0 deletions examples/worlds/python_system_loader.sdf
Original file line number Diff line number Diff line change
@@ -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>
3 changes: 3 additions & 0 deletions include/gz/sim/EventManager.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
13 changes: 2 additions & 11 deletions python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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})

Expand All @@ -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
Expand Down
3 changes: 3 additions & 0 deletions python/test/gz_test_deps/msgs.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
import sys
import gz.msgs10
sys.modules["gz_test_deps.msgs"] = gz.msgs10
1 change: 1 addition & 0 deletions python/test/gz_test_deps/transport.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
from gz.transport13 import *
7 changes: 3 additions & 4 deletions python/test/link_TEST.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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))
Expand Down
74 changes: 74 additions & 0 deletions python/test/plugins/test_model_system.py
Original file line number Diff line number Diff line change
@@ -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()
6 changes: 6 additions & 0 deletions src/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
Loading