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 computeCoriolisFromMass(const Matrix6d& mass, const Vector6d& velocity) const;
 
    void updateMass();
 
    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 isSystem3DOF;
 
    explicit SystemDynamicsBase(const bool is3DOF);
 
    virtual ~SystemDynamicsBase() = default;
 
    /* ------------------------------------------------------------------ */
    /* Const accessors                                                     */
    /* ------------------------------------------------------------------ */
 
    const Matrix6d& getM() const {
        return M_;
    }
 
    const Matrix6d& getMrb() const {
        return Mrb_;
    }
 
    const Matrix6d& getMaMatrix() const {
        return Ma_;
    }
 
    const Matrix6d& getDlMatrix() const {
        return Dl_;
    }
 
    const Matrix6d& getDnlMatrix() const {
        return Dnl_;
    }
 
    const Vector3d& getRgbVec() const {
        return rgb_;
    }
 
    const Vector3d& getRbbVec() const {
        return rbb_;
    }
 
    const Vector3d& getFgn() const {
        return fgn_;
    }
 
    const Vector3d& getFbn() const {
        return fbn_;
    }
 
    double getMassValue() const {
        return mass_;
    }
 
    double getWeight() const {
        return weight_;
    }
 
    double getBuoyancyValue() const {
        return buoyancy_;
    }
 
    /* ------------------------------------------------------------------ */
    /* Dynamics evaluation (const)                                         */
    /* ------------------------------------------------------------------ */
 
    Matrix6d getD(const Vector6d& velocity) const;
 
    Matrix6d getCrb(const Vector6d& velocity) const;
 
    Matrix6d getCa(const Vector6d& velocity) const;
 
    Matrix6d getC(const Vector6d& velocity) const;
 
    Vector6d getG(const Quaterniond& q) const;
 
    Vector6d getG(const Vector3d& rpy) const;
 
    virtual Vector6d getInverseDynamics(const Vector6d& velocity, const Vector6d& acceleration,
                                        const Quaterniond& orientation) const;
 
    /* ------------------------------------------------------------------ */
    /* Mutators (maintain M_ = Mrb_ + Ma_ invariant)                      */
    /* ------------------------------------------------------------------ */
 
    void setMass(double m);
 
    void setBuoyancy(double factor);
 
    void setI(const std::array<double, 6>& I);
 
    void setMa(const std::array<double, 6>& m);
 
    void setDl(const std::array<double, 6>& d);
 
    void setDnl(const std::array<double, 6>& d);
 
    void setRbb(const std::array<double, 3>& rbb);
 
    void setRgb(const std::array<double, 3>& rgb);
};
 
#endif  // CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H