1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Author: Radu Serban
13 // =============================================================================
14 //
15 // Demonstration of the moving patch option for the granular terrain system in
16 // Chrono::Vehicle.
17 //
18 // =============================================================================
19
20 #include "chrono/utils/ChUtilsCreators.h"
21
22 #include "chrono_vehicle/ChVehicleModelData.h"
23 #include "chrono_vehicle/terrain/GranularTerrain.h"
24
25 #include "chrono_multicore/physics/ChSystemMulticore.h"
26
27 #include "chrono_opengl/ChOpenGLWindow.h"
28
29 using namespace chrono;
30 using namespace chrono::collision;
31 using namespace chrono::vehicle;
32
main(int argc,char * argv[])33 int main(int argc, char* argv[]) {
34 GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
35
36 // Simulation parameters
37 double time_step = 5e-3; // Integration time step (s)
38 double time_pitch = 1; // Time when gravity is rotated (s)
39 double time_end = 10; // Final simulation time (s)
40
41 // Terrain parameters
42 ChVector<> center(0, 0, 0); // Center of initial patch
43 double hdimX = 1.5; // Length of patch
44 double hdimY = 0.75; // Width of patch
45 unsigned int num_layers = 6; // Requested number of layers
46 bool rough = false; // Fixed base layer?
47 bool moving_patch = true; // Enable moving patch feature?
48 double buffer_dist = 2.0; // Look-ahead distance (m)
49 double shift_dist = 0.4; // Patch shift distance (m)
50 double slope = 30; // Terrain slope (degrees)
51 double radius = 20; // Particle radius (mm)
52 double rho = 2000; // Granular material density (kg/m3)
53 double mu = 0.9; // Coefficient of friction
54 double coh = 20; // Cohesion pressure (kPa)
55
56 // Convert terrain parameters
57 double slope_g = slope * CH_C_DEG_TO_RAD; // Slope (rad)
58 double r_g = radius / 1000; // Particle radius (m)
59 double rho_g = rho; // Granular material density (kg/m3)
60 double mu_g = mu; // Coefficient of friction
61 double area = CH_C_PI * r_g * r_g; // Particle cross-area (m2)
62 double coh_force = area * (coh * 1e3); // Cohesion force (N)
63 double coh_g = coh_force * time_step; // Cohesion impulse (Ns)
64
65 // Tracked body parameters
66 double kmh_to_ms = 1000.0 / 3600;
67 double body_rad = 0.2; // Radius (m)
68 double body_speed = 50 * kmh_to_ms; // Forward speed (m/s)
69
70 // Collision envelope (10% of particle radius)
71 double envelope = 0.1 * r_g;
72
73 // Camera location
74 enum CameraType { FIXED, FRONT, TRACK };
75 CameraType cam_type = FRONT;
76
77 // ----------------------------------
78 // Create the multicore Chrono system
79 // ----------------------------------
80
81 // Prepare rotated acceleration vector
82 ChVector<> gravity(0, 0, -9.81);
83 ChVector<> gravityR = ChMatrix33<>(slope_g, ChVector<>(0, 1, 0)) * gravity;
84
85 ChSystemMulticoreNSC* system = new ChSystemMulticoreNSC();
86 system->Set_G_acc(gravity);
87
88 // Set number of threads
89 system->SetNumThreads(4);
90
91 // Edit system settings
92 system->GetSettings()->solver.tolerance = 1e-3;
93 system->GetSettings()->solver.solver_mode = SolverMode::SLIDING;
94 system->GetSettings()->solver.max_iteration_normal = 0;
95 system->GetSettings()->solver.max_iteration_sliding = 50;
96 system->GetSettings()->solver.max_iteration_spinning = 0;
97 system->GetSettings()->solver.max_iteration_bilateral = 100;
98 system->GetSettings()->solver.compute_N = false;
99 system->GetSettings()->solver.alpha = 0;
100 system->GetSettings()->solver.cache_step_length = true;
101 system->GetSettings()->solver.use_full_inertia_tensor = false;
102 system->GetSettings()->solver.contact_recovery_speed = 1000;
103 system->GetSettings()->solver.bilateral_clamp_speed = 1e8;
104 system->ChangeSolverType(SolverType::BB);
105
106 system->GetSettings()->collision.collision_envelope = envelope;
107 system->GetSettings()->collision.narrowphase_algorithm = ChNarrowphase::Algorithm::HYBRID;
108 system->GetSettings()->collision.broadphase_grid = ChBroadphase::GridType::FIXED_RESOLUTION;
109 system->GetSettings()->collision.bins_per_axis = vec3(100, 30, 2);
110
111 // ------------------
112 // Create the terrain
113 // ------------------
114
115 GranularTerrain terrain(system);
116 auto mat = std::static_pointer_cast<ChMaterialSurfaceNSC>(terrain.GetContactMaterial());
117 mat->SetFriction((float)mu_g);
118 mat->SetCohesion((float)coh_g);
119 terrain.SetContactMaterial(mat);
120 terrain.SetCollisionEnvelope(envelope / 5);
121 if (rough) {
122 int nx = (int)std::round((2 * hdimX) / (4 * r_g));
123 int ny = (int)std::round((2 * hdimY) / (4 * r_g));
124 terrain.EnableRoughSurface(nx, ny);
125 }
126 terrain.EnableVisualization(true);
127 terrain.EnableVerbose(true);
128
129 terrain.Initialize(center, 2 * hdimX, 2 * hdimY, num_layers, r_g, rho_g, ChVector<>(0, 0, -2));
130 uint actual_num_particles = terrain.GetNumParticles();
131 double terrain_height = terrain.GetHeight(ChVector<>(0, 0, 0));
132
133 std::cout << "Number of particles: " << actual_num_particles << std::endl;
134 std::cout << "Terrain height: " << terrain_height << std::endl;
135
136 // ---------------
137 // Create the body
138 // ---------------
139
140 ChVector<> pos(terrain.GetPatchRear(), (terrain.GetPatchLeft() + terrain.GetPatchRight()) / 2,
141 terrain_height + 2 * body_rad);
142 auto body = std::shared_ptr<ChBody>(system->NewBody());
143 body->SetMass(1);
144 body->SetInertiaXX(ChVector<>(1, 1, 1));
145 body->SetPos_dt(ChVector<>(body_speed, 0, 0));
146 body->SetPos(pos);
147 system->AddBody(body);
148
149 auto body_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>();
150
151 body->GetCollisionModel()->ClearModel();
152 utils::AddSphereGeometry(body.get(), body_mat, body_rad, ChVector<>(0, 0, 0));
153 body->GetCollisionModel()->BuildModel();
154
155 auto joint = chrono_types::make_shared<ChLinkLockPrismatic>();
156 joint->Initialize(terrain.GetGroundBody(), body, ChCoordsys<>(pos, Q_from_AngY(CH_C_PI_2)));
157 system->AddLink(joint);
158
159 // Enable moving patch, based on body location
160 if (moving_patch)
161 terrain.EnableMovingPatch(body, buffer_dist, shift_dist, ChVector<>(0, 0, -2));
162
163 // -----------------
164 // Initialize OpenGL
165 // -----------------
166
167 opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
168 gl_window.Initialize(1280, 720, "Granular terrain demo", system);
169 gl_window.SetCamera(center - ChVector<>(0, 3, 0), center, ChVector<>(0, 0, 1), 0.05f);
170 gl_window.SetRenderMode(opengl::SOLID);
171
172 // ---------------
173 // Simulation loop
174 // ---------------
175
176 bool is_pitched = false;
177 double time = 0;
178
179 while (time < time_end) {
180 // Rotate gravity vector
181 if (!is_pitched && time > time_pitch) {
182 std::cout << time << " Pitch: " << gravityR.x() << " " << gravityR.y() << " " << gravityR.z()
183 << std::endl;
184 system->Set_G_acc(gravityR);
185 is_pitched = true;
186 }
187
188 terrain.Synchronize(time);
189 system->DoStepDynamics(time_step);
190
191 ////if (terrain.PatchMoved()) {
192 //// auto aabb_min = system->data_manager->measures.collision.rigid_min_bounding_point;
193 //// auto aabb_max = system->data_manager->measures.collision.rigid_max_bounding_point;
194 //// std::cout << " Global AABB: " << std::endl;
195 //// std::cout << " " << aabb_min.x << " " << aabb_min.y << " " << aabb_min.z << std::endl;
196 //// std::cout << " " << aabb_max.x << " " << aabb_max.y << " " << aabb_max.z << std::endl;
197 ////}
198
199 ////opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
200 if (gl_window.Active()) {
201 switch (cam_type) {
202 case FRONT: {
203 double body_x = body->GetPos().x();
204 ChVector<> cam_loc(body_x + buffer_dist, -4, 0);
205 ChVector<> cam_point(body_x + buffer_dist, 0, 0);
206 gl_window.SetCamera(cam_loc, cam_point, ChVector<>(0, 0, 1), 0.05f);
207 break;
208 }
209 case TRACK: {
210 ChVector<> cam_point = body->GetPos();
211 ChVector<> cam_loc = cam_point + ChVector<>(-3 * body_rad, -1, 0.6);
212 gl_window.SetCamera(cam_loc, cam_point, ChVector<>(0, 0, 1), 0.05f);
213 break;
214 }
215 default:
216 break;
217 }
218 gl_window.Render();
219 } else {
220 break;
221 }
222
223 time += time_step;
224 }
225
226 return 0;
227 }
228