From 0e175e5e7c7722fafd8e851b123494e4a9404304 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 11 Jan 2026 18:37:35 +0000 Subject: [PATCH 01/19] Fix include headers guards. Fix linking with Chrono. Refactor examples --- CMakeLists.txt | 15 ++++---- config.h.in | 10 ++++-- examples/CMakeLists.txt | 15 ++++---- examples/example_1/CMakeLists.txt | 8 ++--- examples/example_10/CMakeLists.txt | 9 ++--- examples/example_11/CMakeLists.txt | 9 ++--- examples/example_12/CMakeLists.txt | 10 +++--- examples/example_13/CMakeLists.txt | 9 ++--- .../CMakeLists.txt | 11 +++--- .../arduino_example.cpp | 0 .../{arduino => example_16}/CMakeLists.txt | 9 ++--- .../example_16.cpp} | 0 .../{mqtt_exe_1 => example_17}/CMakeLists.txt | 9 ++--- .../extended_kalman_filter.py | 0 .../mqtt_example_1.cpp | 0 examples/{mqtt_exe_1 => example_17}/plot.py | 0 .../{mqtt_exe_2 => example_18}/CMakeLists.txt | 9 ++--- .../{mqtt_exe_2 => example_18}/camera_feed.py | 0 .../mqtt_example_2.cpp | 0 .../{mqtt_exe_3 => example_19}/CMakeLists.txt | 9 ++--- examples/{mqtt_exe_3 => example_19}/feed.py | 0 .../mqtt_example_3.cpp | 0 examples/example_2/CMakeLists.txt | 8 ++--- examples/example_3/CMakeLists.txt | 9 ++--- examples/example_4/CMakeLists.txt | 9 ++--- examples/example_5/CMakeLists.txt | 9 ++--- examples/example_7/CMakeLists.txt | 8 ++--- examples/example_8/CMakeLists.txt | 8 ++--- examples/example_9/CMakeLists.txt | 8 ++--- src/bitrl/bitrl_consts.h | 4 +++ .../boards/arduino/arduino_connector_base.h | 2 +- .../arduino/arduino_connector_usb_base.cpp | 2 +- .../arduino/arduino_connector_usb_base.h | 2 +- .../arduino/arduino_connector_wifi_base.cpp | 2 +- .../arduino/arduino_connector_wifi_base.h | 2 +- src/bitrl/rigid_bodies/quadrotor.h | 34 ------------------- version.h.in | 12 +++---- 37 files changed, 120 insertions(+), 131 deletions(-) rename examples/{arduino_wifi => example_15}/CMakeLists.txt (54%) rename examples/{arduino => example_15}/arduino_example.cpp (100%) rename examples/{arduino => example_16}/CMakeLists.txt (67%) rename examples/{arduino_wifi/arduino_wifi_example.cpp => example_16/example_16.cpp} (100%) rename examples/{mqtt_exe_1 => example_17}/CMakeLists.txt (60%) rename examples/{mqtt_exe_1 => example_17}/extended_kalman_filter.py (100%) rename examples/{mqtt_exe_1 => example_17}/mqtt_example_1.cpp (100%) rename examples/{mqtt_exe_1 => example_17}/plot.py (100%) rename examples/{mqtt_exe_2 => example_18}/CMakeLists.txt (60%) rename examples/{mqtt_exe_2 => example_18}/camera_feed.py (100%) rename examples/{mqtt_exe_2 => example_18}/mqtt_example_2.cpp (100%) rename examples/{mqtt_exe_3 => example_19}/CMakeLists.txt (60%) rename examples/{mqtt_exe_3 => example_19}/feed.py (100%) rename examples/{mqtt_exe_3 => example_19}/mqtt_example_3.cpp (100%) delete mode 100644 src/bitrl/rigid_bodies/quadrotor.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e9da5f3..9a2e439f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,6 @@ MESSAGE(STATUS "Using CMake ${CMAKE_VERSION}") SET(BITRL_VERSION_MAJOR 2) SET(BITRL_VERSION_MINOR 2) SET(BITRL_VERSION_PATCH 4) - SET(BITRL_VERSION "${BITRL_VERSION_MAJOR}.${BITRL_VERSION_MINOR}.${BITRL_VERSION_PATCH}") MESSAGE(STATUS "bitrllib Version ${BITRL_VERSION}") @@ -25,30 +24,32 @@ IF(COMMAND cmake_policy) CMAKE_POLICY(SET CMP0048 NEW) ENDIF(COMMAND cmake_policy) -OPTION(ENABLE_TESTS OFF) + OPTION(ENABLE_WEBOTS OFF) # configure packages SET(ENABLE_EXAMPLES ON) -SET(ENABLE_CHRONO OFF) +SET(ENABLE_TESTS OFF) +SET(ENABLE_CHRONO ON) SET(ENABLE_OPENCV ON) SET(ENABLE_MQTT ON) -SET(ENABLE_DOCS ON) +SET(BITRL_LOG ON) # build options SET(CMAKE_BUILD_TYPE "Release") SET(CMAKE_CXX_STANDARD 20) SET(CMAKE_CXX_STANDARD_REQUIRED True) SET(CMAKE_LINKER_FLAGS "-pthread") +SET(ROBOTS_DATA_DIR ${PROJECT_SOURCE_DIR}/robots) IF(CMAKE_BUILD_TYPE STREQUAL "Debug") SET(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/install/dbg) - SET(CMAKE_CXX_FLAGS "-g -fPIC") # -Wall -Wextra") + SET(CMAKE_CXX_FLAGS "-g -fPIC -DBOOST_LOG_DYN_LINK") # -Wall -Wextra") SET(BITRL_DEBUG ON) ELSE(CMAKE_BUILD_TYPE STREQUAL "Release") SET(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/install/opt) - SET(CMAKE_CXX_FLAGS "-O2 -fPIC") + SET(CMAKE_CXX_FLAGS "-O2 -fPIC -DBOOST_LOG_DYN_LINK") SET(BITRL_DEBUG OFF) ENDIF() @@ -70,6 +71,7 @@ ENDIF() IF(Boost_FOUND) MESSAGE( STATUS "Found needed Boost C++ library.") SET(Boost_USE_SHARED_LIBS ON) + SET(Boost_USE_STATIC_LIBS ON) ELSE() MESSAGE( FATAL_ERROR "Boost C++ library is required but not found.") ENDIF() @@ -152,6 +154,7 @@ FILE(GLOB SRCS src/bitrl/*.cpp src/bitrl/envs/gymnasium/box2d/*.cpp src/bitrl/boards/arduino/*.cpp src/bitrl/rigid_bodies/*.cpp + src/bitrl/rigid_bodies/chrono_robots/*.cpp src/bitrl/rigid_bodies/webots_robots/*.cpp src/bitrl/dynamics/*.cpp src/bitrl/utils/*.cpp diff --git a/config.h.in b/config.h.in index 5ffc2626..b6e3d1c8 100755 --- a/config.h.in +++ b/config.h.in @@ -1,9 +1,15 @@ -#ifndef GYMFCPP_CONFIG_H -#define GYMFCPP_CONFIG_H +#ifndef BITRL_CONFIG_H +#define BITRL_CONFIG_H + +/*Path to robots description*/ +#cmakedefine ROBOTS_DATA_DIR "@ROBOTS_DATA_DIR@" /*DEBUG*/ #cmakedefine BITRL_DEBUG +/*Flag indicating that the library should log various messages*/ +#cmakedefine BITRL_LOG + /*Use Webots*/ #cmakedefine BITRL_WEBOTS diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8c6d510a..10d40f99 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -35,22 +35,19 @@ ADD_SUBDIRECTORY(example_9) ADD_SUBDIRECTORY(example_10) ADD_SUBDIRECTORY(example_11) ADD_SUBDIRECTORY(example_12) -ADD_SUBDIRECTORY(arduino) -ADD_SUBDIRECTORY(arduino_wifi) +ADD_SUBDIRECTORY(example_14) +ADD_SUBDIRECTORY(example_15) +ADD_SUBDIRECTORY(example_16) IF(BITRL_MQTT) - ADD_SUBDIRECTORY(mqtt_exe_1) - ADD_SUBDIRECTORY(mqtt_exe_3) + ADD_SUBDIRECTORY(example_17) + ADD_SUBDIRECTORY(example_19) IF(BITRL_OPENCV) - ADD_SUBDIRECTORY(mqtt_exe_2) + ADD_SUBDIRECTORY(example_18) ENDIF () ENDIF() IF(BITRL_WEBOTS) ADD_SUBDIRECTORY(webots/world_1) ENDIF() - -#IF(BITRL_CHRONO) -# ADD_SUBDIRECTORY(chrono_examples/chrono_exe_1) -#ENDIF() diff --git a/examples/example_1/CMakeLists.txt b/examples/example_1/CMakeLists.txt index ae89b935..3c107826 100644 --- a/examples/example_1/CMakeLists.txt +++ b/examples/example_1/CMakeLists.txt @@ -5,10 +5,10 @@ SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_10/CMakeLists.txt b/examples/example_10/CMakeLists.txt index 7f8b753b..dfa3f64d 100644 --- a/examples/example_10/CMakeLists.txt +++ b/examples/example_10/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_10) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_11/CMakeLists.txt b/examples/example_11/CMakeLists.txt index ece9a57a..0c917b44 100644 --- a/examples/example_11/CMakeLists.txt +++ b/examples/example_11/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_11) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_12/CMakeLists.txt b/examples/example_12/CMakeLists.txt index c195b4cc..6e766313 100644 --- a/examples/example_12/CMakeLists.txt +++ b/examples/example_12/CMakeLists.txt @@ -4,18 +4,20 @@ SET(EXECUTABLE example_12) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () + IF(BITRL_WEBOTS) TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_13/CMakeLists.txt b/examples/example_13/CMakeLists.txt index 04904305..f89731a1 100644 --- a/examples/example_13/CMakeLists.txt +++ b/examples/example_13/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_13) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/arduino_wifi/CMakeLists.txt b/examples/example_15/CMakeLists.txt similarity index 54% rename from examples/arduino_wifi/CMakeLists.txt rename to examples/example_15/CMakeLists.txt index 01dc3b17..b4f8b251 100644 --- a/examples/arduino_wifi/CMakeLists.txt +++ b/examples/example_15/CMakeLists.txt @@ -1,13 +1,14 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.20) -SET(EXECUTABLE arduino_wifi_example) +SET(EXECUTABLE arduino_example) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/arduino/arduino_example.cpp b/examples/example_15/arduino_example.cpp similarity index 100% rename from examples/arduino/arduino_example.cpp rename to examples/example_15/arduino_example.cpp diff --git a/examples/arduino/CMakeLists.txt b/examples/example_16/CMakeLists.txt similarity index 67% rename from examples/arduino/CMakeLists.txt rename to examples/example_16/CMakeLists.txt index df1540f3..63c57ca9 100644 --- a/examples/arduino/CMakeLists.txt +++ b/examples/example_16/CMakeLists.txt @@ -1,13 +1,14 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.20) -SET(EXECUTABLE arduino_example) +SET(EXECUTABLE example_16) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) diff --git a/examples/arduino_wifi/arduino_wifi_example.cpp b/examples/example_16/example_16.cpp similarity index 100% rename from examples/arduino_wifi/arduino_wifi_example.cpp rename to examples/example_16/example_16.cpp diff --git a/examples/mqtt_exe_1/CMakeLists.txt b/examples/example_17/CMakeLists.txt similarity index 60% rename from examples/mqtt_exe_1/CMakeLists.txt rename to examples/example_17/CMakeLists.txt index d1835675..49b5778c 100644 --- a/examples/mqtt_exe_1/CMakeLists.txt +++ b/examples/example_17/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE mqtt_example_1) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/mqtt_exe_1/extended_kalman_filter.py b/examples/example_17/extended_kalman_filter.py similarity index 100% rename from examples/mqtt_exe_1/extended_kalman_filter.py rename to examples/example_17/extended_kalman_filter.py diff --git a/examples/mqtt_exe_1/mqtt_example_1.cpp b/examples/example_17/mqtt_example_1.cpp similarity index 100% rename from examples/mqtt_exe_1/mqtt_example_1.cpp rename to examples/example_17/mqtt_example_1.cpp diff --git a/examples/mqtt_exe_1/plot.py b/examples/example_17/plot.py similarity index 100% rename from examples/mqtt_exe_1/plot.py rename to examples/example_17/plot.py diff --git a/examples/mqtt_exe_2/CMakeLists.txt b/examples/example_18/CMakeLists.txt similarity index 60% rename from examples/mqtt_exe_2/CMakeLists.txt rename to examples/example_18/CMakeLists.txt index 1da22b2d..cd52f43c 100644 --- a/examples/mqtt_exe_2/CMakeLists.txt +++ b/examples/example_18/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE mqtt_example_2) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/mqtt_exe_2/camera_feed.py b/examples/example_18/camera_feed.py similarity index 100% rename from examples/mqtt_exe_2/camera_feed.py rename to examples/example_18/camera_feed.py diff --git a/examples/mqtt_exe_2/mqtt_example_2.cpp b/examples/example_18/mqtt_example_2.cpp similarity index 100% rename from examples/mqtt_exe_2/mqtt_example_2.cpp rename to examples/example_18/mqtt_example_2.cpp diff --git a/examples/mqtt_exe_3/CMakeLists.txt b/examples/example_19/CMakeLists.txt similarity index 60% rename from examples/mqtt_exe_3/CMakeLists.txt rename to examples/example_19/CMakeLists.txt index 22886555..974f8be1 100644 --- a/examples/mqtt_exe_3/CMakeLists.txt +++ b/examples/example_19/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE mqtt_example_3) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/mqtt_exe_3/feed.py b/examples/example_19/feed.py similarity index 100% rename from examples/mqtt_exe_3/feed.py rename to examples/example_19/feed.py diff --git a/examples/mqtt_exe_3/mqtt_example_3.cpp b/examples/example_19/mqtt_example_3.cpp similarity index 100% rename from examples/mqtt_exe_3/mqtt_example_3.cpp rename to examples/example_19/mqtt_example_3.cpp diff --git a/examples/example_2/CMakeLists.txt b/examples/example_2/CMakeLists.txt index 8e724d9e..12e832c4 100644 --- a/examples/example_2/CMakeLists.txt +++ b/examples/example_2/CMakeLists.txt @@ -5,10 +5,10 @@ SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_3/CMakeLists.txt b/examples/example_3/CMakeLists.txt index 609e936b..dc0765f2 100644 --- a/examples/example_3/CMakeLists.txt +++ b/examples/example_3/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_3) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_4/CMakeLists.txt b/examples/example_4/CMakeLists.txt index c095c125..7b85c948 100644 --- a/examples/example_4/CMakeLists.txt +++ b/examples/example_4/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_4) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_5/CMakeLists.txt b/examples/example_5/CMakeLists.txt index 43bc7fc7..711acc42 100644 --- a/examples/example_5/CMakeLists.txt +++ b/examples/example_5/CMakeLists.txt @@ -4,10 +4,11 @@ SET(EXECUTABLE example_5) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -15,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_7/CMakeLists.txt b/examples/example_7/CMakeLists.txt index bb3f0507..8bfa60c0 100644 --- a/examples/example_7/CMakeLists.txt +++ b/examples/example_7/CMakeLists.txt @@ -5,10 +5,10 @@ SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_8/CMakeLists.txt b/examples/example_8/CMakeLists.txt index aa12cde7..2c7726f8 100644 --- a/examples/example_8/CMakeLists.txt +++ b/examples/example_8/CMakeLists.txt @@ -5,10 +5,10 @@ SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/examples/example_9/CMakeLists.txt b/examples/example_9/CMakeLists.txt index a7f22c38..6cb34812 100644 --- a/examples/example_9/CMakeLists.txt +++ b/examples/example_9/CMakeLists.txt @@ -5,10 +5,10 @@ SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) -TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib) +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) -IF(BITRL_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${BITRL_CHRONO_TARGETS}) +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) ENDIF () IF(BITRL_WEBOTS) @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) diff --git a/src/bitrl/bitrl_consts.h b/src/bitrl/bitrl_consts.h index 71458805..a28814b8 100644 --- a/src/bitrl/bitrl_consts.h +++ b/src/bitrl/bitrl_consts.h @@ -6,6 +6,7 @@ */ #include "bitrl/bitrl_types.h" +#include "bitrl/bitrl_config.h" #include #include @@ -29,6 +30,9 @@ inline const std::string INVALID_STR = std::string("INVALID"); /// inline const real_t TOLERANCE = 1.0e-8; +inline const std::string ROBOTS_DIR = std::string(ROBOTS_DATA_DIR); + + namespace maths { diff --git a/src/bitrl/boards/arduino/arduino_connector_base.h b/src/bitrl/boards/arduino/arduino_connector_base.h index 75108ce4..52c607cb 100644 --- a/src/bitrl/boards/arduino/arduino_connector_base.h +++ b/src/bitrl/boards/arduino/arduino_connector_base.h @@ -53,7 +53,7 @@ class ArduinoConnectorBase ArduinoConnectorBase() = default; }; -} // namespace boards::arduino +} // namespace boards::example_15 } // namespace bitrl #endif // ARDUINO_ENV_BASE_H diff --git a/src/bitrl/boards/arduino/arduino_connector_usb_base.cpp b/src/bitrl/boards/arduino/arduino_connector_usb_base.cpp index 9cb3702c..ec37e14d 100644 --- a/src/bitrl/boards/arduino/arduino_connector_usb_base.cpp +++ b/src/bitrl/boards/arduino/arduino_connector_usb_base.cpp @@ -99,5 +99,5 @@ std::string ArduinoConnectorUSBBase::send_cmd(const ArduinoCMDBase &cmd) return "No response from Arduino."; } -} // namespace boards::arduino +} // namespace boards::example_15 } // namespace bitrl diff --git a/src/bitrl/boards/arduino/arduino_connector_usb_base.h b/src/bitrl/boards/arduino/arduino_connector_usb_base.h index f02a3062..80e258ce 100644 --- a/src/bitrl/boards/arduino/arduino_connector_usb_base.h +++ b/src/bitrl/boards/arduino/arduino_connector_usb_base.h @@ -70,6 +70,6 @@ class ArduinoConnectorUSBBase : public ArduinoConnectorBase int_t port_id_{-1}; }; -} // namespace boards::arduino +} // namespace boards::example_15 } // namespace bitrl #endif // ARDUINO_ENV_USB_BASE_H diff --git a/src/bitrl/boards/arduino/arduino_connector_wifi_base.cpp b/src/bitrl/boards/arduino/arduino_connector_wifi_base.cpp index 6c20ad6e..43d943c4 100644 --- a/src/bitrl/boards/arduino/arduino_connector_wifi_base.cpp +++ b/src/bitrl/boards/arduino/arduino_connector_wifi_base.cpp @@ -31,5 +31,5 @@ std::string ArduinoConnectorWIFIBase::send_cmd(const ArduinoCMDBase &cmd) return str_response; } -} // namespace boards::arduino +} // namespace boards::example_15 } // namespace bitrl diff --git a/src/bitrl/boards/arduino/arduino_connector_wifi_base.h b/src/bitrl/boards/arduino/arduino_connector_wifi_base.h index 5105141e..edae0b7e 100644 --- a/src/bitrl/boards/arduino/arduino_connector_wifi_base.h +++ b/src/bitrl/boards/arduino/arduino_connector_wifi_base.h @@ -48,7 +48,7 @@ class ArduinoConnectorWIFIBase : public ArduinoConnectorBase std::string arduino_url_; }; -} // namespace boards::arduino +} // namespace boards::example_15 } // namespace bitrl #endif // ARDUINO_CONNECTOR_WIFI_BASE_H diff --git a/src/bitrl/rigid_bodies/quadrotor.h b/src/bitrl/rigid_bodies/quadrotor.h deleted file mode 100644 index a9a87062..00000000 --- a/src/bitrl/rigid_bodies/quadrotor.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef QUADROTOR_H -#define QUADROTOR_H - -#include "rlenvs/rlenvs_types_v2.h" - -namespace rlenvscpp -{ -namespace rigid_bodies -{ - -/// -/// -class Quadrotor -{ - public: - Quadrotor - - private : - - /// - /// \brief The mass of the quadrotor - /// - real_t mass_; - - /// - /// \brief the arm length - /// - real_t l_; -}; - -} // namespace rigid_bodies -} // namespace rlenvscpp - -#endif \ No newline at end of file diff --git a/version.h.in b/version.h.in index 5e0bfa4e..1768a7a7 100755 --- a/version.h.in +++ b/version.h.in @@ -1,8 +1,8 @@ -#ifndef GYMFCPP_VERSION_H -#define GYMFCPP_VERSION_H +#ifndef BITRL_VERSION_H +#define BITRL_VERSION_H -#define RLENVSCPP_VERSION_MAJOR @RLENVSCPP_VERSION_MAJOR@ -#define RLENVSCPP_VERSION_MINOR @RLENVSCPP_VERSION_MINOR@ -#define RLENVSCPP_VERSION_PATCH @RLENVSCPP_VERSION_PATCH@ -#define RLENVSCPP_VERSION "@RLENVSCPP_VERSION@" +#define BITRL_VERSION_MAJOR @BITRL_VERSION_MAJOR@ +#define BITRL_VERSION_MINOR @BITRL_VERSION_MINOR@ +#define BITRL_VERSION_PATCH @BITRL_VERSION_PATCH@ +#define BITRL_VERSION "@BITRL_VERSION@" #endif From 34297010de1e26c479805a50e6e700cf9d3e8280 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 11 Jan 2026 18:37:59 +0000 Subject: [PATCH 02/19] Add example_14 --- examples/example_14/CMakeLists.txt | 24 ++++++++++++++++++++++ examples/example_14/example_14.cpp | 33 ++++++++++++++++++++++++++++++ examples/example_14/example_14.md | 10 +++++++++ 3 files changed, 67 insertions(+) create mode 100644 examples/example_14/CMakeLists.txt create mode 100644 examples/example_14/example_14.cpp create mode 100644 examples/example_14/example_14.md diff --git a/examples/example_14/CMakeLists.txt b/examples/example_14/CMakeLists.txt new file mode 100644 index 00000000..aae6af56 --- /dev/null +++ b/examples/example_14/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.6 FATAL_ERROR) + +SET(EXECUTABLE example_14) +SET(SOURCE ${EXECUTABLE}.cpp) + +ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) + +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) +ENDIF () + +IF(BITRL_WEBOTS) + TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController) +ENDIF() + +IF(BITRL_MQTT) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) +ENDIF () + +IF(BITRL_OPENCV) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${OpenCV_LIBS}) +ENDIF () diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp new file mode 100644 index 00000000..6d8febb3 --- /dev/null +++ b/examples/example_14/example_14.cpp @@ -0,0 +1,33 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" + +namespace example14 +{ +using bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobotBase; +} + +int main() +{ + using namespace example14; + + CHRONO_DiffDriveRobotBase robot; + robot.load_from_json(bitrl::consts::ROBOTS_DIR + "/bitrl_diff_drive_robot.json"); + + + return 0; +} +#else +#include +int main() +{ + std::cerr<<"You need PROJECTCHRONO configured with " + <<"bitrl in order to run this example " + <<"Reconfigure bitrl and set ENABLE_CHRONO=ON"<Chrono library. +Specifically, the robot we will simulate has + +- Two motorised wheels +- An ultrasound sensor + +In order to be able to run this example you need to configure bitrl with Chrono support \ No newline at end of file From 92eead32635abfa59f1d2b87465ac740950c6c65 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 11 Jan 2026 18:38:19 +0000 Subject: [PATCH 03/19] Add robots directory --- robots/bitrl_diff_drive_robot.json | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 robots/bitrl_diff_drive_robot.json diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json new file mode 100644 index 00000000..2358c6aa --- /dev/null +++ b/robots/bitrl_diff_drive_robot.json @@ -0,0 +1,6 @@ +{ + "name": "Odysseus", + "mass": 1.0, + "mass_units": "kg" + +} \ No newline at end of file From 4ac990122f3568a77b3a94575bc85d39ed132dd4 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 11 Jan 2026 18:38:52 +0000 Subject: [PATCH 04/19] Add chrono_robots directory --- .../chrono_robots/chrono_diff_drive_robot.cpp | 55 +++++++++++ .../chrono_robots/chrono_diff_drive_robot.h | 92 +++++++++++++++++++ .../chrono_robots/rb_chrono_module.h | 17 ++++ 3 files changed, 164 insertions(+) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h create mode 100644 src/bitrl/rigid_bodies/chrono_robots/rb_chrono_module.h diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp new file mode 100644 index 00000000..3678da0d --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -0,0 +1,55 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" +#include "bitrl/utils/io/json_file_reader.h" +#include + +#ifdef BITRL_LOG +#include +#endif + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +namespace +{ +auto build_chassis() +{ + auto chassis = chrono_types::make_shared(); + chassis->SetMass(10.0); + chassis->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); + chassis->SetPos(chrono::ChVector3d(0, 0, 0.15)); + chassis->SetFixed(false); + return chassis; +} +} + +void +CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) +{ +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loading robot from file: " << filename; +#endif + utils::io::JSONFileReader json_reader(filename); + json_reader.open(); + + // set the gravity acceleration + sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); + + auto chassis = build_chassis(); + sys_.Add(chassis); + + + name_ = json_reader.template get_value("name"); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; +#endif +} +} +} +#endif + diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h new file mode 100644 index 00000000..da94ac48 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -0,0 +1,92 @@ +#ifndef CHRONO_DIFF_DRIVE_ROBOT_H +#define CHRONO_DIFF_DRIVE_ROBOT_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_types.h" +#include "chrono/physics/ChSystemSMC.h" + +#include +#include +#include + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +/** + * @class CHRONO_DiffDriveRobotBase + * @ingroup rb_chrono + * @brief Base class for a differential-drive robot using Project Chrono. + * + * This class provides a common foundation for modeling and simulating + * differential-drive robots using Chrono physics objects. It encapsulates + * a Chrono simulation system and supports initialization from an external + * JSON configuration file. + * + * The class can be instantiated directly for simple simulations or + * extended to implement more specialized robot behaviors. + * + * @note This class assumes the use of Chrono's SMC contact model. + */ +class CHRONO_DiffDriveRobotBase +{ +public: + + /** + * @brief Load robot and simulation parameters from a JSON file. + * + * This function initializes the robot and its associated Chrono + * simulation objects based on the contents of the provided JSON + * configuration file. + * + * Typical parameters may include: + * - Physical dimensions + * - Mass and inertia properties + * - Wheel configuration + * - Simulation settings + * + * @param filename Path to the JSON configuration file. + * + * @throws std::runtime_error If the file cannot be read or parsed. + */ + void load_from_json(const std::string& filename); + + + /** + * @brief The name of the robot + * @return + */ + const std::string& get_name() const noexcept{return name_;} + +protected: + + /** + * @brief Chrono physics system used for simulation. + * + * This system owns and manages all physical bodies, constraints, + * and contact interactions associated with the robot and the + * environment. + */ + chrono::ChSystemSMC sys_; + + /** + * @brief Motors for the robot + * motors_.first left motor + * motors.second right motor + */ + std::pair, std::shared_ptr> motors_; + + /** + * @brief The name of the robot + */ + std::string name_; + + +}; +} +} +#endif +#endif //DIFF_DRIVE_ROBOT_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/rb_chrono_module.h b/src/bitrl/rigid_bodies/chrono_robots/rb_chrono_module.h new file mode 100644 index 00000000..088d3fa7 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/rb_chrono_module.h @@ -0,0 +1,17 @@ +// +// Created by alex on 1/11/26. +// + +#ifndef RB_CHRONO_MODULE_H +#define RB_CHRONO_MODULE_H + +/** + * @defgroup rb_chrono rb::chrono + * @brief Chrono-based robotics components. + * + * This module contains robot models, simulation utilities, + * and physics-based components built on Project Chrono. + */ + + +#endif //RB_CHRONO_MODULE_H From 09c3a7ff8aaa4999deece8b5b517b7ce31a89028 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 11 Jan 2026 18:56:46 +0000 Subject: [PATCH 05/19] Fix bug with linking to bitrllib twice --- examples/example_16/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/example_16/CMakeLists.txt b/examples/example_16/CMakeLists.txt index 63c57ca9..3f50d0bf 100644 --- a/examples/example_16/CMakeLists.txt +++ b/examples/example_16/CMakeLists.txt @@ -16,7 +16,7 @@ IF(BITRL_WEBOTS) ENDIF() IF(BITRL_MQTT) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib paho-mqttpp3 paho-mqtt3as) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) ENDIF () IF(BITRL_OPENCV) From b1d29101328897c9d18131dd1c461fc4ef86e19d Mon Sep 17 00:00:00 2001 From: pockerman Date: Mon, 12 Jan 2026 12:21:24 +0000 Subject: [PATCH 06/19] Update JSONFileReader API. Update bitrl_diff_drive_robot (#166) --- robots/bitrl_diff_drive_robot.json | 27 +++- .../chrono_robots/chrono_diff_drive_robot.cpp | 122 +++++++++++++++++- .../chrono_robots/chrono_diff_drive_robot.h | 12 ++ src/bitrl/utils/io/json_file_reader.h | 22 +++- 4 files changed, 177 insertions(+), 6 deletions(-) diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index 2358c6aa..be64672d 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -1,6 +1,31 @@ { "name": "Odysseus", "mass": 1.0, - "mass_units": "kg" + "mass_units": "kg", + "chassis": { + "mass": 1.0, + "mass_units": "kg", + "position": [ 0.0, 0.0, 0.0], + "fixed": false + }, + "left-wheel": { + "mass": 1.0, + "mass_units": "kg", + "position": [ 0.0, 0.18, 0.08], + "radius": 0.05, + "radius_units": "cm" + }, + "right-wheel": { + "mass": 1.0, + "mass_units": "kg", + "position": [ 0.0, -0.18, 0.08], + "radius": 0.05, + "radius_units": "cm" + }, + "caster-wheel": { + "mass": 0.5, + "mass_units": "kg", + "position": [ -0.2, 0.0, 0.05] + } } \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp index 3678da0d..ecb9a6f5 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -3,28 +3,120 @@ #ifdef BITRL_CHRONO #include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" +#include "bitrl/bitrl_consts.h" #include "bitrl/utils/io/json_file_reader.h" +#include "bitrl/extern/nlohmann/json/json.hpp" + +#include #include +#include #ifdef BITRL_LOG #include #endif +#include +#include +#include + + namespace bitrl { namespace rb::bitrl_chrono { namespace { -auto build_chassis() + +using json = nlohmann::json; + + + +// helper class to read chassis +struct Chassis +{ + std::array position; + std::string mass_units; + real_t mass; + bool fixed; + + Chassis(const json &j); +}; + +Chassis::Chassis(const json &j) + : +mass(j["mass"].get()), +mass_units(j["mass_units"].get()), +fixed(j["fixed"].get()), +position() +{ + auto pos = j.at("position"); + for (size_t i = 0; i < 3; ++i) + position[i] = pos.at(i).get(); +} + +// helper struct to read a wheel +struct Wheel +{ + std::array position; + std::string mass_units; + real_t mass; + + Wheel(const json &j); +}; + +Wheel::Wheel(const json &j) + : +mass(j["mass"].get()), +mass_units(j["mass_units"].get()), +position() +{ + auto pos = j.at("position"); + for (size_t i = 0; i < 3; ++i) + position[i] = pos.at(i).get(); +} + + +auto build_chassis(const utils::io::JSONFileReader& json_reader) { + auto chassis_data = json_reader.template at("chassis"); auto chassis = chrono_types::make_shared(); - chassis->SetMass(10.0); + chassis->SetMass(chassis_data.mass); chassis->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); - chassis->SetPos(chrono::ChVector3d(0, 0, 0.15)); + chassis->SetPos(chrono::ChVector3d(chassis_data.position[0], chassis_data.position[1], chassis_data.position[2])); chassis->SetFixed(false); return chassis; } + +auto build_wheel(const utils::io::JSONFileReader& json_reader, const std::string &wheel_label) +{ + + auto wheel_data = json_reader.template at(wheel_label); + auto wheel = chrono_types::make_shared(); + wheel->SetMass(wheel_data.mass); + wheel->SetPos(chrono::ChVector3d(wheel_data.position[0], wheel_data.position[1], wheel_data.position[2])); + wheel->SetName(wheel_label); + return wheel; +} + +auto build_motor(std::shared_ptr wheel, + std::shared_ptr chassis, + const std::string &motor_label) +{ + chrono::ChQuaterniond q; + q.SetFromAngleAxis(consts::maths::PI * 0.5, chrono::VECT_X); + chrono::ChFrame<> frame(wheel->GetPos(), q); + auto motor = chrono_types::make_shared(); + motor->Initialize( + wheel, + chassis, + frame); + motor->SetName(motor_label); + + // set the speed function + motor -> SetSpeedFunction(chrono_types::make_shared(0.0)); + return motor; +} + } void @@ -39,12 +131,34 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) // set the gravity acceleration sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); - auto chassis = build_chassis(); + auto chassis = build_chassis(json_reader); sys_.Add(chassis); name_ = json_reader.template get_value("name"); + auto left_wheel = build_wheel(json_reader, "left-wheel"); + auto right_wheel = build_wheel(json_reader, "right-wheel"); + + sys_.Add(left_wheel); + sys_.Add(right_wheel); + + auto left_motor = build_motor(left_wheel, chassis, "left-motor"); + auto right_motor = build_motor(right_wheel, chassis, "right-motor"); + + sys_.Add(left_motor); + sys_.Add(right_motor); + + auto caster = build_wheel(json_reader, "caster-wheel"); + sys_.Add(caster); + auto caster_joint = chrono_types::make_shared(); + caster_joint->Initialize( + caster, + chassis, + chrono::ChFrame<>(chrono::ChCoordsys<>(caster->GetPos())) + ); + sys_.Add(caster_joint); + #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; #endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h index da94ac48..891ba436 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -61,6 +61,18 @@ class CHRONO_DiffDriveRobotBase */ const std::string& get_name() const noexcept{return name_;} + /** + * @brief Retruns the number of wheels this robot has + * @return + */ + uint_t n_wheels()const noexcept{return 3;} + + /** + * @brief Returns the number of motors this robot has + * @return + */ + uint_t n_motors()const noexcept{return 2;} + protected: /** diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index aeac5861..5f95a49b 100644 --- a/src/bitrl/utils/io/json_file_reader.h +++ b/src/bitrl/utils/io/json_file_reader.h @@ -30,6 +30,16 @@ class JSONFileReader final : public FileReaderBase */ template T get_value(const std::string &label) const; + /** + * Returns the object at the specified label. + * T should be constructed using T(json data) and should + * be copy constructible + * @param label + * @return + */ + template T at(const std::string &label) const; + + private: using json = nlohmann::json; json data_; @@ -37,7 +47,6 @@ class JSONFileReader final : public FileReaderBase template T JSONFileReader::get_value(const std::string &label) const { - if (!this->is_open()) { throw std::logic_error("JSON file is not open. Have you called open()?"); @@ -45,6 +54,17 @@ template T JSONFileReader::get_value(const std::string &label) cons return data_[label].template get(); } +template T JSONFileReader::at(const std::string &label) const +{ + if (!this->is_open()) + { + throw std::logic_error("JSON file is not open. Have you called open()?"); + } + + auto data = data_.at(label); + return T(data); +} + } // namespace utils::io } // namespace bitrl #endif // JSON_FILE_READER_H From 798cf5ae6a2725433ca1cdd0d83d8a70fb07feb3 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 18 Jan 2026 18:08:21 +0000 Subject: [PATCH 07/19] Update example 19 with chrono based ultrasound sensor model (#166) --- CMakeLists.txt | 5 +- examples/example_19/mqtt_example_3.cpp | 107 ++++++++++++++++-- robots/bitrl_diff_drive_robot.json | 12 +- .../chrono_robots/chrono_diff_drive_robot.cpp | 9 +- .../chrono_robots/chrono_diff_drive_robot.h | 1 + 5 files changed, 118 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a2e439f..abb1585e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,8 +139,9 @@ INCLUDE_DIRECTORIES(src/) FILE(GLOB SRCS src/bitrl/*.cpp src/bitrl/network/*.cpp - src/bitrl/sensors/*cpp - src/bitrl/sensors/messages/*cpp + src/bitrl/sensors/*.cpp + src/bitrl/sensors/messages/*.cpp + src/bitrl/sensors/backends/*.cpp src/bitrl/envs/*.cpp src/bitrl/envs/gymnasium/*.cpp src/bitrl/envs/gymnasium/toy_text/*.cpp diff --git a/examples/example_19/mqtt_example_3.cpp b/examples/example_19/mqtt_example_3.cpp index 53e79225..a088cc70 100644 --- a/examples/example_19/mqtt_example_3.cpp +++ b/examples/example_19/mqtt_example_3.cpp @@ -1,17 +1,30 @@ #include "bitrl/bitrl_config.h" -#ifdef BITRL_MQTT +#ifdef BITRL_MQTT #include "bitrl/network/mqtt_subscriber.h" +#endif + #include "bitrl/sensors/messages/ultrasound.h" +#include "bitrl/sensors/ultrasonic_sensor.h" + +#ifdef BITRL_CHRONO +#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" +#include "chrono/physics/ChSystemSMC.h" +#include +#endif #include #include #include #include +#include -int main() +namespace example { +#ifdef BITRL_MQTT +void run_mqtt() +{ using namespace bitrl; network::MqttSubscriber ultrasound_subscriber("tcp://localhost:1883", "ultrasound"); @@ -40,17 +53,91 @@ int main() std::this_thread::sleep_for(std::chrono::microseconds(200)); } +} + +#endif + +#ifdef BITRL_CHRONO +void run_chrono() +{ + using namespace bitrl; + + // build a system to use with the backend + // the system carries the physics + chrono::ChSystemSMC sys; + sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); + sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81)); + + auto material = chrono_types::make_shared(); + material->SetYoungModulus(2e7); + material->SetFriction(0.5f); + material->SetRestitution(0.1f); + + // 2. Create ground (target object) + auto ground = chrono_types::make_shared( + 2.0, 2.0, 0.1, // size + 1000, // density + true, // visual + true, // collision + material + ); + ground->SetPos(chrono::ChVector3d(2.0, 0, 0.2)); // 2m in front + ground->SetFixed(true); + sys.Add(ground); + + // 3. Create robot body (sensor mount) + auto robot = chrono_types::make_shared( + 0.3, 0.3, 0.2, + 1000, + true, + true, + material + ); + robot->SetPos(chrono::ChVector3d(0, 0, 0.2)); + robot->SetFixed(true); + sys.Add(robot); + + sys.DoStepDynamics(0.1); + + auto sensor_backend = std::make_shared(sys, robot); + sensor_backend -> set_max_distance(5.0); + sensor_backend -> set_sensor_units("m"); + + sensors::UltrasonicSensor sensor(sensor_backend, "MyUltrasonicSensor"); + + sensor.init(); + + std::cout<<"Sensor name: "< int main() { - std::cerr << "This example requires MQTT and OpenCV to be enable. " - "Reconfigure bitrl with ENABLE_MQTT=ON and ENABLE_OPENCV=ON" - << std::endl; - return 1; + +#ifdef BITRL_MQTT + //example::run_mqtt(); +#endif + +#ifdef BITRL_CHRONO + example::run_chrono(); +#endif + + return 0; } -#endif \ No newline at end of file + + + diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index be64672d..1b6229f3 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -26,6 +26,16 @@ "mass": 0.5, "mass_units": "kg", "position": [ -0.2, 0.0, 0.05] - } + }, + "sensors": [ + { + "idx": 0, + "type": "distance", + "name": "Ultrasonic", + "update_rate": 50.0, + "mounted_position": [0.3, 0, 0.1], + "max_distance": 5.0 + } + ] } \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp index ecb9a6f5..c0749359 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -11,6 +11,7 @@ #include #include + #ifdef BITRL_LOG #include #endif @@ -29,8 +30,6 @@ namespace using json = nlohmann::json; - - // helper class to read chassis struct Chassis { @@ -117,6 +116,8 @@ auto build_motor(std::shared_ptr wheel, return motor; } + + } void @@ -134,7 +135,6 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) auto chassis = build_chassis(json_reader); sys_.Add(chassis); - name_ = json_reader.template get_value("name"); auto left_wheel = build_wheel(json_reader, "left-wheel"); @@ -159,6 +159,9 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) ); sys_.Add(caster_joint); + // read the sensors attached to the robot + + #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; #endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h index 891ba436..ca559f91 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -8,6 +8,7 @@ #include "bitrl/bitrl_types.h" #include "chrono/physics/ChSystemSMC.h" + #include #include #include From ceeb38da97243c28d404d4ab276a117846f1bf6c Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 18 Jan 2026 18:11:39 +0000 Subject: [PATCH 08/19] Add base class for sensor and sensor backend. Add implementation for Ultrasonic sensor (#166) --- .../backends/chrono_ultrasound_backend.cpp | 89 ++++++++++++ .../backends/chrono_ultrasound_backend.h | 100 +++++++++++++ .../backends/range_sensor_backend_base.h | 71 +++++++++ .../sensors/backends/sensor_backend_base.h | 120 ++++++++++++++++ .../sensors/backends/sensors_backend_module.h | 9 ++ src/bitrl/sensors/sensor_base.cpp | 13 ++ src/bitrl/sensors/sensor_base.h | 136 ++++++++++++++++++ src/bitrl/sensors/sensors_module.h | 10 ++ src/bitrl/sensors/ultrasonic_sensor.cpp | 43 ++++++ src/bitrl/sensors/ultrasonic_sensor.h | 85 +++++++++++ 10 files changed, 676 insertions(+) create mode 100644 src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp create mode 100644 src/bitrl/sensors/backends/chrono_ultrasound_backend.h create mode 100644 src/bitrl/sensors/backends/range_sensor_backend_base.h create mode 100644 src/bitrl/sensors/backends/sensor_backend_base.h create mode 100644 src/bitrl/sensors/backends/sensors_backend_module.h create mode 100644 src/bitrl/sensors/sensor_base.cpp create mode 100644 src/bitrl/sensors/sensor_base.h create mode 100644 src/bitrl/sensors/sensors_module.h create mode 100644 src/bitrl/sensors/ultrasonic_sensor.cpp create mode 100644 src/bitrl/sensors/ultrasonic_sensor.h diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp new file mode 100644 index 00000000..7c86ea8f --- /dev/null +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -0,0 +1,89 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/utils/io/json_file_reader.h" +#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" + +#include "chrono/collision/ChCollisionSystem.h" + +#ifdef BITRL_LOG +#include +#endif + +namespace bitrl +{ +namespace sensors::backends +{ + +const std::string ChronoUltrasonicBackend::BACKEND_TYPE = "ChronoUltrasonicBackend"; + +ChronoUltrasonicBackend::ChronoUltrasonicBackend(chrono::ChSystem& sys_ptr, + std::shared_ptr body) + : +RangeSensorBackendBase(ChronoUltrasonicBackend::BACKEND_TYPE), +sys_ptr_(&sys_ptr), +body_(body) +{} + +void ChronoUltrasonicBackend::load_from_json(const std::string& filename) +{ + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loading sensor backend from file: " << filename; +#endif + + utils::io::JSONFileReader json_reader(filename); + json_reader.open(); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: ChronoUltrasonicBackend"; +#endif +} + +std::vector ChronoUltrasonicBackend::read_values() +{ +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading backend values: "<< this -> backend_type_str(); +#endif + + chrono::ChVector3d start = + body_->GetFrameRefToAbs().TransformPointLocalToParent(position_); + chrono::ChVector3d dir = + body_->GetFrameRefToAbs().GetRotMat().GetAxisX(); + dir.Normalize(); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading backend values start: "<< start; +#endif + + chrono::ChVector3d end = start + dir * this -> max_distance(); +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading backend values end: "<< end; +#endif + + auto collision_sys = sys_ptr_ -> GetCollisionSystem(); + + if (collision_sys == nullptr) + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(warning)<<"Collision system is not enabled for backend. Return max_distance"; +#endif + return {this -> max_distance()}; + } + + chrono::ChCollisionSystem::ChRayhitResult hit; + if (collision_sys -> RayHit(start, end, hit)) + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"A Ray was hit: "<< end; +#endif + return {(hit.abs_hitPoint - start).Length()}; + } + + return {this -> max_distance()}; +} +} +} + +#endif diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h new file mode 100644 index 00000000..9499d46b --- /dev/null +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h @@ -0,0 +1,100 @@ +#ifndef CHRONO_ULTRASOUND_BACKEND_H +#define CHRONO_ULTRASOUND_BACKEND_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/sensors/backends/range_sensor_backend_base.h" + +#include "chrono/physics/ChSystem.h" +#include "chrono/physics/ChBody.h" + +#include +#include + +namespace bitrl +{ +namespace sensors::backends +{ + +/** + * @class ChronoUltrasonicBackend + * @ingroup bitrl_sensors_backends + * @brief Class for modelling ultrasonic sensors + */ +class ChronoUltrasonicBackend final: public RangeSensorBackendBase +{ +public: + + /** + * @brief The type of the sensor + */ + static const std::string BACKEND_TYPE; + + /** + * @brief Constructor + */ + ChronoUltrasonicBackend(chrono::ChSystem& sys_ptr, + std::shared_ptr body); + + /** + * @brief Read the sensor value + * @return + */ + std::vector read_values(); + + /** + * @brief Load robot and simulation parameters from a JSON file. + * + * This function initializes the robot and its associated Chrono + * simulation objects based on the contents of the provided JSON + * configuration file. + * + * Typical parameters may include: + * - Physical dimensions + * - Mass and inertia properties + * - Wheel configuration + * - Simulation settings + * + * @param filename Path to the JSON configuration file. + * + * @throws std::runtime_error If the file cannot be read or parsed. + */ + void load_from_json(const std::string& filename); + + /** + * @brief Set the position of the sensor + * @param pos + */ + void set_position(chrono::ChVector3d& pos){position_ = pos;} + + /** + * @return A read reference of the position of the sensor + */ + const chrono::ChVector3d& position() const{return position_;} + + +private: + + /** + * @brief Pointer to the system this sensor is attached ot + */ + chrono::ChSystem* sys_ptr_; + + /** + * @brief The body on which the sensor sits + */ + std::shared_ptr body_; + + /** + * @brief The position of the sensor + */ + chrono::ChVector3d position_; + +}; +} +} + +#endif +#endif //CHRONO_ULTRASOUND_BACKEND_H diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.h b/src/bitrl/sensors/backends/range_sensor_backend_base.h new file mode 100644 index 00000000..53a32057 --- /dev/null +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.h @@ -0,0 +1,71 @@ +#ifndef RANGE_SENSOR_BACKEND_BASE_H +#define RANGE_SENSOR_BACKEND_BASE_H + + +#include "bitrl/bitrl_types.h" +#include "bitrl/sensors/backends/sensor_backend_base.h" +#include + +namespace bitrl +{ +namespace sensors::backends +{ +/** + * @class RangeSensorBackendBase + * @ingroup bitrl_sensors_backends + * @brief Base class for modelling range sensors + */ +class RangeSensorBackendBase : public SensorBackendBase +{ +public: + /** + * @return The maximum distance the range sensor can read + */ + real_t max_distance()const noexcept {return max_distance_;} + + /** + * @brief Set the maximum distance the range sensor can read + * @param max_distance + */ + void set_max_distance(real_t max_distance) noexcept {max_distance_ = max_distance;} + + /** + * @return An instance of std::string representing the units the sensor + * readings are assumed in + */ + virtual std::string sensor_units()const{return sensor_units_;} + + /** + * @brief Set the sensor units + * @param units The string representing the sensor units + */ + void set_sensor_units(const std::string& units){sensor_units_ = units;} + +protected: + /** + * @brief Constructor + * @param backend_type + */ + explicit RangeSensorBackendBase(const std::string& backend_type); + +private: + + std::string sensor_units_{"METERS"}; + + /** + *@brief The maximum distance the sensor can read + */ + real_t max_distance_{0.0}; + +}; + +inline RangeSensorBackendBase::RangeSensorBackendBase(const std::string &backend_type) + : +SensorBackendBase(backend_type) +{} + +} +} + + +#endif //RANGE_SENSOR_BACKEND_BASE_H diff --git a/src/bitrl/sensors/backends/sensor_backend_base.h b/src/bitrl/sensors/backends/sensor_backend_base.h new file mode 100644 index 00000000..46198ee4 --- /dev/null +++ b/src/bitrl/sensors/backends/sensor_backend_base.h @@ -0,0 +1,120 @@ +#ifndef SENSOR_BACKEND_BASE_H +#define SENSOR_BACKEND_BASE_H + +#include "bitrl/bitrl_types.h" +#include "bitrl/bitrl_consts.h" +#include +#include + + +namespace bitrl +{ +namespace sensors::backends +{ + +/** + * @class SensorBackendBase + * @ingroup bitrl_sensors_backends + * @brief Base class for modelling sensors + * with different implementations + */ +class SensorBackendBase +{ +public: + /** + * @brief Destructor + */ + virtual ~SensorBackendBase()=default; + + /** + * @brief Returns the sampling period of the sensor + * @return + */ + real_t sampling_period()const noexcept {return sampling_period_;} + + /** + * @brief Set the sampling period of the backend + * @param period + */ + void set_sampling_period(real_t period){sampling_period_ = period;} + + /** + * @brief Read the sensor value + * @return + */ + virtual std::vector read_values()=0; + + /** + * @return An instance of std::string representing the units the sensor + * readings are assumed in + */ + virtual std::string sensor_units()const{return bitrl::consts::INVALID_STR;} + + /** + * @brief Load robot and simulation parameters from a JSON file. + * + * This function initializes the robot and its associated Chrono + * simulation objects based on the contents of the provided JSON + * configuration file. + * + * Typical parameters may include: + * - Physical dimensions + * - Mass and inertia properties + * - Wheel configuration + * - Simulation settings + * + * @param filename Path to the JSON configuration file. + * + * @throws std::runtime_error If the file cannot be read or parsed. + */ + virtual void load_from_json(const std::string& filename)=0; + + /** + * @return The number of values the implementation returns + */ + uint_t n_read_values()const noexcept{return n_read_values_;} + + /** + * @bief Return a string that specifies the type of the backend + * @return + */ + std::string backend_type_str()const noexcept{return backend_type_;} + +protected: + /** + * @brief Constructor + * @param backend_type + */ + SensorBackendBase(const std::string& backend_type); + +private: + + /** + * @brief The backend type flag + */ + const std::string backend_type_; + + + /** + * @brief The sampling period + */ + real_t sampling_period_{0.0}; + + /** + * @brief How many values the backend returns + */ + uint_t n_read_values_{1}; + + +}; + +inline +SensorBackendBase::SensorBackendBase(const std::string& backend_type) + : +backend_type_(backend_type) +{} + +} +} + +#endif //SENSOR_BACKEND_BASE_H diff --git a/src/bitrl/sensors/backends/sensors_backend_module.h b/src/bitrl/sensors/backends/sensors_backend_module.h new file mode 100644 index 00000000..41c49138 --- /dev/null +++ b/src/bitrl/sensors/backends/sensors_backend_module.h @@ -0,0 +1,9 @@ +#ifndef SENSORS_BACKEND_MODULE_H +#define SENSORS_BACKEND_MODULE_H + +/** + * @defgroup bitrl_sensors_backends bitrl::sensors::backends + * @brief Backends for modeling sensors. + */ + +#endif //SENSORS_BACKEND_H diff --git a/src/bitrl/sensors/sensor_base.cpp b/src/bitrl/sensors/sensor_base.cpp new file mode 100644 index 00000000..3e0c8d80 --- /dev/null +++ b/src/bitrl/sensors/sensor_base.cpp @@ -0,0 +1,13 @@ +#include "bitrl/sensors/sensor_base.h" + +#include + +namespace bitrl +{ +namespace sensors +{ + +std::string SensorBase::backend_type_str()const{return bitrl::consts::INVALID_STR;} +std::string SensorBase::sensor_units()const{return bitrl::consts::INVALID_STR;} +} +} \ No newline at end of file diff --git a/src/bitrl/sensors/sensor_base.h b/src/bitrl/sensors/sensor_base.h new file mode 100644 index 00000000..7083d5b5 --- /dev/null +++ b/src/bitrl/sensors/sensor_base.h @@ -0,0 +1,136 @@ +#ifndef SENSOR_BASE_H +#define SENSOR_BASE_H + + +#include "../bitrl_consts.h" +#include "bitrl/bitrl_types.h" +#include "bitrl/bitrl_consts.h" +#include +#include +namespace bitrl +{ +namespace sensors +{ +/** + * @class SensorBase + * @ingroup bitrl_sensors + * @brief Class for modelling sensors. The interface follows closely + * the interface exposed by Webots see: https://cyberbotics.com/doc/guide/sensors + * In this token a Sensor is a Device and has: + * - sampling period + * - enabled/disabled + * - Last value + * - Sensor units + * Implementations can utilize noise and resolution. + * + */ +class SensorBase +{ +public: + /** + * @brief Destructor + */ + virtual ~SensorBase()=default; + + /** + * @brief Initialize the sensor. Set the is is_enabled_ flag to true. + * and performs any other initializations required by the sensor + */ + virtual void init()=0; + + /** + * @return Reads the sensor values and updates the values held internally + */ + virtual const std::vector& read_values()=0; + + /** + * @return An instance of std::string of the backend type. If the implementation + * does not use a specific backend returns bitrl::consts::INVALID_STR + */ + virtual std::string backend_type_str()const=0; + + /** + * @return An instance of std::string indicating the units the sensor is using. + * If units have not been established this returns bitrl::consts::INVALID_STR + */ + virtual std::string sensor_units()const=0; + + /** + * @brief Returns true if the sensor is enabled + * @return + */ + bool is_enabled()const noexcept{return is_enabled_;} + + /** + * @brief Set the is_enabled_ flag to true + */ + void enable()noexcept{is_enabled_=true;} + + /** + * @brief Disable the sensor + */ + void disable()noexcept{is_enabled_=false;} + + /** + * @return Read reference to the last values read by the sensor + */ + const std::vector& last_read_values()const noexcept{return values_;} + + /** + * @return An instance of std::string with the name of the sensor + */ + std::string sensor_name()const noexcept{return sensor_name_;} + + /** + * @brief Set the sensor name + * @param sensor_name The sensor name to set + */ + void set_sensor_name(const std::string& sensor_name)noexcept{sensor_name_=sensor_name;} + + /** + * @return An instance of std::string with the type of the sensor + */ + std::string sensor_type()const noexcept{return sensor_type_;} + +protected: + /** + * @param sensor_type The type of the sensor + */ + explicit SensorBase(const std::string& sensor_type, + const std::string& sensor_name=bitrl::consts::INVALID_STR); + + /** + * @brief The values last read by the sensor + */ + std::vector values_; + +private: + + /** + * @brief The type of the sensor + */ + const std::string sensor_type_; + + /** + * @brief The name of the sensor + */ + std::string sensor_name_{bitrl::consts::INVALID_STR}; + + /** + * @brief Flag indicating if the sensor is enabled + */ + bool is_enabled_{false}; +}; + +inline SensorBase::SensorBase(const std::string &sensor_type, const std::string& sensor_name) + : +values_(), +sensor_type_(sensor_type), +sensor_name_(sensor_name) +{} + + +} +} + +#endif //SENSOR_BASE_H diff --git a/src/bitrl/sensors/sensors_module.h b/src/bitrl/sensors/sensors_module.h new file mode 100644 index 00000000..bdf277cd --- /dev/null +++ b/src/bitrl/sensors/sensors_module.h @@ -0,0 +1,10 @@ + +#ifndef SENSORS_MODULE_H +#define SENSORS_MODULE_H + +/** + * @defgroup bitrl_sensors bitrl::sensors + * @brief utilities for modeling sensors. + */ + +#endif //SENSORS_MODULE_H diff --git a/src/bitrl/sensors/ultrasonic_sensor.cpp b/src/bitrl/sensors/ultrasonic_sensor.cpp new file mode 100644 index 00000000..1a2f52f5 --- /dev/null +++ b/src/bitrl/sensors/ultrasonic_sensor.cpp @@ -0,0 +1,43 @@ +#include "bitrl/bitrl_config.h" +#include "bitrl/sensors/ultrasonic_sensor.h" + +#ifdef BITRL_LOG +#include +#endif + +#include + +namespace bitrl +{ +namespace sensors +{ +const std::string UltrasonicSensor::SENSOR_TYPE = "UltrasonicSensor"; + + +void UltrasonicSensor::init() +{ + auto n_values = backend_ -> n_read_values(); + this -> values_.resize(n_values, -1.0); + this -> enable(); +} + +const std::vector& UltrasonicSensor::read_values() +{ + if (!this -> is_enabled()) + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(warning)<<"Sensor: " << this -> sensor_name() << " is not enabled. Returning dummy values"; +#endif + this -> values_; + } + + // TODO: Make sure that len(read_values) == len(values) + auto read_values = backend_ -> read_values(); + + + std::copy(read_values.begin(), read_values.end(), values_.begin()); + return this -> values_; +} + +} +} diff --git a/src/bitrl/sensors/ultrasonic_sensor.h b/src/bitrl/sensors/ultrasonic_sensor.h new file mode 100644 index 00000000..6ab001d0 --- /dev/null +++ b/src/bitrl/sensors/ultrasonic_sensor.h @@ -0,0 +1,85 @@ +#ifndef ULTRASONIC_SENSOR_H +#define ULTRASONIC_SENSOR_H + +#include "bitrl/bitrl_types.h" +#include "bitrl/sensors/sensor_base.h" +#include "bitrl/sensors/backends/range_sensor_backend_base.h" + +#include +#include + +namespace bitrl +{ +namespace sensors +{ +/** + * @class UltrasonicSensor + * @ingroup bitrl_sensors + * @brief Class for modelling ultrasonic sensors + */ +class UltrasonicSensor final: public SensorBase +{ +public: + + /** + * @brief The type of the sensor + */ + static const std::string SENSOR_TYPE; + + /** + * @brief Constructor + * @param backend + */ + explicit UltrasonicSensor(std::shared_ptr backend, + const std::string name = bitrl::consts::INVALID_STR); + + /** + * @brief Initialize the sensor. Set the is is_enabled_ flag to true. + * and performs any other initializations required by the sensor + */ + virtual void init(); + + /** + * @brief Reads the sensor values and updates the values held internally + * @return An instance of str::vector + */ + virtual const std::vector& read_values(); + + /** + * @return An instance of std::string of the backend type. If the implementation + * does not use a specific backend returns bitrl::consts::INVALID_STR + */ + virtual std::string backend_type_str()const; + + /** + * @return An instance of std::string indicating the units the sensor is using. + * If units have not been established this returns bitrl::consts::INVALID_STR + */ + virtual std::string sensor_units()const; + +private: + /** + * @brief The backend used for this sensor + */ + std::shared_ptr backend_; + +}; + +inline UltrasonicSensor::UltrasonicSensor(std::shared_ptr backend, + const std::string name) + : +SensorBase(UltrasonicSensor::SENSOR_TYPE, name), +backend_(backend) +{} + +inline +std::string UltrasonicSensor::backend_type_str()const{return backend_ -> backend_type_str();} + +inline +std::string UltrasonicSensor::sensor_units()const{return backend_ -> sensor_units();} + + +} +} + +#endif //ULTRASONIC_SENSOR_H From 9b09d9ed44e68f713cc8987f37f91bd94fabfe14 Mon Sep 17 00:00:00 2001 From: pockerman Date: Wed, 21 Jan 2026 08:56:01 +0000 Subject: [PATCH 09/19] Add MQTT ultrasound backend --- examples/example_20/CMakeLists.txt | 24 ++++ examples/example_20/example_20.cpp | 58 ++++++++++ examples/example_20/feed.py | 30 +++++ .../backends/mqtt_ultrasound_backend.cpp | 77 +++++++++++++ .../backends/mqtt_ultrasound_backend.h | 104 ++++++++++++++++++ 5 files changed, 293 insertions(+) create mode 100644 examples/example_20/CMakeLists.txt create mode 100644 examples/example_20/example_20.cpp create mode 100644 examples/example_20/feed.py create mode 100644 src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp create mode 100644 src/bitrl/sensors/backends/mqtt_ultrasound_backend.h diff --git a/examples/example_20/CMakeLists.txt b/examples/example_20/CMakeLists.txt new file mode 100644 index 00000000..d166e5b3 --- /dev/null +++ b/examples/example_20/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.20 FATAL_ERROR) + +SET(EXECUTABLE example_20) +SET(SOURCE ${EXECUTABLE}.cpp) + +ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) + +TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) + +IF(ENABLE_CHRONO) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) +ENDIF () + +IF(BITRL_WEBOTS) + TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController) +ENDIF() + +IF(BITRL_MQTT) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as) +ENDIF () + +IF(BITRL_OPENCV) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${OpenCV_LIBS}) +ENDIF () diff --git a/examples/example_20/example_20.cpp b/examples/example_20/example_20.cpp new file mode 100644 index 00000000..b5b24640 --- /dev/null +++ b/examples/example_20/example_20.cpp @@ -0,0 +1,58 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_MQTT + +#include "bitrl/network/mqtt_subscriber.h" +#include "bitrl/sensors/messages/ultrasound.h" +#include "bitrl/sensors/ultrasonic_sensor.h" +#include "bitrl/sensors/backends/mqtt_ultrasound_backend.h" +#include "bitrl/utils/io/io_utils.h" + +#include +#include +#include +#include +#include + +namespace example +{ + + +void run_mqtt() +{ + using namespace bitrl; + + network::MqttSubscriber ultrasound_subscriber("tcp://localhost:1883", "ultrasound"); + ultrasound_subscriber.connect(); + auto sensor_backend = std::make_shared(ultrasound_subscriber); + sensor_backend -> set_max_distance(5.0); + sensor_backend -> set_sensor_units("m"); + + sensors::UltrasonicSensor sensor(sensor_backend, "MyUltrasonicSensor"); + sensor.init(); + + while (true) + { + auto values = sensor.read_values(); + std::cout << "Measured distance: " << values[0] << " in: " < last_value_read(); + + std::cout<<"Distance received: "< +#endif + +namespace bitrl +{ +namespace sensors::backends +{ +const std::string MQTT_UltrasonicBackend::BACKEND_TYPE="MQTT_UltrasonicBackend"; + + +MQTT_UltrasonicBackend::MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber) + : +RangeSensorBackendBase(MQTT_UltrasonicBackend::BACKEND_TYPE), +subscriber_ptr_(&subscriber) +{} + + +void MQTT_UltrasonicBackend::load_from_json(const std::string& filename) +{ + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loading sensor backend from file: " << filename; +#endif + + utils::io::JSONFileReader json_reader(filename); + json_reader.open(); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: MQTT_UltrasonicBackend"; +#endif +} + +std::vector MQTT_UltrasonicBackend::read_values() +{ +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading backend values: "<< this -> backend_type_str(); +#endif + + auto message = subscriber_ptr_ -> poll(std::chrono::milliseconds(3000)); + + if (message.has_value()) + { + auto reading = sensors::UltrasoundMessage::parse(message.value()); + if (reading.has_value()) + { + auto read_message = reading.value(); + last_message_ = read_message; + return {read_message.distance}; + } + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(warning)<<"Couldn't read sensor values. Return max_distance"; +#endif + return {this -> max_distance()}; + } + } + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(warning)<<"Couldn't read sensor values. Return max_distance"; +#endif + return {this -> max_distance()}; + +} + +} +} + + +#endif + diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h new file mode 100644 index 00000000..570740bb --- /dev/null +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h @@ -0,0 +1,104 @@ +#ifndef MQTT_ULTRASOUND_BACKEND_H +#define MQTT_ULTRASOUND_BACKEND_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_MQTT + +#include "bitrl/bitrl_types.h" +#include "bitrl/network/mqtt_subscriber.h" +#include "bitrl/sensors/backends/range_sensor_backend_base.h" +#include "bitrl/sensors/messages/ultrasound.h" + +namespace bitrl{ +namespace sensors::backends +{ + +/** + * @class ChronoUltrasonicBackend + * @ingroup bitrl_sensors_backends + * @brief Class for accessing Ultrasonic sensors via MQTT + * Note this class has no other info other than the position of + * the sensor on the robot + */ +class MQTT_UltrasonicBackend: public RangeSensorBackendBase +{ +public: + + /** + * @brief The type of the sensor + */ + static const std::string BACKEND_TYPE; + + /** + * @brief Constructor + */ + explicit MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber); + + /** + * @brief Load robot and simulation parameters from a JSON file. + * + * This function initializes the robot and its associated Chrono + * simulation objects based on the contents of the provided JSON + * configuration file. + * + * Typical parameters may include: + * - Physical dimensions + * - Mass and inertia properties + * - Wheel configuration + * - Simulation settings + * + * @param filename Path to the JSON configuration file. + * + * @throws std::runtime_error If the file cannot be read or parsed. + */ + void load_from_json(const std::string& filename); + + /** + * @brief Read the sensor value + * @return + */ + std::vector read_values(); + + /** + * @brief Set the position of the sensor + * @param pos + */ + void set_position(const RealColVec3d& pos){position_ = pos;} + + /** + * @return A read reference of the position of the sensor + */ + const RealColVec3d& position() const{return position_;} + + /** + * @return A read reference to the last UltrasoundMessage that was read + */ + const UltrasoundMessage& last_value_read()const{return last_message_;} + +private: + + + network::MqttSubscriber* subscriber_ptr_; + + /** + * @brief The position of the sensor + */ + RealColVec3d position_; + + /** + * @brief The last read message + */ + UltrasoundMessage last_message_; + + /** + * @brief Time for polling the sensor + */ + uint_t polling_time_milli{3000}; + +}; + +} +} +#endif +#endif //MQTT_ULTRASOUND_BACKEND_H From 1ecd9b9b73643228cf04a023d56a902bc1a7db78 Mon Sep 17 00:00:00 2001 From: pockerman Date: Wed, 21 Jan 2026 08:56:29 +0000 Subject: [PATCH 10/19] Add io_utils source file --- src/bitrl/utils/io/io_utils.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 src/bitrl/utils/io/io_utils.cpp diff --git a/src/bitrl/utils/io/io_utils.cpp b/src/bitrl/utils/io/io_utils.cpp new file mode 100644 index 00000000..0f0bacca --- /dev/null +++ b/src/bitrl/utils/io/io_utils.cpp @@ -0,0 +1,18 @@ +#include "bitrl/utils/io/io_utils.h" + +#include +#include + +namespace bitrl +{ +namespace utils::io +{ +std::ostream& print_time_point(std::ostream &out, const std::chrono::system_clock::time_point& tp) +{ + std::time_t t = std::chrono::system_clock::to_time_t(tp); + std::tm tm = *std::localtime(&t); + out<< std::put_time(&tm, "%Y-%m-%d %H:%M:%S"); + return out; +} +} +} From 35111dbe5cade03fe31ece7a1131e58cfa95f6d7 Mon Sep 17 00:00:00 2001 From: pockerman Date: Wed, 21 Jan 2026 08:57:05 +0000 Subject: [PATCH 11/19] Refactoring and bug fixes (#166) --- examples/CMakeLists.txt | 8 +++- examples/example_19/feed.py | 30 ------------ examples/example_19/mqtt_example_3.cpp | 47 +------------------ examples/example_7/example_7.cpp | 2 +- .../backends/chrono_ultrasound_backend.cpp | 23 +++------ .../backends/chrono_ultrasound_backend.h | 8 ++-- src/bitrl/sensors/ultrasonic_sensor.cpp | 1 - src/bitrl/utils/io/io_utils.h | 32 +++++++++++-- 8 files changed, 49 insertions(+), 102 deletions(-) delete mode 100644 examples/example_19/feed.py diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 10d40f99..44129deb 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -41,13 +41,19 @@ ADD_SUBDIRECTORY(example_16) IF(BITRL_MQTT) ADD_SUBDIRECTORY(example_17) - ADD_SUBDIRECTORY(example_19) + ADD_SUBDIRECTORY(example_20) + # Need both OpenCV and MQTT IF(BITRL_OPENCV) ADD_SUBDIRECTORY(example_18) ENDIF () ENDIF() +IF(BITRL_CHRONO) + ADD_SUBDIRECTORY(example_19) +ENDIF () + + IF(BITRL_WEBOTS) ADD_SUBDIRECTORY(webots/world_1) ENDIF() diff --git a/examples/example_19/feed.py b/examples/example_19/feed.py deleted file mode 100644 index 2d4880a8..00000000 --- a/examples/example_19/feed.py +++ /dev/null @@ -1,30 +0,0 @@ -import time -import datetime -import json -import paho.mqtt.client as mqtt - -BROKER = "localhost" -PORT = 1883 -ULTRASOUND_TOPIC = "ultrasound" - -def main(): - # Initialize MQTT - client = mqtt.Client() - client.connect(BROKER, PORT, 60) - - distance = 0.0 - try: - while True: - z_str = json.dumps({"distance": distance, "unit":"cm", - "timestamp": str(datetime.datetime.now(datetime.UTC))} - ) - distance += 0.01 - client.publish(topic=ULTRASOUND_TOPIC, payload=z_str) - time.sleep(2.0) - - except Exception: - print("Stopping...") - - -if __name__ == "__main__": - main() diff --git a/examples/example_19/mqtt_example_3.cpp b/examples/example_19/mqtt_example_3.cpp index a088cc70..72d30cee 100644 --- a/examples/example_19/mqtt_example_3.cpp +++ b/examples/example_19/mqtt_example_3.cpp @@ -1,8 +1,6 @@ #include "bitrl/bitrl_config.h" -#ifdef BITRL_MQTT -#include "bitrl/network/mqtt_subscriber.h" -#endif + #include "bitrl/sensors/messages/ultrasound.h" #include "bitrl/sensors/ultrasonic_sensor.h" @@ -21,41 +19,7 @@ namespace example { -#ifdef BITRL_MQTT - -void run_mqtt() -{ - using namespace bitrl; - - network::MqttSubscriber ultrasound_subscriber("tcp://localhost:1883", "ultrasound"); - ultrasound_subscriber.connect(); - - while (true) - { - auto message = ultrasound_subscriber.poll(std::chrono::milliseconds(3000)); - - if (message.has_value()) - { - auto reading = sensors::UltrasoundMessage::parse(message.value()); - if (reading.has_value()) - { - - auto read_message = reading.value(); - - std::time_t t = std::chrono::system_clock::to_time_t(read_message.source_timestamp); - std::tm tm = *std::localtime(&t); - std::cout<<"Distance received: "<(sys, robot); + auto sensor_backend = std::make_shared(sys, robot); sensor_backend -> set_max_distance(5.0); sensor_backend -> set_sensor_units("m"); sensors::UltrasonicSensor sensor(sensor_backend, "MyUltrasonicSensor"); - sensor.init(); std::cout<<"Sensor name: "< body) : -RangeSensorBackendBase(ChronoUltrasonicBackend::BACKEND_TYPE), +RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), sys_ptr_(&sys_ptr), body_(body) {} -void ChronoUltrasonicBackend::load_from_json(const std::string& filename) +void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) { #ifdef BITRL_LOG @@ -37,11 +37,11 @@ void ChronoUltrasonicBackend::load_from_json(const std::string& filename) json_reader.open(); #ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: ChronoUltrasonicBackend"; + BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: CHRONO_UltrasonicBackend"; #endif } -std::vector ChronoUltrasonicBackend::read_values() +std::vector CHRONO_UltrasonicBackend::read_values() { #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Reading backend values: "<< this -> backend_type_str(); @@ -53,17 +53,9 @@ std::vector ChronoUltrasonicBackend::read_values() body_->GetFrameRefToAbs().GetRotMat().GetAxisX(); dir.Normalize(); -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Reading backend values start: "<< start; -#endif - chrono::ChVector3d end = start + dir * this -> max_distance(); -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Reading backend values end: "<< end; -#endif auto collision_sys = sys_ptr_ -> GetCollisionSystem(); - if (collision_sys == nullptr) { #ifdef BITRL_LOG @@ -75,9 +67,6 @@ std::vector ChronoUltrasonicBackend::read_values() chrono::ChCollisionSystem::ChRayhitResult hit; if (collision_sys -> RayHit(start, end, hit)) { -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"A Ray was hit: "<< end; -#endif return {(hit.abs_hitPoint - start).Length()}; } diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h index 9499d46b..6c18f363 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h @@ -19,11 +19,11 @@ namespace sensors::backends { /** - * @class ChronoUltrasonicBackend + * @class CHRONO_UltrasonicBackend * @ingroup bitrl_sensors_backends * @brief Class for modelling ultrasonic sensors */ -class ChronoUltrasonicBackend final: public RangeSensorBackendBase +class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase { public: @@ -35,7 +35,7 @@ class ChronoUltrasonicBackend final: public RangeSensorBackendBase /** * @brief Constructor */ - ChronoUltrasonicBackend(chrono::ChSystem& sys_ptr, + CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, std::shared_ptr body); /** @@ -67,7 +67,7 @@ class ChronoUltrasonicBackend final: public RangeSensorBackendBase * @brief Set the position of the sensor * @param pos */ - void set_position(chrono::ChVector3d& pos){position_ = pos;} + void set_position(const chrono::ChVector3d& pos){position_ = pos;} /** * @return A read reference of the position of the sensor diff --git a/src/bitrl/sensors/ultrasonic_sensor.cpp b/src/bitrl/sensors/ultrasonic_sensor.cpp index 1a2f52f5..b8e066e9 100644 --- a/src/bitrl/sensors/ultrasonic_sensor.cpp +++ b/src/bitrl/sensors/ultrasonic_sensor.cpp @@ -34,7 +34,6 @@ const std::vector& UltrasonicSensor::read_values() // TODO: Make sure that len(read_values) == len(values) auto read_values = backend_ -> read_values(); - std::copy(read_values.begin(), read_values.end(), values_.begin()); return this -> values_; } diff --git a/src/bitrl/utils/io/io_utils.h b/src/bitrl/utils/io/io_utils.h index dbd09814..1c8c497f 100644 --- a/src/bitrl/utils/io/io_utils.h +++ b/src/bitrl/utils/io/io_utils.h @@ -1,13 +1,24 @@ #ifndef IO_UTILS_H #define IO_UTILS_H +#include "bitrl/bitrl_types.h" #include #include +#include +#include namespace bitrl { + namespace utils::io { +/** +* @param out The stream to write +* @param tp The time point to write on the stream +* @return Read/write reference to the stream we write on +*/ +std::ostream& print_time_point(std::ostream &out, + const std::chrono::system_clock::time_point& tp); template std::ostream &print_vector(std::ostream &out, const std::vector &obs) { @@ -51,12 +62,27 @@ std::ostream &print_vector(std::ostream &out, const std::vector std::ostream &operator<<(std::ostream &out, const std::vector &obs) +} + +inline +std::ostream &operator<<(std::ostream &out, const std::chrono::system_clock::time_point &tp) +{ + return utils::io::print_time_point(out, tp); +} + +/** + * + * @tparam T The type of the value to print + * @param out The stream to write on + * @param obs The values to print on out + * @return Read/write reference to the stream + */ +template std::ostream &operator<<(std::ostream &out, + const std::vector &obs) { - return print_vector(out, obs); + return utils::io::print_vector(out, obs); } -} // namespace utils::io } // namespace bitrl #endif \ No newline at end of file From dd7ac00cf28412652e9e2143f31c1ab4b6caa88e Mon Sep 17 00:00:00 2001 From: pockerman Date: Wed, 21 Jan 2026 20:39:49 +0000 Subject: [PATCH 12/19] Update interface for JSONReader class. Update specification for bitrl diff drive robot (#166) --- examples/example_20/example_20.cpp | 1 - robots/bitrl_diff_drive_robot.json | 10 ++++- .../chrono_robots/chrono_diff_drive_robot.cpp | 38 +++++++++++++++++++ .../chrono_robots/chrono_diff_drive_robot.h | 6 +++ .../backends/mqtt_ultrasound_backend.h | 5 ++- src/bitrl/utils/io/json_file_reader.h | 15 ++++++-- 6 files changed, 68 insertions(+), 7 deletions(-) diff --git a/examples/example_20/example_20.cpp b/examples/example_20/example_20.cpp index b5b24640..6c8c63dd 100644 --- a/examples/example_20/example_20.cpp +++ b/examples/example_20/example_20.cpp @@ -11,7 +11,6 @@ #include #include #include -#include #include namespace example diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index 1b6229f3..e36dfb19 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -34,7 +34,15 @@ "name": "Ultrasonic", "update_rate": 50.0, "mounted_position": [0.3, 0, 0.1], - "max_distance": 5.0 + "max_distance": 5.0, + "backend": "CHRONO" + }, + { + "idx": 1, + "type": "camera", + "name": "cam-0", + "update_rate": 3, + "mounted_position": [0.3, 0, 0.1] } ] diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp index c0749359..3df2e7c2 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -17,6 +17,7 @@ #endif #include +#include #include #include @@ -74,6 +75,38 @@ position() position[i] = pos.at(i).get(); } +struct Sensor +{ + std::array position; + std::string type; + std::string name; + real_t update_rate; + uint_t idx; + // Optional fields + std::optional max_distance; + std::optional backend; + + Sensor(const json &j); +}; + +Sensor::Sensor(const json &j) +{ + idx = j.at("idx").get(); + type = j.at("type").get(); + name = j.at("name").get(); + update_rate = j.at("update_rate").get(); + + auto pos = j.at("mounted_position"); + for (size_t i = 0; i < 3; ++i) + position[i] = pos.at(i).get(); + + // Optional fields + if (j.contains("max_distance")) + max_distance = j.at("max_distance").get(); + + if (j.contains("backend")) + backend = j.at("backend").get(); +} auto build_chassis(const utils::io::JSONFileReader& json_reader) { @@ -160,6 +193,11 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) sys_.Add(caster_joint); // read the sensors attached to the robot + const auto& sensors = json_reader.get("sensors"); + for (auto it = sensors.begin(); it != sensors.end(); ++it) + { + Sensor sensor(it.value()); + } #ifdef BITRL_LOG diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h index ca559f91..823f683c 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -6,6 +6,7 @@ #ifdef BITRL_CHRONO #include "bitrl/bitrl_types.h" +#include "bitrl/sensors/sensor_manager.h" #include "chrono/physics/ChSystemSMC.h" @@ -85,6 +86,11 @@ class CHRONO_DiffDriveRobotBase */ chrono::ChSystemSMC sys_; + /** + * @brief Manages the sensors on the robot + */ + std::unique_ptr sensor_manager_; + /** * @brief Motors for the robot * motors_.first left motor diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h index 570740bb..15145077 100644 --- a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h @@ -78,7 +78,10 @@ class MQTT_UltrasonicBackend: public RangeSensorBackendBase private: - + /** + * @brief Pointer to the instance that handles the MQTT connection + * and message exchange + */ network::MqttSubscriber* subscriber_ptr_; /** diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index 5f95a49b..42abd701 100644 --- a/src/bitrl/utils/io/json_file_reader.h +++ b/src/bitrl/utils/io/json_file_reader.h @@ -18,11 +18,18 @@ class JSONFileReader final : public FileReaderBase { public: + + using json = nlohmann::json; + + /** + * Constructor + * @param filename + */ JSONFileReader(const std::string &filename); - /// - /// \brief Attempts to open the file for reading - /// + /** + * @brief Attempts to open the file for reading + */ virtual void open() override final; /** @@ -39,9 +46,9 @@ class JSONFileReader final : public FileReaderBase */ template T at(const std::string &label) const; + const json& get(const std::string &label) const{return data_.at(label);}; private: - using json = nlohmann::json; json data_; }; From d57dd9880496ac7763461fb51df82c3115c7cfcf Mon Sep 17 00:00:00 2001 From: pockerman Date: Wed, 21 Jan 2026 20:40:30 +0000 Subject: [PATCH 13/19] Add SensorManager class (#166) --- src/bitrl/sensors/sensor_manager.cpp | 34 +++++++++++++++++ src/bitrl/sensors/sensor_manager.h | 57 ++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 src/bitrl/sensors/sensor_manager.cpp create mode 100644 src/bitrl/sensors/sensor_manager.h diff --git a/src/bitrl/sensors/sensor_manager.cpp b/src/bitrl/sensors/sensor_manager.cpp new file mode 100644 index 00000000..e71f16b8 --- /dev/null +++ b/src/bitrl/sensors/sensor_manager.cpp @@ -0,0 +1,34 @@ +#include "bitrl/bitrl_consts.h" +#include "bitrl/sensors/sensor_manager.h" + +namespace bitrl +{ +namespace sensors +{ + +SensorManager::SensorManager(uint_t n_sensors) + : +n_sensors_(n_sensors) +{ + if(n_sensors > 0 && n_sensors != consts::INVALID_ID) + { + sensors_.reserve(n_sensors); + } +} + +void SensorManager::add(std::shared_ptr sensor) +{ + sensors_.push_back(sensor); +} + +void SensorManager::update() +{ + for (auto& s : sensors_) { + if (s->is_enabled()) { + s->read_values(); + } + } +} + +} +} diff --git a/src/bitrl/sensors/sensor_manager.h b/src/bitrl/sensors/sensor_manager.h new file mode 100644 index 00000000..8aefa5b5 --- /dev/null +++ b/src/bitrl/sensors/sensor_manager.h @@ -0,0 +1,57 @@ +#ifndef SENSOR_MANAGER_H +#define SENSOR_MANAGER_H + +#include "bitrl/bitrl_types.h" +#include "bitrl/sensors/sensor_base.h" +#include +#include +#include + +namespace bitrl +{ +namespace sensors +{ + +/** + * @class SensorManager + * @ingroup bitrl_sensors + * @brief Class for managing sensor updates + * + */ +class SensorManager: private boost::noncopyable +{ +public: + + explicit SensorManager(uint_t n_sensors); + + /** + * @brief Add a new sensor to manage + * @param sensor + */ + void add(std::shared_ptr sensor); + + /** + * @brief Update all the enabled sensors + */ + void update(); + + /** + * @return The number of sensors the manager handles + */ + uint_t n_sensors() const { return sensors_.size(); } + +private: + + uint_t n_sensors_; + + /** + * @brief The vailable sensors this manager has + */ + std::vector< std::shared_ptr> sensors_; + +}; +} +} + + +#endif //SENSOR_MANAGER_H From 7219814ee22a33422fa09d6fbcfc463c92fa499f Mon Sep 17 00:00:00 2001 From: pockerman Date: Fri, 23 Jan 2026 18:36:06 +0000 Subject: [PATCH 14/19] Add sensors/ directory --- sensors/ultrasonic_chrono_backend.json | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 sensors/ultrasonic_chrono_backend.json diff --git a/sensors/ultrasonic_chrono_backend.json b/sensors/ultrasonic_chrono_backend.json new file mode 100644 index 00000000..6edb1d6a --- /dev/null +++ b/sensors/ultrasonic_chrono_backend.json @@ -0,0 +1,10 @@ +{ + "idx": 0, + "type": "ultrasonic-range", + "name": "Ultrasonic", + "update_rate": 50.0, + "mounted_position": [0.3, 0, 0.1], + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO" +} \ No newline at end of file From 989032976a10681811213cb3148dc8ea331efcdf Mon Sep 17 00:00:00 2001 From: pockerman Date: Fri, 23 Jan 2026 18:36:23 +0000 Subject: [PATCH 15/19] API updates --- CMakeLists.txt | 7 +- config.h.in | 3 + examples/example_14/CMakeLists.txt | 4 +- examples/example_14/example_14.cpp | 52 +++++++++ examples/example_19/mqtt_example_3.cpp | 3 - robots/bitrl_diff_drive_robot.json | 35 ++++-- src/bitrl/bitrl_consts.h | 37 +++--- .../chrono_robots/chrono_diff_drive_robot.cpp | 108 ++++++++++-------- .../chrono_robots/chrono_diff_drive_robot.h | 33 +++++- .../backends/chrono_ultrasound_backend.cpp | 19 +++ .../backends/range_sensor_backend_base.h | 13 +++ src/bitrl/utils/io/json_file_reader.h | 8 +- 12 files changed, 242 insertions(+), 80 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index abb1585e..05a4bb94 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,7 @@ SET(ENABLE_CHRONO ON) SET(ENABLE_OPENCV ON) SET(ENABLE_MQTT ON) SET(BITRL_LOG ON) +SET(BITRL_IRRLICHT ON) # build options SET(CMAKE_BUILD_TYPE "Release") @@ -41,6 +42,7 @@ SET(CMAKE_CXX_STANDARD 20) SET(CMAKE_CXX_STANDARD_REQUIRED True) SET(CMAKE_LINKER_FLAGS "-pthread") SET(ROBOTS_DATA_DIR ${PROJECT_SOURCE_DIR}/robots) +SET(SENSORS_DATA_DIR ${PROJECT_SOURCE_DIR}/sensors) IF(CMAKE_BUILD_TYPE STREQUAL "Debug") SET(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/install/dbg) @@ -57,6 +59,7 @@ ENDIF() FIND_PACKAGE(Boost 1.74.0 REQUIRED) FIND_PACKAGE(BLAS REQUIRED) FIND_PACKAGE(Eigen3 REQUIRED) +#FIND_PACKAGE(libirrlicht-dev REQUIRED) IF(ENABLE_MQTT) FIND_PACKAGE(PahoMqttCpp REQUIRED) @@ -104,7 +107,7 @@ ENDIF() IF(ENABLE_CHRONO) - FIND_PACKAGE(Chrono REQUIRED) + FIND_PACKAGE(Chrono REQUIRED COMPONENTS Irrlicht) IF (NOT Chrono_FOUND) MESSAGE("Could not find Chrono or one of its required modules") RETURN() @@ -115,8 +118,10 @@ IF(ENABLE_CHRONO) # Manually set this as for some reason it does not work # otherwise + INCLUDE_DIRECTORIES(/usr/include/irrlicht) INCLUDE_DIRECTORIES(/usr/local/include/chrono_thirdparty/HACD) INCLUDE_DIRECTORIES(/usr/local/include/chrono/collision/bullet) + LINK_DIRECTORIES(/usr/local/lib) ENDIF() diff --git a/config.h.in b/config.h.in index b6e3d1c8..aaf7db38 100755 --- a/config.h.in +++ b/config.h.in @@ -4,6 +4,9 @@ /*Path to robots description*/ #cmakedefine ROBOTS_DATA_DIR "@ROBOTS_DATA_DIR@" +/*Path to sensors */ +#cmakedefine SENSORS_DATA_DIR "@SENSORS_DATA_DIR@" + /*DEBUG*/ #cmakedefine BITRL_DEBUG diff --git a/examples/example_14/CMakeLists.txt b/examples/example_14/CMakeLists.txt index aae6af56..635cc2f5 100644 --- a/examples/example_14/CMakeLists.txt +++ b/examples/example_14/CMakeLists.txt @@ -4,11 +4,11 @@ SET(EXECUTABLE example_14) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) - +add_compile_definitions(IRRLICHT_NO_XML) TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log) IF(ENABLE_CHRONO) - TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core) + TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core Chrono::Chrono_irrlicht) ENDIF () IF(BITRL_WEBOTS) diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp index 6d8febb3..ffab29c8 100644 --- a/examples/example_14/example_14.cpp +++ b/examples/example_14/example_14.cpp @@ -2,11 +2,20 @@ #ifdef BITRL_CHRONO + #include "bitrl/bitrl_consts.h" +#include "bitrl/sensors/ultrasonic_sensor.h" #include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" +#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" +#include "bitrl/sensors/sensor_manager.h" + +#include "chrono_irrlicht/ChVisualSystemIrrlicht.h" + namespace example14 { +using namespace bitrl; +using namespace chrono::irrlicht; using bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobotBase; } @@ -17,9 +26,52 @@ int main() CHRONO_DiffDriveRobotBase robot; robot.load_from_json(bitrl::consts::ROBOTS_DIR + "/bitrl_diff_drive_robot.json"); + // create the sensors for the rebot + auto sensor_backend = std::make_shared(robot.CHRONO_sys(), + robot.CHRONO_chassis()); + sensor_backend -> load_from_json(bitrl::consts::SENSORS_DIR + "/ultrasonic_chrono_backend.json"); + + sensors::SensorManager sensor_manager(1); + sensor_manager.add(std::make_shared(sensor_backend, "UltrasoundSensor-1")); + robot.set_sensors(sensor_manager); + + // Create the Irrlicht visualization system + auto vis = chrono_types::make_shared(); + vis->AttachSystem(&robot.CHRONO_sys()); + vis->SetWindowSize(1024, 768); + vis->SetWindowTitle("Odysseus Robot"); + vis->Initialize(); + vis->AddLogo(); + vis->AddSkyBox(); + vis->AddCamera({0, -2, 1}, {0, 0, 0}); + + vis->AddTypicalLights(); + vis->BindAll(); + + while (vis->Run()) + { + // Irrlicht must prepare frame to draw + vis->BeginScene(); + + // .. draw items belonging to Irrlicht scene, if any + vis->Render(); + + // .. draw a grid + tools::drawGrid(vis.get(), 0.5, 0.5); + // .. draw GUI items belonging to Irrlicht screen, if any + vis->GetGUIEnvironment()->drawAll(); + + robot.step(0.001); + + // Irrlicht must finish drawing the frame + vis->EndScene(); + } return 0; } + + + #else #include int main() diff --git a/examples/example_19/mqtt_example_3.cpp b/examples/example_19/mqtt_example_3.cpp index 72d30cee..4c892ddf 100644 --- a/examples/example_19/mqtt_example_3.cpp +++ b/examples/example_19/mqtt_example_3.cpp @@ -1,7 +1,4 @@ #include "bitrl/bitrl_config.h" - - - #include "bitrl/sensors/messages/ultrasound.h" #include "bitrl/sensors/ultrasonic_sensor.h" diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index e36dfb19..0ed226ae 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -3,34 +3,49 @@ "mass": 1.0, "mass_units": "kg", "chassis": { - "mass": 1.0, + "mass": 1.0, "mass_units": "kg", "position": [ 0.0, 0.0, 0.0], - "fixed": false + "fixed": false, + "visual_shape": "shape-box", + "visual_position": [0.4, 0.3, 0.05] }, "left-wheel": { "mass": 1.0, "mass_units": "kg", - "position": [ 0.0, 0.18, 0.08], - "radius": 0.05, - "radius_units": "cm" + "position": [ 0.0, 0.175, -0.02], + "radius": 0.06, + "width": 0.05, + "radius_units": "cm", + "visual_shape": "cylinder", + "rotation_axis": [1, 0, 0], + "rotation_angle_deg": 90 }, "right-wheel": { "mass": 1.0, "mass_units": "kg", - "position": [ 0.0, -0.18, 0.08], - "radius": 0.05, - "radius_units": "cm" + "position": [ 0.0, -0.175, -0.02], + "radius": 0.06, + "width": 0.05, + "radius_units": "cm", + "visual_shape": "cylinder", + "rotation_axis": [1, 0, 0], + "rotation_angle_deg": 90 }, "caster-wheel": { "mass": 0.5, "mass_units": "kg", - "position": [ -0.2, 0.0, 0.05] + "position": [ -0.2, 0.0, -0.02], + "radius": 0.06, + "width": 0.05, + "visual_shape": "cylinder", + "rotation_axis": [1, 0, 0], + "rotation_angle_deg": 90 }, "sensors": [ { "idx": 0, - "type": "distance", + "type": "ultrasonic-range", "name": "Ultrasonic", "update_rate": 50.0, "mounted_position": [0.3, 0, 0.1], diff --git a/src/bitrl/bitrl_consts.h b/src/bitrl/bitrl_consts.h index a28814b8..66b8ec59 100644 --- a/src/bitrl/bitrl_consts.h +++ b/src/bitrl/bitrl_consts.h @@ -15,35 +15,42 @@ namespace bitrl namespace consts { -/// -/// \brief INVALID_ID -/// +/** + * @brief Invalid id. + */ inline const uint_t INVALID_ID = static_cast(-1); -/// -/// \brief Invalid string -/// +/** + * @brief Invalid string + */ inline const std::string INVALID_STR = std::string("INVALID"); -/// -/// \brief Tolerance to use around the library -/// +/** + * @brief Tolerance used around the library + */ inline const real_t TOLERANCE = 1.0e-8; +/** + * @brief Path to robots definitions + */ inline const std::string ROBOTS_DIR = std::string(ROBOTS_DATA_DIR); +/** + * @brief Path to sensors definitions + */ +inline const std::string SENSORS_DIR = std::string(SENSORS_DATA_DIR); namespace maths { -/// -/// \brief The Pi constant -/// +/** + * @brief The PI constant + */ inline const real_t PI = std::numbers::pi; -/// -/// \brief Acceleration due to gravity m/secs -/// +/** + * @brief Acceleration due to gravity m/secs + */ inline const real_t G = 9.82; } // namespace maths diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp index 3df2e7c2..d91c1632 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -10,6 +10,8 @@ #include #include #include +#include +#include #ifdef BITRL_LOG @@ -35,7 +37,9 @@ using json = nlohmann::json; struct Chassis { std::array position; + std::array visual_position; std::string mass_units; + std::string visual_shape; real_t mass; bool fixed; @@ -47,11 +51,17 @@ Chassis::Chassis(const json &j) mass(j["mass"].get()), mass_units(j["mass_units"].get()), fixed(j["fixed"].get()), -position() +visual_shape(j["visual_shape"].get()), +position(), +visual_position() { auto pos = j.at("position"); for (size_t i = 0; i < 3; ++i) position[i] = pos.at(i).get(); + + auto vis_position = j.at("visual_position"); + for (size_t i = 0; i < 3; ++i) + visual_position[i] = vis_position.at(i).get(); } // helper struct to read a wheel @@ -59,54 +69,29 @@ struct Wheel { std::array position; std::string mass_units; + std::string visual_shape; real_t mass; + real_t width; + real_t radius; Wheel(const json &j); }; Wheel::Wheel(const json &j) : -mass(j["mass"].get()), +position(), mass_units(j["mass_units"].get()), -position() +visual_shape(j["visual_shape"].get()), +mass(j["mass"].get()), +width(j["width"].get()), +radius(j["radius"].get()) + { auto pos = j.at("position"); for (size_t i = 0; i < 3; ++i) position[i] = pos.at(i).get(); } -struct Sensor -{ - std::array position; - std::string type; - std::string name; - real_t update_rate; - uint_t idx; - // Optional fields - std::optional max_distance; - std::optional backend; - - Sensor(const json &j); -}; - -Sensor::Sensor(const json &j) -{ - idx = j.at("idx").get(); - type = j.at("type").get(); - name = j.at("name").get(); - update_rate = j.at("update_rate").get(); - - auto pos = j.at("mounted_position"); - for (size_t i = 0; i < 3; ++i) - position[i] = pos.at(i).get(); - - // Optional fields - if (j.contains("max_distance")) - max_distance = j.at("max_distance").get(); - - if (j.contains("backend")) - backend = j.at("backend").get(); -} auto build_chassis(const utils::io::JSONFileReader& json_reader) { @@ -116,6 +101,12 @@ auto build_chassis(const utils::io::JSONFileReader& json_reader) chassis->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); chassis->SetPos(chrono::ChVector3d(chassis_data.position[0], chassis_data.position[1], chassis_data.position[2])); chassis->SetFixed(false); + + // add visual shape for visualization + auto vis_shape = chrono_types::make_shared( + chrono::ChVector3d(chassis_data.visual_position[0], chassis_data.visual_position[1], chassis_data.visual_position[2])); + chassis -> AddVisualShape(vis_shape); + return chassis; } @@ -127,6 +118,27 @@ auto build_wheel(const utils::io::JSONFileReader& json_reader, const std::string wheel->SetMass(wheel_data.mass); wheel->SetPos(chrono::ChVector3d(wheel_data.position[0], wheel_data.position[1], wheel_data.position[2])); wheel->SetName(wheel_label); + + chrono::ChQuaternion<> q; + q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); + wheel->SetRot(q); + + auto collision = chrono_types::make_shared( + material, + wheel_data.radius, + wheel_data.width + ); + + chrono::ChQuaternion<> q_col; + q_col.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); + + wheel->AddCollisionShape( + collision, + chrono::ChFrame<>(chrono::VNULL, q_col) + ); + + wheel->AddVisualShape( + chrono_types::make_shared(wheel_data.radius, wheel_data.width)); return wheel; } @@ -165,8 +177,8 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) // set the gravity acceleration sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); - auto chassis = build_chassis(json_reader); - sys_.Add(chassis); + chassis_ = build_chassis(json_reader); + sys_.Add(chassis_); name_ = json_reader.template get_value("name"); @@ -176,8 +188,8 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) sys_.Add(left_wheel); sys_.Add(right_wheel); - auto left_motor = build_motor(left_wheel, chassis, "left-motor"); - auto right_motor = build_motor(right_wheel, chassis, "right-motor"); + auto left_motor = build_motor(left_wheel, chassis_, "left-motor"); + auto right_motor = build_motor(right_wheel, chassis_, "right-motor"); sys_.Add(left_motor); sys_.Add(right_motor); @@ -187,23 +199,29 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) auto caster_joint = chrono_types::make_shared(); caster_joint->Initialize( caster, - chassis, + chassis_, chrono::ChFrame<>(chrono::ChCoordsys<>(caster->GetPos())) ); sys_.Add(caster_joint); // read the sensors attached to the robot - const auto& sensors = json_reader.get("sensors"); - for (auto it = sensors.begin(); it != sensors.end(); ++it) - { - Sensor sensor(it.value()); - } + // const auto& sensors = json_reader.get("sensors"); + // for (auto it = sensors.begin(); it != sensors.end(); ++it) + // { + // Sensor sensor(it.value()); + // } #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; + BOOST_LOG_TRIVIAL(info)<<"Bodies in loaded robot " << sys_.GetBodies().size(); #endif } + +void CHRONO_DiffDriveRobotBase::step(real_t time_step) +{ + sys_.DoStepDynamics(time_step); +} } } #endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h index 823f683c..015ed6f4 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -7,6 +7,11 @@ #include "bitrl/bitrl_types.h" #include "bitrl/sensors/sensor_manager.h" + +#ifdef xmlChar +#undef xmlChar +#endif +//#include #include "chrono/physics/ChSystemSMC.h" @@ -56,6 +61,12 @@ class CHRONO_DiffDriveRobotBase */ void load_from_json(const std::string& filename); + /** + * @brief Step the underlying chrono::ChSystemSMC one time step + * @param time_step + */ + void step(real_t time_step); + /** * @brief The name of the robot @@ -75,6 +86,21 @@ class CHRONO_DiffDriveRobotBase */ uint_t n_motors()const noexcept{return 2;} + /** + * Set the pointer to the sensor manager + * @param sensors_manager + */ + void set_sensors(sensors::SensorManager& sensor_manager){sensor_manager_ptr_ = &sensor_manager;} + + /** + * + * @return + */ + chrono::ChSystemSMC& CHRONO_sys() noexcept{return sys_;} + + + std::shared_ptr CHRONO_chassis()noexcept{return chassis_;} + protected: /** @@ -86,10 +112,15 @@ class CHRONO_DiffDriveRobotBase */ chrono::ChSystemSMC sys_; + /** + * The chassis of the robot + */ + std::shared_ptr chassis_; + /** * @brief Manages the sensors on the robot */ - std::unique_ptr sensor_manager_; + sensors::SensorManager* sensor_manager_ptr_; /** * @brief Motors for the robot diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp index 33f227ad..162dcf58 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -7,10 +7,14 @@ #include "chrono/collision/ChCollisionSystem.h" + + #ifdef BITRL_LOG #include #endif +#include + namespace bitrl { namespace sensors::backends @@ -36,6 +40,21 @@ void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) utils::io::JSONFileReader json_reader(filename); json_reader.open(); + auto max_distance = json_reader.at("max_distance"); + this -> set_max_distance(max_distance); + + auto min_distance = json_reader.at("min_distance"); + this -> set_min_distance(max_distance); + + auto update_rate = json_reader.at("update_rate"); + this -> set_sampling_period(update_rate); + + auto name = json_reader.at("name"); + + auto pos = json_reader.get("mounted_position"); + for (size_t i = 0; i < 3; ++i) + position_[i] = pos.at(i).get(); + #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: CHRONO_UltrasonicBackend"; #endif diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.h b/src/bitrl/sensors/backends/range_sensor_backend_base.h index 53a32057..6b062617 100644 --- a/src/bitrl/sensors/backends/range_sensor_backend_base.h +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.h @@ -29,6 +29,17 @@ class RangeSensorBackendBase : public SensorBackendBase */ void set_max_distance(real_t max_distance) noexcept {max_distance_ = max_distance;} + /** + * @return The maximum distance the range sensor can read + */ + real_t min_distance()const noexcept {return min_distance_;} + + /** + * @brief Set the maximum distance the range sensor can read + * @param min_distance + */ + void set_min_distance(real_t min_distance) noexcept {min_distance_ = min_distance;} + /** * @return An instance of std::string representing the units the sensor * readings are assumed in @@ -57,6 +68,8 @@ class RangeSensorBackendBase : public SensorBackendBase */ real_t max_distance_{0.0}; + real_t min_distance_{0.0}; + }; inline RangeSensorBackendBase::RangeSensorBackendBase(const std::string &backend_type) diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index 42abd701..86687954 100644 --- a/src/bitrl/utils/io/json_file_reader.h +++ b/src/bitrl/utils/io/json_file_reader.h @@ -1,6 +1,3 @@ -// SPDX-FileCopyrightText: 2024 -// SPDX-License-Identifier: Apache-2.0 - #ifndef JSON_FILE_READER_H #define JSON_FILE_READER_H @@ -46,6 +43,11 @@ class JSONFileReader final : public FileReaderBase */ template T at(const std::string &label) const; + /** + * @param label The label to get the data + * @return Read reference to the underlying json structure that holds the data for the + * given label + */ const json& get(const std::string &label) const{return data_.at(label);}; private: From cf67c3f0eb43af9f8100792fbf87023369d8c59f Mon Sep 17 00:00:00 2001 From: pockerman Date: Sat, 24 Jan 2026 18:14:53 +0000 Subject: [PATCH 16/19] Add chrono_robot_pose headre --- .../{arduino_example.cpp => example_15.cpp} | 0 .../chrono_robots/chrono_robot_pose.h | 73 +++++++++++++++++++ 2 files changed, 73 insertions(+) rename examples/example_15/{arduino_example.cpp => example_15.cpp} (100%) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h diff --git a/examples/example_15/arduino_example.cpp b/examples/example_15/example_15.cpp similarity index 100% rename from examples/example_15/arduino_example.cpp rename to examples/example_15/example_15.cpp diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h new file mode 100644 index 00000000..43ed5299 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h @@ -0,0 +1,73 @@ +#ifndef CHRONO_ROBOT_POSE_H +#define CHRONO_ROBOT_POSE_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO +#include "bitrl/bitrl_types.h" +#include +#include +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/** + * @class CHRONO_RobotPose + * @ingroup rb_chrono + * @brief Wrapper for representing the state of a CHRONO_Robot + */ +class CHRONO_RobotPose +{ +public: + + /** + *@brief Constructr + */ + explicit CHRONO_RobotPose(std::shared_ptr robot_ptr=nullptr); + + /** + * + * @param robot_ptr + */ + void set_body(std::shared_ptr robot_ptr){robot_ptr_ = robot_ptr;} + + /** + * @brief Return the position vector + */ + chrono::ChVector3d position(){return robot_ptr_ -> GetPos();} + + /** + * @brief Return the rotation matrix + */ + chrono::ChMatrix33d rotation_matrix() const {return robot_ptr_ -> GetRotMat();} + + /** + * @brief Return the transformation of the local to the world coordinats + */ + chrono::ChVector3d to_world_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointLocalToParent(point);} + + /** + * @brief Transform the world point to local coordinates + */ + chrono::ChVector3d to_local_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointParentToLocal(point);} +private: + + /** + * @brief Pointer to the robot + */ + std::shared_ptr robot_ptr_; + +}; + +inline +CHRONO_RobotPose::CHRONO_RobotPose(std::shared_ptr robot_ptr) + : +robot_ptr_(robot_ptr) +{} +} +} + +#endif + +#endif //CHRONO_ROBOT_STATE_H From 0259b153ead468f032b35089dab22840d5899ea7 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sat, 24 Jan 2026 18:16:04 +0000 Subject: [PATCH 17/19] Various fixes for diff drive robot --- examples/example_14/example_14.cpp | 350 +++++++++++++++--- examples/example_14/example_14.md | 6 +- examples/example_15/CMakeLists.txt | 2 +- robots/bitrl_diff_drive_robot.json | 21 +- .../chrono_robots/chrono_diff_drive_robot.cpp | 115 ++++-- .../chrono_robots/chrono_diff_drive_robot.h | 53 ++- .../backends/chrono_ultrasound_backend.cpp | 2 + 7 files changed, 450 insertions(+), 99 deletions(-) diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp index ffab29c8..f5bb12f5 100644 --- a/examples/example_14/example_14.cpp +++ b/examples/example_14/example_14.cpp @@ -2,70 +2,336 @@ #ifdef BITRL_CHRONO - #include "bitrl/bitrl_consts.h" -#include "bitrl/sensors/ultrasonic_sensor.h" -#include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" -#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" -#include "bitrl/sensors/sensor_manager.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" + +#include +#include +#include +#include -#include "chrono_irrlicht/ChVisualSystemIrrlicht.h" +#ifdef BITRL_LOG +#define BOOST_LOG_DYN_LINK +#include +#endif +#include +#include namespace example14 { + using namespace bitrl; using namespace chrono::irrlicht; -using bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobotBase; + +// constants we will be using further below +const uint_t WINDOW_HEIGHT = 800; +const uint_t WINDOW_WIDTH = 1024; +const real_t DT = 0.001; +const real_t SIM_TIME = 5.0; +const std::string WINDOW_TITLE( "Example 14"); + +void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, + double scale = 1.0) { + auto* driver = vis.GetVideoDriver(); + + // X axis (red) + driver->draw3DLine( + {0, 0, 0}, + {static_cast(scale), 0, 0}, + irr::video::SColor(255, 255, 0, 0)); + + // Y axis (green) + driver->draw3DLine( + {0, 0, 0}, + {0, static_cast(scale), 0}, + irr::video::SColor(255, 0, 255, 0)); + + // Z axis (blue) + driver->draw3DLine( + {0, 0, 0}, + {0, 0, static_cast(scale)}, + irr::video::SColor(255, 0, 0, 255)); } -int main() +auto build_wheel(const std::string &wheel_label, real_t radius, + real_t width, const chrono::ChVector3d& pos) { - using namespace example14; - CHRONO_DiffDriveRobotBase robot; - robot.load_from_json(bitrl::consts::ROBOTS_DIR + "/bitrl_diff_drive_robot.json"); + auto material = chrono_types::make_shared(); + material->SetYoungModulus(2e7); // stiffness (important) + material->SetPoissonRatio(0.3); + material->SetFriction(0.9f); // traction + material->SetRestitution(0.0f); // no bouncing + + // Optional but recommended + material->SetAdhesion(0.0); + material->SetKn(2e5); // normal stiffness override + material->SetGn(40.0); // normal damping + material->SetKt(2e5); // tangential stiffness + material->SetGt(20.0); + + // rotation axis for the wheel + chrono::ChQuaternion<> q; + q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); + + auto wheel = chrono_types::make_shared(); + wheel->SetMass(1.0); + wheel->SetPos(pos); + wheel->SetName(wheel_label); + //wheel->SetRot(q); + wheel->SetRot(chrono::QUNIT); + wheel->EnableCollision(true); + + chrono::ChQuaternion<> q_cyl; + q_cyl.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); + + chrono::ChFrame<> cyl_frame(chrono::VNULL, q_cyl); + + auto cyl_shape = chrono_types::make_shared( + material, + radius, + width * 0.5 // half-length + ); + + wheel->AddCollisionShape(cyl_shape); + + wheel->AddVisualShape( + chrono_types::make_shared(radius, width)); + return wheel; +} + + +// class to model the robot +class DiffDriveRobot +{ +public: + + struct MotorHandle { + std::shared_ptr motor; + std::shared_ptr speed; + }; + + // add the components of this robot to the systme we simulate + void add_to_sys(chrono::ChSystemSMC& sys); + + // build the robot + void build(); + + // set the speeds of the motors + void set_motor_speed(real_t speed); + + // the pose of the robot + bitrl::rb::bitrl_chrono::CHRONO_RobotPose& pose()noexcept{return pose_;} + +private: + + //The chassis of the robot + std::shared_ptr chassis_; + + bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; + + std::pair, std::shared_ptr> wheels_; + std::shared_ptr caster_wheel_; + std::pair motors_; + MotorHandle caster_motor_; +}; + +DiffDriveRobot::MotorHandle build_motor(std::shared_ptr wheel, + std::shared_ptr chassis, + const std::string &motor_label) +{ + + // Build joint frame in ABSOLUTE coordinates + chrono::ChQuaterniond q; + q.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); + //chrono::ChFrame<> frame(wheel->GetPos(), q); + + // Use the wheel's actual absolute frame + chrono::ChFrame<> frame = wheel->GetFrameRefToAbs(); + + auto motor = chrono_types::make_shared(); + motor->Initialize( + wheel, + chassis, + frame); + motor->SetName(motor_label); + + // set the speed function (rad/s) + auto speed_func = chrono_types::make_shared(0.0); + motor -> SetSpeedFunction(speed_func); + + auto z = motor->GetFrame2Abs().GetRotMat().GetAxisZ(); + std::cout << "Motor Z axis: " << z << std::endl; + return {motor, speed_func}; +} + +void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) +{ + sys.Add(chassis_); + sys.Add(wheels_.first); + sys.Add(wheels_.second); + sys.AddLink(motors_.first.motor); + sys.AddLink(motors_.second.motor); + sys.Add(caster_wheel_); + + auto caster_joint = chrono_types::make_shared(); + caster_joint->Initialize( + caster_wheel_, + chassis_, + chrono::ChFrame<>(chrono::ChCoordsys<>(caster_wheel_->GetPos())) + ); + sys.Add(caster_joint); +} +void DiffDriveRobot::build() +{ + // build the chassis of the robot + + chassis_ = chrono_types::make_shared(); + chassis_->SetMass(1.0); + chassis_->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); + chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.0)); + chassis_->SetFixed(false); + + // add visual shape for visualization + auto vis_shape = chrono_types::make_shared( + chrono::ChVector3d(0.4, 0.3, 0.05)); + chassis_ -> AddVisualShape(vis_shape); - // create the sensors for the rebot - auto sensor_backend = std::make_shared(robot.CHRONO_sys(), - robot.CHRONO_chassis()); - sensor_backend -> load_from_json(bitrl::consts::SENSORS_DIR + "/ultrasonic_chrono_backend.json"); + // build the wheels of the robot + wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, -0.02)); + wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, -0.02)); + caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(-0.2, 0.0, -0.02)); - sensors::SensorManager sensor_manager(1); - sensor_manager.add(std::make_shared(sensor_backend, "UltrasoundSensor-1")); - robot.set_sensors(sensor_manager); + // build the motors of the robot + motors_.first = build_motor(wheels_.first, chassis_, "left_wheel"); + motors_.second = build_motor(wheels_.second, chassis_, "right_wheel"); + caster_motor_ = build_motor(caster_wheel_, chassis_, "caster_wheel"); + + // we want to tract the chassis pose + pose_.set_body(chassis_); + +} + +void DiffDriveRobot::set_motor_speed(real_t speed) +{ + motors_.first.speed -> SetConstant(speed); + motors_.second.speed -> SetConstant(speed); + caster_motor_.speed -> SetConstant(speed); +} + +void build_system(chrono::ChSystemSMC& sys) +{ + // create material for the ground + auto material = chrono_types::make_shared(); + + material->SetYoungModulus(2e7); // stiffness (important) + material->SetPoissonRatio(0.3); + material->SetFriction(0.9f); // traction + material->SetRestitution(0.0f); // no bouncing + + // Optional but recommended + material->SetAdhesion(0.0); + material->SetKn(2e5); // normal stiffness override + material->SetGn(40.0); // normal damping + material->SetKt(2e5); // tangential stiffness + material->SetGt(20.0); + + auto ground = chrono_types::make_shared( + 5.0, 5.0, 0.001, + 1000, + true, // visual + true, // collision + material + ); + ground->SetFixed(true); + + // set the gravity acceleration + sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -bitrl::consts::maths::G)); + //sys.Add(ground); +} + +void simulate(DiffDriveRobot& robot, chrono::ChSystemSMC& sys) +{ - // Create the Irrlicht visualization system - auto vis = chrono_types::make_shared(); - vis->AttachSystem(&robot.CHRONO_sys()); - vis->SetWindowSize(1024, 768); - vis->SetWindowTitle("Odysseus Robot"); - vis->Initialize(); - vis->AddLogo(); - vis->AddSkyBox(); - vis->AddCamera({0, -2, 1}, {0, 0, 0}); + // create the object that handles the visualization + chrono::irrlicht::ChVisualSystemIrrlicht visual; + visual.AttachSystem(&sys); + visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); + visual.SetWindowTitle(WINDOW_TITLE); + visual.Initialize(); + draw_world_axes(visual); + visual.AddLogo(); + visual.AddSkyBox(); + visual.AddCamera({0, -2, 1}, {0, 0, 0}); + visual.AddTypicalLights(); + visual.BindAll(); - vis->AddTypicalLights(); - vis->BindAll(); + real_t current_time = 0.0; + auto& pose = robot.pose(); - while (vis->Run()) + robot.set_motor_speed(5.0); + while (visual.Run()) { - // Irrlicht must prepare frame to draw - vis->BeginScene(); +#ifdef BITRL_LOG + //BOOST_LOG_TRIVIAL(info)<<"Sim time: " << current_time; +#endif + + // Irrlicht must prepare frame to draw + visual.BeginScene(); + + // .. draw items belonging to Irrlicht scene, if any + visual.Render(); - // .. draw items belonging to Irrlicht scene, if any - vis->Render(); + // .. draw a grid + tools::drawGrid(&visual, 0.5, 0.5); + draw_world_axes(visual, 1.5); + // tools::drawCoordinateSystem( + // &visual, + // chrono::ChCoordsys<>(chrono::VNULL, chrono::QUNIT), + // 0.5 // axis length + // ); + + // .. draw GUI items belonging to Irrlicht screen, if any + visual.GetGUIEnvironment()->drawAll(); + + //sys.DoStepDynamics(DT); +#ifdef BITRL_LOG + //BOOST_LOG_TRIVIAL(info)<<"Position: "<GetGUIEnvironment()->drawAll(); + // Irrlicht must finish drawing the frame + visual.EndScene(); - robot.step(0.001); + current_time += DT; +} + +} + +} + +int main() +{ + using namespace example14; + + // the system we will be simulating + chrono::ChSystemSMC sys; + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Building system...: "; +#endif + build_system(sys); + + DiffDriveRobot robot; + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Building robot...: "; +#endif + robot.build(); + robot.add_to_sys(sys); - // Irrlicht must finish drawing the frame - vis->EndScene(); - } + // simulate + simulate(robot, sys); return 0; } diff --git a/examples/example_14/example_14.md b/examples/example_14/example_14.md index a41864b5..75f2153a 100644 --- a/examples/example_14/example_14.md +++ b/examples/example_14/example_14.md @@ -7,4 +7,8 @@ Specifically, the robot we will simulate has - Two motorised wheels - An ultrasound sensor -In order to be able to run this example you need to configure bitrl with Chrono support \ No newline at end of file +In order to be able to run this example you need to configure bitrl with Chrono support. You will also +need the Irrlicht library for visualising the robot. + +The simulation that we will be developing is very simple since the purpose of this example is to +introduce some core components of the Chrono engine diff --git a/examples/example_15/CMakeLists.txt b/examples/example_15/CMakeLists.txt index b4f8b251..a92aa7e1 100644 --- a/examples/example_15/CMakeLists.txt +++ b/examples/example_15/CMakeLists.txt @@ -1,6 +1,6 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.20) -SET(EXECUTABLE arduino_example) +SET(EXECUTABLE example_15) SET(SOURCE ${EXECUTABLE}.cpp) ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE}) diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index 0ed226ae..cd323677 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -41,24 +41,5 @@ "visual_shape": "cylinder", "rotation_axis": [1, 0, 0], "rotation_angle_deg": 90 - }, - "sensors": [ - { - "idx": 0, - "type": "ultrasonic-range", - "name": "Ultrasonic", - "update_rate": 50.0, - "mounted_position": [0.3, 0, 0.1], - "max_distance": 5.0, - "backend": "CHRONO" - }, - { - "idx": 1, - "type": "camera", - "name": "cam-0", - "update_rate": 3, - "mounted_position": [0.3, 0, 0.1] - } - ] - + } } \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp index d91c1632..e85c28d0 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -113,52 +113,70 @@ auto build_chassis(const utils::io::JSONFileReader& json_reader) auto build_wheel(const utils::io::JSONFileReader& json_reader, const std::string &wheel_label) { + // read the wheel daat auto wheel_data = json_reader.template at(wheel_label); + + auto material = chrono_types::make_shared(); + + material->SetYoungModulus(2e7); // stiffness (important) + material->SetPoissonRatio(0.3); + material->SetFriction(0.9f); // traction + material->SetRestitution(0.0f); // no bouncing + + // Optional but recommended + material->SetAdhesion(0.0); + material->SetKn(2e5); // normal stiffness override + material->SetGn(40.0); // normal damping + material->SetKt(2e5); // tangential stiffness + material->SetGt(20.0); + + // rotation axis for the wheel + chrono::ChQuaternion<> q; + q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); + auto wheel = chrono_types::make_shared(); wheel->SetMass(wheel_data.mass); wheel->SetPos(chrono::ChVector3d(wheel_data.position[0], wheel_data.position[1], wheel_data.position[2])); wheel->SetName(wheel_label); - - chrono::ChQuaternion<> q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); wheel->SetRot(q); + wheel->EnableCollision(true); - auto collision = chrono_types::make_shared( + auto cyl_shape = chrono_types::make_shared( material, wheel_data.radius, - wheel_data.width - ); - - chrono::ChQuaternion<> q_col; - q_col.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); - - wheel->AddCollisionShape( - collision, - chrono::ChFrame<>(chrono::VNULL, q_col) + wheel_data.width * 0.5 // half-length ); + wheel->AddCollisionShape(cyl_shape); wheel->AddVisualShape( chrono_types::make_shared(wheel_data.radius, wheel_data.width)); return wheel; } -auto build_motor(std::shared_ptr wheel, +CHRONO_DiffDriveRobotBase::MotorHandle build_motor(std::shared_ptr wheel, std::shared_ptr chassis, const std::string &motor_label) { + + + //chrono::ChVector3d axis = chrono::VECT_Y; + + // Build joint frame in ABSOLUTE coordinates chrono::ChQuaterniond q; q.SetFromAngleAxis(consts::maths::PI * 0.5, chrono::VECT_X); chrono::ChFrame<> frame(wheel->GetPos(), q); - auto motor = chrono_types::make_shared(); + + auto motor = chrono_types::make_shared(); motor->Initialize( wheel, chassis, frame); motor->SetName(motor_label); - // set the speed function - motor -> SetSpeedFunction(chrono_types::make_shared(0.0)); - return motor; + // set the speed function (rad/s) + auto speed_func = chrono_types::make_shared(0.0); + motor -> SetSpeedFunction(speed_func); + return {motor, speed_func}; } @@ -174,14 +192,40 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) utils::io::JSONFileReader json_reader(filename); json_reader.open(); + auto material = chrono_types::make_shared(); + + material->SetYoungModulus(2e7); // stiffness (important) + material->SetPoissonRatio(0.3); + material->SetFriction(0.9f); // traction + material->SetRestitution(0.0f); // no bouncing + + // Optional but recommended + material->SetAdhesion(0.0); + material->SetKn(2e5); // normal stiffness override + material->SetGn(40.0); // normal damping + material->SetKt(2e5); // tangential stiffness + material->SetGt(20.0); + auto ground = chrono_types::make_shared( + 5.0, 5.0, 0.1, + 1000, + true, // visual + true, // collision + material + ); + ground->SetFixed(true); + + // set the gravity acceleration sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); chassis_ = build_chassis(json_reader); sys_.Add(chassis_); + //sys_.Add(ground); name_ = json_reader.template get_value("name"); + BOOST_LOG_TRIVIAL(info)<<"Building wheels...: "; + auto left_wheel = build_wheel(json_reader, "left-wheel"); auto right_wheel = build_wheel(json_reader, "right-wheel"); @@ -191,8 +235,17 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) auto left_motor = build_motor(left_wheel, chassis_, "left-motor"); auto right_motor = build_motor(right_wheel, chassis_, "right-motor"); - sys_.Add(left_motor); - sys_.Add(right_motor); + sys_.AddLink(left_motor.motor); + sys_.AddLink(right_motor.motor); +#ifdef BITRL_LOG + auto zleft = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); + BOOST_LOG_TRIVIAL(info)<<"Left motor rotation: " << zleft; + + auto zright = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); + BOOST_LOG_TRIVIAL(info)<<"Right motor rotation: " << zright; +#endif + + motors_ = std::make_pair(std::move(left_motor), std::move(right_motor)); auto caster = build_wheel(json_reader, "caster-wheel"); sys_.Add(caster); @@ -204,13 +257,8 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) ); sys_.Add(caster_joint); - // read the sensors attached to the robot - // const auto& sensors = json_reader.get("sensors"); - // for (auto it = sensors.begin(); it != sensors.end(); ++it) - // { - // Sensor sensor(it.value()); - // } - + // set upd the state + pose_ = std::make_shared(chassis_); #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; @@ -218,6 +266,19 @@ CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) #endif } +void CHRONO_DiffDriveRobotBase::set_motor_speed(const std::string& motor_name, real_t speed) +{ + if (motor_name == "left-motor") + { + motors_.first.speed -> SetConstant(speed); + } + + if (motor_name == "right-motor") + { + motors_.second.speed -> SetConstant(speed); + } +} + void CHRONO_DiffDriveRobotBase::step(real_t time_step) { sys_.DoStepDynamics(time_step); diff --git a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h index 015ed6f4..390352fd 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -7,12 +7,11 @@ #include "bitrl/bitrl_types.h" #include "bitrl/sensors/sensor_manager.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" -#ifdef xmlChar -#undef xmlChar -#endif -//#include -#include "chrono/physics/ChSystemSMC.h" + +#include +#include #include @@ -42,6 +41,17 @@ class CHRONO_DiffDriveRobotBase { public: + typedef CHRONO_RobotPose pose_type; + + /** + * Handle the motors + */ + struct MotorHandle { + std::shared_ptr motor; + std::shared_ptr speed; + }; + + /** * @brief Load robot and simulation parameters from a JSON file. * @@ -67,6 +77,18 @@ class CHRONO_DiffDriveRobotBase */ void step(real_t time_step); + /** + * @brief Set the motor speed + * @param motor_name The name of the motor + * @param speed The speed + */ + void set_motor_speed(const std::string& motor_name, real_t speed); + + /** + * @brief Set the speed for both motors + * @param speed + */ + void set_motor_speed(real_t speed); /** * @brief The name of the robot @@ -93,7 +115,6 @@ class CHRONO_DiffDriveRobotBase void set_sensors(sensors::SensorManager& sensor_manager){sensor_manager_ptr_ = &sensor_manager;} /** - * * @return */ chrono::ChSystemSMC& CHRONO_sys() noexcept{return sys_;} @@ -101,8 +122,14 @@ class CHRONO_DiffDriveRobotBase std::shared_ptr CHRONO_chassis()noexcept{return chassis_;} + /** + * @return Pointer to the state of the robot + */ + std::shared_ptr pose()noexcept{return pose_;} + protected: + /** * @brief Chrono physics system used for simulation. * @@ -117,6 +144,11 @@ class CHRONO_DiffDriveRobotBase */ std::shared_ptr chassis_; + /** + * The state of the robot + */ + std::shared_ptr pose_; + /** * @brief Manages the sensors on the robot */ @@ -127,15 +159,20 @@ class CHRONO_DiffDriveRobotBase * motors_.first left motor * motors.second right motor */ - std::pair, std::shared_ptr> motors_; + std::pair motors_; /** * @brief The name of the robot */ std::string name_; - }; + +inline void CHRONO_DiffDriveRobotBase::set_motor_speed(real_t speed) +{ + set_motor_speed("left_motor", speed); + set_motor_speed("right_motor", speed); +} } } #endif diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp index 162dcf58..0bfc77f3 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -55,6 +55,8 @@ void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) for (size_t i = 0; i < 3; ++i) position_[i] = pos.at(i).get(); + + #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: CHRONO_UltrasonicBackend"; #endif From b048e02cf10306d245c230daede80fa2f8e179d2 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sat, 31 Jan 2026 16:19:12 +0000 Subject: [PATCH 18/19] Add environment image --- examples/example_14/env.png | Bin 0 -> 49748 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 examples/example_14/env.png diff --git a/examples/example_14/env.png b/examples/example_14/env.png new file mode 100644 index 0000000000000000000000000000000000000000..7ec5b5d977e868810f74a12581e7a2774207c175 GIT binary patch literal 49748 zcmeFZcT`kO(=UnvK|v5vqC^!GBug5SfPjDm$r(YAoQE(&Qc02p1SCt6oO70(!;o`^ z89;K5b9#K9_gi)=ij{_7fWu?<(zfGQa{)7skGd^EE(riH?6jcJWt%sygaxlE?3S%n)t z$GzBoeuR7di~XbB$A&hA?Q_G~>s#w<{u5shRqHY`KgIm~b=W^AaOorVG+L#U$Dt%h zsg}=KMa$#6GU3IkrigBozEAPoF-umYPSRopK~|LxM?<&1{`%a5$PCAT!uEOBPVUdrYml20O1H%= zlhyDg+rNUrpc%zjbQuLyV|Fn5D+v#|c!Ey{z6j*xoPh9~Z_cNI-l2x-0_hf!^|+qz zmmBVddl)XWZs7f1QC)JbLS;CwaTc0Pxy}%!lZwBeVD(QGC`*T?^JhVD| zXYibio&4+CnH!HGWPY2rD~_!HG?CuuW#*!GW`v%mEXu}IUhrI4EHVa_KdsuYIE;;&8^=)-`ToknEHr+W4&8e;^fTwU7?Kp zohcb|z^oHs7uhBk@8qGuSa)@WvO6=sKwil6JS^jsVw$RLank(40tRxg5Gr@FY)$yP$sJ^R>@)~Dg0R9@DMWXOzK(NGzoCJw` zl(j`EN@Q`8ukrW{b$?Dd!cn;??P5Lm$AvzvOzsprWR1l3%@QNaVgfe*giiP@rmLsl z<(->J6^HoNTz8>_;LHaEL}WxIJ8r+}sPTX9DiFMtgZSf(OR63Ndmc4PX}dt8dNYEY z^M{_WdgI6O?o7I}d=(jNtHA4xs0Ugfu#LDsQr}?3v9!lI$p4h#Y1?x@eS_K-&qA^3 zDSe^ZL1XW|cwR@g+Gxx7oVoO&AJieJI5DSrhFOPa*FDA__3mD`Fq!)4OY?;g@$uVO zV~5YRf}41o4nJ-?{a7f&a>}MHI3MtN(dSOoZ5|m8Uz#l3#}ZOy;WqtPSTv#$K!2Np zPlp6I_J<(>HBM7?3TZ}FSTv_;=yG7;HLGW=9CkeinO0GgH#{D2mitP~M_p6@M(glF z+?!KTCu-Z_j?<%vy1bX)2ERJ1Hj5Qz;k|F69w36|TUqAeO0ERgdIfxz6Q!gzYtLh+ z4Ed&M%9s|^FXg^CdEGiUexQeBM4q(imb(dA_(Q)(ZFI&rVWGdT5tQ*DVw7zUsh(_1 z`&QD+&i52ExkJR9G>fh+2~=b)3E0f9DF(hy9}ay|lIQvZfBFT1vtOf=88k@v&)m`< zy+=QfIB?-aTzGNcycwsRZ9^=RY-ONQ7RU7|$joMf;aTSk=A)BI_YkZ{hHLtWceU@Z zZsW253wXmpOj-pO7k6qwVIB*M7EAiIsESMcpBavs;s^EX~GBDsJGG<6XBVqBxO-g!%~*9{a+d3+lP3? zbFq8Hh*IFQ|5&?Jd|iYn%HE3G%0r$GW9<@NsyUA3>FYZwywER#xEY;qmSe0m7S_F~ zHf6NIN>^n1(ckNz&B7#-3i9$`Krq*z2#6*B7QRiV4C4K5G2p%xbPH3Wne=JOnV_tQG7;|Z$`|G^wAtXEdv2+&t&HRaV(=+#*r^0H6lVlRV7 z@1)@t)~83*Zaa@sBQ=;LUN}^@ni{M<$_32UBxble;tx ze4gjCB3C}1`+3OFv&lJhA8EOB1~(SgV>l|=PYl1sqn8xE|VK7}*Jx|MVHO7Qh zv-pX3p(UK_IMP`Y0-AcK zW08v${fpj6!h#E1!`%b4-6ga7MaJC8k@B_z0W|P77S@yFqbZvLy*l_zwIlDeZL_9H zLYc?q8Kx6($ENOZRA9Tdq?thA#du=^kQ%gO*$CIb&`-RN5B+7v^Q5ABtN}hzaCI;i zO?6f#a!HspcbZ4(xu4;2neXMfqTsm{-)nu<;JMbIhu+I}W~`Xp&)jd=^Yl1fPT@}JqjT< zxZILTxcq>#U47|Gnt#55KDO~ZS0K1bYJg8NFekdrca9y`&YLv2&DS`4Y{Yq@)kCST zd}z+QuxVy5ojngj(WeRoLT6)rvrdMW0gN;*_NR%Y7t;zqszIT z2XP{CHp_3#79Kqm+?N5pfZ|6-NqVsM7W+O^N-f(Fcr2V&!<4JB6- z9(3M?3j^H8(?`L`B$(BunXB);rZ(3Emqw(*1nR`mdFI5gfCjoHQis~{Uz)4Aj1qBQ z_mAd@ud`xPoOLN6?nh5QkMrIqNVwQjZ@8SvNvxS?6yY%TlVU~BexP}Dvf_EkMY>sq z@NFIwJRzQ|++9U_EPlHDm4+yp`(C_o$b5C;DOi6Tie4Z+KHVu8d~^>Bi+jRtpWm}! zS`e*#l}P7JD10(M<2<*CUOGG6h-TBT@8y}o*Vw|CDTt9hr+1-g5}Vs*hJvmjk=T!M z%jio621Z8l9~$<9CJH*W2gQ-HeZu>w%_g8;8pnfoJ$I!%9fnfb3eIeoG?f05j3L1(GnwdR!*+kAs>AdgpSpLu{wjc5B?35_Uzj#-2Z*=y2484@! z+kbJKG-Wjt%Qz+{?|Vp7Tj6wB9>=!Fh<^8pgC7t|Q=yj-#4& zp$%7u1%CJHkID)TuXcNR=N{#m;2Ml%tI9^RUe0?FmQR}Jbar-fTa4;B^m1n;I883L za$VteS^hJtjDFx7Ak>oh7Eg#hP6SdJ8NquB9A8b)=$0M*{&3{E(1ku@%%DENW3)aW z<@P+8nu9HWO}dEG7ldVS^IpsrTpYS8d#JYo;Q;Zpv`BI68lh%d1^M|m8*WqTdF+lH z0+lU~J85jtZ@|bSb&~{$5J7-GsPH(Ae_BTdGfcH|(tFwc@Ob(A+)JvaGe+w=htsna z^^3_?^^18=&n?2MRl! z``(?qI;t4U*R6rB4Gt-Y%+Rpk#CqTJR^Im>PcD4fx0`k52&dw{P3Z;)@=-~zyXRHS zocsQ`{zcv#-(I=?X_d%Xod{%$vcc_S*_WOFv@y0~PN#0y)nZicYDa<4S5pib${Hzh z=CXHb9DO+=k~_I_>Bn~IXF{@hOq{XF%fVkY!lUiyymdJ{H&-#&>X+y`I~#r3o9=n! zOqt|-!W!pZ32!B7zud3;ShJXTp)uv}j_zzYuNx9ZrBAZy9Uaa!e5?udT*@(d5o4Q2 zfc1*Fw}3JFg=TR#YfbId*@phbBB`rq{?1|`8+g%%I-}0;zpnJvqTmHa>#ILDcaNb@ zxAoBo5i9r}+Hr7I;Syn-LwMO`lTfkIMyY$UFXD9A@4intCwQ#jxtietJnr6DwELxc z!{LHtoK3~PNBdUw3j2rAI_Un~)o!sAo$evC8*%FTB12Miusd~39V)W%I^R|xN z?mHZGAo_$d?xK^I4Xy9#)LRo+K5N|IaCF6p^st}u@tA09iK?M;8MhWW5#*_H80y40 znt^%&YtCgNEUe#B^1`Rf&)j(|$8E1pkp-G1AHg8jvBM1w7iIO&2GxL3vtQ{Fg}i;C zzuP=VxVO6g@ZIQjtWT`k&Qmt}7Zw%q2OFMf=A@|mH?S0|X4#}-j1DH4GIQ$!rk zc{ZSJc&02%95+Vq+G|61EFf4|?@ejWF`iekjCa=Mv0BdI##jMR&-Dh)8mG^e?c~X&i@V5yTG)W-!8s^p;d9u5h?^pK3-Rikp z=;jdFa^^~a#7a9_UfZ(aU}53+ZU67+>h}NsY!etq@CpA9H2Z&b%=j;fD!L|==S92c z+X_G#e5iPe+*;SKME+D7)qKIgvcR`*y*oPxWkJ#PifUv>%S!aD^4(T{%4~6SGE2t3oNW}|DSg3zxLw4_Ts#~a_L4@rMQi}CLdD0AFzn7UcqNl zFC&yG28O-tUpv^zc-hL_Sf~2_I5r8~b8a}-&3M`1@6XP!3HM)z(cf)4P8a1%$F9<& z@Ky$YTHoHjzb9`dx)h`8#1)iySoG)U20sN3ccq!h%J*D@G$MC0Wcf~xy)SAnkmK7-_F9$WR z{Qvqs?lXk01F2 zA^u$cJZP&Y*{^hXRi)d zsMLKliR;Ei5xkL2#8*n>(#&<;kB8#(81AFVLT|+5otKoK#VMnr`#_}ujEx;szBp|w zY%zV@;b|(}WEy&D>K08NoKzOlRu$ZKIAmH|M;<&?Be^s!`FNA!GduicqUGbA1gn>Q zRySFvBiaBJzPf*mM7ms;JFPMrz$L8$A(B)OvQuM)zDXp8~@bbE}SP zvbRs}gs!LKkuI@^(WG|`rQ|9q*7k-v@mATRm{iIbfz-I27D`NMYyweP{kOzh@;D+T zs0R^jIX>VL#jdqA?LB1CAuCTTUqDA1D0kusu9&f z`+BLjZ5-ol;g5-TxjFjRdiN|Kk2dBn#?$C<#=nt9uIfDCS-vYRPEbhqSErRZ8^~7Zl^iLeKGYbD@!x45kJquIZZZ{V{Mx3j^`d^e zebJEh&UUY1*n<}Oihw2#NxGnrR$|UO?PNY<8_#rJ@gVEoKA6>!qTBQryBOznGALzB z{_3V3+A1x%oWeq4Lr1??&O^dT9@xS}VgP-e81?Q%m^?5dsP(DwgW08)#XRR1@|;n- zZ=VZLONJu7V!t*Ab?|O|ee_Xhmi_fI`{Vm}TAm&TN{~^7Wf;G%H)&f;7o_(i53CR& z;_uN|mLLjlHgIALa;kCFS=#y2-x;W*V}18)-;`Hn8kU`Z=AwNf(e2cwz2%^8nB;P# z5Pful9n9oGSj8q$=Xd&bz4_B3_Ooo}f@lB4gA$~=E}c(HYK4H+|2XnRAhqV0_!Gqm zo$4KrXH*G~TG42~DeLuc>$+fJ!gRr8Ack_jbaN#=9Z)Y$B_5B{=<7SNi-t9jeZ)-j~k-q935nhyTTp-ZcBN?Io2>qJ2q(eV02+~vj{X=VQk6->*=QNr6ZnmM1=+(zEQv%)CnjL}g@{z~{ zGY_#J7t6szK{jNYS&9KRZs{&DY^@Sj&u5z+E{w|(^*^KwVr?a^$DLaU1T-o?u{8;d z(!YKw5d>r4mjt$=P$}L`RIg9gZVwx_ulUU+^8urN0DCp9!a1lQVzrU6Q`HZMV3YWy z+@+D=X3~t}K+}n9emH!pLdQ0R4NFpt()PP!e}aA6_s)IE=#@_WfbCOk{);a<>(Pr; z)}o6EO4SMGPEUfij=YBN=uvmsUl~`ivvi-5G7>U@|4)KgyCWK2AoafmCKbnfiq4)! ztz6)(EA35;vQHA41nl2?fHeNe7oFT%#h=MXC~DXAv)CKadV2p^cg`z2>MzB$RL{Bv z7K^>bTFZ{h`~AE%dpY_FUp=0^`d0nDfc*f)c-hoN z!JDnH44wFn%t{jD3~pZp3#8t9KQHaRo|?t{yG-wjn#g;X!@pA%Zx}$Iy7B)lsmrv& z#uRUairu{_!=1^apGO~b^eE38Cxu#1n0bNP5ANcidYka|Q*YtOdy-?#q!M>)B~_Yk zyt{4l)O-nFV+_ltW=k+|9wuVR!^l# zLPbcBByuODhPo+J=DUok2#KV=UX{&#O^Zc&fD~Aj)>Jl^zz*s@nOhKHL_~UR=lwmP%uKZaDkPpmeV4 z9i0ea<~N&PWp?~}bT*pIPe!vqHBS@j&MD{LQhufRQn0jYtwTZZryQPl_VWu(rU-`w zH#@ZsFD1rk_`|)Q!<_;ncWy(Oo&@DbutiO}$Yp)QE4%|!G+&Z)_Yr*dkb;g`k2dIt zeXMgyiBaTrUKCyp1ZJVd7%E$~Z1M2E^ziZi;Hb4@r#VkZ z2B&3Sz{}Or`)0n%nXff_q$2x8nT1d;!5cBM_w;W`CvK#3D#pC?+WHen&QOnb#uECP zc*@RG6)rYG?tRp7&mpw87B4U6&o8YeRP1R}R+M_Yd6h~|)G;5ZR*@}KV=;hAxZ&l* zNM+cwCNLwdOjcg=V4i$SrpvD{{Fg?gc5m%&R?PIzkJJfp+*H{;<=C!MHC2&l!^nNC zk^Y+AuGs@yV>!0Rb8-8OpFSdF?{(i44`U>TZj8!5O-K>*O8mXmx7Xc zv{E@hB|YlDt|JdKJ*ZQ=U=AuG{NgL#jxo1WhxWX!vDxtR<_*m8Y}BX+bi?H1lM61&}D`dLW1k5snAO%p4+AZsFhHLHb@ zY|z+hAy`$rgLzU6e>|QY{-9o}^eaw|t`1Wf<|~bzWwFtm$N*Y%vS?03sHR;>YYDyr zJ24+|r7NhH_xOw1KMw@Lg2w){7r>mfK!0;_*PLj( zVX}2;gubcy+U-;mssPDniYgN*zngSBKD~ zL@6Js!2zL5Kxvpwi`BAyM)36dpd@t}Pk2>n2zzy~TXI>5dMVFdW?4vuS>9tpGpfc9 zbL~y2oc!99c_x5b>Hk5kZq5s60xT?Cx}cE+gvU*Uo-$1hOU1PPw{U-df1+E#ipB#M zPnp&8|F{>r>@KExEvk_p>~*sp3u9KH!U-tyT7JT065;DvV3ARz`6ue>2Vv8RVq?tO zq^TFg0oUqL>G@Vo-Nha=`7Kq|!C+C|8R$?}Mq;diV)K2kUy(YFBdW+mS8HFu zE!7s$Ix_h2jI)B!Gq}H~g2*Kl*Wl_2r0Zvxti4*9g`$YRywJ0E(tX*Jw>FE~x7g|q z?0rRhbMRv|xmiNxd7qh$kP%d$t>=j2BI6z8*F}wUAp9NMTc*#eWL#l?CUd4jb;p`e zMQ74t$1Qh%BGpfQv=T*AReMzj`+quvb=YTKk~!vUYY%A-x|l^OOKkGVAd9&O8PK;~ zT}F}zyy5aFP{6b_d+RPmo_S_C*ne;`AWleSHDIbbLkalb7aeLsRhB?J;nF#`o>!f& z#wqZxe8=~?qvq&Rms3y=*P>1^RrJla)82NSJJ;LxVsl%-d-pLcVk&(qXKLYNg(KU# zr90V7ouAS4GX78GyJlm}yQ+`B=jzo}A2;TbL#H((lV(j-TgNt92WmFUwOce>MY0~K zj^rQraP@oNI|8ewtnNxNI4ilanu7`{O9Wk-+K`V*x@&AYPe1x&@WJ%W&} z+^&AN(rox>&1x)IWllQnF;OC9(o;Q!r>e@meS4s^783@Y7JWh=Y0cdT&2gl~wbCb> z`7E9hjs>UAe?gkNZZa@s)8M|YX(y|4SORx)=He!>$j(M8br@%Dh^kuEw;=|j?FJMlzcfCEt8Q9NU7Rz%PiLplDqk=;>Z`(%O{Yt+)*kC*Bs$pe<6LB zg;v25m2}FNTtIzfRa3Gp3hIvjl=Sy;G0XU8o3YmXDW1hC`88-~ayyqe%*^Q%PSFi5 zqLfiG$%@5!ve!8`P_SW74$_M3BvG4+8K*3hEE%oCNs9uXDlVW0D(N}!qX`pnT(HQi z;H&AMEy>)&&n!NY8Y&FmX~aRsq%-M`U?lPaFm4U@kSKVb4Z%KU$*4GPmW}z|`zSg1 z99*f7$foy;pfB)C7S%W+r_u#pvCrIfl+0E2MmSR6Ony3#O*4kJy`MZV!z+--l{^%2k+Vw3H*uE8|N_~V{zX~ymkY%3c1ME8R4D- z(`W|m5@%H|EyQZATG1X{yS%s3-Ta6g8UPR?{!*gn)oI<)B=cf!7+!Fo(&5QcD2>bf z=SViQ>3LPiLV?`0KM3PsG6w!I+J?)2v zD6q%ic~_-%y!2Fi-A)r8op)6@3bj=2{v$GI<0)dIEj^RdI@bMY;vCGjjdc^=@}*y^ z2=cac=9l?MUrK9Sx_G-ba#)+h^Ox3FO~A4Y+L34Ph2{hUZI)!85wd$c2f+M6RgTt! zSG+0;B0J=aFI3Fet_~C`!|-Z^te2Y%a<(d$LKNKBl_%OFnI2Wx4biVv zE=jT;SehvvUT%9f95R?8zaQF7lxb9Q?*^Af@ zz|hQ83i=%3)8%Ru(R&vzg$VE@p#+(q|53$Arlf5E0eBhD-_On1@n=jtS!NW8vC=tc zD*wZ8zhJ5gd}#lL0rbFsM!5gN$3gXQABEw=vKtnn^TTQyY-9s&?;wm3(qHLQ)OM(} z=JABe-B7#FVYvx1WS#|Oy#1@je7w?Sfql{;z4_+AL<{^gO<{k{cKsCEB`2?(0`|FF zM`VTVMWqRt|6GPBC^45xStk2hC8@p}hj$@x(I4Z;iV9g=SK0ok_C~&zqKR$*R}4(k z&U5aGu+<66rRsO%Q9|3b`+cVv=Gu}A!-b+*bt%M95`!=XhHUe928nj+dNAW{ul+uO z{?1|L@%T5{IfBJhu16E_ zH1rTm*969RTd_WW1RkipYNE8MwYw68oVA4EP1&a%J~&cZ15B|^wLpEWCQ76i6sK8d zRP8$m;tm@xHOSmYBGqq(!jYAiRgS*ybowP9<+i=BxlkI!4_9Ac)Jq0G0iM#-;{%|D z60Sk0&`}4^JA=L^%?P0FlKECkVQQ+Ca~|iymj_(@j7&W`dP{c^^j4yOV^!=9#f9mN{6VO#^eM%7A`_`?vv zB3T~^Ujpq)uq)zT(_(AjhQ8{8sn3uhkQ~=AQ~W9bT9cmWO9y$CAJw+zo4E$ifaE)} z4`+crY^U*b>+`o_d%;;My>Jl*4e-CiMGC~=&@aS0)$+RRj>*%=Ex|^hIjJ?`SV9cI z1=P*rI2aE&*!8MtF9?=hl0nV}?4G^ggnBnc>wCZe0JCY$#`Y_(vo*h;V3pYvFOJ!A zk&y_8AX^pzpHVFF+@T@DMbddqt7ZSfKne>K-vbiz$u!%7qchs zD|e}`iMEkxmX#~-Ym>O~v~ow2xGLML$$z0(w_s^`6TLW>jqC4g=OU-e%2pHQDh9m1 zv+l<;?NrV>DgJWi0=DJScz$_x|3Wce@hQi0315=AqxGBu0=jsIK7MINo-C{FiPh(i z3XlFDpv+f^(fMr!0N_+N2R;~kch-Xm7mW&Q$>A_Ya?=@N1 zY(_blPK$}-yod)F_2o;4+V^$CJhV(r}ZjU$A7m{`?~~MaAgA zH47wRD)HT39gXbrrsQMbfs5Ly<6x#KTK#_5Gxpsq2A|$Qwgwn{e0j1q(+BI`K&A%L ze0=*>24&ec-7zTaWeoF2Gweoa8NeL=4`1;v)R@a(VnM!FBI~kcfA5k*SbRf-$C_iS zoyMEg_1%G5I$QCTN^hRvTPac6p?aP`TBq%VfwJ8o@z{&N5!`KwD8h8MetV1uikZJU zgcj?Q*m#)v5TPK<511)i;sOLZe8%)#tu&s6%X2up$ycgu=K5T!K-?a@H6fD`{j!#g zo7$a2_sP2Bc*4NA@;3bDOl5Xv`4QMN2jZOf_*8bLvKHDEJ33_r^=Ia|$w{w~wSUoN zqPEE`q3BUv@$cEXg@vTz7_3O-7orUw^Z-b&NsI#;P$g%s?cDKj^e6%qFFqofOO?Fy z`-aHr$&$iLa)@;AWDE?NSe1o0(zgWM;$1qov!SFuerS(HAltNTx{2gq7P36O z+!2pZX-eTQVvKi4CFSI6gc3EO+}Z>`S*L8yeVK|B?N6U%_BFRSuMq2}5YLFq;W68! z^#Qd6RlD%0dLw_s{U5lfjb_X#*B4M=N`P9gYW*U5Yw|cqUKUGNW_P>8*pnQ3%gQaU zSM40UfzQ428KyT%b(AS4aw!pjp7l&7~RT7XXow8O7*DR<=cps0t)x=~sTSF~+7b${1Rr~X($SMBz zuZa7S?TnwSQ#Iy(*4Pmr;7*>jJ%?KX@2G5u5_G81)wVq-EGuZZI_neIj`y^lvcL-o z(Gak>pI~6kY>pSwr|3E$iX(h9zI2LIQ`NSGPD}#72zA&WBYC82wF$tk)1WltN>W^A z`8;ZLM9=f`u#S-AZTeE5l?G@XZGPT(fMF-zL3xaaZ5vo(kiOu%Ik5C`l6Rx1_a~Cm zI$Fl2;nInW)+D);IVx2ORDxeSM=pEjpOh;2}yM&(ChBxEX$yS#GtfPsI*lh!fO~f1Yi=l7N>nz zta4fSs--s{s_P)m-zNe~1MXqdtN?r}MHfV!fE8P2F?J+OwtSntZ?0b8B48>oxnkmD z+(oMLFv+8t!7{KTmzpFXS-HnbL)bS|rMm1~8XAC*ueqA^A{0g;q-ZE}6sk^8lcR|- zQc_7%g|)I7x(1t@6?4>Ld;p#9hlXIJh%=hV9NxzcF#+sF7R)H5iyT+=x5k3z!DX9I ze^AG;!}+e8Hhm*M=zv_z)TS5oDtEh*Lw`I5hNk)k<7g+MJDP3IEF*y)$a!`U!J=EE zUjP1nf*~|VA=j}CFY34nMV~q}@|R@{UKZU$JxJAt8)8c_YMle5yp2S}NR81 z;3Iil0}yfbi9pm&T|%rig!9Us#Seg~YevPdNcL}oW$2R@M&Wlx&;$EaDb^&BAbAMH zGY_#3n867V-0PqR5~UFOBCu8Y7^BHZ)UMzTMCw>+P47tp47zTgigSR0rs&;IjnG^~ zQP-3}HT9P@Y>-}J{Pc^lUv40M__VIRueKOOGNFd3_*CVwC>EO~)I(;qKLG7oP1la3 zQbuKvVf4fVnz*<7NXYV(&hODh$Ky~Ea;lD?>^_v{YT zgB2c{Jg3RvL4K)F9qsbkoVa7JwSJGIk{BsQ7MtxQRqyw3wI&qJ{Z~U{oA_`eiTZ6) z&GjtWf!m_=y%B+@)ONZeBA;tT`W;=%W1smU4E2;jYC;svr)s`?u_a4B+cKt%fFF3Ves;T0*e$?;|S^%xqHr+eAq8O=!xI_Cygv|b7 z*M2j>d6wMQKW$0g#ueO3Dyt=x_6yAzgh_@z^~pnccofy9Z=>1Dj6BgM3a0J%sZ=5Y$Uyp=+f2_ z&U4BK4%2kyFq#IAq9YW!coT-`+IXSQ4zd(lFrO9B2l6&!{5M1RN>b&X=8VfyFvf#} zC(0s^mDkvx{^;Wo65sq0O=yAdwPtramGaiW;|Lw~Zn*(R^Q)&rY0cnhLDPHSb=LAc zsy59}0g5IPLop&^GMEbrFVds+CP7kAMowMN;1od3@-@)8CpdCLAab#zG!!|eHd`YB z2RLsHy7DmUn0{?YULdanx`ODAYxrEZo}B}d5J@sXtL|Ec?ZJYi*=JVRt5vXsI6ED0 zaiW|$_ZKI-e~RO3TUQxRV}GxxaQj9EnK?L$BGspLgoy;Pa_WM21OO0KY)n|;?78y1 zE5iq{hJ4Oe|7xRa8~B=gO}p(h`bO1zce-7}fK7<(EBpk?=UE3;$iz5LxX(%}>9eaf zXTBd>T4XcTSa758jpUA>HFRfznryy_F`|rG@8i1z+1OQ1yCRuM+4OYG;Rkz*c+g*N zAm7-0eMwEv?Xzf?s{u4?ZLKSiZD4)xUK2R7ajP?xn1#I$Q2)_zkzcd5zJieWmB9-5 zlI^ysUi-!V7DQ{;RvrrPo5-b?$l1Ju?PA+UIluU^1@?vrUpwe%a}E6|5#YPhLpBX-JnZCb=t$tvPGiRt+% zU=V!;;>lyXb}CYfYsk6uOV_M}TriihwbTK(g6BX@?=|rw<0Cvjn}dN+bVF~uZ@XTG zYCm{e@jQs+m3n64@}%6rTdvJceouU%H-5f&%iHX$=Y%7elLBke)Z(6D|Fk;x@K$HIRO%0BQUE4>!#(N<0nM415;vO&v0^aQ0;eEBQ5(!Ai z@-9~6Y(N-b-}OA5axNS_d3Fe7)I}_ti)2T39nrpNCtJzueyt z5% HsKa7F!)gRJC_5Rf*tEz_$mw=69@n>!!k;+1oNfRdw&J;Yu6`UY9ni1sykko6{EScaVj%4z)*^q+o&7x%mHTHI@aX7X zm)F|A8&n!qv9NwhuvVj*WLZzQn$Y0v-aWC*rfelbgLApy1i5Q$=Qr-^qdr~=s7-}nK`d(ntP zBU2&HOhD)8w+Q�bZg9?g=f&u0|xET$dIAVS^BzH!uw4!`E5X72Q57QCS1HK8wSc ze&&P*(~uy=D-ft{ff-*!GC-#WXuaJid=F5M zA3f-7pqd~QxKZHI?z{UrQGFr6i<$#G;;kb5Nd}U`v2U#n$fhfRlL%h~hUdU5v{$KM1kkXXqVajyb&4F1U{*H;{B$*_TElYPANPb9#$ueKHXL+qH8gdb13~{S0Dw**c5;v*GPB-KZ|JuwXFg6R;RjP%vjopa5r7__{Oz%4zx$`a zTl<;US!8gvJXc-vW&)e~)72g_2$}*z5G2LR!AX?|`r>{NfwvZ%o@5Eucd6Fb*i|Ax z#|Pjbev}#FGFz%e=Th( zD)`BaW>$#@yoYF!v3&Zdkyn2k!qw@h;5*lW_%_z`l`7>W3_zE!U#0fXNR zz%ahH7zX3YJvqCOnHo}yzQPG8c=wl7aw}>oy&C(>GhlqUu9HK} z6W-?MR6m5<>cU6N#*;fJFo!(Lr>rTR{hrC0V_vDuo!;vS!r$rZV45%JH7rjY_^uks zK~P4dD;Q-8LS`sh1whZt!eJooSCAx?F-0d6L4jdrLSY%u!lR3a4oY*o(I>#(GHX75 z=U1h))?;C`1&Fg;zGel+|Ej(D){wX#Liq+t4rE&DM|Iz}kj3Cv%SU=9jCwL3C~BQi zC~s2iLs2vLgjoFh8YKG|xS^)!6#vFEI&Q&}5RDI41p9u7iP2|+XWVMzzAfAPOm>z6_Pc6=<0|sieqfinpxmIlPz|1f> zEbc%V6T6hmQ>;NW9wR8cZVoy4BC>9v5MT4O4Fu7&bVETGBM<_PgWPQRrKwp&#hiyL zNT)xqmpmXie7Uwm0Ycd|o~-3JsV~X$@>#R#1rGo?<7!xmnN(dOv^I*kIrrQB-*iDxqRV0bLhwC|uP!h&;bb1-HM z^)^Cg^y{EeBZ2Z?$HJ@QY1KTD_qg0z{+670UB1hYNBqIVcbuUkNi zrJq_nu^RlFn*alF@k(nlZI}X_pBCTt;_Bl|{loAa!(w~v+fDW@gM;}tE_AFPeys1F zpilK#q;_Yw^ezJ?4-Ik!x^8#nCZ~)NpFn09xM{vJH1qJU{zwI@GW8TZ(!*)t+|@Hi z-vlB33|2OPi_d)YqU-UjACJUM+_wxC; z(KrDmZ#=IsI7Etb+Fu>(c6eeu@S5XK-hdqbN#xW}O2M32(wJ#nMRg}Qf;eV zSPgZpc+(%0#%04P7SR)z<`F(z(m5|De3+$hD2$qUHiK@zh%@DM@ym`q;6h;(4e!^~ z-kK?=Rs{B(iJZk^AIebcfos+6*k_QV*V(7CmElPtzbpnD=&DZ`Vq4ag(>enlGCNJ% zwNvmQ((2?s)^Sx2bHOw2g>rX)xom zvY=sdFU=eyK&W*j`~&T@CfIGNWI${H#rxh>3n8IWQtZ=C-caGGvOdfiehCtc)<5jU zp^b9%M~ElyB2LeHVqa%y|N0zL($prkkOTJiR9o~)W6mKBPPoXAkH$u zUGOHBe^xIYAWUTe$fq#uXAH(f|9yil9wN!mRLvw-PE-0lXxGm~^`9EsPE+(^YqHMjjNsjllTS;%=jjp*rLB)1VN6 zt#ObkeBIpwhAPxp(t^{1TGu=#9`s$*qe3WjpoqJ$(@a=k5QF-AYhLcx>@!`rm@ogl zPMGmo7WD5D2y@NbK_FA8Sxj)8&8rQd$C@`#StJ);KZH0f02kAx1*3?1rqq}Il|3L3 zEuXgUo^5Z9jybZqleok8f=i30JE)(tL@xS@i7w+(_mOL8SXSTJk%@%qwVo; zFuK@{KbRKLjX#pHXB;AOb;ABTcn}svo-D1}-9uAuw54nmlXVR$@Z6tLY{ z5+sn?-Z4)Og&1yZ*)ct8n00l&O=AZh$kt-Ov;rgkF(RwtjfvjB zml@f{k|+m|6!!_ezrNW0WK(&Bv*=8v>}^B;uuAm_7>&REc_U`j#cv5+FLGR&zB6Oa z-SCXyzBqC^<9$T_(etvNe?!%N;k8(3nSyHf!8!gz$&9ItzUHhnaEU)_Vosf4ofJha zpD3)#LY#y7k%6Y2FldT#^(5e6?_f=0&_I$&*C^)b2=qsBHVw3+jt~N!Ud7fDYDgI_oR0c!L51fwo^jNDJSWcFDCEXkZuDkB8bq5oF zQ^H`?o8kzO@xeXO<7~}fAEh~R1UvnxQLFGz#wg4EU`;9MZ?IXuKQK^$N6Om-;w+hnd94EWUXbgbKKJJ&iq!FyyTdsSXv3hYo@VUL!e=kHhXRv99s*5+w>5MsTy)i>XLa5) ziTtiC55hSYu*pN8b{Of0Zc9LHKJ{X?c*JQ@A;u+lKoegNvH)PwM&f8qBk612IeH_XQTbmyiuy@K@Au_tm|FA%$&#MJA5?$7PZP-D4}@CkuXHVh*gj z8*Zj5tO^rp_fDFu2M==pNmV!iSHS~F5aDjQ5AP&H^}owZZkge1V8e@y-jS9 zq;}vxo}J_VtBis@RIgAW*WDb9x~lp){>b`*9}NK=03uN4kx@oNW_J>3zmUkuhX(QV zL9UUbEEvz|P)3rBUntHHwz|0)c=Oe@$3fOOyrQB0i+_2VQ0$+@WT6dFt9h&Mv_T8= z4M4m_`0*2jLz($(S`&B`4DB53^BxK?2S91^?6sw;cG9yvu!tQ4@P9=_l2(E{eR(#R zvUz6}Z+QOzroa3{XjiyS*n*)bt~N-(cZ9=uvbVlNF0v#nx!)v#^ky|+pjfF-_?;GJC9aN8 z@=dx)_PTN)=AEu#G?hDCl&j&ZXAr_>nKmrOGtSGDpKM#$8WRikSJKFXzY zxtTYE&YOWW?~WsvBNTwDG^kgjZ6r0G_h;}hFuVWhLv0k)Ftk!dqeWbutBs|dX0Qdb zYh<_&(x{*0;5g=J7aIkE1a-ETXcXg*lNGKeYe#Tv znM(^fGtx9UI&Kiyec(g32s3NgF_W!E{k^ww&g_Rk=EuK^D|LRjpotEC3fP_ zc0*LPNM=A2q&vYr6rzlXy8v!tU__-309~ruuQRJuDepM6Xq%IWqq3X9^pHm)-OUEB zh)NEqX)LQb%r~|a+@!86B2bH19k^bdkf%zEmI}^j743KkG#p{8K27$J|2y2x`Uga8W%0Bbjt7;sN zP$+GTF43Cpc&teqp#Vb-5&12iF6EBF`U>WL1PdcJxp$p7%wx*-mxJ+23@;C~>$Fp~ zRTdvIY}yjPYI7?h{^eK7|EfLth+%f`+%dDCEIj&2ymnN)ME*`n!%@oV?RlnL{^&}7 z?%WAJRM&USyaBJeR_jgebytNS*Lm|NDEpyw4)>xEoT0stiHgR&ZplRJD!(9xLfvf` z87pl^tq536^W|Phm~d9$LWqWG-{#)~sv6{crm_k?;)u3HH(xu0bh3P4kdyc|SrLIM zC^A{&CvEd%>OT4@BRswq)ag#Z0uWGcT$)iLt2uX=oeJuPLd;jm-12{V!u^TroK+eR z@h^6zdi-7aUU36;9#ue!?E)aZ8(f>KW{2*piixuU48|<|3RbhEnp-sT3~@?!y$F_{ zP>}=`6JARMnf8gR`{CWqxO1U;^PawOK}-%t2eBIon}3ATN4?6G+4)Nf68xw;5 zr8W$Ny-b;yANevvmW9JpO5OKCS+;e zCrT8H%7oP$!ZMS5xxY=?H$%u{ zg92k?%?YkCmJX55W=NPIYcuYE?jG|p<8JNd;ZU7b ze3Ko-F}p>25~{b7q)^XnJ^DNDk70gc9HxmsL?dZSFHCmp(Yp zvJC2X8G{?U^kK2Sb$obV6+ay*&h+(zrfMPGjQ!9k7?1f|3)U+ z7%vt$FGdt2%CF)uNP1CIXIx{a>3k&2RwcKns_VOA-ayMmldb5e$F@R#h0t?2ylua- z@E2vRNWGP!%f~0LurGF*O%CsRO~9UCJO&e(2#CYE1>TR_ZS8pDtBl7Q2lOZTMM`f; zXHUG)n<@)M7CjvUkm)WUqds`#rFGk<&<2I46}6N1^G89v2EZfju@G0mGv?B3ulf$R zdXQ2ORkcGIr{nx*Mo-K2; z^`2}h70$9)(2+^m#2bh-W{CMVg3-IQefVsxrzTkk@qbANxyPPx2u%tg3yuclh#kNtapr->KY(Y?qFS!ITA@Cuik37Em#Dly9!P z?(ya)S^qckensQDJUYDXG>dq_6`6R#44t5f(D1+&(QFG|F}zRUvJX*gKPDmlrrM}` zE)&w(N63#xwZF!hH@LHhTljq4R_pD{viDPh2JDx_hC`6*+=mqzr+>joTumF6(_Zc3 zk2>XFIBgI~y?im|5r^7dBYl0ptAdy}^m*%96H|SE)L`qV1004~T!9Qf2s_!m?OU~k z3|IxSK8xmOi@IQYmp2g!<6_EvZIGglx4uZF>IE6#ATbIX_FY3!N^_a;VsBOnD63fvgf;mChENy^i<8B_@K5ByGbw_Tl83KGcZMUO(iW< zJoZEBKzi3T#a~=s&}Yh(evRZs3gi}^6~K-!m^n?rj`^jNAubxX)eO?&yrBn8G<_RQ zEA1Zo&>g_&o!ZPWeK;}dRPY{HC6)m;z`Da4;ikO4jp|W}sEISgU1e&%g;+Xma7B5T zsDz$W&1GV&edvx_pyV(BMmeKoNjtI|GfA;388?@$ZqsI0@4x@}mnKQ&xaBYmhw;e`d(>mw0Pg`?xYGEVm~n0xXoh_g0XtWsgi`@tf`N)~A=UHDQ0d zMDt|jr9u8SSv`T&2h0DhMtu`|| z`SXI2QL3KTfriCZxp5X|4PUYRc4j411N+AJ{H)X1`UFIZHFMIQxzC4zz|6E0voBTO zxAYMSwSQ`YME|LoE}aipdC9hhCXii-)Vlq?uHC1k+gbqgeao8?Y*V_kc3WV$!pnpD zs%HIfRjK(9RW#o*bryc44?4g`@3Mf#H8J4THgIl-a%UAc8F`P-Cyr}}l=KC3CGvd_ zf4hv26ei00`GG-_5F|Zx&)!_FC+M9)sBE6Y~^;TrH4rJxyf|Q%x zdlwB=H-iR0a+8gks`22`LwT_9JrxOK4HPp@i`eKrZXascKG|VAB#xSQ<2dzV!-=e< z;8Q806)NOCQ>vD;Y>bAs+t>*8MYfWJ_vUBYiOY*vE+i)++hXj!W7O!6fwtJ)zwhUr z$tE{3p0JVTgZ@sNF{`mD1=Vde+pG2u%{>ie)wEC-V9a_McC?XniBj_cwgMS7s@)i2 z>6v%$sd3sk+pQgg^aim=GakIUHso*<(^4s5OnjlC=LWXPT5jBwFZCjfHhIjCR7Snn zPX18oFi3s>{$80nIwq%0*F@<#W#2gEAZdl|G#f%m)%}q9fRfg>e~7(f88((^mcwba z^6x;si>1%5O?j=|=H&`AS9r1`!<`C7X$#Bkl$5=sjr?>Vv3KCgGd`bA71;Y6;uhizMy1~ItT&_!o03T(??L?cuSbUsGM55 zIq4TV&d4LwLwrs(9mzVR#yYgrm8V!NZ(cYOQoDwtF24S-WWZJ#;mhVwsI)n5=eT5` zzC$guH>9JJnEi=npWWOYo=ly<|1%=Thi8C>#w_R0cJl4VV~PVfoUY%2OX_8YZUZ;( z9zE(SIRiWz%yUUsK0Vh-yclT9UOIh~2J5VJW_!Ep54rZ~cpqOW;v=eE+7Mh}J;!*ZqPoy^4#p+xPf{=fWajgh`D?&Q<41zRRzb48?~P1><=~>|4rl4-R0O zBX`U<0Xikon>}yt^=q_`4`dY>HM&8)93yYi_i|x)I zJ~ccyCJ{?W#75=2*1`!7`idReFM*yS8&H15OYQFiXitYr0#TC@IoWjL^%l{|nV~ne z!Bz;_Kt$p`S4ch>o(hXAzA_YP{zS3o>C4`SZw2BtBp0MDcZY}dwlnB#e05U9dc<+M?7wqsr6U$BSdOga)y?k&P!&p`wAzfO(;=KDDLuw|a;ADkC>=JOCBACvNg}=$1 zLW|=flg#cX#k+^eTr%~f3!xdrd6LwW#rJ+0ekG;46C&j2z2`607_)A195HFZ-;qOOW_4%fqe$Onuh|xAmm0f~`}F!biA-wt6(UGxLB$?rO7i%kgXZtWn`wJMV&0Cu z*Bd%9C#?tJr|X&3VuX*9qJw`OgH-<7%$GSW4rISxsZBt1h`j>!+p*o}*VA&A%A)?N z^ItZ+U7@d0XH($M7WU;H|4DnxacvI+E>1*|5eySo-y(Lh_M0B9k!nxjOBvG14shdPB8_o5579%!fPE zJzE4o%QzlToq0IuBS{Xg8hnc8x-ABNv&j_4*b2v9{o@|p3b&q;0K6j(=Om%m9Yj7%*ZdqC(^itR(a)@Q>JIf{I$Kqu zRW}we2pEQX_f-B%0k-wDFvjPrs>V4mDY-cmkgk>t*7rlloyT8)T>)RPM z4+>Z_-j%;5N;y(+1Q-zM$c8PS9R~iITXmZ(w~yOCd+e;wE3W3iwe zF>>AKz{u2(yktsPAl}jZ-Put1ENAzJGm9*H%+or{|8yK}-Ux zVd5F4;pv0O^tylQ7#&2oz+MrR6QEswwPy|PuL4r@>&I`jd1W0w7S&|Pu`%tsx;LOQMcK)i;N?CD3wfXp< zYa{iHcVELlT6&J%9%xnM3&kE^zc|SC$>ICbGgfKL5(JMM@N%@{zTL1i3Y2a6v>*YG z&lX4ZKsTbWbd#@UhTiGCqusjRk|I8cUDwkQSA3%MHHGT5=P_JGF8}J;8*hUH|Mbr{ z%{n)>peK*(lH~gG#b2tDEY#!W_)pdaW_pZ)qz7*bc-V-Bv{OKM2`HX$M-AP1C@B-e zYVHx1ke>Tts9RUmqNgP2be-++7Zp5F8j+Cvt#bqn#`u&-8}I<`lyJp@aBxkd$oGrN z%nh*vf!$W2mAm|vh9b!RN*mISZ?`JHpSKnCI3pJ!a~t_KxY6HMD9Z1#<5W$8@zR|H zuU?4O-rt6#H4bb4zt(K)Xu@E|nI}WZQX;}GuiO>}hQMRUDzTx~sVsTEEAS5m8;Ifb z?5k0xXF28P6?uJ+Xu6h)k9Zokc}{OvFy(3{;F$2+Lh-yt9?Ii)M1>}V^GQ$N!|v*Bu5cKPzdPM~u-5eWb=+d;-|U2t1~&AMWGT8jJ`ui-9bSk|-h zwEE$!BK&J|fakD_Dlyerri`qnb((8V%&inTaNa3i;&U7dE?))#YREJz3d2$W%ob2Q zJz18WX(*#XB#uhGmh?f{7-bVWsk28RmsQ%ih4EpVMtFzt%4tACJzZ#*ZHIrwg<#)o zcj7Ap_s0*wAq8)`=TmkCUt#)dIRVO=1`6ZOZ#PwzW-9lQv^ zBUZv9rkfx{Lk3bS_bXB5>vfwYuC)_~))M+`r~w$4touo$Z{HKD*xih?)r@F|I#XWT zhJ!kgajV-XvNg7iPt)B8V^W)>*!NH}ro8jP<4MHjj*Ms=EdT262SpB#2*C zeJ??EHRB{M$o-2VTM$Nh0G&`yCVw7FNjQ(2@ruowjzi+(p=E!;q9Nuk@25TPtC9dU zGaCP6>vPL9LnR295hmq~v}Mwteqm%O%v6}7PqS*D(zogV&&cxyrK}jd?9^qpkw4?R z&GfQTOEI=l1-Gx%k21iL>@|ut_>W2=fRwc8l8Hje{v|Kic$}{%g|F$CNsJ5V7s&^T zwfnrYHSx^mRZOBPG6qKD{}DrxjwO;1?5a(0iSqyCKDMHDs-bOC&KPH1$&6= ziwOpNq3+Rt5GM2G(Q}#nJ6zjl%#5EplV65(@ybuqsN-z;9(Ool7*^*OS0a)%A$@p7 z^Fq_nfH8|d@5;9?ZZz}32{TH1`d>}!|893WOTnn(4>(o{YTVM{cpZaA`vEE*u^Ol;PcKe6ei%0T>;l($4!h=o@wKtdc=5#~$fgY(Mo0k% zF7R@%qJ#9pRK#D=jZ*dHLfh8{1nZSVGrv_+*1OJsP@PLi(@~banodh9LRa8nGaF%_ zF<16vmY$03sqeUd{Q87CYrd(Sg{3ghy7h|82btG659ECY8tmy}@qUsB+nvX2o&Gy|qMIF^(W@4BsgV6OsB45*`~t!bltCIu z_OSYgXF)d(jvGkF%4?;O))EtyUD^EMB-7Z^zhqqSR7%2f(Q;U?*yWFZJo5Hi17B^3 zbiLD+|6Tb&I|bI(Z(wa*RGz5sKqf}%kJ}07Mj3@@^ty=ffNgJwybME56a#g*O1QJ2&o6~bS+Wdc*hb`+s8^! z>Wuc-FdXc`wU;^(?W!Y|lt#&Cpv2{VOsIcxaq;_-nF1+O{YAm}odByG?s~T+nNXQq z%90~Siw}4}iiPAZw)Hn!!1f>nr=nkDxES92d=BZy;K%JCBH6q6U4VmPJ*rTijRvkx z(V@rGo`5*{eM8fqbUYI@7skb%Y_#Yfzmsdt6;UFx12+0K?9!%EhXKU`t=cuu7G>ZB zLf!zzC((ewU3;6GtE4vU>zAep!w?M(<2p*ltnornp*!Cibvu1UV%&4^4A*e%Q|&ra zA8@>%CJ7)@fbOGR19wsYzvr&>Clk@?b_}fTE8JnX( zq33r*F8WqPWK71GnfXSt#P!6rI2NWBysaxE(=P|XBDB*ju{#} z`)v^e2yRWiGkvZ{X4arP`A?6n7$j~wc==k{8@9CP{`sTnSH5~F@VGkYUG1q-+Ydtb2VwcAiF={C8xK$K|YwIVYm&es9mu=={qgz>Zz-7nL;wYo9=~^|D zW)Cn{pkIFdg!_VV%xHyywaV14^NkJEYsTdX8jSl*H2hQb0T{{7WX<|_+67vZ=2@kR^ctxx7dI1o`DDVS3hS^KRst)jyEa|^>(2Q zgn1lBnfrj7&Zdu{#Pctt#V39-EYg-CS;H9T1?4$^=W!*1|KqSGWvu!Scl-p(q}m0a zwSXhB)?!G@2Q$+hVwp-3pGib;Xl#+y@p_+H1r#D=&p?)D6I970?wA#tx^Aho^&0y( zY;XSOF{lHG-hWr5&c-mb#4doO5pXI(gB~qBcO@;}G_o@phS~OQuB(UYkb#N${!$?UeZpF>b z#I=UE(Kod+^%)$k{>WejQ#Sjp0iI4%uXbQTz;P?7!&#ER} z(>n-S)X5+Y4K)Cwz`HX|(yFYwUIAykU36QE2T6iAa4G+-bxTES^r`E^VaR|SgS{c`%Cw(lS6V}JP^ zJ%=U})vXT@BA|RVfZzdufql{en7d-G)3xAz1VM-NBul?f$l?^CwHzm64CyfDSd4vU z$8LLtv~7OYb%u%E@clK%m6&~FB*s+S=hTmD#(m2*VBG|aneUcmI1g*lVUCg?Z0pX= zK4Vkn9{#7XA4DPk+o-E_R1j>9-d10maX1?U3mt_v1fJdts|THur7I}2duX$Rwhr2I z2QX5^XZe%U&sYP#$YtDIOQ(9A&K)CTBt><oe2)t%yZ67!xW5(|v=(%g*L5n=* zt20U|DjPcpGx;}`!qC?Y$x*j<Q`uP=SKbvcFTmlXBy_goU^PNf41Lgsvbp9T z-j7Q%!u!7Mhc6e)Vh&xji1*?(wEc6EeC+n-YSw3NJikKP5j*hGhU?isbvVeKJ-Yfi zZ$UMjGbn*Z@Z>*Tp-=pl_|S~4_n+GIYw*Cmeynw7lNL13AnR$X@adz%7LU=AYH)LV zjG#-mt&e+}j6uZJABlxZ=7kwvZ|T>$!GD<79rAsUsd*^<%HQTxU@gE9Sub?pq(ItY z3vjn%s8QoAe9`=G^|T+)aAN3dc@}G4iJt{x9B?Mm=?v+#l9GyZIDfD-`TiqmvK;y>`|Po_(9O{p167W+2Q|pstijG9~p~)X12rsVW*48hWXwSy_wCO zwHOddp;BS`8~#Ug4oxdG(jNuS8-|3QUu<|LC7^vx+0*OAsY!*~X}PjzqDbT!=AvS( zXA^LyZ7MN$R_QNtY&2&M4OUyT#GUPxt-VAfF}4RfvuU&*bUZmjK)Wz0|KOD=TqB0M zqRyUg1hU+v7b3sA)n%H{~tO&LH1BGtla1A)S zHcF4z-6kzw=wLPcWS(Du#v+mki(RbB2Ax6<yfpjRhkNEEIxfR`?>Szl%iJpI@GUhLL<+jWhFeG^$MGbtC?#u_| z+21+K^$o$5lE6LWDMoMRWNOdR*G)C`R$Vy$#H}23q7Ytt6*_P+gX^0dbKP@2wtNta zWl75eA@BUb9TlmTT%gX=v)FJz9<_tf>`Vs4}Zk|(5 z)Ei=}V6V#we6WTzFAoGsop<-WCWtx+*^p`Il^iO2y?`phVKGV z2cs^QJmMNtaZv5Uw5l^*^%2dnxv3KqH5+4z)0gEJIgYHpJvXj1g4?56di}@pIDL2P zr^Y+qL~QNF%&!VS=epS{0=F4;*3G-KnMN{HP7!;9RqarcO$7H+5P&$rj=tBYzT%yv z^Xc7;7odC6cX_s#F_Ag1hs?9~iOa^2RC7L${P28mWP1|kAubrJFQ$CGN7*PbPjY-$ z4xCR>ud`93n$zstB~;qO8Oe(Dqq5^xm-h@@GtU6BJl&V=7{jDlm0uS)!MN6EzXjBy z%)usXZT#;1x{n?%EBuAqYPT>_akxs-^18X;I$YmC@p=z4<@@=mai3vudUmSz z3E6IDP-6bQWrS~b0WAE)W?s85|CUU%TT~_Gv}}_@FvvkVlW-sReAepZn}Jy?>k$sr z-|DoTOK#Y0h75vjqw+|Wa#Jwtwf5*ARc@D=)_=gZ2tYi&P4;XHU`K>`Mil5PVvX;) zn|O)*tSWN_yEH09an|Z|&G_fXwEw_lj1Omfg$bmDA8~p1uo38$R3D2G8c2b%wvBJC zR*GWvf3Sx=__VFa>86bz_Y7vlLtg|lR;+^peTu!r%BNgI1M7As!oHVlOlrXij|_7) zDfxX5EGec!2meKZu`_Sl%<_aYS_^~2q0{}6%Rgc=HyTK1IK}O>v-44J_^8@i|ItfV z<-~rqM2KH%Bn%OqD=&FnUfy2$$g6O@R>24e!7R{bbfmO%UNH?Hdqo?CXmDp%)#LfKov z)njxmFtx!?wtlKc8DMQ9$YnCE0B>T1-Vjrv;nkRsAE5H}Y+{`6@J8h$w|H*$uDxAN)g zx;e>@)XJBmF>Pfif55FwBd(#fUi{*RRcn^T(fvwx&p3)I6w&bJY#rz`$Yg#mWm zL{a0ftJ}DZz-p6PcIm=z?}im31fdi50*J0(S)K)q6)5bB?{NmVLBn)~XB~)J4?;sN z_tH)15T+;G7H!kJe;IAjmDnsb(9pt4d1rHMw(9MOVlE8CbxXBjR~3iTkt9~*kXyWP zpEo5hP)S3!7FrT|5#tkY1zx6LYd&WV!ZV#xJ;d*2dYHdTcW`KJI7uXx>msvKuwd7F z`lC2LDoGG(s#M9>%nQ!>{HyTa3aqi==$~J#!G!ezFq>ndhZH*&T;PP}DGUf1+g#tj z1|+(dwIOoAOCA2!wyD;hsN+Rfo!7V$*l7P4O;OrwC@Z0D)S;-kb@ z;S1Q1rWFt6-QY{IP~D;2Z!`{wF2MJ8U%<9O%SZS}z?DaX;K#QKTMKy!%S7J`)5Y!3 zWkAE+<4PaPy>!18h?zG^+!j7DS^b~|_GKnt2|BB4(2Ke_ zd1mPhy_KcQ>iYLXL)F+nVwrsBI6tNOdQLy_Q@$QhxW;Ded3l=MIT6(0nkSw|J5_-$ z+{;x9l>C0GUO}&1uW32U@~r3M7#;vkePT}5u!1WxSl$}S|HjYPSJ$1u1@8r=8&R2> z1&eJA(5i9Udodbgwo*_}hwrL0R{My%I=6?XvkW;#uUBA)7&$mrL!e8S$3R=bH^iBh z^#wnK*th@q2Jxfq`$emISNR8hHi*YD8x4~KeO~Al!PUN)zU)NLZ;FXY&~TB0x~_xz zgSz7NX%OD*@a39JP|*R~KYybuqu_?%3HgD<4g~U0{9b5ip=i8Z%&c+}pDiR!FjD4o zrsnmPl>xOxfvBZLtPTc}soKrg!ZJK&3*>f27gH0^Hyv|S<)KUD!1&cPldoyzynf~* zSI0+TNwnqLoTAMm>7T@0-f*Ygi)4*^o-sHS!^!?3AJ`-@&Z*2AP^Nl?H{eR;P_ALN zG1?y_$0U2R<9u!*5>l#ApdZvI=P;n&DAaInut&;}>1!ehF@~Q*FMPy`3-3Frxw>#m z%SM{PpJ(W-(nEN>AC0sxb`#)IkuswXtI4)4xOaMrSF@+pVrhB3$e;f6cN=J0d5LqZ zQf#>c6Mi%^jwucxxzxbFjNA+D43nukcVF&CJCpfEHSwy_WTUaO?QUkU8f>b4RvJupt z<1WmgU1pBq!7ui-;aiJUlmlz`;GeKm?@3n~Dk2mbF^?UDAH--3*mKi=QD_09L{^!R z)nN_jrpVDM<2!#lO$x)Im>n|UJ#NM9%)p|3YscT_K8p%mZZJ+iF;+5C=G8p?Z5D^G zRb4|;e%{%f!qn6GWnCSkE5|dWz)`Oa0LHqDN#tp5Dnvmwz9@CvRMnI$XdiWlu##XOxiJ9Q$FKXVxS848gJ| zuuFzoL3lrAaQIphq0n_3T-ugjjffgZ9<{%bg4NH=teq{9ynkuo&!{Y9A3zs8IA z6KKqmp)2T=<9zTf2=?Yu4Lahc!iM9&E)+vbl;g_*Sx6^d?aFZI@@H)`}`wp=2GZR0Y z8niDm>$*)RH_-7eO1MOeLlz?uOQC=BekcLz|GdR2?wnh!fsv{FH-qPYli$VGHPuZx z>XsW=msI(b8&EJU67(kWC*N5%^Ep6p(>R`SN1SHZow{%l?r}%oYyk5<#aP>vyz(g_ zf;8hJW+xqx6Y0F_`|6P+)@giA6DQ)LK!2%T?nZ55gmLa@&Em>J)RIGaVnji%!8Zi} ztS|tPqpMvBP5wncfFo)Q zJj^5lq+!wgs=g2nwyOhS6aisLFgd(4?af@r-_&QUcL3YYn%gx_SB!D`Y8qFUT$>NC zO1E|i}nh9Y8qUt4%VV4SM|!ZC`!Dl(#)Lwn?|=Szyvr{LMN;nkL3@W zak3TqBlT~{GV^vMpm9y%SF;Q#zi@8Ux>jh=Ik*sH&j_*;vlGsJ3aats*i z0?&3bWC7X@d~XmWgQB5QC=SaK-Q59XhuLYCC!6vzLFCVOgWqI7{x~=K;=D8Oghd8~ zWlm{(Z){)3Z&=)X&ItnT{0%&Vfg<~1;Dw>z|7;hI%RV+9gD%fGt? zt2o*(JSZ*s#@}tuzRf7M~F8?#w1S~T4NUK{!5H4V{0bqHuHzadUI`I zVv_K5?D~ex&nJ=T6MbQKt|pjYE0e3dmo3oqk6C{b-ru?S7}AiIanS@;P0R)U_UWDN zu$yF}zBOjCj;JGfk$tri-@23Uz*Jx`>kZNiCV}2GNQ0dt2*+mj>jYHI1TlAD1?a$`>ggs*)rIBz`Q_GiEw6UtwN_oi!8Zo^BtMWH|{ z(H9VDy_E~o-5Fh&W_NO0y^*qR$`gPviOceA@-}K(xh1gs3Jnh=;B z#`CMNIVqpMWhnSRE!G8&Jwtq!C53nmZ`yZW&4BUgoVfI3Y602bWt10c zZDn7Ty`OJ5|4aeCFPXy#d@sH9h!@>P-FwOWqhKnEiL>~boHvMG~5 zMaSAqJI=*C?!co0U+(SnF4)fogNPWQvO6oh?Z)7DpDVEc2@Up1*v6%E>oCdUHMv;rJZeJn(jaj6;*{Xc}VH zD=4B@ah2{egCU}dkOLK(V$UrTn4vej9FIKQpmHLeUMRa7q`oP!JZ2B;KXMyOWq-AH2V9uLMU& z$ww_p7PGR}{PL1aH;wHbb-5U0VU6$faaz4_tc1p-s2pshJNX=OnF;LDCLoLwnOQEN z4|&v-g@5TfQz3Aze5KFYK(ZZhD9J)_0IufiFLsnhOB2RUc%9EKbotbdxm2^`qM~!C zCZbk<_OCf)(-b1DdvpDc|5YUkDml$rfc}gYLmKO3}8qvRQC(CQ$~xUIGo!)>eGv*H2m7WiyIKw2mFC) zU7{S=!=a_P0>69=vCkjc-1tI$N0+WYy-BiV5hzy^ZSg;W*h!`2kE7@wuBUBxr9T@< zYDlB3ecROZV_0ruKwFe^^I)c&%CEj_vKR1tmVmg`@j}=s&iwTS(;s; z)?erIiUoWzYIl#BiGY>s3et?VtzIcEJ1ibPQsbT!qPRg2X2Y(bE=&A z!fmQjN+f@lN^_T|T9RUnYx0L^N(n}LbcmXeZyQ;wshVB7e11ER$vJbLTfKT*z-O@O zZU~mPOzmzIGs~r36_NgeiRR>6>bbLeVmOO7y=0e6a1&jUtf>=Pd8@i6D?e2Rc|A>Y z9qOZ-?3b=;mt?I;t60p~HKfW9KNI>`)gxSoMx#Myh)ld*A1M;Ni*c4pzSfldfPXOR zt{Y(}+E=~V%no|jz=4Hqf@6)ErSPL8`f4F?54__}*8^f**`MX=(>t5=Xky+c`hqrq z4ljPB$OLI__brdFA~z4|W174AtW5xvF|ItF5{|#LHK)Mm5f47&ip&EeS4~?AHrnVTJ2yVfieV98+$Y*CqN2#`NNL+3D?7)j8y}CTpq@`Z-)8&fDjs+`biw zg63F-85C}eCL|(S`p+Q_i@2^T9y!l}!Gj!4XbeV~E_o*xr@xGMA_4#tgE?*r-3UCu zq|^?mOMf@9XIaRPDpU`_4*fX#GC_a{cO-g0(=P~SaE9y`6tA`{uG~>iylTtni0-5? z&U5|9(=$;SlLJzdYzMAi9nkX;x;pCgm$DrcDKM@e$mqdWSi%Z|;riyud?R9K@qB=M zBc?7tXmz(sSeh`o7SCu;wt@_!z5zjyZ*Vs$FB!%iX?LT*FfT#xInv)tOj(z1p+co{ zwCLK6Pjv&jzPmYj;!cnUBc3$sal6vsREt`b2ae$oT+aYSJV{okxvp*UIW$x`s~QnZ zGNk(a@^g*K;2q!Hmb{t}baV2OJ+K?EItMBnDU&tI(>{4gsvE3gR2~GDZ#>tr6i$8h zSnlde3a6-em_^v7`%8K7K|yw@!WN=S)v9qo^`NKr3@+SLj1R6c;2E|?D9y7xR1BGl8FJAIKf&2uH}NPonB!8Ims0g)aLAU#+lLi;Kd zlH63NOF_lHVCwwQ5ulo7^~-W^Z6fw?+bzYzJ~~X8^%NU(0nWbY>XcG$p+-Rm!4mtI`&$umTR z?*9{3ZM6>ttUf}@mwLJbD9^Nn3$A-l2AD5<)S-#SjDrE`@y+j`> zU#B~k8}9#ComV|(HRVZ+%;VO0|xQed^Z9B0Gy% zlb;Kh0t}q+aOigLUxwA#04ifHrUiW_$GgkqA3N|6Y-xL|^@`>7^HtBT)1gYW>5%qM z`y|@4BlhlIg&4SZs8>bbaFGAGAi(dGROMrPr{WXdgnht^oC16vvU=x=RRF3O zUGjwGLB&;Q`VHB0m%79rDOr6`*5|r6g2s)!;j4Le03KlTy3D0ptgp?4PRB_C(`^&vCOC*mbcN(jU0$GNp2^ z@-ez%_RzTbO4gavImF!~W3Fq<=>}VF1~{~v4~VRPOs^=EtJOUk$qK%BNXIi-rjqam zaM6IU4NUW>oq{%;8!Q51ubF)Es1GT=uct zoM9KQ>J7rD#GBr*Ld%c;eY)y4OGv5P^uk~^KM+9NQuK~-n?-ug{0%9Y46t#u6!uID zvAAjSn9vd>r)~i*o8gK{KVQ>#x9-b<;p=%wBz^3 z5jad}vd1L^gd2_WE`Il}Pt=Ni8WvA5u`fC{#*Ow+12N-F0)ghX%x?e7$r}8+_l(ms z|EYjI8YYDb2S5OXB_L=@8R@>n$HU&6lvhawiurWrz_>t63OiT%$u-`UA|~9^J5Tt z5amghFb)Ko`QAPit4%n~38w=uj*0p6EcM=&!X}`6#d+IYS(82N;-%gLkr5F=5v3GG zO?)~apsc5t9{vj7?I3heG3Q|j9T;$%j5Js`xa}GR-2UGJBSc#JzP_AkewDKFaCI zi7Ao!m+r%pb^q~3YPtUeDCBbF$B9o4M!%>DZ%)*b(S z9}sQ!)(=i-2})56RAUp1pU_8(RF{SWy3fS{a9QHnNmrl5uRK24iJlFrs|tgU1%y9p zw`gv_?a$d&X!pps`R*oE{J)ltJRa)p`<+H(okG%>WF1tb!6?g+EVGnl!jtss8Cydt zg~F@eC{32JNA_)GOQq1F)MOA(XtF$MqbO9$(yO%io$tKgUq177zGpw@o_p^1-Yf1I zr^|J0I{EG5@~~^)-=pNWY__x2_-XNcry5YshsO3w*=x=l5^!3YJItFizl&`~M-rdo z>t?$CIKhpb?sEI!ZDEH|dL_1aeuK;<=M6%4KS)Zp8l!b%{jcxZa=VZ1kNS++q~UUO zdhxon#jlFAezqlMzv5fWoJiJNsj>k3K>gor1*6Y<=?&rfN8dVo2e+!a{ln!w_ zqrmzHLC;=84-5E}{2OlKz&Gu9vRy;i@hRlX@ws>VzZu9!Dmh?R;p0#gKI8ML9jb4{ z))x&-3j+ob+}eDFe@)Ph3ys#k++pdPKd#BH+Ad54eHhKa+4C=bgdut`AA=4vf;bGXsev&eSz z49WeeNI!ZDgU_E*C4N5>DV}3+_Yu>1`o@HcuQt)!_?ub6M{b>Y&$n3ryQ^KZj$6i* z4`{B#I_BI*l!6$hLipGq1EOm|;o@>dHqQ@RzF72j%Tawp1IWc=UkeBCoOhGrW?NLn z)Tp101MUI!u4h{QmN8}EutCu)fIWAW%+4`cmvXFw>aFzczr+k z53?$_<(9hL`ol$T?^7_so_4|1z}&^Pa$uE zI%=||%f^tE-HPwNKSTESKlR)mD#hZZ5s_t~$1gP14SYGH`>-x%Ng&Sx3|MC6OhWAMrp+cn7fXR75?=G8@rVc}DIzP(%vP`@* zN1}MOP-pCm57T^DH>hARH)D0T(?zHBav_1(Q94SB$q5WWEDU@-_w3YY-sqzm%v<>Y zc-8W+wYA$Kf^U%@JwL$~Noz#;)_)lt(Y^Tc?1o;}%VRgAN&SsOp*~Up#EpV(&E$xiMlNS+F>QSD~@|N?JZCCNjGk<9p}y$9y<7c5(Ni=192v;>*WPi_gvM2Ui5}Wz3}1 z{zGIUnyFrs8N@Je_rz$XX!$$lupbgyCR$1Ty3Ba)GitA+emRb70^g$if@9Tg_X=(t zU7*LMU8t`+9t3DYkMljv_|OcpX;)SXk};)iP`X8Kl9K&w3rfReOl-59!6M7v!)Gda ziyG@?ZBQBbN^JfaGv__&Jur7|l)Cm*?e$k!TfY)?TDFlu)j?ey{$;xlvW;grB_t-r8kEGL4N_z5B6mc_9e+S* zm$7YclQA8DwPk~Z=*`!uv`V0A@7zF-+A^T)gL&Sa&IuA8X=O z&`J>b2kc4L*h0HH*iJBWs4g`LpQn6^Ol%<7io;6d=qMd z$tkMwZIkI|w>;pynkfE@sbeBSb-N<%#+Bpt8+f=96W~oNi(CCinuGx6fsAPHC5-p((r>5IO z+S=}YI0HgVLKkiA!mfwrLBd<9ToOwA$PooL<@_G!s1}lx(0^h^xE78ohW)K$qeqv9C;!>DpY>lpd)xwCE6CZkPEeSM}y0y*#MV9W`7S@STM|8xTLRe97HzLtE8el;iKa!>i8 zHl}LPaC%tAGv{jo%An=(z41j`?eY0~3A`4F4$rL`Hq7LiWTuzWzMMpTCiDtgn29Ex zZOo=-cAWV4xtO)+Rta#)_ma!4kLS=J!%txiE^l3EK&j^bRvZ0fE|wuUzEG)&${p&9!; zdv5{T3%bGxRNn$T6opwH&>S;a76x*-59H z8>|oW)`i!eCFlLYIjJ3klQnnLO5hzh@=97rot;?nDvrLV{M;R&IrN|^>D;F5g0cTP zz#-5#+IuVqYtrkr_e6$qFS&*Dt0xF7o5sQln?RF_iBkWPLXfeI|CL?A=kIa4|_G=IHdpcxKZ9h44E%&9BJ1YC#W4&u@a# z^YwW>-7i#)NC`Oi85gx7VswO}eQ)@)+{{g;4Fqxg>cuWA_bULMdj!zg!gm;VyL>U^kvQ8XF6ivfxwS&(GdZM zy-r(;3RWG>Irt&k9d9<@NcueD2@^j*xj9_!?E%oj+Rv&f0*eFT#LnVs0=*mDU`FU>dTC-13?PC46oL+8ZdoXoj% zDx-wt6P5?zQn^6YGE1hti#JRjiULElpfUAlYa!OYdqqf&@euVbZ(W^^O>j|TT%G~b z%b0&KlaNPJ{h!3_&k)?b}EwR9X%>um9UK0Nn+c{p@uVV}$;WoB_fs-On_JLRhPK||JtcP0-Z8f#<0)#( z#8L0~ASdOJY|vS4i>Y~NHZ|t;seq(r&XZkle<}DJE3)OG*PI=VZuo9y^afr~g*|Ct zc7H}0rcB9Qq}m~vY`p; zpEKZwc8l{^*V(?%NYG)y>!()d3{ry@UfPvG>4x%YiegtH@^E=V?uoa1U|D*fP0KlpREpt6c>O)Jw!@n`6X>uCp zrPU^3Tg{$-;Pj3CKUXVN$bFrR_^~{H%tKb-P^XAreYHCd=ETTqQh7y4wjg1h?_IUy z<;LxvrL-pKJ;Rl}FJO1;v1e7`8E$&~y}1M;$GSAz5i5i-fF&PIxxbYEVM$8=nSzkd z>2N57R>s~FHU1d-l)E!PXNFsH!-pAFdSZkO0a}($_fT5>x9+VS*0&i8zD^5KHOVil z*k%>Vv@6h_Av^4mg(XEKL(#onCz@iOSo~zEdbPaeZ?Wk;g~NvrmYaGc@A&B1GV!?e zk)2PZUO!^D-t`AUaFCg=7O@4zBkAxjLUa4~*6MV@fS4_5Z~8E9-AK?p@eR-E2xZZrGBVhQ(duM{{g6` zwVW*6wC0C$&oXl)%#QIK7fpGeqXJUFU>*na~^$Zjo&Gjk2~BhZtXT-oqc~Q@&H6UCO=1?)61aFG2gzy#if@qwaC{Dk+ucY<~&)Xf|{e2L0ys)80EnHnHcu{W#ozQJCtue!FNTRUbFgp&l5ytyeIw zm=8q-+K0pJ%zPSI67cwkM;Q>#VRspv^iJWm)C@J?t-a5OU(c*Osvwi^@^-Z|;Y92i z4);X&|H(eNUgx|amDdwU-jfQ#`ScmWhD7{`*@R9v-`3T7^)$%J_8DZ%4 zBxq&}O6AydFJsIG*|K15x$pl0F2V^q+;Eck%q{^AQC`3J{ZKRo?EYhOzwh(f@7exN z3*MikJqa*bA?PG5vj@*8&x%C3|16ikNbB*wE&j5hQ+{=iMy-6<%j+66`CSMuDnkKv zp|P|86c8Vu?LU1n=K3>GoA=MV2R8FzX^ZTvyk{KQOMbh!acEf;=laT><*8Y>4#CNZ8yvCx(ep=lYk@wGzyasq zU%++>xXt9_$m?I7nB?liCYGm8USGZWVC`L<6Gk4JnoG$(3V3n`DW(ew2sPvFVQAZ=k|qeo;dqEQZf6G zsSkrf$@Z)2Hdz43Xr2tQ{SNy4aka_sCr-qX7b9kjvUSd&_0^uXskTf8{{BGL@f!8sRVr z5YEAL5jA6SLIJ(lA4&t6oXgW)dHHU8(rr~or}qr;FY6SkI$(u^XgJBl_19g#w87<% z<7^Sr>(-x{sgv<(B_Hb_N!P{g+u@@Fg^N%BO?8`u6wK~ z^LACnqlA1FqUW39`pLDIFHFi9K7Ozudr?-9+O|kSWT07p7&JKovji=JlS1ZiMlOHc zJ89FAIDa=$G=cT1#Bgg+Ruf?@bnlq&0`$J0KVP-!{mZ>4T8;uEuY$R2_KR6)ROS0w zHes@>pIpn4wuGJl+8O$}@Oi)>)RJ#|ec=Pn`wWRlgiKVzo}J+BstDR|4~BzHwCLel z(?NaBvnvi7@@;)1i9<`6l+d_1A~&c*aD&F^R&^XofBv6)Hk+2e(iON%@L=Cp^OMmLi4$SU@d(nr4B|eZe*u`*_NQ`StE3R-@kIwKl5=o zJ0j*)ux}(RVrc>fy-q=Ax2gHc&IXFjj}~6$@6Nqv#Cq|Su^F&D7JV`itECxDxmf4y z64rTo@(FNZq2W$wUMB5^&1ICuvTAo=)8eRktRKW8j8iAy4ph7XOz1wO zv#@n;8ZLhtd3X$!Uzm*B82U9Sk6BYE2ZXINI1<#m^;3g%U$_RFYMR^PrzNE2Nm_!iUjj}(`IUU|*}iyko_uScVkUeywo-bLGWGIvoI;&y zH02y*la%LwE&fFB?_F&Fv|a1pII;7akzj;-;wU(6a=mkN;Lo6WINW7Rl<^i7T0Izc zZLbf-?l=%LHKIEN*iU^Ja)K?}F)ydhwh~;xt{vNy@vMy?%CcEbRmzGjK8?^K>fEBm zuaKIdl*9TI2V&0GGMYjWnMQu#?JPedt`*(yBcBHDtbY)2fAOFZaeR!k8#U9^4*T9$S z!3rF~rK`0Mfn-{RL5z>Qf(nmwy{%KDzYQw(Qbtv5qIU{zFfez41S?!Yk$D|paZhLh z#p$KumJEM2g~jk`?IL{{C|Q4l5Zl@zh)oP-=lT%vtKuuyxxs7(@|$q|nAq-vk6iT` zF*)~iB}T#+^3daTf@TZHJ7mOAGiWY)DQl_QKe;!r+8seJk{Z@UwD>%omS47s9Q8*S zmXh1BQj`pLMtu+y=6dxnm|B!dNxPZxVAQKP^0zd(NF>1YtULPZESWesIug7LCahX3 zp|u5WTYthto0C9wo_8@7BV z0s|)V<;D@^GU-?qT|UlO(XA4egpnZVZF$XdxTbMpVZgGYA{S*mLL!y&pk0M5l@Y{V zsCC66tq?Rwi!u#6nNaD5a$oCYkB3$QuET|ZKc`g$6DmZ^s2a_o615E3l)4m`HB?yb zLqgy^P=;*_M=OrEvl8Y}b{|Onp~(QYf=yBM6hU^h!$SHHf@hQv&D;B+O~G|ZXZ0y` zi4jX6pSW~O6nu_~JSt>X-Huv}I*kQh$Ku3x*7lcNVJQ>g?GotQ0u3KP5V*gs55(lc znuw!rDvrS*;6t~6%Ctcc3w;=ZHvvAEuB%oDHV}~#MAp0w1YkH3@L{QEW*R`JzRp5-7{RsIL0@>9{s?2c@=&F^Bs&O$=2rOMX{0ScT7dThlUb@IVN5_J~hA61Yot4MdhH#0X8j0vg z1u>VkI|aANm4N?G^qkQEl+)|Z@F3F?HCr?sbA;~Xh7ZKvFRp%k&mcA#xE8uYnS$hu z!8JOtQ~TaK^$=fOutHi9`U+b*dfr6V{E-GQ3lW1ud@W_oS?2Hy&DsdE`q*L17?}C6 z%7I6FPh(393im@7qs`6e)-Ft{lWYu=WybCzdf_cO3^8L9fSt+zke)TQ9u`nSbgRe%xCF(N%MR9&g2axS%;CnI`oKsI$ zvao&>m`U0VXy{BJ+UqI7o(K-zLkTF z8216zH}eyxRYDk!d>k^I3F@2f6KwRT($)vEgsrHWjL#4&RtPX$ODgdBDeG{=AvtqC z2#RRuKxR5#PEW05IT!#Bisw*(ayq?)B{~7W(|G4?5N_b4*koOQSRybT5C~3VXD4U= zg9g;5(x9en;R`%Gsz$W|T_Z?srxz1pB4>V43FtLOf!P=?sm5-l6=dMXJ8u$jrf4*4 zoyj8t``j(4gykiy%oM===?oBe^jGgbq!OY7>>{pn;BVwYjqbym@}EVP*`w$yx1E9J zx_Qn)Q$Y}lv&06}MsP^2usoA@5(QAY^Bx11p__5-T~>e$LMhZ#0hO%qY#^d|3W)q3 zr0$|FXKsyxmy87?BNjG!9V#J3=x++DqxX*lold2D)8K`)Ea17Wl%82=zb7AlOKfdt zBNwR#xa|VLQ)=5S@|@Q8tAT-#{WHMwxk`Gy(HSe$?7qQ3oRy)0-=Z>Q0qBumT5ec7 zd<^A1tEy5FWc+H68fd9p0xB6N(QI0w*vI-n1hfL9=1*Mmw`fr1RU0QWLqpnZ4tj1T-* zYb_{j__07+usHsDa=*ijQ=VLnEJP9m|<_Eg;6klj^_@!~CiHQeJNh<7#>FSMHHjGTgA z+MMaa=hV3`N(hpK`m68Dbo-w_bC(^O!XU!-xwQ9P_J|xg1-I~|VZ84UZM6#)5%Rza z)stc&;l?j%UJIz|ILQs0mZX7K2s5PJo6VC(koRG}I4uhs;>4w0{1@ar9Ku}#%9kpq zWA}Vc6kiFoLy!?eG{%N$trK_wsK^48?@N8nDJhmUS;!ItR`s_(6og0&&=J#Sng@QAojX@ z`@~My#|RR37Pz;DS4!tiVniGwf*j1K#6(@7V=s9oiLb-~D;!|OyFdd^*AikF$hq`o$V z^yFuSPdlf9|6QL}?2F3s6Aib<1o0|a`1>fIidW1$ObJUSJm>yHMWD3ZcZjP6?=LZF zvO(2D5JeDub!>@8%iBgoJFBt^ak}-)PvO%VbPd|0+9NNXg z!+sGmt|9~>CrwgN#|ld$tVu_;>kKw40IBKmVzC-()reV&aAyqVQx>6+j{-RQnlC}Z zvI&JbWMFqIWG5=<@lfl8&U=tYOC_4jA)mdNc;n(&`H? zA;TRM4F28}6kjV4%mM&wlbB!{$gG$P!R@I-o7;%WC(~0R#b~u44k-%0Ip+Yrug;aR zBcOmRLie%uRS1#>8Oc#V$8KrmxSF8^9mj%wt!#)<%7R;Ms2Lw0=1U_Lih|%y$uE;a z91i?e!;kV)2B@V$Hk~h_Lvq*ss-TDFaP zN07C4kPPlB3lb#AL((c^UlQn|YptAD5^=W`e2Ucsa3)`0;=Wa_5k%EWy699Z#~t;| z5{+o0mw+w`Y2`egyQB$cs_H?q(etAWNT{*f*RuGBDMn8G3e&fq2vXG@L6ie_*7#B6 zO;G7($AUW;sXV86vC%eE9Zns6X{R*shKhpw`!x`xPIV`OJ!(f27!1K$=eCMWNbLXc0} zig?FjC}2$o6_lQkN5wCymCutaTC{obBY)Qx1@M@Hci8AjaatY$Z*KWJh$#*E zfXpM{C!2fAUPeCx#w=Wm^i5xLI=1t9&r;4!;8EfhT@TL3mC+sC{V1fPrH+RzSn%qq z99OeK4Ll4KX#+o%fA*k0bCXsSM71G4RuaVaq9Mh#2BmCje^KQNq~CT6Q)_6yQg3-1&bt>~>c4jRPDEtgV6ffCU8@-T2xx2hPRrvM&;=`FHd^ z0p^|kD6JP9KcZO~p&gNC(o=cdR*uZJ1Y3!#6FMWTmf8@@P}}J(EJHO&a4Sk-rt(+- zMNc@`aXtd1Hkyr4jG?qbsUdM#MBns0M%94kU=$=4Su6z&hv&L3_gBYnjTbA%E89qN z4!Mx}JQaH7@aT;4nzLwLkf-xvA(CR0I zl(Gr2Vn^9O5-7*(K}CZCHr0gayA~BJ{!llh1&9*$ATjmM8w`Ss?U?=Lr;b0JASTBu zchgbtv{MbN=!>C1ktX77qtMT(uK^a$jCaE`C_hE4ZIpk|5JDzNf1_Za5t6Pm{APZ` zsendA7C~alTM{rdwlc)(fI1!vQ4_D+jYx2ZgwHi-;G2QmDdp}c$s^2Z^wFLK@uNaz z7b(djeDnFuF_d{Ic8wmO8dDx-2DdT(eum#adNl>b*mluIXQAv?} z@!2WyqtgU@^ICDhTU56t<){0CZHRd*1f}wNc`*`asyW#Od^bRq83NexjQTi^N$N~t%Jcu)W1{l5gHMR*B~b$*?MjsP}Ut= zCw?R;0P0Mm^R_|8a!TN(qXHqtnHewx_-CD%ocPvG4_(Q%K>QvBn^VJXV^z^|?VFtT}; zf=(emMM>=W-gIL0?mf6m;9mV-IEKiGs5;7r!?EjnBl6^bEqwpD`0$sSmb4x*J5Yi> zi`u=jXp-x2jx)l9jEz_@ih>;YypQl(_ewNyU+{|9h}NOQ5_T=xB4W(_D971wY|_Y$ zpYZ=x9eq#_)Y0LScnJigqs!p+f2gJQVEsn$W95k?m$nU zALX5C63)U2RT<&RW9eEyN|JjSX1Xc@juk-|q|XsP{2-@o6uaG$SLz|i{3u;J$}mmn zIE>^Nh896(%}tRItdKWdtt{A;w2LBiL<>_psrm{4mk01yGPC literal 0 HcmV?d00001 From 79157270aadce461c78fe9eaa7be79c152569fa7 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sat, 31 Jan 2026 16:19:47 +0000 Subject: [PATCH 19/19] update example 14 --- examples/example_14/example_14.cpp | 350 ++++++++++++++++------------- examples/example_14/example_14.md | 254 ++++++++++++++++++++- src/bitrl/envs/env_base.h | 8 +- src/bitrl/envs/time_step_type.h | 1 + 4 files changed, 443 insertions(+), 170 deletions(-) diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp index f5bb12f5..7247248e 100644 --- a/examples/example_14/example_14.cpp +++ b/examples/example_14/example_14.cpp @@ -4,10 +4,15 @@ #include "bitrl/bitrl_consts.h" #include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" +#include "bitrl/envs/env_base.h" +#include "bitrl/envs/time_step.h" +#include "bitrl/envs/time_step_type.h" +#include "bitrl/envs/env_types.h" +#include "bitrl/utils/bitrl_utils.h" #include #include -#include + #include #ifdef BITRL_LOG @@ -15,8 +20,9 @@ #include #endif -#include #include +#include +#include namespace example14 { @@ -27,10 +33,23 @@ using namespace chrono::irrlicht; // constants we will be using further below const uint_t WINDOW_HEIGHT = 800; const uint_t WINDOW_WIDTH = 1024; -const real_t DT = 0.001; +const real_t DT = 0.0001; const real_t SIM_TIME = 5.0; const std::string WINDOW_TITLE( "Example 14"); +constexpr uint_t STATE_SPACE_SIZE = 2; +constexpr uint_t ACTION_SPACE_SIZE = 1; + +using bitrl::TimeStepTp; +using bitrl::TimeStep; + +//typedef TimeStep > time_step_type; +typedef TimeStep time_step_type; +typedef bitrl::envs::ContinuousVectorStateContinuousVectorActionEnv space_type; + +std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, + real_t width, const chrono::ChVector3d& pos); + void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, double scale = 1.0) { auto* driver = vis.GetVideoDriver(); @@ -54,72 +73,22 @@ void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, irr::video::SColor(255, 0, 0, 255)); } -auto build_wheel(const std::string &wheel_label, real_t radius, - real_t width, const chrono::ChVector3d& pos) -{ - - auto material = chrono_types::make_shared(); - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - - // rotation axis for the wheel - chrono::ChQuaternion<> q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); - - auto wheel = chrono_types::make_shared(); - wheel->SetMass(1.0); - wheel->SetPos(pos); - wheel->SetName(wheel_label); - //wheel->SetRot(q); - wheel->SetRot(chrono::QUNIT); - wheel->EnableCollision(true); - - chrono::ChQuaternion<> q_cyl; - q_cyl.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); - - chrono::ChFrame<> cyl_frame(chrono::VNULL, q_cyl); - - auto cyl_shape = chrono_types::make_shared( - material, - radius, - width * 0.5 // half-length - ); - - wheel->AddCollisionShape(cyl_shape); - - wheel->AddVisualShape( - chrono_types::make_shared(radius, width)); - return wheel; -} - - // class to model the robot class DiffDriveRobot { public: - struct MotorHandle { - std::shared_ptr motor; - std::shared_ptr speed; - }; - - // add the components of this robot to the systme we simulate + // add the components of this robot to the system we simulate void add_to_sys(chrono::ChSystemSMC& sys); // build the robot void build(); // set the speeds of the motors - void set_motor_speed(real_t speed); + void set_speed(real_t speed); + + // reset the robot + void reset(); // the pose of the robot bitrl::rb::bitrl_chrono::CHRONO_RobotPose& pose()noexcept{return pose_;} @@ -128,42 +97,70 @@ class DiffDriveRobot //The chassis of the robot std::shared_ptr chassis_; - - bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; - std::pair, std::shared_ptr> wheels_; std::shared_ptr caster_wheel_; - std::pair motors_; - MotorHandle caster_motor_; + bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; + }; -DiffDriveRobot::MotorHandle build_motor(std::shared_ptr wheel, - std::shared_ptr chassis, - const std::string &motor_label) +void DiffDriveRobot::build() +{ + // build the chassis of the robot + chassis_ = chrono_types::make_shared(); + chassis_->SetMass(1.0); + chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); + + // allow the chassis to move + chassis_->SetFixed(false); + + // add visual shape for visualization + auto vis_shape = chrono_types::make_shared( + chrono::ChVector3d(0.4, 0.3, 0.05)); + chassis_ -> AddVisualShape(vis_shape); + + // build the wheels of the robot + wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, 0.16)); + wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, 0.16)); + caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(0.2, 0.0, 0.16)); + + // we want to tract the chassis pose + pose_.set_body(chassis_); + +} + +void +DiffDriveRobot::reset() +{ + chassis_ -> SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); + wheels_.first -> SetPos(chrono::ChVector3d(0.0, 0.175, 0.16)); + wheels_.second -> SetPos(chrono::ChVector3d(0.0, -0.175, 0.16)); + caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0.0, 0.16)); +} + +std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, + real_t width, const chrono::ChVector3d& pos) { - // Build joint frame in ABSOLUTE coordinates - chrono::ChQuaterniond q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); - //chrono::ChFrame<> frame(wheel->GetPos(), q); + // rotation axis for the wheel + chrono::ChQuaternion<> q; + q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); + + auto wheel = chrono_types::make_shared(); + wheel->SetMass(1.0); + wheel->SetPos(pos); + wheel->SetName(wheel_label); + wheel->SetRot(chrono::QUNIT); + wheel->SetFixed(false); - // Use the wheel's actual absolute frame - chrono::ChFrame<> frame = wheel->GetFrameRefToAbs(); + auto visual_shape = chrono_types::make_shared(radius, width); - auto motor = chrono_types::make_shared(); - motor->Initialize( - wheel, - chassis, - frame); - motor->SetName(motor_label); + chrono::ChQuaterniond qvis; + qvis.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); - // set the speed function (rad/s) - auto speed_func = chrono_types::make_shared(0.0); - motor -> SetSpeedFunction(speed_func); + chrono::ChFrame<> vis_frame(chrono::VNULL, qvis); - auto z = motor->GetFrame2Abs().GetRotMat().GetAxisZ(); - std::cout << "Motor Z axis: " << z << std::endl; - return {motor, speed_func}; + wheel->AddVisualShape(visual_shape, vis_frame); + return wheel; } void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) @@ -171,56 +168,59 @@ void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) sys.Add(chassis_); sys.Add(wheels_.first); sys.Add(wheels_.second); - sys.AddLink(motors_.first.motor); - sys.AddLink(motors_.second.motor); sys.Add(caster_wheel_); - auto caster_joint = chrono_types::make_shared(); - caster_joint->Initialize( - caster_wheel_, - chassis_, - chrono::ChFrame<>(chrono::ChCoordsys<>(caster_wheel_->GetPos())) - ); - sys.Add(caster_joint); } -void DiffDriveRobot::build() + +void DiffDriveRobot::set_speed(real_t speed) { - // build the chassis of the robot + chassis_ -> SetAngVelLocal(chrono::VNULL); + chassis_ -> SetAngAccLocal(chrono::VNULL); + chassis_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); + wheels_.first -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); + wheels_.second -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); + caster_wheel_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - chassis_ = chrono_types::make_shared(); - chassis_->SetMass(1.0); - chassis_->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); - chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.0)); - chassis_->SetFixed(false); +} - // add visual shape for visualization - auto vis_shape = chrono_types::make_shared( - chrono::ChVector3d(0.4, 0.3, 0.05)); - chassis_ -> AddVisualShape(vis_shape); +class DiffDriveRobotEnv final: public bitrl::envs::EnvBase +{ +public: - // build the wheels of the robot - wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, -0.02)); - wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, -0.02)); - caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(-0.2, 0.0, -0.02)); - // build the motors of the robot - motors_.first = build_motor(wheels_.first, chassis_, "left_wheel"); - motors_.second = build_motor(wheels_.second, chassis_, "right_wheel"); - caster_motor_ = build_motor(caster_wheel_, chassis_, "caster_wheel"); + typedef typename space_type::action_type action_type; - // we want to tract the chassis pose - pose_.set_body(chassis_); + DiffDriveRobotEnv(); -} + virtual void make(const std::string &version, + const std::unordered_map &make_options, + const std::unordered_map &reset_options) override; -void DiffDriveRobot::set_motor_speed(real_t speed) -{ - motors_.first.speed -> SetConstant(speed); - motors_.second.speed -> SetConstant(speed); - caster_motor_.speed -> SetConstant(speed); -} + virtual void close()override{} + + virtual time_step_type reset()override; + virtual time_step_type step(const action_type &/*action*/)override; + + void simulate(); + +private: -void build_system(chrono::ChSystemSMC& sys) + DiffDriveRobot robot_; + chrono::ChSystemSMC sys_; + uint_t sim_counter_{0}; + real_t current_time_{0.0}; + void build_system_(); + +}; + +DiffDriveRobotEnv::DiffDriveRobotEnv() + : +bitrl::envs::EnvBase(bitrl::utils::uuid4(), "DiffDriveRobotEnv"), +robot_(), +sys_() +{} + +void DiffDriveRobotEnv::build_system_() { // create material for the ground auto material = chrono_types::make_shared(); @@ -238,25 +238,77 @@ void build_system(chrono::ChSystemSMC& sys) material->SetGt(20.0); auto ground = chrono_types::make_shared( - 5.0, 5.0, 0.001, + 5.0, 5.0, 0.2, 1000, true, // visual true, // collision material ); ground->SetFixed(true); + ground->SetPos({0, 0, -0.1}); // set the gravity acceleration - sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -bitrl::consts::maths::G)); - //sys.Add(ground); + sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -bitrl::consts::maths::G)); + sys_.Add(ground); } -void simulate(DiffDriveRobot& robot, chrono::ChSystemSMC& sys) +void +DiffDriveRobotEnv::make(const std::string &version, + const std::unordered_map &make_options, + const std::unordered_map &reset_options) { + robot_.build(); + build_system_(); + + robot_.add_to_sys(sys_); + + this -> set_make_options_(make_options); + this -> set_reset_options_(reset_options); + this -> set_version_(version); + this -> make_created_(); + +} + +time_step_type +DiffDriveRobotEnv::reset() +{ + sim_counter_ = 1; + current_time_ = 0.0; + robot_.reset(); + robot_.set_speed(15.0); + + auto robot_position =robot_.pose().position(); + return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); +} + +time_step_type +DiffDriveRobotEnv::step(const action_type &/*action*/) +{ + + if (sim_counter_ % 100 == 0) + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; +#endif + return reset(); + } + + sys_.DoStepDynamics(DT); + auto robot_position =robot_.pose().position(); + + sim_counter_++; + current_time_ += DT; + return time_step_type(TimeStepTp::MID, 1.0, robot_position); +} + + + +void DiffDriveRobotEnv::simulate() +{ // create the object that handles the visualization chrono::irrlicht::ChVisualSystemIrrlicht visual; - visual.AttachSystem(&sys); + visual.AttachSystem(&sys_); visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); visual.SetWindowTitle(WINDOW_TITLE); visual.Initialize(); @@ -267,15 +319,9 @@ void simulate(DiffDriveRobot& robot, chrono::ChSystemSMC& sys) visual.AddTypicalLights(); visual.BindAll(); - real_t current_time = 0.0; - auto& pose = robot.pose(); - - robot.set_motor_speed(5.0); + // we need this while (visual.Run()) { -#ifdef BITRL_LOG - //BOOST_LOG_TRIVIAL(info)<<"Sim time: " << current_time; -#endif // Irrlicht must prepare frame to draw visual.BeginScene(); @@ -285,26 +331,16 @@ void simulate(DiffDriveRobot& robot, chrono::ChSystemSMC& sys) // .. draw a grid tools::drawGrid(&visual, 0.5, 0.5); - draw_world_axes(visual, 1.5); - // tools::drawCoordinateSystem( - // &visual, - // chrono::ChCoordsys<>(chrono::VNULL, chrono::QUNIT), - // 0.5 // axis length - // ); + draw_world_axes(visual, 1.5); - // .. draw GUI items belonging to Irrlicht screen, if any - visual.GetGUIEnvironment()->drawAll(); - - //sys.DoStepDynamics(DT); + auto time_step = step( action_type()); #ifdef BITRL_LOG - //BOOST_LOG_TRIVIAL(info)<<"Position: "<(), + std::unordered_map()); + env.reset(); + env.simulate(); + env.close(); return 0; } diff --git a/examples/example_14/example_14.md b/examples/example_14/example_14.md index 75f2153a..69656438 100644 --- a/examples/example_14/example_14.md +++ b/examples/example_14/example_14.md @@ -1,14 +1,250 @@ -\page bitrl_example_14 Example 14 Simulate a differential drive system with Chrono +\page bitrl_example_14 Example 14 Create an environment using Chrono -In this example we will create a differential drive system and simulate it using the -Chrono library. -Specifically, the robot we will simulate has - -- Two motorised wheels -- An ultrasound sensor +in this example we will create an environment for reinforcement learning based +on the Chrono library. +Specifically, we will create an environment that includes a Irrlicht library for visualising the robot. -The simulation that we will be developing is very simple since the purpose of this example is to -introduce some core components of the Chrono engine +The following image shows an image of the environment we will create + +| ![average-per-epoch-loss](./env.png) | +|:------------------------------------:| + + +## Create the robot + +Below is the class that handles the robot model. + +@code +class DiffDriveRobot +{ +public: + + void add_to_sys(chrono::ChSystemSMC& sys); + void build(); + void set_speed(real_t speed); + void reset(); + bitrl::rb::bitrl_chrono::CHRONO_RobotPose& pose()noexcept{return pose_;} + +private: + + std::shared_ptr chassis_; + std::pair, std::shared_ptr> wheels_; + std::shared_ptr caster_wheel_; + bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; + +}; +@endcode + +The chassis of the robot is a simple rectangular plate. It also has three wheels. The model robot we will develop will not consider motors. +However, Chrono allows for high fidelity models is this is needed. Below is the function that build the robot + +@node +void DiffDriveRobot::build() +{ +// build the chassis of the robot +chassis_ = chrono_types::make_shared(); +chassis_->SetMass(1.0); +chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); + + // allow the chassis to move + chassis_->SetFixed(false); + + // add visual shape for visualization + auto vis_shape = chrono_types::make_shared( + chrono::ChVector3d(0.4, 0.3, 0.05)); + chassis_ -> AddVisualShape(vis_shape); + + // build the wheels of the robot + wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, 0.16)); + wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, 0.16)); + caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(0.2, 0.0, 0.16)); + + // we want to tract the chassis pose + pose_.set_body(chassis_); + +} +@endcode + +The reset function simple resets the robot to its original position + +@code +void +DiffDriveRobot::reset() +{ +chassis_ -> SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); +wheels_.first -> SetPos(chrono::ChVector3d(0.0, 0.175, 0.16)); +wheels_.second -> SetPos(chrono::ChVector3d(0.0, -0.175, 0.16)); +caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0.0, 0.16)); +} +@endcode + +Below are some helper functions for the robot. + +@code +void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) +{ +sys.Add(chassis_); +sys.Add(wheels_.first); +sys.Add(wheels_.second); +sys.Add(caster_wheel_); + +} + +void DiffDriveRobot::set_speed(real_t speed) +{ +chassis_ -> SetAngVelLocal(chrono::VNULL); +chassis_ -> SetAngAccLocal(chrono::VNULL); +chassis_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); +wheels_.first -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); +wheels_.second -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); +caster_wheel_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); + +} + +@endcode + +## Create the environment + +The environment class inherits from the \ref bitrl::envs::EnvBase "`bitrl::envs::EnvBase`" class. We will need to specify the +time step type and the space type: + +@code +constexpr uint_t STATE_SPACE_SIZE = 2; +constexpr uint_t ACTION_SPACE_SIZE = 1; + +using bitrl::TimeStepTp; +using bitrl::TimeStep; + +//typedef TimeStep > time_step_type; +typedef TimeStep time_step_type; +typedef bitrl::envs::ContinuousVectorStateContinuousVectorActionEnv space_type; +@endcode + +@code +class DiffDriveRobotEnv final: public bitrl::envs::EnvBase +{ +public: + + typedef typename space_type::action_type action_type; + + DiffDriveRobotEnv(); + virtual void make(const std::string &version, + const std::unordered_map &make_options, + const std::unordered_map &reset_options) override; + + virtual void close()override{} + virtual time_step_type reset()override; + virtual time_step_type step(const action_type &/*action*/)override; + void simulate(); + +private: + + DiffDriveRobot robot_; + chrono::ChSystemSMC sys_; + uint_t sim_counter_{0}; + real_t current_time_{0.0}; + void build_system_(); +}; +@endcode + +Below are the implementations for reset, step and make + +@code +void +DiffDriveRobotEnv::make(const std::string &version, +const std::unordered_map &make_options, +const std::unordered_map &reset_options) +{ + + robot_.build(); + build_system_(); + + robot_.add_to_sys(sys_); + + this -> set_make_options_(make_options); + this -> set_reset_options_(reset_options); + this -> set_version_(version); + this -> make_created_(); + +} + +time_step_type +DiffDriveRobotEnv::reset() +{ + sim_counter_ = 1; + current_time_ = 0.0; + robot_.reset(); + robot_.set_speed(15.0); + + auto robot_position =robot_.pose().position(); + return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); +} + +time_step_type +DiffDriveRobotEnv::step(const action_type &/*action*/) +{ + + if (sim_counter_ % 100 == 0) + { +#ifdef BITRL_LOG +BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; +#endif + return reset(); + } + + sys_.DoStepDynamics(DT); + auto robot_position =robot_.pose().position(); + + sim_counter_++; + current_time_ += DT; + return time_step_type(TimeStepTp::MID, 1.0, robot_position); +} +@endcode + +The simulate function wraps everything together + +@code +void DiffDriveRobotEnv::simulate() +{ + + chrono::irrlicht::ChVisualSystemIrrlicht visual; + visual.AttachSystem(&sys_); + visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); + visual.SetWindowTitle(WINDOW_TITLE); + visual.Initialize(); + draw_world_axes(visual); + visual.AddLogo(); + visual.AddSkyBox(); + visual.AddCamera({0, -2, 1}, {0, 0, 0}); + visual.AddTypicalLights(); + visual.BindAll(); + + // we need this + while (visual.Run()) + { + + // Irrlicht must prepare frame to draw + visual.BeginScene(); + + // .. draw items belonging to Irrlicht scene, if any + visual.Render(); + + // .. draw a grid + tools::drawGrid(&visual, 0.5, 0.5); + draw_world_axes(visual, 1.5); + + auto time_step = step( action_type()); +#ifdef BITRL_LOG +BOOST_LOG_TRIVIAL(info)<<"At time: "< class EnvBase : public Spac */ void set_idx_(const std::string &idx) noexcept { idx_ = idx; } - /** @brief Store make() options for future access */ + /** @brief Store options for future access */ void set_make_options_(const std::unordered_map &options) noexcept { make_options_ = options; } + /** @brief Store reset options for future access */ + void set_reset_options_(const std::unordered_map &options) noexcept + { + reset_options_ = options; + } + /** @brief Mark environment as not created */ void invalidate_is_created_flag_() noexcept { is_created_ = false; } diff --git a/src/bitrl/envs/time_step_type.h b/src/bitrl/envs/time_step_type.h index a75cee6c..e7b73961 100644 --- a/src/bitrl/envs/time_step_type.h +++ b/src/bitrl/envs/time_step_type.h @@ -8,6 +8,7 @@ namespace bitrl { + /// /// \brief The TimeStepTp enum ///