Class FishSchool¶

Defined in File FishSchool.hpp

Inheritance Relationships¶

Base Type¶

public IAlgorithm

Class Documentation¶

class mimir::algorithm::FishSchool : public IAlgorithm ¶

Kinematic fish school simulation model.

This class implements a simple kinematic fish school, which can be controlled by its inputs. The fish school is modelled as a 5 degrees of freedom kinematic body. It has a three dimensional position represented in an inertial reference frame, {NED}, with axes pointing North, East, and Down, respectively. The body can rotate about its y-axis – pitch: \(\theta\), and z-axis – yaw: \(\psi\). The fish school is modeled as an under-actuated entity, meaning that it can only change forward speed (surge) and its angular velocities. The input to the simulation model are surge \(u\), rate of turn \(r\), and desired depth \(D_d\). The desired depth is achieved with a first order response using pitch angular velocity as manipulated variable. The commanded surge and rate of turn is immidiate, i.e. there is no dynamic response. Let us define some quantities to describe the fish school with an ordinary differential equation.

\begin{align} \boldsymbol{p} = \begin{bmatrix}N\\E\\D\\\end{bmatrix} &\in \mathbb{R}^3 &\text{position}\\ \boldsymbol{\Theta} = \begin{bmatrix}\theta\\\psi\end{bmatrix} &\in \mathbb{S}^2 &\text{attitude}\\ \boldsymbol{v} = \begin{bmatrix}u\\v\\w\end{bmatrix} &\in \mathbb{R}^3 &\text{linear velocity}\\ \boldsymbol{w} = \begin{bmatrix}p\\q\\r\\\end{bmatrix} &\in \mathbb{R}^3 &\text{angular velocity}\\ D_d &\in \mathbb{R} &\text{desired depth} \\ \boldsymbol{\eta}(t) = \begin{bmatrix}\boldsymbol{p}\\\Theta\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^3 \times \mathbb{S}^2&\text{state vector} \end{align}

Let \(R_y(\theta), R_z(\psi) \in SO(3)\) be rotation matrixes, so that \(R(\boldsymbol{\eta})\) is the chained rotation defined as follows:

\begin{align*} R_y(\theta) &= \left[\begin{array}{ccc}\cos(\theta)&0 &\sin(\theta)\\0&1&0\\-\sin(\theta)&0&\cos(\theta) \end{array} \right] \\ R_z(\psi) &= \left[\begin{array}{ccc}\cos(\psi)&-\sin(\psi)&0\\\sin(\psi)&\cos(\psi)&0\\0&0&1 \end{array} \right]\\ R(\boldsymbol{\eta}) &= R_z(\psi)R_y(\theta) \\ \end{align*}

We then use the angular velocity transformation \(T(\theta)\) as defined in [10] and the combined state vector transformation \(J(\boldsymbol{\eta})\) for our 5-dimensional system becomes:

\begin{align*} T(\theta) &= \left[\begin{array}{cc}1&0\\0& \frac{1}{\cos(\theta)}\end{array} \right] \\ J(\boldsymbol{\eta}) &= \left[\begin{array}{cc}R(\boldsymbol{\eta})&0\\0& T(\theta)\end{array} \right] \\ \end{align*}

The depth response has a proportional feedback on depth error and a stabilizing term to level out the pitch: \(q_d = 0.1(D - D_d)\cos(2\theta) - \theta\). The resulting ordinary differential equation and input vector are, respectively,

\begin{align*} \dat \eta(t) &= J(\boldsymbol{\eta}) \begin{bmatrix}v\\q_d\\q\end{bmatrix} \\ u(t_d) &= \begin{bmatrix}v_d\\r_d\\Z_d\end{bmatrix}, \end{align*}

where \(t_d\) indicates discrete time points indicated by time_step_ms in the YAML config.

The table below describes the inputs, outputs and parameters of the algorithm. These variables are specified using the YAML configuration file defined further below.

Name	Symbol	Description	Causality	Variability	Default	Unit
`fish_ctrl`	\(u\)	Desired: Surge, rate of turn, depth	`input`	`discrete`		[m/s, rad/s, m]
`kinematics`	\(y\)	`fkin::Kinematics6D`	`output`	`continuous`
`position`	\(p_{p,0}\)	Initial; North, East, Down	`parameter`	`fixed`	\([0, 100, 50]\)	m
`euler`	\([\theta_0, \psi_0]\)	Initial; Pitch, Yaw	`parameter`	`fixed`	\([0,0]\)	rad

Warning

This numerical model is DEPRECATED in favor of a more detailed model with stochastic behavior defined using FMI [33]. Nevertheless, it may serve as an example on how to implement a simple mimir::IAlgorithm.

Public Functions

explicit FishSchool(const YAML::Node &config, boost::statechart::fifo_scheduler<> &scheduler, boost::statechart::fifo_scheduler<>::processor_handle machine, dds::pub::Publisher publisher, dds::sub::Subscriber subscriber)¶

FishSchool constructor.

The constructor parses the specification from the given YAML node. It establishes data structures and sets up DDS readers and writers according to the input/output scheme of the algorithm. The following YAML code block shows the expected layout of the FishSchool map of a input config file. The inputs, outputs, and initial conditions are communicated with DDS communication. Common is their DDS topic and DDS identifier. The specification is deemed self-explanatory.

time_step_ms: 200
inputs:
  fish_ctrl:                # DDS type: fkin::IdVec3d
    topic: fkinFishCtrl
    id: Fish
    max_age_ms: -1
outputs:                    # DDS type: fkin::Kinematics6D
  kinematics:
    topic: fkinKinematics6D
    id: Fish
initial_conditions:
  position:                 # DDS type: fkin::IdVec3d
    topic: fkinPosition
    id: FishInit
    max_wait_ms: 80
    fallback: [0, 100, 50]
  euler:                    # DDS type: fkin::IdVec3d
    topic: fkinEuler
    id: FishInit
    max_wait_ms: 80
    fallback: [0, 0, 0]

Parameters

config – [in] YAML configuration from input file.
scheduler – [in] State machine scheduler, needed to post events to state machine.
machine – [in] State machine processor handle, needed to post events to state machine.
publisher – DDS data writer to send data.
subscriber – DDS data reader to receive data.

virtual ~FishSchool()¶: Destructor.

virtual void solve(const std::atomic<bool> &cancel_token)¶

See base class for details.

The algorithm uses CVODES from SUNDIALS [17].

virtual void initialize(const std::atomic<bool> &cancel_token)¶: See base class.

virtual void timer(const std::atomic<bool> &cancel_token)¶: See base class.

inline virtual const char *name()¶: Name identifier of algorithm.

void event(boost::statechart::event_base *const event)¶: Helper function that queues event to state machine.

Private Functions

inline void step_time()¶: March simulation time one time step ahead.

FishSchool() = delete¶

Private Members

std::unique_ptr<Impl> m_impl¶: Holds the implementation of the algorithm.

boost::statechart::fifo_scheduler &m_scheduler¶: Members for state machine.

boost::statechart::fifo_scheduler::processor_handle m_stateMachine¶: Member for state machine.

const std::chrono::milliseconds m_time_step¶: Discrete time step.

std::chrono::steady_clock::time_point m_next_step¶: Simulation time point.

const YAML::Node m_config¶: YAML configuration for algorithm.