VehicleDifferential.h

Go to the documentation of this file.

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2023  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/math/TPoint2D.h>
#include <mvsim/PID_Controller.h>
#include <mvsim/VehicleBase.h>

namespace mvsim
{
class DynamicsDifferential : public VehicleBase
{
        DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsDifferential)
   public:
        enum
        {
                // common to all:
                WHEEL_L = 0,
                WHEEL_R = 1,
                // 3 wheels:
                WHEEL_CASTER_FRONT = 2,
                // 4 wheels:
                WHEEL_LR = 0,
                WHEEL_RR = 1,
                WHEEL_LF = 2,
                WHEEL_RF = 3
        };

        struct ConfigPerWheel
        {
                ConfigPerWheel() = default;
                ConfigPerWheel(
                        const std::string& _name, const mrpt::math::TPoint2D& _pos)
                        : name(_name), pos(_pos)
                {
                }

                std::string name;
                mrpt::math::TPoint2D pos;
        };

        DynamicsDifferential(World* parent)
                : DynamicsDifferential(
                          parent, {
                                                  {"l_wheel", {0.0, 0.5}},
                                                  {"r_wheel", {0.0, -0.5}},
                                          })
        {
        }

        DynamicsDifferential(
                World* parent, const std::vector<ConfigPerWheel>& cfgPerWheel);

        struct TControllerInput
        {
                TSimulContext context;
        };

        struct TControllerOutput
        {
                TControllerOutput() = default;

                double wheel_torque_l = 0;
                double wheel_torque_r = 0;
        };

        using ControllerBase = ControllerBaseTempl<DynamicsDifferential>;

        class ControllerRawForces : public ControllerBase
        {
           public:
                ControllerRawForces(DynamicsDifferential& veh) : ControllerBase(veh) {}

                static const char* class_name() { return "raw"; }

                double setpoint_wheel_torque_l = 0, setpoint_wheel_torque_r = 0;

                double setpoint_teleop_steps = 5e-2;

                virtual void control_step(
                        const DynamicsDifferential::TControllerInput& ci,
                        DynamicsDifferential::TControllerOutput& co) override;
                virtual void teleop_interface(
                        const TeleopInput& in, TeleopOutput& out) override;
        };

        class ControllerTwistPID : public ControllerBase
        {
           public:
                ControllerTwistPID(DynamicsDifferential& veh);
                static const char* class_name() { return "twist_pid"; }

                virtual void control_step(
                        const DynamicsDifferential::TControllerInput& ci,
                        DynamicsDifferential::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 = 10, KI = 0, KD = 0;

                double max_torque = 100;

                // See base docs.
                bool setTwistCommand(const mrpt::math::TTwist2D& t) override
                {
                        setpointMtx_.lock();
                        setpoint_ = t;
                        setpointMtx_.unlock();
                        return true;
                }

                mrpt::math::TTwist2D setpoint() const
                {
                        setpointMtx_.lock();
                        auto t = setpoint_;
                        setpointMtx_.unlock();
                        return t;
                }

           private:
                double distWheels_ = 0;
                std::array<PID_Controller, 2> PIDs_;
                mrpt::math::TTwist2D setpoint_{0, 0, 0};  
                mutable std::mutex setpointMtx_;
        };

        class ControllerTwistIdeal : public ControllerBase
        {
           public:
                ControllerTwistIdeal(DynamicsDifferential& veh);
                static const char* class_name() { return "twist_ideal"; }

                void control_step(
                        const DynamicsDifferential::TControllerInput& ci,
                        DynamicsDifferential::TControllerOutput& co) override;
                void on_post_step(const TSimulContext& context) override;

                virtual void teleop_interface(
                        const TeleopInput& in, TeleopOutput& out) override;

                // See base docs.
                bool setTwistCommand(const mrpt::math::TTwist2D& t) override
                {
                        setpointMtx_.lock();
                        setpoint_ = t;
                        setpointMtx_.unlock();
                        return true;
                }

                mrpt::math::TTwist2D setpoint() const
                {
                        setpointMtx_.lock();
                        auto t = setpoint_;
                        setpointMtx_.unlock();
                        return t;
                }

           private:
                double distWheels_ = 0;
                mrpt::math::TTwist2D setpoint_{0, 0, 0};  
                mutable std::mutex setpointMtx_;
        };

        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;

   protected:
        // See base class docs
        virtual void dynamics_load_params_from_xml(
                const rapidxml::xml_node<char>* xml_node) override;
        // See base class docs
        virtual std::vector<double> invoke_motor_controllers(
                const TSimulContext& context) override;
        virtual void invoke_motor_controllers_post_step(
                const TSimulContext& context) override;

        const std::vector<ConfigPerWheel> configPerWheel_;

   private:
        ControllerBase::Ptr controller_;  
};

class DynamicsDifferential_3_wheels : public DynamicsDifferential
{
        DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsDifferential_3_wheels)

   public:
        DynamicsDifferential_3_wheels(World* parent)
                : DynamicsDifferential(
                          parent, {
                                                  {"l_wheel", {0.0, 0.5}},
                                                  {"r_wheel", {0.0, -0.5}},
                                                  {"caster_wheel", {0.5, 0.0}},
                                          })
        {
        }
};

class DynamicsDifferential_4_wheels : public DynamicsDifferential
{
        DECLARES_REGISTER_VEHICLE_DYNAMICS(DynamicsDifferential_4_wheels)

   public:
        DynamicsDifferential_4_wheels(World* parent)
                : DynamicsDifferential(
                          parent, {
                                                  {"lr_wheel", {0.0, 0.5}},
                                                  {"rr_wheel", {0.0, -0.5}},
                                                  {"lf_wheel", {0.5, 0.5}},
                                                  {"rf_wheel", {0.5, -0.5}},
                                          })
        {
        }
};

}  // namespace mvsim