SystemDynamicsBase.h

Go to the documentation of this file.

#ifndef CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H
#define CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H
 
#include <utils/Transforms.h>
 
#include <array>
 
class SystemDynamicsBase {
private:
    Matrix6d compute_coriolis_from_mass(const Matrix6d& mass, const Vector6d& velocity) const;
 
    void update_mass();
 
    double mass_ = NAN;
 
    double weight_ = NAN;
 
    double buoyancy_ = NAN;
 
    Matrix3d Irb_ = Matrix3d::Zero();
 
    Matrix6d Mrb_ = Matrix6d::Zero();
 
    Matrix6d Ma_ = Matrix6d::Zero();
 
    Matrix6d M_ = Matrix6d::Zero();
 
    Matrix6d Dl_ = Matrix6d::Zero();
 
    Matrix6d Dnl_ = Matrix6d::Zero();
 
    Vector3d rgb_ = Vector3d::Zero();
 
    Vector3d rbb_ = Vector3d::Zero();
 
    Vector3d fgn_ = Vector3d::Zero();
 
    Vector3d fbn_ = Vector3d::Zero();
 
public:
    const bool is_system_3dof_;
 
    explicit SystemDynamicsBase(const bool is_3dof);
 
    virtual ~SystemDynamicsBase() = default;
 
    /* ------------------------------------------------------------------ */
    /* Const accessors                                                     */
    /* ------------------------------------------------------------------ */
 
    const Matrix6d& get_m() const {
        return M_;
    }
 
    const Matrix6d& get_mrb() const {
        return Mrb_;
    }
 
    const Matrix6d& get_ma_matrix() const {
        return Ma_;
    }
 
    const Matrix6d& get_dl_matrix() const {
        return Dl_;
    }
 
    const Matrix6d& get_dnl_matrix() const {
        return Dnl_;
    }
 
    const Vector3d& get_rgb_vec() const {
        return rgb_;
    }
 
    const Vector3d& get_rbb_vec() const {
        return rbb_;
    }
 
    const Vector3d& get_fgn() const {
        return fgn_;
    }
 
    const Vector3d& get_fbn() const {
        return fbn_;
    }
 
    double get_mass_value() const {
        return mass_;
    }
 
    double get_weight() const {
        return weight_;
    }
 
    double get_buoyancy_value() const {
        return buoyancy_;
    }
 
    /* ------------------------------------------------------------------ */
    /* Dynamics evaluation (const)                                         */
    /* ------------------------------------------------------------------ */
 
    Matrix6d get_d(const Vector6d& velocity) const;
 
    Matrix6d get_crb(const Vector6d& velocity) const;
 
    Matrix6d get_ca(const Vector6d& velocity) const;
 
    Matrix6d get_c(const Vector6d& velocity) const;
 
    Vector6d get_g(const Quaterniond& q) const;
 
    Vector6d get_g(const Vector3d& rpy) const;
 
    virtual Vector6d get_inverse_dynamics(const Vector6d& velocity, const Vector6d& acceleration,
                                          const Quaterniond& orientation) const;
 
    /* ------------------------------------------------------------------ */
    /* Mutators (maintain M_ = Mrb_ + Ma_ invariant)                      */
    /* ------------------------------------------------------------------ */
 
    void set_mass(double m);
 
    void set_buoyancy(double factor);
 
    void set_i(const std::array<double, 6>& I);
 
    void set_ma(const std::array<double, 6>& m);
 
    void set_dl(const std::array<double, 6>& d);
 
    void set_dnl(const std::array<double, 6>& d);
 
    void set_rbb(const std::array<double, 3>& rbb);
 
    void set_rgb(const std::array<double, 3>& rgb);
};
 
#endif  // CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H