diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e9da5f..05a4bb9 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,34 @@ 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) +SET(BITRL_IRRLICHT 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) +SET(SENSORS_DATA_DIR ${PROJECT_SOURCE_DIR}/sensors) 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() @@ -56,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) @@ -70,6 +74,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() @@ -102,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() @@ -113,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() @@ -137,8 +144,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 @@ -152,6 +160,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 5ffc262..aaf7db3 100755 --- a/config.h.in +++ b/config.h.in @@ -1,9 +1,18 @@ -#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@" + +/*Path to sensors */ +#cmakedefine SENSORS_DATA_DIR "@SENSORS_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 8c6d510..44129de 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -35,22 +35,25 @@ 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_20) + # Need both OpenCV and MQTT IF(BITRL_OPENCV) - ADD_SUBDIRECTORY(mqtt_exe_2) + ADD_SUBDIRECTORY(example_18) ENDIF () ENDIF() +IF(BITRL_CHRONO) + ADD_SUBDIRECTORY(example_19) +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 ae89b93..3c10782 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 7f8b753..dfa3f64 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 ece9a57..0c917b4 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 c195b4c..6e76631 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 0490430..f89731a 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/example_14/CMakeLists.txt b/examples/example_14/CMakeLists.txt new file mode 100644 index 0000000..635cc2f --- /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}) +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 Chrono::Chrono_irrlicht) +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/env.png b/examples/example_14/env.png new file mode 100644 index 0000000..7ec5b5d Binary files /dev/null and b/examples/example_14/env.png differ diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp new file mode 100644 index 0000000..7247248 --- /dev/null +++ b/examples/example_14/example_14.cpp @@ -0,0 +1,381 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#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 + +#ifdef BITRL_LOG +#define BOOST_LOG_DYN_LINK +#include +#endif + +#include +#include +#include + +namespace example14 +{ + +using namespace bitrl; +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.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(); + + // 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)); +} + +// class to model the robot +class DiffDriveRobot +{ +public: + + // 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_speed(real_t speed); + + // reset the robot + void reset(); + + // 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_; + std::pair, std::shared_ptr> wheels_; + std::shared_ptr caster_wheel_; + bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; + +}; + +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) +{ + + // 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); + + auto visual_shape = chrono_types::make_shared(radius, width); + + chrono::ChQuaterniond qvis; + qvis.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); + + chrono::ChFrame<> vis_frame(chrono::VNULL, qvis); + + wheel->AddVisualShape(visual_shape, vis_frame); + return wheel; +} + +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)); + +} + +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_(); + +}; + +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(); + + 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.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); +} + +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.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: "<(), + std::unordered_map()); + env.reset(); + + env.simulate(); + env.close(); + 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, we will create an environment that includes a Irrlicht library for visualising the robot. + +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: "< +#endif + +#include +#include +#include +#include +#include + +namespace example +{ + + +#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: "< +#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: "< -#include -#include -#include - -int main() -{ - - 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: "< -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; -} -#endif \ No newline at end of file diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json new file mode 100644 index 0000000..cd32367 --- /dev/null +++ b/robots/bitrl_diff_drive_robot.json @@ -0,0 +1,45 @@ +{ + "name": "Odysseus", + "mass": 1.0, + "mass_units": "kg", + "chassis": { + "mass": 1.0, + "mass_units": "kg", + "position": [ 0.0, 0.0, 0.0], + "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.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.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.02], + "radius": 0.06, + "width": 0.05, + "visual_shape": "cylinder", + "rotation_axis": [1, 0, 0], + "rotation_angle_deg": 90 + } +} \ No newline at end of file diff --git a/sensors/ultrasonic_chrono_backend.json b/sensors/ultrasonic_chrono_backend.json new file mode 100644 index 0000000..6edb1d6 --- /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 diff --git a/src/bitrl/bitrl_consts.h b/src/bitrl/bitrl_consts.h index 7145880..66b8ec5 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 @@ -14,32 +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/boards/arduino/arduino_connector_base.h b/src/bitrl/boards/arduino/arduino_connector_base.h index 75108ce..52c607c 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 9cb3702..ec37e14 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 f02a306..80e258c 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 6c20ad6..43d943c 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 5105141..edae0b7 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/envs/env_base.h b/src/bitrl/envs/env_base.h index 50a0072..77ee0b7 100644 --- a/src/bitrl/envs/env_base.h +++ b/src/bitrl/envs/env_base.h @@ -165,12 +165,18 @@ template 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 a75cee6..e7b7396 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 /// 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 0000000..e85c28d --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.cpp @@ -0,0 +1,289 @@ +#include "bitrl/bitrl_config.h" + +#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 +#include +#include + + +#ifdef BITRL_LOG +#include +#endif + +#include +#include +#include +#include + + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +namespace +{ + +using json = nlohmann::json; + +// helper class to read chassis +struct Chassis +{ + std::array position; + std::array visual_position; + std::string mass_units; + std::string visual_shape; + 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()), +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 +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) + : +position(), +mass_units(j["mass_units"].get()), +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(); +} + + +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(chassis_data.mass); + 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; +} + +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); + wheel->SetRot(q); + wheel->EnableCollision(true); + + auto cyl_shape = chrono_types::make_shared( + material, + wheel_data.radius, + 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; +} + +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(); + 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); + return {motor, speed_func}; +} + + + +} + +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(); + + 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"); + + 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_.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); + auto caster_joint = chrono_types::make_shared(); + caster_joint->Initialize( + caster, + chassis_, + chrono::ChFrame<>(chrono::ChCoordsys<>(caster->GetPos())) + ); + sys_.Add(caster_joint); + + // set upd the state + pose_ = std::make_shared(chassis_); + +#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::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); +} +} +} +#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 0000000..390352f --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h @@ -0,0 +1,179 @@ +#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 "bitrl/sensors/sensor_manager.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" + + +#include +#include + + +#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: + + 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. + * + * 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 Step the underlying chrono::ChSystemSMC one time step + * @param time_step + */ + 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 + * @return + */ + 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;} + + /** + * 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_;} + + /** + * @return Pointer to the state of the robot + */ + std::shared_ptr pose()noexcept{return pose_;} + +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_; + + /** + * The chassis of the robot + */ + std::shared_ptr chassis_; + + /** + * The state of the robot + */ + std::shared_ptr pose_; + + /** + * @brief Manages the sensors on the robot + */ + sensors::SensorManager* sensor_manager_ptr_; + + /** + * @brief Motors for the robot + * motors_.first left motor + * motors.second right motor + */ + 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 +#endif //DIFF_DRIVE_ROBOT_H 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 0000000..43ed529 --- /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 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 0000000..088d3fa --- /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 diff --git a/src/bitrl/rigid_bodies/quadrotor.h b/src/bitrl/rigid_bodies/quadrotor.h deleted file mode 100644 index a9a8706..0000000 --- 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/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp new file mode 100644 index 0000000..0bfc77f --- /dev/null +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -0,0 +1,99 @@ +#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 + +#include + +namespace bitrl +{ +namespace sensors::backends +{ + +const std::string CHRONO_UltrasonicBackend::BACKEND_TYPE = "CHRONO_UltrasonicBackend"; + +CHRONO_UltrasonicBackend::CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, + std::shared_ptr body) + : +RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), +sys_ptr_(&sys_ptr), +body_(body) +{} + +void CHRONO_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(); + + 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 +} + +std::vector CHRONO_UltrasonicBackend::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(); + + chrono::ChVector3d end = start + dir * this -> max_distance(); + + 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)) + { + 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 0000000..6c18f36 --- /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 CHRONO_UltrasonicBackend + * @ingroup bitrl_sensors_backends + * @brief Class for modelling ultrasonic sensors + */ +class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase +{ +public: + + /** + * @brief The type of the sensor + */ + static const std::string BACKEND_TYPE; + + /** + * @brief Constructor + */ + CHRONO_UltrasonicBackend(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(const 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/mqtt_ultrasound_backend.cpp b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp new file mode 100644 index 0000000..a57f250 --- /dev/null +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp @@ -0,0 +1,77 @@ +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_MQTT +#include "bitrl/sensors/backends/mqtt_ultrasound_backend.h" +#include "bitrl/utils/io/json_file_reader.h" + +#ifdef BITRL_LOG +#include +#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 0000000..1514507 --- /dev/null +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h @@ -0,0 +1,107 @@ +#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: + + /** + * @brief Pointer to the instance that handles the MQTT connection + * and message exchange + */ + 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 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 0000000..6b06261 --- /dev/null +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.h @@ -0,0 +1,84 @@ +#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 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 + */ + 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}; + + real_t min_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 0000000..46198ee --- /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 0000000..41c4913 --- /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 0000000..3e0c8d8 --- /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 0000000..7083d5b --- /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/sensor_manager.cpp b/src/bitrl/sensors/sensor_manager.cpp new file mode 100644 index 0000000..e71f16b --- /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 0000000..8aefa5b --- /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 diff --git a/src/bitrl/sensors/sensors_module.h b/src/bitrl/sensors/sensors_module.h new file mode 100644 index 0000000..bdf277c --- /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 0000000..b8e066e --- /dev/null +++ b/src/bitrl/sensors/ultrasonic_sensor.cpp @@ -0,0 +1,42 @@ +#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 0000000..6ab001d --- /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 diff --git a/src/bitrl/utils/io/io_utils.cpp b/src/bitrl/utils/io/io_utils.cpp new file mode 100644 index 0000000..0f0bacc --- /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; +} +} +} diff --git a/src/bitrl/utils/io/io_utils.h b/src/bitrl/utils/io/io_utils.h index dbd0981..1c8c497 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 diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index aeac586..8668795 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 @@ -18,11 +15,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; /** @@ -30,14 +34,28 @@ 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; + + /** + * @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: - using json = nlohmann::json; json data_; }; 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 +63,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 diff --git a/version.h.in b/version.h.in index 5e0bfa4..1768a7a 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