Merge pull request #177 from pockerman/feat/174-add-diff-drive-robot-model

Feat/174 add diff drive robot model

Author: pockerman

config.h.in

@@ -16,7 +16,7 @@
 /*Use Webots*/
 #cmakedefine BITRL_WEBOTS
 
-/*Use Chron*/
+/*Use Chrono*/
 #cmakedefine BITRL_CHRONO
 
 /*Use OpenCV*/

examples/CMakeLists.txt

@@ -29,6 +29,7 @@ ADD_SUBDIRECTORY(example_2)
 ADD_SUBDIRECTORY(example_3)
 ADD_SUBDIRECTORY(example_4)
 ADD_SUBDIRECTORY(example_5)
+ADD_SUBDIRECTORY(example_6)
 ADD_SUBDIRECTORY(example_7)
 ADD_SUBDIRECTORY(example_8)
 ADD_SUBDIRECTORY(example_9)

examples/example_11/example_11.md

@@ -3,14 +3,14 @@
 The <a href="https://github.com/projectchrono/chrono">Chrono</a> library is the main library that _bitrl_ is using in order
 to simulate robots and create environments for reinforcement learning agents. As such, knowing your way around
 Chrono is essential. However, Chrono is a relatively large library with many components and therefore not necessarily easy
-to grasp. In a series of examples, we will see main components of the library that _bitrl_ utilizes.
+to grasp. In a series of examples, we will see the main components of the library that _bitrl_ utilizes.
 Note that you should have compiled Chrono with <a href="https://irrlicht.sourceforge.io/">Irrlicht</a> support in order to be able to run this example.
 
 The main interface for creating rigid bodies in Chrono is the <a href="https://api.projectchrono.org/9.0.0/classchrono_1_1_ch_body.html">ChBody</a>
 class. You can also find this <a href="https://api.projectchrono.org/9.0.0/rigid_bodies.html"> Rigid Bodies</a> helpful.
-_ChBody_ is an abstract class, and therefore we cannot instantiate it directly. Chrono provides various classes however we can 
+_ChBody_ is an abstract class, and therefore we cannot instantiate it directly. Chrono provides various classes however, we can 
 immediately use. The one we will use in this example is the _ChBodyEasyBox_ class. This is defined in the _chrono/physics/ChBodyEasy.h_ header.
-Let's first create a box and try to visualize it. Below is the function that constructs a box.
+Let's first create a box and try to visualize it. The function below does exactly that.
 
 @code{.cpp}
 std::shared_ptr<chrono::ChBody> create_box(real_t xlength, real_t ylength, real_t zlength,
@@ -30,12 +30,14 @@ real_t density, bool create_visualization)
 @endcode
 
 
-The following is a helper for setting up the visualization
+The following is a helper for setting up the visualization. We need to build Chrono with Irrlicht support in order to be able to
+use it as a visualization engine. You can find more information about the 
+visualisation system in Chrono at <a href="https://api.projectchrono.org/9.0.0/visualization_system.html">Visualization System</a>.
 
 @code{.cpp}
 void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual)
 {
-visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT);
+visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); 
 visual.SetWindowTitle(WINDOW_TITLE);
 visual.Initialize();
 
@@ -47,8 +49,9 @@ visual.Initialize();
 }
 @endcode
 
-Below is the main function:
-
+Below is the main driver for this example. 
+It creates a box and attaches it to a _ChSystem_ (see <a href="https://api.projectchrono.org/9.0.0/simulation_system.html">Simulation system</a> for more information).
+It also sets up a minimal visualization obejct _ChVisualSystemIrrlicht_ for the simulation and finally runs the simulation loop.
 @code{.cpp}
 int main()
 {

examples/example_12/CMakeLists.txt

@@ -8,7 +8,7 @@ ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread  boost_log)
 
 IF(ENABLE_CHRONO)
-	TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core)
+	TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core Chrono::Chrono_irrlicht)
 ENDIF ()
 
 

examples/example_12/example_12.cpp

@@ -1,97 +1,235 @@
-/**
- * This example utilises the ```TensorboardServer``` class to log values of interest when running
- * an experiment. We can monitor the experimet  using <a
- *href="https://www.tensorflow.org/tensorboard">tensorboard</a>. The   ```TensorboardServer``` class
- *is a simple wrapper that exposes three functions
- *
- *	- ```add_scalar```
- *	- ```add_scalars```
- *	- ```add_text```
- *
- *	We will use ```add_scalar``` and ```add_text```. In order to run this example, fire up the
- *server using the ```torchboard_server/start_uvicorn.sh```. The server listens at port 8002. You
- *can change this however you want just make sure that the port is not used and also update the
- *	variable ```TORCH_SERVER_HOST``` in the code below accordingly. Note that the implementation
- *uses <a
- *href="https://pytorch.org/docs/stable/_modules/torch/utils/tensorboard/writer.html#SummaryWriter">SummaryWriter</a>
- *class. Thus you will need to have PyTorch installed on the machine that you run the server.
- */
+#include "bitrl/bitrl_config.h"
+
+#ifdef BITRL_CHRONO
 
 #include "bitrl/bitrl_types.h"
