Program Listing for File quadrotor_dynamics.h

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

#ifndef QUADROTOR_DYNAMICS_H
#define QUADROTOR_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 "rlenvs/dynamics/system_state.h"

#include <any>


namespace rlenvscpp::dynamics {


struct QuadrotorDynamicsConfig
{
    bool use_gravity{true};

    bool use_description_matrices{false};

    real_t mass;

    real_t l;

    real_t k_1;

    real_t k_2;

    real_t dt;

    real_t Jx;

    real_t Jy;

    real_t Jz;
};

class QuadrotorDynamics: public MotionModelDynamicsBase<SysState<12>,
                                                        DynamicsMatrixDescriptor,
                                                        RealVec>
{
public:

    typedef MotionModelDynamicsBase<SysState<12>,
                                    DynamicsMatrixDescriptor,
                                    RealVec> base_type;

    typedef base_type::state_type state_type;

    typedef base_type::input_type input_type;

    typedef base_type::matrix_type matrix_type;

    typedef base_type::vector_type vector_type;

    QuadrotorDynamics(QuadrotorDynamicsConfig config,
                      SysState<12>& state);

    virtual state_type& evaluate(const input_type& input )override;

    void integrate(const RealVec& motor_w);

    void translational_dynamics(const RealVec& motor_w);

    void rotational_dynamics(const RealVec& motor_w);

    RealColVec3d get_position()const{return get_position_from_state_();}

    RealColVec3d get_velocity()const{return get_velocity_from_state_();}

    RealColVec3d get_angular_velocity()const{return get_angular_velocity_from_state_();}

    RealColVec3d get_euler_angles()const{return get_euler_angles_from_state_();}

    real_t phi()const noexcept {return get_euler_angles_from_state_()[0];}

    real_t theta()const noexcept {return get_euler_angles_from_state_()[1];}

    real_t psi()const noexcept {return get_euler_angles_from_state_()[2];}

private:

    QuadrotorDynamicsConfig config_;

    RealMat3d rotation_mat_;

    RealMat3d euler_mat_;

    RealColVec3d v_dot_;

    RealColVec3d omega_dot_;

    RealColVec3d euler_dot_;

    void update_position_();

    void update_rotation_matrix_();

    void update_euler_angles_();


    RealColVec3d get_velocity_from_state_()const;
    RealColVec3d get_angular_velocity_from_state_()const;
    RealColVec3d get_position_from_state_()const;
    RealColVec3d get_euler_angles_from_state_()const;



};

inline
RealColVec3d
QuadrotorDynamics::get_velocity_from_state_()const{

    RealColVec3d v = RealColVec3d::Zero();
    v[0] = this -> get_state_property("u");
    v[1] = this -> get_state_property("v");
    v[2] = this -> get_state_property("w");
    return v;

}

inline
RealColVec3d
QuadrotorDynamics::get_position_from_state_()const{

    RealColVec3d v = RealColVec3d::Zero();
    v[0] = this -> get_state_property("x");
    v[1] = this -> get_state_property("y");
    v[2] = this -> get_state_property("z");
    return v;

}

inline
RealColVec3d
QuadrotorDynamics::get_angular_velocity_from_state_()const{

    RealColVec3d v = RealColVec3d::Zero();
    v[0] = this -> get_state_property("p");
    v[1] = this -> get_state_property("q");
    v[2] = this -> get_state_property("r");
    return v;
}

inline
RealColVec3d
QuadrotorDynamics::get_euler_angles_from_state_()const{

    RealColVec3d v = RealColVec3d::Zero();
    v[0] = this -> get_state_property("phi");
    v[1] = this -> get_state_property("theta");
    v[2] = this -> get_state_property("psi");
    return v;
}

}



#endif // QUADROTOR_DYNAMICS_H