VehicleDifferential.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/VehicleDynamics/VehicleDifferential.h>
00011 #include <mvsim/World.h>
00012 
00013 #include "xml_utils.h"
00014 
00015 #include <mrpt/opengl/COpenGLScene.h>
00016 #include <rapidxml.hpp>
00017 
00018 using namespace mvsim;
00019 using namespace std;
00020 
00021 // Ctor:
00022 DynamicsDifferential::DynamicsDifferential(World* parent)
00023         : VehicleBase(parent, 2 /*num wheels*/)
00024 {
00025         using namespace mrpt::math;
00026 
00027         m_chassis_mass = 15.0;
00028         m_chassis_z_min = 0.05;
00029         m_chassis_z_max = 0.6;
00030         m_chassis_color = TColor(0xff, 0x00, 0x00);
00031 
00032         // Default shape:
00033         m_chassis_poly.clear();
00034         m_chassis_poly.push_back(TPoint2D(-0.4, -0.5));
00035         m_chassis_poly.push_back(TPoint2D(-0.4, 0.5));
00036         m_chassis_poly.push_back(TPoint2D(0.4, 0.5));
00037         m_chassis_poly.push_back(TPoint2D(0.6, 0.3));
00038         m_chassis_poly.push_back(TPoint2D(0.6, -0.3));
00039         m_chassis_poly.push_back(TPoint2D(0.4, -0.5));
00040         updateMaxRadiusFromPoly();
00041 
00042         m_fixture_chassis = NULL;
00043         for (int i = 0; i < 2; i++) m_fixture_wheels[i] = NULL;
00044 }
00045 
00047 void DynamicsDifferential::dynamics_load_params_from_xml(
00048         const rapidxml::xml_node<char>* xml_node)
00049 {
00050         // <chassis ...> </chassis>
00051         const rapidxml::xml_node<char>* xml_chassis =
00052                 xml_node->first_node("chassis");
00053         if (xml_chassis)
00054         {
00055                 // Attribs:
00056                 std::map<std::string, TParamEntry> attribs;
00057                 attribs["mass"] = TParamEntry("%lf", &this->m_chassis_mass);
00058                 attribs["zmin"] = TParamEntry("%lf", &this->m_chassis_z_min);
00059                 attribs["zmax"] = TParamEntry("%lf", &this->m_chassis_z_max);
00060                 attribs["color"] = TParamEntry("%color", &this->m_chassis_color);
00061 
00062                 parse_xmlnode_attribs(
00063                         *xml_chassis, attribs,
00064                         "[DynamicsDifferential::dynamics_load_params_from_xml]");
00065 
00066                 // Shape node (optional, fallback to default shape if none found)
00067                 const rapidxml::xml_node<char>* xml_shape =
00068                         xml_chassis->first_node("shape");
00069                 if (xml_shape)
00070                         mvsim::parse_xmlnode_shape(
00071                                 *xml_shape, m_chassis_poly,
00072                                 "[DynamicsDifferential::dynamics_load_params_from_xml]");
00073         }
00074 
00075         // <l_wheel ...>, <r_wheel ...>
00076         const char* w_names[2] = {"l_wheel", "r_wheel"};
00077         const double w_default_y[2] = {0.5, -0.5};
00078         // Load common params:
00079         for (size_t i = 0; i < 2; i++)
00080         {
00081                 const rapidxml::xml_node<char>* xml_wheel =
00082                         xml_node->first_node(w_names[i]);
00083                 if (xml_wheel)
00084                         m_wheels_info[i].loadFromXML(xml_wheel);
00085                 else
00086                 {
00087                         m_wheels_info[i] = Wheel();
00088                         m_wheels_info[i].y = w_default_y[i];
00089                 }
00090         }
00091 
00092         // Vehicle controller:
00093         // -------------------------------------------------
00094         {
00095                 const rapidxml::xml_node<char>* xml_control =
00096                         xml_node->first_node("controller");
00097                 if (xml_control)
00098                 {
00099                         rapidxml::xml_attribute<char>* control_class =
00100                                 xml_control->first_attribute("class");
00101                         if (!control_class || !control_class->value())
00102                                 throw runtime_error(
00103                                         "[DynamicsDifferential] Missing 'class' attribute in "
00104                                         "<controller> XML node");
00105 
00106                         const std::string sCtrlClass = std::string(control_class->value());
00107                         if (sCtrlClass == ControllerRawForces::class_name())
00108                                 m_controller =
00109                                         ControllerBasePtr(new ControllerRawForces(*this));
00110                         else if (sCtrlClass == ControllerTwistPID::class_name())
00111                                 m_controller = ControllerBasePtr(new ControllerTwistPID(*this));
00112                         else
00113                                 throw runtime_error(
00114                                         mrpt::format(
00115                                                 "[DynamicsDifferential] Unknown 'class'='%s' in "
00116                                                 "<controller> XML node",
00117                                                 sCtrlClass.c_str()));
00118 
00119                         m_controller->load_config(*xml_control);
00120                 }
00121         }
00122 
00123         // Default controller:
00124         if (!m_controller)
00125                 m_controller = ControllerBasePtr(new ControllerRawForces(*this));
00126 }
00127 
00128 // See docs in base class:
00129 void DynamicsDifferential::invoke_motor_controllers(
00130         const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
00131 {
00132         // Longitudinal forces at each wheel:
00133         out_torque_per_wheel.assign(2, 0.0);
00134 
00135         if (m_controller)
00136         {
00137                 // Invoke controller:
00138                 TControllerInput ci;
00139                 ci.context = context;
00140                 TControllerOutput co;
00141                 m_controller->control_step(ci, co);
00142                 // Take its output:
00143                 out_torque_per_wheel[WHEEL_L] = co.wheel_torque_l;
00144                 out_torque_per_wheel[WHEEL_R] = co.wheel_torque_r;
00145         }
00146 }
00147 
00148 // See docs in base class:
00149 vec3 DynamicsDifferential::getVelocityLocalOdoEstimate() const
00150 {
00151         vec3 odo_vel;
00152         // Equations:
00153 
00154         // Velocities in local +X at each wheel i={0,1}:
00155         // v_i = vx - w_veh * wheel_{i,y}  =  w_i * R_i
00156         // Re-arranging:
00157         const double w0 = m_wheels_info[WHEEL_L].getW();
00158         const double w1 = m_wheels_info[WHEEL_R].getW();
00159         const double R0 = m_wheels_info[WHEEL_L].diameter * 0.5;
00160         const double R1 = m_wheels_info[WHEEL_R].diameter * 0.5;
00161 
00162         const double Ay = m_wheels_info[WHEEL_L].y - m_wheels_info[WHEEL_R].y;
00163         ASSERTMSG_(
00164                 Ay != 0.0,
00165                 "The two wheels of a differential vehicle CAN'T by at the same Y "
00166             "coordinate!");
00167 
00168         const double w_veh = (w1 * R1 - w0 * R0) / Ay;
00169         const double vx_veh = w0 * R0 + w_veh * m_wheels_info[WHEEL_L].y;
00170 
00171         odo_vel.vals[0] = vx_veh;
00172         odo_vel.vals[2] = w_veh;
00173 
00174         // v_y = 0
00175         odo_vel.vals[1] = 0.0;
00176 
00177 #if 0  // Debug
00178         {
00179                 vec3 gt_vel = this->getVelocityLocal();
00180                 printf("\n gt: vx=%7.03f, vy=%7.03f, w= %7.03fdeg\n", gt_vel.vals[0], gt_vel.vals[1], mrpt::utils::RAD2DEG(gt_vel.vals[2]));
00181                 printf("odo: vx=%7.03f, vy=%7.03f, w= %7.03fdeg\n", odo_vel.vals[0], odo_vel.vals[1], mrpt::utils::RAD2DEG(odo_vel.vals[2]));
00182         }
00183 #endif
00184         return odo_vel;
00185 }


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