1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All right 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: Radu Serban, Rainer Gericke
13 // =============================================================================
14 //
15 // MTV full vehicle model
16 //
17 // =============================================================================
18
19 #include "chrono_vehicle/ChVehicleModelData.h"
20
21 #include "chrono_models/vehicle/mtv/MTV_Vehicle.h"
22 #include "chrono_models/vehicle/mtv/MTV_ChassisRear.h"
23 #include "chrono_models/vehicle/mtv/MTV_Balancer.h"
24 #include "chrono_models/vehicle/mtv/MTV_LeafspringAxle1.h"
25 #include "chrono_models/vehicle/mtv/MTV_LeafspringAxle2.h"
26 #include "chrono_models/vehicle/mtv/MTV_Solid3LinkAxle1.h"
27 #include "chrono_models/vehicle/mtv/MTV_Solid3LinkAxle2.h"
28
29 #include "chrono_models/vehicle/mtv/FMTV_ChassisFront.h"
30 #include "chrono_models/vehicle/mtv/FMTV_BrakeSimple.h"
31 #include "chrono_models/vehicle/mtv/FMTV_BrakeShafts.h"
32 #include "chrono_models/vehicle/mtv/FMTV_Driveline4WD.h"
33 #include "chrono_models/vehicle/mtv/FMTV_AntiRollBar.h"
34 #include "chrono_models/vehicle/mtv/FMTV_RotaryArm.h"
35 #include "chrono_models/vehicle/mtv/FMTV_SimpleMapPowertrain.h"
36 #include "chrono_models/vehicle/mtv/FMTV_ToebarLeafspringAxle.h"
37 #include "chrono_models/vehicle/mtv/FMTV_Wheel.h"
38
39 namespace chrono {
40 namespace vehicle {
41 namespace fmtv {
42
43 // -----------------------------------------------------------------------------
44 // -----------------------------------------------------------------------------
MTV_Vehicle(const bool fixed,bool use_walking_beam,BrakeType brake_type,ChContactMethod contact_method,CollisionType chassis_collision_type)45 MTV_Vehicle::MTV_Vehicle(const bool fixed,
46 bool use_walking_beam,
47 BrakeType brake_type,
48 ChContactMethod contact_method,
49 CollisionType chassis_collision_type)
50 : ChWheeledVehicle("MTV", contact_method), m_omega({0, 0, 0, 0, 0, 0}) {
51 Create(fixed, use_walking_beam, brake_type, chassis_collision_type);
52 }
53
MTV_Vehicle(ChSystem * system,const bool fixed,bool use_walking_beam,BrakeType brake_type,CollisionType chassis_collision_type)54 MTV_Vehicle::MTV_Vehicle(ChSystem* system,
55 const bool fixed,
56 bool use_walking_beam,
57 BrakeType brake_type,
58 CollisionType chassis_collision_type)
59 : ChWheeledVehicle("MTV", system), m_omega({0, 0, 0, 0, 0, 0}) {
60 Create(fixed, use_walking_beam, brake_type, chassis_collision_type);
61 }
62
Create(bool fixed,bool use_walking_beam,BrakeType brake_type,CollisionType chassis_collision_type)63 void MTV_Vehicle::Create(bool fixed,
64 bool use_walking_beam,
65 BrakeType brake_type,
66 CollisionType chassis_collision_type) {
67 // Create the front and rear chassis subsystems
68 m_chassis = chrono_types::make_shared<FMTV_ChassisFront>("ChassisFront", fixed, chassis_collision_type);
69 m_chassis_rear.resize(1);
70 m_chassis_rear[0] = chrono_types::make_shared<MTV_ChassisRear>("ChassisRear");
71
72 // Create the torsion articulation between front and rear chassis
73 m_chassis_connectors.resize(1);
74 m_chassis_connectors[0] = chrono_types::make_shared<MTV_ChassisConnector>("ChassisConnector");
75
76 // Create the balancer subsystem
77 m_subchassis.resize(1);
78 m_subchassis[0] = chrono_types::make_shared<MTV_Balancer>("Balancer");
79
80 // Create the steering subsystem
81 m_steerings.resize(1);
82 m_steerings[0] = chrono_types::make_shared<FMTV_RotaryArm>("Steering");
83
84 // Create the axle subsystems
85 m_axles.resize(3);
86 m_axles[0] = chrono_types::make_shared<ChAxle>();
87 m_axles[1] = chrono_types::make_shared<ChAxle>();
88 m_axles[2] = chrono_types::make_shared<ChAxle>();
89
90 m_axles[0]->m_suspension = chrono_types::make_shared<FMTV_ToebarLeafspringAxle>("FrontSusp");
91 if (use_walking_beam) {
92 m_axles[1]->m_suspension = chrono_types::make_shared<MTV_Solid3LinkAxle1>("RearSusp1");
93 m_axles[2]->m_suspension = chrono_types::make_shared<MTV_Solid3LinkAxle2>("RearSusp2");
94 } else {
95 m_axles[1]->m_suspension = chrono_types::make_shared<MTV_LeafspringAxle1>("RearSusp1");
96 m_axles[2]->m_suspension = chrono_types::make_shared<MTV_LeafspringAxle2>("RearSusp2");
97 }
98
99 m_axles[0]->m_wheels.resize(2);
100 m_axles[0]->m_wheels[0] = chrono_types::make_shared<FMTV_Wheel>("Wheel_FL");
101 m_axles[0]->m_wheels[1] = chrono_types::make_shared<FMTV_Wheel>("Wheel_FR");
102 m_axles[1]->m_wheels.resize(2);
103 m_axles[1]->m_wheels[0] = chrono_types::make_shared<FMTV_Wheel>("Wheel_RL1");
104 m_axles[1]->m_wheels[1] = chrono_types::make_shared<FMTV_Wheel>("Wheel_RR1");
105 m_axles[2]->m_wheels.resize(2);
106 m_axles[2]->m_wheels[0] = chrono_types::make_shared<FMTV_Wheel>("Wheel_RL2");
107 m_axles[2]->m_wheels[1] = chrono_types::make_shared<FMTV_Wheel>("Wheel_RR2");
108
109 switch (brake_type) {
110 case BrakeType::SIMPLE:
111 m_axles[0]->m_brake_left = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_FL");
112 m_axles[0]->m_brake_right = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_FR");
113 m_axles[1]->m_brake_left = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_RL1");
114 m_axles[1]->m_brake_right = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_RR1");
115 m_axles[2]->m_brake_left = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_RL2");
116 m_axles[2]->m_brake_right = chrono_types::make_shared<FMTV_BrakeSimple>("Brake_RR2");
117 break;
118 case BrakeType::SHAFTS:
119 m_axles[0]->m_brake_left = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_FL");
120 m_axles[0]->m_brake_right = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_FR");
121 m_axles[1]->m_brake_left = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_RL1");
122 m_axles[1]->m_brake_right = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_RR1");
123 m_axles[2]->m_brake_left = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_RL2");
124 m_axles[2]->m_brake_right = chrono_types::make_shared<FMTV_BrakeShafts>("Brake_RR2");
125 break;
126 }
127
128 // Create the antirollbar system
129 // m_axles[1]->m_antirollbar = chrono_types::make_shared<FMTV_AntirollBarRSD>("AntirollBar");
130
131 // Create the driveline
132 m_driveline = chrono_types::make_shared<FMTV_Driveline4WD>("Driveline");
133 ////m_driveline = chrono_types::make_shared<FMTV_SimpleDriveline>("Driveline");
134 }
135
~MTV_Vehicle()136 MTV_Vehicle::~MTV_Vehicle() {}
137
138 // -----------------------------------------------------------------------------
139 // -----------------------------------------------------------------------------
Initialize(const ChCoordsys<> & chassisPos,double chassisFwdVel)140 void MTV_Vehicle::Initialize(const ChCoordsys<>& chassisPos, double chassisFwdVel) {
141 // Initialize the chassis subsystems.
142 m_chassis->Initialize(m_system, chassisPos, chassisFwdVel, WheeledCollisionFamily::CHASSIS);
143 m_chassis_rear[0]->Initialize(m_chassis, WheeledCollisionFamily::CHASSIS);
144
145 // Initialize the connection between front and rear chassis
146 m_chassis_connectors[0]->Initialize(m_chassis, m_chassis_rear[0]);
147
148 // Initialize the balancer subsystem
149 m_subchassis[0]->Initialize(m_chassis_rear[0], ChVector<>(-4.1, 0.0, 0.26));
150
151 // Initialize the steering subsystem (specify the steering frame relative to the chassis reference frame)
152 ChVector<> offset = ChVector<>(0, 0, 0);
153 ChQuaternion<> rotation = Q_from_AngY(0);
154 m_steerings[0]->Initialize(m_chassis, offset, rotation);
155
156 // Initialize the axle subsystems
157 m_axles[0]->Initialize(m_chassis, nullptr, m_steerings[0], ChVector<>(0, 0, 0), ChVector<>(0), 0.0, m_omega[0],
158 m_omega[1]);
159
160 // rear axles mounted on rear chassis and subchassis
161 m_axles[1]->Initialize(m_chassis_rear[0], m_subchassis[0], nullptr, ChVector<>(-3.4, 0, 0), ChVector<>(0), 0.0,
162 m_omega[2], m_omega[3]);
163 m_axles[2]->Initialize(m_chassis_rear[0], m_subchassis[0], nullptr, ChVector<>(-4.8, 0, 0), ChVector<>(0), 0.0,
164 m_omega[4], m_omega[5]);
165
166 // Initialize the driveline subsystem
167 std::vector<int> driven_susp_indexes(m_driveline->GetNumDrivenAxles());
168
169 driven_susp_indexes[0] = 1;
170 driven_susp_indexes[1] = 2;
171
172 m_driveline->Initialize(m_chassis, m_axles, driven_susp_indexes);
173 }
174
175 // -----------------------------------------------------------------------------
176 // -----------------------------------------------------------------------------
GetSpringForce(int axle,VehicleSide side) const177 double MTV_Vehicle::GetSpringForce(int axle, VehicleSide side) const {
178 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetSpringForce(side);
179 }
180
GetSpringLength(int axle,VehicleSide side) const181 double MTV_Vehicle::GetSpringLength(int axle, VehicleSide side) const {
182 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetSpringLength(side);
183 }
184
GetSpringDeformation(int axle,VehicleSide side) const185 double MTV_Vehicle::GetSpringDeformation(int axle, VehicleSide side) const {
186 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetSpringDeformation(side);
187 }
188
189 // -----------------------------------------------------------------------------
190 // -----------------------------------------------------------------------------
GetShockForce(int axle,VehicleSide side) const191 double MTV_Vehicle::GetShockForce(int axle, VehicleSide side) const {
192 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetShockForce(side);
193 }
194
GetShockLength(int axle,VehicleSide side) const195 double MTV_Vehicle::GetShockLength(int axle, VehicleSide side) const {
196 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetShockLength(side);
197 }
198
GetShockVelocity(int axle,VehicleSide side) const199 double MTV_Vehicle::GetShockVelocity(int axle, VehicleSide side) const {
200 return std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[axle]->m_suspension)->GetShockVelocity(side);
201 }
202
203 // -----------------------------------------------------------------------------
204 // Log the hardpoint locations for the front-right and rear-right suspension
205 // subsystems (display in inches)
206 // -----------------------------------------------------------------------------
LogHardpointLocations()207 void MTV_Vehicle::LogHardpointLocations() {
208 GetLog().SetNumFormat("%7.3f");
209
210 GetLog() << "\n---- FRONT suspension hardpoint locations (LEFT side)\n";
211 std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[0]->m_suspension)
212 ->LogHardpointLocations(ChVector<>(0, 0, 0), false);
213
214 GetLog() << "\n---- REAR suspension hardpoint locations (LEFT side)\n";
215 std::static_pointer_cast<ChToeBarLeafspringAxle>(m_axles[1]->m_suspension)
216 ->LogHardpointLocations(ChVector<>(0, 0, 0), false);
217
218 GetLog() << "\n\n";
219
220 GetLog().SetNumFormat("%g");
221 }
222
223 // -----------------------------------------------------------------------------
224 // Log the spring length, deformation, and force.
225 // Log the shock length, velocity, and force.
226 // Log constraint violations of suspension joints.
227 //
228 // Lengths are reported in inches, velocities in inches/s, and forces in lbf
229 // -----------------------------------------------------------------------------
DebugLog(int what)230 void MTV_Vehicle::DebugLog(int what) {
231 GetLog().SetNumFormat("%10.2f");
232
233 if (what & OUT_SPRINGS) {
234 GetLog() << "\n---- Spring (front-left, front-right, rear-left, rear-right)\n";
235 GetLog() << "Length [m] " << GetSpringLength(0, LEFT) << " " << GetSpringLength(0, RIGHT) << " "
236 << GetSpringLength(1, LEFT) << " " << GetSpringLength(1, RIGHT) << "\n";
237 GetLog() << "Deformation [m] " << GetSpringDeformation(0, LEFT) << " " << GetSpringDeformation(0, RIGHT)
238 << " " << GetSpringDeformation(1, LEFT) << " " << GetSpringDeformation(1, RIGHT) << "\n";
239 GetLog() << "Force [N] " << GetSpringForce(0, LEFT) << " " << GetSpringForce(0, RIGHT) << " "
240 << GetSpringForce(1, LEFT) << " " << GetSpringForce(1, RIGHT) << "\n";
241 }
242
243 if (what & OUT_SHOCKS) {
244 GetLog() << "\n---- Shock (front-left, front-right, rear-left, rear-right)\n";
245 GetLog() << "Length [m] " << GetShockLength(0, LEFT) << " " << GetShockLength(0, RIGHT) << " "
246 << GetShockLength(1, LEFT) << " " << GetShockLength(1, RIGHT) << "\n";
247 GetLog() << "Velocity [m/s] " << GetShockVelocity(0, LEFT) << " " << GetShockVelocity(0, RIGHT) << " "
248 << GetShockVelocity(1, LEFT) << " " << GetShockVelocity(1, RIGHT) << "\n";
249 GetLog() << "Force [N] " << GetShockForce(0, LEFT) << " " << GetShockForce(0, RIGHT) << " "
250 << GetShockForce(1, LEFT) << " " << GetShockForce(1, RIGHT) << "\n";
251 }
252
253 if (what & OUT_CONSTRAINTS) {
254 // Report constraint violations for all joints
255 LogConstraintViolations();
256 }
257
258 GetLog().SetNumFormat("%g");
259 }
260
261 } // namespace fmtv
262 } // end namespace vehicle
263 } // end namespace chrono
264