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 granular terrain system in Chrono::Vehicle.
16 //
17 // =============================================================================
18
19 #include "chrono/physics/ChLinkMotorRotationAngle.h"
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 = 8; // Requested number of layers
46 bool rough = false; // Fixed base layer?
47 bool moving_patch = true; // Enable moving patch feature?
48 double slope = 0; // Terrain slope (degrees)
49 double radius = 20; // Particle radius (mm)
50 double rho = 2000; // Granular material density (kg/m3)
51 double mu = 0.9; // Coefficient of friction
52 double coh = 20; // Cohesion pressure (kPa)
53
54 // Convert terrain parameters
55 double slope_g = slope * CH_C_DEG_TO_RAD; // Slope (rad)
56 double r_g = radius / 1000; // Particle radius (m)
57 double rho_g = rho; // Granular material density (kg/m3)
58 double mu_g = mu; // Coefficient of friction
59 double area = CH_C_PI * r_g * r_g; // Particle cross-area (m2)
60 double coh_force = area * (coh * 1e3); // Cohesion force (N)
61 double coh_g = coh_force * time_step; // Cohesion impulse (Ns)
62
63 // Tire parameters
64 double tire_rad = 0.8; // Radius (m)
65 double tire_ang_vel = CH_C_2PI; // Tire angular velocity (rad/s)
66
67 // Collision envelope (10% of particle radius)
68 double envelope = 0.1 * r_g;
69
70 // Camera location
71 enum CameraType { FIXED, FRONT, TRACK };
72 CameraType cam_type = FIXED;
73
74 // ----------------------------------
75 // Create the multicore Chrono system
76 // ----------------------------------
77
78 // Prepare rotated acceleration vector
79 ChVector<> gravity(0, 0, -9.81);
80 ChVector<> gravityR = ChMatrix33<>(slope_g, ChVector<>(0, 1, 0)) * gravity;
81
82 ChSystemMulticoreNSC* system = new ChSystemMulticoreNSC();
83 system->Set_G_acc(gravity);
84
85 // Set number of threads
86 system->SetNumThreads(4);
87
88 // Edit system settings
89 system->GetSettings()->solver.tolerance = 1e-3;
90 system->GetSettings()->solver.solver_mode = SolverMode::SLIDING;
91 system->GetSettings()->solver.max_iteration_normal = 0;
92 system->GetSettings()->solver.max_iteration_sliding = 50;
93 system->GetSettings()->solver.max_iteration_spinning = 0;
94 system->GetSettings()->solver.max_iteration_bilateral = 100;
95 system->GetSettings()->solver.compute_N = false;
96 system->GetSettings()->solver.alpha = 0;
97 system->GetSettings()->solver.cache_step_length = true;
98 system->GetSettings()->solver.use_full_inertia_tensor = false;
99 system->GetSettings()->solver.contact_recovery_speed = 1000;
100 system->GetSettings()->solver.bilateral_clamp_speed = 1e8;
101 system->ChangeSolverType(SolverType::BB);
102
103 system->GetSettings()->collision.collision_envelope = envelope;
104 system->GetSettings()->collision.narrowphase_algorithm = ChNarrowphase::Algorithm::HYBRID;
105 system->GetSettings()->collision.broadphase_grid = ChBroadphase::GridType::FIXED_RESOLUTION;
106 system->GetSettings()->collision.bins_per_axis = vec3(100, 30, 2);
107
108 // ------------------
109 // Create the terrain
110 // ------------------
111
112 GranularTerrain terrain(system);
113 auto mat = std::static_pointer_cast<ChMaterialSurfaceNSC>(terrain.GetContactMaterial());
114 mat->SetFriction((float)mu_g);
115 mat->SetCohesion((float)coh_g);
116 terrain.SetContactMaterial(mat);
117 terrain.SetCollisionEnvelope(envelope / 5);
118 if (rough) {
119 int nx = (int)std::round((2 * hdimX) / (4 * r_g));
120 int ny = (int)std::round((2 * hdimY) / (4 * r_g));
121 terrain.EnableRoughSurface(nx, ny);
122 }
123 terrain.EnableVisualization(true);
124 terrain.EnableVerbose(true);
125
126 terrain.Initialize(center, 2 * hdimX, 2 * hdimY, num_layers, r_g, rho_g);
127 uint actual_num_particles = terrain.GetNumParticles();
128 double terrain_height = terrain.GetHeight(ChVector<>(0, 0, 0));
129
130 std::cout << "Number of particles: " << actual_num_particles << std::endl;
131 std::cout << "Terrain height: " << terrain_height << std::endl;
132
133 // ---------------
134 // Create the tire
135 // ---------------
136
137 ChVector<> tire_center(terrain.GetPatchRear() + tire_rad, (terrain.GetPatchLeft() + terrain.GetPatchRight()) / 2,
138 terrain_height + 1.01 * tire_rad);
139 auto body = std::shared_ptr<ChBody>(system->NewBody());
140 body->SetMass(500);
141 body->SetInertiaXX(ChVector<>(20, 20, 20));
142 body->SetPos(tire_center);
143 body->SetRot(Q_from_AngZ(CH_C_PI_2));
144 system->AddBody(body);
145
146 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
147 trimesh->LoadWavefrontMesh(GetChronoDataFile("models/tractor_wheel/tractor_wheel.obj"));
148
149 auto trimesh_shape = chrono_types::make_shared<ChTriangleMeshShape>();
150 trimesh_shape->SetMesh(trimesh);
151 body->AddAsset(trimesh_shape);
152
153 auto body_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>();
154
155 body->GetCollisionModel()->ClearModel();
156 body->GetCollisionModel()->AddTriangleMesh(body_mat, trimesh, false, false, VNULL, ChMatrix33<>(1), 0.01);
157 ////utils::AddSphereGeometry(body.get(), body_mat, tire_rad, ChVector<>(0, 0, 0));
158 body->GetCollisionModel()->BuildModel();
159
160 body->SetCollide(true);
161
162 auto col = chrono_types::make_shared<ChColorAsset>();
163 col->SetColor(ChColor(0.3f, 0.3f, 0.3f));
164 body->AddAsset(col);
165
166 auto motor = chrono_types::make_shared<ChLinkMotorRotationAngle>();
167 motor->SetSpindleConstraint(ChLinkMotorRotation::SpindleConstraint::OLDHAM);
168 motor->SetAngleFunction(chrono_types::make_shared<ChFunction_Ramp>(0, -tire_ang_vel));
169 motor->Initialize(body, terrain.GetGroundBody(), ChFrame<>(tire_center, Q_from_AngX(CH_C_PI_2)));
170 system->Add(motor);
171
172 std::cout << "Tire location: " << tire_center.x() << " " << tire_center.y() << " " << tire_center.z() << std::endl;
173
174 // Enable moving patch, based on tire location
175 if (moving_patch)
176 terrain.EnableMovingPatch(body, 2.0, 0.2);
177
178 // -----------------
179 // Initialize OpenGL
180 // -----------------
181
182 opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
183 gl_window.Initialize(1280, 720, "Granular terrain demo", system);
184 gl_window.SetCamera(center - ChVector<>(0, 3, 0), center, ChVector<>(0, 0, 1), 0.05f);
185 gl_window.SetRenderMode(opengl::SOLID);
186
187 // ---------------
188 // Simulation loop
189 // ---------------
190
191 bool is_pitched = false;
192 double time = 0;
193
194 while (time < time_end) {
195 // Rotate gravity vector
196 if (!is_pitched && time > time_pitch) {
197 std::cout << time << " Pitch: " << gravityR.x() << " " << gravityR.y() << " " << gravityR.z()
198 << std::endl;
199 system->Set_G_acc(gravityR);
200 is_pitched = true;
201 }
202
203 terrain.Synchronize(time);
204 system->DoStepDynamics(time_step);
205
206 ////if (terrain.PatchMoved()) {
207 //// auto aabb_min = system->data_manager->measures.collision.rigid_min_bounding_point;
208 //// auto aabb_max = system->data_manager->measures.collision.rigid_max_bounding_point;
209 //// std::cout << " Global AABB: " << std::endl;
210 //// std::cout << " " << aabb_min.x << " " << aabb_min.y << " " << aabb_min.z << std::endl;
211 //// std::cout << " " << aabb_max.x << " " << aabb_max.y << " " << aabb_max.z << std::endl;
212 ////}
213
214 ////opengl::ChOpenGLWindow& gl_window = opengl::ChOpenGLWindow::getInstance();
215 if (gl_window.Active()) {
216 switch (cam_type) {
217 case FRONT: {
218 ChVector<> cam_loc(terrain.GetPatchFront(), -3, 0);
219 ChVector<> cam_point(terrain.GetPatchFront(), 0, 0);
220 gl_window.SetCamera(cam_loc, cam_point, ChVector<>(0, 0, 1), 0.05f);
221 break;
222 }
223 case TRACK: {
224 ChVector<> cam_point = body->GetPos();
225 ChVector<> cam_loc = cam_point + ChVector<>(-3 * tire_rad, -1, 0.6);
226 gl_window.SetCamera(cam_loc, cam_point, ChVector<>(0, 0, 1), 0.05f);
227 break;
228 }
229 default:
230 break;
231 }
232 gl_window.Render();
233 } else {
234 break;
235 }
236
237 time += time_step;
238 }
239
240 return 0;
241 }
242