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: Justin Madsen, Markus Schmid
13 // =============================================================================
14 //
15 // Base class for a Pacejka type Magic formula 2002 tire model
16 //
17 // =============================================================================
18
19 #include <algorithm>
20 #include <cmath>
21 #include <cstdlib>
22
23 #include "chrono/core/ChTimer.h"
24 #include "chrono/core/ChGlobal.h"
25 #include "chrono/core/ChLog.h"
26
27 #include "chrono_vehicle/wheeled_vehicle/tire/ChPacejkaTire.h"
28
29 namespace chrono {
30 namespace vehicle {
31
32 // -----------------------------------------------------------------------------
33 // Static variables
34 // -----------------------------------------------------------------------------
35
36 // Threshold value for small forward tangential velocity.
37 // static const double v_x_threshold = 0.2;
38
39 // large Fz leads to large m_dF_z, leads to large Fx, Fy, etc.
40 static double Fz_thresh = 30000;
41 // max reaction threshold
42 static double Fx_thresh = 20000.0;
43 static double Fy_thresh = 20000.0;
44 static double Mx_thresh = Fz_thresh / 20.0;
45 static double My_thresh = Fx_thresh / 20.0;
46 static double Mz_thresh = Fz_thresh / 20.0;
47
48 static double kappaP_thresh = 0.01;
49 static double alphaP_thresh = 0.1;
50 static double gammaP_thresh = 0.05;
51 static double phiP_thresh = 99;
52 static double phiT_thresh = 99;
53
54 // -----------------------------------------------------------------------------
55 // Constructors
56 // -----------------------------------------------------------------------------
ChPacejkaTire(const std::string & name,const std::string & pacTire_paramFile)57 ChPacejkaTire::ChPacejkaTire(const std::string& name, const std::string& pacTire_paramFile)
58 : ChTire(name),
59 m_paramFile(pacTire_paramFile),
60 m_params_defined(false),
61 m_use_transient_slip(true),
62 m_use_Fz_override(false),
63 m_driven(false),
64 m_mu0(0.8) {}
65
ChPacejkaTire(const std::string & name,const std::string & pacTire_paramFile,double Fz_override,bool use_transient_slip)66 ChPacejkaTire::ChPacejkaTire(const std::string& name,
67 const std::string& pacTire_paramFile,
68 double Fz_override,
69 bool use_transient_slip)
70 : ChTire(name),
71 m_paramFile(pacTire_paramFile),
72 m_params_defined(false),
73 m_use_transient_slip(use_transient_slip),
74 m_use_Fz_override(Fz_override > 0),
75 m_Fz_override(Fz_override),
76 m_driven(false),
77 m_mu0(0.8) {}
78
79 // -----------------------------------------------------------------------------
80 // Destructor
81 //
82 // Delete private structures
83 // -----------------------------------------------------------------------------
~ChPacejkaTire()84 ChPacejkaTire::~ChPacejkaTire() {
85 delete m_slip;
86 delete m_params;
87 delete m_pureLong;
88 delete m_pureLat;
89 delete m_pureTorque;
90 delete m_combinedLong;
91 delete m_combinedLat;
92 delete m_combinedTorque;
93 delete m_zeta;
94 delete m_relaxation;
95 delete m_bessel;
96 }
97
98 // -----------------------------------------------------------------------------
99 // -----------------------------------------------------------------------------
100 // NOTE: no initial conditions passed in at this point, e.g. m_tireState is empty
Initialize(std::shared_ptr<ChWheel> wheel)101 void ChPacejkaTire::Initialize(std::shared_ptr<ChWheel> wheel) {
102 ChTire::Initialize(wheel);
103
104 // Create private structures
105 m_slip = new slips;
106 m_params = new Pac2002_data;
107
108 m_pureLong = new pureLongCoefs;
109 m_pureLat = new pureLatCoefs;
110 m_pureTorque = new pureTorqueCoefs;
111
112 m_combinedLong = new combinedLongCoefs;
113 m_combinedLat = new combinedLatCoefs;
114 m_combinedTorque = new combinedTorqueCoefs;
115 // m_combinedTorque->M_zr = 0;
116 // m_combinedTorque->M_z_x = 0;
117 // m_combinedTorque->M_z_y = 0;
118 m_zeta = new zetaCoefs;
119 m_relaxation = new relaxationL;
120 m_bessel = new bessel;
121
122 // negative number indicates no steps have been taken yet
123 m_time_since_last_step = 0;
124 m_initial_step = false; // have not taken a step at time = 0 yet
125 m_num_ODE_calls = 0;
126 m_sum_ODE_time = 0.0;
127 m_num_Advance_calls = 0;
128 m_sum_Advance_time = 0.0;
129
130 // load all the empirical tire parameters from *.tir file
131 loadPacTireParamFile();
132
133 //// TODO: Need to figure out a better way of indicating errors
134 //// Right now, we cannot have this function throw an exception since
135 //// it's called from the constructor! Must fix this...
136 if (!m_params_defined) {
137 GetLog() << " couldn't load pacTire parameters from file, not updating initial quantities \n\n";
138 return;
139 }
140
141 // LEFT or RIGHT side of the vehicle?
142 if (m_wheel->GetSide() == LEFT) {
143 m_sameSide = (!m_params->model.tyreside.compare("LEFT")) ? 1 : -1;
144 } else {
145 // on right
146 m_sameSide = (!m_params->model.tyreside.compare("RIGHT")) ? 1 : -1;
147 }
148
149 // any variables that are calculated once
150 m_R0 = m_params->dimension.unloaded_radius;
151
152 //// TODO: why do we have to initialize m_R_l and m_R_eff here?
153 //// This is done in Synchronize(), when we have a proper wheel state.
154
155 m_R_l = m_R0 - 8000.0 / m_params->vertical.vertical_stiffness;
156 double qV1 = 1.5;
157 double rho = (m_R0 - m_R_l) * exp(-qV1 * m_R0 * pow(1.05 * m_params->model.longvl / m_params->model.longvl, 2));
158 m_R_eff = m_R0 - rho;
159
160 // Build the lookup table for penetration depth as function of intersection area
161 // (used only with the ChTire::ENVELOPE method for terrain-tire collision detection)
162 ConstructAreaDepthTable(m_R0, m_areaDep);
163
164 m_Fz = 0;
165 m_dF_z = 0;
166
167 // spin slip coefficients, unused for now
168 {
169 zetaCoefs tmp = {1, 1, 1, 1, 1, 1, 1, 1, 1};
170 *m_zeta = tmp;
171 }
172
173 m_combinedTorque->alpha_r_eq = 0.0;
174 m_pureLat->D_y = m_params->vertical.fnomin; // initial approximation
175 m_C_Fx = 161000; // calibrated, sigma_kappa = sigma_kappa_ref = 1.29
176 m_C_Fy = 144000; // calibrated, sigma_alpha = sigma_alpha_ref = 0.725
177
178 // Initialize all other variables
179 m_Num_WriteOutData = 0;
180 zero_slips(); // zeros slips, and some other vars
181 }
182
183 // -----------------------------------------------------------------------------
184 // -----------------------------------------------------------------------------
AddVisualizationAssets(VisualizationType vis)185 void ChPacejkaTire::AddVisualizationAssets(VisualizationType vis) {
186 if (vis == VisualizationType::NONE)
187 return;
188
189 m_cyl_shape = chrono_types::make_shared<ChCylinderShape>();
190 m_cyl_shape->GetCylinderGeometry().rad = GetRadius();
191 m_cyl_shape->GetCylinderGeometry().p1 = ChVector<>(0, GetOffset() + GetVisualizationWidth() / 2, 0);
192 m_cyl_shape->GetCylinderGeometry().p2 = ChVector<>(0, GetOffset() - GetVisualizationWidth() / 2, 0);
193 m_wheel->GetSpindle()->AddAsset(m_cyl_shape);
194
195 m_texture = chrono_types::make_shared<ChTexture>();
196 m_texture->SetTextureFilename(GetChronoDataFile("textures/greenwhite.png"));
197 m_wheel->GetSpindle()->AddAsset(m_texture);
198 }
199
RemoveVisualizationAssets()200 void ChPacejkaTire::RemoveVisualizationAssets() {
201 // Make sure we only remove the assets added by ChPacejkaTire::AddVisualizationAssets.
202 // This is important for the ChTire object because a wheel may add its own assets
203 // to the same body (the spindle/wheel).
204 auto& assets = m_wheel->GetSpindle()->GetAssets();
205 {
206 auto it = std::find(assets.begin(), assets.end(), m_cyl_shape);
207 if (it != assets.end())
208 assets.erase(it);
209 }
210 {
211 auto it = std::find(assets.begin(), assets.end(), m_texture);
212 if (it != assets.end())
213 assets.erase(it);
214 }
215 }
216
217 // -----------------------------------------------------------------------------
218 // Return computed tire forces and moment (pure slip or combined slip and in
219 // local or global frame). The main GetTireForce() function returns the combined
220 // slip tire forces, expressed in the global frame.
221 // -----------------------------------------------------------------------------
GetTireForce() const222 TerrainForce ChPacejkaTire::GetTireForce() const {
223 return GetTireForce_combinedSlip(false);
224 }
225
ReportTireForce(ChTerrain * terrain) const226 TerrainForce ChPacejkaTire::ReportTireForce(ChTerrain* terrain) const {
227 return GetTireForce_combinedSlip(false);
228 }
229
GetTireForce_pureSlip(const bool local) const230 TerrainForce ChPacejkaTire::GetTireForce_pureSlip(const bool local) const {
231 if (local)
232 return m_FM_pure;
233
234 // reactions are on wheel CM
235 TerrainForce m_FM_global;
236 m_FM_global.point = m_tireState.pos;
237 // only transform the directions of the forces, moments, from local to global
238 m_FM_global.force = m_W_frame.TransformDirectionLocalToParent(m_FM_pure.force);
239 m_FM_global.moment = m_W_frame.TransformDirectionLocalToParent(m_FM_pure.moment);
240
241 return m_FM_global;
242 }
243
244 /// Return the reactions for the combined slip EQs, in local or global coordinates
GetTireForce_combinedSlip(const bool local) const245 TerrainForce ChPacejkaTire::GetTireForce_combinedSlip(const bool local) const {
246 if (local)
247 return m_FM_combined;
248
249 // reactions are on wheel CM
250 TerrainForce m_FM_global;
251 m_FM_global.point = m_W_frame.pos;
252 // only transform the directions of the forces, moments, from local to global
253 m_FM_global.force = m_W_frame.TransformDirectionLocalToParent(m_FM_combined.force);
254 m_FM_global.moment = m_W_frame.TransformDirectionLocalToParent(m_FM_combined.moment);
255
256 return m_FM_global;
257 }
258
259 // -----------------------------------------------------------------------------
260 // Update the internal state of this tire using the specified wheel state. The
261 // quantities calculated here will be kept constant until the next call to the
262 // Synchronize() function.
263 // -----------------------------------------------------------------------------
Synchronize(double time,const ChTerrain & terrain)264 void ChPacejkaTire::Synchronize(double time,
265 const ChTerrain& terrain) {
266 //// TODO: This must be removed from here. A tire with unspecified or
267 //// incorrect parameters should have been invalidate at initialization.
268 // Check that input tire model parameters are defined
269 if (!m_params_defined) {
270 GetLog() << " ERROR: cannot update tire w/o setting the model parameters first! \n\n\n";
271 return;
272 }
273
274 m_tireState = m_wheel->GetState();
275 CalculateKinematics(time, m_tireState, terrain);
276
277 // Update the tire coordinate system.
278 m_simTime = time;
279 update_W_frame(terrain);
280
281 // If not using the transient slip model, check that the tangential forward
282 // velocity is not too small.
283 ChVector<> V = m_W_frame.TransformDirectionParentToLocal(m_tireState.lin_vel);
284 if (!m_use_transient_slip && std::abs(V.x()) < 0.1) {
285 GetLog() << " ERROR: tangential forward velocity below threshold.... \n\n";
286 return;
287 }
288
289 // keep the last calculated reaction force or moment, to use later
290 m_FM_pure_last = m_FM_pure;
291 m_FM_combined_last = m_FM_combined;
292 }
293
294 // -----------------------------------------------------------------------------
295 // Advance the state of this tire by the specified time step. If using the
296 // transient slip model, perform integration taking as many integration steps
297 // as needed.
298 // -----------------------------------------------------------------------------
Advance(double step)299 void ChPacejkaTire::Advance(double step) {
300 // Initialize the output tire forces to ensure that we do not report any tire
301 // forces if the tire does not contact the terrain.
302 m_FM_pure.point = ChVector<>();
303 m_FM_pure.force = ChVector<>();
304 m_FM_pure.moment = ChVector<>();
305
306 m_FM_combined.point = ChVector<>();
307 m_FM_combined.force = ChVector<>();
308 m_FM_combined.moment = ChVector<>();
309
310 // increment the counter
311 ChTimer<double> advance_time;
312 m_num_Advance_calls++;
313
314 // Do nothing if the wheel does not contact the terrain. In this case, all
315 // reported tire forces will be zero. Still have to update slip quantities, since
316 // displacements won't go to zero immediately
317 /*
318 if (!m_in_contact)
319 {
320 zero_slips();
321 return;
322 }
323 */
324
325 // If using single point contact model, slips are calculated from compliance
326 // between tire and contact patch.
327 if (m_use_transient_slip) {
328 // 1 of 2 ways to deal with user input time step increment
329
330 // 1) ...
331 // a) step <= m_stepsize, so integrate using input step
332 // b) step > m_stepsize, use m_stepsize until step <= m_stepsize
333 double remaining_time = step;
334 // only count the time take to do actual calculations in Advance time
335 advance_time.start();
336 // keep track of the ODE calculation time
337 ChTimer<double> ODE_timer;
338 ODE_timer.start();
339 while (remaining_time > m_stepsize) {
340 advance_tire(m_stepsize);
341 remaining_time -= m_stepsize;
342 }
343 // take one final step to reach the specified time.
344 advance_tire(remaining_time);
345
346 // stop the timers
347 ODE_timer.stop();
348 m_num_ODE_calls++;
349 m_sum_ODE_time += ODE_timer();
350
351 /*
352
353 // ... or, 2)
354 // assume input step is smaller than it has to be. Then, take x steps
355 // until actually re-calculating reactions.
356 m_time_since_last_step =+ step;
357 // enough time has accumulated to do a macro step, OR, it's the first step
358 if( m_time_since_last_step >= m_stepsize || !m_initial_step)
359 {
360 // only count the time take to do actual calculations in Advance time
361 advance_time.start();
362 // keep track of the ODE calculation time
363 ChTimer<double> ODE_timer;
364 ODE_timer.start();
365 advance_slip_transient(m_time_since_last_step);
366 ODE_timer.stop();
367 // increment the timer, counter
368 m_num_ODE_calls++;
369 m_sum_ODE_time += ODE_timer();
370
371 m_time_since_last_step = 0;
372 m_initial_step = true; // 2nd term in if() above will always be false
373 } else {
374 return;
375 }
376 */
377
378 } else {
379 // Calculate the vertical load and update tire deflection and rolling radius
380 update_verticalLoad(step);
381
382 // Calculate kinematic slip quantities
383 slip_kinematic();
384 }
385
386 // Calculate the force and moment reaction, pure slip case
387 pureSlipReactions();
388
389 // Update m_FM_combined.forces, m_FM_combined.moment.z
390 combinedSlipReactions();
391
392 // Update M_x, apply to both m_FM and m_FM_combined
393 // gamma should already be corrected for L/R side, so need to swap Fy if on opposite side
394 double Mx = m_sameSide * calc_Mx(m_sameSide * m_FM_combined.force.y(), m_slip->gammaP);
395 m_FM_pure.moment.x() = Mx;
396 m_FM_combined.moment.x() = Mx;
397
398 // Update M_y, apply to both m_FM and m_FM_combined
399 // be sure signs ARE THE SAME, else it becomes self-energizing on one side
400 double My = calc_My(m_FM_combined.force.x());
401 m_FM_pure.moment.y() = My;
402 m_FM_combined.moment.y() = My;
403
404 // all the reactions have been calculated, stop the advance timer
405 advance_time.stop();
406 m_sum_Advance_time += advance_time();
407
408 // DEBUGGING
409 // m_FM_combined.moment.y() = 0;
410 // m_FM_combined.moment.z() = 0;
411
412 // evaluate the reaction forces calculated
413 evaluate_reactions(false, false);
414 }
415
advance_tire(double step)416 void ChPacejkaTire::advance_tire(double step) {
417 // Calculate the vertical load and update tire deflection and tire rolling
418 // radius.
419 update_verticalLoad(step);
420
421 // Calculate kinematic slip quantities, based on specified wheel state. This
422 // assumes that the inputs to wheel spindle instantly affect tire contact.
423 // Note: kappaP, alphaP, gammaP may be overridden in Advance
424 slip_kinematic();
425
426 advance_slip_transient(step);
427 }
428
429 // Calculate the tire contact coordinate system.
430 // TYDEX W-axis system is at the contact point "C", Z-axis normal to the terrain
431 // and X-axis along the wheel centerline.
432 // Local frame transforms output forces/moments to global frame, to be applied
433 // to the wheel body CM.
434 // Pacejka (2006), Fig 2.3, all forces are calculated at the contact point "C"
update_W_frame(const ChTerrain & terrain)435 void ChPacejkaTire::update_W_frame(const ChTerrain& terrain) {
436 // Check contact with terrain, using a disc of radius R0.
437 ChCoordsys<> contact_frame;
438
439 m_mu = terrain.GetCoefficientFriction(m_tireState.pos);
440
441 double depth;
442 double dum_cam;
443 switch (m_collision_type) {
444 case CollisionType::SINGLE_POINT:
445 m_in_contact =
446 DiscTerrainCollision(terrain, m_tireState.pos, m_tireState.rot.GetYaxis(), m_R0, contact_frame, depth);
447 break;
448 case CollisionType::FOUR_POINTS:
449 m_in_contact = DiscTerrainCollision4pt(terrain, m_tireState.pos, m_tireState.rot.GetYaxis(), m_R0,
450 m_params->dimension.width, contact_frame, depth, dum_cam);
451 break;
452 case CollisionType::ENVELOPE:
453 m_in_contact = DiscTerrainCollisionEnvelope(terrain, m_tireState.pos, m_tireState.rot.GetYaxis(), m_R0,
454 m_areaDep, contact_frame, depth);
455 break;
456 default:
457 m_in_contact = false;
458 depth = 0;
459 break;
460 }
461
462 // set the depth if there is contact with terrain
463 m_depth = (m_in_contact) ? depth : 0;
464
465 // Wheel normal (expressed in global frame)
466 ChVector<> wheel_normal = m_tireState.rot.GetYaxis();
467
468 // Terrain normal at wheel center location (expressed in global frame)
469
470 //// RADU: what frame is m_tireState in?!? Will this work with a non-ISO world frame?
471 //// why don't we use the wheel's normal direction?!?!
472
473 ChVector<> Z_dir = terrain.GetNormal(m_tireState.pos);
474
475 // Longitudinal (heading) and lateral directions, in the terrain plane.
476 ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
477 X_dir.Normalize();
478 ChVector<> Y_dir = Vcross(Z_dir, X_dir);
479
480 // Create a rotation matrix from the three unit vectors
481 ChMatrix33<> rot;
482 rot.Set_A_axis(X_dir, Y_dir, Z_dir);
483
484 // W-Axis system position at the contact point
485 m_W_frame.pos = contact_frame.pos;
486 m_W_frame.rot = rot.Get_A_quaternion();
487 }
488
489 // -----------------------------------------------------------------------------
490 // Calculate the tire vertical load at the current configuration and update the
491 // tire deflection and tire rolling radius.
492 // -----------------------------------------------------------------------------
update_verticalLoad(double step)493 void ChPacejkaTire::update_verticalLoad(double step) {
494 double Fz;
495
496 if (m_use_Fz_override) {
497 // Vertical load specified as input.
498 Fz = m_Fz_override;
499 m_Fz = m_Fz_override;
500
501 // Estimate the tire deformation and set the statically loaded radius
502 // (assuming static loading).
503 m_R_l = m_R0 - Fz / m_params->vertical.vertical_stiffness;
504
505 // In this case, the tire is always assumed to be in contact with the
506 // terrain.
507 m_in_contact = true;
508 } else {
509 // Calculate the tire vertical load using a spring-damper model. Note that
510 // this also sets the statically loaded radius m_R_l, as well as the boolean
511 // flag m_in_contact.
512 Fz = calc_Fz();
513 // assert( Fz >= m_params->vertical_force_range.fzmin);
514
515 double capped_Fz = 0;
516 // use the vertical load in the reaction to the wheel, but not in the Magic Formula Eqs
517 if (Fz > Fz_thresh) {
518 // large Fz leads to large m_dF_z, leads to large Fx, Fy, etc.
519 capped_Fz = Fz_thresh;
520 } else {
521 capped_Fz = Fz;
522 }
523
524 // damp the change in Fz
525 m_Fz = capped_Fz; // - delta_Fz_damped;
526 // if( Fz_damp * delta_Fz_damped > capped_Fz)
527 // {
528 // m_Fz = m_params->vertical_force_range.fzmin;
529 // GetLog() << " \n **** \n the damper on m_Fz should not cause it to go negative !!!! \n\n\n";
530 // }
531 }
532
533 assert(m_Fz > 0);
534 // ratio of actual load to nominal
535 m_dF_z = (m_Fz - m_params->vertical.fnomin) / m_params->vertical.fnomin;
536
537 // Calculate tire vertical deflection, rho.
538 double qV1 = 0.000071; // from example
539 double K1 = pow(m_tireState.omega * m_R0 / m_params->model.longvl, 2);
540 double rho = m_R0 - m_R_l + qV1 * m_R0 * K1;
541 double rho_Fz0 = m_params->vertical.fnomin / (m_params->vertical.vertical_stiffness);
542 double rho_d = rho / rho_Fz0;
543
544 // Calculate tire rolling radius, R_eff. Clamp this value to R0.
545 m_R_eff = m_R0 + qV1 * m_R0 * K1 -
546 rho_Fz0 * (m_params->vertical.dreff * std::atan(m_params->vertical.breff * rho_d) +
547 m_params->vertical.freff * rho_d);
548 if (m_R_eff > m_R0)
549 m_R_eff = m_R0;
550
551 if (m_in_contact) {
552 // Load vertical component of tire force
553 m_FM_pure.force.z() = Fz;
554 m_FM_combined.force.z() = Fz;
555 }
556 }
557
558 // NOTE: must
calc_Fz()559 double ChPacejkaTire::calc_Fz() {
560 // Initialize the statically loaded radius to R0. This is the returned value
561 // if no vertical force is generated.
562 m_R_l = m_R0;
563
564 if (!m_in_contact)
565 return m_params->vertical_force_range.fzmin;
566
567 // Calculate relative velocity (wheel - terrain) at contact point, in the
568 // global frame and then express it in the contact frame.
569 ChVector<> relvel_abs = m_tireState.lin_vel + Vcross(m_tireState.ang_vel, m_W_frame.pos - m_tireState.pos);
570 ChVector<> relvel_loc = m_W_frame.TransformDirectionParentToLocal(relvel_abs);
571
572 // Calculate normal contact force, using a spring-damper model.
573 // Note: depth is always positive, so the damping should always subtract
574 double Fz = m_params->vertical.vertical_stiffness * m_depth - m_params->vertical.vertical_damping * relvel_loc.z();
575
576 // Adams reference, Fz = Fz(omega, Fx, Fy, gamma, m_depth, relvel_loc.z)
577 double q_v2 = 2.0; // linear stiffness increase with spin
578 double q_Fcx = 0.2; // Fx stiffness reduction
579 double q_Fcy = 0.35; // Fy stiffness reduction
580 double q_FcG = 0.001; // camber stiffness increase
581 double C_Fz = m_params->vertical.vertical_damping; // damping
582 double q_Fz1 = m_params->vertical.vertical_stiffness; // linear stiffness
583 double q_Fz2 = 500.0; // 2nd order stiffness
584 // scale the stiffness by considering forces and spin rate
585 double force_term = 1.0 + q_v2 * std::abs(m_tireState.omega) * m_R0 / m_params->model.longvl -
586 pow(q_Fcx * m_FM_combined_last.force.x() / m_params->vertical.fnomin, 2) -
587 pow(q_Fcy * m_FM_combined_last.force.y() / m_params->vertical.fnomin, 2) +
588 q_FcG * pow(m_slip->gammaP, 2);
589 double rho_term = q_Fz1 * m_depth + q_Fz2 * pow(m_depth, 2);
590 // Fz = force_term*rho_term*Fz0 + C_Fz * v_z
591 double Fz_adams = force_term * rho_term - C_Fz * relvel_loc.z();
592
593 // for a 37x12.5 R16.5 Wrangler MT @ 30 psi
594 // double k1 = 550000.0;
595 // double k2 = 1e5;
596 // vertical_damping can be some small percentage of vertical_stiffness (e.g., 2%)
597 // double c = 0.001 * k1;
598 // double Fz = k1 * m_depth - c * relvel_loc.z(); // + k2 * pow(m_depth,2);
599
600 // if (Fz < m_params->vertical_force_range.fzmin)
601 if (Fz_adams < m_params->vertical_force_range.fzmin)
602 return m_params->vertical_force_range.fzmin;
603
604 m_R_l = m_R0 - m_depth;
605
606 return Fz;
607 // return Fz_adams;
608 }
609
610 // -----------------------------------------------------------------------------
611 // Calculate kinematic slip quantities from the current wheel state.
612 // Note: when not in contact, all slips are assumed zero, but velocities still set
613 // The wheel state structure contains the following member variables:
614 // ChVector<> pos; global position
615 // ChQuaternion<> rot; orientation with respect to global frame
616 // ChVector<> lin_vel; linear velocity, expressed in the global frame
617 // ChVector<> ang_vel; angular velocity, expressed in the global frame
618 // double omega; wheel angular speed about its rotation axis
619 // -----------------------------------------------------------------------------
slip_kinematic()620 void ChPacejkaTire::slip_kinematic() {
621 // Express the wheel velocity in the tire coordinate system and calculate the
622 // slip angle alpha. (Override V.x if too small)
623 ChVector<> V = m_W_frame.TransformDirectionParentToLocal(m_tireState.lin_vel);
624 // regardless of contact
625 m_slip->V_cx = V.x(); // tire center x-vel, tire c-sys
626 m_slip->V_cy = V.y(); // tire center y-vel, tire c-sys
627
628 if (m_in_contact) {
629 m_slip->V_sx = V.x() - m_tireState.omega * m_R_eff; // x-slip vel, tire c-sys
630 m_slip->V_sy = V.y(); // approx.
631
632 // ensure V_x is not too small, else scale V_x to the threshold
633 if (std::abs(V.x()) < m_params->model.vxlow) {
634 V.x() = (V.x() < 0) ? -m_params->model.vxlow : m_params->model.vxlow;
635 }
636
637 double V_x_abs = std::abs(V.x());
638 // lateral slip angle, in the range: (-pi/2 ,pi/2)
639 double alpha = std::atan(V.y() / V_x_abs);
640
641 // Express the wheel normal in the tire coordinate system and calculate the
642 // wheel camber angle gamma.
643 ChVector<> n = m_W_frame.TransformDirectionParentToLocal(m_tireState.rot.GetYaxis());
644 double gamma = std::atan2(n.z(), n.y());
645
646 double kappa = -m_slip->V_sx / V_x_abs;
647
648 // alpha_star = tan(alpha) = v_y / v_x
649 double alpha_star = m_slip->V_sy / V_x_abs;
650
651 // Set the struct data members, input slips to wheel
652 m_slip->kappa = kappa;
653 m_slip->alpha = alpha;
654 m_slip->alpha_star = alpha_star;
655 m_slip->gamma = gamma;
656
657 // Express the wheel angular velocity in the tire coordinate system and
658 // extract the turn slip velocity, psi_dot
659 ChVector<> w = m_W_frame.TransformDirectionParentToLocal(m_tireState.ang_vel);
660 m_slip->psi_dot = w.z();
661
662 // For aligning torque, to handle large slips, and backwards operation
663 double V_mag = std::sqrt(V.x() * V.x() + V.y() * V.y());
664 m_slip->cosPrime_alpha = V.x() / V_mag;
665
666 // Finally, if non-transient, use wheel slips as input to Magic Formula.
667 // These get over-written if enable transient slips to be calculated
668 m_slip->kappaP = kappa;
669 m_slip->alphaP = alpha_star;
670 m_slip->gammaP = std::sin(gamma);
671 } else {
672 // not in contact, set input slips to 0
673 m_slip->V_sx = 0;
674 m_slip->V_sy = 0;
675
676 m_slip->kappa = 0;
677 m_slip->alpha = 0;
678 m_slip->alpha_star = 0;
679 m_slip->gamma = 0;
680 // further, set the slips used in the MF eqs. to zero
681 m_slip->kappaP = 0;
682 m_slip->alphaP = 0;
683 m_slip->gammaP = 0;
684 // slip velocities should likely be zero also
685 m_slip->cosPrime_alpha = 1;
686
687 // Express the wheel angular velocity in the tire coordinate system and
688 // extract the turn slip velocity, psi_dot
689 ChVector<> w = m_W_frame.TransformDirectionParentToLocal(m_tireState.ang_vel);
690 m_slip->psi_dot = w.z();
691 }
692 }
693
zero_slips()694 void ChPacejkaTire::zero_slips() {
695 // reset relevant slip variables here
696 slips zero_slip = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
697 *m_slip = zero_slip;
698 }
699
700 // -----------------------------------------------------------------------------
701 // Calculate transient slip properties, using first order ODEs to find slip
702 // displacements from velocities.
703 // -----------------------------------------------------------------------------
advance_slip_transient(double step_size)704 void ChPacejkaTire::advance_slip_transient(double step_size) {
705 // hard-coded, for now
706 double EPS_GAMMA = 0.6;
707 // update relaxation lengths
708 relaxationLengths();
709
710 // local c-sys velocities
711 double V_cx = m_slip->V_cx;
712 double V_cx_abs = std::abs(V_cx);
713 double V_cx_low = 2.5; // cut-off for low velocity zone
714 double V_sx = m_slip->V_sx;
715 double V_sy = m_slip->V_sy * m_sameSide; // due to asymmetry about centerline
716 double gamma = m_slip->gamma * m_sameSide; // due to asymmetry
717 // see if low velocity considerations should be made
718 double alpha_sl = std::abs(3.0 * m_pureLat->D_y / m_relaxation->C_Falpha);
719 // Eq. 7.25 from Pacejka (2006), solve du_dt and dvalpha_dt
720 if ((std::abs(m_combinedTorque->alpha_r_eq) > alpha_sl) && (V_cx_abs < V_cx_low)) {
721 // Eq. 7.9, else du/dt = 0 and u remains unchanged
722 if ((V_sx + V_cx_abs * m_slip->u / m_relaxation->sigma_kappa) * m_slip->u >= 0) {
723 // solve the ODE using RK - 45 integration
724 m_slip->Idu_dt = ODE_RK_uv(V_sx, m_relaxation->sigma_kappa, V_cx, step_size, m_slip->u);
725 m_slip->u += m_slip->Idu_dt;
726 } else {
727 m_slip->Idu_dt = 0;
728 }
729
730 // Eq. 7.7, else dv/dt = 0 and v remains unchanged
731 if ((V_sy + std::abs(V_cx) * m_slip->v_alpha / m_relaxation->sigma_alpha) * m_slip->v_alpha >= 0) {
732 m_slip->Idv_alpha_dt = ODE_RK_uv(V_sy, m_relaxation->sigma_alpha, V_cx, step_size, m_slip->v_alpha);
733 m_slip->v_alpha += m_slip->Idv_alpha_dt;
734 } else {
735 m_slip->Idv_alpha_dt = 0;
736 }
737 } else {
738 // don't check for du/dt =0 or dv/dt = 0
739
740 // Eq 7.9
741 m_slip->Idu_dt = ODE_RK_uv(V_sx, m_relaxation->sigma_kappa, V_cx, step_size, m_slip->u);
742 m_slip->u += m_slip->Idu_dt;
743
744 // Eq. 7.7
745 m_slip->Idv_alpha_dt = ODE_RK_uv(V_sy, m_relaxation->sigma_alpha, V_cx, step_size, m_slip->v_alpha);
746 m_slip->v_alpha += m_slip->Idv_alpha_dt;
747 }
748
749 // Eq. 7.11, lateral force from wheel camber
750 m_slip->Idv_gamma_dt = ODE_RK_gamma(m_relaxation->C_Fgamma, m_relaxation->C_Falpha, m_relaxation->sigma_alpha, V_cx,
751 step_size, gamma, m_slip->v_gamma);
752 m_slip->v_gamma += m_slip->Idv_gamma_dt;
753
754 // Eq. 7.12, total spin, phi, including slip and camber
755 m_slip->Idv_phi_dt =
756 ODE_RK_phi(m_relaxation->C_Fphi, m_relaxation->C_Falpha, V_cx, m_slip->psi_dot, m_tireState.omega, gamma,
757 m_relaxation->sigma_alpha, m_slip->v_phi, EPS_GAMMA, step_size);
758 m_slip->v_phi += m_slip->Idv_phi_dt;
759
760 // calculate slips from contact point deflections u and v
761 slip_from_uv(m_in_contact, 600.0, 100.0, 2.0);
762 }
763
764 // don't have to call this each advance_tire(), but once per macro-step (at least)
evaluate_slips()765 void ChPacejkaTire::evaluate_slips() {
766 if (std::abs(m_slip->kappaP) > kappaP_thresh) {
767 GetLog() << "\n ~~~~~~~~~ kappaP exceeded threshold:, tire " << m_name << ", = " << m_slip->kappaP << "\n";
768 }
769 if (std::abs(m_slip->alphaP) > alphaP_thresh) {
770 GetLog() << "\n ~~~~~~~~~ alphaP exceeded threshold:, tire " << m_name << ", = " << m_slip->alphaP << "\n";
771 }
772 if (std::abs(m_slip->gammaP) > gammaP_thresh) {
773 GetLog() << "\n ~~~~~~~~~ gammaP exceeded threshold:, tire " << m_name << ", = " << m_slip->gammaP << "\n";
774 }
775 if (std::abs(m_slip->phiP) > phiP_thresh) {
776 GetLog() << "\n ~~~~~~~~~ phiP exceeded threshold:, tire " << m_name << ", = " << m_slip->phiP << "\n";
777 }
778 if (std::abs(m_slip->phiT) > phiT_thresh) {
779 GetLog() << "\n ~~~~~~~~~ phiT exceeded threshold:, tire " << m_name << ", = " << m_slip->phiT << "\n";
780 }
781 }
782
783 // after calculating all the reactions, evaluate output for any fishy business
evaluate_reactions(bool write_violations,bool enforce_threshold)784 void ChPacejkaTire::evaluate_reactions(bool write_violations, bool enforce_threshold) {
785 // any thresholds exceeded? then print some details about slip state
786
787 if (std::abs(m_FM_combined.force.x()) > Fx_thresh) {
788 if (enforce_threshold)
789 m_FM_combined.force.x() = m_FM_combined.force.x() * (Fx_thresh / std::abs(m_FM_combined.force.x()));
790 }
791 if (std::abs(m_FM_combined.force.y()) > Fy_thresh) {
792 if (enforce_threshold)
793 m_FM_combined.force.y() = m_FM_combined.force.y() * (Fy_thresh / std::abs(m_FM_combined.force.y()));
794 }
795
796 // m_Fz, the Fz input to the tire model, must be limited based on the Fz_threshold
797 // e.g., should never need t;his
798 if (std::abs(m_Fz) > Fz_thresh) {
799 ////GetLog() << "\n *** !!! *** Fz exceeded threshold:, tire " << m_name << ", = " << m_Fz << "\n";
800 }
801
802 if (std::abs(m_FM_combined.moment.x()) > Mx_thresh) {
803 if (enforce_threshold)
804 m_FM_combined.moment.x() = m_FM_combined.moment.x() * (Mx_thresh / std::abs(m_FM_combined.moment.x()));
805 }
806
807 if (std::abs(m_FM_combined.moment.y()) > My_thresh) {
808 if (enforce_threshold)
809 m_FM_combined.moment.y() = m_FM_combined.moment.y() * (My_thresh / std::abs(m_FM_combined.moment.y()));
810 }
811
812 if (std::abs(m_FM_combined.moment.z()) > Mz_thresh) {
813 if (enforce_threshold)
814 m_FM_combined.moment.z() = m_FM_combined.moment.z() * (Mz_thresh / std::abs(m_FM_combined.moment.z()));
815
816 ////GetLog() << " *** !!! *** Mz exceeded threshold, tire " << m_name << ", = " << m_FM_combined.moment.z()
817 //// << "\n";
818 }
819
820 if (write_violations) {
821 GetLog() << " *********** time = " << m_simTime << ", slip data: \n(u,v_alpha,v_gamma) = " << m_slip->u
822 << ", " << m_slip->v_alpha << ", " << m_slip->v_gamma << "\n velocity, center (x,y) = " << m_slip->V_cx
823 << ", " << m_slip->V_cy << "\n velocity, slip (x,y) = " << m_slip->V_sx << ", " << m_slip->V_sy
824 << "\n\n";
825 }
826 }
827
828 // these are both for the linear case, small alpha
ODE_RK_uv(double V_s,double sigma,double V_cx,double step_size,double x_curr)829 double ChPacejkaTire::ODE_RK_uv(double V_s, double sigma, double V_cx, double step_size, double x_curr) {
830 double V_cx_abs = std::abs(V_cx);
831 double k1 = -V_s - (V_cx_abs / sigma) * x_curr;
832 double k2 = -V_s - (V_cx_abs / sigma) * (x_curr + 0.5 * step_size * k1);
833 double k3 = -V_s - (V_cx_abs / sigma) * (x_curr + 0.5 * step_size * k2);
834 double k4 = -V_s - (V_cx_abs / sigma) * (x_curr + step_size * k3);
835
836 double delta_x = (step_size / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
837
838 return delta_x;
839 }
840
get_dFy_dtan_alphaP(double x_curr)841 double ChPacejkaTire::get_dFy_dtan_alphaP(double x_curr) {
842 // I suppose I could do a diff(Fx_c, alphaP) w/ a symbolic toolkit.
843 // for now, just use forward differencing
844 double dx = 0.01; // x_curr is tan(alpha'), range (-1,1) over x < (-pi/2,pi/2)
845 // changing tan(alpha') has no effect on kappa', gamma'
846 double Fy_x_dx = Fy_combined(m_slip->alphaP + dx, m_slip->gammaP, m_slip->kappaP, m_FM_combined.force.y());
847 // dFy_dx = (f(x+dx) - f(x)) / dx
848 double d_Fy = Fy_x_dx - m_FM_combined.force.y();
849
850 return d_Fy / dx;
851 }
852
853 // non-linear model, just use alphaP = alpha'
854 // small alpha, use Eq. 7.37
855 // here, we integrate d[tan(alphaP)]/dt for the step size
856 // returns delta_v_alpha = delta_tan_alphaP * sigma_a
ODE_RK_kappaAlpha(double V_s,double sigma,double V_cx,double step_size,double x_curr)857 double ChPacejkaTire::ODE_RK_kappaAlpha(double V_s, double sigma, double V_cx, double step_size, double x_curr) {
858 double V_cx_abs = std::abs(V_cx);
859 // double sigma_alpha = get_dFy_dtan_alphaP(x_curr) / C_Fy;
860 double k1 = (-V_s - V_cx_abs * x_curr) / sigma;
861 double k2 = (-V_s - V_cx_abs * (x_curr + 0.5 * step_size * k1)) / sigma;
862 double k3 = (-V_s - V_cx_abs * (x_curr + 0.5 * step_size * k2)) / sigma;
863 double k4 = (-V_s - V_cx_abs * (x_curr + step_size * k3)) / sigma;
864
865 double delta_x = (step_size / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
866
867 return delta_x;
868 }
869
ODE_RK_gamma(double C_Fgamma,double C_Falpha,double sigma_alpha,double V_cx,double step_size,double gamma,double v_gamma)870 double ChPacejkaTire::ODE_RK_gamma(double C_Fgamma,
871 double C_Falpha,
872 double sigma_alpha,
873 double V_cx,
874 double step_size,
875 double gamma,
876 double v_gamma) {
877 double V_cx_abs = std::abs(V_cx);
878 double g0 = C_Fgamma / C_Falpha * V_cx_abs * gamma;
879 double g1 = V_cx_abs / sigma_alpha;
880 double k1 = g0 - g1 * v_gamma;
881 double k2 = g0 - g1 * (v_gamma + 0.5 * step_size * k1);
882 double k3 = g0 - g1 * (v_gamma + 0.5 * step_size * k2);
883 double k4 = g0 - g1 * (v_gamma + step_size * k3);
884
885 double delta_g = (step_size / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
886
887 return delta_g;
888 }
889
ODE_RK_phi(double C_Fphi,double C_Falpha,double V_cx,double psi_dot,double omega,double gamma,double sigma_alpha,double v_phi,double eps_gamma,double step_size)890 double ChPacejkaTire::ODE_RK_phi(double C_Fphi,
891 double C_Falpha,
892 double V_cx,
893 double psi_dot,
894 double omega,
895 double gamma,
896 double sigma_alpha,
897 double v_phi,
898 double eps_gamma,
899 double step_size) {
900 int sign_Vcx = 0;
901 if (V_cx < 0)
902 sign_Vcx = -1;
903 else
904 sign_Vcx = 1;
905
906 double p0 = (C_Fphi / C_Falpha) * sign_Vcx * (psi_dot - (1.0 - eps_gamma) * omega * std::sin(gamma));
907 double p1 = (1.0 / sigma_alpha) * std::abs(V_cx);
908
909 double k1 = -p0 - p1 * v_phi;
910 double k2 = -p0 - p1 * (v_phi + 0.5 * step_size * k1);
911 double k3 = -p0 - p1 * (v_phi + 0.5 * step_size * k2);
912 double k4 = -p0 - p1 * (v_phi + step_size * k3);
913
914 double delta_phi = (step_size / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
915
916 return delta_phi;
917 }
918
slip_from_uv(bool use_besselink,double bessel_Cx,double bessel_Cy,double V_low)919 void ChPacejkaTire::slip_from_uv(bool use_besselink, double bessel_Cx, double bessel_Cy, double V_low) {
920 // separate long and lateral damping components
921 double d_Vxlow = 0;
922 double d_Vylow = 0;
923 double V_cx_abs = std::abs(m_slip->V_cx);
924 // damp gradually to zero velocity at low velocity
925 if (V_cx_abs <= V_low && use_besselink) {
926 // d_vlow = bessel_c * (1.0 + std::cos(CH_C_PI * V_cx_abs / V_low));
927 // damp between C * (2, 1) for V_cx (0,V_low)
928 d_Vxlow = bessel_Cx * (1.0 + std::cos(CH_C_PI * V_cx_abs / 2.0 * V_low));
929 d_Vylow = bessel_Cy * (1.0 + std::cos(CH_C_PI * V_cx_abs / 2.0 * V_low));
930 } else {
931 // also damp if not a driven wheel (can linger around slip = 0)
932 // if(!m_driven && use_besselink)
933 {
934 d_Vxlow = bessel_Cx * (std::exp(-(V_cx_abs - V_low) / m_params->model.longvl));
935 d_Vylow = bessel_Cy * (std::exp(-(V_cx_abs - V_low) / m_params->model.longvl));
936 }
937 }
938
939 // Besselink is RH term in kappa_p, alpha_p
940 double u_sigma = m_slip->u / m_relaxation->sigma_kappa;
941 double u_Bessel = d_Vxlow * m_slip->V_sx / m_relaxation->C_Fkappa;
942
943 // either u_Bessel or u_tow should be zero (or both)
944 double kappa_p = u_sigma - u_Bessel;
945 // don't allow damping to switch sign of kappa, clamp to zero
946 if (u_sigma * kappa_p < 0)
947 kappa_p = 0;
948
949 // tan(alpha') ~= alpha' for small slip
950 double v_sigma = -m_slip->v_alpha / m_relaxation->sigma_alpha;
951 // double v_sigma = std::atan(m_slip->v_alpha / m_relaxation->sigma_alpha);
952 double v_Bessel = -d_Vylow * m_slip->V_sy * m_sameSide / m_relaxation->C_Falpha;
953
954 double alpha_p = v_sigma - v_Bessel; // - v_tow;
955 // don't allow damping to switch sign of alpha
956 if (v_sigma * alpha_p < 0)
957 alpha_p = 0;
958
959 // gammaP, phiP, phiT not effected by Besselink
960 double gamma_p = m_relaxation->C_Falpha * m_slip->v_gamma / (m_relaxation->C_Fgamma * m_relaxation->sigma_alpha);
961 double phi_p =
962 (m_relaxation->C_Falpha * m_slip->v_phi) / (m_relaxation->C_Fphi * m_relaxation->sigma_alpha); // turn slip
963 double phi_t = -m_slip->psi_dot / m_slip->V_cx; // for turn slip
964
965 // set transient slips
966 m_slip->alphaP = alpha_p;
967 m_slip->kappaP = kappa_p;
968 m_slip->gammaP = gamma_p;
969 m_slip->phiP = phi_p;
970 m_slip->phiT = phi_t;
971
972 bessel tmp = {u_Bessel, u_sigma, v_Bessel, v_sigma};
973 *m_bessel = tmp;
974 }
975
976 // -----------------------------------------------------------------------------
977 // Calculate tire reactions.
978 // -----------------------------------------------------------------------------
979 // pure slip reactions, if the tire is in contact with the ground, in the TYDEX W-Axis system.
980 // calcFx alphaP ~= 0
981 // calcFy and calcMz kappaP ~= 0
982 // NOTE: alphaP, gammaP and kappaP defined with respect to tire side specified from *.tir input file.
983 // e.g., positive alpha = turn wheel toward vehicle centerline
984 // e.g., positive gamma = wheel vertical axis top pointing toward vehicle center
pureSlipReactions()985 void ChPacejkaTire::pureSlipReactions() {
986 if (!m_in_contact)
987 return;
988
989 double mu_scale = m_mu / m_mu0;
990
991 // calculate Fx, pure long. slip condition
992 m_FM_pure.force.x() = mu_scale * Fx_pureLong(m_slip->gammaP, m_slip->kappaP);
993
994 // calc. Fy, pure lateral slip.
995 m_FM_pure.force.y() = m_sameSide * mu_scale * Fy_pureLat(m_slip->alphaP, m_slip->gammaP);
996
997 // calc Mz, pure lateral slip. Negative y-input force, and also the output Mz.
998 m_FM_pure.moment.z() = m_sameSide * Mz_pureLat(m_slip->alphaP, m_slip->gammaP, m_sameSide * m_FM_pure.force.y());
999 }
1000
1001 // combined slip reactions, if the tire is in contact with the ground
1002 // MUST call pureSlipReactions() first, as these functions rely on intermediate values
1003 // NOTE: alphaP and gammaP have already been modified for being on the L/R side
1004 // e.g., positive alpha = turn wheel toward vehicle centerline
1005 // e.g., positive gamma = wheel vertical axis top pointing toward vehicle center
combinedSlipReactions()1006 void ChPacejkaTire::combinedSlipReactions() {
1007 if (!m_in_contact)
1008 return;
1009
1010 // calculate Fx for combined slip
1011 m_FM_combined.force.x() = Fx_combined(m_slip->alphaP, m_slip->gammaP, m_slip->kappaP, m_FM_pure.force.x());
1012
1013 // calc Fy for combined slip.
1014 m_FM_combined.force.y() =
1015 m_sameSide * Fy_combined(m_slip->alphaP, m_slip->gammaP, m_slip->kappaP, m_sameSide * m_FM_pure.force.y());
1016
1017 // calc Mz for combined slip
1018 m_FM_combined.moment.z() =
1019 m_sameSide * Mz_combined(m_pureTorque->alpha_r, m_pureTorque->alpha_t, m_slip->gammaP, m_slip->kappaP,
1020 m_FM_combined.force.x(), m_sameSide * m_FM_combined.force.y());
1021 }
1022
relaxationLengths()1023 void ChPacejkaTire::relaxationLengths() {
1024 double p_Ky4 = 2; // according to Pac2002 model
1025 double p_Ky6 = 2.5; // 0.92;
1026 double p_Ky7 = 0.24;
1027
1028 // NOTE: C_Falpha is positive according to Pacejka, negative in Pac2002.
1029 // Positive stiffness makes sense, so ensure all these are positive
1030 double C_Falpha =
1031 std::abs(m_params->lateral.pky1 * m_params->vertical.fnomin *
1032 std::sin(p_Ky4 * std::atan(m_Fz / (m_params->lateral.pky2 * m_params->vertical.fnomin))) * m_zeta->z3 *
1033 m_params->scaling.lyka);
1034 double sigma_alpha = C_Falpha / m_C_Fy;
1035 double C_Fkappa = m_Fz * (m_params->longitudinal.pkx1 + m_params->longitudinal.pkx2 * m_dF_z) *
1036 exp(m_params->longitudinal.pkx3 * m_dF_z) * m_params->scaling.lky;
1037 double sigma_kappa = C_Fkappa / m_C_Fx;
1038 double C_Fgamma = m_Fz * (p_Ky6 + p_Ky7 * m_dF_z) * m_params->scaling.lgay;
1039 double C_Fphi = 2.0 * (C_Fgamma * m_R0);
1040
1041 assert(C_Falpha > 0);
1042 assert(sigma_alpha > 0);
1043 assert(C_Fkappa > 0);
1044 assert(sigma_kappa > 0);
1045 assert(C_Fgamma > 0);
1046 assert(C_Fphi > 0);
1047
1048 // NOTE: reference does not include the negative in the exponent for sigma_kappa_ref
1049 // double sigma_kappa_ref = m_Fz * (m_params->longitudinal.ptx1 + m_params->longitudinal.ptx2 *
1050 // m_dF_z)*(m_R0*m_params->scaling.lsgkp / m_params->vertical.fnomin) * exp( -m_params->longitudinal.ptx3 * m_dF_z);
1051 // double sigma_alpha_ref = m_params->lateral.pty1 * (1.0 - m_params->lateral.pky3 * std::abs( m_slip->gammaP ) ) *
1052 // m_R0 * m_params->scaling.lsgal * sin(p_Ky4 * atan(m_Fz / (m_params->lateral.pty2 * m_params->vertical.fnomin) )
1053 // );
1054 {
1055 relaxationL tmp = {C_Falpha, sigma_alpha, C_Fkappa, sigma_kappa, C_Fgamma, C_Fphi};
1056 *m_relaxation = tmp;
1057 }
1058 }
1059
Fx_pureLong(double gamma,double kappa)1060 double ChPacejkaTire::Fx_pureLong(double gamma, double kappa) {
1061 // double eps_Vx = 0.6;
1062 double eps_x = 0;
1063 // Fx, pure long slip
1064 double S_Hx = (m_params->longitudinal.phx1 + m_params->longitudinal.phx2 * m_dF_z) * m_params->scaling.lhx;
1065 double kappa_x = kappa + S_Hx; // * 0.1;
1066
1067 double mu_x = (m_params->longitudinal.pdx1 + m_params->longitudinal.pdx2 * m_dF_z) *
1068 (1.0 - m_params->longitudinal.pdx3 * pow(gamma, 2)) * m_params->scaling.lmux; // >0
1069 double K_x = m_Fz * (m_params->longitudinal.pkx1 + m_params->longitudinal.pkx2 * m_dF_z) *
1070 exp(m_params->longitudinal.pkx3 * m_dF_z) * m_params->scaling.lkx;
1071 double C_x = m_params->longitudinal.pcx1 * m_params->scaling.lcx; // >0
1072 double D_x = mu_x * m_Fz * m_zeta->z1; // >0
1073 double B_x = K_x / (C_x * D_x + eps_x);
1074
1075 double sign_kap = (kappa_x >= 0) ? 1 : -1;
1076
1077 double E_x = (m_params->longitudinal.pex1 + m_params->longitudinal.pex2 * m_dF_z +
1078 m_params->longitudinal.pex3 * pow(m_dF_z, 2)) *
1079 (1.0 - m_params->longitudinal.pex4 * sign_kap) * m_params->scaling.lex;
1080 double S_Vx = m_Fz * (m_params->longitudinal.pvx1 + m_params->longitudinal.pvx2 * m_dF_z) * m_params->scaling.lvx *
1081 m_params->scaling.lmux * m_zeta->z1;
1082 double F_x =
1083 D_x * std::sin(C_x * std::atan(B_x * kappa_x - E_x * (B_x * kappa_x - std::atan(B_x * kappa_x)))) - S_Vx;
1084
1085 // hold onto these coefs
1086 {
1087 pureLongCoefs tmp = {S_Hx, kappa_x, mu_x, K_x, B_x, C_x, D_x, E_x, F_x, S_Vx};
1088 *m_pureLong = tmp;
1089 }
1090
1091 return F_x;
1092 }
1093
Fy_pureLat(double alpha,double gamma)1094 double ChPacejkaTire::Fy_pureLat(double alpha, double gamma) {
1095 double C_y = m_params->lateral.pcy1 * m_params->scaling.lcy; // > 0
1096 double mu_y = (m_params->lateral.pdy1 + m_params->lateral.pdy2 * m_dF_z) *
1097 (1.0 - m_params->lateral.pdy3 * pow(gamma, 2)) * m_params->scaling.lmuy; // > 0
1098 double D_y = mu_y * m_Fz * m_zeta->z2;
1099
1100 // doesn't make sense to ever have K_y be negative (it can be interpreted as lateral stiffness)
1101 double K_y = m_params->lateral.pky1 * m_params->vertical.fnomin *
1102 std::sin(2.0 * std::atan(m_Fz / (m_params->lateral.pky2 * m_params->vertical.fnomin))) *
1103 (1.0 - m_params->lateral.pky3 * std::abs(gamma)) * m_zeta->z3 * m_params->scaling.lyka;
1104 double B_y = K_y / (C_y * D_y);
1105
1106 // double S_Hy = (m_params->lateral.phy1 + m_params->lateral.phy2 * m_dF_z) * m_params->scaling.lhy + (K_yGamma_0 *
1107 // m_slip->gammaP - S_VyGamma) * m_zeta->z0 / (K_yAlpha + 0.1) + m_zeta->z4 - 1.0;
1108 // Adams S_Hy is a bit different
1109 double S_Hy = (m_params->lateral.phy1 + m_params->lateral.phy2 * m_dF_z) * m_params->scaling.lhy +
1110 (m_params->lateral.phy3 * gamma * m_zeta->z0) + m_zeta->z4 - 1;
1111
1112 double alpha_y = alpha + S_Hy;
1113
1114 int sign_alpha = (alpha_y >= 0) ? 1 : -1;
1115
1116 double E_y = (m_params->lateral.pey1 + m_params->lateral.pey2 * m_dF_z) *
1117 (1.0 - (m_params->lateral.pey3 + m_params->lateral.pey4 * gamma) * sign_alpha) *
1118 m_params->scaling.ley; // + p_Ey5 * pow(gamma,2)
1119 double S_Vy = m_Fz * ((m_params->lateral.pvy1 + m_params->lateral.pvy2 * m_dF_z) * m_params->scaling.lvy +
1120 (m_params->lateral.pvy3 + m_params->lateral.pvy4 * m_dF_z) * gamma) *
1121 m_params->scaling.lmuy * m_zeta->z2;
1122
1123 double F_y =
1124 D_y * std::sin(C_y * std::atan(B_y * alpha_y - E_y * (B_y * alpha_y - std::atan(B_y * alpha_y)))) + S_Vy;
1125
1126 // hold onto coefs
1127 {
1128 pureLatCoefs tmp = {S_Hy, alpha_y, mu_y, K_y, S_Vy, B_y, C_y, D_y, E_y};
1129 *m_pureLat = tmp;
1130 }
1131
1132 return F_y;
1133 }
1134
Mz_pureLat(double alpha,double gamma,double Fy_pureSlip)1135 double ChPacejkaTire::Mz_pureLat(double alpha, double gamma, double Fy_pureSlip) {
1136 // some constants
1137 int sign_Vx = (m_slip->V_cx >= 0) ? 1 : -1;
1138
1139 double S_Hf = m_pureLat->S_Hy + m_pureLat->S_Vy / m_pureLat->K_y;
1140 double alpha_r = alpha + S_Hf;
1141 double S_Ht = m_params->aligning.qhz1 + m_params->aligning.qhz2 * m_dF_z +
1142 (m_params->aligning.qhz3 + m_params->aligning.qhz4 * m_dF_z) * gamma;
1143 double alpha_t = alpha + S_Ht;
1144
1145 double B_r = (m_params->aligning.qbz9 * (m_params->scaling.lky / m_params->scaling.lmuy) +
1146 m_params->aligning.qbz10 * m_pureLat->B_y * m_pureLat->C_y) *
1147 m_zeta->z6;
1148 double C_r = m_zeta->z7;
1149 // no terms (Dz10, Dz11) for gamma^2 term seen in Pacejka
1150 // double D_r = m_Fz*m_R0 * ((m_params->aligning.qdz6 + m_params->aligning.qdz7 *
1151 // m_dF_z)*m_params->scaling.lres*m_zeta->z2 + (m_params->aligning.qdz8 + m_params->aligning.qdz9 *
1152 // m_dF_z)*gamma*m_params->scaling.lgaz*m_zeta->z0) * m_slip->cosPrime_alpha*m_params->scaling.lmuy*sign_Vx +
1153 // m_zeta->z8 - 1.0;
1154 // reference
1155 double D_r = m_Fz * m_R0 * ((m_params->aligning.qdz6 + m_params->aligning.qdz7 * m_dF_z) * m_params->scaling.lres +
1156 (m_params->aligning.qdz8 + m_params->aligning.qdz9 * m_dF_z) * gamma) *
1157 m_params->scaling.lmuy * m_slip->cosPrime_alpha * sign_Vx +
1158 m_zeta->z8 - 1.0;
1159 // qbz4 is not in Pacejka
1160 double B_t =
1161 (m_params->aligning.qbz1 + m_params->aligning.qbz2 * m_dF_z + m_params->aligning.qbz3 * pow(m_dF_z, 2)) *
1162 (1.0 + m_params->aligning.qbz4 * gamma + m_params->aligning.qbz5 * std::abs(gamma)) * m_params->scaling.lvyka /
1163 m_params->scaling.lmuy;
1164 double C_t = m_params->aligning.qcz1;
1165 double D_t0 = m_Fz * (m_R0 / m_params->vertical.fnomin) *
1166 (m_params->aligning.qdz1 + m_params->aligning.qdz2 * m_dF_z) * sign_Vx;
1167 // no abs on qdz3 gamma in reference
1168 double D_t = D_t0 * (1.0 + m_params->aligning.qdz3 * std::abs(gamma) + m_params->aligning.qdz4 * pow(gamma, 2)) *
1169 m_zeta->z5 * m_params->scaling.ltr;
1170 double E_t =
1171 (m_params->aligning.qez1 + m_params->aligning.qez2 * m_dF_z + m_params->aligning.qez3 * pow(m_dF_z, 2)) *
1172 (1.0 +
1173 (m_params->aligning.qez4 + m_params->aligning.qez5 * gamma) * (2.0 / chrono::CH_C_PI) *
1174 std::atan(B_t * C_t * alpha_t));
1175 double t = D_t * std::cos(C_t * std::atan(B_t * alpha_t - E_t * (B_t * alpha_t - std::atan(B_t * alpha_t)))) *
1176 m_slip->cosPrime_alpha;
1177
1178 double MP_z = -t * Fy_pureSlip;
1179 double M_zr = D_r * std::cos(C_r * std::atan(B_r * alpha_r)); // this is in the D_r term: * m_slip->cosPrime_alpha;
1180
1181 double M_z = MP_z + M_zr;
1182
1183 // hold onto coefs
1184 {
1185 pureTorqueCoefs tmp = {
1186 S_Hf, alpha_r, S_Ht, alpha_t, m_slip->cosPrime_alpha, m_pureLat->K_y, B_r, C_r, D_r, B_t, C_t, D_t0, D_t,
1187 E_t, t, MP_z, M_zr};
1188 *m_pureTorque = tmp;
1189 }
1190
1191 return M_z;
1192 }
1193
Fx_combined(double alpha,double gamma,double kappa,double Fx_pureSlip)1194 double ChPacejkaTire::Fx_combined(double alpha, double gamma, double kappa, double Fx_pureSlip) {
1195 double rbx3 = 1.0;
1196
1197 double S_HxAlpha = m_params->longitudinal.rhx1;
1198 double alpha_S = alpha + S_HxAlpha;
1199 double B_xAlpha = (m_params->longitudinal.rbx1 + rbx3 * pow(gamma, 2)) *
1200 std::cos(std::atan(m_params->longitudinal.rbx2 * kappa)) * m_params->scaling.lxal;
1201 double C_xAlpha = m_params->longitudinal.rcx1;
1202 double E_xAlpha = m_params->longitudinal.rex1 + m_params->longitudinal.rex2 * m_dF_z;
1203
1204 // double G_xAlpha0 = std::cos(C_xAlpha * std::atan(B_xAlpha * S_HxAlpha - E_xAlpha * (B_xAlpha * S_HxAlpha -
1205 // std::atan(B_xAlpha * S_HxAlpha)) ) );
1206 double G_xAlpha0 =
1207 std::cos(C_xAlpha *
1208 std::atan(B_xAlpha * S_HxAlpha - E_xAlpha * (B_xAlpha * S_HxAlpha - std::atan(B_xAlpha * S_HxAlpha))));
1209
1210 // double G_xAlpha = std::cos(C_xAlpha * std::atan(B_xAlpha * alpha_S - E_xAlpha * (B_xAlpha * alpha_S -
1211 // std::atan(B_xAlpha * alpha_S)) ) ) / G_xAlpha0;
1212 double G_xAlpha = std::cos(C_xAlpha * std::atan(B_xAlpha * alpha_S -
1213 E_xAlpha * (B_xAlpha * alpha_S - std::atan(B_xAlpha * alpha_S)))) /
1214 G_xAlpha0;
1215
1216 double F_x = G_xAlpha * Fx_pureSlip;
1217
1218 {
1219 combinedLongCoefs tmp = {S_HxAlpha, alpha_S, B_xAlpha, C_xAlpha, E_xAlpha, G_xAlpha0, G_xAlpha};
1220 *m_combinedLong = tmp;
1221 }
1222
1223 return F_x;
1224 }
1225
Fy_combined(double alpha,double gamma,double kappa,double Fy_pureSlip)1226 double ChPacejkaTire::Fy_combined(double alpha, double gamma, double kappa, double Fy_pureSlip) {
1227 double rby4 = 0;
1228
1229 double S_HyKappa = m_params->lateral.rhy1 + m_params->lateral.rhy2 * m_dF_z;
1230 double kappa_S = kappa + S_HyKappa;
1231 double B_yKappa = (m_params->lateral.rby1 + rby4 * pow(gamma, 2)) *
1232 std::cos(std::atan(m_params->lateral.rby2 * (alpha - m_params->lateral.rby3))) *
1233 m_params->scaling.lyka;
1234 double C_yKappa = m_params->lateral.rcy1;
1235 double E_yKappa = m_params->lateral.rey1 + m_params->lateral.rey2 * m_dF_z;
1236 double D_VyKappa = m_pureLat->mu_y * m_Fz *
1237 (m_params->lateral.rvy1 + m_params->lateral.rvy2 * m_dF_z + m_params->lateral.rvy3 * gamma) *
1238 std::cos(std::atan(m_params->lateral.rvy4 * alpha)) * m_zeta->z2;
1239 double S_VyKappa = D_VyKappa * std::sin(m_params->lateral.rvy5 * std::atan(m_params->lateral.rvy6 * kappa)) *
1240 m_params->scaling.lvyka;
1241 double G_yKappa0 =
1242 std::cos(C_yKappa *
1243 std::atan(B_yKappa * S_HyKappa - E_yKappa * (B_yKappa * S_HyKappa - std::atan(B_yKappa * S_HyKappa))));
1244 double G_yKappa = std::cos(C_yKappa * std::atan(B_yKappa * kappa_S -
1245 E_yKappa * (B_yKappa * kappa_S - std::atan(B_yKappa * kappa_S)))) /
1246 G_yKappa0;
1247
1248 double F_y = G_yKappa * Fy_pureSlip + S_VyKappa;
1249
1250 {
1251 combinedLatCoefs tmp = {S_HyKappa, kappa_S, B_yKappa, C_yKappa, E_yKappa,
1252 D_VyKappa, S_VyKappa, G_yKappa0, G_yKappa};
1253 *m_combinedLat = tmp;
1254 }
1255
1256 return F_y;
1257 }
1258
Mz_combined(double alpha_r,double alpha_t,double gamma,double kappa,double Fx_combined,double Fy_combined)1259 double ChPacejkaTire::Mz_combined(double alpha_r,
1260 double alpha_t,
1261 double gamma,
1262 double kappa,
1263 double Fx_combined,
1264 double Fy_combined) {
1265 double FP_y = Fy_combined - m_combinedLat->S_VyKappa;
1266 double s = m_R0 * (m_params->aligning.ssz1 + m_params->aligning.ssz2 * (Fy_combined / m_params->vertical.fnomin) +
1267 (m_params->aligning.ssz3 + m_params->aligning.ssz4 * m_dF_z) * gamma) *
1268 m_params->scaling.ls;
1269 int sign_alpha_t = (alpha_t >= 0) ? 1 : -1;
1270 int sign_alpha_r = (alpha_r >= 0) ? 1 : -1;
1271
1272 double alpha_t_eq =
1273 sign_alpha_t * sqrt(pow(alpha_t, 2) + pow(m_pureLong->K_x / m_pureTorque->K_y, 2) * pow(kappa, 2));
1274 double alpha_r_eq =
1275 sign_alpha_r * sqrt(pow(alpha_r, 2) + pow(m_pureLong->K_x / m_pureTorque->K_y, 2) * pow(kappa, 2));
1276
1277 double M_zr = m_pureTorque->D_r * std::cos(m_pureTorque->C_r * std::atan(m_pureTorque->B_r * alpha_r_eq)) *
1278 m_slip->cosPrime_alpha;
1279 double t =
1280 m_pureTorque->D_t *
1281 std::cos(m_pureTorque->C_t * std::atan(m_pureTorque->B_t * alpha_t_eq -
1282 m_pureTorque->E_t * (m_pureTorque->B_t * alpha_t_eq -
1283 std::atan(m_pureTorque->B_t * alpha_t_eq)))) *
1284 m_slip->cosPrime_alpha;
1285
1286 double M_z_y = -t * FP_y;
1287 double M_z_x = s * Fx_combined;
1288 double M_z = M_z_y + M_zr + M_z_x;
1289
1290 {
1291 combinedTorqueCoefs tmp = {m_slip->cosPrime_alpha, FP_y, s, alpha_t_eq, alpha_r_eq, M_zr, t, M_z_x, M_z_y};
1292 *m_combinedTorque = tmp;
1293 }
1294
1295 return M_z;
1296 }
1297
calc_Mx(double gamma,double Fy_combined)1298 double ChPacejkaTire::calc_Mx(double gamma, double Fy_combined) {
1299 double M_x = 0;
1300 if (m_in_contact) {
1301 // according to Pacejka
1302 M_x = m_Fz * m_R0 * (m_params->overturning.qsx1 - m_params->overturning.qsx2 * gamma +
1303 m_params->overturning.qsx3 * (Fy_combined / m_params->vertical.fnomin)) *
1304 m_params->scaling.lmx;
1305 }
1306
1307 return M_x;
1308 }
1309
calc_My(double Fx_combined)1310 double ChPacejkaTire::calc_My(double Fx_combined) {
1311 double M_y = 0;
1312 if (m_in_contact) {
1313 double V_r = m_tireState.omega * m_R_eff;
1314 M_y = -m_Fz * m_R0 * (m_params->rolling.qsy1 * std::atan(V_r / m_params->model.longvl) +
1315 m_params->rolling.qsy2 * (Fx_combined / m_params->vertical.fnomin)) *
1316 m_params->scaling.lmy;
1317 // reference calc
1318 // M_y = m_R0*m_Fz * (m_params->rolling.qsy1 + m_params->rolling.qsy2*Fx_combined/m_params->vertical.fnomin +
1319 // m_params->rolling.qsy3*std::abs(m_slip->V_cx/m_params->model.longvl) +
1320 // m_params->rolling.qsy4*pow(m_slip->V_cx/m_params->model.longvl,4));
1321 }
1322 return M_y;
1323 }
1324
1325 // -----------------------------------------------------------------------------
1326 // Load a PacTire specification file.
1327 //
1328 // For an example, see the file data/vehicle/hmmwv/tire/HMMWV_pacejka.tir
1329 // -----------------------------------------------------------------------------
loadPacTireParamFile()1330 void ChPacejkaTire::loadPacTireParamFile() {
1331 // try to load the file
1332 std::ifstream inFile(this->getPacTireParamFile().c_str(), std::ios::in);
1333
1334 // if not loaded, say something and exit
1335 if (!inFile.is_open()) {
1336 GetLog() << "\n\n !!!!!!! couldn't load the pac tire file: " << getPacTireParamFile().c_str() << "\n\n";
1337 GetLog() << " pacTire param file opened in a text editor somewhere ??? \n\n\n";
1338 return;
1339 }
1340
1341 // success in opening file, load the data, broken down into sections
1342 // according to what is found in the PacTire input file
1343 readPacTireInput(inFile);
1344
1345 // this bool will allow you to query the pac tire for output
1346 // Forces, moments based on wheel state info.
1347 m_params_defined = true;
1348 }
1349
readPacTireInput(std::ifstream & inFile)1350 void ChPacejkaTire::readPacTireInput(std::ifstream& inFile) {
1351 // advance to the first part of the file with data we need to read
1352 std::string tline;
1353
1354 while (std::getline(inFile, tline)) {
1355 // first main break
1356 if (tline[0] == '$')
1357 break;
1358 }
1359
1360 // to keep things sane (and somewhat orderly), create a function to read
1361 // each subsection. there may be overlap between different PacTire versions,
1362 // where these section read functions can be reused
1363
1364 // 0: [UNITS], all token values are strings
1365 readSection_UNITS(inFile);
1366
1367 // 1: [MODEL]
1368 readSection_MODEL(inFile);
1369
1370 // 2: [DIMENSION]
1371 readSection_DIMENSION(inFile);
1372
1373 // 3: [SHAPE]
1374 readSection_SHAPE(inFile);
1375
1376 // 4: [VERTICAL]
1377 readSection_VERTICAL(inFile);
1378
1379 // 5-8, ranges for: LONG_SLIP, SLIP_ANGLE, INCLINATION_ANGLE, VETRICAL_FORCE,
1380 // in that order
1381 readSection_RANGES(inFile);
1382
1383 // 9: [scaling]
1384 readSection_scaling(inFile);
1385
1386 // 10: [longitudinal]
1387 readSection_longitudinal(inFile);
1388
1389 // 11: [overturning]
1390 readSection_overturning(inFile);
1391
1392 // 12: [lateral]
1393 readSection_lateral(inFile);
1394
1395 // 13: [rolling]
1396 readSection_rolling(inFile);
1397
1398 // 14: [aligning]
1399 readSection_aligning(inFile);
1400 }
1401
readSection_UNITS(std::ifstream & inFile)1402 void ChPacejkaTire::readSection_UNITS(std::ifstream& inFile) {
1403 // skip the first line
1404 std::string tline;
1405 std::getline(inFile, tline);
1406
1407 // string util stuff
1408 std::string tok; // name of token
1409 std::string val_str; // temp for string token values
1410 std::vector<std::string> split;
1411
1412 while (std::getline(inFile, tline)) {
1413 // made it to the next section
1414 if (tline[0] == '$')
1415 break;
1416 }
1417 }
1418
readSection_MODEL(std::ifstream & inFile)1419 void ChPacejkaTire::readSection_MODEL(std::ifstream& inFile) {
1420 // skip the first line
1421 std::string tline;
1422 std::getline(inFile, tline);
1423
1424 // string util stuff
1425 std::vector<std::string> split;
1426 std::string val_str; // string token values
1427
1428 // token value changes type in this section, do it manually
1429 std::getline(inFile, tline);
1430
1431 // get the token / value
1432 split = splitStr(tline, '=');
1433 m_params->model.property_file_format = splitStr(split[1], '\'')[1];
1434
1435 std::getline(inFile, tline);
1436 m_params->model.use_mode = fromTline<int>(tline);
1437
1438 std::getline(inFile, tline);
1439 m_params->model.vxlow = fromTline<double>(tline);
1440
1441 std::getline(inFile, tline);
1442 m_params->model.longvl = fromTline<double>(tline);
1443
1444 std::getline(inFile, tline);
1445 split = splitStr(tline, '=');
1446 m_params->model.tyreside = splitStr(split[1], '\'')[1];
1447 }
1448
readSection_DIMENSION(std::ifstream & inFile)1449 void ChPacejkaTire::readSection_DIMENSION(std::ifstream& inFile) {
1450 // skip the first two lines
1451 std::string tline;
1452 std::getline(inFile, tline);
1453 std::getline(inFile, tline);
1454 // if all the data types are the same in a subsection, life is a little easier
1455 // push each token value to this vector, check the # of items added, then
1456 // create the struct by hand only
1457 std::vector<double> dat;
1458
1459 while (std::getline(inFile, tline)) {
1460 // made it to the next section
1461 if (tline[0] == '$')
1462 break;
1463 dat.push_back(fromTline<double>(tline));
1464 }
1465
1466 if (dat.size() != 5) {
1467 GetLog() << " error reading DIMENSION section of pactire input file!!! \n\n";
1468 return;
1469 }
1470 // right size, create the struct
1471 struct dimension dim = {dat[0], dat[1], dat[2], dat[3], dat[4]};
1472 m_params->dimension = dim;
1473 }
1474
readSection_SHAPE(std::ifstream & inFile)1475 void ChPacejkaTire::readSection_SHAPE(std::ifstream& inFile) {
1476 // skip the first two lines
1477 std::string tline;
1478 std::getline(inFile, tline);
1479 std::getline(inFile, tline);
1480 // if all the data types are the same in a subsection, life is a little easier
1481 // push each token value to this vector, check the # of items added, then
1482 // create the struct by hand only
1483 std::vector<double> rad;
1484 std::vector<double> wid;
1485 std::vector<std::string> split;
1486 while (std::getline(inFile, tline)) {
1487 // made it to the next section
1488 if (tline[0] == '$')
1489 break;
1490 split = splitStr(tline, ' ');
1491 rad.push_back(std::atof(split[1].c_str()));
1492 wid.push_back(std::atof(split[5].c_str()));
1493 }
1494 m_params->shape.radial = rad;
1495 m_params->shape.width = wid;
1496 }
1497
readSection_VERTICAL(std::ifstream & inFile)1498 void ChPacejkaTire::readSection_VERTICAL(std::ifstream& inFile) {
1499 // skip the first line
1500 std::string tline;
1501 std::getline(inFile, tline);
1502
1503 // push each token value to this vector, check the # of items added
1504 std::vector<double> dat;
1505
1506 while (std::getline(inFile, tline)) {
1507 // made it to the next section
1508 if (tline[0] == '$')
1509 break;
1510 dat.push_back(fromTline<double>(tline));
1511 }
1512
1513 if (dat.size() != 6) {
1514 GetLog() << " error reading VERTICAL section of pactire input file!!! \n\n";
1515 return;
1516 }
1517 // right size, create the struct
1518 struct vertical vert = {dat[0], dat[1], dat[2], dat[3], dat[4], dat[5]};
1519 m_params->vertical = vert;
1520 }
1521
readSection_RANGES(std::ifstream & inFile)1522 void ChPacejkaTire::readSection_RANGES(std::ifstream& inFile) {
1523 // skip the first line
1524 std::string tline;
1525 std::getline(inFile, tline);
1526 // if all the data types are the same in a subsection, life is a little easier
1527 // push each token value to this vector, check the # of items added, then
1528 // create the struct by hand only
1529 std::vector<double> dat;
1530
1531 // LONG_SLIP_RANGE
1532 while (std::getline(inFile, tline)) {
1533 // made it to the next section
1534 if (tline[0] == '$')
1535 break;
1536 dat.push_back(fromTline<double>(tline));
1537 }
1538
1539 if (dat.size() != 2) {
1540 GetLog() << " error reading LONG_SLIP_RANGE section of pactire input file!!! \n\n";
1541 return;
1542 }
1543 // right size, create the struct
1544 struct long_slip_range long_slip = {dat[0], dat[1]};
1545 m_params->long_slip_range = long_slip;
1546 dat.clear();
1547 std::getline(inFile, tline);
1548
1549 // SLIP_ANGLE_RANGE
1550 while (std::getline(inFile, tline)) {
1551 // made it to the next section
1552 if (tline[0] == '$')
1553 break;
1554 dat.push_back(fromTline<double>(tline));
1555 }
1556
1557 if (dat.size() != 2) {
1558 GetLog() << " error reading LONG_SLIP_RANGE section of pactire input file!!! \n\n";
1559 return;
1560 }
1561 // right size, create the struct
1562 struct slip_angle_range slip_ang = {dat[0], dat[1]};
1563 m_params->slip_angle_range = slip_ang;
1564 dat.clear();
1565 std::getline(inFile, tline);
1566
1567 // INCLINATION_ANGLE_RANGE
1568 while (std::getline(inFile, tline)) {
1569 // made it to the next section
1570 if (tline[0] == '$')
1571 break;
1572 dat.push_back(fromTline<double>(tline));
1573 }
1574
1575 if (dat.size() != 2) {
1576 GetLog() << " error reading INCLINATION_ANGLE_RANGE section of pactire input file!!! \n\n";
1577 return;
1578 }
1579 struct inclination_angle_range incl_ang = {dat[0], dat[1]};
1580 m_params->inclination_angle_range = incl_ang;
1581 dat.clear();
1582 std::getline(inFile, tline);
1583
1584 // VERTICAL_FORCE_RANGE
1585 while (std::getline(inFile, tline)) {
1586 // made it to the next section
1587 if (tline[0] == '$')
1588 break;
1589 dat.push_back(fromTline<double>(tline));
1590 }
1591
1592 if (dat.size() != 2) {
1593 GetLog() << " error reading VERTICAL_FORCE_RANGE section of pactire input file!!! \n\n";
1594 return;
1595 }
1596 struct vertical_force_range vert_range = {dat[0], dat[1]};
1597 m_params->vertical_force_range = vert_range;
1598 }
1599
readSection_scaling(std::ifstream & inFile)1600 void ChPacejkaTire::readSection_scaling(std::ifstream& inFile) {
1601 std::string tline;
1602 std::getline(inFile, tline);
1603 // if all the data types are the same in a subsection, life is a little easier
1604 // push each token value to this vector, check the # of items added, then
1605 // create the struct by hand only
1606 std::vector<double> dat;
1607
1608 while (std::getline(inFile, tline)) {
1609 // made it to the next section
1610 if (tline[0] == '$')
1611 break;
1612 dat.push_back(fromTline<double>(tline));
1613 }
1614
1615 if (dat.size() != 28) {
1616 GetLog() << " error reading scaling section of pactire input file!!! \n\n";
1617 return;
1618 }
1619 struct scaling_coefficients coefs = {dat[0], dat[1], dat[2], dat[3], dat[4], dat[5], dat[6],
1620 dat[7], dat[8], dat[9], dat[10], dat[11], dat[12], dat[13],
1621 dat[14], dat[15], dat[16], dat[17], dat[18], dat[19], dat[20],
1622 dat[21], dat[22], dat[23], dat[24], dat[25], dat[26], dat[27]};
1623 m_params->scaling = coefs;
1624 }
1625
readSection_longitudinal(std::ifstream & inFile)1626 void ChPacejkaTire::readSection_longitudinal(std::ifstream& inFile) {
1627 std::string tline;
1628 std::getline(inFile, tline);
1629 std::vector<double> dat;
1630
1631 while (std::getline(inFile, tline)) {
1632 // made it to the next section
1633 if (tline[0] == '$')
1634 break;
1635 dat.push_back(fromTline<double>(tline));
1636 }
1637
1638 if (dat.size() != 24) {
1639 GetLog() << " error reading longitudinal section of pactire input file!!! \n\n";
1640 return;
1641 }
1642 struct longitudinal_coefficients coefs = {dat[0], dat[1], dat[2], dat[3], dat[4], dat[5], dat[6], dat[7],
1643 dat[8], dat[9], dat[10], dat[11], dat[12], dat[13], dat[14], dat[15],
1644 dat[16], dat[17], dat[18], dat[19], dat[20], dat[21], dat[22], dat[23]};
1645 m_params->longitudinal = coefs;
1646 }
1647
readSection_overturning(std::ifstream & inFile)1648 void ChPacejkaTire::readSection_overturning(std::ifstream& inFile) {
1649 std::string tline;
1650 std::getline(inFile, tline);
1651 std::vector<double> dat;
1652
1653 while (std::getline(inFile, tline)) {
1654 // made it to the next section
1655 if (tline[0] == '$')
1656 break;
1657 dat.push_back(fromTline<double>(tline));
1658 }
1659
1660 if (dat.size() != 3) {
1661 GetLog() << " error reading overturning section of pactire input file!!! \n\n";
1662 return;
1663 }
1664 struct overturning_coefficients coefs = {dat[0], dat[1], dat[2]};
1665 m_params->overturning = coefs;
1666 }
1667
readSection_lateral(std::ifstream & inFile)1668 void ChPacejkaTire::readSection_lateral(std::ifstream& inFile) {
1669 std::string tline;
1670 std::getline(inFile, tline);
1671 std::vector<double> dat;
1672
1673 while (std::getline(inFile, tline)) {
1674 // made it to the next section
1675 if (tline[0] == '$')
1676 break;
1677 dat.push_back(fromTline<double>(tline));
1678 }
1679
1680 if (dat.size() != 34) {
1681 GetLog() << " error reading lateral section of pactire input file!!! \n\n";
1682 return;
1683 }
1684 struct lateral_coefficients coefs = {
1685 dat[0], dat[1], dat[2], dat[3], dat[4], dat[5], dat[6], dat[7], dat[8], dat[9], dat[10], dat[11],
1686 dat[12], dat[13], dat[14], dat[15], dat[16], dat[17], dat[18], dat[19], dat[20], dat[21], dat[22], dat[23],
1687 dat[24], dat[25], dat[26], dat[27], dat[28], dat[29], dat[30], dat[31], dat[32], dat[33]};
1688 m_params->lateral = coefs;
1689 }
1690
readSection_rolling(std::ifstream & inFile)1691 void ChPacejkaTire::readSection_rolling(std::ifstream& inFile) {
1692 std::string tline;
1693 std::getline(inFile, tline);
1694 std::vector<double> dat;
1695
1696 while (std::getline(inFile, tline)) {
1697 // made it to the next section
1698 if (tline[0] == '$')
1699 break;
1700 dat.push_back(fromTline<double>(tline));
1701 }
1702
1703 if (dat.size() != 4) {
1704 GetLog() << " error reading rolling section of pactire input file!!! \n\n";
1705 return;
1706 }
1707 struct rolling_coefficients coefs = {dat[0], dat[1], dat[2], dat[3]};
1708 m_params->rolling = coefs;
1709 }
1710
readSection_aligning(std::ifstream & inFile)1711 void ChPacejkaTire::readSection_aligning(std::ifstream& inFile) {
1712 std::string tline;
1713 std::getline(inFile, tline);
1714 std::vector<double> dat;
1715
1716 while (std::getline(inFile, tline)) {
1717 // made it to the next section
1718 if (tline[0] == '$')
1719 break;
1720 dat.push_back(fromTline<double>(tline));
1721 }
1722
1723 if (dat.size() != 31) {
1724 GetLog() << " error reading LONG_SLIP_RANGE section of pactire input file!!! \n\n";
1725 return;
1726 }
1727 struct aligning_coefficients coefs = {dat[0], dat[1], dat[2], dat[3], dat[4], dat[5], dat[6], dat[7],
1728 dat[8], dat[9], dat[10], dat[11], dat[12], dat[13], dat[14], dat[15],
1729 dat[16], dat[17], dat[18], dat[19], dat[20], dat[21], dat[22], dat[23],
1730 dat[24], dat[25], dat[26], dat[27], dat[28], dat[29], dat[30]};
1731 m_params->aligning = coefs;
1732 }
1733
1734 // -----------------------------------------------------------------------------
1735 // Functions providing access to private structures
1736 // -----------------------------------------------------------------------------
get_kappa() const1737 double ChPacejkaTire::get_kappa() const {
1738 return m_slip->kappa;
1739 }
1740
get_alpha() const1741 double ChPacejkaTire::get_alpha() const {
1742 return m_slip->alpha;
1743 }
1744
get_gamma() const1745 double ChPacejkaTire::get_gamma() const {
1746 return m_slip->gamma;
1747 }
1748
get_kappaPrime() const1749 double ChPacejkaTire::get_kappaPrime() const {
1750 return m_slip->kappaP;
1751 }
1752
get_alphaPrime() const1753 double ChPacejkaTire::get_alphaPrime() const {
1754 return m_slip->alphaP;
1755 }
1756
get_gammaPrime() const1757 double ChPacejkaTire::get_gammaPrime() const {
1758 return m_slip->gammaP;
1759 }
1760
get_min_long_slip() const1761 double ChPacejkaTire::get_min_long_slip() const {
1762 return m_params->long_slip_range.kpumin;
1763 }
1764
get_max_long_slip() const1765 double ChPacejkaTire::get_max_long_slip() const {
1766 return m_params->long_slip_range.kpumax;
1767 }
1768
get_min_lat_slip() const1769 double ChPacejkaTire::get_min_lat_slip() const {
1770 return m_params->slip_angle_range.alpmin;
1771 }
1772
get_max_lat_slip() const1773 double ChPacejkaTire::get_max_lat_slip() const {
1774 return m_params->slip_angle_range.alpmax;
1775 }
1776
get_longvl() const1777 double ChPacejkaTire::get_longvl() const {
1778 return m_params->model.longvl;
1779 }
1780
1781 // -----------------------------------------------------------------------------
1782 // Write output file for post-processing with the Python pandas module.
1783 // -----------------------------------------------------------------------------
WriteOutData(double time,const std::string & outFilename)1784 void ChPacejkaTire::WriteOutData(double time, const std::string& outFilename) {
1785 // first time thru, write headers
1786 if (m_Num_WriteOutData == 0) {
1787 m_outFilename = outFilename;
1788 std::ofstream oFile(outFilename.c_str(), std::ios_base::out);
1789 if (!oFile.is_open()) {
1790 GetLog() << " couldn't open file for writing: " << outFilename << " \n\n";
1791 return;
1792 } else {
1793 // write the headers, Fx, Fy are pure forces, Fxc and Fyc are the combined forces
1794 oFile << "time,kappa,alpha,gamma,kappaP,alphaP,gammaP,Vx,Vy,omega,Fx,Fy,Fz,Mx,My,Mz,Fxc,Fyc,Mzc,Mzx,Mzy,M_"
1795 "zrc,contact,m_Fz,m_dF_z,u,valpha,vgamma,vphi,du,dvalpha,dvgamma,dvphi,R0,R_l,Reff,MP_z,M_zr,t,s,"
1796 "FX,FY,FZ,MX,MY,MZ,u_Bessel,u_sigma,v_Bessel,v_sigma"
1797 << std::endl;
1798 m_Num_WriteOutData++;
1799 oFile.close();
1800 }
1801 } else {
1802 // already written the headers, just increment the function counter
1803 m_Num_WriteOutData++;
1804 }
1805 // ensure file was able to be opened, headers are written
1806 if (m_Num_WriteOutData > 0) {
1807 // open file, append
1808 std::ofstream appFile(outFilename.c_str(), std::ios_base::app);
1809 // global force/moments applied to wheel rigid body
1810 TerrainForce global_FM = GetTireForce_combinedSlip(false);
1811 // write the slip info, reaction forces for pure & combined slip cases
1812 appFile << time << "," << m_slip->kappa << "," << m_slip->alpha * 180. / 3.14159 << "," << m_slip->gamma << ","
1813 << m_slip->kappaP << "," << m_slip->alphaP << "," << m_slip->gammaP << "," << m_slip->V_cx << ","
1814 << m_slip->V_cy << "," << m_tireState.omega
1815 << "," // m_tireState.lin_vel.x() <<","<< m_tireState.lin_vel.y() <<","
1816 << m_FM_pure.force.x() << "," << m_FM_pure.force.y() << "," << m_FM_pure.force.z() << ","
1817 << m_FM_pure.moment.x() << "," << m_FM_pure.moment.y() << "," << m_FM_pure.moment.z() << ","
1818 << m_FM_combined.force.x() << "," << m_FM_combined.force.y() << "," << m_FM_combined.moment.z() << ","
1819 << m_combinedTorque->M_z_x << "," << m_combinedTorque->M_z_y << "," << m_combinedTorque->M_zr << ","
1820 << (int)m_in_contact << "," << m_Fz << "," << m_dF_z << "," << m_slip->u << "," << m_slip->v_alpha
1821 << "," << m_slip->v_gamma << "," << m_slip->v_phi << "," << m_slip->Idu_dt << ","
1822 << m_slip->Idv_alpha_dt << "," << m_slip->Idv_gamma_dt << "," << m_slip->Idv_phi_dt << "," << m_R0
1823 << "," << m_R_l << "," << m_R_eff << "," << m_pureTorque->MP_z << "," << m_pureTorque->M_zr << ","
1824 << m_combinedTorque->t << "," << m_combinedTorque->s << "," << global_FM.force.x() << ","
1825 << global_FM.force.y() << "," << global_FM.force.z() << "," << global_FM.moment.x() << ","
1826 << global_FM.moment.y() << "," << global_FM.moment.z() << "," << m_bessel->u_Bessel << ","
1827 << m_bessel->u_sigma << "," << m_bessel->v_Bessel << "," << m_bessel->v_sigma << std::endl;
1828 // close the file
1829 appFile.close();
1830 }
1831 }
1832
1833 // -----------------------------------------------------------------------------
1834 // Utility function for validation studies.
1835 //
1836 // Calculate a wheel state consistent with the specified kinematic slip
1837 // quantities (kappa, alpha, and gamma) and magnitude of tangential velocity.
1838 // Out of the infinitely many consistent wheel states, we
1839 // - set position at origin
1840 // - set linear velocity along global X direction with given magnitude
1841 // - set orientation based on given alpha and gamma
1842 // - set omega from given kappa and using current R_eff
1843 // - set angular velocity along the local wheel Y axis
1844 // -----------------------------------------------------------------------------
getState_from_KAG(double kappa,double alpha,double gamma,double Vx)1845 WheelState ChPacejkaTire::getState_from_KAG(double kappa, double alpha, double gamma, double Vx) {
1846 WheelState state;
1847
1848 // Set wheel position at origin.
1849 state.pos = ChVector<>(0, 0, 0);
1850
1851 // Set the wheel velocity aligned with the global X axis.
1852 state.lin_vel = ChVector<>(Vx, 0, 0);
1853
1854 // Rotate wheel to satisfy specified camber angle and slip angle. For this,
1855 // we first rotate the wheel about the global Z-axis by an angle (-alpha),
1856 // followed by a rotation about the new X-axis by an angle gamma.
1857 state.rot = Q_from_AngZ(-alpha) * Q_from_AngX(gamma);
1858
1859 // Calculate forward tangential velocity.
1860 double Vcx = Vx * std::cos(alpha);
1861
1862 // Set the wheel angular velocity about its axis, calculated from the given
1863 // value kappa and using the current value m_R_eff.
1864 // Note that here we assume that the specified velocity is such that Vcx is
1865 // larger than the threshold model.vxlow.
1866 state.omega = (kappa * std::abs(Vcx) + Vcx) / m_R_eff;
1867
1868 // Set the wheel angular velocity (expressed in global frame).
1869 state.ang_vel = state.rot.RotateBack(ChVector<>(0, state.omega, 0));
1870
1871 return state;
1872 }
1873
1874 } // end namespace vehicle
1875 } // end namespace chrono
1876