Skip to content

Commit c82e9e4

Browse files
pockermanpockerman
committed
Fix example 12 (#176)
1 parent 6d78557 commit c82e9e4

4 files changed

Lines changed: 471 additions & 73 deletions

File tree

‎examples/example_12/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
88
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log)
99

1010
IF(ENABLE_CHRONO)
11-
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core)
11+
TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core Chrono::Chrono_irrlicht)
1212
ENDIF ()
1313

1414

‎examples/example_12/example_12.cpp‎

Lines changed: 208 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -1,97 +1,235 @@
1-
/**
2-
* This example utilises the ```TensorboardServer``` class to log values of interest when running
3-
* an experiment. We can monitor the experimet using <a
4-
*href="https://www.tensorflow.org/tensorboard">tensorboard</a>. The ```TensorboardServer``` class
5-
*is a simple wrapper that exposes three functions
6-
*
7-
* - ```add_scalar```
8-
* - ```add_scalars```
9-
* - ```add_text```
10-
*
11-
* We will use ```add_scalar``` and ```add_text```. In order to run this example, fire up the
12-
*server using the ```torchboard_server/start_uvicorn.sh```. The server listens at port 8002. You
13-
*can change this however you want just make sure that the port is not used and also update the
14-
* variable ```TORCH_SERVER_HOST``` in the code below accordingly. Note that the implementation
15-
*uses <a
16-
*href="https://pytorch.org/docs/stable/_modules/torch/utils/tensorboard/writer.html#SummaryWriter">SummaryWriter</a>
17-
*class. Thus you will need to have PyTorch installed on the machine that you run the server.
18-
*/
1+
#include "bitrl/bitrl_config.h"
2+
3+
#ifdef BITRL_CHRONO
194

205
#include "bitrl/bitrl_types.h"
21-
#include "bitrl/utils/io/json_file_reader.h"
22-
#include "bitrl/network/tensorboard_server.h"
6+
#include "bitrl/bitrl_consts.h"
237

24-
#include <filesystem>
25-
#include <iostream>
26-
#include <random>
27-
#include <string>
8+
#ifdef BITRL_LOG
9+
#define BOOST_LOG_DYN_LINK
10+
#include <boost/log/trivial.hpp>
11+
#endif
12+
13+
#include <chrono/physics/ChSystemNSC.h>
14+
#include <chrono/physics/ChBodyEasy.h>
15+
#include "chrono/assets/ChVisualSystem.h"
16+
#include <chrono_irrlicht/ChVisualSystemIrrlicht.h>
17+
18+
#include <chrono/physics/ChLinkMotorRotationSpeed.h>
19+
#include <chrono/functions/ChFunctionConst.h>
20+
21+
#include <memory>
2822

2923
namespace example_12
3024
{
25+
26+
using namespace chrono;
27+
using namespace chrono::irrlicht;
3128
using namespace bitrl;
32-
using utils::io::JSONFileReader;
33-
using network::TensorboardServer;
3429

35-
namespace fs = std::filesystem;
36-
const std::string CONFIG = "config.json";
30+
const real_t WHEEL_RADIUS = 0.1;
31+
const real_t CHASSIS_HEIGHT = 0.1;
32+
const real_t WHEEL_WIDTH = 0.05;
33+
const real_t CASTER_RADIUS = 0.05;
34+
35+
void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis,
36+
real_t scale = 1.0) {
37+
auto* driver = vis.GetVideoDriver();
38+
39+
// X axis (red)
40+
driver->draw3DLine(
41+
{0, 0, 0},
42+
{static_cast<float>(scale), 0, 0},
43+
irr::video::SColor(255, 255, 0, 0));
44+
45+
// Y axis (green)
46+
driver->draw3DLine(
47+
{0, 0, 0},
48+
{0, static_cast<float>(scale), 0},
49+
irr::video::SColor(255, 0, 255, 0));
50+
51+
// Z axis (blue)
52+
driver->draw3DLine(
53+
{0, 0, 0},
54+
{0, 0, static_cast<float>(scale)},
55+
irr::video::SColor(255, 0, 0, 255));
56+
}
57+
58+
auto build_generic_material()
59+
{
60+
auto material = chrono_types::make_shared<ChContactMaterialNSC>();
61+
material->SetFriction(0.8f);
62+
material->SetRestitution(0.1f);
63+
return material;
64+
}
3765

