VehicleAckermann_Drivetrain.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014 Jose Luis Blanco Claraco (University of Almeria) |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under GNU General Public License version 3 |
7  | See <http://www.gnu.org/licenses/> |
8  +-------------------------------------------------------------------------+ */
9 
10 #include <mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h>
11 #include <mvsim/World.h>
12 
13 #include "xml_utils.h"
14 
15 #include <mrpt/opengl/COpenGLScene.h>
16 #include <rapidxml.hpp>
17 #include <cmath>
18 
19 using namespace mvsim;
20 using namespace std;
21 
22 // Ctor:
23 DynamicsAckermannDrivetrain::DynamicsAckermannDrivetrain(World* parent)
24  : VehicleBase(parent, 4 /*num wheels*/),
25  m_max_steer_ang(DEG2RAD(30))
26 {
27  using namespace mrpt::math;
28 
29  m_chassis_mass = 500.0;
30  m_chassis_z_min = 0.20;
31  m_chassis_z_max = 1.40;
32  m_chassis_color = TColor(0xe8, 0x30, 0x00);
33 
34  // Default shape:
35  m_chassis_poly.clear();
36  m_chassis_poly.push_back(TPoint2D(-0.8, -1.0));
37  m_chassis_poly.push_back(TPoint2D(-0.8, 1.0));
38  m_chassis_poly.push_back(TPoint2D(1.5, 0.9));
39  m_chassis_poly.push_back(TPoint2D(1.8, 0.8));
40  m_chassis_poly.push_back(TPoint2D(1.8, -0.8));
41  m_chassis_poly.push_back(TPoint2D(1.5, -0.9));
42  updateMaxRadiusFromPoly();
43 
44  m_fixture_chassis = NULL;
45  for (int i = 0; i < 4; i++) m_fixture_wheels[i] = NULL;
46 
47  // Default values:
48  // rear-left:
49  m_wheels_info[WHEEL_RL].x = 0;
50  m_wheels_info[WHEEL_RL].y = 0.9;
51  // rear-right:
52  m_wheels_info[WHEEL_RR].x = 0;
53  m_wheels_info[WHEEL_RR].y = -0.9;
54  // Front-left:
55  m_wheels_info[WHEEL_FL].x = 1.3;
56  m_wheels_info[WHEEL_FL].y = 0.9;
57  // Front-right:
58  m_wheels_info[WHEEL_FR].x = 1.3;
59  m_wheels_info[WHEEL_FR].y = -0.9;
60 
61  m_FrontRearSplit = 0.5;
62  m_FrontLRSplit = 0.5;
63  m_RearLRSplit = 0.5;
64 
65  m_FrontRearBias = 1.5;
66  m_FrontLRBias = 1.5;
67  m_RearLRBias = 1.5;
68 
69  m_diff_type = DIFF_TORSEN_4WD;
70 }
71 
73 void DynamicsAckermannDrivetrain::dynamics_load_params_from_xml(
74  const rapidxml::xml_node<char>* xml_node)
75 {
76  // <chassis ...> </chassis>
77  const rapidxml::xml_node<char>* xml_chassis =
78  xml_node->first_node("chassis");
79  if (xml_chassis)
80  {
81  // Attribs:
82  std::map<std::string, TParamEntry> attribs;
83  attribs["mass"] = TParamEntry("%lf", &this->m_chassis_mass);
84  attribs["zmin"] = TParamEntry("%lf", &this->m_chassis_z_min);
85  attribs["zmax"] = TParamEntry("%lf", &this->m_chassis_z_max);
86  attribs["color"] = TParamEntry("%color", &this->m_chassis_color);
87 
88  parse_xmlnode_attribs(
89  *xml_chassis, attribs,
90  "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
91 
92  // Shape node (optional, fallback to default shape if none found)
93  const rapidxml::xml_node<char>* xml_shape =
94  xml_chassis->first_node("shape");
95  if (xml_shape)
96  mvsim::parse_xmlnode_shape(
97  *xml_shape, m_chassis_poly,
98  "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
99  }
100 
101  //<rl_wheel pos="0 1" mass="6.0" width="0.30" diameter="0.62" />
102  //<rr_wheel pos="0 -1" mass="6.0" width="0.30" diameter="0.62" />
103  //<fl_wheel mass="6.0" width="0.30" diameter="0.62" />
104  //<fr_wheel mass="6.0" width="0.30" diameter="0.62" />
105  const char* w_names[4] = {
106  "rl_wheel", // 0
107  "rr_wheel", // 1
108  "fl_wheel", // 2
109  "fr_wheel" // 3
110  };
111  // Load common params:
112  for (size_t i = 0; i < 4; i++)
113  {
114  const rapidxml::xml_node<char>* xml_wheel =
115  xml_node->first_node(w_names[i]);
116  if (xml_wheel) m_wheels_info[i].loadFromXML(xml_wheel);
117  }
118 
119  //<f_wheels_x>1.3</f_wheels_x>
120  //<f_wheels_d>2.0</f_wheels_d>
121  // Load front ackermann wheels and other params:
122  {
123  double front_x = 1.3;
124  double front_d = 2.0;
125  std::map<std::string, TParamEntry> ack_ps;
126  // Front wheels:
127  ack_ps["f_wheels_x"] = TParamEntry("%lf", &front_x);
128  ack_ps["f_wheels_d"] = TParamEntry("%lf", &front_d);
129  // other params:
130  ack_ps["max_steer_ang_deg"] = TParamEntry("%lf_deg", &m_max_steer_ang);
131 
132  parse_xmlnode_children_as_param(
133  *xml_node, ack_ps,
134  "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
135 
136  // Front-left:
137  m_wheels_info[WHEEL_FL].x = front_x;
138  m_wheels_info[WHEEL_FL].y = 0.5 * front_d;
139  // Front-right:
140  m_wheels_info[WHEEL_FR].x = front_x;
141  m_wheels_info[WHEEL_FR].y = -0.5 * front_d;
142  }
143 
144  // Drivetrain parameters
145  // Watch the order of DifferentialType enum!
146  const char* drive_names[6] = {"open_front", "open_rear", "open_4wd",
147  "torsen_front", "torsen_rear", "torsen_4wd"};
148 
149  const rapidxml::xml_node<char>* xml_drive =
150  xml_node->first_node("drivetrain");
151  if (xml_drive)
152  {
153  std::map<std::string, TParamEntry> attribs;
154  std::string diff_type;
155  attribs["type"] = TParamEntry("%s", &diff_type);
156 
157  parse_xmlnode_attribs(
158  *xml_drive, attribs,
159  "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
160 
161  for (size_t i = 0; i < DifferentialType::DIFF_MAX; ++i)
162  {
163  if (diff_type == drive_names[i])
164  {
165  m_diff_type = (DifferentialType)i;
166  break;
167  }
168  }
169 
170  std::map<std::string, TParamEntry> drive_params;
171  drive_params["front_rear_split"] =
172  TParamEntry("%lf", &m_FrontRearSplit);
173  drive_params["front_rear_bias"] = TParamEntry("%lf", &m_FrontRearBias);
174  drive_params["front_left_right_split"] =
175  TParamEntry("%lf", &m_FrontLRSplit);
176  drive_params["front_left_right_bias"] =
177  TParamEntry("%lf", &m_FrontLRBias);
178  drive_params["rear_left_right_split"] =
179  TParamEntry("%lf", &m_RearLRSplit);
180  drive_params["rear_left_right_bias"] =
181  TParamEntry("%lf", &m_RearLRBias);
182 
183  parse_xmlnode_children_as_param(
184  *xml_drive, drive_params,
185  "[DynamicsAckermannDrivetrain::dynamics_load_params_from_xml]");
186  }
187 
188  // Vehicle controller:
189  // -------------------------------------------------
190  {
191  const rapidxml::xml_node<char>* xml_control =
192  xml_node->first_node("controller");
193  if (xml_control)
194  {
195  rapidxml::xml_attribute<char>* control_class =
196  xml_control->first_attribute("class");
197  if (!control_class || !control_class->value())
198  throw runtime_error(
199  "[DynamicsAckermannDrivetrain] Missing 'class' attribute "
200  "in <controller> XML node");
201 
202  const std::string sCtrlClass = std::string(control_class->value());
203  if (sCtrlClass == ControllerRawForces::class_name())
204  m_controller =
205  ControllerBasePtr(new ControllerRawForces(*this));
206  else if (sCtrlClass == ControllerTwistFrontSteerPID::class_name())
207  m_controller =
208  ControllerBasePtr(new ControllerTwistFrontSteerPID(*this));
209  else if (sCtrlClass == ControllerFrontSteerPID::class_name())
210  m_controller =
211  ControllerBasePtr(new ControllerFrontSteerPID(*this));
212  else
213  throw runtime_error(
214  mrpt::format(
215  "[DynamicsAckermannDrivetrain] Unknown 'class'='%s' in "
216  "<controller> XML node",
217  sCtrlClass.c_str()));
218 
219  m_controller->load_config(*xml_control);
220  }
221  }
222  // Default controller:
223  if (!m_controller)
224  m_controller = ControllerBasePtr(new ControllerRawForces(*this));
225 }
226 
227 // See docs in base class:
228 void DynamicsAckermannDrivetrain::invoke_motor_controllers(
229  const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
230 {
231  // Longitudinal forces at each wheel:
232  out_torque_per_wheel.assign(4, 0.0);
233  double torque_split_per_wheel[4] = {0.0};
234 
235  if (m_controller)
236  {
237  // Invoke controller:
238  TControllerInput ci;
239  ci.context = context;
240  TControllerOutput co;
241  m_controller->control_step(ci, co);
242  // Take its output:
243 
244  switch (m_diff_type)
245  {
246  case DIFF_OPEN_FRONT:
247  {
248  torque_split_per_wheel[WHEEL_FL] = m_FrontLRSplit;
249  torque_split_per_wheel[WHEEL_FR] = 1. - m_FrontLRSplit;
250  torque_split_per_wheel[WHEEL_RL] = 0.0;
251  torque_split_per_wheel[WHEEL_RR] = 0.0;
252  }
253  break;
254  case DIFF_OPEN_REAR:
255  {
256  torque_split_per_wheel[WHEEL_FL] = 0.0;
257  torque_split_per_wheel[WHEEL_FR] = 0.0;
258  torque_split_per_wheel[WHEEL_RL] = m_RearLRSplit;
259  torque_split_per_wheel[WHEEL_RR] = 1. - m_RearLRSplit;
260  }
261  break;
262  case DIFF_OPEN_4WD:
263  {
264  const double front = m_FrontRearSplit;
265  const double rear = 1. - m_FrontRearSplit;
266 
267  torque_split_per_wheel[WHEEL_FL] = front * m_FrontLRSplit;
268  torque_split_per_wheel[WHEEL_FR] =
269  front * (1. - m_FrontLRSplit);
270  torque_split_per_wheel[WHEEL_RL] = rear * m_RearLRSplit;
271  torque_split_per_wheel[WHEEL_RR] = rear * (1. - m_RearLRSplit);
272  }
273  break;
274  case DIFF_TORSEN_FRONT:
275  {
276  // no torque to rear
277  torque_split_per_wheel[WHEEL_RL] = 0.0;
278  torque_split_per_wheel[WHEEL_RR] = 0.0;
279 
280  computeDiffTorqueSplit(
281  m_wheels_info[WHEEL_FL].getW(),
282  m_wheels_info[WHEEL_FR].getW(), m_FrontLRBias,
283  m_FrontLRSplit, torque_split_per_wheel[WHEEL_FL],
284  torque_split_per_wheel[WHEEL_FR]);
285  }
286  break;
287  case DIFF_TORSEN_REAR:
288  {
289  // no torque to front
290  torque_split_per_wheel[WHEEL_FL] = 0.0;
291  torque_split_per_wheel[WHEEL_FR] = 0.0;
292 
293  computeDiffTorqueSplit(
294  m_wheels_info[WHEEL_RL].getW(),
295  m_wheels_info[WHEEL_RR].getW(), m_RearLRBias, m_RearLRSplit,
296  torque_split_per_wheel[WHEEL_RL],
297  torque_split_per_wheel[WHEEL_RR]);
298  }
299  break;
300  case DIFF_TORSEN_4WD:
301  {
302  // here we have magic
303  const double w_FL = m_wheels_info[WHEEL_FL].getW();
304  const double w_FR = m_wheels_info[WHEEL_FR].getW();
305  const double w_RL = m_wheels_info[WHEEL_RL].getW();
306  const double w_RR = m_wheels_info[WHEEL_RR].getW();
307  const double w_F = w_FL + w_FR;
308  const double w_R = w_RL + w_RR;
309 
310  double t_F = 0.0;
311  double t_R = 0.0;
312  // front-rear
313  computeDiffTorqueSplit(
314  w_F, w_R, m_FrontRearBias, m_FrontRearSplit, t_F, t_R);
315 
316  double t_FL = 0.0;
317  double t_FR = 0.0;
318  // front left-right
319  computeDiffTorqueSplit(
320  w_FL, w_FR, m_FrontLRBias, m_FrontLRSplit, t_FL, t_FR);
321 
322  double t_RL = 0.0;
323  double t_RR = 0.0;
324  // rear left-right
325  computeDiffTorqueSplit(
326  w_RL, w_RR, m_RearLRBias, m_RearLRSplit, t_RL, t_RR);
327 
328  torque_split_per_wheel[WHEEL_FL] = t_F * t_FL;
329  torque_split_per_wheel[WHEEL_FR] = t_F * t_FR;
330  torque_split_per_wheel[WHEEL_RL] = t_R * t_RL;
331  torque_split_per_wheel[WHEEL_RR] = t_R * t_RR;
332  }
333  break;
334  default:
335  {
336  // fatal - unknown diff!
337  ASSERTMSG_(
338  0,
339  "DynamicsAckermannDrivetrain::invoke_motor_controllers: \
340  Unknown differential type!");
341  }
342  break;
343  }
344 
345  ASSERT_(out_torque_per_wheel.size() == 4);
346  for (int i = 0; i < 4; i++)
347  {
348  out_torque_per_wheel[i] =
349  co.drive_torque * torque_split_per_wheel[i];
350  }
351 
352  // Kinematically-driven steering wheels:
353  // Ackermann formulas for inner&outer weels turning angles wrt the
354  // equivalent (central) one:
355  computeFrontWheelAngles(
356  co.steer_ang, m_wheels_info[WHEEL_FL].yaw,
357  m_wheels_info[WHEEL_FR].yaw);
358  }
359 }
360 
361 void DynamicsAckermannDrivetrain::computeFrontWheelAngles(
362  const double desired_equiv_steer_ang, double& out_fl_ang,
363  double& out_fr_ang) const
364 {
365  // EQ1: cot(d)+0.5*w/l = cot(do)
366  // EQ2: cot(di)=cot(do)-w/l
367  const double w = m_wheels_info[WHEEL_FL].y - m_wheels_info[WHEEL_FR].y;
368  const double l = m_wheels_info[WHEEL_FL].x - m_wheels_info[WHEEL_RL].x;
369  ASSERT_(l > 0);
370  const double w_l = w / l;
371  const double delta =
372  b2Clamp(std::abs(desired_equiv_steer_ang), 0.0, m_max_steer_ang);
373 
374  const bool delta_neg = (desired_equiv_steer_ang < 0);
375  ASSERT_BELOW_(delta, 0.5 * M_PI - 0.01);
376  const double cot_do = 1.0 / tan(delta) + 0.5 * w_l;
377  const double cot_di = cot_do - w_l;
378  // delta>0: do->right, di->left wheel
379  // delta<0: do->left , di->right wheel
380  (delta_neg ? out_fr_ang : out_fl_ang) =
381  atan(1.0 / cot_di) * (delta_neg ? -1.0 : 1.0);
382  (delta_neg ? out_fl_ang : out_fr_ang) =
383  atan(1.0 / cot_do) * (delta_neg ? -1.0 : 1.0);
384 }
385 
386 void DynamicsAckermannDrivetrain::computeDiffTorqueSplit(
387  const double w1, const double w2, const double diffBias,
388  const double splitRatio, double& t1, double& t2)
389 {
390  if (signWithZero(w1) == 0.0 ||
391  signWithZero(w2) == 0.0)
392  {
393  t1 = splitRatio;
394  t2 = 1.0 - splitRatio;
395  return;
396  }
397 
398  const double w1Abs = std::abs(w1);
399  const double w2Abs = std::abs(w2);
400  const double omegaMax = std::max(w1Abs, w2Abs);
401  const double omegaMin = std::min(w1Abs, w2Abs);
402 
403  const double delta = omegaMax - diffBias * omegaMin;
404 
405  const double deltaTorque = (delta > 0) ? delta / omegaMax : 0.0f;
406  const double f1 = (w1Abs - w2Abs > 0) ? splitRatio * (1.0f - deltaTorque)
407  : splitRatio * (1.0f + deltaTorque);
408  const double f2 = (w1Abs - w2Abs > 0)
409  ? (1.0f - splitRatio) * (1.0f + deltaTorque)
410  : (1.0f - splitRatio) * (1.0f - deltaTorque);
411  const double denom = 1.0f / (f1 + f2);
412 
413  t1 = f1 * denom;
414  t2 = f2 * denom;
415 }
416 
417 // See docs in base class:
418 vec3 DynamicsAckermannDrivetrain::getVelocityLocalOdoEstimate() const
419 {
420  vec3 odo_vel;
421  // Equations:
422 
423  // Velocities in local +X at each wheel i={0,1}:
424  // v_i = vx - w_veh * wheel_{i,y} = w_i * R_i
425  // Re-arranging:
426  const double w0 = m_wheels_info[WHEEL_RL].getW();
427  const double w1 = m_wheels_info[WHEEL_RR].getW();
428  const double R0 = m_wheels_info[WHEEL_RL].diameter * 0.5;
429  const double R1 = m_wheels_info[WHEEL_RR].diameter * 0.5;
430 
431  const double Ay = m_wheels_info[WHEEL_RL].y - m_wheels_info[WHEEL_RR].y;
432  ASSERTMSG_(
433  Ay != 0.0,
434  "The two wheels of a differential vehicle CAN'T by at the same Y "
435  "coordinate!");
436 
437  const double w_veh = (w1 * R1 - w0 * R0) / Ay;
438  const double vx_veh = w0 * R0 + w_veh * m_wheels_info[WHEEL_RL].y;
439 
440  odo_vel.vals[0] = vx_veh;
441  odo_vel.vals[2] = w_veh;
442 
443  // v_y = 0
444  odo_vel.vals[1] = 0.0;
445 
446 #if 0 // Debug
447  {
448  vec3 gt_vel = this->getVelocityLocal();
449  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]));
450  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]));
451  }
452 #endif
453  return odo_vel;
454 }
rapidxml.hpp
This file contains rapidxml parser and DOM implementation.
mvsim::parse_xmlnode_shape
void parse_xmlnode_shape(const rapidxml::xml_node< char > &xml_node, mrpt::math::TPolygon2D &out_poly, const char *function_name_context="")
Definition: xml_utils.cpp:239
mvsim::DynamicsAckermannDrivetrain::m_diff_type
DifferentialType m_diff_type
min turning radius
Definition: VehicleAckermann_Drivetrain.h:208
rapidxml::xml_base::value
Ch * value() const
Definition: rapidxml.hpp:692
f
f
mvsim::vec3::vals
double vals[3]
Definition: basic_types.h:64
rapidxml::xml_node::first_node
xml_node< Ch > * first_node(const Ch *name=0, std::size_t name_size=0, bool case_sensitive=true) const
Definition: rapidxml.hpp:936
VehicleAckermann_Drivetrain.h
mvsim::parse_xmlnode_attribs
void parse_xmlnode_attribs(const rapidxml::xml_node< char > &xml_node, const std::map< std::string, TParamEntry > &params, const char *function_name_context="")
Definition: xml_utils.cpp:160
mvsim::parse_xmlnode_children_as_param
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const std::map< std::string, TParamEntry > &params, const char *function_name_context="")
Definition: xml_utils.cpp:196
mvsim::DynamicsAckermannDrivetrain::computeDiffTorqueSplit
void computeDiffTorqueSplit(const double w1, const double w2, const double diffBias, const double defaultSplitRatio, double &t1, double &t2)
Definition: VehicleAckermann_Drivetrain.cpp:386
rapidxml::xml_node::first_attribute
xml_attribute< Ch > * first_attribute(const Ch *name=0, std::size_t name_size=0, bool case_sensitive=true) const
Definition: rapidxml.hpp:1025
xml_utils.h
rapidxml::xml_attribute
Definition: rapidxml.hpp:138
mvsim::DynamicsAckermannDrivetrain::invoke_motor_controllers
virtual void invoke_motor_controllers(const TSimulContext &context, std::vector< double > &out_force_per_wheel)
Definition: VehicleAckermann_Drivetrain.cpp:228
mvsim::DynamicsAckermannDrivetrain::ControllerBasePtr
std::shared_ptr< ControllerBase > ControllerBasePtr
Definition: VehicleAckermann_Drivetrain.h:89
mvsim::DynamicsAckermannDrivetrain::TControllerOutput::steer_ang
double steer_ang
Equivalent ackerman steering angle.
Definition: VehicleAckermann_Drivetrain.h:82
w
TFSIMD_FORCE_INLINE const tfScalar & w() const
b2Clamp
T b2Clamp(T a, T low, T high)
Definition: b2Math.h:654
mvsim::DynamicsAckermannDrivetrain::m_controller
ControllerBasePtr m_controller
The installed controller.
Definition: VehicleAckermann_Drivetrain.h:203
mvsim::DynamicsAckermannDrivetrain::dynamics_load_params_from_xml
virtual void dynamics_load_params_from_xml(const rapidxml::xml_node< char > *xml_node)
Definition: VehicleAckermann_Drivetrain.cpp:73
rapidxml::xml_node
Definition: rapidxml.hpp:137
mvsim::VehicleBase::getVelocityLocal
vec3 getVelocityLocal() const
Definition: VehicleBase.cpp:525
mvsim::DynamicsAckermannDrivetrain::computeFrontWheelAngles
void computeFrontWheelAngles(const double desired_equiv_steer_ang, double &out_fl_ang, double &out_fr_ang) const
Definition: VehicleAckermann_Drivetrain.cpp:361

mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40