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 // Authors: Daniel Melanz
13 // =============================================================================
14 //
15 // Base class for a MacPherson strut modeled with bodies and constraints.
16 //
17 // The suspension subsystem is modeled with respect to a right-handed frame,
18 // with X pointing towards the front, Y to the left, and Z up (ISO standard).
19 // The suspension reference frame is assumed to be always aligned with that of
20 // the vehicle. When attached to a chassis, only an offset is provided.
21 //
22 // All point locations are assumed to be given for the left half of the
23 // suspension and will be mirrored (reflecting the y coordinates) to construct
24 // the right side.
25 //
26 // =============================================================================
27
28 #include "chrono/assets/ChCylinderShape.h"
29 #include "chrono/assets/ChSphereShape.h"
30 #include "chrono/assets/ChPointPointDrawing.h"
31 #include "chrono/assets/ChColorAsset.h"
32
33 #include "chrono_vehicle/wheeled_vehicle/suspension/ChMacPhersonStrut.h"
34
35 namespace chrono {
36 namespace vehicle {
37
38 // -----------------------------------------------------------------------------
39 // Static variables
40 // -----------------------------------------------------------------------------
41 const std::string ChMacPhersonStrut::m_pointNames[] = {"SPINDLE ", "UPRIGHT ", "LCA_F ", "LCA_B ",
42 "LCA_U ", "LCA_CM ", "SHOCK_C ", "SHOCK_A ",
43 "SPRING_C", "SPRING_A", "TIEROD_C", "TIEROD_U"};
44
45 // -----------------------------------------------------------------------------
46 // -----------------------------------------------------------------------------
ChMacPhersonStrut(const std::string & name)47 ChMacPhersonStrut::ChMacPhersonStrut(const std::string& name) : ChSuspension(name) {}
48
~ChMacPhersonStrut()49 ChMacPhersonStrut::~ChMacPhersonStrut() {
50 auto sys = m_upright[0]->GetSystem();
51 if (sys) {
52 for (int i = 0; i < 2; i++) {
53 sys->Remove(m_upright[i]);
54 sys->Remove(m_strut[i]);
55 sys->Remove(m_LCA[i]);
56
57 sys->Remove(m_cylindricalStrut[i]);
58 sys->Remove(m_universalStrut[i]);
59
60 ChChassis::RemoveJoint(m_revoluteLCA[i]);
61 ChChassis::RemoveJoint(m_sphericalLCA[i]);
62
63 if (m_tierod[i]) {
64 sys->Remove(m_tierod[i]);
65 ChChassis::RemoveJoint(m_sphericalTierod[i]);
66 ChChassis::RemoveJoint(m_universalTierod[i]);
67 }
68 if (m_distTierod[i]) {
69 sys->Remove(m_distTierod[i]);
70 }
71
72 sys->Remove(m_shock[i]);
73 sys->Remove(m_spring[i]);
74 }
75 }
76 }
77
78 // -----------------------------------------------------------------------------
79 // -----------------------------------------------------------------------------
Initialize(std::shared_ptr<ChChassis> chassis,std::shared_ptr<ChSubchassis> subchassis,std::shared_ptr<ChSteering> steering,const ChVector<> & location,double left_ang_vel,double right_ang_vel)80 void ChMacPhersonStrut::Initialize(std::shared_ptr<ChChassis> chassis,
81 std::shared_ptr<ChSubchassis> subchassis,
82 std::shared_ptr<ChSteering> steering,
83 const ChVector<>& location,
84 double left_ang_vel,
85 double right_ang_vel) {
86 m_location = location;
87
88 // Express the suspension reference frame in the absolute coordinate system.
89 ChFrame<> suspension_to_abs(location);
90 suspension_to_abs.ConcatenatePreTransformation(chassis->GetBody()->GetFrame_REF_to_abs());
91
92 // Transform all hardpoints to absolute frame.
93 m_pointsL.resize(NUM_POINTS);
94 m_pointsR.resize(NUM_POINTS);
95 for (int i = 0; i < NUM_POINTS; i++) {
96 ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
97 m_pointsL[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
98 rel_pos.y() = -rel_pos.y();
99 m_pointsR[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
100 }
101
102 // Initialize left and right sides.
103 std::shared_ptr<ChBody> tierod_body = (steering == nullptr) ? chassis->GetBody() : steering->GetSteeringLink();
104 InitializeSide(LEFT, chassis, tierod_body, m_pointsL, left_ang_vel);
105 InitializeSide(RIGHT, chassis, tierod_body, m_pointsR, right_ang_vel);
106 }
107
InitializeSide(VehicleSide side,std::shared_ptr<ChChassis> chassis,std::shared_ptr<ChBody> tierod_body,const std::vector<ChVector<>> & points,double ang_vel)108 void ChMacPhersonStrut::InitializeSide(VehicleSide side,
109 std::shared_ptr<ChChassis> chassis,
110 std::shared_ptr<ChBody> tierod_body,
111 const std::vector<ChVector<>>& points,
112 double ang_vel) {
113 std::string suffix = (side == LEFT) ? "_L" : "_R";
114
115 // Chassis orientation (expressed in absolute frame)
116 // Recall that the suspension reference frame is aligned with the chassis.
117 ChQuaternion<> chassisRot = chassis->GetBody()->GetFrame_REF_to_abs().GetRot();
118
119 // Unit vectors for orientation matrices.
120 ChVector<> u;
121 ChVector<> v;
122 ChVector<> w;
123 ChMatrix33<> rot;
124
125 // Create and initialize spindle body (same orientation as the chassis)
126 m_spindle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
127 m_spindle[side]->SetNameString(m_name + "_spindle" + suffix);
128 m_spindle[side]->SetPos(points[SPINDLE]);
129 m_spindle[side]->SetRot(chassisRot);
130 m_spindle[side]->SetWvel_loc(ChVector<>(0, ang_vel, 0));
131 m_spindle[side]->SetMass(getSpindleMass());
132 m_spindle[side]->SetInertiaXX(getSpindleInertia());
133 chassis->GetSystem()->AddBody(m_spindle[side]);
134
135 // Create and initialize upright body (same orientation as the chassis)
136 m_upright[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
137 m_upright[side]->SetNameString(m_name + "_upright" + suffix);
138 m_upright[side]->SetPos(points[UPRIGHT]);
139 m_upright[side]->SetRot(chassisRot);
140 m_upright[side]->SetMass(getUprightMass());
141 m_upright[side]->SetInertiaXX(getUprightInertia());
142 chassis->GetSystem()->AddBody(m_upright[side]);
143
144 // Create and initialize Strut body.
145 m_strut[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
146 m_strut[side]->SetNameString(m_name + "_Strut" + suffix);
147 m_strut[side]->SetPos((points[SPRING_C] + points[SPRING_U]) / 2);
148 m_strut[side]->SetRot(chassisRot);
149 m_strut[side]->SetMass(getStrutMass());
150 m_strut[side]->SetInertiaXX(getStrutInertia());
151 chassis->GetSystem()->AddBody(m_strut[side]);
152
153 // Create and initialize Lower Control Arm body.
154 // Determine the rotation matrix of the LCA, based on the plane of the hard points
155 // (z axis normal to the plane of the LCA)
156 w = Vcross(points[LCA_B] - points[LCA_U], points[LCA_F] - points[LCA_U]);
157 w.Normalize();
158 u = points[LCA_F] - points[LCA_B];
159 u.Normalize();
160 v = Vcross(w, u);
161 rot.Set_A_axis(u, v, w);
162
163 m_LCA[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
164 m_LCA[side]->SetNameString(m_name + "_LCA" + suffix);
165 m_LCA[side]->SetPos(points[LCA_CM]);
166 m_LCA[side]->SetRot(rot);
167 m_LCA[side]->SetMass(getLCAMass());
168 m_LCA[side]->SetInertiaXX(getLCAInertia());
169 chassis->GetSystem()->AddBody(m_LCA[side]);
170
171 // Create and initialize the revolute joint between upright and spindle.
172 ChCoordsys<> rev_csys(points[SPINDLE], chassisRot * Q_from_AngAxis(-CH_C_PI / 2.0, VECT_X));
173 m_revolute[side] = chrono_types::make_shared<ChLinkLockRevolute>();
174 m_revolute[side]->SetNameString(m_name + "_revolute" + suffix);
175 m_revolute[side]->Initialize(m_spindle[side], m_upright[side], rev_csys);
176 chassis->GetSystem()->AddLink(m_revolute[side]);
177
178 // Create and initialize the cylindrical joint between upright and strut.
179 // Determine the joint orientation matrix from the hardpoint locations by
180 // constructing a rotation matrix with the z axis along the joint direction
181 // and the y axis normal to the plane of the UCA.
182 w = points[SPRING_C] - points[SPRING_U];
183 w.Normalize();
184 v = Vcross(points[SPRING_U] - points[LCA_B], points[SPRING_C] - points[LCA_B]);
185 v.Normalize();
186 u = Vcross(v, w);
187 u.Normalize();
188 rot.Set_A_axis(u, v, w);
189
190 m_cylindricalStrut[side] = chrono_types::make_shared<ChLinkLockCylindrical>();
191 m_cylindricalStrut[side]->SetNameString(m_name + "_cylindricalStrut" + suffix);
192 m_cylindricalStrut[side]->Initialize(m_strut[side], m_upright[side],
193 ChCoordsys<>(points[SPRING_U], rot.Get_A_quaternion()));
194 chassis->GetSystem()->AddLink(m_cylindricalStrut[side]);
195
196 // Create and initialize the universal joint between chassis and UCA.
197 //// Determine the joint orientation matrix from the hardpoint locations by
198 //// constructing a rotation matrix with the z axis along the joint direction
199 //// and the y axis normal to the plane of the UCA.
200 // v = Vcross(points[UCA_B] - points[UCA_U], points[UCA_F] - points[UCA_U]);
201 // v.Normalize();
202 // w = points[UCA_F] - points[UCA_B];
203 // w.Normalize();
204 // u = Vcross(v, w);
205 // rot.Set_A_axis(u, v, w);
206 // TODO: Is this the correct rotation matrix?
207 m_universalStrut[side] = chrono_types::make_shared<ChLinkUniversal>();
208 m_universalStrut[side]->SetNameString(m_name + "_universalStrut" + suffix);
209 m_universalStrut[side]->Initialize(chassis->GetBody(), m_strut[side],
210 ChFrame<>(points[SPRING_C], rot.Get_A_quaternion()));
211 chassis->GetSystem()->AddLink(m_universalStrut[side]);
212
213 // Create and initialize the revolute joint between chassis and LCA.
214 // Determine the joint orientation matrix from the hardpoint locations by
215 // constructing a rotation matrix with the z axis along the joint direction
216 // and the y axis normal to the plane of the LCA.
217 v = Vcross(points[LCA_B] - points[LCA_U], points[LCA_F] - points[LCA_U]);
218 v.Normalize();
219 w = points[LCA_F] - points[LCA_B];
220 w.Normalize();
221 u = Vcross(v, w);
222 rot.Set_A_axis(u, v, w);
223
224 m_revoluteLCA[side] = chrono_types::make_shared<ChVehicleJoint>(
225 ChVehicleJoint::Type::REVOLUTE, m_name + "_revoluteLCA" + suffix, chassis->GetBody(), m_LCA[side],
226 ChCoordsys<>((points[LCA_F] + points[LCA_B]) / 2, rot.Get_A_quaternion()), getLCABushingData());
227 chassis->AddJoint(m_revoluteLCA[side]);
228
229 // Create and initialize the spherical joint between upright and LCA.
230 m_sphericalLCA[side] =
231 chrono_types::make_shared<ChVehicleJoint>(ChVehicleJoint::Type::SPHERICAL, m_name + "_sphericalLCA" + suffix,
232 m_LCA[side], m_upright[side], ChCoordsys<>(points[LCA_U], QUNIT));
233 chassis->AddJoint(m_sphericalLCA[side]);
234
235 if (UseTierodBodies()) {
236 // Orientation of tierod body
237 w = (points[TIEROD_U] - points[TIEROD_C]).GetNormalized();
238 u = chassisRot.GetXaxis();
239 v = Vcross(w, u).GetNormalized();
240 u = Vcross(v, w);
241 rot.Set_A_axis(u, v, w);
242
243 // Create the tierod body
244 m_tierod[side] = std::shared_ptr<ChBody>(chassis->GetBody()->GetSystem()->NewBody());
245 m_tierod[side]->SetNameString(m_name + "_tierodBody" + suffix);
246 m_tierod[side]->SetPos((points[TIEROD_U] + points[TIEROD_C]) / 2);
247 m_tierod[side]->SetRot(rot.Get_A_quaternion());
248 m_tierod[side]->SetMass(getTierodMass());
249 m_tierod[side]->SetInertiaXX(getTierodInertia());
250 chassis->GetBody()->GetSystem()->AddBody(m_tierod[side]);
251
252 // Connect tierod body to upright (spherical) and chassis (universal)
253 m_sphericalTierod[side] = chrono_types::make_shared<ChVehicleJoint>(
254 ChVehicleJoint::Type::SPHERICAL, m_name + "_sphericalTierod" + suffix, m_upright[side], m_tierod[side],
255 ChCoordsys<>(points[TIEROD_U], QUNIT), getTierodBushingData());
256 chassis->AddJoint(m_sphericalTierod[side]);
257 m_universalTierod[side] = chrono_types::make_shared<ChVehicleJoint>(
258 ChVehicleJoint::Type::UNIVERSAL, m_name + "_universalTierod" + suffix, tierod_body, m_tierod[side],
259 ChCoordsys<>(points[TIEROD_C], rot.Get_A_quaternion()), getTierodBushingData());
260 chassis->AddJoint(m_universalTierod[side]);
261 } else {
262 // Create and initialize the tierod distance constraint between chassis and upright.
263 m_distTierod[side] = chrono_types::make_shared<ChLinkDistance>();
264 m_distTierod[side]->SetNameString(m_name + "_distTierod" + suffix);
265 m_distTierod[side]->Initialize(tierod_body, m_upright[side], false, points[TIEROD_C], points[TIEROD_U]);
266 chassis->GetSystem()->AddLink(m_distTierod[side]);
267 }
268
269 // Create and initialize the spring/damper
270 m_shock[side] = chrono_types::make_shared<ChLinkTSDA>();
271 m_shock[side]->SetNameString(m_name + "_shock" + suffix);
272 m_shock[side]->Initialize(chassis->GetBody(), m_upright[side], false, points[SHOCK_C], points[SHOCK_U]);
273 m_shock[side]->RegisterForceFunctor(getShockForceFunctor());
274 chassis->GetSystem()->AddLink(m_shock[side]);
275
276 m_spring[side] = chrono_types::make_shared<ChLinkTSDA>();
277 m_spring[side]->SetNameString(m_name + "_spring" + suffix);
278 m_spring[side]->Initialize(chassis->GetBody(), m_upright[side], false, points[SPRING_C], points[SPRING_U], false,
279 getSpringRestLength());
280 m_spring[side]->RegisterForceFunctor(getSpringForceFunctor());
281 chassis->GetSystem()->AddLink(m_spring[side]);
282
283 // Create and initialize the axle shaft and its connection to the spindle. Note that the
284 // spindle rotates about the Y axis.
285 m_axle[side] = chrono_types::make_shared<ChShaft>();
286 m_axle[side]->SetNameString(m_name + "_axle" + suffix);
287 m_axle[side]->SetInertia(getAxleInertia());
288 m_axle[side]->SetPos_dt(-ang_vel);
289 chassis->GetSystem()->Add(m_axle[side]);
290
291 m_axle_to_spindle[side] = chrono_types::make_shared<ChShaftsBody>();
292 m_axle_to_spindle[side]->SetNameString(m_name + "_axle_to_spindle" + suffix);
293 m_axle_to_spindle[side]->Initialize(m_axle[side], m_spindle[side], ChVector<>(0, -1, 0));
294 chassis->GetSystem()->Add(m_axle_to_spindle[side]);
295 }
296
297 // -----------------------------------------------------------------------------
298 // Get the total mass of the suspension subsystem.
299 // -----------------------------------------------------------------------------
GetMass() const300 double ChMacPhersonStrut::GetMass() const {
301 return 2 * (getSpindleMass() + getStrutMass() + getLCAMass() + getUprightMass());
302 }
303
304 // -----------------------------------------------------------------------------
305 // Get the current COM location of the suspension subsystem.
306 // -----------------------------------------------------------------------------
GetCOMPos() const307 ChVector<> ChMacPhersonStrut::GetCOMPos() const {
308 ChVector<> com(0, 0, 0);
309
310 com += getSpindleMass() * m_spindle[LEFT]->GetPos();
311 com += getSpindleMass() * m_spindle[RIGHT]->GetPos();
312
313 com += getStrutMass() * m_strut[LEFT]->GetPos();
314 com += getStrutMass() * m_strut[RIGHT]->GetPos();
315
316 com += getLCAMass() * m_LCA[LEFT]->GetPos();
317 com += getLCAMass() * m_LCA[RIGHT]->GetPos();
318
319 com += getUprightMass() * m_upright[LEFT]->GetPos();
320 com += getUprightMass() * m_upright[RIGHT]->GetPos();
321
322 return com / GetMass();
323 }
324
325 // -----------------------------------------------------------------------------
326 // Get the wheel track using the spindle local position.
327 // -----------------------------------------------------------------------------
GetTrack()328 double ChMacPhersonStrut::GetTrack() {
329 return 2 * getLocation(SPINDLE).y();
330 }
331
332 // -----------------------------------------------------------------------------
333 // Return current suspension forces
334 // -----------------------------------------------------------------------------
ReportSuspensionForce(VehicleSide side) const335 ChSuspension::Force ChMacPhersonStrut::ReportSuspensionForce(VehicleSide side) const {
336 ChSuspension::Force force;
337
338 force.spring_force = m_spring[side]->GetForce();
339 force.spring_length = m_spring[side]->GetLength();
340 force.spring_velocity = m_spring[side]->GetVelocity();
341
342 force.shock_force = m_shock[side]->GetForce();
343 force.shock_length = m_shock[side]->GetLength();
344 force.shock_velocity = m_shock[side]->GetVelocity();
345
346 return force;
347 }
348
349 // -----------------------------------------------------------------------------
350 // -----------------------------------------------------------------------------
LogHardpointLocations(const ChVector<> & ref,bool inches)351 void ChMacPhersonStrut::LogHardpointLocations(const ChVector<>& ref, bool inches) {
352 double unit = inches ? 1 / 0.0254 : 1.0;
353
354 for (int i = 0; i < NUM_POINTS; i++) {
355 ChVector<> pos = ref + unit * getLocation(static_cast<PointId>(i));
356
357 GetLog() << " " << m_pointNames[i].c_str() << " " << pos.x() << " " << pos.y() << " " << pos.z() << "\n";
358 }
359 }
360
361 // -----------------------------------------------------------------------------
362 // -----------------------------------------------------------------------------
LogConstraintViolations(VehicleSide side)363 void ChMacPhersonStrut::LogConstraintViolations(VehicleSide side) {
364 // Revolute joints
365 {
366 ChVectorDynamic<> C = m_revoluteLCA[side]->GetConstraintViolation();
367 GetLog() << "LCA revolute ";
368 GetLog() << " " << C(0) << " ";
369 GetLog() << " " << C(1) << " ";
370 GetLog() << " " << C(2) << " ";
371 GetLog() << " " << C(3) << " ";
372 GetLog() << " " << C(4) << "\n";
373 }
374 {
375 ChVectorDynamic<> C = m_revolute[side]->GetConstraintViolation();
376 GetLog() << "Spindle revolute ";
377 GetLog() << " " << C(0) << " ";
378 GetLog() << " " << C(1) << " ";
379 GetLog() << " " << C(2) << " ";
380 GetLog() << " " << C(3) << " ";
381 GetLog() << " " << C(4) << "\n";
382 }
383
384 // Spherical joints
385 {
386 ChVectorDynamic<> C = m_sphericalLCA[side]->GetConstraintViolation();
387 GetLog() << "LCA spherical ";
388 GetLog() << " " << C(0) << " ";
389 GetLog() << " " << C(1) << " ";
390 GetLog() << " " << C(2) << "\n";
391 }
392
393 // Universal joints
394 {
395 ChVectorDynamic<> C = m_universalStrut[side]->GetConstraintViolation();
396 GetLog() << "Strut universal ";
397 GetLog() << " " << C(0) << " ";
398 GetLog() << " " << C(1) << " ";
399 GetLog() << " " << C(2) << "\n";
400 GetLog() << " " << C(3) << "\n";
401 }
402
403 // Cylindrical joints
404 {
405 ChVectorDynamic<> C = m_cylindricalStrut[side]->GetConstraintViolation();
406 GetLog() << "Strut cylindrical ";
407 GetLog() << " " << C(0) << " ";
408 GetLog() << " " << C(1) << " ";
409 GetLog() << " " << C(2) << "\n";
410 GetLog() << " " << C(3) << "\n";
411 }
412
413 // Tierod constraint
414 if (UseTierodBodies()) {
415 {
416 const auto& C = m_sphericalTierod[side]->GetConstraintViolation();
417 GetLog() << "Tierod spherical ";
418 GetLog() << " " << C(0) << " ";
419 GetLog() << " " << C(1) << " ";
420 GetLog() << " " << C(2) << "\n";
421 }
422 {
423 const auto& C = m_universalTierod[side]->GetConstraintViolation();
424 GetLog() << "Tierod universal ";
425 GetLog() << " " << C(0) << " ";
426 GetLog() << " " << C(1) << " ";
427 GetLog() << " " << C(2) << "\n";
428 GetLog() << " " << C(3) << "\n";
429 }
430 } else {
431 GetLog() << "Tierod distance ";
432 GetLog() << " " << m_distTierod[side]->GetCurrentDistance() - m_distTierod[side]->GetImposedDistance() << "\n";
433 }
434 }
435
436 // -----------------------------------------------------------------------------
437 // -----------------------------------------------------------------------------
AddVisualizationAssets(VisualizationType vis)438 void ChMacPhersonStrut::AddVisualizationAssets(VisualizationType vis) {
439 ChSuspension::AddVisualizationAssets(vis);
440
441 if (vis == VisualizationType::NONE)
442 return;
443
444 AddVisualizationUpright(m_upright[LEFT], 0.5 * (m_pointsL[SPINDLE] + m_pointsL[UPRIGHT]), m_pointsL[SPRING_U],
445 m_pointsL[LCA_U], m_pointsL[TIEROD_U], getUprightRadius());
446 AddVisualizationUpright(m_upright[RIGHT], 0.5 * (m_pointsR[SPINDLE] + m_pointsR[UPRIGHT]), m_pointsR[SPRING_U],
447 m_pointsR[LCA_U], m_pointsR[TIEROD_U], getUprightRadius());
448
449 AddVisualizationStrut(m_strut[LEFT], m_pointsL[SPRING_C], m_pointsL[SPRING_U], getStrutRadius());
450 AddVisualizationStrut(m_strut[RIGHT], m_pointsR[SPRING_C], m_pointsR[SPRING_U], getStrutRadius());
451
452 AddVisualizationControlArm(m_LCA[LEFT], m_pointsL[LCA_F], m_pointsL[LCA_B], m_pointsL[LCA_U], getLCARadius());
453 AddVisualizationControlArm(m_LCA[RIGHT], m_pointsR[LCA_F], m_pointsR[LCA_B], m_pointsR[LCA_U], getLCARadius());
454
455 // Add visualization for the springs and shocks
456 m_spring[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
457 m_spring[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
458
459 m_shock[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
460 m_shock[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
461
462 // Add visualization for the tie-rods
463 if (UseTierodBodies()) {
464 AddVisualizationTierod(m_tierod[LEFT], m_pointsL[TIEROD_C], m_pointsL[TIEROD_U], getTierodRadius());
465 AddVisualizationTierod(m_tierod[RIGHT], m_pointsR[TIEROD_C], m_pointsR[TIEROD_U], getTierodRadius());
466 } else {
467 m_distTierod[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
468 m_distTierod[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
469 m_distTierod[LEFT]->AddAsset(chrono_types::make_shared<ChColorAsset>(0.8f, 0.3f, 0.3f));
470 m_distTierod[RIGHT]->AddAsset(chrono_types::make_shared<ChColorAsset>(0.8f, 0.3f, 0.3f));
471 }
472 }
473
RemoveVisualizationAssets()474 void ChMacPhersonStrut::RemoveVisualizationAssets() {
475 ChSuspension::RemoveVisualizationAssets();
476
477 m_upright[LEFT]->GetAssets().clear();
478 m_upright[RIGHT]->GetAssets().clear();
479
480 m_strut[LEFT]->GetAssets().clear();
481 m_strut[RIGHT]->GetAssets().clear();
482
483 m_LCA[LEFT]->GetAssets().clear();
484 m_LCA[RIGHT]->GetAssets().clear();
485
486 m_spring[LEFT]->GetAssets().clear();
487 m_spring[RIGHT]->GetAssets().clear();
488
489 m_shock[LEFT]->GetAssets().clear();
490 m_shock[RIGHT]->GetAssets().clear();
491
492 if (UseTierodBodies()) {
493 m_tierod[LEFT]->GetAssets().clear();
494 m_tierod[RIGHT]->GetAssets().clear();
495 } else {
496 m_distTierod[LEFT]->GetAssets().clear();
497 m_distTierod[RIGHT]->GetAssets().clear();
498 }
499 }
500
501 // -----------------------------------------------------------------------------
502 // -----------------------------------------------------------------------------
AddVisualizationStrut(std::shared_ptr<ChBody> strut,const ChVector<> pt_c,const ChVector<> pt_u,double radius)503 void ChMacPhersonStrut::AddVisualizationStrut(std::shared_ptr<ChBody> strut,
504 const ChVector<> pt_c,
505 const ChVector<> pt_u,
506 double radius) {
507 // Express hardpoint locations in body frame.
508 ChVector<> p_c = strut->TransformPointParentToLocal(pt_c);
509 ChVector<> p_u = strut->TransformPointParentToLocal(pt_u);
510
511 auto cyl = chrono_types::make_shared<ChCylinderShape>();
512 cyl->GetCylinderGeometry().p1 = p_c;
513 cyl->GetCylinderGeometry().p2 = p_u;
514 cyl->GetCylinderGeometry().rad = radius;
515 strut->AddAsset(cyl);
516
517 auto col = chrono_types::make_shared<ChColorAsset>();
518 col->SetColor(ChColor(0.7f, 0.7f, 0.7f));
519 strut->AddAsset(col);
520 }
521
AddVisualizationControlArm(std::shared_ptr<ChBody> arm,const ChVector<> pt_F,const ChVector<> pt_B,const ChVector<> pt_U,double radius)522 void ChMacPhersonStrut::AddVisualizationControlArm(std::shared_ptr<ChBody> arm,
523 const ChVector<> pt_F,
524 const ChVector<> pt_B,
525 const ChVector<> pt_U,
526 double radius) {
527 // Express hardpoint locations in body frame.
528 ChVector<> p_F = arm->TransformPointParentToLocal(pt_F);
529 ChVector<> p_B = arm->TransformPointParentToLocal(pt_B);
530 ChVector<> p_U = arm->TransformPointParentToLocal(pt_U);
531
532 auto cyl_F = chrono_types::make_shared<ChCylinderShape>();
533 cyl_F->GetCylinderGeometry().p1 = p_F;
534 cyl_F->GetCylinderGeometry().p2 = p_U;
535 cyl_F->GetCylinderGeometry().rad = radius;
536 arm->AddAsset(cyl_F);
537
538 auto cyl_B = chrono_types::make_shared<ChCylinderShape>();
539 cyl_B->GetCylinderGeometry().p1 = p_B;
540 cyl_B->GetCylinderGeometry().p2 = p_U;
541 cyl_B->GetCylinderGeometry().rad = radius;
542 arm->AddAsset(cyl_B);
543
544 auto col = chrono_types::make_shared<ChColorAsset>();
545 col->SetColor(ChColor(0.7f, 0.7f, 0.7f));
546 arm->AddAsset(col);
547 }
548
AddVisualizationUpright(std::shared_ptr<ChBody> upright,const ChVector<> pt_C,const ChVector<> pt_U,const ChVector<> pt_L,const ChVector<> pt_T,double radius)549 void ChMacPhersonStrut::AddVisualizationUpright(std::shared_ptr<ChBody> upright,
550 const ChVector<> pt_C,
551 const ChVector<> pt_U,
552 const ChVector<> pt_L,
553 const ChVector<> pt_T,
554 double radius) {
555 static const double threshold2 = 1e-6;
556
557 // Express hardpoint locations in body frame.
558 ChVector<> p_C = upright->TransformPointParentToLocal(pt_C);
559 ChVector<> p_U = upright->TransformPointParentToLocal(pt_U);
560 ChVector<> p_L = upright->TransformPointParentToLocal(pt_L);
561 ChVector<> p_T = upright->TransformPointParentToLocal(pt_T);
562
563 if ((p_L - p_C).Length2() > threshold2) {
564 auto cyl_L = chrono_types::make_shared<ChCylinderShape>();
565 cyl_L->GetCylinderGeometry().p1 = p_L;
566 cyl_L->GetCylinderGeometry().p2 = p_C;
567 cyl_L->GetCylinderGeometry().rad = radius;
568 upright->AddAsset(cyl_L);
569 }
570
571 if ((p_U - p_C).Length2() > threshold2) {
572 auto cyl_U = chrono_types::make_shared<ChCylinderShape>();
573 cyl_U->GetCylinderGeometry().p1 = p_U;
574 cyl_U->GetCylinderGeometry().p2 = p_C;
575 cyl_U->GetCylinderGeometry().rad = radius;
576 upright->AddAsset(cyl_U);
577 }
578
579 if ((p_T - p_C).Length2() > threshold2) {
580 auto cyl_T = chrono_types::make_shared<ChCylinderShape>();
581 cyl_T->GetCylinderGeometry().p1 = p_T;
582 cyl_T->GetCylinderGeometry().p2 = p_C;
583 cyl_T->GetCylinderGeometry().rad = radius;
584 upright->AddAsset(cyl_T);
585 }
586
587 auto col = chrono_types::make_shared<ChColorAsset>();
588 col->SetColor(ChColor(0.2f, 0.2f, 0.6f));
589 upright->AddAsset(col);
590 }
591
AddVisualizationTierod(std::shared_ptr<ChBody> tierod,const ChVector<> pt_C,const ChVector<> pt_U,double radius)592 void ChMacPhersonStrut::AddVisualizationTierod(std::shared_ptr<ChBody> tierod,
593 const ChVector<> pt_C,
594 const ChVector<> pt_U,
595 double radius) {
596 // Express hardpoint locations in body frame.
597 ChVector<> p_C = tierod->TransformPointParentToLocal(pt_C);
598 ChVector<> p_U = tierod->TransformPointParentToLocal(pt_U);
599
600 auto cyl = chrono_types::make_shared<ChCylinderShape>();
601 cyl->GetCylinderGeometry().p1 = p_C;
602 cyl->GetCylinderGeometry().p2 = p_U;
603 cyl->GetCylinderGeometry().rad = radius;
604 tierod->AddAsset(cyl);
605
606 tierod->AddAsset(chrono_types::make_shared<ChColorAsset>(0.8f, 0.3f, 0.3f));
607 }
608
609 // -----------------------------------------------------------------------------
610 // -----------------------------------------------------------------------------
ExportComponentList(rapidjson::Document & jsonDocument) const611 void ChMacPhersonStrut::ExportComponentList(rapidjson::Document& jsonDocument) const {
612 ChPart::ExportComponentList(jsonDocument);
613
614 std::vector<std::shared_ptr<ChBody>> bodies;
615 bodies.push_back(m_spindle[0]);
616 bodies.push_back(m_spindle[1]);
617 bodies.push_back(m_upright[0]);
618 bodies.push_back(m_upright[1]);
619 bodies.push_back(m_strut[0]);
620 bodies.push_back(m_strut[1]);
621 bodies.push_back(m_LCA[0]);
622 bodies.push_back(m_LCA[1]);
623 if (UseTierodBodies()) {
624 bodies.push_back(m_tierod[0]);
625 bodies.push_back(m_tierod[1]);
626 }
627 ChPart::ExportBodyList(jsonDocument, bodies);
628
629 std::vector<std::shared_ptr<ChShaft>> shafts;
630 shafts.push_back(m_axle[0]);
631 shafts.push_back(m_axle[1]);
632 ChPart::ExportShaftList(jsonDocument, shafts);
633
634 std::vector<std::shared_ptr<ChLink>> joints;
635 std::vector<std::shared_ptr<ChLoadBodyBody>> bushings;
636 joints.push_back(m_revolute[0]);
637 joints.push_back(m_revolute[1]);
638 joints.push_back(m_cylindricalStrut[0]);
639 joints.push_back(m_cylindricalStrut[1]);
640 joints.push_back(m_universalStrut[0]);
641 joints.push_back(m_universalStrut[1]);
642 m_revoluteLCA[0]->IsKinematic() ? joints.push_back(m_revoluteLCA[0]->GetAsLink())
643 : bushings.push_back(m_revoluteLCA[0]->GetAsBushing());
644 m_revoluteLCA[1]->IsKinematic() ? joints.push_back(m_revoluteLCA[1]->GetAsLink())
645 : bushings.push_back(m_revoluteLCA[1]->GetAsBushing());
646 m_sphericalLCA[0]->IsKinematic() ? joints.push_back(m_sphericalLCA[0]->GetAsLink())
647 : bushings.push_back(m_sphericalLCA[0]->GetAsBushing());
648 m_sphericalLCA[1]->IsKinematic() ? joints.push_back(m_sphericalLCA[1]->GetAsLink())
649 : bushings.push_back(m_sphericalLCA[1]->GetAsBushing());
650 if (UseTierodBodies()) {
651 m_sphericalTierod[0]->IsKinematic() ? joints.push_back(m_sphericalTierod[0]->GetAsLink())
652 : bushings.push_back(m_sphericalTierod[0]->GetAsBushing());
653 m_sphericalTierod[1]->IsKinematic() ? joints.push_back(m_sphericalTierod[1]->GetAsLink())
654 : bushings.push_back(m_sphericalTierod[1]->GetAsBushing());
655 m_universalTierod[0]->IsKinematic() ? joints.push_back(m_universalTierod[0]->GetAsLink())
656 : bushings.push_back(m_universalTierod[0]->GetAsBushing());
657 m_universalTierod[1]->IsKinematic() ? joints.push_back(m_universalTierod[1]->GetAsLink())
658 : bushings.push_back(m_universalTierod[1]->GetAsBushing());
659 } else {
660 joints.push_back(m_distTierod[0]);
661 joints.push_back(m_distTierod[1]);
662 }
663 ChPart::ExportJointList(jsonDocument, joints);
664 ChPart::ExportBodyLoadList(jsonDocument, bushings);
665
666 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
667 springs.push_back(m_spring[0]);
668 springs.push_back(m_spring[1]);
669 springs.push_back(m_shock[0]);
670 springs.push_back(m_shock[1]);
671 ChPart::ExportLinSpringList(jsonDocument, springs);
672 }
673
Output(ChVehicleOutput & database) const674 void ChMacPhersonStrut::Output(ChVehicleOutput& database) const {
675 if (!m_output)
676 return;
677
678 std::vector<std::shared_ptr<ChBody>> bodies;
679 bodies.push_back(m_spindle[0]);
680 bodies.push_back(m_spindle[1]);
681 bodies.push_back(m_upright[0]);
682 bodies.push_back(m_upright[1]);
683 bodies.push_back(m_strut[0]);
684 bodies.push_back(m_strut[1]);
685 bodies.push_back(m_LCA[0]);
686 bodies.push_back(m_LCA[1]);
687 if (UseTierodBodies()) {
688 bodies.push_back(m_tierod[0]);
689 bodies.push_back(m_tierod[1]);
690 }
691 database.WriteBodies(bodies);
692
693 std::vector<std::shared_ptr<ChShaft>> shafts;
694 shafts.push_back(m_axle[0]);
695 shafts.push_back(m_axle[1]);
696 database.WriteShafts(shafts);
697
698 std::vector<std::shared_ptr<ChLink>> joints;
699 std::vector<std::shared_ptr<ChLoadBodyBody>> bushings;
700 joints.push_back(m_revolute[0]);
701 joints.push_back(m_revolute[1]);
702 joints.push_back(m_cylindricalStrut[0]);
703 joints.push_back(m_cylindricalStrut[1]);
704 joints.push_back(m_universalStrut[0]);
705 joints.push_back(m_universalStrut[1]);
706 m_revoluteLCA[0]->IsKinematic() ? joints.push_back(m_revoluteLCA[0]->GetAsLink())
707 : bushings.push_back(m_revoluteLCA[0]->GetAsBushing());
708 m_revoluteLCA[1]->IsKinematic() ? joints.push_back(m_revoluteLCA[1]->GetAsLink())
709 : bushings.push_back(m_revoluteLCA[1]->GetAsBushing());
710 m_sphericalLCA[0]->IsKinematic() ? joints.push_back(m_sphericalLCA[0]->GetAsLink())
711 : bushings.push_back(m_sphericalLCA[0]->GetAsBushing());
712 m_sphericalLCA[1]->IsKinematic() ? joints.push_back(m_sphericalLCA[1]->GetAsLink())
713 : bushings.push_back(m_sphericalLCA[1]->GetAsBushing());
714 if (UseTierodBodies()) {
715 m_sphericalTierod[0]->IsKinematic() ? joints.push_back(m_sphericalTierod[0]->GetAsLink())
716 : bushings.push_back(m_sphericalTierod[0]->GetAsBushing());
717 m_sphericalTierod[1]->IsKinematic() ? joints.push_back(m_sphericalTierod[1]->GetAsLink())
718 : bushings.push_back(m_sphericalTierod[1]->GetAsBushing());
719 m_universalTierod[0]->IsKinematic() ? joints.push_back(m_universalTierod[0]->GetAsLink())
720 : bushings.push_back(m_universalTierod[0]->GetAsBushing());
721 m_universalTierod[1]->IsKinematic() ? joints.push_back(m_universalTierod[1]->GetAsLink())
722 : bushings.push_back(m_universalTierod[1]->GetAsBushing());
723 } else {
724 joints.push_back(m_distTierod[0]);
725 joints.push_back(m_distTierod[1]);
726 }
727 database.WriteJoints(joints);
728 database.WriteBodyLoads(bushings);
729
730 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
731 springs.push_back(m_spring[0]);
732 springs.push_back(m_spring[1]);
733 springs.push_back(m_shock[0]);
734 springs.push_back(m_shock[1]);
735 database.WriteLinSprings(springs);
736 }
737
738 } // end namespace vehicle
739 } // end namespace chrono
740