VehicleAckermann_Drivetrain.h

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <mrpt/img/TColor.h>
#include <mvsim/PID_Controller.h>
#include <mvsim/VehicleBase.h>
 
namespace mvsim
{
class DynamicsAckermannDrivetrain : public VehicleBase
{
    DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsAckermannDrivetrain)
   public:
    // Wheels: [0]:rear-left, [1]:rear-right, [2]: front-left, [3]: front-right
    enum
    {
        WHEEL_RL = 0,
        WHEEL_RR = 1,
        WHEEL_FL = 2,
        WHEEL_FR = 3
    };
 
    enum DifferentialType
    {
        DIFF_OPEN_FRONT = 0,
        DIFF_OPEN_REAR = 1,
        DIFF_OPEN_4WD = 2,
 
        DIFF_TORSEN_FRONT = 3,
        DIFF_TORSEN_REAR = 4,
        DIFF_TORSEN_4WD = 5,
 
        DIFF_MAX
    };
 
    DynamicsAckermannDrivetrain(World* parent);
 
    double getMaxSteeringAngle() const { return max_steer_ang_; }
    void setMaxSteeringAngle(double val) { max_steer_ang_ = val; }
    struct TControllerInput
    {
        TSimulContext context;
    };
    struct TControllerOutput
    {
        double drive_torque;
        double steer_ang;  
        TControllerOutput() : drive_torque(0), steer_ang(0) {}
    };
 
    using ControllerBase = ControllerBaseTempl<DynamicsAckermannDrivetrain>;
 
    class ControllerRawForces : public ControllerBase
    {
       public:
        ControllerRawForces(DynamicsAckermannDrivetrain& veh);
        static const char* class_name() { return "raw"; }
        double setpoint_wheel_torque, setpoint_steer_ang;
        virtual void control_step(
            const DynamicsAckermannDrivetrain::TControllerInput& ci,
            DynamicsAckermannDrivetrain::TControllerOutput& co) override;
        virtual void load_config(const rapidxml::xml_node<char>& node) override;
        virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override;
    };
 
    class ControllerTwistFrontSteerPID : public ControllerBase
    {
       public:
        ControllerTwistFrontSteerPID(DynamicsAckermannDrivetrain& veh);
        static const char* class_name() { return "twist_front_steer_pid"; }
        double setpoint_lin_speed,
            setpoint_ang_speed;  
        virtual void control_step(
            const DynamicsAckermannDrivetrain::TControllerInput& ci,
            DynamicsAckermannDrivetrain::TControllerOutput& co) override;
        virtual void load_config(const rapidxml::xml_node<char>& node) override;
        virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override;
 
        double KP, KI, KD;  
        double N = 0;  
        double max_torque;  
 
        bool enable_antiwindup = false;
        bool enable_feedforward = false;
        double feedforward_gain = 1.0;
        bool enable_reference_filter = false;
        double reference_filter_tau = 0.1;
        int reference_filter_order = 1;
 
        // See base docs.
        virtual bool setTwistCommand(const mrpt::math::TTwist2D& t) override
        {
            setpoint_lin_speed = t.vx;
            setpoint_ang_speed = t.omega;
            return true;
        }
 
       private:
        double dist_fWheels_, r2f_L_;
        PID_Controller PID_;
 
        double joyMaxLinSpeed = 1.0;
        double joyMaxAngSpeed = 0.7;
    };
 
    class ControllerFrontSteerPID : public ControllerBase
    {
       public:
        ControllerFrontSteerPID(DynamicsAckermannDrivetrain& veh);
        static const char* class_name() { return "front_steer_pid"; }
        double setpoint_lin_speed, setpoint_steer_ang;  
        virtual void control_step(
            const DynamicsAckermannDrivetrain::TControllerInput& ci,
            DynamicsAckermannDrivetrain::TControllerOutput& co) override;
        virtual void load_config(const rapidxml::xml_node<char>& node) override;
        virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override;
 
        double KP, KI, KD;  
        double max_torque;  
       private:
        ControllerTwistFrontSteerPID twist_control_;
        double r2f_L_;
    };
 
    const ControllerBase::Ptr& getController() const { return controller_; }
    ControllerBase::Ptr& getController() { return controller_; }
    virtual ControllerBaseInterface* getControllerInterface() override { return controller_.get(); }
  // end controllers
 
    virtual mrpt::math::TTwist2D getVelocityLocalOdoEstimate() const override;
 
    void computeFrontWheelAngles(
        const double desired_equiv_steer_ang, double& out_fl_ang, double& out_fr_ang) const;
 
    void computeDiffTorqueSplit(
        const double w1, const double w2, const double diffBias, const double defaultSplitRatio,
        double& t1, double& t2);
 
   protected:
    // See base class docs
    virtual void dynamics_load_params_from_xml(const rapidxml::xml_node<char>* xml_node) override;
    // See base class doc
    virtual std::vector<double> invoke_motor_controllers(const TSimulContext& context) override;
 
   private:
    ControllerBase::Ptr controller_;  
 
    double max_steer_ang_;  
 
    DifferentialType diff_type_;
 
    double FrontRearSplit_;
    double FrontLRSplit_;
    double RearLRSplit_;
 
    double FrontRearBias_;
    double FrontLRBias_;
    double RearLRBias_;
};
}  // namespace mvsim