WardIagnemmaFriction.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #include <mvsim/World.h>
00011 #include <mvsim/VehicleBase.h>
00012 #include <mvsim/FrictionModels/WardIagnemmaFriction.h>
00013 
00014 #include <rapidxml.hpp>
00015 #include "xml_utils.h"
00016 
00017 using namespace mvsim;
00018 
00019 static double sign(double x) { return (double)((x > 0) - (x < 0)); }
00020 WardIagnemmaFriction::WardIagnemmaFriction(
00021         VehicleBase& my_vehicle, const rapidxml::xml_node<char>* node)
00022         : FrictionBase(my_vehicle),
00023           m_mu(0.8),
00024           m_C_damping(1.0),
00025           m_A_roll(50),
00026           m_R1(0.08),
00027           m_R2(0.05)
00028 {
00029         // Sanity: we can tolerate node==NULL (=> means use default params).
00030         if (node && 0 != strcmp(node->name(), "friction"))
00031                 throw std::runtime_error(
00032                         "<friction>...</friction> XML node was expected!!");
00033 
00034         if (node)
00035         {
00036                 // Parse params:
00037                 std::map<std::string, TParamEntry> params;
00038                 params["mu"] = TParamEntry("%lf", &m_mu);
00039                 params["C_damping"] = TParamEntry("%lf", &m_C_damping);
00040                 params["A_roll"] = TParamEntry("%lf", &m_A_roll);
00041                 params["R1"] = TParamEntry("%lf", &m_R1);
00042                 params["R2"] = TParamEntry("%lf", &m_R2);
00043                 // Parse XML params:
00044                 parse_xmlnode_children_as_param(*node, params);
00045         }
00046 
00047         MRPT_UNSCOPED_LOGGER_START;
00048         MRPT_LOG_DEBUG("WardIagnemma Creates!");
00049         MRPT_UNSCOPED_LOGGER_END;
00050 }
00051 
00052 // See docs in base class.
00053 void WardIagnemmaFriction::evaluate_friction(
00054         const FrictionBase::TFrictionInput& input,
00055         mrpt::math::TPoint2D& out_result_force_local) const
00056 {
00057         // Rotate wheel velocity vector from veh. frame => wheel frame
00058         const mrpt::poses::CPose2D wRot(0, 0, input.wheel.yaw);
00059         const mrpt::poses::CPose2D wRotInv(0, 0, -input.wheel.yaw);
00060         mrpt::math::TPoint2D vel_w;
00061         wRotInv.composePoint(input.wheel_speed, vel_w);
00062 
00063         // Action/Reaction, slippage, etc:
00064         // --------------------------------------
00065         const double mu = m_mu;
00066         const double gravity = m_my_vehicle.getWorldObject()->get_gravity();
00067         const double partial_mass = input.weight / gravity + input.wheel.mass;
00068         const double max_friction = mu * partial_mass * gravity;
00069 
00070         // 1) Lateral friction (decoupled sub-problem)
00071         // --------------------------------------------
00072         double wheel_lat_friction = 0.0;  // direction: +y local wrt the wheel
00073         {
00074                 // Impulse required to step the lateral slippage:
00075                 wheel_lat_friction = -vel_w.y * partial_mass / input.context.dt;
00076 
00077                 wheel_lat_friction =
00078                         b2Clamp(wheel_lat_friction, -max_friction, max_friction);
00079         }
00080 
00081         // 2) Longitudinal friction (decoupled sub-problem)
00082         // -------------------------------------------------
00083         double wheel_long_friction = 0.0;  // direction: +x local wrt the wheel
00084 
00085         // (eq. 1)==> desired impulse in wheel spinning speed.
00086         // wheel_C_lon_vel = vel_w.x - input.wheel.w * 0.5*input.wheel.diameter
00087 
00088         // It should be = 0 for no slippage (nonholonomic constraint): find out
00089         // required wheel \omega:case '4':
00090         const double R = 0.5 * input.wheel.diameter;  // Wheel radius
00091         const double lon_constraint_desired_wheel_w = vel_w.x / R;
00092         const double desired_wheel_w_impulse =
00093                 (lon_constraint_desired_wheel_w - input.wheel.getW());
00094         const double desired_wheel_alpha =
00095                 desired_wheel_w_impulse / input.context.dt;
00096 
00097         // (eq. 3)==> Find out F_r
00098         // Iyy_w * \Delta\omega_w = dt*\tau-  R*dt*Fri    -C_damp * \omega_w * dt
00099         // "Damping" / internal friction of the wheel's shaft, etc.
00100         const double C_damping = m_C_damping;
00101         // const mrpt::math::TPoint2D wheel_damping(- C_damping *
00102         // input.wheel_speed.x, 0.0);
00103 
00104         // Actually, Ward-Iagnemma rolling resistance is here (longitudal one):
00105 
00106         const double F_rr =
00107                 -sign(vel_w.x) * partial_mass * gravity *
00108                 (m_R1 * (1 - exp(-m_A_roll * fabs(vel_w.x))) + m_R2 * fabs(vel_w.x));
00109 
00110         if (!m_logger.expired())
00111         {
00112                 m_logger.lock()->updateColumn("F_rr", F_rr);
00113         }
00114 
00115         const double I_yy = input.wheel.Iyy;
00116         //                                  There are torques this is force   v
00117         double F_friction_lon = (input.motor_torque - I_yy * desired_wheel_alpha -
00118                                                          C_damping * input.wheel.getW()) /
00119                                                                 R +
00120                                                         F_rr;
00121 
00122         // Slippage: The friction with the ground is not infinite:
00123         F_friction_lon = b2Clamp(F_friction_lon, -max_friction, max_friction);
00124 
00125         // Recalc wheel ang. velocity impulse with this reduced force:
00126         const double actual_wheel_alpha = (input.motor_torque - R * F_friction_lon -
00127                                                                            C_damping * input.wheel.getW()) /
00128                                                                           I_yy;
00129 
00130         // Apply impulse to wheel's spinning:
00131         input.wheel.setW(
00132                 input.wheel.getW() + actual_wheel_alpha * input.context.dt);
00133 
00134         wheel_long_friction = F_friction_lon;
00135 
00136         // Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel
00137         // -----------------------------------------------------------------------
00138         const mrpt::math::TPoint2D result_force_wrt_wheel(
00139                 wheel_long_friction, wheel_lat_friction);
00140 
00141         // Rotate to put: Wheel frame ==> vehicle local framework:
00142         wRot.composePoint(result_force_wrt_wheel, out_result_force_local);
00143 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35