1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2021 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 // Authors: Jason Zhou
13 // =============================================================================
14 //
15 // Turtlebot Robot Class
16 //
17 // =============================================================================
18
19 #include <cmath>
20
21 #include "chrono/assets/ChBoxShape.h"
22 #include "chrono/physics/ChBodyEasy.h"
23 #include "chrono/assets/ChColorAsset.h"
24 #include "chrono/assets/ChCylinderShape.h"
25 #include "chrono/assets/ChSphereShape.h"
26 #include "chrono/assets/ChTexture.h"
27 #include "chrono/assets/ChTriangleMeshShape.h"
28
29 #include "chrono/motion_functions/ChFunction_Setpoint.h"
30
31 #include "chrono/physics/ChLinkMotorRotationAngle.h"
32 #include "chrono/physics/ChLinkMotorRotationSpeed.h"
33 #include "chrono/physics/ChLinkMotorRotationTorque.h"
34 #include "chrono/physics/ChSystemNSC.h"
35 #include "chrono/physics/ChSystemSMC.h"
36
37 #include "chrono_models/robot/turtlebot/Turtlebot.h"
38
39 namespace chrono {
40 namespace turtlebot {
41
42 // =============================================================================
43 // Create default contact material for the robot
DefaultContactMaterial(ChContactMethod contact_method)44 std::shared_ptr<ChMaterialSurface> DefaultContactMaterial(ChContactMethod contact_method) {
45 float mu = 0.4f; // coefficient of friction
46 float cr = 0.0f; // coefficient of restitution
47 float Y = 2e7f; // Young's modulus
48 float nu = 0.3f; // Poisson ratio
49 float kn = 2e5f; // normal stiffness
50 float gn = 40.0f; // normal viscous damping
51 float kt = 2e5f; // tangential stiffness
52 float gt = 20.0f; // tangential viscous damping
53
54 switch (contact_method) {
55 case ChContactMethod::NSC: {
56 auto matNSC = chrono_types::make_shared<ChMaterialSurfaceNSC>();
57 matNSC->SetFriction(mu);
58 matNSC->SetRestitution(cr);
59 return matNSC;
60 }
61 case ChContactMethod::SMC: {
62 auto matSMC = chrono_types::make_shared<ChMaterialSurfaceSMC>();
63 matSMC->SetFriction(mu);
64 matSMC->SetRestitution(cr);
65 matSMC->SetYoungModulus(Y);
66 matSMC->SetPoissonRatio(nu);
67 matSMC->SetKn(kn);
68 matSMC->SetGn(gn);
69 matSMC->SetKt(kt);
70 matSMC->SetGt(gt);
71 return matSMC;
72 }
73 default:
74 return std::shared_ptr<ChMaterialSurface>();
75 }
76 }
77
78 // Add a revolute joint between body_1 and body_2
79 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point
AddRevoluteJoint(std::shared_ptr<ChBodyAuxRef> body_1,std::shared_ptr<ChBodyAuxRef> body_2,std::shared_ptr<ChBodyAuxRef> chassis,ChSystem * system,const ChVector<> & rel_joint_pos,const ChQuaternion<> & rel_joint_rot)80 void AddRevoluteJoint(std::shared_ptr<ChBodyAuxRef> body_1,
81 std::shared_ptr<ChBodyAuxRef> body_2,
82 std::shared_ptr<ChBodyAuxRef> chassis,
83 ChSystem* system,
84 const ChVector<>& rel_joint_pos,
85 const ChQuaternion<>& rel_joint_rot) {
86 const ChFrame<>& X_GP = chassis->GetFrame_REF_to_abs(); // global -> parent
87 ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child
88 ChFrame<> X_GC = X_GP * X_PC; // global -> child
89
90 auto revo = chrono_types::make_shared<ChLinkLockRevolute>();
91 revo->Initialize(body_1, body_2, ChCoordsys<>(X_GC.GetCoord().pos, X_GC.GetCoord().rot));
92 system->AddLink(revo);
93 }
94
95 // Add a revolute joint between body_1 and body_2
96 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point
AddRevoluteJoint(std::shared_ptr<ChBodyEasyBox> body_1,std::shared_ptr<ChBodyAuxRef> body_2,std::shared_ptr<ChBodyAuxRef> chassis,ChSystem * system,const ChVector<> & rel_joint_pos,const ChQuaternion<> & rel_joint_rot)97 void AddRevoluteJoint(std::shared_ptr<ChBodyEasyBox> body_1,
98 std::shared_ptr<ChBodyAuxRef> body_2,
99 std::shared_ptr<ChBodyAuxRef> chassis,
100 ChSystem* system,
101 const ChVector<>& rel_joint_pos,
102 const ChQuaternion<>& rel_joint_rot) {
103 const ChFrame<>& X_GP = chassis->GetFrame_REF_to_abs(); // global -> parent
104 ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child
105 ChFrame<> X_GC = X_GP * X_PC; // global -> child
106
107 auto revo = chrono_types::make_shared<ChLinkLockRevolute>();
108 revo->Initialize(body_1, body_2, ChCoordsys<>(X_GC.GetCoord().pos, X_GC.GetCoord().rot));
109 system->AddLink(revo);
110 }
111
112 // Add a revolute joint between body_1 and body_2
113 // rel_joint_pos and rel_joint_rot are the position and the rotation of the revolute point
AddLockJoint(std::shared_ptr<ChBodyAuxRef> body_1,std::shared_ptr<ChBodyAuxRef> body_2,std::shared_ptr<ChBodyAuxRef> chassis,ChSystem * system,const ChVector<> & rel_joint_pos,const ChQuaternion<> & rel_joint_rot)114 void AddLockJoint(std::shared_ptr<ChBodyAuxRef> body_1,
115 std::shared_ptr<ChBodyAuxRef> body_2,
116 std::shared_ptr<ChBodyAuxRef> chassis,
117 ChSystem* system,
118 const ChVector<>& rel_joint_pos,
119 const ChQuaternion<>& rel_joint_rot) {
120 const ChFrame<>& X_GP = chassis->GetFrame_REF_to_abs(); // global -> parent
121 ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child
122 ChFrame<> X_GC = X_GP * X_PC; // global -> child
123
124 // auto revo = chrono_types::make_shared<ChLinkLockRevolute>();
125 auto revo = chrono_types::make_shared<ChLinkLockLock>();
126 revo->Initialize(body_1, body_2, ChCoordsys<>(X_GC.GetCoord().pos, X_GC.GetCoord().rot));
127 system->AddLink(revo);
128 }
129
130 // Add a rotational speed controlled motor between body_1 and body_2
131 // rel_joint_pos and rel_joint_rot are the position and the rotation of the motor
AddMotor(std::shared_ptr<ChBody> body_1,std::shared_ptr<ChBodyAuxRef> body_2,std::shared_ptr<ChBodyAuxRef> chassis,ChSystem * system,const ChVector<> & rel_joint_pos,const ChQuaternion<> & rel_joint_rot,std::shared_ptr<ChFunction_Const> speed_func)132 std::shared_ptr<ChLinkMotorRotationSpeed> AddMotor(std::shared_ptr<ChBody> body_1,
133 std::shared_ptr<ChBodyAuxRef> body_2,
134 std::shared_ptr<ChBodyAuxRef> chassis,
135 ChSystem* system,
136 const ChVector<>& rel_joint_pos,
137 const ChQuaternion<>& rel_joint_rot,
138 std::shared_ptr<ChFunction_Const> speed_func) {
139 const ChFrame<>& X_GP = chassis->GetFrame_REF_to_abs(); // global -> parent
140 ChFrame<> X_PC(rel_joint_pos, rel_joint_rot); // parent -> child
141 ChFrame<> X_GC = X_GP * X_PC; // global -> child
142
143 auto motor_angle = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
144 motor_angle->Initialize(body_1, body_2, ChFrame<>(X_GC.GetCoord().pos, X_GC.GetCoord().rot));
145 system->AddLink(motor_angle);
146 motor_angle->SetSpeedFunction(speed_func);
147 return motor_angle;
148 }
149
150 // ===============================================================================
Turtlebot_Part(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis_body,bool collide)151 Turtlebot_Part::Turtlebot_Part(const std::string& name,
152 bool fixed,
153 std::shared_ptr<ChMaterialSurface> mat,
154 ChSystem* system,
155 const ChVector<>& body_pos,
156 const ChQuaternion<>& body_rot,
157 std::shared_ptr<ChBodyAuxRef> chassis_body,
158 bool collide) {
159 m_body = std::shared_ptr<ChBodyAuxRef>(system->NewBodyAuxRef());
160 m_body->SetNameString(name + "_body");
161 m_chassis = chassis_body;
162 m_mat = mat;
163 m_pos = body_pos;
164 m_rot = body_rot;
165 m_system = system;
166 m_collide = collide;
167 m_fixed = fixed;
168 }
169
170 // Create Visulization assets
AddVisualizationAssets()171 void Turtlebot_Part::AddVisualizationAssets() {
172 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
173 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
174 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), true, false);
175 trimesh->Transform(m_offset, ChMatrix33<>(1));
176 auto trimesh_shape = chrono_types::make_shared<ChTriangleMeshShape>();
177 trimesh_shape->SetMesh(trimesh);
178 trimesh_shape->SetName(m_mesh_name);
179 trimesh_shape->SetStatic(true);
180 m_body->AddAsset(trimesh_shape);
181 return;
182 }
183
SetCollide(bool state)184 void Turtlebot_Part::SetCollide(bool state) {
185 m_collide = state;
186 }
187
188 // Add collision assets
AddCollisionShapes()189 void Turtlebot_Part::AddCollisionShapes() {
190 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
191 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
192 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), true, false);
193 trimesh->Transform(m_offset, ChMatrix33<>(1));
194 auto trimesh_shape = chrono_types::make_shared<ChTriangleMeshShape>();
195 trimesh_shape->SetMesh(trimesh);
196 trimesh_shape->SetName(m_mesh_name);
197 trimesh_shape->SetStatic(true);
198
199 m_body->GetCollisionModel()->ClearModel();
200 m_body->GetCollisionModel()->AddTriangleMesh(m_mat, trimesh, false, false, VNULL, ChMatrix33<>(1), 0.005);
201 m_body->GetCollisionModel()->BuildModel();
202 m_body->SetCollide(m_collide);
203 }
204
205 // =============================================================================
206 // Robot Chassis
Turtlebot_Chassis(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,bool collide)207 Turtlebot_Chassis::Turtlebot_Chassis(const std::string& name,
208 bool fixed,
209 std::shared_ptr<ChMaterialSurface> mat,
210 ChSystem* system,
211 const ChVector<>& body_pos,
212 const ChQuaternion<>& body_rot,
213 bool collide)
214 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, NULL, collide) {
215 m_mesh_name = "chassis";
216 m_offset = ChVector<>(0, 0, 0);
217 m_color = ChColor(0.4f, 0.4f, 0.7f);
218 m_density = 100;
219 }
220
Initialize()221 void Turtlebot_Chassis::Initialize() {
222 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
223 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
224 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
225 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
226 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
227
228 double mmass;
229 ChVector<> mcog;
230 ChMatrix33<> minertia;
231 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
232 ChMatrix33<> principal_inertia_rot;
233 ChVector<> principal_I;
234 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
235
236 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
237
238 // Set inertia
239 m_body->SetMass(mmass * m_density);
240 m_body->SetInertiaXX(m_density * principal_I);
241 m_body->SetFrame_REF_to_abs(ChFrame<>(m_pos, m_rot));
242 m_body->SetBodyFixed(m_fixed);
243
244 AddCollisionShapes();
245
246 m_body->GetCollisionModel()->SetFamily(CollisionFamily::CHASSIS);
247 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::ACTIVE_WHEEL);
248
249 AddVisualizationAssets();
250
251 m_system->Add(m_body);
252 }
253
SetCollide(bool state)254 void Turtlebot_Chassis::SetCollide(bool state) {
255 m_collide = state;
256 m_body->SetCollide(state);
257 }
258
Translate(const ChVector<> & shift)259 void Turtlebot_Chassis::Translate(const ChVector<>& shift) {
260 m_body->SetPos(m_body->GetPos() + shift);
261 }
262
263 // ==========================================================
Turtlebot_ActiveWheel(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)264 Turtlebot_ActiveWheel::Turtlebot_ActiveWheel(const std::string& name,
265 bool fixed,
266 std::shared_ptr<ChMaterialSurface> mat,
267 ChSystem* system,
268 const ChVector<>& body_pos,
269 const ChQuaternion<>& body_rot,
270 std::shared_ptr<ChBodyAuxRef> chassis,
271 bool collide)
272 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
273 m_mesh_name = "active_wheel";
274 m_offset = ChVector<>(0, 0, 0);
275 m_color = ChColor(0.4f, 0.4f, 0.7f);
276 m_density = 200;
277 }
278
Initialize()279 void Turtlebot_ActiveWheel::Initialize() {
280 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
281 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
282 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
283 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
284 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
285
286 double mmass;
287 ChVector<> mcog;
288 ChMatrix33<> minertia;
289 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
290 ChMatrix33<> principal_inertia_rot;
291 ChVector<> principal_I;
292 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
293
294 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
295
296 // Set inertia
297 m_body->SetMass(mmass * m_density);
298 m_body->SetInertiaXX(m_density * principal_I);
299
300 // set relative position to chassis
301 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
302 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
303 ChFrame<> X_GC = X_GP * X_PC; // global -> child
304 m_body->SetFrame_REF_to_abs(X_GC);
305 m_body->SetBodyFixed(m_fixed);
306
307 AddCollisionShapes();
308
309 m_body->GetCollisionModel()->SetFamily(CollisionFamily::ACTIVE_WHEEL);
310 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
311
312 AddVisualizationAssets();
313
314 m_system->Add(m_body);
315 }
316
SetCollide(bool state)317 void Turtlebot_ActiveWheel::SetCollide(bool state) {
318 m_collide = state;
319 m_body->SetCollide(state);
320 }
321
Translate(const ChVector<> & shift)322 void Turtlebot_ActiveWheel::Translate(const ChVector<>& shift) {
323 m_body->SetPos(m_body->GetPos() + shift);
324 }
325
326 // ==========================================================
Turtlebot_PassiveWheel(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)327 Turtlebot_PassiveWheel::Turtlebot_PassiveWheel(const std::string& name,
328 bool fixed,
329 std::shared_ptr<ChMaterialSurface> mat,
330 ChSystem* system,
331 const ChVector<>& body_pos,
332 const ChQuaternion<>& body_rot,
333 std::shared_ptr<ChBodyAuxRef> chassis,
334 bool collide)
335 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
336 m_mesh_name = "passive_wheel";
337 m_offset = ChVector<>(0, 0, 0);
338 m_color = ChColor(0.4f, 0.4f, 0.7f);
339 m_density = 200;
340 }
341
Initialize()342 void Turtlebot_PassiveWheel::Initialize() {
343 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
344 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
345 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
346 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
347 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
348
349 double mmass;
350 ChVector<> mcog;
351 ChMatrix33<> minertia;
352 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
353 ChMatrix33<> principal_inertia_rot;
354 ChVector<> principal_I;
355 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
356
357 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
358
359 // Set inertia
360 m_body->SetMass(mmass * m_density);
361 m_body->SetInertiaXX(m_density * principal_I);
362
363 // set relative position to chassis
364 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
365 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
366 ChFrame<> X_GC = X_GP * X_PC; // global -> child
367 m_body->SetFrame_REF_to_abs(X_GC);
368 m_body->SetBodyFixed(m_fixed);
369
370 AddCollisionShapes();
371
372 m_body->GetCollisionModel()->SetFamily(CollisionFamily::PASSIVE_WHEEL);
373 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
374
375 AddVisualizationAssets();
376
377 m_system->Add(m_body);
378 }
379
SetCollide(bool state)380 void Turtlebot_PassiveWheel::SetCollide(bool state) {
381 m_collide = state;
382 m_body->SetCollide(state);
383 }
384
Translate(const ChVector<> & shift)385 void Turtlebot_PassiveWheel::Translate(const ChVector<>& shift) {
386 m_body->SetPos(m_body->GetPos() + shift);
387 }
388
389 // ==========================================================
Turtlebot_Rod_Short(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)390 Turtlebot_Rod_Short::Turtlebot_Rod_Short(const std::string& name,
391 bool fixed,
392 std::shared_ptr<ChMaterialSurface> mat,
393 ChSystem* system,
394 const ChVector<>& body_pos,
395 const ChQuaternion<>& body_rot,
396 std::shared_ptr<ChBodyAuxRef> chassis,
397 bool collide)
398 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
399 m_mesh_name = "support_rod_short";
400 m_offset = ChVector<>(0, 0, 0);
401 m_color = ChColor(0.4f, 0.4f, 0.7f);
402 m_density = 100;
403 }
404
Initialize()405 void Turtlebot_Rod_Short::Initialize() {
406 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
407 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
408 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
409 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
410 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
411
412 double mmass;
413 ChVector<> mcog;
414 ChMatrix33<> minertia;
415 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
416 ChMatrix33<> principal_inertia_rot;
417 ChVector<> principal_I;
418 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
419
420 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
421
422 // Set inertia
423 m_body->SetMass(mmass * m_density);
424 m_body->SetInertiaXX(m_density * principal_I);
425
426 // set relative position to chassis
427 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
428 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
429 ChFrame<> X_GC = X_GP * X_PC; // global -> child
430 m_body->SetFrame_REF_to_abs(X_GC);
431 m_body->SetBodyFixed(m_fixed);
432
433 AddCollisionShapes();
434
435 m_body->GetCollisionModel()->SetFamily(CollisionFamily::ROD);
436 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
437 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::BOTTOM_PLATE);
438 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::MIDDLE_PLATE);
439
440 AddVisualizationAssets();
441
442 m_system->Add(m_body);
443 }
444
SetCollide(bool state)445 void Turtlebot_Rod_Short::SetCollide(bool state) {
446 m_collide = state;
447 m_body->SetCollide(state);
448 }
449
Translate(const ChVector<> & shift)450 void Turtlebot_Rod_Short::Translate(const ChVector<>& shift) {
451 m_body->SetPos(m_body->GetPos() + shift);
452 }
453
454 // ==========================================================
Turtlebot_BottomPlate(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)455 Turtlebot_BottomPlate::Turtlebot_BottomPlate(const std::string& name,
456 bool fixed,
457 std::shared_ptr<ChMaterialSurface> mat,
458 ChSystem* system,
459 const ChVector<>& body_pos,
460 const ChQuaternion<>& body_rot,
461 std::shared_ptr<ChBodyAuxRef> chassis,
462 bool collide)
463 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
464 m_mesh_name = "plate_1";
465 m_offset = ChVector<>(0, 0, 0);
466 m_color = ChColor(0.4f, 0.4f, 0.7f);
467 m_density = 20;
468 }
469
Initialize()470 void Turtlebot_BottomPlate::Initialize() {
471 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
472 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
473 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
474 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
475 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
476
477 double mmass;
478 ChVector<> mcog;
479 ChMatrix33<> minertia;
480 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
481 ChMatrix33<> principal_inertia_rot;
482 ChVector<> principal_I;
483 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
484
485 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
486
487 // Set inertia
488 m_body->SetMass(mmass * m_density);
489 m_body->SetInertiaXX(m_density * principal_I);
490
491 // set relative position to chassis
492 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
493 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
494 ChFrame<> X_GC = X_GP * X_PC; // global -> child
495 m_body->SetFrame_REF_to_abs(X_GC);
496 m_body->SetBodyFixed(m_fixed);
497
498 AddCollisionShapes();
499
500 m_body->GetCollisionModel()->SetFamily(CollisionFamily::BOTTOM_PLATE);
501 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::ROD);
502 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
503
504 AddVisualizationAssets();
505
506 m_system->Add(m_body);
507 }
508
SetCollide(bool state)509 void Turtlebot_BottomPlate::SetCollide(bool state) {
510 m_collide = state;
511 m_body->SetCollide(state);
512 }
513
Translate(const ChVector<> & shift)514 void Turtlebot_BottomPlate::Translate(const ChVector<>& shift) {
515 m_body->SetPos(m_body->GetPos() + shift);
516 }
517
518 // ==========================================================
Turtlebot_MiddlePlate(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)519 Turtlebot_MiddlePlate::Turtlebot_MiddlePlate(const std::string& name,
520 bool fixed,
521 std::shared_ptr<ChMaterialSurface> mat,
522 ChSystem* system,
523 const ChVector<>& body_pos,
524 const ChQuaternion<>& body_rot,
525 std::shared_ptr<ChBodyAuxRef> chassis,
526 bool collide)
527 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
528 m_mesh_name = "plate_2";
529 m_offset = ChVector<>(0, 0, 0);
530 m_color = ChColor(0.4f, 0.4f, 0.7f);
531 m_density = 20;
532 }
533
Initialize()534 void Turtlebot_MiddlePlate::Initialize() {
535 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
536 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
537 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
538 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
539 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
540
541 double mmass;
542 ChVector<> mcog;
543 ChMatrix33<> minertia;
544 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
545 ChMatrix33<> principal_inertia_rot;
546 ChVector<> principal_I;
547 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
548
549 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
550
551 // Set inertia
552 m_body->SetMass(mmass * m_density);
553 m_body->SetInertiaXX(m_density * principal_I);
554
555 // set relative position to chassis
556 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
557 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
558 ChFrame<> X_GC = X_GP * X_PC; // global -> child
559 m_body->SetFrame_REF_to_abs(X_GC);
560 m_body->SetBodyFixed(m_fixed);
561
562 AddCollisionShapes();
563
564 m_body->GetCollisionModel()->SetFamily(CollisionFamily::MIDDLE_PLATE);
565 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::ROD);
566 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
567
568 AddVisualizationAssets();
569
570 m_system->Add(m_body);
571 }
572
SetCollide(bool state)573 void Turtlebot_MiddlePlate::SetCollide(bool state) {
574 m_collide = state;
575 m_body->SetCollide(state);
576 }
577
Translate(const ChVector<> & shift)578 void Turtlebot_MiddlePlate::Translate(const ChVector<>& shift) {
579 m_body->SetPos(m_body->GetPos() + shift);
580 }
581
582 // ==========================================================
Turtlebot_TopPlate(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)583 Turtlebot_TopPlate::Turtlebot_TopPlate(const std::string& name,
584 bool fixed,
585 std::shared_ptr<ChMaterialSurface> mat,
586 ChSystem* system,
587 const ChVector<>& body_pos,
588 const ChQuaternion<>& body_rot,
589 std::shared_ptr<ChBodyAuxRef> chassis,
590 bool collide)
591 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
592 m_mesh_name = "plate_3";
593 m_offset = ChVector<>(0, 0, 0);
594 m_color = ChColor(0.4f, 0.4f, 0.7f);
595 m_density = 20;
596 }
597
Initialize()598 void Turtlebot_TopPlate::Initialize() {
599 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
600 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
601 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
602 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
603 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
604
605 double mmass;
606 ChVector<> mcog;
607 ChMatrix33<> minertia;
608 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
609 ChMatrix33<> principal_inertia_rot;
610 ChVector<> principal_I;
611 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
612
613 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
614
615 // Set inertia
616 m_body->SetMass(mmass * m_density);
617 m_body->SetInertiaXX(m_density * principal_I);
618
619 // set relative position to chassis
620 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
621 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
622 ChFrame<> X_GC = X_GP * X_PC; // global -> child
623 m_body->SetFrame_REF_to_abs(X_GC);
624 m_body->SetBodyFixed(m_fixed);
625
626 AddCollisionShapes();
627
628 m_body->GetCollisionModel()->SetFamily(CollisionFamily::TOP_PLATE);
629 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::ROD);
630 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
631 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::MIDDLE_PLATE);
632 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::BOTTOM_PLATE);
633
634 AddVisualizationAssets();
635
636 m_system->Add(m_body);
637 }
638
SetCollide(bool state)639 void Turtlebot_TopPlate::SetCollide(bool state) {
640 m_collide = state;
641 m_body->SetCollide(state);
642 }
643
Translate(const ChVector<> & shift)644 void Turtlebot_TopPlate::Translate(const ChVector<>& shift) {
645 m_body->SetPos(m_body->GetPos() + shift);
646 }
647
648 // ==========================================================
Turtlebot_Rod_Long(const std::string & name,bool fixed,std::shared_ptr<ChMaterialSurface> mat,ChSystem * system,const ChVector<> & body_pos,const ChQuaternion<> & body_rot,std::shared_ptr<ChBodyAuxRef> chassis,bool collide)649 Turtlebot_Rod_Long::Turtlebot_Rod_Long(const std::string& name,
650 bool fixed,
651 std::shared_ptr<ChMaterialSurface> mat,
652 ChSystem* system,
653 const ChVector<>& body_pos,
654 const ChQuaternion<>& body_rot,
655 std::shared_ptr<ChBodyAuxRef> chassis,
656 bool collide)
657 : Turtlebot_Part(name, fixed, mat, system, body_pos, body_rot, chassis, collide) {
658 m_mesh_name = "support_rod_long";
659 m_offset = ChVector<>(0, 0, 0);
660 m_color = ChColor(0.4f, 0.4f, 0.7f);
661 m_density = 100;
662 }
663
Initialize()664 void Turtlebot_Rod_Long::Initialize() {
665 std::string vis_mesh_file = "robot/turtlebot/" + m_mesh_name + ".obj";
666 auto trimesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
667 trimesh->LoadWavefrontMesh(GetChronoDataFile(vis_mesh_file), false, false);
668 trimesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1)); // scale to a different size
669 trimesh->RepairDuplicateVertexes(1e-9); // if meshes are not watertight
670
671 double mmass;
672 ChVector<> mcog;
673 ChMatrix33<> minertia;
674 trimesh->ComputeMassProperties(true, mmass, mcog, minertia);
675 ChMatrix33<> principal_inertia_rot;
676 ChVector<> principal_I;
677 ChInertiaUtils::PrincipalInertia(minertia, principal_I, principal_inertia_rot);
678
679 m_body->SetFrame_COG_to_REF(ChFrame<>(mcog, principal_inertia_rot));
680
681 // Set inertia
682 m_body->SetMass(mmass * m_density);
683 m_body->SetInertiaXX(m_density * principal_I);
684
685 // set relative position to chassis
686 const ChFrame<>& X_GP = m_chassis->GetFrame_REF_to_abs(); // global -> parent
687 ChFrame<> X_PC(m_pos, m_rot); // parent -> child
688 ChFrame<> X_GC = X_GP * X_PC; // global -> child
689 m_body->SetFrame_REF_to_abs(X_GC);
690 m_body->SetBodyFixed(m_fixed);
691
692 AddCollisionShapes();
693
694 m_body->GetCollisionModel()->SetFamily(CollisionFamily::ROD);
695 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::CHASSIS);
696 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::BOTTOM_PLATE);
697 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::MIDDLE_PLATE);
698 m_body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(CollisionFamily::ROD);
699
700 AddVisualizationAssets();
701
702 m_system->Add(m_body);
703 }
704
SetCollide(bool state)705 void Turtlebot_Rod_Long::SetCollide(bool state) {
706 m_collide = state;
707 m_body->SetCollide(state);
708 }
709
Translate(const ChVector<> & shift)710 void Turtlebot_Rod_Long::Translate(const ChVector<>& shift) {
711 m_body->SetPos(m_body->GetPos() + shift);
712 }
713
714 // ==========================================================
715 // Turtlebot Class for the complete robot model
TurtleBot(ChSystem * system,const ChVector<> & robot_pos,const ChQuaternion<> & robot_rot,std::shared_ptr<ChMaterialSurface> wheel_mat)716 TurtleBot::TurtleBot(ChSystem* system,
717 const ChVector<>& robot_pos,
718 const ChQuaternion<>& robot_rot,
719 std::shared_ptr<ChMaterialSurface> wheel_mat)
720 : m_system(system), m_robot_pos(robot_pos), m_robot_rot(robot_rot), m_wheel_material(wheel_mat) {
721 // Set default collision model envelope commensurate with model dimensions.
722 // Note that an SMC system automatically sets envelope to 0.
723 auto contact_method = m_system->GetContactMethod();
724 if (contact_method == ChContactMethod::NSC) {
725 collision::ChCollisionModel::SetDefaultSuggestedEnvelope(0.01);
726 collision::ChCollisionModel::SetDefaultSuggestedMargin(0.005);
727 }
728
729 // Create the contact materials
730 m_chassis_material = DefaultContactMaterial(contact_method);
731 if (!m_wheel_material)
732 m_wheel_material = DefaultContactMaterial(contact_method);
733
734 Create();
735 }
736
~TurtleBot()737 TurtleBot::~TurtleBot() {}
738
Create()739 void TurtleBot::Create() {
740 // initialize robot chassis
741 m_chassis = chrono_types::make_shared<Turtlebot_Chassis>("chassis", false, m_chassis_material, m_system,
742 m_robot_pos, m_robot_rot, true);
743
744 // active drive wheels' positions relative to the chassis
745 double dwx = 0;
746 double dwy = 0.11505;
747 double dwz = 0.03735;
748 for (int i = 0; i < 2; i++) {
749 if (i == 0) {
750 m_drive_wheels.push_back(chrono_types::make_shared<Turtlebot_ActiveWheel>(
751 "LDWheel", false, m_wheel_material, m_system, ChVector<>(dwx, +dwy, dwz), ChQuaternion<>(1, 0, 0, 0),
752 m_chassis->GetBody(), true));
753 }
754
755 if (i == 1) {
756 m_drive_wheels.push_back(chrono_types::make_shared<Turtlebot_ActiveWheel>(
757 "RDWheel", false, m_wheel_material, m_system, ChVector<>(dwx, -dwy, dwz), ChQuaternion<>(1, 0, 0, 0),
758 m_chassis->GetBody(), true));
759 }
760 }
761
762 // passive driven wheels' positions relative to the chassis
763 double pwx = 0.11505;
764 double pwy = 0;
765 double pwz = 0.02015;
766
767 for (int i = 0; i < 2; i++) {
768 if (i == 0) {
769 m_passive_wheels.push_back(chrono_types::make_shared<Turtlebot_PassiveWheel>(
770 "FPWheel", false, m_wheel_material, m_system, ChVector<>(pwx, pwy, pwz), ChQuaternion<>(1, 0, 0, 0),
771 m_chassis->GetBody(), true));
772 }
773
774 if (i == 1) {
775 m_passive_wheels.push_back(chrono_types::make_shared<Turtlebot_PassiveWheel>(
776 "RPWheel", false, m_wheel_material, m_system, ChVector<>(-pwx, pwy, pwz), ChQuaternion<>(1, 0, 0, 0),
777 m_chassis->GetBody(), true));
778 }
779 }
780
781 // create the first level supporting rod
782 double rod_s_0_x = -0.0565;
783 double rod_s_0_y = 0.11992;
784 double rod_s_0_z = 0.09615;
785
786 double rod_s_1_x = 0.0535;
787 double rod_s_1_y = 0.11992;
788 double rod_s_1_z = 0.09615;
789
790 double rod_s_2_x = 0.11850;
791 double rod_s_2_y = 0.08192;
792 double rod_s_2_z = 0.09615;
793
794 double rod_s_3_x = 0.11850;
795 double rod_s_3_y = -0.08192;
796 double rod_s_3_z = 0.09615;
797
798 double rod_s_4_x = 0.0535;
799 double rod_s_4_y = -0.11992;
800 double rod_s_4_z = 0.09615;
801
802 double rod_s_5_x = -0.0565;
803 double rod_s_5_y = -0.11992;
804 double rod_s_5_z = 0.09615;
805
806 for (int i = 0; i < 6; i++) {
807 if (i == 0) {
808 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
809 "0-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_0_x, rod_s_0_y, rod_s_0_z),
810 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
811 }
812
813 if (i == 1) {
814 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
815 "1-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_1_x, rod_s_1_y, rod_s_1_z),
816 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
817 }
818
819 if (i == 2) {
820 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
821 "2-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_2_x, rod_s_2_y, rod_s_2_z),
822 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
823 }
824
825 if (i == 3) {
826 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
827 "3-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_3_x, rod_s_3_y, rod_s_3_z),
828 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
829 }
830
831 if (i == 4) {
832 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
833 "4-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_4_x, rod_s_4_y, rod_s_4_z),
834 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
835 }
836
837 if (i == 5) {
838 m_1st_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
839 "5-bottom-rod", false, m_wheel_material, m_system, ChVector<>(rod_s_5_x, rod_s_5_y, rod_s_5_z),
840 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
841 }
842 }
843
844 // add the bottom plate
845 double bt_plate_x = 0;
846 double bt_plate_y = 0;
847 double bt_plate_z = 0.14615;
848
849 m_bottom_plate = chrono_types::make_shared<Turtlebot_BottomPlate>(
850 "bottom_plate", false, m_wheel_material, m_system, ChVector<>(bt_plate_x, bt_plate_y, bt_plate_z),
851 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true);
852
853 // create the second level support rod
854 double rod_m_0_x = -0.10394;
855 double rod_m_0_y = 0.09792;
856 double rod_m_0_z = 0.15015;
857
858 double rod_m_1_x = -0.0015;
859 double rod_m_1_y = 0.16192;
860 double rod_m_1_z = 0.15015;
861
862 double rod_m_2_x = 0.0687;
863 double rod_m_2_y = 0.13132;
864 double rod_m_2_z = 0.15015;
865
866 double rod_m_3_x = 0.0687;
867 double rod_m_3_y = -0.13132;
868 double rod_m_3_z = 0.15015;
869
870 double rod_m_4_x = -0.0015;
871 double rod_m_4_y = -0.16192;
872 double rod_m_4_z = 0.15015;
873
874 double rod_m_5_x = -0.10394;
875 double rod_m_5_y = -0.09792;
876 double rod_m_5_z = 0.15015;
877
878 for (int i = 0; i < 6; i++) {
879 if (i == 0) {
880 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
881 "0-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_0_x, rod_m_0_y, rod_m_0_z),
882 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
883 }
884
885 if (i == 1) {
886 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
887 "1-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_1_x, rod_m_1_y, rod_m_1_z),
888 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
889 }
890
891 if (i == 2) {
892 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
893 "2-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_2_x, rod_m_2_y, rod_m_2_z),
894 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
895 }
896
897 if (i == 3) {
898 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
899 "3-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_3_x, rod_m_3_y, rod_m_3_z),
900 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
901 }
902
903 if (i == 4) {
904 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
905 "4-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_4_x, rod_m_4_y, rod_m_4_z),
906 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
907 }
908
909 if (i == 5) {
910 m_2nd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Short>(
911 "5-middle-rod", false, m_wheel_material, m_system, ChVector<>(rod_m_5_x, rod_m_5_y, rod_m_5_z),
912 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
913 }
914 }
915
916 // add the middle plate
917 double mi_plate_x = 0;
918 double mi_plate_y = 0;
919 double mi_plate_z = 0.20015;
920 m_middle_plate = chrono_types::make_shared<Turtlebot_MiddlePlate>(
921 "middle_plate", false, m_wheel_material, m_system, ChVector<>(mi_plate_x, mi_plate_y, mi_plate_z),
922 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true);
923
924 // create the third level support rod
925 double rod_u_0_x = -0.10394;
926 double rod_u_0_y = 0.09792;
927 double rod_u_0_z = 0.20615;
928
929 double rod_u_1_x = -0.0015;
930 double rod_u_1_y = 0.16192;
931 double rod_u_1_z = 0.20615;
932
933 double rod_u_2_x = 0.0687;
934 double rod_u_2_y = 0.13132;
935 double rod_u_2_z = 0.20615;
936
937 double rod_u_3_x = 0.0687;
938 double rod_u_3_y = -0.13132;
939 double rod_u_3_z = 0.20615;
940
941 double rod_u_4_x = -0.0015;
942 double rod_u_4_y = -0.16192;
943 double rod_u_4_z = 0.20615;
944
945 double rod_u_5_x = -0.10394;
946 double rod_u_5_y = -0.09792;
947 double rod_u_5_z = 0.20615;
948
949 for (int i = 0; i < 6; i++) {
950 if (i == 0) {
951 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
952 "0-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_0_x, rod_u_0_y, rod_u_0_z),
953 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
954 }
955
956 if (i == 1) {
957 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
958 "1-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_1_x, rod_u_1_y, rod_u_1_z),
959 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
960 }
961
962 if (i == 2) {
963 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
964 "2-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_2_x, rod_u_2_y, rod_u_2_z),
965 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
966 }
967
968 if (i == 3) {
969 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
970 "3-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_3_x, rod_u_3_y, rod_u_3_z),
971 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
972 }
973
974 if (i == 4) {
975 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
976 "4-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_4_x, rod_u_4_y, rod_u_4_z),
977 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
978 }
979
980 if (i == 5) {
981 m_3rd_level_rods.push_back(chrono_types::make_shared<Turtlebot_Rod_Long>(
982 "5-top-rod", false, m_wheel_material, m_system, ChVector<>(rod_u_5_x, rod_u_5_y, rod_u_5_z),
983 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true));
984 }
985 }
986
987 // add the top plate
988 double top_plate_x = 0;
989 double top_plate_y = 0;
990 double top_plate_z = 0.40615;
991 m_top_plate = chrono_types::make_shared<Turtlebot_TopPlate>("top_plate", false, m_wheel_material, m_system,
992 ChVector<>(top_plate_x, top_plate_y, top_plate_z),
993 ChQuaternion<>(1, 0, 0, 0), m_chassis->GetBody(), true);
994 }
995
996 /// Initialize the complete rover and add all constraints
Initialize()997 void TurtleBot::Initialize() {
998 m_chassis->Initialize();
999 m_bottom_plate->Initialize();
1000 m_middle_plate->Initialize();
1001 m_top_plate->Initialize();
1002 for (int i = 0; i < 2; i++) {
1003 m_drive_wheels[i]->Initialize();
1004 m_passive_wheels[i]->Initialize();
1005 }
1006
1007 for (int i = 0; i < 6; i++) {
1008 m_1st_level_rods[i]->Initialize();
1009 m_2nd_level_rods[i]->Initialize();
1010 m_3rd_level_rods[i]->Initialize();
1011 }
1012
1013 // redeclare necessary location variables
1014 double dwx = 0;
1015 double dwy = 0.11505;
1016 double dwz = 0.03735;
1017
1018 double pwx = 0.11505;
1019 double pwy = 0;
1020 double pwz = 0.02015;
1021
1022 double rod_s_0_x = -0.0565;
1023 double rod_s_0_y = 0.11992;
1024 double rod_s_0_z = 0.09615;
1025
1026 double rod_s_1_x = 0.0535;
1027 double rod_s_1_y = 0.11992;
1028 double rod_s_1_z = 0.09615;
1029
1030 double rod_s_2_x = 0.11850;
1031 double rod_s_2_y = 0.08192;
1032 double rod_s_2_z = 0.09615;
1033
1034 double rod_s_3_x = 0.11850;
1035 double rod_s_3_y = -0.08192;
1036 double rod_s_3_z = 0.09615;
1037
1038 double rod_s_4_x = 0.0535;
1039 double rod_s_4_y = -0.11992;
1040 double rod_s_4_z = 0.09615;
1041
1042 double rod_s_5_x = -0.0565;
1043 double rod_s_5_y = -0.11992;
1044 double rod_s_5_z = 0.09615;
1045
1046 double rod_m_0_x = -0.10394;
1047 double rod_m_0_y = 0.09792;
1048 double rod_m_0_z = 0.15015;
1049
1050 double rod_m_1_x = -0.0015;
1051 double rod_m_1_y = 0.16192;
1052 double rod_m_1_z = 0.15015;
1053
1054 double rod_m_2_x = 0.0687;
1055 double rod_m_2_y = 0.13132;
1056 double rod_m_2_z = 0.15015;
1057
1058 double rod_m_3_x = 0.0687;
1059 double rod_m_3_y = -0.13132;
1060 double rod_m_3_z = 0.15015;
1061
1062 double rod_m_4_x = -0.0015;
1063 double rod_m_4_y = -0.16192;
1064 double rod_m_4_z = 0.15015;
1065
1066 double rod_m_5_x = -0.10394;
1067 double rod_m_5_y = -0.09792;
1068 double rod_m_5_z = 0.15015;
1069
1070 double rod_u_0_x = -0.10394;
1071 double rod_u_0_y = 0.09792;
1072 double rod_u_0_z = 0.20615;
1073
1074 double rod_u_1_x = -0.0015;
1075 double rod_u_1_y = 0.16192;
1076 double rod_u_1_z = 0.20615;
1077
1078 double rod_u_2_x = 0.0687;
1079 double rod_u_2_y = 0.13132;
1080 double rod_u_2_z = 0.20615;
1081
1082 double rod_u_3_x = 0.0687;
1083 double rod_u_3_y = -0.13132;
1084 double rod_u_3_z = 0.20615;
1085
1086 double rod_u_4_x = -0.0015;
1087 double rod_u_4_y = -0.16192;
1088 double rod_u_4_z = 0.20615;
1089
1090 double rod_u_5_x = -0.10394;
1091 double rod_u_5_y = -0.09792;
1092 double rod_u_5_z = 0.20615;
1093
1094 // add motors and revolute joints on the active and passive wheels
1095 auto const_speed_function_l = chrono_types::make_shared<ChFunction_Const>(-CH_C_PI);
1096 auto const_speed_function_r = chrono_types::make_shared<ChFunction_Const>(-CH_C_PI);
1097 m_motors_func.push_back(const_speed_function_l);
1098 m_motors_func.push_back(const_speed_function_r);
1099
1100 ChQuaternion<> z2y;
1101 z2y.Q_from_AngAxis(CH_C_PI / 2, ChVector<>(1, 0, 0));
1102
1103 ChQuaternion<> z2x;
1104 z2x.Q_from_AngAxis(-CH_C_PI / 2, ChVector<>(0, 1, 0));
1105
1106 for (int i = 0; i < 2; i++) {
1107 ChVector<> active_rel_pos;
1108 ChVector<> passive_rel_pos;
1109 if (i == 0) {
1110 active_rel_pos = ChVector<>(dwx, dwy, dwz);
1111 passive_rel_pos = ChVector<>(pwx, pwy, pwz);
1112 } else {
1113 active_rel_pos = ChVector<>(dwx, -dwy, dwz);
1114 passive_rel_pos = ChVector<>(-pwx, pwy, pwz);
1115 }
1116 if (i == 0) {
1117 m_motors.push_back(AddMotor(m_drive_wheels[i]->GetBody(), m_chassis->GetBody(), m_chassis->GetBody(),
1118 m_system, active_rel_pos, z2y, const_speed_function_l));
1119 } else {
1120 m_motors.push_back(AddMotor(m_drive_wheels[i]->GetBody(), m_chassis->GetBody(), m_chassis->GetBody(),
1121 m_system, active_rel_pos, z2y, const_speed_function_r));
1122 }
1123
1124 AddRevoluteJoint(m_passive_wheels[i]->GetBody(), m_chassis->GetBody(), m_chassis->GetBody(), m_system,
1125 passive_rel_pos, z2x);
1126 }
1127
1128 // add fixity on all rods and plates
1129 // There are six constraints needed:
1130 // chassis -> bottom rods
1131 // bottom rods -> bottom plate
1132 // bottom plate -> middle rods
1133 // middle rods -> middle plate
1134 // middle plate -> top rods
1135 // top rods -> top plate
1136 for (int i = 0; i < 6; i++) {
1137 ChVector<> bottom_rod_rel_pos;
1138 ChVector<> bottom_plate_rel_pos;
1139 ChVector<> middle_rod_rel_pos;
1140 ChVector<> middle_plate_rel_pos;
1141 ChVector<> top_rod_rel_pos;
1142 ChVector<> top_plate_rel_pos;
1143 if (i == 0) {
1144 bottom_rod_rel_pos = ChVector<>(rod_s_0_x, rod_s_0_y, rod_s_0_z);
1145 middle_rod_rel_pos = ChVector<>(rod_m_0_x, rod_m_0_y, rod_m_0_z);
1146 top_rod_rel_pos = ChVector<>(rod_u_0_x, rod_u_0_y, rod_u_0_z);
1147 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1148 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1149 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1150 } else if (i == 1) {
1151 bottom_rod_rel_pos = ChVector<>(rod_s_1_x, rod_s_1_y, rod_s_1_z);
1152 middle_rod_rel_pos = ChVector<>(rod_m_1_x, rod_m_1_y, rod_m_1_z);
1153 top_rod_rel_pos = ChVector<>(rod_u_1_x, rod_u_1_y, rod_u_1_z);
1154 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1155 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1156 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1157 } else if (i == 2) {
1158 bottom_rod_rel_pos = ChVector<>(rod_s_2_x, rod_s_2_y, rod_s_2_z);
1159 middle_rod_rel_pos = ChVector<>(rod_m_2_x, rod_m_2_y, rod_m_2_z);
1160 top_rod_rel_pos = ChVector<>(rod_u_2_x, rod_u_2_y, rod_u_2_z);
1161 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1162 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1163 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1164 } else if (i == 3) {
1165 bottom_rod_rel_pos = ChVector<>(rod_s_3_x, rod_s_3_y, rod_s_3_z);
1166 middle_rod_rel_pos = ChVector<>(rod_m_3_x, rod_m_3_y, rod_m_3_z);
1167 top_rod_rel_pos = ChVector<>(rod_u_3_x, rod_u_3_y, rod_u_3_z);
1168 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1169 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1170 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1171 } else if (i == 4) {
1172 bottom_rod_rel_pos = ChVector<>(rod_s_4_x, rod_s_4_y, rod_s_4_z);
1173 middle_rod_rel_pos = ChVector<>(rod_m_4_x, rod_m_4_y, rod_m_4_z);
1174 top_rod_rel_pos = ChVector<>(rod_u_4_x, rod_u_4_y, rod_u_4_z);
1175 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1176 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1177 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1178 } else if (i == 5) {
1179 bottom_rod_rel_pos = ChVector<>(rod_s_5_x, rod_s_5_y, rod_s_5_z);
1180 middle_rod_rel_pos = ChVector<>(rod_m_5_x, rod_m_5_y, rod_m_5_z);
1181 top_rod_rel_pos = ChVector<>(rod_u_5_x, rod_u_5_y, rod_u_5_z);
1182 bottom_plate_rel_pos = bottom_rod_rel_pos + ChVector<>(0, 0, 0.05);
1183 middle_plate_rel_pos = middle_rod_rel_pos + ChVector<>(0, 0, 0.05);
1184 top_plate_rel_pos = top_rod_rel_pos + ChVector<>(0, 0, 0.2);
1185 }
1186
1187 AddLockJoint(m_1st_level_rods[i]->GetBody(), m_chassis->GetBody(), m_chassis->GetBody(), m_system,
1188 bottom_rod_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1189 AddLockJoint(m_1st_level_rods[i]->GetBody(), m_bottom_plate->GetBody(), m_chassis->GetBody(), m_system,
1190 bottom_plate_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1191 AddLockJoint(m_2nd_level_rods[i]->GetBody(), m_bottom_plate->GetBody(), m_chassis->GetBody(), m_system,
1192 bottom_rod_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1193 AddLockJoint(m_2nd_level_rods[i]->GetBody(), m_middle_plate->GetBody(), m_chassis->GetBody(), m_system,
1194 middle_plate_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1195 AddLockJoint(m_3rd_level_rods[i]->GetBody(), m_middle_plate->GetBody(), m_chassis->GetBody(), m_system,
1196 top_rod_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1197 AddLockJoint(m_3rd_level_rods[i]->GetBody(), m_top_plate->GetBody(), m_chassis->GetBody(), m_system,
1198 top_plate_rel_pos, ChQuaternion<>(1, 0, 0, 0));
1199 }
1200 }
1201
SetMotorSpeed(double rad_speed,WheelID id)1202 void TurtleBot::SetMotorSpeed(double rad_speed, WheelID id) {
1203 m_motors_func[id]->Set_yconst(rad_speed);
1204 }
1205
GetActiveWheelSpeed(WheelID id)1206 ChVector<> TurtleBot::GetActiveWheelSpeed(WheelID id) {
1207 return m_drive_wheels[id]->GetBody()->GetPos_dt();
1208 }
1209
GetActiveWheelAngVel(WheelID id)1210 ChVector<> TurtleBot::GetActiveWheelAngVel(WheelID id) {
1211 return m_drive_wheels[id]->GetBody()->GetWvel_par();
1212 }
1213
1214 } // namespace turtlebot
1215 } // namespace chrono
1216