DefaultFriction.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/DefaultFriction.h>
00013 
00014 #include <rapidxml.hpp>
00015 #include "xml_utils.h"
00016 
00017 using namespace mvsim;
00018 
00019 DefaultFriction::DefaultFriction(
00020         VehicleBase& my_vehicle, const rapidxml::xml_node<char>* node)
00021         : FrictionBase(my_vehicle), m_mu(0.8), m_C_damping(1.0)
00022 {
00023         // Sanity: we can tolerate node==NULL (=> means use default params).
00024         if (node && 0 != strcmp(node->name(), "friction"))
00025                 throw std::runtime_error(
00026                         "<friction>...</friction> XML node was expected!!");
00027 
00028         if (node)
00029         {
00030                 // Parse params:
00031                 std::map<std::string, TParamEntry> params;
00032                 params["mu"] = TParamEntry("%lf", &m_mu);
00033                 params["C_damping"] = TParamEntry("%lf", &m_C_damping);
00034 
00035                 // Parse XML params:
00036                 parse_xmlnode_children_as_param(*node, params);
00037         }
00038 }
00039 
00040 // See docs in base class.
00041 void DefaultFriction::evaluate_friction(
00042         const FrictionBase::TFrictionInput& input,
00043         mrpt::math::TPoint2D& out_result_force_local) const
00044 {
00045         // Rotate wheel velocity vector from veh. frame => wheel frame
00046         const mrpt::poses::CPose2D wRot(0, 0, input.wheel.yaw);
00047         const mrpt::poses::CPose2D wRotInv(0, 0, -input.wheel.yaw);
00048         mrpt::math::TPoint2D vel_w;
00049         wRotInv.composePoint(input.wheel_speed, vel_w);
00050 
00051         // Action/Reaction, slippage, etc:
00052         // --------------------------------------
00053         const double mu = m_mu;
00054         const double gravity = m_my_vehicle.getWorldObject()->get_gravity();
00055         const double partial_mass = input.weight / gravity + input.wheel.mass;
00056         const double max_friction = mu * partial_mass * gravity;
00057 
00058         // 1) Lateral friction (decoupled sub-problem)
00059         // --------------------------------------------
00060         double wheel_lat_friction = 0.0;  // direction: +y local wrt the wheel
00061         {
00062                 // Impulse required to step the lateral slippage:
00063                 wheel_lat_friction = -vel_w.y * partial_mass / input.context.dt;
00064 
00065                 wheel_lat_friction =
00066                         b2Clamp(wheel_lat_friction, -max_friction, max_friction);
00067         }
00068 
00069         // 2) Longitudinal friction (decoupled sub-problem)
00070         // -------------------------------------------------
00071         double wheel_long_friction = 0.0;  // direction: +x local wrt the wheel
00072 
00073         // (eq. 1)==> desired impulse in wheel spinning speed.
00074         // wheel_C_lon_vel = vel_w.x - input.wheel.w * 0.5*input.wheel.diameter
00075 
00076         // It should be = 0 for no slippage (nonholonomic constraint): find out
00077         // required wheel \omega:case '4':
00078         const double R = 0.5 * input.wheel.diameter;  // Wheel radius
00079         const double lon_constraint_desired_wheel_w = vel_w.x / R;
00080         const double desired_wheel_w_impulse =
00081                 (lon_constraint_desired_wheel_w - input.wheel.getW());
00082         const double desired_wheel_alpha =
00083                 desired_wheel_w_impulse / input.context.dt;
00084 
00085         // (eq. 3)==> Find out F_r
00086         // Iyy_w * \Delta\omega_w = dt*\tau-  R*dt*Fri    -C_damp * \omega_w * dt
00087         // "Damping" / internal friction of the wheel's shaft, etc.
00088         const double C_damping = m_C_damping;
00089         // const mrpt::math::TPoint2D wheel_damping(- C_damping *
00090         // input.wheel_speed.x, 0.0);
00091 
00092         const double I_yy = input.wheel.Iyy;
00093         double F_friction_lon = (input.motor_torque - I_yy * desired_wheel_alpha -
00094                                                          C_damping * input.wheel.getW()) /
00095                                                         R;
00096 
00097         // Slippage: The friction with the ground is not infinite:
00098         F_friction_lon = b2Clamp(F_friction_lon, -max_friction, max_friction);
00099 
00100         // Recalc wheel ang. velocity impulse with this reduced force:
00101         const double actual_wheel_alpha = (input.motor_torque - R * F_friction_lon -
00102                                                                            C_damping * input.wheel.getW()) /
00103                                                                           I_yy;
00104 
00105         // Apply impulse to wheel's spinning:
00106         input.wheel.setW(
00107                 input.wheel.getW() + actual_wheel_alpha * input.context.dt);
00108 
00109         wheel_long_friction = F_friction_lon;
00110 
00111         // Resultant force: In local (x,y) coordinates (Newtons) wrt the Wheel
00112         // -----------------------------------------------------------------------
00113         const mrpt::math::TPoint2D result_force_wrt_wheel(
00114                 wheel_long_friction, wheel_lat_friction);
00115 
00116         // Rotate to put: Wheel frame ==> vehicle local framework:
00117         wRot.composePoint(result_force_wrt_wheel, out_result_force_local);
00118 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:48