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