First commit for diff-drive robot (#174)

CMakeLists.txt

@@ -163,6 +163,8 @@ FILE(GLOB SRCS src/bitrl/*.cpp
 		src/bitrl/planning/*.cpp
 		src/bitrl/rigid_bodies/*.cpp
 		src/bitrl/rigid_bodies/chrono_robots/*.cpp
+		src/bitrl/rigid_bodies/chrono_robots/impl/*.cpp
+		src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/*.cpp
 		src/bitrl/rigid_bodies/webots_robots/*.cpp
 		src/bitrl/dynamics/*.cpp
 		src/bitrl/utils/*.cpp

examples/example_13/example_13.cpp

@@ -10,14 +10,14 @@
 #include <boost/log/trivial.hpp>
 #endif
 
+#include "bitrl/bitrl_consts.h"
 #include "chrono/core/ChRealtimeStep.h"
 #include "chrono/physics/ChSystemNSC.h"
-#include "chrono/physics/ChLinkMotorRotationSpeed.h"
+#include <chrono/physics/ChBodyEasy.h>
 #include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
+#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h"
 
 #include <filesystem>
-#include <iostream>
-#include <random>
 #include <string>
 
 namespace example_13
@@ -45,59 +45,34 @@ void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual)
     visual.BindAll();
 }
 
+
+
+
 } // namespace example_13
 
 int main()
 {
-    using namespace example_13;
+   using namespace example_13;
     chrono::ChSystemNSC sys;
-    sys.SetGravityY();
-
-    // 2- Create the rigid bodies of the slider-crank mechanical system
-    //   (a crank, a rod, a truss), maybe setting position/mass/inertias of
-    //   their center of mass (COG) etc.
-
-    // ..the truss
-    auto my_body_A = chrono_types::make_shared<chrono::ChBody>();
-    sys.AddBody(my_body_A);
-    my_body_A->SetFixed(true);  // truss does not move!
-    my_body_A->SetName("Ground-Truss");
-
-    // ..the crank
-    auto my_body_B = chrono_types::make_shared<chrono::ChBody>();
-    sys.AddBody(my_body_B);
-    my_body_B->SetPos(chrono::ChVector3d(1, 0, 0));  // position of COG of crank
-    my_body_B->SetMass(2);
-    my_body_B->SetName("Crank");
-
-    // ..the rod
-    auto my_body_C = chrono_types::make_shared<chrono::ChBody>();
-    sys.AddBody(my_body_C);
-    my_body_C->SetPos(chrono::ChVector3d(4, 0, 0));  // position of COG of rod
-    my_body_C->SetMass(3);
-    my_body_C->SetName("Rod");
-
-    // 3- Create constraints: the mechanical joints between the rigid bodies.
-
-    // .. a revolute joint between crank and rod
-    auto my_link_BC = chrono_types::make_shared<chrono::ChLinkLockRevolute>();
-    my_link_BC->SetName("RevJointCrankRod");
-    my_link_BC->Initialize(my_body_B, my_body_C, chrono::ChFrame<>(chrono::ChVector3d(2, 0, 0)));
-    sys.AddLink(my_link_BC);
-
-    // .. a slider joint between rod and truss
-    auto my_link_CA = chrono_types::make_shared<chrono::ChLinkLockPointLine>();
-    my_link_CA->SetName("TransJointRodGround");
-    my_link_CA->Initialize(my_body_C, my_body_A, chrono::ChFrame<>(chrono::ChVector3d(6, 0, 0)));
-    sys.AddLink(my_link_CA);
-
-    // .. a motor between crank and truss
-    auto my_link_AB = chrono_types::make_shared<chrono::ChLinkMotorRotationSpeed>();
-    my_link_AB->Initialize(my_body_A, my_body_B, chrono::ChFrame<>(chrono::ChVector3d(0, 0, 0)));
-    my_link_AB->SetName("RotationalMotor");
-    sys.AddLink(my_link_AB);
-    auto my_speed_function = chrono_types::make_shared<chrono::ChFunctionConst>(chrono::CH_PI);  // speed w=3.145 rad/sec
-    my_link_AB->SetSpeedFunction(my_speed_function);
+    sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81));
+
+    sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
+    chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025);
+    chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025);
+
+    auto floor_mat = chrono_types::make_shared<chrono::ChContactMaterialNSC>();
+    auto mfloor = chrono_types::make_shared<chrono::ChBodyEasyBox>(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);
+
+    bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys,
+                                                 chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT);
+
+    robot.init();
+    robot.set_motor_speed(bitrl::consts::maths::PI, 0);
+    robot.set_motor_speed(bitrl::consts::maths::PI, 1);
 
     chrono::irrlicht::ChVisualSystemIrrlicht visual;
     prepare_visualization(visual);
@@ -123,35 +98,16 @@ int main()
         // .. draw GUI items belonging to Irrlicht screen, if any
         visual.GetGUIEnvironment()->drawAll();
 
-        // .. draw the rod (from joint BC to joint CA)
-        tools::drawSegment(&visual, my_link_BC->GetMarker1()->GetAbsCoordsys().pos,
-                           my_link_CA->GetMarker1()->GetAbsCoordsys().pos,
-                           chrono::ChColor(0, 1, 0));
-        // .. draw the crank (from joint AB to joint BC)
-        tools::drawSegment(&visual, my_link_AB->GetFrame2Abs().GetCoordsys().pos,
-                           my_link_BC->GetMarker1()->GetAbsCoordsys().pos,
-                           chrono::ChColor(1, 0, 0));
-
-        // .. draw a small circle at crank origin
-        tools::drawCircle(&visual, 0.1, chrono::ChCoordsys<>(chrono::ChVector3d(0, 0, 0), chrono::QUNIT));
-
-        /* test: delete a link after 10 seconds
-        if (sys.GetChTime() >10 && (!removed))
-        {
-                sys.RemoveLink(my_link_AB);
-                removed = true;
-        }*/
-
         // ADVANCE SYSTEM STATE BY ONE STEP
         sys.DoStepDynamics(DT);
+
         // Enforce soft real-time
         realtime_timer.Spin(DT);
 
         // Irrlicht must finish drawing the frame
         visual.EndScene();
     }
 
-
     return 0;
 }
 #else