-#include "bitrl/utils/io/json_file_reader.h"
-#include "bitrl/network/tensorboard_server.h"
+#include "bitrl/bitrl_consts.h"
 
-#include <filesystem>
-#include <iostream>
-#include <random>
-#include <string>
+#ifdef BITRL_LOG
+#define BOOST_LOG_DYN_LINK
+#include <boost/log/trivial.hpp>
+#endif
+
+#include <chrono/physics/ChSystemNSC.h>
+#include <chrono/physics/ChBodyEasy.h>
+#include "chrono/assets/ChVisualSystem.h"
+#include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
+
+#include <chrono/physics/ChLinkMotorRotationSpeed.h>
+#include <chrono/functions/ChFunctionConst.h>
+
+#include <memory>
 
 namespace example_12
 {
+
+using namespace chrono;
+using namespace chrono::irrlicht;
 using namespace bitrl;
-using utils::io::JSONFileReader;
-using network::TensorboardServer;
 
-namespace fs = std::filesystem;
-const std::string CONFIG = "config.json";
+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;
+
+void 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<float>(scale), 0, 0},
+        irr::video::SColor(255, 255, 0, 0));
+
+    // Y axis (green)
+    driver->draw3DLine(
+        {0, 0, 0},
+        {0, static_cast<float>(scale), 0},
+        irr::video::SColor(255, 0, 255, 0));
+
+    // Z axis (blue)
+    driver->draw3DLine(
+        {0, 0, 0},
+        {0, 0, static_cast<float>(scale)},
+        irr::video::SColor(255, 0, 0, 255));
+}
+
+auto build_generic_material()
+{
+    auto material = chrono_types::make_shared<ChContactMaterialNSC>();
+    material->SetFriction(0.8f);
+    material->SetRestitution(0.1f);
+    return material;
+}
 
-} // namespace example_12
+auto build_floor(std::shared_ptr<ChContactMaterialNSC> material)
+{
+    auto floor = chrono_types::make_shared<chrono::ChBodyEasyBox>(
+         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;
+}
 
-int main()
+auto build_chassis(std::shared_ptr<ChContactMaterialNSC> material)
 {
+    auto chassis = chrono_types::make_shared<ChBodyEasyBox>(
+        0.5, 0.3, CHASSIS_HEIGHT,
+        1000,
+        true, true, material);
+
+    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;
+}
 
-    using namespace example_12;
+auto build_wheel(std::shared_ptr<ChContactMaterialNSC> material, const chrono::ChVector3d& pos)
+{
+    auto wheel = chrono_types::make_shared<chrono::ChBodyEasyCylinder>(
+          chrono::ChAxis::Y,
+          WHEEL_RADIUS,
+          WHEEL_WIDTH,
+          1000,
+          true, true, material);
+
+    wheel->SetPos(pos);
+    wheel->EnableCollision(true);
+    return wheel;
+}
 
-    try
-    {
+auto build_motor(std::shared_ptr<ChBodyEasyCylinder> wheel,
+                 std::shared_ptr<ChBodyEasyBox> chassis,
+                 std::shared_ptr<ChFunctionConst> speed_func,
+                 const chrono::ChVector3d& pos)
+{
+    auto motor = chrono_types::make_shared<chrono::ChLinkMotorRotationSpeed>();
 
-        // load the json configuration
-        JSONFileReader json_reader(CONFIG);
-        json_reader.open();
+    chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2);
+    motor->Initialize(
+        wheel,
+        chassis,
+        chrono::ChFrame<>(pos, rot)
+    );
 
-        auto experiment_dict = json_reader.template get_value<std::string>("experiment_dict");
-        auto experiment_id = json_reader.template get_value<std::string>("experiment_id");
+    motor->SetSpeedFunction(speed_func);
+    return motor;
+}
 
-        std::cout << "Experiment directory: " << experiment_dict << std::endl;
-        std::cout << "Experiment id: " << experiment_id << std::endl << std::endl;
+auto build_caster_wheel(std::shared_ptr<ChContactMaterialNSC> material)
+{
 
-        const fs::path EXPERIMENT_DIR_PATH = experiment_dict + experiment_id;
+    auto caster = chrono_types::make_shared<ChBodyEasySphere>(
+    CASTER_RADIUS,       // radius
+    1000,       // density
+    true,       // visualization
+    true,       // collision
+    material);
 
-        // the first thing we want to do when monitoring experiments
-        // is to create a directory where all data will reside
-        std::filesystem::create_directories(experiment_dict + experiment_id);
+    caster->SetPos(chrono::ChVector3d(0.2, 0, 0.05));
+    caster->EnableCollision(true);
+    return caster;
+}
 
-        const auto input_size = json_reader.template get_value<uint_t>("input_size");
-        const auto output_size = json_reader.template get_value<uint_t>("output_size");
-        const auto num_epochs = json_reader.template get_value<uint_t>("num_epochs");
-        const auto learning_rate = json_reader.template get_value<real_t>("lr");
-        auto log_path = json_reader.template get_value<std::string>("log_path");
+auto build_caster_joint(std::shared_ptr<ChBodyEasySphere> caster,
+                        std::shared_ptr<ChBodyEasyBox> chassis)
+{
+    auto caster_joint = chrono_types::make_shared<ChLinkLockRevolute>();
 
-        // log the hyperparameters
-        std::cout << "Input size: " << input_size << std::endl;
-        std::cout << "Output size: " << output_size << std::endl;
-        std::cout << "Max epochs: " << num_epochs << std::endl;
-        std::cout << "Learning rate: " << learning_rate << std::endl;
+    caster_joint->Initialize(
+        caster,
+        chassis,
+        ChFrame<>(chrono::ChVector3d(-0.25, 0, CASTER_RADIUS), QUNIT));
+    return caster_joint;
 
-        TensorboardServer logger("http://0.0.0.0:8002");
+}
+}
 