38-
} // namespace example_12
66+
auto build_floor(std::shared_ptr<ChContactMaterialNSC> material)
67+
{
68+
auto floor = chrono_types::make_shared<chrono::ChBodyEasyBox>(
69+
5, 5, 0.1, // size
70+
1000, // density
71+
true, true, material); // visual + collision
72+
73+
floor->SetPos(chrono::ChVector3d(0,0,-0.05));
74+
floor->SetFixed(true);
75+
floor->EnableCollision(true);
76+
return floor;
77+
}
3978

40-
int main()
79+
auto build_chassis(std::shared_ptr<ChContactMaterialNSC> material)
4180
{
81+
auto chassis = chrono_types::make_shared<ChBodyEasyBox>(
82+
0.5, 0.3, CHASSIS_HEIGHT,
83+
1000,
84+
true, true, material);
85+
86+
chrono::ChVector3d pos(0.0, 0.0, WHEEL_RADIUS + CHASSIS_HEIGHT/2.0 + 0.01);
87+
chassis->SetPos(pos);
88+
89+
chrono::ChVector3d vel(0.5, 0.0, 0.0);
90+
chassis->SetPosDt(vel);
91+
chassis->EnableCollision(true);
92+
return chassis;
93+
}
4294

43-
using namespace example_12;
95+
auto build_wheel(std::shared_ptr<ChContactMaterialNSC> material, const chrono::ChVector3d& pos)
96+
{
97+
auto wheel = chrono_types::make_shared<chrono::ChBodyEasyCylinder>(
98+
chrono::ChAxis::Y,
99+
WHEEL_RADIUS,
100+
WHEEL_WIDTH,
101+
1000,
102+
true, true, material);
103+
104+
wheel->SetPos(pos);
105+
wheel->EnableCollision(true);
106+
return wheel;
107+
}
44108

45-
try
46-
{
109+
auto build_motor(std::shared_ptr<ChBodyEasyCylinder> wheel,
110+
std::shared_ptr<ChBodyEasyBox> chassis,
111+
std::shared_ptr<ChFunctionConst> speed_func,
112+
const chrono::ChVector3d& pos)
113+
{
114+
auto motor = chrono_types::make_shared<chrono::ChLinkMotorRotationSpeed>();
47115

48-
// load the json configuration
49-
JSONFileReader json_reader(CONFIG);
50-
json_reader.open();
116+
chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2);
117+
motor->Initialize(
118+
wheel,
119+
chassis,
120+
chrono::ChFrame<>(pos, rot)
121+
);
51122

52-
auto experiment_dict = json_reader.template get_value<std::string>("experiment_dict");
53-
auto experiment_id = json_reader.template get_value<std::string>("experiment_id");
123+
motor->SetSpeedFunction(speed_func);
124+
return motor;
125+
}
54126

55-
std::cout << "Experiment directory: " << experiment_dict << std::endl;
56-
std::cout << "Experiment id: " << experiment_id << std::endl << std::endl;
127+
auto build_caster_wheel(std::shared_ptr<ChContactMaterialNSC> material)
128+
{
57129

58-
const fs::path EXPERIMENT_DIR_PATH = experiment_dict + experiment_id;
130+
auto caster = chrono_types::make_shared<ChBodyEasySphere>(
131+
CASTER_RADIUS, // radius
132+
1000, // density
133+
true, // visualization
134+
true, // collision
135+
material);
59136

60-
// the first thing we want to do when monitoring experiments
61-
// is to create a directory where all data will reside
62-
std::filesystem::create_directories(experiment_dict + experiment_id);
137+
caster->SetPos(chrono::ChVector3d(0.2, 0, 0.05));
138+
caster->EnableCollision(true);
139+
return caster;
140+
}
63141

