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 a leaf-spring solid axle suspension.
16 //
17 // This class is meant for modelling a very simple nonsteerable solid leafspring
18 // axle. The guiding function of leafspring is modelled by a ChLinkLockRevolutePrismatic
19 // joint, it allows vertical movement and tilting of the axle tube but no elasticity.
20 // The spring function of the leafspring is modelled by a vertical spring element.
21 // Tie up of the leafspring is not possible with this approach, as well as the
22 // characteristic kinematics along wheel travel. The roll center and roll stability
23 // is met well, if spring track is defined correctly. The class has been designed
24 // for maximum easyness and numerical efficiency.
25 //
26 // The suspension subsystem is modeled with respect to a right-handed frame,
27 // with X pointing towards the front, Y to the left, and Z up (ISO standard).
28 // The suspension reference frame is assumed to be always aligned with that of
29 // the vehicle. When attached to a chassis, only an offset is provided.
30 //
31 // All point locations are assumed to be given for the left half of the
32 // suspension and will be mirrored (reflecting the y coordinates) to construct
33 // the right side.
34 //
35 // =============================================================================
36
37 #include "chrono/assets/ChColorAsset.h"
38 #include "chrono/assets/ChCylinderShape.h"
39 #include "chrono/assets/ChPointPointDrawing.h"
40
41 #include "chrono_vehicle/wheeled_vehicle/suspension/ChLeafspringAxle.h"
42
43 namespace chrono {
44 namespace vehicle {
45
46 // -----------------------------------------------------------------------------
47 // Static variables
48 // -----------------------------------------------------------------------------
49 const std::string ChLeafspringAxle::m_pointNames[] = {"SHOCK_A ", "SHOCK_C ", "SPRING_A ", "SPRING_C ",
50 "SPINDLE "};
51
52 // -----------------------------------------------------------------------------
53 // -----------------------------------------------------------------------------
ChLeafspringAxle(const std::string & name)54 ChLeafspringAxle::ChLeafspringAxle(const std::string& name) : ChSuspension(name) {}
55
~ChLeafspringAxle()56 ChLeafspringAxle::~ChLeafspringAxle() {
57 auto sys = m_axleTube->GetSystem();
58 if (sys) {
59 sys->Remove(m_axleTube);
60 sys->Remove(m_axleTubeGuide);
61 for (int i = 0; i < 2; i++) {
62 sys->Remove(m_shock[i]);
63 sys->Remove(m_spring[i]);
64 }
65 }
66 }
67
68 // -----------------------------------------------------------------------------
69 // -----------------------------------------------------------------------------
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)70 void ChLeafspringAxle::Initialize(std::shared_ptr<ChChassis> chassis,
71 std::shared_ptr<ChSubchassis> subchassis,
72 std::shared_ptr<ChSteering> steering,
73 const ChVector<>& location,
74 double left_ang_vel,
75 double right_ang_vel) {
76 m_location = location;
77
78 // Unit vectors for orientation matrices.
79 ChVector<> u;
80 ChVector<> v;
81 ChVector<> w;
82 ChMatrix33<> rot;
83
84 // Express the suspension reference frame in the absolute coordinate system.
85 ChFrame<> suspension_to_abs(location);
86 suspension_to_abs.ConcatenatePreTransformation(chassis->GetBody()->GetFrame_REF_to_abs());
87
88 // Transform the location of the axle body COM to absolute frame.
89 ChVector<> axleCOM_local = getAxleTubeCOM();
90 ChVector<> axleCOM = suspension_to_abs.TransformLocalToParent(axleCOM_local);
91
92 // Calculate end points on the axle body, expressed in the absolute frame
93 // (for visualization)
94 ////ChVector<> midpoint_local = 0.0;
95 ////ChVector<> outer_local(axleCOM_local.x(), midpoint_local.y(), axleCOM_local.z());
96 ChVector<> outer_local(getLocation(SPINDLE));
97 m_axleOuterL = suspension_to_abs.TransformPointLocalToParent(outer_local);
98 outer_local.y() = -outer_local.y();
99 m_axleOuterR = suspension_to_abs.TransformPointLocalToParent(outer_local);
100
101 // Create and initialize the axle body.
102 m_axleTube = std::shared_ptr<ChBody>(chassis->GetBody()->GetSystem()->NewBody());
103 m_axleTube->SetNameString(m_name + "_axleTube");
104 m_axleTube->SetPos(axleCOM);
105 m_axleTube->SetRot(chassis->GetBody()->GetFrame_REF_to_abs().GetRot());
106 m_axleTube->SetMass(getAxleTubeMass());
107 m_axleTube->SetInertiaXX(getAxleTubeInertia());
108 chassis->GetBody()->GetSystem()->AddBody(m_axleTube);
109
110 // Fix the axle body to the chassis
111 m_axleTubeGuide = chrono_types::make_shared<ChLinkLockRevolutePrismatic>();
112 m_axleTubeGuide->SetNameString(m_name + "_revolutePrismaticAxleTube");
113 const ChQuaternion<>& guideRot = chassis->GetBody()->GetFrame_REF_to_abs().GetRot();
114 m_axleTubeGuide->Initialize(chassis->GetBody(), m_axleTube,
115 ChCoordsys<>(axleCOM, guideRot * Q_from_AngY(CH_C_PI_2)));
116 chassis->GetBody()->GetSystem()->AddLink(m_axleTubeGuide);
117
118 // Transform all hardpoints to absolute frame.
119 m_pointsL.resize(NUM_POINTS);
120 m_pointsR.resize(NUM_POINTS);
121 for (int i = 0; i < NUM_POINTS; i++) {
122 ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
123 m_pointsL[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
124 rel_pos.y() = -rel_pos.y();
125 m_pointsR[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
126 }
127
128 // Initialize left and right sides.
129 std::shared_ptr<ChBody> scbeamL = (subchassis == nullptr) ? chassis->GetBody() : subchassis->GetBeam(LEFT);
130 std::shared_ptr<ChBody> scbeamR = (subchassis == nullptr) ? chassis->GetBody() : subchassis->GetBeam(RIGHT);
131 InitializeSide(LEFT, chassis->GetBody(), scbeamL, m_pointsL, left_ang_vel);
132 InitializeSide(RIGHT, chassis->GetBody(), scbeamR, m_pointsR, right_ang_vel);
133 }
134
InitializeSide(VehicleSide side,std::shared_ptr<ChBodyAuxRef> chassis,std::shared_ptr<ChBody> scbeam,const std::vector<ChVector<>> & points,double ang_vel)135 void ChLeafspringAxle::InitializeSide(VehicleSide side,
136 std::shared_ptr<ChBodyAuxRef> chassis,
137 std::shared_ptr<ChBody> scbeam,
138 const std::vector<ChVector<>>& points,
139 double ang_vel) {
140 std::string suffix = (side == LEFT) ? "_L" : "_R";
141
142 // Unit vectors for orientation matrices.
143 ChVector<> u;
144 ChVector<> v;
145 ChVector<> w;
146 ChMatrix33<> rot;
147
148 // Chassis orientation (expressed in absolute frame)
149 // Recall that the suspension reference frame is aligned with the chassis.
150 ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();
151
152 // Create and initialize spindle body (same orientation as the chassis)
153 m_spindle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
154 m_spindle[side]->SetNameString(m_name + "_spindle" + suffix);
155 m_spindle[side]->SetPos(points[SPINDLE]);
156 m_spindle[side]->SetRot(chassisRot);
157 m_spindle[side]->SetWvel_loc(ChVector<>(0, ang_vel, 0));
158 m_spindle[side]->SetMass(getSpindleMass());
159 m_spindle[side]->SetInertiaXX(getSpindleInertia());
160 chassis->GetSystem()->AddBody(m_spindle[side]);
161
162 // Create and initialize the revolute joint between axle tube and spindle.
163 ChCoordsys<> rev_csys(points[SPINDLE], chassisRot * Q_from_AngAxis(CH_C_PI / 2.0, VECT_X));
164 m_revolute[side] = chrono_types::make_shared<ChLinkLockRevolute>();
165 m_revolute[side]->SetNameString(m_name + "_revolute" + suffix);
166 m_revolute[side]->Initialize(m_spindle[side], m_axleTube, rev_csys);
167 chassis->GetSystem()->AddLink(m_revolute[side]);
168
169 // Create and initialize the shock damper
170 m_shock[side] = chrono_types::make_shared<ChLinkTSDA>();
171 m_shock[side]->SetNameString(m_name + "_shock" + suffix);
172 m_shock[side]->Initialize(chassis, m_axleTube, false, points[SHOCK_C], points[SHOCK_A]);
173 m_shock[side]->RegisterForceFunctor(getShockForceFunctor());
174 chassis->GetSystem()->AddLink(m_shock[side]);
175
176 // Create and initialize the spring
177 m_spring[side] = chrono_types::make_shared<ChLinkTSDA>();
178 m_spring[side]->SetNameString(m_name + "_spring" + suffix);
179 m_spring[side]->Initialize(scbeam, m_axleTube, false, points[SPRING_C], points[SPRING_A], false,
180 getSpringRestLength());
181 m_spring[side]->RegisterForceFunctor(getSpringForceFunctor());
182 chassis->GetSystem()->AddLink(m_spring[side]);
183
184 // Create and initialize the axle shaft and its connection to the spindle. Note that the
185 // spindle rotates about the Y axis.
186 m_axle[side] = chrono_types::make_shared<ChShaft>();
187 m_axle[side]->SetNameString(m_name + "_axle" + suffix);
188 m_axle[side]->SetInertia(getAxleInertia());
189 m_axle[side]->SetPos_dt(-ang_vel);
190 chassis->GetSystem()->Add(m_axle[side]);
191
192 m_axle_to_spindle[side] = chrono_types::make_shared<ChShaftsBody>();
193 m_axle_to_spindle[side]->SetNameString(m_name + "_axle_to_spindle" + suffix);
194 m_axle_to_spindle[side]->Initialize(m_axle[side], m_spindle[side], ChVector<>(0, -1, 0));
195 chassis->GetSystem()->Add(m_axle_to_spindle[side]);
196 }
197
198 // -----------------------------------------------------------------------------
199 // Get the total mass of the suspension subsystem.
200 // -----------------------------------------------------------------------------
GetMass() const201 double ChLeafspringAxle::GetMass() const {
202 return getAxleTubeMass() + 2 * (getSpindleMass());
203 }
204
205 // -----------------------------------------------------------------------------
206 // Get the current COM location of the suspension subsystem.
207 // -----------------------------------------------------------------------------
GetCOMPos() const208 ChVector<> ChLeafspringAxle::GetCOMPos() const {
209 ChVector<> com(0, 0, 0);
210
211 com += getAxleTubeMass() * m_axleTube->GetPos();
212
213 com += getSpindleMass() * m_spindle[LEFT]->GetPos();
214 com += getSpindleMass() * m_spindle[RIGHT]->GetPos();
215
216 return com / GetMass();
217 }
218
219 // -----------------------------------------------------------------------------
220 // Get the wheel track using the spindle local position.
221 // -----------------------------------------------------------------------------
GetTrack()222 double ChLeafspringAxle::GetTrack() {
223 return 2 * getLocation(SPINDLE).y();
224 }
225
226 // -----------------------------------------------------------------------------
227 // Return current suspension forces
228 // -----------------------------------------------------------------------------
ReportSuspensionForce(VehicleSide side) const229 ChSuspension::Force ChLeafspringAxle::ReportSuspensionForce(VehicleSide side) const {
230 ChSuspension::Force force;
231
232 force.spring_force = m_spring[side]->GetForce();
233 force.spring_length = m_spring[side]->GetLength();
234 force.spring_velocity = m_spring[side]->GetVelocity();
235
236 force.shock_force = m_shock[side]->GetForce();
237 force.shock_length = m_shock[side]->GetLength();
238 force.shock_velocity = m_shock[side]->GetVelocity();
239
240 return force;
241 }
242
243 // -----------------------------------------------------------------------------
244 // -----------------------------------------------------------------------------
LogHardpointLocations(const ChVector<> & ref,bool inches)245 void ChLeafspringAxle::LogHardpointLocations(const ChVector<>& ref, bool inches) {
246 double unit = inches ? 1 / 0.0254 : 1.0;
247
248 for (int i = 0; i < NUM_POINTS; i++) {
249 ChVector<> pos = ref + unit * getLocation(static_cast<PointId>(i));
250
251 GetLog() << " " << m_pointNames[i].c_str() << " " << pos.x() << " " << pos.y() << " " << pos.z() << "\n";
252 }
253 }
254
255 // -----------------------------------------------------------------------------
256 // -----------------------------------------------------------------------------
LogConstraintViolations(VehicleSide side)257 void ChLeafspringAxle::LogConstraintViolations(VehicleSide side) {
258 {
259 ChVectorDynamic<> C = m_axleTubeGuide->GetConstraintViolation();
260 GetLog() << "Axle tube prismatic ";
261 GetLog() << " " << C(0) << " ";
262 GetLog() << " " << C(1) << " ";
263 GetLog() << " " << C(2) << "\n";
264 }
265 }
266
267 // -----------------------------------------------------------------------------
268 // -----------------------------------------------------------------------------
AddVisualizationAssets(VisualizationType vis)269 void ChLeafspringAxle::AddVisualizationAssets(VisualizationType vis) {
270 ChSuspension::AddVisualizationAssets(vis);
271
272 if (vis == VisualizationType::NONE)
273 return;
274
275 AddVisualizationLink(m_axleTube, m_axleOuterL, m_axleOuterR, getAxleTubeRadius(), ChColor(0.7f, 0.7f, 0.7f));
276
277 // Add visualization for the springs and shocks
278 m_spring[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
279 m_spring[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
280
281 m_shock[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
282 m_shock[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
283 }
284
RemoveVisualizationAssets()285 void ChLeafspringAxle::RemoveVisualizationAssets() {
286 ChSuspension::RemoveVisualizationAssets();
287
288 m_axleTube->GetAssets().clear();
289
290 m_spring[LEFT]->GetAssets().clear();
291 m_spring[RIGHT]->GetAssets().clear();
292
293 m_shock[LEFT]->GetAssets().clear();
294 m_shock[RIGHT]->GetAssets().clear();
295 }
296
297 // -----------------------------------------------------------------------------
298 // -----------------------------------------------------------------------------
AddVisualizationLink(std::shared_ptr<ChBody> body,const ChVector<> pt_1,const ChVector<> pt_2,double radius,const ChColor & color)299 void ChLeafspringAxle::AddVisualizationLink(std::shared_ptr<ChBody> body,
300 const ChVector<> pt_1,
301 const ChVector<> pt_2,
302 double radius,
303 const ChColor& color) {
304 // Express hardpoint locations in body frame.
305 ChVector<> p_1 = body->TransformPointParentToLocal(pt_1);
306 ChVector<> p_2 = body->TransformPointParentToLocal(pt_2);
307
308 auto cyl = chrono_types::make_shared<ChCylinderShape>();
309 cyl->GetCylinderGeometry().p1 = p_1;
310 cyl->GetCylinderGeometry().p2 = p_2;
311 cyl->GetCylinderGeometry().rad = radius;
312 body->AddAsset(cyl);
313
314 auto col = chrono_types::make_shared<ChColorAsset>();
315 col->SetColor(color);
316 body->AddAsset(col);
317 }
318
319 // -----------------------------------------------------------------------------
320 // -----------------------------------------------------------------------------
ExportComponentList(rapidjson::Document & jsonDocument) const321 void ChLeafspringAxle::ExportComponentList(rapidjson::Document& jsonDocument) const {
322 ChPart::ExportComponentList(jsonDocument);
323
324 std::vector<std::shared_ptr<ChBody>> bodies;
325 bodies.push_back(m_spindle[0]);
326 bodies.push_back(m_spindle[1]);
327 bodies.push_back(m_axleTube);
328 ChPart::ExportBodyList(jsonDocument, bodies);
329
330 std::vector<std::shared_ptr<ChShaft>> shafts;
331 shafts.push_back(m_axle[0]);
332 shafts.push_back(m_axle[1]);
333 ChPart::ExportShaftList(jsonDocument, shafts);
334
335 std::vector<std::shared_ptr<ChLink>> joints;
336 joints.push_back(m_revolute[0]);
337 joints.push_back(m_revolute[1]);
338 ChPart::ExportJointList(jsonDocument, joints);
339
340 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
341 springs.push_back(m_spring[0]);
342 springs.push_back(m_spring[1]);
343 springs.push_back(m_shock[0]);
344 springs.push_back(m_shock[1]);
345 ChPart::ExportLinSpringList(jsonDocument, springs);
346 }
347
Output(ChVehicleOutput & database) const348 void ChLeafspringAxle::Output(ChVehicleOutput& database) const {
349 if (!m_output)
350 return;
351
352 std::vector<std::shared_ptr<ChBody>> bodies;
353 bodies.push_back(m_spindle[0]);
354 bodies.push_back(m_spindle[1]);
355 bodies.push_back(m_axleTube);
356 database.WriteBodies(bodies);
357
358 std::vector<std::shared_ptr<ChShaft>> shafts;
359 shafts.push_back(m_axle[0]);
360 shafts.push_back(m_axle[1]);
361 database.WriteShafts(shafts);
362
363 std::vector<std::shared_ptr<ChLink>> joints;
364 joints.push_back(m_revolute[0]);
365 joints.push_back(m_revolute[1]);
366 database.WriteJoints(joints);
367
368 std::vector<std::shared_ptr<ChLinkTSDA>> springs;
369 springs.push_back(m_spring[0]);
370 springs.push_back(m_spring[1]);
371 springs.push_back(m_shock[0]);
372 springs.push_back(m_shock[1]);
373 database.WriteLinSprings(springs);
374 }
375
376 } // end namespace vehicle
377 } // end namespace chrono
378