Line data Source code
1 : #ifndef CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H 2 : #define CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H 3 : 4 : #include <utils/Transforms.h> 5 : 6 : #include <array> 7 : 8 : /** 9 : * @brief Base class encoding the 6DOF hydrodynamic model of a marine vehicle 10 : * 11 : * Stores mass, inertia, damping, and buoyancy parameters and provides methods 12 : * to evaluate the Coriolis matrix C(v), damping matrix D(v), restoring vector 13 : * G(q), and the inertial + Coriolis + damping expression M*a + C(v)*v + D(v)*v. 14 : * G(q) is computed separately and applied by BuoyancyController. 15 : * Subclasses may override getInverseDynamics() to zero out irrelevant DOFs. 16 : * 17 : * Invariant: M_ = Mrb_ + Ma_ must hold after every mutation. All setters 18 : * that modify Mrb_ or Ma_ call updateMass() to re-establish this. 19 : */ 20 1 : class SystemDynamicsBase { 21 : private: 22 : /** 23 : * @brief Computes a 6x6 Coriolis matrix from a given mass matrix 24 : * 25 : * @param mass [in] 6x6 mass matrix (rigid body or added mass) 26 : * @param velocity [in] 6DOF body-frame velocity 27 : * 28 : * @return 6x6 Coriolis matrix 29 : */ 30 : Matrix6d computeCoriolisFromMass(const Matrix6d& mass, const Vector6d& velocity) const; 31 : 32 : /** 33 : * @brief Recomputes the total mass matrix M = Mrb + Ma and symmetrises it 34 : */ 35 : void updateMass(); 36 : 37 : /** @brief Vehicle mass in kg */ 38 : double mass_ = NAN; 39 : 40 : /** @brief Vehicle weight (mass * g) in N */ 41 : double weight_ = NAN; 42 : 43 : /** @brief Net buoyancy force in N */ 44 : double buoyancy_ = NAN; 45 : 46 : /** @brief Rigid-body inertia tensor (3x3) */ 47 : Matrix3d Irb_ = Matrix3d::Zero(); 48 : 49 : /** @brief Rigid-body mass matrix (6x6) */ 50 : Matrix6d Mrb_ = Matrix6d::Zero(); 51 : 52 : /** @brief Added-mass matrix (6x6) */ 53 : Matrix6d Ma_ = Matrix6d::Zero(); 54 : 55 : /** @brief Total mass matrix M = Mrb + Ma (6x6) */ 56 : Matrix6d M_ = Matrix6d::Zero(); 57 : 58 : /** @brief Linear damping matrix (6x6) */ 59 : Matrix6d Dl_ = Matrix6d::Zero(); 60 : 61 : /** @brief Quadratic (nonlinear) damping matrix (6x6) */ 62 : Matrix6d Dnl_ = Matrix6d::Zero(); 63 : 64 : /** @brief Centre of gravity position relative to the body origin in the body frame (m) */ 65 : Vector3d rgb_ = Vector3d::Zero(); 66 : 67 : /** @brief Centre of buoyancy position relative to the body origin in the body frame (m) */ 68 : Vector3d rbb_ = Vector3d::Zero(); 69 : 70 : /** @brief Gravitational force vector in the world frame (N) */ 71 : Vector3d fgn_ = Vector3d::Zero(); 72 : 73 : /** @brief Buoyancy force vector in the world frame (N) */ 74 : Vector3d fbn_ = Vector3d::Zero(); 75 : 76 : public: 77 : /** 78 : * @brief True if the vehicle is constrained to the XY-Yaw plane (3DOF surface vehicle) 79 : */ 80 : const bool isSystem3DOF; 81 : 82 : /** 83 : * @brief Constructs the dynamics base and sets the DOF flag 84 : * 85 : * @param is3DOF [in] True for a surface vehicle constrained to XY-Yaw 86 : */ 87 1 : explicit SystemDynamicsBase(const bool is3DOF); 88 : 89 : /** 90 : * @brief Virtual destructor 91 : */ 92 1 : virtual ~SystemDynamicsBase() = default; 93 : 94 : /* ------------------------------------------------------------------ */ 95 : /* Const accessors */ 96 : /* ------------------------------------------------------------------ */ 97 : 98 : /** @brief Returns the total mass matrix M = Mrb + Ma */ 99 1 : const Matrix6d& getM() const { 100 : return M_; 101 : } 102 : 103 : /** @brief Returns the rigid-body mass matrix Mrb */ 104 1 : const Matrix6d& getMrb() const { 105 : return Mrb_; 106 : } 107 : 108 : /** @brief Returns the added-mass matrix Ma */ 109 1 : const Matrix6d& getMaMatrix() const { 110 : return Ma_; 111 : } 112 : 113 : /** @brief Returns the linear damping matrix Dl */ 114 1 : const Matrix6d& getDlMatrix() const { 115 : return Dl_; 116 : } 117 : 118 : /** @brief Returns the quadratic damping matrix Dnl */ 119 1 : const Matrix6d& getDnlMatrix() const { 120 : return Dnl_; 121 : } 122 : 123 : /** @brief Returns the centre of gravity in the body frame */ 124 1 : const Vector3d& getRgbVec() const { 125 : return rgb_; 126 : } 127 : 128 : /** @brief Returns the centre of buoyancy in the body frame */ 129 1 : const Vector3d& getRbbVec() const { 130 : return rbb_; 131 : } 132 : 133 : /** @brief Returns the gravitational force vector in the world frame */ 134 1 : const Vector3d& getFgn() const { 135 : return fgn_; 136 : } 137 : 138 : /** @brief Returns the buoyancy force vector in the world frame */ 139 1 : const Vector3d& getFbn() const { 140 : return fbn_; 141 : } 142 : 143 : /** @brief Returns the vehicle mass in kg */ 144 1 : double getMassValue() const { 145 : return mass_; 146 : } 147 : 148 : /** @brief Returns the vehicle weight in N */ 149 1 : double getWeight() const { 150 : return weight_; 151 : } 152 : 153 : /** @brief Returns the net buoyancy force in N */ 154 1 : double getBuoyancyValue() const { 155 : return buoyancy_; 156 : } 157 : 158 : /* ------------------------------------------------------------------ */ 159 : /* Dynamics evaluation (const) */ 160 : /* ------------------------------------------------------------------ */ 161 : 162 : /** 163 : * @brief Evaluates the total damping matrix D(v) = Dl + Dnl*diag(|v|) 164 : * 165 : * @param velocity [in] 6DOF body-frame velocity 166 : * 167 : * @return 6x6 velocity-dependent damping matrix 168 : */ 169 1 : Matrix6d getD(const Vector6d& velocity) const; 170 : 171 : /** 172 : * @brief Evaluates the rigid-body Coriolis matrix Crb(v) 173 : * 174 : * @param velocity [in] 6DOF body-frame velocity 175 : * 176 : * @return 6x6 rigid-body Coriolis matrix 177 : */ 178 1 : Matrix6d getCrb(const Vector6d& velocity) const; 179 : 180 : /** 181 : * @brief Evaluates the added-mass Coriolis matrix Ca(v) 182 : * 183 : * @param velocity [in] 6DOF body-frame velocity 184 : * 185 : * @return 6x6 added-mass Coriolis matrix 186 : */ 187 1 : Matrix6d getCa(const Vector6d& velocity) const; 188 : 189 : /** 190 : * @brief Evaluates the total Coriolis matrix C(v) = Crb(v) + Ca(v) 191 : * 192 : * @param velocity [in] 6DOF body-frame velocity 193 : * 194 : * @return 6x6 total Coriolis matrix 195 : */ 196 1 : Matrix6d getC(const Vector6d& velocity) const; 197 : 198 : /** 199 : * @brief Evaluates the 6DOF gravitational and buoyancy restoring vector G(q) 200 : * 201 : * @param q [in] Vehicle orientation as a unit quaternion 202 : * 203 : * @return 6-element restoring force/torque vector in N and Nm 204 : */ 205 1 : Vector6d getG(const Quaterniond& q) const; 206 : 207 : /** 208 : * @brief Evaluates the 6DOF gravitational and buoyancy restoring vector G(rpy) 209 : * 210 : * @param rpy [in] Vehicle orientation as roll-pitch-yaw in radians 211 : * 212 : * @return 6-element restoring force/torque vector in N and Nm 213 : */ 214 1 : Vector6d getG(const Vector3d& rpy) const; 215 : 216 : /** 217 : * @brief Computes the inertial + Coriolis + damping force M*a + C(v)*v + D(v)*v 218 : * 219 : * Does NOT include the gravitational/buoyancy restoring term G(q); that is 220 : * handled separately by BuoyancyController. Subclasses may override this to 221 : * zero out DOFs that are not actuated (e.g. Z, Roll, Pitch for a 3DOF vehicle). 222 : * 223 : * @param velocity [in] 6DOF body-frame velocity 224 : * @param acceleration [in] 6DOF body-frame acceleration 225 : * @param orientation [in] Vehicle orientation as a unit quaternion (unused in base implementation) 226 : * 227 : * @return 6-element force/torque vector in N and Nm 228 : */ 229 1 : virtual Vector6d getInverseDynamics(const Vector6d& velocity, const Vector6d& acceleration, 230 : const Quaterniond& orientation) const; 231 : 232 : /* ------------------------------------------------------------------ */ 233 : /* Mutators (maintain M_ = Mrb_ + Ma_ invariant) */ 234 : /* ------------------------------------------------------------------ */ 235 : 236 : /** 237 : * @brief Sets vehicle mass and recomputes weight, buoyancy, and the rigid-body mass matrix 238 : * 239 : * The existing buoyancy-to-weight ratio is preserved when mass changes. 240 : * 241 : * @param m [in] Vehicle mass in kg 242 : */ 243 1 : void setMass(double m); 244 : 245 : /** 246 : * @brief Sets the buoyancy force as a scale factor relative to vehicle weight 247 : * 248 : * @param factor [in] Buoyancy scale factor (1.0 = neutrally buoyant) 249 : */ 250 1 : void setBuoyancy(double factor); 251 : 252 : /** 253 : * @brief Sets the rigid-body inertia tensor 254 : * 255 : * @param I [in] Array of 6 inertia components [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] 256 : */ 257 1 : void setI(const std::array<double, 6>& I); 258 : 259 : /** 260 : * @brief Sets the diagonal added-mass coefficients 261 : * 262 : * @param m [in] Array of 6 diagonal added-mass values [Ma_x, Ma_y, Ma_z, Ma_p, Ma_q, Ma_r] 263 : */ 264 1 : void setMa(const std::array<double, 6>& m); 265 : 266 : /** 267 : * @brief Sets the diagonal linear damping coefficients 268 : * 269 : * @param d [in] Array of 6 diagonal linear damping values [Dl_x, ..., Dl_r] 270 : */ 271 1 : void setDl(const std::array<double, 6>& d); 272 : 273 : /** 274 : * @brief Sets the diagonal quadratic (nonlinear) damping coefficients 275 : * 276 : * @param d [in] Array of 6 diagonal quadratic damping values [Dnl_x, ..., Dnl_r] 277 : */ 278 1 : void setDnl(const std::array<double, 6>& d); 279 : 280 : /** 281 : * @brief Sets the centre-of-buoyancy position in the body frame 282 : * 283 : * @param rbb [in] Array of 3 coordinates [x, y, z] in metres 284 : */ 285 1 : void setRbb(const std::array<double, 3>& rbb); 286 : 287 : /** 288 : * @brief Sets the centre-of-gravity position in the body frame 289 : * 290 : * @param rgb [in] Array of 3 coordinates [x, y, z] in metres 291 : */ 292 1 : void setRgb(const std::array<double, 3>& rgb); 293 : }; 294 : 295 : #endif // CONTROLS_DYNAMICS_SYSTEM_DYNAMICS_BASE_H
