00001
00002
00003
00004
00005
00006
00007
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
00023 DynamicsAckermannDrivetrain::DynamicsAckermannDrivetrain(World* parent)
00024 : VehicleBase(parent, 4 ),
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
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
00048
00049 m_wheels_info[WHEEL_RL].x = 0;
00050 m_wheels_info[WHEEL_RL].y = 0.9;
00051
00052 m_wheels_info[WHEEL_RR].x = 0;
00053 m_wheels_info[WHEEL_RR].y = -0.9;
00054
00055 m_wheels_info[WHEEL_FL].x = 1.3;
00056 m_wheels_info[WHEEL_FL].y = 0.9;
00057
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
00077 const rapidxml::xml_node<char>* xml_chassis =
00078 xml_node->first_node("chassis");
00079 if (xml_chassis)
00080 {
00081
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
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
00102
00103
00104
00105 const char* w_names[4] = {
00106 "rl_wheel",
00107 "rr_wheel",
00108 "fl_wheel",
00109 "fr_wheel"
00110 };
00111
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
00120
00121
00122 {
00123 double front_x = 1.3;
00124 double front_d = 2.0;
00125 std::map<std::string, TParamEntry> ack_ps;
00126
00127 ack_ps["f_wheels_x"] = TParamEntry("%lf", &front_x);
00128 ack_ps["f_wheels_d"] = TParamEntry("%lf", &front_d);
00129
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
00137 m_wheels_info[WHEEL_FL].x = front_x;
00138 m_wheels_info[WHEEL_FL].y = 0.5 * front_d;
00139
00140 m_wheels_info[WHEEL_FR].x = front_x;
00141 m_wheels_info[WHEEL_FR].y = -0.5 * front_d;
00142 }
00143
00144
00145
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
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
00223 if (!m_controller)
00224 m_controller = ControllerBasePtr(new ControllerRawForces(*this));
00225 }
00226
00227
00228 void DynamicsAckermannDrivetrain::invoke_motor_controllers(
00229 const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
00230 {
00231
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
00238 TControllerInput ci;
00239 ci.context = context;
00240 TControllerOutput co;
00241 m_controller->control_step(ci, co);
00242
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
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
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
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
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
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
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
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
00353
00354
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
00366
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
00379
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
00418 vec3 DynamicsAckermannDrivetrain::getVelocityLocalOdoEstimate() const
00419 {
00420 vec3 odo_vel;
00421
00422
00423
00424
00425
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
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 }