VehicleAckermann_Drivetrain.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_Drivetrain.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 DynamicsAckermannDrivetrain::DynamicsAckermannDrivetrain(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         m_FrontRearSplit = 0.5;
00062         m_FrontLRSplit = 0.5;
00063         m_RearLRSplit = 0.5;
00064 
00065         m_FrontRearBias = 1.5;
00066         m_FrontLRBias = 1.5;
00067         m_RearLRBias = 1.5;
00068 
00069         m_diff_type = DIFF_TORSEN_4WD;
00070 }
00071 
00073 void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml(
00074         const rapidxml::xml_node<char>* xml_node)
00075 {
00076         // <chassis ...> </chassis>
00077         const rapidxml::xml_node<char>* xml_chassis =
00078                 xml_node->first_node("chassis");
00079         if (xml_chassis)
00080         {
00081                 // Attribs:
00082                 std::map<std::string, TParamEntry> attribs;
00083                 attribs["mass"] = TParamEntry("%lf", &this->m_chassis_mass);
00084                 attribs["zmin"] = TParamEntry("%lf", &this->m_chassis_z_min);
00085                 attribs["zmax"] = TParamEntry("%lf", &this->m_chassis_z_max);
00086                 attribs["color"] = TParamEntry("%color", &this->m_chassis_color);
00087 
00088                 parse_xmlnode_attribs(
00089                         *xml_chassis, attribs,
00090                         "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
00091 
00092                 // Shape node (optional, fallback to default shape if none found)
00093                 const rapidxml::xml_node<char>* xml_shape =
00094                         xml_chassis->first_node("shape");
00095                 if (xml_shape)
00096                         mvsim::parse_xmlnode_shape(
00097                                 *xml_shape, m_chassis_poly,
00098                                 "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
00099         }
00100 
00101         //<rl_wheel pos="0  1" mass="6.0" width="0.30" diameter="0.62" />
00102         //<rr_wheel pos="0 -1" mass="6.0" width="0.30" diameter="0.62" />
00103         //<fl_wheel mass="6.0" width="0.30" diameter="0.62" />
00104         //<fr_wheel mass="6.0" width="0.30" diameter="0.62" />
00105         const char* w_names[4] = {
00106                 "rl_wheel",  // 0
00107                 "rr_wheel",  // 1
00108                 "fl_wheel",  // 2
00109                 "fr_wheel"  // 3
00110         };
00111         // Load common params:
00112         for (size_t i = 0; i < 4; i++)
00113         {
00114                 const rapidxml::xml_node<char>* xml_wheel =
00115                         xml_node->first_node(w_names[i]);
00116                 if (xml_wheel) m_wheels_info[i].loadFromXML(xml_wheel);
00117         }
00118 
00119         //<f_wheels_x>1.3</f_wheels_x>
00120         //<f_wheels_d>2.0</f_wheels_d>
00121         // Load front ackermann wheels and other params:
00122         {
00123                 double front_x = 1.3;
00124                 double front_d = 2.0;
00125                 std::map<std::string, TParamEntry> ack_ps;
00126                 // Front wheels:
00127                 ack_ps["f_wheels_x"] = TParamEntry("%lf", &front_x);
00128                 ack_ps["f_wheels_d"] = TParamEntry("%lf", &front_d);
00129                 // other params:
00130                 ack_ps["max_steer_ang_deg"] = TParamEntry("%lf_deg", &m_max_steer_ang);
00131 
00132                 parse_xmlnode_children_as_param(
00133                         *xml_node, ack_ps,
00134                         "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
00135 
00136                 // Front-left:
00137                 m_wheels_info[WHEEL_FL].x = front_x;
00138                 m_wheels_info[WHEEL_FL].y = 0.5 * front_d;
00139                 // Front-right:
00140                 m_wheels_info[WHEEL_FR].x = front_x;
00141                 m_wheels_info[WHEEL_FR].y = -0.5 * front_d;
00142         }
00143 
00144         // Drivetrain parameters
00145         // Watch the order of DifferentialType enum!
00146         const char* drive_names[6] = {"open_front",   "open_rear",   "open_4wd",
00147                                                                   "torsen_front", "torsen_rear", "torsen_4wd"};
00148 
00149         const rapidxml::xml_node<char>* xml_drive =
00150                 xml_node->first_node("drivetrain");
00151         if (xml_drive)
00152         {
00153                 std::map<std::string, TParamEntry> attribs;
00154                 std::string diff_type;
00155                 attribs["type"] = TParamEntry("%s", &diff_type);
00156 
00157                 parse_xmlnode_attribs(
00158                         *xml_drive, attribs,
00159                         "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
00160 
00161                 for (size_t i = 0; i < DifferentialType::DIFF_MAX; ++i)
00162                 {
00163                         if (diff_type == drive_names[i])
00164                         {
00165                                 m_diff_type = (DifferentialType)i;
00166                                 break;
00167                         }
00168                 }
00169 
00170                 std::map<std::string, TParamEntry> drive_params;
00171                 drive_params["front_rear_split"] =
00172                         TParamEntry("%lf", &m_FrontRearSplit);
00173                 drive_params["front_rear_bias"] = TParamEntry("%lf", &m_FrontRearBias);
00174                 drive_params["front_left_right_split"] =
00175                         TParamEntry("%lf", &m_FrontLRSplit);
00176                 drive_params["front_left_right_bias"] =
00177                         TParamEntry("%lf", &m_FrontLRBias);
00178                 drive_params["rear_left_right_split"] =
00179                         TParamEntry("%lf", &m_RearLRSplit);
00180                 drive_params["rear_left_right_bias"] =
00181                         TParamEntry("%lf", &m_RearLRBias);
00182 
00183                 parse_xmlnode_children_as_param(
00184                         *xml_drive, drive_params,
00185                         "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
00186         }
00187 
00188         // Vehicle controller:
00189         // -------------------------------------------------
00190         {
00191                 const rapidxml::xml_node<char>* xml_control =
00192                         xml_node->first_node("controller");
00193                 if (xml_control)
00194                 {
00195                         rapidxml::xml_attribute<char>* control_class =
00196                                 xml_control->first_attribute("class");
00197                         if (!control_class || !control_class->value())
00198                                 throw runtime_error(
00199                                         "[DynamicsAckermannDrivetrain] Missing 'class' attribute "
00200                                         "in <controller> XML node");
00201 
00202                         const std::string sCtrlClass = std::string(control_class->value());
00203                         if (sCtrlClass == ControllerRawForces::class_name())
00204                                 m_controller =
00205                                         ControllerBasePtr(new ControllerRawForces(*this));
00206                         else if (sCtrlClass == ControllerTwistFrontSteerPID::class_name())
00207                                 m_controller =
00208                                         ControllerBasePtr(new ControllerTwistFrontSteerPID(*this));
00209                         else if (sCtrlClass == ControllerFrontSteerPID::class_name())
00210                                 m_controller =
00211                                         ControllerBasePtr(new ControllerFrontSteerPID(*this));
00212                         else
00213                                 throw runtime_error(
00214                                         mrpt::format(
00215                                                 "[DynamicsAckermannDrivetrain] Unknown 'class'='%s' in "
00216                                                 "<controller> XML node",
00217                                                 sCtrlClass.c_str()));
00218 
00219                         m_controller->load_config(*xml_control);
00220                 }
00221         }
00222         // Default controller:
00223         if (!m_controller)
00224                 m_controller = ControllerBasePtr(new ControllerRawForces(*this));
00225 }
00226 
00227 // See docs in base class:
00228 void DynamicsAckermannDrivetrain::invoke_motor_controllers(
00229         const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
00230 {
00231         // Longitudinal forces at each wheel:
00232         out_torque_per_wheel.assign(4, 0.0);
00233         double torque_split_per_wheel[4] = {0.0};
00234 
00235         if (m_controller)
00236         {
00237                 // Invoke controller:
00238                 TControllerInput ci;
00239                 ci.context = context;
00240                 TControllerOutput co;
00241                 m_controller->control_step(ci, co);
00242                 // Take its output:
00243 
00244                 switch (m_diff_type)
00245                 {
00246                         case DIFF_OPEN_FRONT:
00247                         {
00248                                 torque_split_per_wheel[WHEEL_FL] = m_FrontLRSplit;
00249                                 torque_split_per_wheel[WHEEL_FR] = 1. - m_FrontLRSplit;
00250                                 torque_split_per_wheel[WHEEL_RL] = 0.0;
00251                                 torque_split_per_wheel[WHEEL_RR] = 0.0;
00252                         }
00253                         break;
00254                         case DIFF_OPEN_REAR:
00255                         {
00256                                 torque_split_per_wheel[WHEEL_FL] = 0.0;
00257                                 torque_split_per_wheel[WHEEL_FR] = 0.0;
00258                                 torque_split_per_wheel[WHEEL_RL] = m_RearLRSplit;
00259                                 torque_split_per_wheel[WHEEL_RR] = 1. - m_RearLRSplit;
00260                         }
00261                         break;
00262                         case DIFF_OPEN_4WD:
00263                         {
00264                                 const double front = m_FrontRearSplit;
00265                                 const double rear = 1. - m_FrontRearSplit;
00266 
00267                                 torque_split_per_wheel[WHEEL_FL] = front * m_FrontLRSplit;
00268                                 torque_split_per_wheel[WHEEL_FR] =
00269                                         front * (1. - m_FrontLRSplit);
00270                                 torque_split_per_wheel[WHEEL_RL] = rear * m_RearLRSplit;
00271                                 torque_split_per_wheel[WHEEL_RR] = rear * (1. - m_RearLRSplit);
00272                         }
00273                         break;
00274                         case DIFF_TORSEN_FRONT:
00275                         {
00276                                 // no torque to rear
00277                                 torque_split_per_wheel[WHEEL_RL] = 0.0;
00278                                 torque_split_per_wheel[WHEEL_RR] = 0.0;
00279 
00280                                 computeDiffTorqueSplit(
00281                                         m_wheels_info[WHEEL_FL].getW(),
00282                                         m_wheels_info[WHEEL_FR].getW(), m_FrontLRBias,
00283                                         m_FrontLRSplit, torque_split_per_wheel[WHEEL_FL],
00284                                         torque_split_per_wheel[WHEEL_FR]);
00285                         }
00286                         break;
00287                         case DIFF_TORSEN_REAR:
00288                         {
00289                                 // no torque to front
00290                                 torque_split_per_wheel[WHEEL_FL] = 0.0;
00291                                 torque_split_per_wheel[WHEEL_FR] = 0.0;
00292 
00293                                 computeDiffTorqueSplit(
00294                                         m_wheels_info[WHEEL_RL].getW(),
00295                                         m_wheels_info[WHEEL_RR].getW(), m_RearLRBias, m_RearLRSplit,
00296                                         torque_split_per_wheel[WHEEL_RL],
00297                                         torque_split_per_wheel[WHEEL_RR]);
00298                         }
00299                         break;
00300                         case DIFF_TORSEN_4WD:
00301                         {
00302                                 // here we have magic
00303                                 const double w_FL = m_wheels_info[WHEEL_FL].getW();
00304                                 const double w_FR = m_wheels_info[WHEEL_FR].getW();
00305                                 const double w_RL = m_wheels_info[WHEEL_RL].getW();
00306                                 const double w_RR = m_wheels_info[WHEEL_RR].getW();
00307                                 const double w_F = w_FL + w_FR;
00308                                 const double w_R = w_RL + w_RR;
00309 
00310                                 double t_F = 0.0;
00311                                 double t_R = 0.0;
00312                                 // front-rear
00313                                 computeDiffTorqueSplit(
00314                                         w_F, w_R, m_FrontRearBias, m_FrontRearSplit, t_F, t_R);
00315 
00316                                 double t_FL = 0.0;
00317                                 double t_FR = 0.0;
00318                                 // front left-right
00319                                 computeDiffTorqueSplit(
00320                                         w_FL, w_FR, m_FrontLRBias, m_FrontLRSplit, t_FL, t_FR);
00321 
00322                                 double t_RL = 0.0;
00323                                 double t_RR = 0.0;
00324                                 // rear left-right
00325                                 computeDiffTorqueSplit(
00326                                         w_RL, w_RR, m_RearLRBias, m_RearLRSplit, t_RL, t_RR);
00327 
00328                                 torque_split_per_wheel[WHEEL_FL] = t_F * t_FL;
00329                                 torque_split_per_wheel[WHEEL_FR] = t_F * t_FR;
00330                                 torque_split_per_wheel[WHEEL_RL] = t_R * t_RL;
00331                                 torque_split_per_wheel[WHEEL_RR] = t_R * t_RR;
00332                         }
00333                         break;
00334                         default:
00335                         {
00336                                 // fatal - unknown diff!
00337                                 ASSERTMSG_(
00338                                         0,
00339                                         "DynamicsAckermannDrivetrain::invoke_motor_controllers: \
00340                                        Unknown differential type!");
00341                         }
00342                         break;
00343                 }
00344 
00345                 ASSERT_(out_torque_per_wheel.size() == 4);
00346                 for (int i = 0; i < 4; i++)
00347                 {
00348                         out_torque_per_wheel[i] =
00349                                 co.drive_torque * torque_split_per_wheel[i];
00350                 }
00351 
00352                 // Kinematically-driven steering wheels:
00353                 // Ackermann formulas for inner&outer weels turning angles wrt the
00354                 // equivalent (central) one:
00355                 computeFrontWheelAngles(
00356                         co.steer_ang, m_wheels_info[WHEEL_FL].yaw,
00357                         m_wheels_info[WHEEL_FR].yaw);
00358         }
00359 }
00360 
00361 void DynamicsAckermannDrivetrain::computeFrontWheelAngles(
00362         const double desired_equiv_steer_ang, double& out_fl_ang,
00363         double& out_fr_ang) const
00364 {
00365         // EQ1: cot(d)+0.5*w/l = cot(do)
00366         // EQ2: cot(di)=cot(do)-w/l
00367         const double w = m_wheels_info[WHEEL_FL].y - m_wheels_info[WHEEL_FR].y;
00368         const double l = m_wheels_info[WHEEL_FL].x - m_wheels_info[WHEEL_RL].x;
00369         ASSERT_(l > 0);
00370         const double w_l = w / l;
00371         const double delta =
00372                 b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, m_max_steer_ang);
00373 
00374         const bool delta_neg = (desired_equiv_steer_ang < 0);
00375         ASSERT_BELOW_(delta, 0.5 * M_PI - 0.01);
00376         const double cot_do = 1.0 / tan(delta) + 0.5 * w_l;
00377         const double cot_di = cot_do - w_l;
00378         // delta>0: do->right, di->left wheel
00379         // delta<0: do->left , di->right wheel
00380         (delta_neg ? out_fr_ang : out_fl_ang) =
00381                 atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0);
00382         (delta_neg ? out_fl_ang : out_fr_ang) =
00383                 atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0);
00384 }
00385 
00386 void DynamicsAckermannDrivetrain::computeDiffTorqueSplit(
00387         const double w1, const double w2, const double diffBias,
00388         const double splitRatio, double& t1, double& t2)
00389 {
00390         if (signWithZero(w1) == 0.0 ||
00391             signWithZero(w2) == 0.0)
00392         {
00393                 t1 = splitRatio;
00394                 t2 = 1.0 - splitRatio;
00395                 return;
00396         }
00397 
00398         const double w1Abs = std::abs(w1);
00399         const double w2Abs = std::abs(w2);
00400         const double omegaMax = std::max(w1Abs, w2Abs);
00401         const double omegaMin = std::min(w1Abs, w2Abs);
00402 
00403         const double delta = omegaMax - diffBias * omegaMin;
00404 
00405         const double deltaTorque = (delta > 0) ? delta / omegaMax : 0.0f;
00406         const double f1 = (w1Abs - w2Abs > 0) ? splitRatio * (1.0f - deltaTorque)
00407                                                                                   : splitRatio * (1.0f + deltaTorque);
00408         const double f2 = (w1Abs - w2Abs > 0)
00409                                                   ? (1.0f - splitRatio) * (1.0f + deltaTorque)
00410                                                   : (1.0f - splitRatio) * (1.0f - deltaTorque);
00411         const double denom = 1.0f / (f1 + f2);
00412 
00413         t1 = f1 * denom;
00414         t2 = f2 * denom;
00415 }
00416 
00417 // See docs in base class:
00418 vec3 DynamicsAckermannDrivetrain::getVelocityLocalOdoEstimate() const
00419 {
00420         vec3 odo_vel;
00421         // Equations:
00422 
00423         // Velocities in local +X at each wheel i={0,1}:
00424         // v_i = vx - w_veh * wheel_{i,y}  =  w_i * R_i
00425         // Re-arranging:
00426         const double w0 = m_wheels_info[WHEEL_RL].getW();
00427         const double w1 = m_wheels_info[WHEEL_RR].getW();
00428         const double R0 = m_wheels_info[WHEEL_RL].diameter * 0.5;
00429         const double R1 = m_wheels_info[WHEEL_RR].diameter * 0.5;
00430 
00431         const double Ay = m_wheels_info[WHEEL_RL].y - m_wheels_info[WHEEL_RR].y;
00432         ASSERTMSG_(
00433                 Ay != 0.0,
00434                 "The two wheels of a differential vehicle CAN'T by at the same Y "
00435             "coordinate!");
00436 
00437         const double w_veh = (w1 * R1 - w0 * R0) / Ay;
00438         const double vx_veh = w0 * R0 + w_veh * m_wheels_info[WHEEL_RL].y;
00439 
00440         odo_vel.vals[0] = vx_veh;
00441         odo_vel.vals[2] = w_veh;
00442 
00443         // v_y = 0
00444         odo_vel.vals[1] = 0.0;
00445 
00446 #if 0  // Debug
00447         {
00448                 vec3 gt_vel = this->getVelocityLocal();
00449                 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]));
00450                 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]));
00451         }
00452 #endif
00453         return odo_vel;
00454 }


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