examples/example_13/example_13.md

@@ -10,9 +10,132 @@ A _ChSystem_ is an abstract class. The Chrono library provides the following sub
 - _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 create and simulate a differential drive system using Chrono
+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 <a href="https://api.projectchrono.org/group__robot__models__turtlebot.html">Turtlebot robot model</a> 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.
 
 @code{.cpp}
+#include "bitrl/bitrl_config.h"
+
+#ifdef BITRL_CHRONO
+
+#include "bitrl/bitrl_types.h"
+
+
+#ifdef BITRL_LOG
+#define BOOST_LOG_DYN_LINK
+#include <boost/log/trivial.hpp>
+#endif
+
+#include "chrono/core/ChRealtimeStep.h"
+#include "chrono/physics/ChSystemNSC.h"
+#include <chrono/physics/ChBodyEasy.h>
+#include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
+#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h"
+
+#include <filesystem>
+#include <string>
+
+namespace example_13
+{
+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.01;
+const real_t SIM_TIME = 5.0;
+const std::string WINDOW_TITLE( "Example 13");
+
+void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual)
+{
+visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT);
+visual.SetWindowTitle(WINDOW_TITLE);
+visual.Initialize();
+
+    visual.AddLogo();
+    visual.AddSkyBox();
+    visual.AddCamera({0, -2, 1}, {0, 0, 0});
+    visual.AddTypicalLights();
+    visual.BindAll();
+}
+
+} // namespace example_13
+
+int main()
+{
+using namespace example_13;
+chrono::ChSystemNSC sys;
+sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81));
+
+    sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
+    chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025);
+    chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025);
+
+    auto floor_mat = chrono_types::make_shared<chrono::ChContactMaterialNSC>();
+    auto mfloor = chrono_types::make_shared<chrono::ChBodyEasyBox>(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);
+
+    bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys,
+                                                 chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT);
+
+    robot.init();
+    robot.set_motor_speed(bitrl::consts::maths::PI, 0);
+    robot.set_motor_speed(bitrl::consts::maths::PI, 1);
+
+    chrono::irrlicht::ChVisualSystemIrrlicht visual;
+    prepare_visualization(visual);
+    visual.AttachSystem(&sys);
+
+    // Simulation loop
+
+    // Timer for enforcing soft real-time
+    chrono::ChRealtimeStepTimer realtime_timer;
+
+    // bool removed = false;
+    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();
+
+        // ADVANCE SYSTEM STATE BY ONE STEP
+        sys.DoStepDynamics(DT);
+
+        // Enforce soft real-time
+        realtime_timer.Spin(DT);
+
+        // Irrlicht must finish drawing the frame
+        visual.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
 @endcode