Skip to content

Commit f7f1c0d

Browse files
pockermanpockerman
authored
Merge pull request #175 from pockerman/feat/174-add-diff-drive-robot-model
First commit for diff-drive robot (#174)
2 parents 9769226 + f253e8d commit f7f1c0d

19 files changed

+2074
-72
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,8 @@ FILE(GLOB SRCS src/bitrl/*.cpp
163163
src/bitrl/planning/*.cpp
164164
src/bitrl/rigid_bodies/*.cpp
165165
src/bitrl/rigid_bodies/chrono_robots/*.cpp
166+
src/bitrl/rigid_bodies/chrono_robots/impl/*.cpp
167+
src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/*.cpp
166168
src/bitrl/rigid_bodies/webots_robots/*.cpp
167169
src/bitrl/dynamics/*.cpp
168170
src/bitrl/utils/*.cpp

‎examples/example_13/example_13.cpp‎

Lines changed: 27 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -10,14 +10,14 @@
1010
#include <boost/log/trivial.hpp>
1111
#endif
1212

13+
#include "bitrl/bitrl_consts.h"
1314
#include "chrono/core/ChRealtimeStep.h"
1415
#include "chrono/physics/ChSystemNSC.h"
15-
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
16+
#include <chrono/physics/ChBodyEasy.h>
1617
#include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
18+
#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h"
1719

1820
#include <filesystem>
19-
#include <iostream>
20-
#include <random>
2121
#include <string>
2222

2323
namespace example_13
@@ -45,59 +45,34 @@ void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual)
4545
visual.BindAll();
4646
}
4747

48+
49+
50+
4851
} // namespace example_13
4952

5053
int main()
5154
{
52-
using namespace example_13;
55+
using namespace example_13;
5356
chrono::ChSystemNSC sys;
54-
sys.SetGravityY();
55-
56-
// 2- Create the rigid bodies of the slider-crank mechanical system
57-
// (a crank, a rod, a truss), maybe setting position/mass/inertias of
58-
// their center of mass (COG) etc.
59-
60-
// ..the truss
61-
auto my_body_A = chrono_types::make_shared<chrono::ChBody>();
62-
sys.AddBody(my_body_A);
63-
my_body_A->SetFixed(true); // truss does not move!
64-
my_body_A->SetName("Ground-Truss");
65-
66-
// ..the crank
67-
auto my_body_B = chrono_types::make_shared<chrono::ChBody>();
68-
sys.AddBody(my_body_B);
69-
my_body_B->SetPos(chrono::ChVector3d(1, 0, 0)); // position of COG of crank
70-
my_body_B->SetMass(2);
71-
my_body_B->SetName("Crank");
72-
73-
// ..the rod
74-
auto my_body_C = chrono_types::make_shared<chrono::ChBody>();
75-
sys.AddBody(my_body_C);
76-
my_body_C->SetPos(chrono::ChVector3d(4, 0, 0)); // position of COG of rod
77-
my_body_C->SetMass(3);
78-
my_body_C->SetName("Rod");
79-
80-
// 3- Create constraints: the mechanical joints between the rigid bodies.
81-
82-
// .. a revolute joint between crank and rod
83-
auto my_link_BC = chrono_types::make_shared<chrono::ChLinkLockRevolute>();
84-
my_link_BC->SetName("RevJointCrankRod");
85-
my_link_BC->Initialize(my_body_B, my_body_C, chrono::ChFrame<>(chrono::ChVector3d(2, 0, 0)));
86-
sys.AddLink(my_link_BC);
87-
88-
// .. a slider joint between rod and truss
89-
auto my_link_CA = chrono_types::make_shared<chrono::ChLinkLockPointLine>();
90-
my_link_CA->SetName("TransJointRodGround");
91-
my_link_CA->Initialize(my_body_C, my_body_A, chrono::ChFrame<>(chrono::ChVector3d(6, 0, 0)));
92-
sys.AddLink(my_link_CA);
93-
94-
// .. a motor between crank and truss
95-
auto my_link_AB = chrono_types::make_shared<chrono::ChLinkMotorRotationSpeed>();
96-
my_link_AB->Initialize(my_body_A, my_body_B, chrono::ChFrame<>(chrono::ChVector3d(0, 0, 0)));
97-
my_link_AB->SetName("RotationalMotor");
98-
sys.AddLink(my_link_AB);
99-
auto my_speed_function = chrono_types::make_shared<chrono::ChFunctionConst>(chrono::CH_PI); // speed w=3.145 rad/sec
100-
my_link_AB->SetSpeedFunction(my_speed_function);
57+
sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81));
58+
59+
sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
60+
chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025);
61+
chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025);
62+
63+
auto floor_mat = chrono_types::make_shared<chrono::ChContactMaterialNSC>();
64+
auto mfloor = chrono_types::make_shared<chrono::ChBodyEasyBox>(20, 20, 1, 1000, true, true, floor_mat);
65+
mfloor->SetPos(chrono::ChVector3d(0, 0, -1));
66+
mfloor->SetFixed(true);
67+
mfloor->GetVisualShape(0)->SetTexture(chrono::GetChronoDataFile("textures/concrete.jpg"));
68+
sys.Add(mfloor);
69+
70+
bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys,
71+
chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT);
72+
73+
robot.init();
74+
robot.set_motor_speed(bitrl::consts::maths::PI, 0);
75+
robot.set_motor_speed(bitrl::consts::maths::PI, 1);
10176

10277
chrono::irrlicht::ChVisualSystemIrrlicht visual;
10378
prepare_visualization(visual);
@@ -123,35 +98,16 @@ int main()
12398
// .. draw GUI items belonging to Irrlicht screen, if any
12499
visual.GetGUIEnvironment()->drawAll();
125100

126-
// .. draw the rod (from joint BC to joint CA)
127-
tools::drawSegment(&visual, my_link_BC->GetMarker1()->GetAbsCoordsys().pos,
128-
my_link_CA->GetMarker1()->GetAbsCoordsys().pos,
129-
chrono::ChColor(0, 1, 0));
130-
// .. draw the crank (from joint AB to joint BC)
131-
tools::drawSegment(&visual, my_link_AB->GetFrame2Abs().GetCoordsys().pos,
132-
my_link_BC->GetMarker1()->GetAbsCoordsys().pos,
133-
chrono::ChColor(1, 0, 0));
134-
135-
// .. draw a small circle at crank origin
136-
tools::drawCircle(&visual, 0.1, chrono::ChCoordsys<>(chrono::ChVector3d(0, 0, 0), chrono::QUNIT));
137-
138-
/* test: delete a link after 10 seconds
139-
if (sys.GetChTime() >10 && (!removed))
140-
{
141-
sys.RemoveLink(my_link_AB);
142-
removed = true;
143-
}*/
144-
145101
// ADVANCE SYSTEM STATE BY ONE STEP
146102
sys.DoStepDynamics(DT);
103+
147104
// Enforce soft real-time
148105
realtime_timer.Spin(DT);
149106

150107
// Irrlicht must finish drawing the frame
151108
visual.EndScene();
152109
}
153110

154-
155111
return 0;
156112
}
157113
#else

