00001
00002
00003
00004
00005
00006
00007
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
00023 DynamicsAckermann::DynamicsAckermann(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
00063 void DynamicsAckermann::dynamics_load_params_from_xml(
00064 const rapidxml::xml_node<char>* xml_node)
00065 {
00066
00067 const rapidxml::xml_node<char>* xml_chassis =
00068 xml_node->first_node("chassis");
00069 if (xml_chassis)
00070 {
00071
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
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
00092
00093
00094
00095 const char* w_names[4] = {
00096 "rl_wheel",
00097 "rr_wheel",
00098 "fl_wheel",
00099 "fr_wheel"
00100 };
00101
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
00110
00111
00112 {
00113 double front_x = 1.3;
00114 double front_d = 2.0;
00115 std::map<std::string, TParamEntry> ack_ps;
00116
00117 ack_ps["f_wheels_x"] = TParamEntry("%lf", &front_x);
00118 ack_ps["f_wheels_d"] = TParamEntry("%lf", &front_d);
00119
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
00127 m_wheels_info[WHEEL_FL].x = front_x;
00128 m_wheels_info[WHEEL_FL].y = 0.5 * front_d;
00129
00130 m_wheels_info[WHEEL_FR].x = front_x;
00131 m_wheels_info[WHEEL_FR].y = -0.5 * front_d;
00132 }
00133
00134
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
00169 if (!m_controller)
00170 m_controller = ControllerBasePtr(new ControllerRawForces(*this));
00171 }
00172
00173
00174 void DynamicsAckermann::invoke_motor_controllers(
00175 const TSimulContext& context, std::vector<double>& out_torque_per_wheel)
00176 {
00177
00178 out_torque_per_wheel.assign(4, 0.0);
00179
00180 if (m_controller)
00181 {
00182
00183 TControllerInput ci;
00184 ci.context = context;
00185 TControllerOutput co;
00186 m_controller->control_step(ci, co);
00187
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
00194
00195
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
00207
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
00220
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
00228 vec3 DynamicsAckermann::getVelocityLocalOdoEstimate() const
00229 {
00230 vec3 odo_vel;
00231
00232
00233
00234
00235
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
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 }