VehicleAckermann.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/VehicleAckermann.h>
00011 #include <mvsim/World.h>
00012 
00013 #include "xml_utils.h"
00014 
00015 #include <mrpt/opengl/COpenGLScene.h>
00016 #include <rapidxml.hpp>
00017 #include <cmath>
00018 
00019 using namespace mvsim;
00020 using namespace std;
00021 
00022 // Ctor:
00023 DynamicsAckermann::DynamicsAckermann(World* parent)
00024         : VehicleBase(parent, 4 /*num wheels*/),
00025       m_max_steer_ang(DEG2RAD(30))
00026 {
00027         using namespace mrpt::math;
00028 
00029         m_chassis_mass = 500.0;
00030         m_chassis_z_min = 0.20;
00031         m_chassis_z_max = 1.40;
00032         m_chassis_color = TColor(0xe8, 0x30, 0x00);
00033 
00034         // Default shape:
00035         m_chassis_poly.clear();
00036         m_chassis_poly.push_back(TPoint2D(-0.8, -1.0));
00037         m_chassis_poly.push_back(TPoint2D(-0.8, 1.0));
00038         m_chassis_poly.push_back(TPoint2D(1.5, 0.9));
00039         m_chassis_poly.push_back(TPoint2D(1.8, 0.8));
00040         m_chassis_poly.push_back(TPoint2D(1.8, -0.8));
00041         m_chassis_poly.push_back(TPoint2D(1.5, -0.9));
00042         updateMaxRadiusFromPoly();
00043 
00044         m_fixture_chassis = NULL;
00045         for (int i = 0; i < 4; i++) m_fixture_wheels[i] = NULL;
00046 
00047         // Default values:
00048         // rear-left:
00049         m_wheels_info[WHEEL_RL].x = 0;
00050         m_wheels_info[WHEEL_RL].y = 0.9;
00051         // rear-right:
00052         m_wheels_info[WHEEL_RR].x = 0;
00053         m_wheels_info[WHEEL_RR].y = -0.9;
00054         // Front-left:
00055         m_wheels_info[WHEEL_FL].x = 1.3;
00056         m_wheels_info[WHEEL_FL].y = 0.9;
00057         // Front-right:
00058         m_wheels_info[WHEEL_FR].x = 1.3;
00059         m_wheels_info[WHEEL_FR].y = -0.9;
00060 }
00061 
00063 void DynamicsAckermann::dynamics_load_params_from_xml(
00064         const rapidxml::xml_node<char>* xml_node)
00065 {
00066         // <chassis ...> </chassis>
00067         const rapidxml::xml_node<char>* xml_chassis =
00068                 xml_node->first_node("chassis");
00069         if (xml_chassis)
00070         {
00071                 // Attribs:
00072                 std::map<std::string, TParamEntry> attribs;
00073                 attribs["mass"] = TParamEntry("%lf", &this->m_chassis_mass);
00074                 attribs["zmin"] = TParamEntry("%lf", &this->m_chassis_z_min);
00075                 attribs["zmax"] = TParamEntry("%lf", &this->m_chassis_z_max);
00076                 attribs["color"] = TParamEntry("%color", &this->m_chassis_color);
00077 
00078                 parse_xmlnode_attribs(
00079                         *xml_chassis, attribs,
00080                         "[DynamicsAckermann::dynamics_load_params_from_xml]");
00081 
00082                 // Shape node (optional, fallback to default shape if none found)
00083                 const rapidxml::xml_node<char>* xml_shape =
00084                         xml_chassis->first_node("shape");
00085                 if (xml_shape)
00086                         mvsim::parse_xmlnode_shape(
00087                                 *xml_shape, m_chassis_poly,
00088                                 "[DynamicsAckermann::dynamics_load_params_from_xml]");
00089         }
00090 
00091         //<rl_wheel pos="0  1" mass="6.0" width="0.30" diameter="0.62" />
00092         //<rr_wheel pos="0 -1" mass="6.0" width="0.30" diameter="0.62" />
00093         //<fl_wheel mass="6.0" width="0.30" diameter="0.62" />
00094         //<fr_wheel mass="6.0" width="0.30" diameter="0.62" />
00095         const char* w_names[4] = {
00096                 "rl_wheel",  // 0
00097                 "rr_wheel",  // 1
00098                 "fl_wheel",  // 2
00099                 "fr_wheel"  // 3
00100         };
00101         // Load common params:
00102         for (size_t i = 0; i < 4; i++)
00103         {
00104                 const rapidxml::xml_node<char>* xml_wheel =
00105                         xml_node->first_node(w_names[i]);
00106                 if (xml_wheel) m_wheels_info[i].loadFromXML(xml_wheel);
00107         }
00108 
00109         //<f_wheels_x>1.3</f_wheels_x>
00110         //<f_wheels_d>2.0</f_wheels_d>
00111         // Load front ackermann wheels and other params:
00112         {
00113                 double front_x = 1.3;
00114                 double front_d = 2.0;
00115                 std::map<std::string, TParamEntry> ack_ps;
00116                 // Front wheels:
00117                 ack_ps["f_wheels_x"] = TParamEntry("%lf", &front_x);
00118                 ack_ps["f_wheels_d"] = TParamEntry("%lf", &front_d);
00119                 // other params:
00120                 ack_ps["max_steer_ang_deg"] = TParamEntry("%lf_deg", &m_max_steer_ang);
00121 
00122                 parse_xmlnode_children_as_param(
00123                         *xml_node, ack_ps,
00124                         "[DynamicsAckermann::dynamics_load_params_from_xml]");
00125 
00126                 // Front-left:
00127                 m_wheels_info[WHEEL_FL].x = front_x;
00128                 m_wheels_info[WHEEL_FL].y = 0.5 * front_d;
00129                 // Front-right:
00130                 m_wheels_info[WHEEL_FR].x = front_x;
00131                 m_wheels_info[WHEEL_FR].y = -0.5 * front_d;
00132         }
00133 
00134         // Vehicle controller:
00135         // -------------------------------------------------
00136         {
00137                 const rapidxml::xml_node<char>* xml_control =
00138                         xml_node->first_node("controller");
00139                 if (xml_control)
00140                 {
00141                         rapidxml::xml_attribute<char>* control_class =
00142                                 xml_control->first_attribute("class");
00143                         if (!control_class || !control_class->value())
00144                                 throw runtime_error(
00145                                         "[DynamicsAckermann] Missing 'class' attribute in "
00146                                         "<controller> XML node");
00147 
00148                         const std::string sCtrlClass = std::string(control_class->value());
00149                         if (sCtrlClass == ControllerRawForces::class_name())
00150                                 m_controller =
00151                                         ControllerBasePtr(new ControllerRawForces(*this));
00152                         else if (sCtrlClass == ControllerTwistFrontSteerPID::class_name())
00153                                 m_controller =
00154                                         ControllerBasePtr(new ControllerTwistFrontSteerPID(*this));
00155                         else if (sCtrlClass == ControllerFrontSteerPID::class_name())
00156                                 m_controller =
00157                                         ControllerBasePtr(new ControllerFrontSteerPID(*this));
00158                         else
00159                                 throw runtime_error(
00160                                         mrpt::format(
00161                                                 "[DynamicsAckermann] Unknown 'class'='%s' in "
00162                                                 "<controller> XML node",
00163                                                 sCtrlClass.c_str()));
00164 
00165                         m_controller->load_config(*xml_control);
00166                 }
00167         }
00168         // Default controller:
00169         if (!m_controller)
00170                 m_controller = ControllerBasePtr(new ControllerRawForces(*this));
00171 }
00172 
00173 // See docs in base class:
00174 void DynamicsAckermann::invoke_motor_controllers(
00175         const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
00176 {
00177         // Longitudinal forces at each wheel:
00178         out_torque_per_wheel.assign(4, 0.0);
00179 
00180         if (m_controller)
00181         {
00182                 // Invoke controller:
00183                 TControllerInput ci;
00184                 ci.context = context;
00185                 TControllerOutput co;
00186                 m_controller->control_step(ci, co);
00187                 // Take its output:
00188                 out_torque_per_wheel[WHEEL_RL] = co.rl_torque;
00189                 out_torque_per_wheel[WHEEL_RR] = co.rr_torque;
00190                 out_torque_per_wheel[WHEEL_FL] = co.fl_torque;
00191                 out_torque_per_wheel[WHEEL_FR] = co.fr_torque;
00192 
00193                 // Kinematically-driven steering wheels:
00194                 // Ackermann formulas for inner&outer weels turning angles wrt the
00195                 // equivalent (central) one:
00196                 computeFrontWheelAngles(
00197                         co.steer_ang, m_wheels_info[WHEEL_FL].yaw,
00198                         m_wheels_info[WHEEL_FR].yaw);
00199         }
00200 }
00201 
00202 void DynamicsAckermann::computeFrontWheelAngles(
00203         const double desired_equiv_steer_ang, double& out_fl_ang,
00204         double& out_fr_ang) const
00205 {
00206         // EQ1: cot(d)+0.5*w/l = cot(do)
00207         // EQ2: cot(di)=cot(do)-w/l
00208         const double w = m_wheels_info[WHEEL_FL].y - m_wheels_info[WHEEL_FR].y;
00209         const double l = m_wheels_info[WHEEL_FL].x - m_wheels_info[WHEEL_RL].x;
00210         ASSERT_(l > 0);
00211         const double w_l = w / l;
00212         const double delta =
00213                 b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, m_max_steer_ang);
00214 
00215         const bool delta_neg = (desired_equiv_steer_ang < 0);
00216         ASSERT_BELOW_(delta, 0.5 * M_PI - 0.01);
00217         const double cot_do = 1.0 / tan(delta) + 0.5 * w_l;
00218         const double cot_di = cot_do - w_l;
00219         // delta>0: do->right, di->left wheel
00220         // delta<0: do->left , di->right wheel
00221         (delta_neg ? out_fr_ang : out_fl_ang) =
00222                 atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0);
00223         (delta_neg ? out_fl_ang : out_fr_ang) =
00224                 atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0);
00225 }
00226 
00227 // See docs in base class:
00228 vec3 DynamicsAckermann::getVelocityLocalOdoEstimate() const
00229 {
00230         vec3 odo_vel;
00231         // Equations:
00232 
00233         // Velocities in local +X at each wheel i={0,1}:
00234         // v_i = vx - w_veh * wheel_{i,y}  =  w_i * R_i
00235         // Re-arranging:
00236         const double w0 = m_wheels_info[WHEEL_RL].getW();
00237         const double w1 = m_wheels_info[WHEEL_RR].getW();
00238         const double R0 = m_wheels_info[WHEEL_RL].diameter * 0.5;
00239         const double R1 = m_wheels_info[WHEEL_RR].diameter * 0.5;
00240 
00241         const double Ay = m_wheels_info[WHEEL_RL].y - m_wheels_info[WHEEL_RR].y;
00242         ASSERTMSG_(
00243                 Ay != 0.0,
00244                 "The two wheels of a differential vehicle CAN'T by at the same Y "
00245             "coordinate!");
00246 
00247         const double w_veh = (w1 * R1 - w0 * R0) / Ay;
00248         const double vx_veh = w0 * R0 + w_veh * m_wheels_info[WHEEL_RL].y;
00249 
00250         odo_vel.vals[0] = vx_veh;
00251         odo_vel.vals[2] = w_veh;
00252 
00253         // v_y = 0
00254         odo_vel.vals[1] = 0.0;
00255 
00256 #if 0  // Debug
00257         {
00258                 vec3 gt_vel = this->getVelocityLocal();
00259                 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]));
00260                 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]));
00261         }
00262 #endif
00263         return odo_vel;
00264 }


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