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: Rainer Gericke, Radu Serban
13 // =============================================================================
14 //
15 // Base class for solid axle suspension with triangular and longitudinal guides.
16 //
17 //
18 // The suspension subsystem is modeled with respect to a right-handed frame,
19 // with X pointing towards the front, Y to the left, and Z up (ISO standard).
20 // The suspension reference frame is assumed to be always aligned with that of
21 // the vehicle. When attached to a chassis, only an offset is provided.
22 //
23 // All point locations are assumed to be given for the left half of the
24 // suspension and will be mirrored (reflecting the y coordinates) to construct
25 // the right side.
26 //
27 // =============================================================================
28
29 #include "chrono/assets/ChColorAsset.h"
30 #include "chrono/assets/ChCylinderShape.h"
31 #include "chrono/assets/ChPointPointDrawing.h"
32
33 #include "chrono_vehicle/wheeled_vehicle/suspension/ChSolidBellcrankThreeLinkAxle.h"
34
35 namespace chrono {
36 namespace vehicle {
37
38 // -----------------------------------------------------------------------------
39 // Static variables
40 // -----------------------------------------------------------------------------
41 const std::string ChSolidBellcrankThreeLinkAxle::m_pointNames[] = {
42 "SHOCK_A ", "SHOCK_C ", "SPRING_A ", "SPRING_C ", "SPINDLE ", "TRIANGLE_A ",
43 "TRIANGLE_C ", "LINK_A ", "LINK_C ", "BELLCRANK_A", "BELLCRANK_D", "BELLCRANK_T",
44 "DRAGLINK_S ", "KNUCKLE_L ", "KNUCKLE_U ", "KNUCKLE_T ", "KNUCKLE_CM "};
45
46 // -----------------------------------------------------------------------------
47 // -----------------------------------------------------------------------------
ChSolidBellcrankThreeLinkAxle(const std::string & name)48 ChSolidBellcrankThreeLinkAxle::ChSolidBellcrankThreeLinkAxle(const std::string& name) : ChSuspension(name) {}
49
~ChSolidBellcrankThreeLinkAxle()50 ChSolidBellcrankThreeLinkAxle::~ChSolidBellcrankThreeLinkAxle() {
51 auto sys = m_axleTube->GetSystem();
52 if (sys) {
53 sys->Remove(m_axleTube);
54 sys->Remove(m_bellcrank);
55 sys->Remove(m_draglink);
56
57 sys->Remove(m_revBellcrank);
58 sys->Remove(m_sphericalDraglink);
59 sys->Remove(m_universalDraglink);
60
61 sys->Remove(m_triangleBody);
62 sys->Remove(m_triangleRev);
63 sys->Remove(m_triangleSph);
64
65 for (int i = 0; i < 2; i++) {
66 sys->Remove(m_knuckle[i]);
67 sys->Remove(m_revKingpin[i]);
68 sys->Remove(m_linkBody[i]);
69 sys->Remove(m_linkBodyToChassis[i]);
70 sys->Remove(m_linkBodyToAxleTube[i]);
71 sys->Remove(m_tierodBody[i]);
72 sys->Remove(m_tierodBodyToKnuckle[i]);
73 sys->Remove(m_tierodBodyToBellcrank[i]);
74
75 sys->Remove(m_shock[i]);
76 sys->Remove(m_spring[i]);
77 }
78 }
79 }
80
81 // -----------------------------------------------------------------------------
82 // -----------------------------------------------------------------------------
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)83 void ChSolidBellcrankThreeLinkAxle::Initialize(std::shared_ptr<ChChassis> chassis,
84 std::shared_ptr<ChSubchassis> subchassis,
85 std::shared_ptr<ChSteering> steering,
86 const ChVector<>& location,
87 double left_ang_vel,
88 double right_ang_vel) {
89 m_location = location;
90
91 // Unit vectors for orientation matrices.
92 ChVector<> u;
93 ChVector<> v;
94 ChVector<> w;
95 ChMatrix33<> rot;
96
97 // Express the suspension reference frame in the absolute coordinate system.
98 ChFrame<> suspension_to_abs(location);
99 suspension_to_abs.ConcatenatePreTransformation(chassis->GetBody()->GetFrame_REF_to_abs());
100
101 // Transform the location of the axle body COM to absolute frame.
102 ChVector<> axleCOM_local = getAxleTubeCOM();
103 ChVector<> axleCOM = suspension_to_abs.TransformLocalToParent(axleCOM_local);
104
105 // Calculate end points on the axle body, expressed in the absolute frame
106 // (for visualization)
107 ChVector<> outer_local((getLocation(KNUCKLE_U) + getLocation(KNUCKLE_L)) / 2);
108 m_axleOuterL = suspension_to_abs.TransformPointLocalToParent(outer_local);
109 outer_local.y() = -outer_local.y();
110 m_axleOuterR = suspension_to_abs.TransformPointLocalToParent(outer_local);
111
112 // Create and initialize the axle body.
113 m_axleTube = std::shared_ptr<ChBody>(chassis->GetBody()->GetSystem()->NewBody());
114 m_axleTube->SetNameString(m_name + "_axleTube");
115 m_axleTube->SetPos(axleCOM);
116 m_axleTube->SetRot(chassis->GetBody()->GetFrame_REF_to_abs().GetRot());
117 m_axleTube->SetMass(getAxleTubeMass());
118 m_axleTube->SetInertiaXX(getAxleTubeInertia());
119 chassis->GetBody()->GetSystem()->AddBody(m_axleTube);
120
121 // Transform all hardpoints to absolute frame.
122 m_pointsL.resize(NUM_POINTS);
123 m_pointsR.resize(NUM_POINTS);
124 for (int i = 0; i < NUM_POINTS; i++) {
125 ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
126 m_pointsL[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
127 rel_pos.y() = -rel_pos.y();
128 m_pointsR[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
129 }
130
131 m_tierodInnerL = m_pointsL[BELLCRANK_T];
132 m_tierodInnerR = m_pointsR[BELLCRANK_T];
133 m_tierodOuterL = m_pointsL[KNUCKLE_T];
134 m_tierodOuterR = m_pointsR[KNUCKLE_T];
135
136 // Fix the triangle body to the axle tube body
137 ChVector<> pt_tri_axle = (m_pointsL[TRIANGLE_A] + m_pointsR[TRIANGLE_A]) / 2.0;
138 ChVector<> pt_tri_chassis = (m_pointsL[TRIANGLE_C] + m_pointsR[TRIANGLE_C]) / 2.0;
139 ChVector<> pt_tri_cog = (pt_tri_axle + pt_tri_chassis) / 2.0;
140 m_triangle_left_point = m_pointsL[TRIANGLE_C];
141 m_triangle_right_point = m_pointsR[TRIANGLE_C];
142 m_triangle_sph_point = pt_tri_axle;
143
144 // generate triangle body
145 m_triangleBody = std::shared_ptr<ChBody>(chassis->GetBody()->GetSystem()->NewBody());
146 m_triangleBody->SetNameString(m_name + "_triangleGuide");
147 m_triangleBody->SetPos(pt_tri_cog);
148 m_triangleBody->SetRot(chassis->GetBody()->GetFrame_REF_to_abs().GetRot());
149 m_triangleBody->SetMass(getTriangleMass());
150 m_triangleBody->SetInertiaXX(getTriangleInertia());
151 chassis->GetBody()->GetSystem()->AddBody(m_triangleBody);
152
153 // Create and initialize the revolute joint between chassis and triangle.
154 ChCoordsys<> rev_csys(pt_tri_chassis, chassis->GetBody()->GetFrame_REF_to_abs().GetRot() * Q_from_AngX(CH_C_PI_2));
155 m_triangleRev = chrono_types::make_shared<ChLinkLockRevolute>();
156 m_triangleRev->SetNameString(m_name + "_revoluteTriangle");
157 m_triangleRev->Initialize(m_triangleBody, chassis->GetBody(), rev_csys);
158 chassis->GetBody()->GetSystem()->AddLink(m_triangleRev);
159
160 // Create and initialize the spherical joint between axle tube and triangle.
161 ChCoordsys<> sph_csys(pt_tri_axle, chassis->GetBody()->GetFrame_REF_to_abs().GetRot());
162 m_triangleSph = chrono_types::make_shared<ChLinkLockSpherical>();
163 m_triangleSph->SetNameString(m_name + "_sphericalTriangle");
164 m_triangleSph->Initialize(m_triangleBody, m_axleTube, sph_csys);
165 chassis->GetBody()->GetSystem()->AddLink(m_triangleSph);
166
167 m_link_axleL = m_pointsL[LINK_A];
168 m_link_axleR = m_pointsR[LINK_A];
169 m_link_chassisL = m_pointsL[LINK_C];
170 m_link_chassisR = m_pointsR[LINK_C];
171
172 // Initialize left and right sides.
173 std::shared_ptr<ChBody> tierod_body = (steering == nullptr) ? chassis->GetBody() : steering->GetSteeringLink();
174 InitializeSide(LEFT, chassis->GetBody(), tierod_body, m_pointsL, left_ang_vel);
175 InitializeSide(RIGHT, chassis->GetBody(), tierod_body, m_pointsR, right_ang_vel);
176 }
177
InitializeSide(VehicleSide side,std::shared_ptr<ChBodyAuxRef> chassis,std::shared_ptr<ChBody> tierod_body,const std::vector<ChVector<>> & points,double ang_vel)178 void ChSolidBellcrankThreeLinkAxle::InitializeSide(VehicleSide side,
179 std::shared_ptr<ChBodyAuxRef> chassis,
180 std::shared_ptr<ChBody> tierod_body,
181 const std::vector<ChVector<>>& points,
182 double ang_vel) {
183 std::string suffix = (side == LEFT) ? "_L" : "_R";
184
185 // Unit vectors for orientation matrices.
186 ChVector<> u;
187 ChVector<> v;
188 ChVector<> w;
189 ChMatrix33<> rot;
190
191 // Chassis orientation (expressed in absolute frame)
192 // Recall that the suspension reference frame is aligned with the chassis.
193 ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();
194
195 // Create and initialize knuckle body (same orientation as the chassis)
196 m_knuckle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
197 m_knuckle[side]->SetNameString(m_name + "_knuckle" + suffix);
198 m_knuckle[side]->SetPos(points[KNUCKLE_CM]);
199 m_knuckle[side]->SetRot(chassisRot);
200 m_knuckle[side]->SetMass(getKnuckleMass());
201 m_knuckle[side]->SetInertiaXX(getKnuckleInertia());
202 chassis->GetSystem()->AddBody(m_knuckle[side]);
203
204 // Create and initialize spindle body (same orientation as the chassis)
205 m_spindle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
206 m_spindle[side]->SetNameString(m_name + "_spindle" + suffix);
207 m_spindle[side]->SetPos(points[SPINDLE]);
208 m_spindle[side]->SetRot(chassisRot);
209 m_spindle[side]->SetWvel_loc(ChVector<>(0, ang_vel, 0));
210 m_spindle[side]->SetMass(getSpindleMass());
211 m_spindle[side]->SetInertiaXX(getSpindleInertia());
212 chassis->GetSystem()->AddBody(m_spindle[side]);
213
214 // Create and initialize the revolute joint between axle and knuckle.
215 // Determine the joint orientation matrix from the hardpoint locations by
216 // constructing a rotation matrix with the z axis along the joint direction.
217 w = points[KNUCKLE_L] - points[KNUCKLE_U];
218 w.Normalize();
219 u = Vcross(points[KNUCKLE_U] - points[SPINDLE], points[KNUCKLE_L] - points[SPINDLE]);
220 u.Normalize();
221 v = Vcross(w, u);
222 rot.Set_A_axis(u, v, w);
223
224 m_revKingpin[side] = chrono_types::make_shared<ChLinkLockRevolute>();
225 m_revKingpin[side]->SetNameString(m_name + "_revoluteKingpin" + suffix);
226 m_revKingpin[side]->Initialize(m_axleTube, m_knuckle[side],
227 ChCoordsys<>((points[KNUCKLE_U] + points[KNUCKLE_L]) / 2, rot.Get_A_quaternion()));
228 chassis->GetSystem()->AddLink(m_revKingpin[side]);
229
230 // Create and initialize the revolute joint between knuckle and spindle.
231 ChCoordsys<> rev_csys(points[SPINDLE], chassisRot * Q_from_AngAxis(CH_C_PI / 2.0, VECT_X));
232 m_revolute[side] = chrono_types::make_shared<ChLinkLockRevolute>();
233 m_revolute[side]->SetNameString(m_name + "_revoluteSpindle" + suffix);
234 m_revolute[side]->Initialize(m_spindle[side], m_knuckle[side], rev_csys);
235 chassis->GetSystem()->AddLink(m_revolute[side]);
236
237 // Create and initialize the spring/damper
238 m_shock[side] = chrono_types::make_shared<ChLinkTSDA>();
239 m_shock[side]->SetNameString(m_name + "_shock" + suffix);
240 m_shock[side]->Initialize(chassis, m_axleTube, false, points[SHOCK_C], points[SHOCK_A]);
241 m_shock[side]->RegisterForceFunctor(getShockForceFunctor());
242 chassis->GetSystem()->AddLink(m_shock[side]);
243
244 m_spring[side] = chrono_types::make_shared<ChLinkTSDA>();
245 m_spring[side]->SetNameString(m_name + "_spring" + suffix);
246 m_spring[side]->Initialize(chassis, m_axleTube, false, points[SPRING_C], points[SPRING_A], false,
247 getSpringRestLength());
248 m_spring[side]->RegisterForceFunctor(getSpringForceFunctor());
249 chassis->GetSystem()->AddLink(m_spring[side]);
250
251 // Create and initialize the axle shaft and its connection to the spindle. Note that the
252 // spindle rotates about the Y axis.
253 m_axle[side] = chrono_types::make_shared<ChShaft>();
254 m_axle[side]->SetNameString(m_name + "_axle" + suffix);
255 m_axle[side]->SetInertia(getAxleInertia());
256 m_axle[side]->SetPos_dt(-ang_vel);
257 chassis->GetSystem()->Add(m_axle[side]);
258
259 m_axle_to_spindle[side] = chrono_types::make_shared<ChShaftsBody>();
260 m_axle_to_spindle[side]->SetNameString(m_name + "_axle_to_spindle" + suffix);
261 m_axle_to_spindle[side]->Initialize(m_axle[side], m_spindle[side], ChVector<>(0, -1, 0));
262 chassis->GetSystem()->Add(m_axle_to_spindle[side]);
263
264 if (side == LEFT) {
265 // create bellcrank body
266 m_bellcrank = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
267 m_bellcrank->SetNameString(m_name + "m_bellcrank");
268 m_bellcrank->SetPos(points[BELLCRANK_A]);
269 m_bellcrank->SetRot(chassis->GetRot() * ChQuaternion<>(1, 0, 0, 0));
270 m_bellcrank->SetMass(getBellcrankMass());
271 m_bellcrank->SetInertiaXX(getBellcrankInertia());
272 chassis->GetSystem()->AddBody(m_bellcrank);
273
274 // create bellcrank rev joint on the axleTube
275 rev_csys = ChCoordsys<>(points[BELLCRANK_A], chassis->GetRot() * ChQuaternion<>(1, 0, 0, 0));
276 m_revBellcrank = chrono_types::make_shared<ChLinkLockRevolute>();
277 m_revBellcrank->SetNameString(m_name + "m_revBellcrank");
278 m_revBellcrank->Initialize(m_axleTube, m_bellcrank, rev_csys);
279 chassis->GetSystem()->AddLink(m_revBellcrank);
280
281 // create draglink body
282 m_draglink = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
283 m_draglink->SetNameString(m_name + "m_draglink");
284 m_draglink->SetPos((points[DRAGLINK_S] + points[BELLCRANK_D]) / 2.0);
285 m_draglink->SetRot(chassis->GetRot() * ChQuaternion<>(1, 0, 0, 0));
286 m_draglink->SetMass(getDraglinkMass());
287 m_draglink->SetInertiaXX(getDraglinkInertia());
288 chassis->GetSystem()->Add(m_draglink);
289
290 // Create and initialize the spherical joint between steering mechanism and draglink.
291 m_sphericalDraglink = chrono_types::make_shared<ChLinkLockSpherical>();
292 m_sphericalDraglink->SetNameString(m_name + "_sphericalDraglink");
293 m_sphericalDraglink->Initialize(m_draglink, tierod_body, ChCoordsys<>(points[DRAGLINK_S], QUNIT));
294 // m_sphericalDraglink->Initialize(m_draglink, chassis, ChCoordsys<>(points[DRAGLINK_C], QUNIT));
295 chassis->GetSystem()->AddLink(m_sphericalDraglink);
296
297 // Create and initialize the draglink body (one side only).
298 // Determine the rotation matrix of the draglink based on the plane of the hard points
299 // (z-axis along the length of the draglink)
300 v = Vcross(points[DRAGLINK_S] - points[BELLCRANK_D], ChVector<>(0, 0, 1));
301 v.Normalize();
302 w = points[DRAGLINK_S] - points[BELLCRANK_D];
303 w.Normalize();
304 u = Vcross(v, w);
305 rot.Set_A_axis(u, v, w);
306
307 // Create and initialize the universal joint between draglink and knuckle
308 m_universalDraglink = chrono_types::make_shared<ChLinkUniversal>();
309 m_universalDraglink->SetNameString(m_name + "_universalDraglink");
310 m_universalDraglink->Initialize(m_draglink, m_bellcrank,
311 ChFrame<>(points[BELLCRANK_D], rot.Get_A_quaternion()));
312 chassis->GetSystem()->AddLink(m_universalDraglink);
313 }
314
315 // Create and initialize spindle body (same orientation as the chassis)
316 m_linkBody[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
317 m_linkBody[side]->SetNameString(m_name + "_linkBody" + suffix);
318 m_linkBody[side]->SetPos((points[LINK_A] + points[LINK_C]) / 2.0);
319 m_linkBody[side]->SetRot(chassisRot);
320 m_linkBody[side]->SetMass(getLinkMass());
321 m_linkBody[side]->SetInertiaXX(getLinkInertia());
322 chassis->GetSystem()->AddBody(m_linkBody[side]);
323
324 // Create and initialize the spherical joint between axle tube and link body.
325 ChCoordsys<> sph1_csys(points[LINK_A], chassisRot);
326 m_linkBodyToAxleTube[side] = chrono_types::make_shared<ChLinkLockSpherical>();
327 m_linkBodyToAxleTube[side]->SetNameString(m_name + "_sphericalLinkToAxle" + suffix);
328 m_linkBodyToAxleTube[side]->Initialize(m_linkBody[side], m_axleTube, sph1_csys);
329 chassis->GetSystem()->AddLink(m_linkBodyToAxleTube[side]);
330
331 // Create and initialize the spherical joint between chassis and link body.
332 v = Vcross(points[LINK_C] - points[LINK_A], ChVector<>(0, 1, 0));
333 v.Normalize();
334 w = points[LINK_C] - points[LINK_A];
335 w.Normalize();
336 u = Vcross(v, w);
337 rot.Set_A_axis(u, v, w);
338
339 // Create and initialize the universal joint between draglink and knuckle
340 m_linkBodyToChassis[side] = chrono_types::make_shared<ChLinkUniversal>();
341 m_linkBodyToChassis[side]->SetNameString(m_name + "_universalDraglink");
342 m_linkBodyToChassis[side]->Initialize(m_linkBody[side], chassis, ChFrame<>(points[LINK_C], rot.Get_A_quaternion()));
343 chassis->GetSystem()->AddLink(m_linkBodyToChassis[side]);
344
345 // Create the tierod as rigid body
346 m_tierodBody[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
347 m_tierodBody[side]->SetNameString(m_name + "_tierodBody" + suffix);
348 m_tierodBody[side]->SetPos((points[KNUCKLE_T] + points[BELLCRANK_T]) / 2.0);
349 m_tierodBody[side]->SetRot(chassisRot);
350 m_tierodBody[side]->SetMass(getTierodMass());
351 m_tierodBody[side]->SetInertiaXX(getTierodInertia());
352 chassis->GetSystem()->AddBody(m_tierodBody[side]);
353
354 // Connect tierod to knuckle via spherical link
355 ChCoordsys<> sph3_csys(points[KNUCKLE_T], chassisRot);
356 m_tierodBodyToKnuckle[side] = chrono_types::make_shared<ChLinkLockSpherical>();
357 m_tierodBodyToKnuckle[side]->SetNameString(m_name + "_tierodLinkToKnuckle" + suffix);
358 m_tierodBodyToKnuckle[side]->Initialize(m_tierodBody[side], m_knuckle[side], sph3_csys);
359 chassis->GetSystem()->AddLink(m_tierodBodyToKnuckle[side]);
360
361 // Connect tierod to bellcrank via spherical link
362 ChCoordsys<> sph4_csys(points[BELLCRANK_T], chassisRot);
363 m_tierodBodyToBellcrank[side] = chrono_types::make_shared<ChLinkLockSpherical>();
364 m_tierodBodyToBellcrank[side]->SetNameString(m_name + "_tierodLinkToBellcrank" + suffix);
365 m_tierodBodyToBellcrank[side]->Initialize(m_tierodBody[side], m_bellcrank, sph4_csys);
366 chassis->GetSystem()->AddLink(m_tierodBodyToBellcrank[side]);
367 }
368
369 // -----------------------------------------------------------------------------
370 // Get the total mass of the suspension subsystem.
371 // -----------------------------------------------------------------------------
GetMass() const372 double ChSolidBellcrankThreeLinkAxle::GetMass() const {
373 return getAxleTubeMass() + getTriangleMass() + getBellcrankMass() + getDraglinkMass() +
374 2 * (getSpindleMass() + getKnuckleMass() + getTierodMass() + getLinkMass());
375 }
376
377 // -----------------------------------------------------------------------------
378 // Get the current COM location of the suspension subsystem.
379 // -----------------------------------------------------------------------------
GetCOMPos() const380 ChVector<> ChSolidBellcrankThreeLinkAxle::GetCOMPos() const {
381 ChVector<> com(0, 0, 0);
382
383 com += getAxleTubeMass() * m_axleTube->GetPos();
384
385 com += getSpindleMass() * m_spindle[LEFT]->GetPos();
386 com += getSpindleMass() * m_spindle[RIGHT]->GetPos();
387
388 com += getKnuckleMass() * m_knuckle[LEFT]->GetPos();
389 com += getKnuckleMass() * m_knuckle[RIGHT]->GetPos();
390
391 com += getLinkMass() * m_linkBody[LEFT]->GetPos();
392 com += getLinkMass() * m_linkBody[RIGHT]->GetPos();
393
394 com += getTierodMass() * m_tierodBody[LEFT]->GetPos();
395 com += getTierodMass() * m_tierodBody[RIGHT]->GetPos();
396
397 com += getBellcrankMass() * m_bellcrank->GetPos();
398
399 com += getDraglinkMass() * m_draglink->GetPos();
400
401 com += getTriangleMass() * m_triangleBody->GetPos();
402
403 return com / GetMass();
404 }
405
406 // -----------------------------------------------------------------------------
407 // Get the wheel track using the spindle local position.
408 // -----------------------------------------------------------------------------
GetTrack()409 double ChSolidBellcrankThreeLinkAxle::GetTrack() {
410 return 2 * getLocation(SPINDLE).y();
411 }
412
413 // -----------------------------------------------------------------------------
414 // Return current suspension forces
415 // -----------------------------------------------------------------------------
ReportSuspensionForce(VehicleSide side) const416 ChSuspension::Force ChSolidBellcrankThreeLinkAxle::ReportSuspensionForce(VehicleSide side) const {
417 ChSuspension::Force force;
418
419 force.spring_force = m_spring[side]->GetForce();
420 force.spring_length = m_spring[side]->GetLength();
421 force.spring_velocity = m_spring[side]->GetVelocity();
422
423 force.shock_force = m_shock[side]->GetForce();
424 force.shock_length = m_shock[side]->GetLength();
425 force.shock_velocity = m_shock[side]->GetVelocity();
426
427 return force;
428 }
429
430 // -----------------------------------------------------------------------------
431 // -----------------------------------------------------------------------------
LogHardpointLocations(const ChVector<> & ref,bool inches)432 void ChSolidBellcrankThreeLinkAxle::LogHardpointLocations(const ChVector<>& ref, bool inches) {
433 double unit = inches ? 1 / 0.0254 : 1.0;
434
435 for (int i = 0; i < NUM_POINTS; i++) {
436 ChVector<> pos = ref + unit * getLocation(static_cast<PointId>(i));
437
438 GetLog() << " " << m_pointNames[i].c_str() << " " << pos.x() << " " << pos.y() << " " << pos.z() << "\n";
439 }
440 }
441
442 // -----------------------------------------------------------------------------
443 // -----------------------------------------------------------------------------
LogConstraintViolations(VehicleSide side)444 void ChSolidBellcrankThreeLinkAxle::LogConstraintViolations(VehicleSide side) {
445 // TODO: Update this to reflect new suspension joints
446 // Revolute joints
447
448 {}
449 }
450
451 // -----------------------------------------------------------------------------
452 // -----------------------------------------------------------------------------
AddVisualizationAssets(VisualizationType vis)453 void ChSolidBellcrankThreeLinkAxle::AddVisualizationAssets(VisualizationType vis) {
454 ChSuspension::AddVisualizationAssets(vis);
455
456 if (vis == VisualizationType::NONE)
457 return;
458
459 // visualize the axle tube
460 AddVisualizationLink(m_axleTube, m_axleOuterL, m_axleOuterR, getAxleTubeRadius(), ChColor(0.7f, 0.7f, 0.7f));
461
462 // visualize the trangle body
463 AddVisualizationLink(m_triangleBody, m_triangle_sph_point, m_triangle_left_point, getAxleTubeRadius() / 2.0,
464 ChColor(0.7f, 0.3f, 0.8f));
465 AddVisualizationLink(m_triangleBody, m_triangle_sph_point, m_triangle_right_point, getAxleTubeRadius() / 2.0,
466 ChColor(0.7f, 0.3f, 0.8f));
467
468 // visualize the trangle body
469 AddVisualizationLink(m_linkBody[LEFT], m_link_axleL, m_link_chassisL, getAxleTubeRadius() / 2.0,
470 ChColor(0.3f, 0.3f, 0.8f));
471 AddVisualizationLink(m_linkBody[RIGHT], m_link_axleR, m_link_chassisR, getAxleTubeRadius() / 2.0,
472 ChColor(0.3f, 0.3f, 0.8f));
473
474 // visualize the draglink
475 AddVisualizationLink(m_draglink, m_pointsL[DRAGLINK_S], m_pointsL[BELLCRANK_D], 0.02, ChColor(0.3f, 0.3f, 0.3f));
476
477 // visualize the bellcrank base
478 AddVisualizationLink(m_bellcrank, m_pointsL[BELLCRANK_A] - ChVector<>(0, 0, 0.1),
479 m_pointsL[BELLCRANK_A] + ChVector<>(0, 0, 0.1), 0.05, ChColor(0.7f, 0.3f, 0.7f));
480 // visualize the bellcrank drag arm
481 AddVisualizationLink(m_bellcrank, m_pointsL[BELLCRANK_A], m_pointsL[BELLCRANK_D], 0.02, ChColor(0.7f, 0.3f, 0.7f));
482 // visualize the bellcrank tierod arms
483 AddVisualizationLink(m_bellcrank, m_pointsL[BELLCRANK_A], m_pointsL[BELLCRANK_T], 0.02, ChColor(0.7f, 0.3f, 0.7f));
484 AddVisualizationLink(m_bellcrank, m_pointsL[BELLCRANK_A], m_pointsR[BELLCRANK_T], 0.02, ChColor(0.7f, 0.3f, 0.7f));
485
486 // visualize the knuckles, kingpin
487 AddVisualizationLink(m_knuckle[LEFT], m_pointsL[KNUCKLE_L], m_pointsL[KNUCKLE_U], 0.03, ChColor(0.7f, 0.3f, 0.3f));
488 AddVisualizationLink(m_knuckle[RIGHT], m_pointsR[KNUCKLE_L], m_pointsR[KNUCKLE_U], 0.03, ChColor(0.7f, 0.3f, 0.3f));
489
490 // visualize the knuckles, upright
491 AddVisualizationLink(m_knuckle[LEFT], m_axleOuterL, m_pointsL[SPINDLE], 0.03, ChColor(0.7f, 0.3f, 0.3f));
492 AddVisualizationLink(m_knuckle[RIGHT], m_axleOuterR, m_pointsR[SPINDLE], 0.03, ChColor(0.7f, 0.3f, 0.3f));
493
494 // visualize the knuckles, steering arms
495 AddVisualizationLink(m_knuckle[LEFT], m_axleOuterL, m_pointsL[KNUCKLE_T], 0.03, ChColor(0.7f, 0.3f, 0.3f));
496 AddVisualizationLink(m_knuckle[RIGHT], m_axleOuterR, m_pointsR[KNUCKLE_T], 0.03, ChColor(0.7f, 0.3f, 0.3f));
497
498 // visualize the tierods
499 AddVisualizationLink(m_tierodBody[LEFT], m_tierodOuterL, m_tierodInnerL, 0.03, ChColor(0.3f, 0.7f, 0.3f));
500 AddVisualizationLink(m_tierodBody[RIGHT], m_tierodOuterR, m_tierodInnerR, 0.03, ChColor(0.3f, 0.7f, 0.3f));
501
502 // Add visualization for the springs and shocks
503 m_spring[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
504 m_spring[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
505
506 m_shock[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
507 m_shock[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
508 }
509
RemoveVisualizationAssets()510 void ChSolidBellcrankThreeLinkAxle::RemoveVisualizationAssets() {
511 ChSuspension::RemoveVisualizationAssets();
512
513 m_axleTube->GetAssets().clear();
514
515 m_spring[LEFT]->GetAssets().clear();
516 m_spring[RIGHT]->GetAssets().clear();
517
518 m_shock[LEFT]->GetAssets().clear();
519 m_shock[RIGHT]->GetAssets().clear();
520 }
521
522 // -----------------------------------------------------------------------------
523 // -----------------------------------------------------------------------------
AddVisualizationLink(std::shared_ptr<ChBody> body,const ChVector<> pt_1,const ChVector<> pt_2,double radius,const ChColor & color)524 void ChSolidBellcrankThreeLinkAxle::AddVisualizationLink(std::shared_ptr<ChBody> body,
525 const ChVector<> pt_1,
526 const ChVector<> pt_2,
527 double radius,
528 const ChColor& color) {
529 // Express hardpoint locations in body frame.
530 ChVector<> p_1 = body->TransformPointParentToLocal(pt_1);
531 ChVector<> p_2 = body->TransformPointParentToLocal(pt_2);
532
533 auto cyl = chrono_types::make_shared<ChCylinderShape>();
534 cyl->GetCylinderGeometry().p1 = p_1;
535 cyl->GetCylinderGeometry().p2 = p_2;
536 cyl->GetCylinderGeometry().rad = radius;
537 body->AddAsset(cyl);
538
539 auto col = chrono_types::make_shared<ChColorAsset>();
540 col->SetColor(color);
541 body->AddAsset(col);
542 }
543
544 // -----------------------------------------------------------------------------
545 // -----------------------------------------------------------------------------
ExportComponentList(rapidjson::Document & jsonDocument) const546 void ChSolidBellcrankThreeLinkAxle::ExportComponentList(rapidjson::Document& jsonDocument) const {
547 ChPart::ExportComponentList(jsonDocument);
548
549 std::vector<std::shared_ptr<ChBody>> bodies;
550 bodies.push_back(m_spindle[0]);
551 bodies.push_back(m_spindle[1]);
552 bodies.push_back(m_axleTube);
553 ChPart::ExportBodyList(jsonDocument, bodies);
554
555 std::vector<std::shared_ptr<ChShaft>> shafts;
556 shafts.push_back(m_axle[0]);
557 shafts.push_back(m_axle[1]);
558 ChPart::ExportShaftList(jsonDocument, shafts);
559
560 std::vector<std::shared_ptr<ChLink>> joints;
561 joints.push_back(m_revolute[0]);
562 joints.push_back(m_revolute[1]);
563 ChPart::ExportJointList(jsonDocument, joints);
564
565 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
566 springs.push_back(m_spring[0]);
567 springs.push_back(m_spring[1]);
568 springs.push_back(m_shock[0]);
569 springs.push_back(m_shock[1]);
570 ChPart::ExportLinSpringList(jsonDocument, springs);
571 }
572
Output(ChVehicleOutput & database) const573 void ChSolidBellcrankThreeLinkAxle::Output(ChVehicleOutput& database) const {
574 if (!m_output)
575 return;
576
577 std::vector<std::shared_ptr<ChBody>> bodies;
578 bodies.push_back(m_spindle[0]);
579 bodies.push_back(m_spindle[1]);
580 bodies.push_back(m_axleTube);
581 database.WriteBodies(bodies);
582
583 std::vector<std::shared_ptr<ChShaft>> shafts;
584 shafts.push_back(m_axle[0]);
585 shafts.push_back(m_axle[1]);
586 database.WriteShafts(shafts);
587
588 std::vector<std::shared_ptr<ChLink>> joints;
589 joints.push_back(m_revolute[0]);
590 joints.push_back(m_revolute[1]);
591 database.WriteJoints(joints);
592
593 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
594 springs.push_back(m_spring[0]);
595 springs.push_back(m_spring[1]);
596 springs.push_back(m_shock[0]);
597 springs.push_back(m_shock[1]);
598 database.WriteLinSprings(springs);
599 }
600
601 } // namespace vehicle
602 } // end namespace chrono
603