VehicleAckermann.h
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 #pragma once
00011 
00012 #include <mvsim/VehicleBase.h>
00013 #include <mvsim/PID_Controller.h>
00014 
00015 #include <mrpt/opengl/CSetOfObjects.h>
00016 #include <mrpt/math/lightweight_geom_data.h>
00017 #include <mrpt/utils/TColor.h>
00018 
00019 namespace mvsim
00020 {
00024 class DynamicsAckermann : public VehicleBase
00025 {
00026         DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsAckermann)
00027    public:
00028         // Wheels: [0]:rear-left, [1]:rear-right, [2]: front-left, [3]: front-right
00029         enum
00030         {
00031                 WHEEL_RL = 0,
00032                 WHEEL_RR = 1,
00033                 WHEEL_FL = 2,
00034                 WHEEL_FR = 3
00035         };
00036 
00037         DynamicsAckermann(World* parent);
00038 
00040         double getMaxSteeringAngle() const { return m_max_steer_ang; }
00041         void setMaxSteeringAngle(double val) { m_max_steer_ang = val; }
00045         struct TControllerInput
00046         {
00047                 TSimulContext context;
00048         };
00049         struct TControllerOutput
00050         {
00051                 double fl_torque, fr_torque, rl_torque, rr_torque;
00052                 double steer_ang;  
00053                 TControllerOutput()
00054                         : fl_torque(0),
00055                           fr_torque(0),
00056                           rl_torque(0),
00057                           rr_torque(0),
00058                           steer_ang(0)
00059                 {
00060                 }
00061         };
00062 
00064         typedef ControllerBaseTempl<DynamicsAckermann> ControllerBase;
00065         typedef std::shared_ptr<ControllerBase> ControllerBasePtr;
00066 
00067         class ControllerRawForces : public ControllerBase
00068         {
00069            public:
00070                 ControllerRawForces(DynamicsAckermann& veh);
00071                 static const char* class_name() { return "raw"; }
00074                 double setpoint_wheel_torque_l, setpoint_wheel_torque_r,
00075                         setpoint_steer_ang;
00076                 virtual void control_step(
00077                         const DynamicsAckermann::TControllerInput& ci,
00078                         DynamicsAckermann::TControllerOutput& co);  // See base class docs
00079                 virtual void load_config(
00080                         const rapidxml::xml_node<char>& node);  // See base class docs
00081                 virtual void teleop_interface(
00082                         const TeleopInput& in, TeleopOutput& out);  // See base class docs
00083         };
00084 
00087         class ControllerTwistFrontSteerPID : public ControllerBase
00088         {
00089            public:
00090                 ControllerTwistFrontSteerPID(DynamicsAckermann& veh);
00091                 static const char* class_name() { return "twist_front_steer_pid"; }
00094                 double setpoint_lin_speed,
00095                         setpoint_ang_speed;  
00096                 virtual void control_step(
00097                         const DynamicsAckermann::TControllerInput& ci,
00098                         DynamicsAckermann::TControllerOutput& co);  // See base class docs
00099                 virtual void load_config(
00100                         const rapidxml::xml_node<char>& node);  // See base class docs
00101                 virtual void teleop_interface(
00102                         const TeleopInput& in, TeleopOutput& out);  // See base class docs
00103 
00104                 double KP, KI, KD;  
00105                 double max_torque;  
00106 
00107                 // See base docs.
00108                 virtual bool setTwistCommand(const double vx, const double wz)
00109                 {
00110                         setpoint_lin_speed = vx;
00111                         setpoint_ang_speed = wz;
00112                         return true;
00113                 }
00114 
00115            private:
00116                 double m_dist_fWheels, m_r2f_L;
00117                 PID_Controller m_PID[2];  //<! [0]:fl, [1]: fr
00118         };
00119 
00122         class ControllerFrontSteerPID : public ControllerBase
00123         {
00124            public:
00125                 ControllerFrontSteerPID(DynamicsAckermann& veh);
00126                 static const char* class_name() { return "front_steer_pid"; }
00129                 double setpoint_lin_speed, setpoint_steer_ang;  
00130 
00131 
00132                 virtual void control_step(
00133                         const DynamicsAckermann::TControllerInput& ci,
00134                         DynamicsAckermann::TControllerOutput& co);  // See base class docs
00135                 virtual void load_config(
00136                         const rapidxml::xml_node<char>& node);  // See base class docs
00137                 virtual void teleop_interface(
00138                         const TeleopInput& in, TeleopOutput& out);  // See base class docs
00139 
00140                 double KP, KI, KD;  
00141                 double max_torque;  
00142            private:
00143                 ControllerTwistFrontSteerPID m_twist_control;
00144                 double m_r2f_L;
00145         };
00146 
00147         const ControllerBasePtr& getController() const { return m_controller; }
00148         ControllerBasePtr& getController() { return m_controller; }
00149         virtual ControllerBaseInterface* getControllerInterface()
00150         {
00151                 return m_controller.get();
00152         }
00153   // end controllers
00155 
00156         virtual vec3 getVelocityLocalOdoEstimate() const;  // See docs of base class
00157 
00163         void computeFrontWheelAngles(
00164                 const double desired_equiv_steer_ang, double& out_fl_ang,
00165                 double& out_fr_ang) const;
00166 
00167    protected:
00168         // See base class docs
00169         virtual void dynamics_load_params_from_xml(
00170                 const rapidxml::xml_node<char>* xml_node);
00171         // See base class doc
00172         virtual void invoke_motor_controllers(
00173                 const TSimulContext& context, std::vector<double>& out_force_per_wheel);
00174 
00175    private:
00176         ControllerBasePtr m_controller;  
00177 
00178         double m_max_steer_ang;  
00179 
00180 };
00181 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:48