diff_drive_dynamics.h

Go to the documentation of this file.

#ifndef DIFF_DRIVE_DYNAMICS_H
#define DIFF_DRIVE_DYNAMICS_H
 
#include "bitrl/bitrl_types.h"
#include "bitrl/dynamics/dynamics_matrix_descriptor.h"
#include "bitrl/dynamics/motion_model_base.h"
#include "bitrl/dynamics/system_state.h"
 
#include <any>
#include <array>
#include <map>
 
namespace bitrl
{
namespace dynamics
{
 
class DiffDriveDynamics : public MotionModelDynamicsBase<SysState<3>, DynamicsMatrixDescriptor,
                                                         std::map<std::string, std::any>>
{
  public:
    enum class DynamicVersion
    {
        V1,
        V2,
        V3
    };
 
    typedef MotionModelDynamicsBase<SysState<3>, DynamicsMatrixDescriptor,
                                    std::map<std::string, std::any>>::state_type state_type;
 
    typedef MotionModelDynamicsBase<SysState<3>, DynamicsMatrixDescriptor,
                                    std::map<std::string, std::any>>::input_type input_type;
 
    typedef MotionModelDynamicsBase<SysState<3>, DynamicsMatrixDescriptor,
                                    std::map<std::string, std::any>>::matrix_type matrix_type;
 
    typedef MotionModelDynamicsBase<SysState<3>, DynamicsMatrixDescriptor,
                                    std::map<std::string, std::any>>::vector_type vector_type;
 
    static SysState<3> integrate_state_v1(const SysState<3> &state, real_t tol, real_t dt, real_t v,
                                          real_t w, const std::array<real_t, 2> &errors);
 
    static SysState<3> integrate_state_v2(const SysState<3> &state, real_t dt, real_t v, real_t w,
                                          const std::array<real_t, 2> &errors);
 
    static SysState<3> integrate_state_v3(const SysState<3> &state, real_t r, real_t l, real_t dt,
                                          real_t w1, real_t w2,
                                          const std::array<real_t, 2> &errors);
 
    static SysState<3> integrate(const SysState<3> &state, const input_type &input,
                                 const DynamicVersion version);
 
    explicit DiffDriveDynamics(DynamicVersion type = DynamicVersion::V1,
                               bool update_description_matrices_on_evaluate = true);
 
    explicit DiffDriveDynamics(state_type &&state);
 
    virtual state_type &evaluate(const input_type &input) override;
 
    void integrate(const input_type &input);
 
    real_t get_x_position() const { return this->state_.get("X"); }
 
    void set_x_position(real_t x) { this->state_.set("X", x); }
 
    real_t get_y_position() const { return this->state_.get("Y"); }
 
    void set_y_position(real_t y) { this->state_.set("Y", y); }
 
    real_t get_orientation() const { return this->state_.get("Theta"); }
 
    void set_orientation(real_t theta) { this->state_.set("Theta", theta); }
 
    real_t get_velocity() const { return v_; }
 
    real_t get_angular_velocity() const { return w_; }
 
    void update_matrices(const input_type &input);
 
    void initialize_matrices(const input_type &input);
 
  private:
    real_t v_;
 
    real_t w_;
 
    DynamicVersion type_;
};
 
} // namespace dynamics
 
} // namespace bitrl
 
#endif // DIFF_DRIVE_DYNAMICS_H