64-
const auto input_size = json_reader.template get_value<uint_t>("input_size");
65-
const auto output_size = json_reader.template get_value<uint_t>("output_size");
66-
const auto num_epochs = json_reader.template get_value<uint_t>("num_epochs");
67-
const auto learning_rate = json_reader.template get_value<real_t>("lr");
68-
auto log_path = json_reader.template get_value<std::string>("log_path");
142+
auto build_caster_joint(std::shared_ptr<ChBodyEasySphere> caster,
143+
std::shared_ptr<ChBodyEasyBox> chassis)
144+
{
145+
auto caster_joint = chrono_types::make_shared<ChLinkLockRevolute>();
69146

70-
// log the hyperparameters
71-
std::cout << "Input size: " << input_size << std::endl;
72-
std::cout << "Output size: " << output_size << std::endl;
73-
std::cout << "Max epochs: " << num_epochs << std::endl;
74-
std::cout << "Learning rate: " << learning_rate << std::endl;
147+
caster_joint->Initialize(
148+
caster,
149+
chassis,
150+
ChFrame<>(chrono::ChVector3d(-0.25, 0, CASTER_RADIUS), QUNIT));
151+
return caster_joint;
75152

76-
TensorboardServer logger("http://0.0.0.0:8002");
153+
}
154+
}
77155

78-
std::cout << "Logging results at " << logger.get_log_dir_path() << std::endl;
79-
logger.init(log_path);
156+
int main() {
80157

81-
logger.add_scalar("lr", learning_rate);
82-
logger.add_scalar("seed", 42);
83-
logger.add_scalar("num_epochs", num_epochs);
84-
logger.add_text("optimizer", "torch::optim::SGD");
85-
logger.close();
86-
}
87-
catch (std::exception &e)
88-
{
89-
std::cout << e.what() << std::endl;
90-
}
91-
catch (...)
92-
{
93-
std::cout << "Unknown exception occured" << std::endl;
158+
using namespace example_12;
159+
ChSystemNSC system;
160+
system.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G));
161+
system.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET);
162+
163+
// create a generic material to use
164+
auto material = example_12::build_generic_material();
165+
166+
// build floor
167+
auto floor = example_12::build_floor(material);
168+
system.Add(floor);
169+
170+
// build chassis
171+
auto chassis = example_12::build_chassis(material);
172+
system.Add(chassis);
173+
174+
// build wheels
175+
auto left_wheel = example_12::build_wheel(material,chrono::ChVector3d(0, 0.2, 0.1));
176+
auto right_wheel = example_12::build_wheel(material,chrono::ChVector3d(0,-0.2, 0.1));
177+
system.Add(left_wheel);
178+
system.Add(right_wheel);
179+
180+
// build motor
181+
auto motor_left = example_12::build_motor(left_wheel, chassis,
182+
chrono_types::make_shared<chrono::ChFunctionConst>(-4.5), chrono::ChVector3d(0,0.2,0.1));
183+
auto motor_right = build_motor(right_wheel, chassis,
184+
chrono_types::make_shared<chrono::ChFunctionConst>(-4.5), chrono::ChVector3d(0,-0.2,0.1));
185+
186+
system.Add(motor_left);
187+
system.Add(motor_right);
188+
189+
auto caster = example_12::build_caster_wheel(material);
190+
system.Add(caster);
191+
192+
auto caster_joint = example_12::build_caster_joint(caster, chassis);
193+
system.Add(caster_joint);
194+
195+
// create the visualization system
196+
chrono::irrlicht::ChVisualSystemIrrlicht vis;
197+
198+
vis.AttachSystem(&system);
199+
vis.SetWindowSize(1280,720);
200+
vis.SetWindowTitle("Example 12 Differential Drive Robot");
201+
vis.Initialize();
202+
vis.AddSkyBox();
203+
vis.AddLogo();
204+
vis.AddSkyBox();
205+
vis.AddCamera({0, -2, 1}, {0, 0, 0});
206+
vis.AddTypicalLights();
207+
vis.BindAll();
208+
209+
real_t step_size = 0.002;
210+
while (vis.Run()) {
211+
212+
vis.BeginScene();
213+
vis.Render();
214+
draw_world_axes(vis, 2.0);
215+
216+
auto position = chassis -> GetPos();
217+
std::cout << position <<std::endl;
218+
219+
system.DoStepDynamics(step_size);
220+
vis.EndScene();
94221
}
95222

96223
return 0;
97224
}
225+
#else
226+
#include <iostream>
227+
int main()
228+
{
229+
std::cerr<<"You need PROJECTCHRONO configured with "
230+
<<"bitrl in order to run this example "
231+
<<"Reconfigure bitrl and set ENABLE_CHRONO=ON"<<std::endl;
232+
return 1;
233+
}
234+
235+
#endif

0 commit comments

Comments
 (0)