-        std::cout << "Logging results at " << logger.get_log_dir_path() << std::endl;
-        logger.init(log_path);
+int main() {
 
-        logger.add_scalar("lr", learning_rate);
-        logger.add_scalar("seed", 42);
-        logger.add_scalar("num_epochs", num_epochs);
-        logger.add_text("optimizer", "torch::optim::SGD");
-        logger.close();
-    }
-    catch (std::exception &e)
-    {
-        std::cout << e.what() << std::endl;
-    }
-    catch (...)
-    {
-        std::cout << "Unknown exception occured" << std::endl;
+    using namespace example_12;
+    ChSystemNSC system;
+    system.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G));
+    system.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
+
+    // create a generic material to use
+    auto material = example_12::build_generic_material();
+
+    // build floor
+    auto floor = example_12::build_floor(material);
+    system.Add(floor);
+
+    // build chassis
+    auto chassis = example_12::build_chassis(material);
+    system.Add(chassis);
+
+    // build wheels
+    auto left_wheel = example_12::build_wheel(material,chrono::ChVector3d(0, 0.2, 0.1));
+    auto right_wheel = example_12::build_wheel(material,chrono::ChVector3d(0,-0.2, 0.1));
+    system.Add(left_wheel);
+    system.Add(right_wheel);
+
+    // build motor
+    auto motor_left = example_12::build_motor(left_wheel, chassis,
+        chrono_types::make_shared<chrono::ChFunctionConst>(-4.5), chrono::ChVector3d(0,0.2,0.1));
+    auto motor_right = build_motor(right_wheel, chassis,
+        chrono_types::make_shared<chrono::ChFunctionConst>(-4.5), chrono::ChVector3d(0,-0.2,0.1));
+
+    system.Add(motor_left);
+    system.Add(motor_right);
+
+    auto caster = example_12::build_caster_wheel(material);
+    system.Add(caster);
+
+    auto caster_joint = example_12::build_caster_joint(caster, chassis);
+    system.Add(caster_joint);
+
+    // create the visualization system
+    chrono::irrlicht::ChVisualSystemIrrlicht vis;
+
+    vis.AttachSystem(&system);
+    vis.SetWindowSize(1280,720);
+    vis.SetWindowTitle("Example 12 Differential Drive Robot");
+    vis.Initialize();
+    vis.AddSkyBox();
+    vis.AddLogo();
+    vis.AddSkyBox();
+    vis.AddCamera({0, -2, 1}, {0, 0, 0});
+    vis.AddTypicalLights();
+    vis.BindAll();
+
+    real_t step_size = 0.002;
+    while (vis.Run()) {
+
+        vis.BeginScene();
+        vis.Render();
+        draw_world_axes(vis, 2.0);
+
+        auto position = chassis -> GetPos();
+        std::cout << position <<std::endl;
+
+        system.DoStepDynamics(step_size);
+        vis.EndScene();
     }
 
     return 0;
 }
+#else
+#include <iostream>
+int main()
+{
+    std::cerr<<"You need PROJECTCHRONO configured with "
+             <<"bitrl in order to run this example "
+             <<"Reconfigure bitrl and set ENABLE_CHRONO=ON"<<std::endl;
+    return 1;
+}
+
+#endif