VehicleAckermann_ControllerTwistFrontSteerPID.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 "xml_utils.h"
00012 
00013 using namespace mvsim;
00014 using namespace std;
00015 
00016 DynamicsAckermann::ControllerTwistFrontSteerPID::ControllerTwistFrontSteerPID(
00017         DynamicsAckermann& veh)
00018         : ControllerBase(veh),
00019           setpoint_lin_speed(0),
00020           setpoint_ang_speed(0),
00021           KP(100),
00022           KI(0),
00023           KD(0),
00024           max_torque(100.0)
00025 {
00026         // Get distance between wheels:
00027         m_dist_fWheels =
00028                 m_veh.m_wheels_info[WHEEL_FL].y - m_veh.m_wheels_info[WHEEL_FR].y;
00029         m_r2f_L = m_veh.m_wheels_info[WHEEL_FL].x - m_veh.m_wheels_info[WHEEL_RL].x;
00030         ASSERT_(m_dist_fWheels > 0.0);
00031         ASSERT_(m_r2f_L > 0.0);
00032 }
00033 
00034 // See base class docs
00035 void DynamicsAckermann::ControllerTwistFrontSteerPID::control_step(
00036         const DynamicsAckermann::TControllerInput& ci,
00037         DynamicsAckermann::TControllerOutput& co)
00038 {
00039         // For each wheel:
00040         // 1) Compute desired velocity set-point (in m/s)
00041         // 2) Run the PI/PID for that wheel independently (in newtons)
00042 
00043         // 1st: desired steering angle:
00044         // --------------------------------
00045         if (setpoint_ang_speed == 0)
00046         {
00047                 co.steer_ang = 0.0;
00048         }
00049         else
00050         {
00051                 const double R = setpoint_lin_speed / setpoint_ang_speed;
00052                 co.steer_ang = atan(m_r2f_L / R);
00053         }
00054 
00055         // Desired velocities for each wheel:
00056         std::vector<mrpt::math::TPoint2D>
00057                 desired_wheel_vels;  // In local vehicle frame
00058         m_veh.getWheelsVelocityLocal(
00059                 desired_wheel_vels, vec3(setpoint_lin_speed, 0.0, setpoint_ang_speed));
00060 
00061         ASSERT_(desired_wheel_vels.size() == 4);
00062 
00063         // Rotate to obtain the actual desired longitudinal velocity for each wheel:
00064         // FL:
00065         double vel_fl, vel_fr;
00066         double desired_fl_steer_ang, desired_fr_steer_ang;
00067         m_veh.computeFrontWheelAngles(
00068                 co.steer_ang, desired_fl_steer_ang, desired_fr_steer_ang);
00069         {
00070                 const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fl_steer_ang);
00071                 mrpt::math::TPoint2D vel_w;
00072                 wRotInv.composePoint(
00073                         desired_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w);
00074                 vel_fl = vel_w.x;
00075         }
00076         {
00077                 const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fr_steer_ang);
00078                 mrpt::math::TPoint2D vel_w;
00079                 wRotInv.composePoint(
00080                         desired_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w);
00081                 vel_fr = vel_w.x;
00082         }
00083 
00084         // Compute each wheel actual velocity (from an "odometry" estimation of
00085         // velocity, not ground-truth!):
00086         double act_vel_fl, act_vel_fr;
00087         {
00088                 std::vector<mrpt::math::TPoint2D>
00089                         odo_wheel_vels;  // In local vehicle frame
00090                 m_veh.getWheelsVelocityLocal(
00091                         odo_wheel_vels, m_veh.getVelocityLocalOdoEstimate());
00092                 ASSERT_(odo_wheel_vels.size() == 4);
00093 
00094                 const double actual_fl_steer_ang =
00095                         m_veh.getWheelInfo(DynamicsAckermann::WHEEL_FL).yaw;
00096                 const double actual_fr_steer_ang =
00097                         m_veh.getWheelInfo(DynamicsAckermann::WHEEL_FR).yaw;
00098 
00099                 {
00100                         const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fl_steer_ang);
00101                         mrpt::math::TPoint2D vel_w;
00102                         wRotInv.composePoint(
00103                                 odo_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w);
00104                         act_vel_fl = vel_w.x;
00105                 }
00106                 {
00107                         const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fr_steer_ang);
00108                         mrpt::math::TPoint2D vel_w;
00109                         wRotInv.composePoint(
00110                                 odo_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w);
00111                         act_vel_fr = vel_w.x;
00112                 }
00113         }
00114 
00115         // Apply controller:
00116         for (int i = 0; i < 2; i++)
00117         {
00118                 m_PID[i].KP = KP;
00119                 m_PID[i].KI = KI;
00120                 m_PID[i].KD = KD;
00121                 m_PID[i].max_out = max_torque;
00122         }
00123 
00124         co.rl_torque = .0;
00125         co.rr_torque = .0;
00126         co.fl_torque = -m_PID[0].compute(
00127                 vel_fl - act_vel_fl,
00128                 ci.context.dt);  // "-" because \tau<0 makes robot moves forwards.
00129         co.fr_torque = -m_PID[1].compute(vel_fr - act_vel_fr, ci.context.dt);
00130 }
00131 
00132 void DynamicsAckermann::ControllerTwistFrontSteerPID::load_config(
00133         const rapidxml::xml_node<char>& node)
00134 {
00135         std::map<std::string, TParamEntry> params;
00136         params["KP"] = TParamEntry("%lf", &KP);
00137         params["KI"] = TParamEntry("%lf", &KI);
00138         params["KD"] = TParamEntry("%lf", &KD);
00139         params["max_torque"] = TParamEntry("%lf", &max_torque);
00140 
00141         // Initial speed.
00142         params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
00143         params["W"] = TParamEntry("%lf_deg", &this->setpoint_ang_speed);
00144 
00145         parse_xmlnode_children_as_param(node, params);
00146 }
00147 
00148 void DynamicsAckermann::ControllerTwistFrontSteerPID::teleop_interface(
00149         const TeleopInput& in, TeleopOutput& out)
00150 {
00151         ControllerBase::teleop_interface(in, out);
00152 
00153         switch (in.keycode)
00154         {
00155                 case 'W':
00156                 case 'w':
00157                         setpoint_lin_speed += 0.1;
00158                         break;
00159 
00160                 case 'S':
00161                 case 's':
00162                         setpoint_lin_speed -= 0.1;
00163                         break;
00164 
00165                 case 'A':
00166                 case 'a':
00167                         setpoint_ang_speed += 1.0 * M_PI / 180.0;
00168                         break;
00169 
00170                 case 'D':
00171                 case 'd':
00172                         setpoint_ang_speed -= 1.0 * M_PI / 180.0;
00173                         break;
00174 
00175                 case ' ':
00176                         setpoint_lin_speed = .0;
00177                         setpoint_ang_speed = .0;
00178                         break;
00179         };
00180         out.append_gui_lines += "[Controller=" + string(class_name()) +
00181                                                         "] Teleop keys: w/s=incr/decr lin speed. "
00182                                                         "a/d=left/right steering. spacebar=stop.\n";
00183         out.append_gui_lines += mrpt::format(
00184                 "setpoint: v=%.03f w=%.03f deg/s\n", setpoint_lin_speed,
00185                 setpoint_ang_speed * 180.0 / M_PI);
00186 }


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