Program Listing for File diff_drive_dynamics.h

↰ Return to documentation for file (src/rlenvs/dynamics/diff_drive_dynamics.h)

#ifndef DIFF_DRIVE_DYNAMICS_H
#define DIFF_DRIVE_DYNAMICS_H

#include "rlenvs/rlenvs_types_v2.h"
#include "rlenvs/dynamics/system_state.h"
#include "rlenvs/dynamics/motion_model_base.h"
#include "rlenvs/dynamics/dynamics_matrix_descriptor.h"

#include <array>
#include <map>
#include <any>

namespace rlenvscpp{
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_;

};

}

}

#endif // DIFF_DRIVE_DYNAMICS_H