Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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 #if MRPT_VERSION<0x199
00018 #include <mrpt/utils/TColor.h>
00019 using mrpt::utils::TColor;
00020 using mrpt::utils::keep_min;
00021 using mrpt::utils::keep_max;
00022 using mrpt::utils::signWithZero;
00023 #else
00024 #include <mrpt/img/TColor.h>
00025 using mrpt::img::TColor;
00026 using mrpt::keep_min;
00027 using mrpt::keep_max;
00028 using mrpt::signWithZero;
00029 #endif
00030
00031 namespace mvsim
00032 {
00041 class DynamicsAckermannDrivetrain : public VehicleBase
00042 {
00043 DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsAckermannDrivetrain)
00044 public:
00045
00046 enum
00047 {
00048 WHEEL_RL = 0,
00049 WHEEL_RR = 1,
00050 WHEEL_FL = 2,
00051 WHEEL_FR = 3
00052 };
00053
00054 enum DifferentialType
00055 {
00056 DIFF_OPEN_FRONT = 0,
00057 DIFF_OPEN_REAR = 1,
00058 DIFF_OPEN_4WD = 2,
00059
00060 DIFF_TORSEN_FRONT = 3,
00061 DIFF_TORSEN_REAR = 4,
00062 DIFF_TORSEN_4WD = 5,
00063
00064 DIFF_MAX
00065 };
00066
00067 DynamicsAckermannDrivetrain(World* parent);
00068
00070 double getMaxSteeringAngle() const { return m_max_steer_ang; }
00071 void setMaxSteeringAngle(double val) { m_max_steer_ang = val; }
00075 struct TControllerInput
00076 {
00077 TSimulContext context;
00078 };
00079 struct TControllerOutput
00080 {
00081 double drive_torque;
00082 double steer_ang;
00083 TControllerOutput() : drive_torque(0), steer_ang(0) {}
00084 };
00085
00088 typedef ControllerBaseTempl<DynamicsAckermannDrivetrain> ControllerBase;
00089 typedef std::shared_ptr<ControllerBase> ControllerBasePtr;
00090
00091 class ControllerRawForces : public ControllerBase
00092 {
00093 public:
00094 ControllerRawForces(DynamicsAckermannDrivetrain& veh);
00095 static const char* class_name() { return "raw"; }
00098 double setpoint_wheel_torque, setpoint_steer_ang;
00099 virtual void control_step(
00100 const DynamicsAckermannDrivetrain::TControllerInput& ci,
00101 DynamicsAckermannDrivetrain::TControllerOutput&
00102 co);
00103 virtual void load_config(
00104 const rapidxml::xml_node<char>& node);
00105 virtual void teleop_interface(
00106 const TeleopInput& in, TeleopOutput& out);
00107 };
00108
00111 class ControllerTwistFrontSteerPID : public ControllerBase
00112 {
00113 public:
00114 ControllerTwistFrontSteerPID(DynamicsAckermannDrivetrain& veh);
00115 static const char* class_name() { return "twist_front_steer_pid"; }
00118 double setpoint_lin_speed,
00119 setpoint_ang_speed;
00120 virtual void control_step(
00121 const DynamicsAckermannDrivetrain::TControllerInput& ci,
00122 DynamicsAckermannDrivetrain::TControllerOutput& co);
00123 virtual void load_config(const rapidxml::xml_node<char>& node);
00124 virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out);
00125
00126 double KP, KI, KD;
00127 double max_torque;
00128
00129
00130 virtual bool setTwistCommand(const double vx, const double wz)
00131 {
00132 setpoint_lin_speed = vx;
00133 setpoint_ang_speed = wz;
00134 return true;
00135 }
00136
00137 private:
00138 double m_dist_fWheels, m_r2f_L;
00139 PID_Controller m_PID;
00140 };
00141
00142 class ControllerFrontSteerPID : public ControllerBase
00143 {
00144 public:
00145 ControllerFrontSteerPID(DynamicsAckermannDrivetrain& veh);
00146 static const char* class_name() { return "front_steer_pid"; }
00149 double setpoint_lin_speed, setpoint_steer_ang;
00150
00151
00152 virtual void control_step(
00153 const DynamicsAckermannDrivetrain::TControllerInput& ci,
00154 DynamicsAckermannDrivetrain::TControllerOutput&
00155 co);
00156 virtual void load_config(
00157 const rapidxml::xml_node<char>& node);
00158 virtual void teleop_interface(
00159 const TeleopInput& in, TeleopOutput& out);
00160
00161 double KP, KI, KD;
00162 double max_torque;
00163 private:
00164 ControllerTwistFrontSteerPID m_twist_control;
00165 double m_r2f_L;
00166 };
00167
00168 const ControllerBasePtr& getController() const { return m_controller; }
00169 ControllerBasePtr& getController() { return m_controller; }
00170 virtual ControllerBaseInterface* getControllerInterface()
00171 {
00172 return m_controller.get();
00173 }
00174
00176
00177 virtual vec3 getVelocityLocalOdoEstimate() const;
00178
00184 void computeFrontWheelAngles(
00185 const double desired_equiv_steer_ang, double& out_fl_ang,
00186 double& out_fr_ang) const;
00187
00190 void computeDiffTorqueSplit(
00191 const double w1, const double w2, const double diffBias,
00192 const double defaultSplitRatio, double& t1, double& t2);
00193
00194 protected:
00195
00196 virtual void dynamics_load_params_from_xml(
00197 const rapidxml::xml_node<char>* xml_node);
00198
00199 virtual void invoke_motor_controllers(
00200 const TSimulContext& context, std::vector<double>& out_force_per_wheel);
00201
00202 private:
00203 ControllerBasePtr m_controller;
00204
00205 double m_max_steer_ang;
00206
00207
00208 DifferentialType m_diff_type;
00209
00210 double m_FrontRearSplit;
00211 double m_FrontLRSplit;
00212 double m_RearLRSplit;
00213
00214 double m_FrontRearBias;
00215 double m_FrontLRBias;
00216 double m_RearLRBias;
00217 };
00218 }