WardIagnemmaFriction.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014-2020 Jose Luis Blanco Claraco |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under 3-clause BSD License |
7  | See COPYING |
8  +-------------------------------------------------------------------------+ */
9 
11 #include <mvsim/VehicleBase.h>
12 #include <mvsim/World.h>
13 
14 #include <rapidxml.hpp>
15 #include "xml_utils.h"
16 
17 using namespace mvsim;
18 
19 static double sign(double x) { return (double)((x > 0) - (x < 0)); }
21  VehicleBase& my_vehicle, const rapidxml::xml_node<char>* node)
22  : FrictionBase(my_vehicle),
23  m_mu(0.8),
24  m_C_damping(1.0),
25  m_A_roll(50),
26  m_R1(0.08),
27  m_R2(0.05)
28 {
29  // Sanity: we can tolerate node==nullptr (=> means use default params).
30  if (node && 0 != strcmp(node->name(), "friction"))
31  throw std::runtime_error(
32  "<friction>...</friction> XML node was expected!!");
33 
34  if (node)
35  {
36  // Parse params:
38  params["mu"] = TParamEntry("%lf", &m_mu);
39  params["C_damping"] = TParamEntry("%lf", &m_C_damping);
40  params["A_roll"] = TParamEntry("%lf", &m_A_roll);
41  params["R1"] = TParamEntry("%lf", &m_R1);
42  params["R2"] = TParamEntry("%lf", &m_R2);
43  // Parse XML params:
44  parse_xmlnode_children_as_param(*node, params);
45  }
46 
47  MRPT_UNSCOPED_LOGGER_START;
48  MRPT_LOG_DEBUG("WardIagnemma Creates!");
49  MRPT_UNSCOPED_LOGGER_END;
50 }
51 
52 // See docs in base class.
55  mrpt::math::TPoint2D& out_result_force_local) const
56 {
57  // Rotate wheel velocity vector from veh. frame => wheel frame
58  const mrpt::poses::CPose2D wRot(0, 0, input.wheel.yaw);
59  const mrpt::poses::CPose2D wRotInv(0, 0, -input.wheel.yaw);
61  wRotInv.composePoint(input.wheel_speed, vel_w);
62 
63  // Action/Reaction, slippage, etc:
64  // --------------------------------------
65  const double mu = m_mu;
66  const double gravity = m_my_vehicle.getWorldObject()->get_gravity();
67  const double partial_mass = input.weight / gravity + input.wheel.mass;
68  const double max_friction = mu * partial_mass * gravity;
69 
70  // 1) Lateral friction (decoupled sub-problem)
71  // --------------------------------------------
72  double wheel_lat_friction = 0.0; // direction: +y local wrt the wheel
73  {
74  // Impulse required to step the lateral slippage:
75  wheel_lat_friction = -vel_w.y * partial_mass / input.context.dt;
76 
77  wheel_lat_friction =
78  b2Clamp(wheel_lat_friction, -max_friction, max_friction);
79  }
80 
81  // 2) Longitudinal friction (decoupled sub-problem)
82  // -------------------------------------------------
83  double wheel_long_friction = 0.0; // direction: +x local wrt the wheel
84 
85  // (eq. 1)==> desired impulse in wheel spinning speed.
86  // wheel_C_lon_vel = vel_w.x - input.wheel.w * 0.5*input.wheel.diameter
87 
88  // It should be = 0 for no slippage (nonholonomic constraint): find out
89  // required wheel \omega:case '4':
90  const double R = 0.5 * input.wheel.diameter; // Wheel radius
91  const double lon_constraint_desired_wheel_w = vel_w.x / R;
92  const double desired_wheel_w_impulse =
93  (lon_constraint_desired_wheel_w - input.wheel.getW());
94  const double desired_wheel_alpha =
95  desired_wheel_w_impulse / input.context.dt;
96 
97  // (eq. 3)==> Find out F_r
98  // Iyy_w * \Delta\omega_w = dt*\tau- R*dt*Fri -C_damp * \omega_w * dt
99  // "Damping" / internal friction of the wheel's shaft, etc.
100  const double C_damping = m_C_damping;
101  // const mrpt::math::TPoint2D wheel_damping(- C_damping *
102  // input.wheel_speed.x, 0.0);
103 
104  // Actually, Ward-Iagnemma rolling resistance is here (longitudal one):
105 
106  const double F_rr =
107  -sign(vel_w.x) * partial_mass * gravity *
108  (m_R1 * (1 - exp(-m_A_roll * fabs(vel_w.x))) + m_R2 * fabs(vel_w.x));
109 
110  if (!m_logger.expired())
111  {
112  m_logger.lock()->updateColumn("F_rr", F_rr);
113  }
114 
115  const double I_yy = input.wheel.Iyy;
116  // There are torques this is force v
117  double F_friction_lon = (input.motor_torque - I_yy * desired_wheel_alpha -
118  C_damping * input.wheel.getW()) /
119  R +
120  F_rr;
121 
122  // Slippage: The friction with the ground is not infinite:
123  F_friction_lon = b2Clamp(F_friction_lon, -max_friction, max_friction);
124 
125  // Recalc wheel ang. velocity impulse with this reduced force:
126  const double actual_wheel_alpha = (input.motor_torque - R * F_friction_lon -
127  C_damping * input.wheel.getW()) /
128  I_yy;
129 
130  // Apply impulse to wheel's spinning:
131  input.wheel.setW(
132  input.wheel.getW() + actual_wheel_alpha * input.context.dt);
133 
134  wheel_long_friction = F_friction_lon;
135 
136  // Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel
137  // -----------------------------------------------------------------------
138  const mrpt::math::TPoint2D result_force_wrt_wheel(
139  wheel_long_friction, wheel_lat_friction);
140 
141  // Rotate to put: Wheel frame ==> vehicle local framework:
142  wRot.composePoint(result_force_wrt_wheel, out_result_force_local);
143 }
This file contains rapidxml parser and DOM implementation.
void setW(double val)
Spinning velocity (rad/s) wrt shaft.
Definition: Wheel.h:70
std::map< std::string, TParamEntry > TParameterDefinitions
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const TParameterDefinitions &params, const std::map< std::string, std::string > &variableNamesValues={}, const char *functionNameContext="")
Definition: xml_utils.cpp:179
GLenum GLenum GLenum input
void composePoint(double lx, double ly, double &gx, double &gy) const
WardIagnemmaFriction(VehicleBase &my_vehicle, const rapidxml::xml_node< char > *node)
std::weak_ptr< CSVLogger > m_logger
Definition: FrictionBase.h:68
#define MRPT_LOG_DEBUG(_STRING)
static double sign(double x)
const TSimulContext & context
Definition: FrictionBase.h:35
double get_gravity() const
Definition: World.h:82
double dt
timestep
Definition: basic_types.h:61
World * getWorldObject()
Definition: VisualObject.h:34
double getW() const
Spinning velocity (rad/s) wrt shaft.
Definition: Wheel.h:69
GLint GLint GLint GLint GLint x
Ch * name() const
Definition: rapidxml.hpp:673
double m_C_damping
For wheels "internal friction" (N*m*s/rad)
double diameter
Definition: Wheel.h:39
virtual void evaluate_friction(const FrictionBase::TFrictionInput &input, mrpt::math::TPoint2D &out_result_force_local) const override
double yaw
Definition: Wheel.h:36
double Iyy
Definition: Wheel.h:44
double mass
[kg]
Definition: Wheel.h:40
mrpt::math::TPoint2D wheel_speed
Definition: FrictionBase.h:42
T b2Clamp(T a, T low, T high)
Definition: b2Math.h:653
double motor_torque
(Newtons), excluding the weight of the wheel itself.
Definition: FrictionBase.h:39
GLfloat * params
double m_mu
friction coeficient (non-dimensional)
double m_R2
Ward-Iagnemma rolling resistance coefficient.
VehicleBase & m_my_vehicle
Definition: FrictionBase.h:66
const float R


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51