‎examples/example_13/example_13.md‎

Lines changed: 124 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,132 @@ A _ChSystem_ is an abstract class. The Chrono library provides the following sub
1010
- _ChSystemSMC_ for SMooth Contacts (SMC): contacts are handled using penalty methods, i.e. contacts are deformable
1111

1212
Note that if there are no contacts or collisions in your system, it is indifferent to use _ChSystemNSC_ or _ChSystemSMC_.
13-
In this example we will create and simulate a differential drive system using Chrono
13+
In this example we will use the \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot" class
14+
to simulate a differential drive system using Chrono. The \ref bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot "bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot"
15+
follows the <a href="https://api.projectchrono.org/group__robot__models__turtlebot.html">Turtlebot robot model</a> defined in Chrono.
16+
You will need the mesh files from the Chrono project in order to visualize the robot. These files should be place in the
17+
\ref ROBOTS_DATA_DIR directory under the _diff_drive_robot_ subdirectory of the project. Below is the driver code.
1418

1519
@code{.cpp}
20+
#include "bitrl/bitrl_config.h"
21+
22+
#ifdef BITRL_CHRONO
23+
24+
#include "bitrl/bitrl_types.h"
25+
26+
27+
#ifdef BITRL_LOG
28+
#define BOOST_LOG_DYN_LINK
29+
#include <boost/log/trivial.hpp>
30+
#endif
31+
32+
#include "chrono/core/ChRealtimeStep.h"
33+
#include "chrono/physics/ChSystemNSC.h"
34+
#include <chrono/physics/ChBodyEasy.h>
35+
#include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
36+
#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h"
37+
38+
#include <filesystem>
39+
#include <string>
40+
41+
namespace example_13
42+
{
43+
using namespace bitrl;
44+
using namespace chrono::irrlicht;
45+
46+
// constants we will be using further below
47+
const uint_t WINDOW_HEIGHT = 800;
48+
const uint_t WINDOW_WIDTH = 1024;
49+
const real_t DT = 0.01;
50+
const real_t SIM_TIME = 5.0;
51+
const std::string WINDOW_TITLE( "Example 13");
52+
53+
void prepare_visualization(chrono::irrlicht::ChVisualSystemIrrlicht& visual)
54+
{
55+
visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT);
56+
visual.SetWindowTitle(WINDOW_TITLE);
57+
visual.Initialize();
58+
59+
visual.AddLogo();
60+
visual.AddSkyBox();
61+
visual.AddCamera({0, -2, 1}, {0, 0, 0});
62+
visual.AddTypicalLights();
63+
visual.BindAll();
64+
}
65+
66+
} // namespace example_13
67+
68+
int main()
69+
{
70+
using namespace example_13;
71+
chrono::ChSystemNSC sys;
72+
sys.SetGravitationalAcceleration(chrono::ChVector3d(0, 0, -9.81));
73+
74+
sys.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
75+
chrono::ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025);
76+
chrono::ChCollisionModel::SetDefaultSuggestedMargin(0.0025);
77+
78+
auto floor_mat = chrono_types::make_shared<chrono::ChContactMaterialNSC>();
79+
auto mfloor = chrono_types::make_shared<chrono::ChBodyEasyBox>(20, 20, 1, 1000, true, true, floor_mat);
80+
mfloor->SetPos(chrono::ChVector3d(0, 0, -1));
81+
mfloor->SetFixed(true);
82+
mfloor->GetVisualShape(0)->SetTexture(chrono::GetChronoDataFile("textures/concrete.jpg"));
83+
sys.Add(mfloor);
84+
85+
bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot robot(sys,
86+
chrono::ChVector3d(0, 0, -0.45), chrono::QUNIT);
87+
88+
robot.init();
89+
robot.set_motor_speed(bitrl::consts::maths::PI, 0);
90+
robot.set_motor_speed(bitrl::consts::maths::PI, 1);
91+
92+
chrono::irrlicht::ChVisualSystemIrrlicht visual;
93+
prepare_visualization(visual);
94+
visual.AttachSystem(&sys);
95+
96+
// Simulation loop
97+
98+
// Timer for enforcing soft real-time
99+
chrono::ChRealtimeStepTimer realtime_timer;
100+
101+
// bool removed = false;
102+
while (visual.Run()) {
103+
// Irrlicht must prepare frame to draw
104+
visual.BeginScene();
105+
106+
// Irrlicht now draws simple lines in 3D world representing a
107+
// skeleton of the mechanism, in this instant:
108+
//
109+
// .. draw items belonging to Irrlicht scene, if any
110+
visual.Render();
111+
// .. draw a grid
112+
tools::drawGrid(&visual, 0.5, 0.5);
113+
// .. draw GUI items belonging to Irrlicht screen, if any
114+
visual.GetGUIEnvironment()->drawAll();
115+
116+
// ADVANCE SYSTEM STATE BY ONE STEP
117+
sys.DoStepDynamics(DT);
118+
119+
// Enforce soft real-time
120+
realtime_timer.Spin(DT);
121+
122+
// Irrlicht must finish drawing the frame
123+
visual.EndScene();
124+
}
125+
126+
return 0;
127+
}
128+
#else
129+
#include <iostream>
130+
int main()
131+
{
132+
std::cerr<<"You need PROJECTCHRONO configured with "
133+
<<"bitrl in order to run this example "
134+
<<"Reconfigure bitrl and set ENABLE_CHRONO=ON"<<std::endl;
135+
return 1;
136+
}
137+
138+
#endif
16139
@endcode
17140

18141

0 commit comments

Comments
 (0)