#include <RBModel.hpp>
Public Types | |
enum | { x = 0, y, z, phi, theta, psi } |
enum | { u = 0, v, w, p, q, r } |
enum | { X = 0, Y, Z, K, M, N } |
Public Member Functions | |
void | alloc_step (const vector &tauIn, vector &tauAch) |
const vector & | Eta () const |
const vector & | EtaNoisy () const |
double | getPressure (double h) const |
void | init () |
const vector & | Nu () const |
const vector & | NuAcc () const |
const vector & | NuNoisy () const |
RBModel () | |
void | reset () |
void | setNuAndEta (const vector &nu, const vector &eta) |
void | step (const vector &tau) |
~RBModel () | |
Public Attributes | |
double | ae |
labust::vehicles::ThrustAllocator | allocator |
double | be |
double | ce |
vector3 | current |
double | dT |
vector | eta0 |
bool | isCoupled |
labust::simulation::NoiseModel | noise |
vector | nu0 |
double | waterLevel |
Protected Member Functions | |
void | calculate_mrb () |
void | coriolis () |
void | restoring_force (const matrix3 &J1) |
Protected Attributes | |
double | B |
vector | eta |
vector | etaN |
vector | g |
vector | nu |
vector | nuacc |
vector | nuN |
This class implements a 6DOF marine vehicle model. This model is based on the equations of motion derived in chapter 2 of Guidance and control of Ocean Vehicles by Fossen (1994).
Definition at line 56 of file RBModel.hpp.
anonymous enum |
Definition at line 59 of file RBModel.hpp.
anonymous enum |
Definition at line 60 of file RBModel.hpp.
anonymous enum |
Definition at line 61 of file RBModel.hpp.
RBModel::RBModel | ( | ) |
Default constructor for common use without configuration.
Definition at line 42 of file RBModel.cpp.
Generic destructor.
Definition at line 56 of file RBModel.cpp.
void labust::simulation::RBModel::alloc_step | ( | const vector & | tauIn, |
vector & | tauAch | ||
) | [inline] |
The method the additional allocation step and generates a new tau.
Definition at line 82 of file RBModel.hpp.
void RBModel::calculate_mrb | ( | ) | [protected] |
The method calculates the rigid-body mass matrix of the model.
Definition at line 58 of file RBModel.cpp.
void RBModel::coriolis | ( | ) | [protected] |
The method calculates the Coriolis matrix of the model.
Definition at line 121 of file RBModel.cpp.
const vector& labust::simulation::RBModel::Eta | ( | ) | const [inline] |
Method to get the current model states. This vector returns the model position and orientation in world coordinates.
Definition at line 94 of file RBModel.hpp.
const vector& labust::simulation::RBModel::EtaNoisy | ( | ) | const [inline] |
Method to get the current model states. This vector returns the model position and orientation in world coordinates.
Definition at line 115 of file RBModel.hpp.
double labust::simulation::RBModel::getPressure | ( | double | h | ) | const [inline] |
Returns the pressure based on depth.
Definition at line 138 of file RBModel.hpp.
void labust::simulation::RBModel::init | ( | ) | [inline] |
Initialize the model.
Definition at line 143 of file RBModel.hpp.
const vector& labust::simulation::RBModel::Nu | ( | ) | const [inline] |
Method to get the current model states. This vector returns the model linear and rotational speeds in the body-fixed coordinate frame.
Definition at line 101 of file RBModel.hpp.
const vector& labust::simulation::RBModel::NuAcc | ( | ) | const [inline] |
Method to get the current model acceleration. This vector returns the model linear and rotational accelerations in the body-fixed coordinate frame.
Definition at line 108 of file RBModel.hpp.
const vector& labust::simulation::RBModel::NuNoisy | ( | ) | const [inline] |
Method to get the current model states. This vector returns the model linear and rotational speeds in the body-fixed coordinate frame.
Definition at line 122 of file RBModel.hpp.
void labust::simulation::RBModel::reset | ( | ) | [inline] |
The method restarts the model to initial parameters.
Definition at line 153 of file RBModel.hpp.
void RBModel::restoring_force | ( | const matrix3 & | J1 | ) | [protected] |
The method calculates the restoring forces.
Definition at line 142 of file RBModel.cpp.
void labust::simulation::RBModel::setNuAndEta | ( | const vector & | nu, |
const vector & | eta | ||
) | [inline] |
The method sets the initial states of the model.
nu | Linear and rotational velocities in the body-fixed coordinate frame. |
eta | Position and orientation in the earth-fixed coordinate frame. |
Definition at line 130 of file RBModel.hpp.
void RBModel::step | ( | const vector & | tau | ) |
The method performs one simulation step. The model is propagated in time for one sampling period. To access states after the propagation use the accessor methods.
tau | Vector of forces and moments acting on the model. |
Definition at line 67 of file RBModel.cpp.
The bounding ellipsoid parameters.
Definition at line 171 of file RBModel.hpp.
The thrust allocator.
Definition at line 193 of file RBModel.hpp.
double labust::simulation::RBModel::B [protected] |
The calculated buoyancy.
Definition at line 212 of file RBModel.hpp.
Definition at line 171 of file RBModel.hpp.
Definition at line 171 of file RBModel.hpp.
The external current disturbance vector.
Definition at line 179 of file RBModel.hpp.
The sampling step.
Definition at line 158 of file RBModel.hpp.
vector labust::simulation::RBModel::eta [protected] |
Position and orientation and their initial state.
Definition at line 220 of file RBModel.hpp.
Definition at line 184 of file RBModel.hpp.
vector labust::simulation::RBModel::etaN [mutable, protected] |
The noisy measurements.
Definition at line 224 of file RBModel.hpp.
vector labust::simulation::RBModel::g [protected] |
The restoring forces vector.
Definition at line 228 of file RBModel.hpp.
Coupled dynamics flag.
Definition at line 167 of file RBModel.hpp.
The noise generator.
Definition at line 188 of file RBModel.hpp.
vector labust::simulation::RBModel::nu [protected] |
Linear and orientation velocity and their initial state.
Definition at line 216 of file RBModel.hpp.
The initial speeds and position vector.
Definition at line 184 of file RBModel.hpp.
vector labust::simulation::RBModel::nuacc [protected] |
Definition at line 216 of file RBModel.hpp.
vector labust::simulation::RBModel::nuN [mutable, protected] |
Definition at line 224 of file RBModel.hpp.
The current water-level for simple wave simulation.
Definition at line 175 of file RBModel.hpp.