quadrotor_dynamics.h

Go to the documentation of this file.

#ifndef QUADROTOR_DYNAMICS_H
#define QUADROTOR_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>
 
namespace bitrl::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;
}
 
} // namespace bitrl::dynamics
 
#endif // QUADROTOR_DYNAMICS_H