From b6ca39a16c89f886c748e932f2227382a1c917f4 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 15 Mar 2026 18:01:41 +0000 Subject: [PATCH] finish RL environment example with Chrono (#174) --- CMakeLists.txt | 10 +- config.h.in | 3 + examples/example_12/example_12.cpp | 1 - examples/example_12/example_12.md | 4 +- examples/example_13/example_13.cpp | 406 ++++++++++++---------- examples/example_13/example_13.md | 357 +++++++++++++++---- examples/example_13/robot.h | 64 ++++ examples/example_13/robot_env.h | 62 ++++ src/bitrl/utils/bitrl_utils.h | 5 - src/bitrl/utils/render/irrlicht_utils.cpp | 47 +++ src/bitrl/utils/render/irrlicht_utils.h | 27 ++ 11 files changed, 721 insertions(+), 265 deletions(-) create mode 100644 examples/example_13/robot.h create mode 100644 examples/example_13/robot_env.h create mode 100644 src/bitrl/utils/render/irrlicht_utils.cpp create mode 100644 src/bitrl/utils/render/irrlicht_utils.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0fb4bea..033464b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,11 +30,11 @@ OPTION(ENABLE_WEBOTS OFF) # configure packages SET(ENABLE_EXAMPLES ON) SET(ENABLE_TESTS OFF) -SET(ENABLE_CHRONO ON) SET(ENABLE_OPENCV ON) SET(ENABLE_MQTT ON) SET(BITRL_LOG ON) SET(BITRL_IRRLICHT ON) +SET(ENABLE_CHRONO ON) # build options SET(CMAKE_BUILD_TYPE "Release") @@ -143,6 +143,7 @@ INCLUDE_DIRECTORIES(/usr/local/include/) # mqtt INCLUDE_DIRECTORIES(src/) FILE(GLOB SRCS src/bitrl/*.cpp + src/bitrl/boards/arduino/*.cpp src/bitrl/network/*.cpp src/bitrl/sensors/*.cpp src/bitrl/sensors/messages/*.cpp @@ -170,12 +171,13 @@ FILE(GLOB SRCS src/bitrl/*.cpp src/bitrl/utils/*.cpp src/bitrl/utils/io/*.cpp src/bitrl/utils/maths/statistics/distributions/*.cpp + src/bitrl/utils/render/*.cpp + src/bitrl/systems/*.cpp #src/bitrl/utils/geometry/*.cpp #src/bitrl/utils/geometry/shapes/*.cpp #src/bitrl/utils/geometry/mesh/*.cpp - #src/bitrl/utils/trajectory/*.cpp - src/bitrl/boards/arduino/*.cpp - src/bitrl/systems/*.cpp + #src/bitrl/utils/trajectory/*.cp + ) ADD_LIBRARY(bitrllib SHARED ${SRCS}) diff --git a/config.h.in b/config.h.in index 140ce9d..1619a3b 100755 --- a/config.h.in +++ b/config.h.in @@ -19,6 +19,9 @@ /*Use Chrono*/ #cmakedefine BITRL_CHRONO +/*Use Irrlicht for visualization*/ +#cmakedefine BITRL_IRRLICHT + /*Use OpenCV*/ #cmakedefine BITRL_OPENCV diff --git a/examples/example_12/example_12.cpp b/examples/example_12/example_12.cpp index 8c8c20e..7f7d5f3 100644 --- a/examples/example_12/example_12.cpp +++ b/examples/example_12/example_12.cpp @@ -194,7 +194,6 @@ int main() { // create the visualization system chrono::irrlicht::ChVisualSystemIrrlicht vis; - vis.AttachSystem(&system); vis.SetWindowSize(1280,720); vis.SetWindowTitle("Example 12 Differential Drive Robot"); diff --git a/examples/example_12/example_12.md b/examples/example_12/example_12.md index 5f4a785..94b3e90 100644 --- a/examples/example_12/example_12.md +++ b/examples/example_12/example_12.md @@ -17,7 +17,7 @@ Running a simulation with Chrono requires that we create a _ChSystem_ instance A _ChSystem_ is an abstract class. The Chrono library provides the following subclasses: - _ChSystemNSC_ for Non Smooth Contacts (NSC): in case of contacts a complementarity solver will take care of them using non-smooth dynamics; this is very efficient even with large time steps. -- _ChSystemSMC_ for SMooth Contacts (SMC): contacts are handled using penalty methods, i.e. contacts are deformable. +- _ChSystemSMC_ for Smooth Contacts (SMC): contacts are handled using penalty methods, i.e. contacts are deformable. Note that if there are no contacts or collisions in your system, it is indifferent to use _ChSystemNSC_ or _ChSystemSMC_. @@ -59,7 +59,7 @@ $$ v = \frac{r}{2}(\omega_R + \omega_L) $$ -where \f$r\f$ is the radius of the wheel and \f$\omega_i\f$ if the angular velocity of motor \f$\i\f$ in rad/s. +where \f$r\f$ is the radius of the wheel and \f$\omega_i\f$ if the angular velocity of motor \f$i\f$ in rad/s. We can use this relationship in a control simulation in order to control either the linear velocity or the motor speed. @code{.cpp} diff --git a/examples/example_13/example_13.cpp b/examples/example_13/example_13.cpp index ed455bf..09d3f06 100644 --- a/examples/example_13/example_13.cpp +++ b/examples/example_13/example_13.cpp @@ -1,234 +1,282 @@ #include "bitrl/bitrl_config.h" -#ifdef BITRL_CHRONO +#if defined(BITRL_CHRONO) && defined(BITRL_IRRLICHT) #include "bitrl/bitrl_types.h" - +#include "bitrl/bitrl_consts.h" +#include "bitrl/utils/render/irrlicht_utils.h" #ifdef BITRL_LOG #define BOOST_LOG_DYN_LINK #include #endif -#include "bitrl/bitrl_consts.h" -#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" - -#include "chrono/core/ChRealtimeStep.h" -#include "chrono/physics/ChSystemNSC.h" -#include -#include -#include "chrono/assets/ChVisualSystem.h" -#include -// #include "chrono/assets/ChColorAsset.h" -// #include "chrono/assets/ChLineShape.h" - - -#include -#include - -#include "chrono/physics/ChSystemNSC.h" -#include "chrono/physics/ChBodyEasy.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" -#include "chrono/functions/ChFunctionConst.h" - -#include "chrono_irrlicht/ChVisualSystemIrrlicht.h" +#include "robot.h" +#include "robot_env.h" +namespace example_13 +{ using namespace chrono; using namespace chrono::irrlicht; -namespace -{ -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() { - - // ----------------------------- - // Create Chrono physical system - // ----------------------------- - ChSystemNSC system; - //ChSystemSMC system; - system.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-9.81)); - system.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); - - // ----------------------------- - // Create ground - // ----------------------------- - auto material = chrono_types::make_shared(); +const uint_t WINDOW_HEIGHT = 800; +const uint_t WINDOW_WIDTH = 1024; +const real_t DT = 0.002; +const real_t SIM_TIME = 5.0; +const std::string WINDOW_TITLE( "Example 13"); +// helper functions to build the various components +auto build_generic_material() +{ + auto material = chrono_types::make_shared(); material->SetFriction(0.8f); material->SetRestitution(0.1f); + return material; +} + +auto build_floor(std::shared_ptr material) +{ auto floor = chrono_types::make_shared( - 5, 5, 0.1, // size - 1000, // density - true, true, material); // visual + collision + 5, 5, 0.1, // size + 1000, // density + true, true, material); // visual + collision floor->SetPos(chrono::ChVector3d(0,0,-0.05)); floor->SetFixed(true); floor->EnableCollision(true); + return floor; +} - system.Add(floor); - - // ----------------------------- - // Robot chassis - // ----------------------------- - double wheel_radius = 0.1; - double chassis_height = 0.1; +auto build_chassis(std::shared_ptr material) +{ auto chassis = chrono_types::make_shared( - 0.5, 0.3, chassis_height, + 0.5, 0.3, CHASSIS_HEIGHT, 1000, true, true, material); - // 0,0,wheel_radius + chassis_height/2.0 - //chrono::ChVector3d pos(0,0,0.2); - chrono::ChVector3d pos(0.0, 0.0, wheel_radius + chassis_height/2.0 + 0.01); + chrono::ChVector3d pos(0.0, 0.0, WHEEL_RADIUS + CHASSIS_HEIGHT/2.0 + 0.01); chassis->SetPos(pos); chrono::ChVector3d vel(0.5, 0.0, 0.0); chassis->SetPosDt(vel); - system.Add(chassis); - - // ----------------------------- - // Wheels - // ----------------------------- - - double wheel_width = 0.05; - //auto material = chrono_types::make_shared(); - auto left_wheel = chrono_types::make_shared( - chrono::ChAxis::Y, - wheel_radius, - wheel_width, - 1000, - true, true, material); - - auto right_wheel = chrono_types::make_shared( - chrono::ChAxis::Y, - wheel_radius, - wheel_width, - 1000, - true, true, material); - - left_wheel->SetPos(chrono::ChVector3d(0, 0.2, 0.1)); - right_wheel->SetPos(chrono::ChVector3d(0,-0.2, 0.1)); - - system.Add(left_wheel); - system.Add(right_wheel); - - // ----------------------------- - // Motors (differential drive) - // ----------------------------- - auto motor_left = chrono_types::make_shared(); - auto motor_right = chrono_types::make_shared(); - - chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2); - motor_left->Initialize( - left_wheel, - chassis, - chrono::ChFrame<>(chrono::ChVector3d(0,0.2,0.1), rot) - ); - - motor_right->Initialize( - right_wheel, - chassis, - chrono::ChFrame<>(chrono::ChVector3d(0,-0.2,0.1), rot) - ); + chassis->EnableCollision(true); + return chassis; +} - system.Add(motor_left); - system.Add(motor_right); +auto build_wheel(std::shared_ptr material, const chrono::ChVector3d& pos) +{ + auto wheel = chrono_types::make_shared(chrono::ChAxis::Y, + WHEEL_RADIUS,WHEEL_WIDTH, + 1000, true, true, material); - // ----------------------------- - // Wheel speeds - // ----------------------------- - auto speed_left = chrono_types::make_shared(-4.5); - auto speed_right = chrono_types::make_shared(-4.5); + wheel->SetPos(pos); + wheel->EnableCollision(true); + return wheel; +} - motor_left->SetSpeedFunction(speed_left); - motor_right->SetSpeedFunction(speed_right); +auto build_caster_wheel(std::shared_ptr material, const chrono::ChVector3d& pos) +{ - double caster_radius = 0.05; auto caster = chrono_types::make_shared( - caster_radius, // radius + CASTER_RADIUS, // radius 1000, // density true, // visualization true, // collision material); - caster->SetPos(chrono::ChVector3d(0.2, 0, 0.05)); + caster->SetPos(pos); caster->EnableCollision(true); + return caster; +} - system.Add(caster); - +auto build_caster_joint(std::shared_ptr caster, + std::shared_ptr chassis) +{ auto caster_joint = chrono_types::make_shared(); caster_joint->Initialize( caster, chassis, - ChFrame<>(chrono::ChVector3d(-0.25, 0, caster_radius), QUNIT)); + ChFrame<>(chrono::ChVector3d(-0.25, 0, CASTER_RADIUS), QUNIT)); + return caster_joint; - system.Add(caster_joint); +} - floor->EnableCollision(true); - chassis->EnableCollision(true); - left_wheel->EnableCollision(true); - right_wheel->EnableCollision(true); - - // ----------------------------- - // Irrlicht visualization - // ----------------------------- - chrono::irrlicht::ChVisualSystemIrrlicht vis; - - vis.AttachSystem(&system); - - vis.SetWindowSize(1280,720); - vis.SetWindowTitle("Chrono Differential Drive Robot"); - - vis.Initialize(); - vis.AddSkyBox(); - vis.AddLogo(); - vis.AddSkyBox(); - vis.AddCamera({0, -2, 1}, {0, 0, 0}); - vis.AddTypicalLights(); - vis.BindAll(); - // 1 meter length - // ----------------------------- - // Simulation loop - // ----------------------------- - double step_size = 0.002; - - while (vis.Run()) { - - vis.BeginScene(); - vis.Render(); - draw_world_axes(vis, 2.0); - - auto position = chassis -> GetPos(); - std::cout << position < wheel, + std::shared_ptr speed_func, + std::shared_ptr chassis, + const chrono::ChVector3d& pos) +{ + auto motor = chrono_types::make_shared(); + chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2); + motor->Initialize( + wheel, + chassis, + chrono::ChFrame<>(pos, rot) + ); + + motor->SetSpeedFunction(speed_func); + return motor; +} + + +void +Robot::build(std::shared_ptr material) +{ + chassis_ = build_chassis(material); + left_wheel_ = build_wheel(material, chrono::ChVector3d(0, 0.2, 0.1)); + right_wheel_ = build_wheel(material, chrono::ChVector3d(0,-0.2, 0.1)); + left_wheel_motor_ = build_motor(left_wheel_, chrono_types::make_shared(-4.5), + chassis_, + chrono::ChVector3d(0,0.2,0.1)); + right_wheel_motor_ = build_motor(right_wheel_, chrono_types::make_shared(-4.5), + chassis_, + chrono::ChVector3d(0,-0.2,0.1)); + caster_wheel_ = build_caster_wheel(material, chrono::ChVector3d(0.2, 0, 0.05)); + caster_link_joint_ = build_caster_joint(caster_wheel_, chassis_); +} + + +void Robot::add_to_sys(chrono::ChSystemNSC& sys) const +{ + sys.Add(chassis_); + sys.Add(left_wheel_); + sys.Add(right_wheel_); + sys.Add(left_wheel_motor_); + sys.Add(right_wheel_motor_); + sys.Add(caster_wheel_); + sys.Add(caster_link_joint_); +} + +void +Robot::set_speed(real_t speed) +{ + chrono::ChVector3d vel(speed, 0.0, 0.0); + chassis_->SetPosDt(vel); + +} + +void Robot::reset() +{ + chrono::ChVector3d pos(0.0, 0.0, WHEEL_RADIUS + CHASSIS_HEIGHT/2.0 + 0.01); + chassis_ -> SetPos(pos); + left_wheel_ -> SetPos(chrono::ChVector3d(0, 0.2, 0.1)); + right_wheel_ -> SetPos(chrono::ChVector3d(0,-0.2, 0.1)); + caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0, 0.05)); +} + +void RobotEnv::make(const std::string &version, + const std::unordered_map &make_options, + const std::unordered_map &reset_options) +{ + + auto material = build_generic_material(); + robot_.build(material); + build_system_(material); + + 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 +RobotEnv::reset() +{ + sim_counter_ = 1; + current_time_ = 0.0; + robot_.reset(); + robot_.set_speed(0.5); + + auto robot_position = robot_.position(); + return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); +} + + +time_step_type +RobotEnv::step(const action_type &/*action*/) +{ + + if (sim_counter_ % 600 == 0) + { +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; +#endif + return reset(); } + sys_.DoStepDynamics(DT); + auto robot_position = robot_.position(); + + sim_counter_++; + current_time_ += DT; + return time_step_type(TimeStepTp::MID, 1.0, robot_position); +} + +void RobotEnv::build_system_(std::shared_ptr material) +{ + sys_.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G)); + sys_.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); + + auto floor = build_floor(material); + sys_.Add(floor); +} + + +void RobotEnv::simulate() +{ + + chrono::irrlicht::ChVisualSystemIrrlicht visual; + visual.AttachSystem(&sys_); + visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); + visual.SetWindowTitle(WINDOW_TITLE); + visual.Initialize(); + + 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); + utils::render::irr_draw_world_axes(visual); + + 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 diff --git a/examples/example_13/example_13.md b/examples/example_13/example_13.md index 057dd06..95ce802 100644 --- a/examples/example_13/example_13.md +++ b/examples/example_13/example_13.md @@ -1,128 +1,298 @@ \page bitrl_example_13 BitRL Example 13 Using chrono::ChSystemSMC -Example \ref bitrl_example_11 discussed _ChBody_. Specifically, how to create a _ChBodyEasyBox_. -Understanding rigid bodies is fundamental to robotics. In this example we will discuss _ChSystem_. -This is the cornerstone of a Chrono simulation. Most information in this example -can be found in the official doumentation here: Simulation system. -A _ChSystem_ is an abstract class. The Chrono library provides the following subclasses: - -- _ChSystemNSC_ for Non Smooth Contacts (NSC): in case of contacts a complementarity solver will take care of them using non-smooth dynamics; this is very efficient even with large time steps. -- _ChSystemSMC_ for SMooth Contacts (SMC): contacts are handled using penalty methods, i.e. contacts are deformable - -Note that if there are no contacts or collisions in your system, it is indifferent to use _ChSystemNSC_ or _ChSystemSMC_. -In this example we will use the \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot" class -to simulate a differential drive system using Chrono. The \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot" -follows the Turtlebot robot model defined in Chrono. -You will need the mesh files from the Chrono project in order to visualize the robot. These files should be place in the -\ref ROBOTS_DATA_DIR directory under the _diff_drive_robot_ subdirectory of the project. Below is the driver code. +Example \ref bitrl_example_12 showed you how to create a rigid body simulation in Chrono. We will extend this example +and show how to create a reinforcement learning environment that can be queried by reinforcement learning agents. + +Reinforcement learning in _bitrl_ should implement the \ref bitrl::envs::EnvBase "`bitrl::envs::EnvBase`" class. +This is an abstract base class that defines the main endpoints that a reinforcement learning environment should implement. +We will keep things simple in this example and assume that the action space is discrete with three actions available. +We won't be doing anything meaningful in this example other than showing how a reinforcement learning environment can be +implemented using Chrono. + +The state space is equally simple and returns the position of the robot. +Below is the driver implementing this. To a large extent this is the same as example \ref bitrl_example_12. +We have also introduced the headers _robot.h_ and _robot_env.h_ to separate concerns. @code{.cpp} #include "bitrl/bitrl_config.h" -#ifdef BITRL_CHRONO +#if defined(BITRL_CHRONO) && defined(BITRL_IRRLICHT) #include "bitrl/bitrl_types.h" - +#include "bitrl/bitrl_consts.h" +#include "bitrl/utils/render/irrlicht_utils.h" #ifdef BITRL_LOG #define BOOST_LOG_DYN_LINK #include #endif -#include "chrono/core/ChRealtimeStep.h" -#include "chrono/physics/ChSystemNSC.h" -#include -#include -#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" - -#include -#include +#include "robot.h" +#include "robot_env.h" namespace example_13 { -using namespace bitrl; +using namespace chrono; 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.01; +const real_t DT = 0.002; const real_t SIM_TIME = 5.0; const std::string WINDOW_TITLE( "Example 13"); -void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual) +// helper functions to build the various components +auto build_generic_material() { -visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); -visual.SetWindowTitle(WINDOW_TITLE); -visual.Initialize(); +auto material = chrono_types::make_shared(); +material->SetFriction(0.8f); +material->SetRestitution(0.1f); +return material; +} - visual.AddLogo(); - visual.AddSkyBox(); - visual.AddCamera({0, -2, 1}, {0, 0, 0}); - visual.AddTypicalLights(); - visual.BindAll(); +auto build_floor(std::shared_ptr material) +{ +auto floor = chrono_types::make_shared( +5, 5, 0.1, // size +1000, // density +true, true, material); // visual + collision + + floor->SetPos(chrono::ChVector3d(0,0,-0.05)); + floor->SetFixed(true); + floor->EnableCollision(true); + return floor; } -} // namespace example_13 +auto build_chassis(std::shared_ptr material) +{ +auto chassis = chrono_types::make_shared( +0.5, 0.3, CHASSIS_HEIGHT, +1000, +true, true, material); -int main() + chrono::ChVector3d pos(0.0, 0.0, WHEEL_RADIUS + CHASSIS_HEIGHT/2.0 + 0.01); + chassis->SetPos(pos); + + chrono::ChVector3d vel(0.5, 0.0, 0.0); + chassis->SetPosDt(vel); + chassis->EnableCollision(true); + return chassis; +} + +auto build_wheel(std::shared_ptr material, const chrono::ChVector3d& pos) { -using namespace example_13; -chrono::ChSystemNSC sys; -sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81)); +auto wheel = chrono_types::make_shared(chrono::ChAxis::Y, +WHEEL_RADIUS,WHEEL_WIDTH, +1000, true, true, material); - sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); - chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025); - chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025); + wheel->SetPos(pos); + wheel->EnableCollision(true); + return wheel; +} - auto floor_mat = chrono_types::make_shared(); - auto mfloor = chrono_types::make_shared(20, 20, 1, 1000, true, true, floor_mat); - mfloor->SetPos(chrono::ChVector3d(0, 0, -1)); - mfloor->SetFixed(true); - mfloor->GetVisualShape(0)->SetTexture(chrono::GetChronoDataFile("textures/concrete.jpg")); - sys.Add(mfloor); +auto build_caster_wheel(std::shared_ptr material, const chrono::ChVector3d& pos) +{ - bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys, - chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT); + auto caster = chrono_types::make_shared( + CASTER_RADIUS, // radius + 1000, // density + true, // visualization + true, // collision + material); - robot.init(); - robot.set_motor_speed(bitrl::consts::maths::PI, 0); - robot.set_motor_speed(bitrl::consts::maths::PI, 1); + caster->SetPos(pos); + caster->EnableCollision(true); + return caster; +} - chrono::irrlicht::ChVisualSystemIrrlicht visual; - prepare_visualization(visual); - visual.AttachSystem(&sys); +auto build_caster_joint(std::shared_ptr caster, +std::shared_ptr chassis) +{ +auto caster_joint = chrono_types::make_shared(); + + caster_joint->Initialize( + caster, + chassis, + ChFrame<>(chrono::ChVector3d(-0.25, 0, CASTER_RADIUS), QUNIT)); + return caster_joint; + +} + +auto build_motor(std::shared_ptr wheel, +std::shared_ptr speed_func, +std::shared_ptr chassis, +const chrono::ChVector3d& pos) +{ +auto motor = chrono_types::make_shared(); +chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2); +motor->Initialize( +wheel, +chassis, +chrono::ChFrame<>(pos, rot) +); + + motor->SetSpeedFunction(speed_func); + return motor; +} + + +void +Robot::build(std::shared_ptr material) +{ +chassis_ = build_chassis(material); +left_wheel_ = build_wheel(material, chrono::ChVector3d(0, 0.2, 0.1)); +right_wheel_ = build_wheel(material, chrono::ChVector3d(0,-0.2, 0.1)); +left_wheel_motor_ = build_motor(left_wheel_, chrono_types::make_shared(-4.5), +chassis_, +chrono::ChVector3d(0,0.2,0.1)); +right_wheel_motor_ = build_motor(right_wheel_, chrono_types::make_shared(-4.5), +chassis_, +chrono::ChVector3d(0,-0.2,0.1)); +caster_wheel_ = build_caster_wheel(material, chrono::ChVector3d(0.2, 0, 0.05)); +caster_link_joint_ = build_caster_joint(caster_wheel_, chassis_); +} + + +void Robot::add_to_sys(chrono::ChSystemNSC& sys) const +{ +sys.Add(chassis_); +sys.Add(left_wheel_); +sys.Add(right_wheel_); +sys.Add(left_wheel_motor_); +sys.Add(right_wheel_motor_); +sys.Add(caster_wheel_); +sys.Add(caster_link_joint_); +} + +void +Robot::set_speed(real_t speed) +{ +chrono::ChVector3d vel(speed, 0.0, 0.0); +chassis_->SetPosDt(vel); + +} + +void Robot::reset() +{ +chrono::ChVector3d pos(0.0, 0.0, WHEEL_RADIUS + CHASSIS_HEIGHT/2.0 + 0.01); +chassis_ -> SetPos(pos); +left_wheel_ -> SetPos(chrono::ChVector3d(0, 0.2, 0.1)); +right_wheel_ -> SetPos(chrono::ChVector3d(0,-0.2, 0.1)); +caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0, 0.05)); +} + +void RobotEnv::make(const std::string &version, +const std::unordered_map &make_options, +const std::unordered_map &reset_options) +{ + + auto material = build_generic_material(); + robot_.build(material); + build_system_(material); + + robot_.add_to_sys(sys_); + + this -> set_make_options_(make_options); + this -> set_reset_options_(reset_options); + this -> set_version_(version); + this -> make_created_(); + +} - // Simulation loop +time_step_type +RobotEnv::reset() +{ +sim_counter_ = 1; +current_time_ = 0.0; +robot_.reset(); +robot_.set_speed(0.5); + + auto robot_position = robot_.position(); + return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); +} + + +time_step_type +RobotEnv::step(const action_type &/*action*/) +{ + + if (sim_counter_ % 600 == 0) + { +#ifdef BITRL_LOG +BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; +#endif +return reset(); +} + + sys_.DoStepDynamics(DT); + auto robot_position = robot_.position(); + + sim_counter_++; + current_time_ += DT; + return time_step_type(TimeStepTp::MID, 1.0, robot_position); +} + +void RobotEnv::build_system_(std::shared_ptr material) +{ +sys_.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G)); +sys_.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); + + auto floor = build_floor(material); + sys_.Add(floor); +} - // Timer for enforcing soft real-time - chrono::ChRealtimeStepTimer realtime_timer; - // bool removed = false; - while (visual.Run()) { +void RobotEnv::simulate() +{ + + chrono::irrlicht::ChVisualSystemIrrlicht visual; + visual.AttachSystem(&sys_); + visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); + visual.SetWindowTitle(WINDOW_TITLE); + visual.Initialize(); + + 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(); - // Irrlicht now draws simple lines in 3D world representing a - // skeleton of the mechanism, in this instant: - // // .. draw items belonging to Irrlicht scene, if any visual.Render(); + // .. draw a grid tools::drawGrid(&visual, 0.5, 0.5); - // .. draw GUI items belonging to Irrlicht screen, if any - visual.GetGUIEnvironment()->drawAll(); + utils::render::irr_draw_world_axes(visual); - // ADVANCE SYSTEM STATE BY ONE STEP - sys.DoStepDynamics(DT); - - // Enforce soft real-time - realtime_timer.Spin(DT); + 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 @@ -138,4 +308,43 @@ return 1; #endif @endcode +Running the code above produces the following: + +@code +[2026-03-15 17:49:19.445734] [0x00007fbe766b0800] [info] At time: 0.002 position: 0.000830566 1.24587e-06 0.160031 +[2026-03-15 17:49:19.462184] [0x00007fbe766b0800] [info] At time: 0.004 position: 0.00166125 2.2391e-06 0.160032 +[2026-03-15 17:49:19.472812] [0x00007fbe766b0800] [info] At time: 0.006 position: 0.00249691 2.28551e-06 0.160009 +[2026-03-15 17:49:19.490501] [0x00007fbe766b0800] [info] At time: 0.008 position: 0.00333187 4.48252e-06 0.159995 +[2026-03-15 17:49:19.505314] [0x00007fbe766b0800] [info] At time: 0.01 position: 0.00416369 6.52889e-06 0.159995 +[2026-03-15 17:49:19.522646] [0x00007fbe766b0800] [info] At time: 0.012 position: 0.00499371 5.18348e-06 0.159992 +[2026-03-15 17:49:19.538717] [0x00007fbe766b0800] [info] At time: 0.014 position: 0.00582212 4.4504e-06 0.159994 +[2026-03-15 17:49:19.555803] [0x00007fbe766b0800] [info] At time: 0.016 position: 0.00664943 5.01632e-06 0.159993 +[2026-03-15 17:49:19.573243] [0x00007fbe766b0800] [info] At time: 0.018 position: 0.00747616 5.43181e-06 0.159994 +[2026-03-15 17:49:19.589256] [0x00007fbe766b0800] [info] At time: 0.02 position: 0.00830281 1.09076e-05 0.159984 +[2026-03-15 17:49:19.605600] [0x00007fbe766b0800] [info] At time: 0.022 position: 0.00912996 1.29684e-05 0.159984 +[2026-03-15 17:49:19.623059] [0x00007fbe766b0800] [info] At time: 0.024 position: 0.00995797 9.89451e-06 0.159999 +[2026-03-15 17:49:19.639142] [0x00007fbe766b0800] [info] At time: 0.026 position: 0.0107875 7.19161e-06 0.159997 +[2026-03-15 17:49:19.657061] [0x00007fbe766b0800] [info] At time: 0.028 position: 0.0116187 9.68353e-06 0.159997 +[2026-03-15 17:49:19.673839] [0x00007fbe766b0800] [info] At time: 0.03 position: 0.0124514 1.10808e-05 0.159997 +[2026-03-15 17:49:19.690629] [0x00007fbe766b0800] [info] At time: 0.032 position: 0.0132852 1.08047e-05 0.160001 +[2026-03-15 17:49:19.706856] [0x00007fbe766b0800] [info] At time: 0.034 position: 0.01412 1.23457e-05 0.159996 +[2026-03-15 17:49:19.723398] [0x00007fbe766b0800] [info] At time: 0.036 position: 0.0149556 1.28846e-05 0.159996 +[2026-03-15 17:49:19.741549] [0x00007fbe766b0800] [info] At time: 0.038 position: 0.0157916 1.17644e-05 0.160001 +[2026-03-15 17:49:19.758843] [0x00007fbe766b0800] [info] At time: 0.04 position: 0.0166281 1.43541e-05 0.159996 +[2026-03-15 17:49:19.773935] [0x00007fbe766b0800] [info] At time: 0.042 position: 0.0174651 2.02247e-05 0.15999 +[2026-03-15 17:49:19.791439] [0x00007fbe766b0800] [info] At time: 0.044 position: 0.0183026 2.4407e-05 0.159992 +[2026-03-15 17:49:19.806636] [0x00007fbe766b0800] [info] At time: 0.046 position: 0.0191408 2.38908e-05 0.159993 +[2026-03-15 17:49:19.823233] [0x00007fbe766b0800] [info] At time: 0.048 position: 0.0199796 2.83747e-05 0.159994 +[2026-03-15 17:49:19.839922] [0x00007fbe766b0800] [info] At time: 0.05 position: 0.0208194 3.02409e-05 0.159994 +[2026-03-15 17:49:19.857106] [0x00007fbe766b0800] [info] At time: 0.052 position: 0.0216601 3.2111e-05 0.159996 +[2026-03-15 17:49:19.874365] [0x00007fbe766b0800] [info] At time: 0.054 position: 0.0225017 3.56516e-05 0.159994 +[2026-03-15 17:49:19.889853] [0x00007fbe766b0800] [info] At time: 0.056 position: 0.0233442 3.70565e-05 0.159993 +[2026-03-15 17:49:19.907112] [0x00007fbe766b0800] [info] At time: 0.058 position: 0.0241875 3.91014e-05 0.159997 +[2026-03-15 17:49:19.923881] [0x00007fbe766b0800] [info] At time: 0.06 position: 0.0250316 3.76584e-05 0.159992 +[2026-03-15 17:49:19.940460] [0x00007fbe766b0800] [info] At time: 0.062 position: 0.0258765 3.69224e-05 0.159992 +[2026-03-15 17:49:19.957294] [0x00007fbe766b0800] [info] At time: 0.064 position: 0.0267219 3.70038e-05 0.159996 +[2026-03-15 17:49:19.974156] [0x00007fbe766b0800] [info] At time: 0.066 position: 0.0275681 3.7808e-05 0.159993 +... +@endcode + diff --git a/examples/example_13/robot.h b/examples/example_13/robot.h new file mode 100644 index 0000000..3bc524f --- /dev/null +++ b/examples/example_13/robot.h @@ -0,0 +1,64 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#include "bitrl/bitrl_types.h" +#include +#include +#include +#include + + +#include +namespace example_13 +{ + +using namespace bitrl; + +const real_t WHEEL_RADIUS = 0.1; +const real_t CHASSIS_HEIGHT = 0.1; +const real_t WHEEL_WIDTH = 0.05; +const real_t CASTER_RADIUS = 0.05; + +auto build_wheel(std::shared_ptr material, const chrono::ChVector3d& pos); +auto build_motor(std::shared_ptr wheel, + std::shared_ptr speed_func, + std::shared_ptr chassis, + const chrono::ChVector3d& pos); + +class Robot +{ +public: + + // constructor + Robot()=default; + + // add the components of this robot to the system we simulate + void add_to_sys(chrono::ChSystemNSC& sys) const; + + // build the robot + void build(std::shared_ptr material); + + // set the speeds of the motors + void set_speed(real_t speed); + + // reset the robot + void reset(); + + // the pose of the robot + chrono::ChVector3d position()noexcept{return chassis_ -> GetPos();} + +private: + + std::shared_ptr chassis_; + std::shared_ptr left_wheel_; + std::shared_ptr right_wheel_; + std::shared_ptr left_wheel_motor_; + std::shared_ptr right_wheel_motor_; + std::shared_ptr caster_wheel_; + std::shared_ptr caster_link_joint_; + +}; +} + + +#endif //ROBOT_H diff --git a/examples/example_13/robot_env.h b/examples/example_13/robot_env.h new file mode 100644 index 0000000..ed8e161 --- /dev/null +++ b/examples/example_13/robot_env.h @@ -0,0 +1,62 @@ +#ifndef ROBOT_ENV_H +#define ROBOT_ENV_H + +#include "bitrl/bitrl_types.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 + +#include "robot.h" + +#include +#include +#include + +namespace example_13 +{ + +using namespace bitrl; + +constexpr uint_t STATE_SPACE_SIZE = 3; +constexpr uint_t ACTION_SPACE_SIZE = 3; + +using bitrl::TimeStepTp; +using bitrl::TimeStep; + +typedef TimeStep time_step_type; +typedef bitrl::envs::ContinuousVectorStateContinuousVectorActionEnv space_type; + +class RobotEnv final: public bitrl::envs::EnvBase +{ +public: + + typedef typename space_type::action_type action_type; + + RobotEnv()=default; + + 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: + + Robot robot_; + chrono::ChSystemNSC sys_; + uint_t sim_counter_{0}; + real_t current_time_{0.0}; + void build_system_(std::shared_ptr material); + +}; + + +} + +#endif //ROBOT_ENV_H diff --git a/src/bitrl/utils/bitrl_utils.h b/src/bitrl/utils/bitrl_utils.h index d2fda1f..ff12476 100644 --- a/src/bitrl/utils/bitrl_utils.h +++ b/src/bitrl/utils/bitrl_utils.h @@ -1,11 +1,6 @@ -// -// Created by alex on 12/13/25. -// - #ifndef BITRL_UTILS_H #define BITRL_UTILS_H -//#include #include #include #include diff --git a/src/bitrl/utils/render/irrlicht_utils.cpp b/src/bitrl/utils/render/irrlicht_utils.cpp new file mode 100644 index 0000000..bf56000 --- /dev/null +++ b/src/bitrl/utils/render/irrlicht_utils.cpp @@ -0,0 +1,47 @@ +#ifndef IRRLICHT_UTILS_H +#define IRRLICHT_UTILS_H + +#include "bitrl/bitrl_config.h" + +#if defined(BITRL_IRRLICHT) && defined(BITRL_CHRONO) +#include "bitrl/utils/render/irrlicht_utils.h" +#include "bitrl/bitrl_types.h" +#include + +namespace bitrl +{ +namespace utils +{ +namespace render +{ + +void irr_draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, + real_t 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)); +} + +} +} +} + +#endif +#endif //IRRLICHT_UTILS_H + diff --git a/src/bitrl/utils/render/irrlicht_utils.h b/src/bitrl/utils/render/irrlicht_utils.h new file mode 100644 index 0000000..a7ffce0 --- /dev/null +++ b/src/bitrl/utils/render/irrlicht_utils.h @@ -0,0 +1,27 @@ +#ifndef IRRLICHT_UTILS_H +#define IRRLICHT_UTILS_H + +#include "bitrl/bitrl_config.h" + +#if defined(BITRL_IRRLICHT) && defined(BITRL_CHRONO) +#include "bitrl/bitrl_types.h" +#include + +namespace bitrl +{ +namespace utils +{ +namespace render +{ + +/// +/// Draw the global axis on the given ChVisualSystemIrrlicht system +/// +void irr_draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, real_t scale = 1.0); + +} +} +} + +#endif +#endif //IRRLICHT_UTILS_H