Fossen Dynamics

Used for attaching Fossen dynamics to a HoloOcean agent.

Classes:

FossenDynamics(vehicle[, initial_u_actual])

Class for handling the connection between a HoloOcean agent and the Fossen model to control the motion of the vehicle.
class holoocean.fossen_dynamics.dynamics.FossenDynamics(vehicle, initial_u_actual=None)

Class for handling the connection between a HoloOcean agent and the Fossen model to control the motion of the vehicle.

Methods:

R2euler(R)

Computes the Euler angles from the rotation matrix R.

convert_accel_NED_to_NWU(x, accel)

Calculates rotation to world frame and changes the coordinate system from NED to NWU Rotates linear and angular accelaration from body to world

convert_state_NWU_to_NED(x)

Converts position and velocity from NWU to NED and world frame velocities to body frame

set_u_control_rad(u_control)

Set the control surface commands for the vehicle control surfaces with angles in radians

update(state)

Update state of the vehicle to return the acceleration of the vehicle in holoocean world frame
R2euler(R)

Computes the Euler angles from the rotation matrix R.

Parameters:

R (numpy.ndarray) – 3x3 rotation matrix

Returns:

Euler angles (in radians)

Return type:

phi, theta, psi (float)

convert_accel_NED_to_NWU(x, accel)

Calculates rotation to world frame and changes the coordinate system from NED to NWU Rotates linear and angular accelaration from body to world

Parameters:

  • x – Holoocean Dynamic Sensor return of position (with quaternion), velocity, and acceleration (len = 19)

  • accel (np.ndarray) –

Returns:

[accelx, accely, accelz, pdot, qdot, rdot] - World frame in NWU

Return type:

accel (np.ndarray - float)

convert_state_NWU_to_NED(x)

Converts position and velocity from NWU to NED and world frame velocities to body frame

Parameters:

x – Holoocean Dynamic Sensor return of position (with quaternion), velocity, and acceleration (len = 19)

Returns:

[x, y, z, roll, pitch, yaw], [xdot, ydot, zdot, p, q, r]

Return type:

eta, nu (np.ndarray - float)

set_u_control_rad(u_control)

Set the control surface commands for the vehicle control surfaces with angles in radians

update(state)

Update state of the vehicle to return the acceleration of the vehicle in holoocean world frame

Torpedo Dynamics

Classes:

TAUV([scenario, vehicle_name, ...])

Parent class of the torpedo-shaped vehicle.

fourFinDep([scenario, vehicle_name, ...])

Torpedo Vehicle with four fins where two fins move together on same plane (Rudder, Stern)

fourFinInd([scenario, vehicle_name, ...])

Torpedo vehicle with four independetly controlled fins (Rudder Top, Rudder Bottom, Stern left, Stern Right)

threeFinInd([scenario, vehicle_name, ...])

Torpedo vehicle with four independently controlled fins (Rudder Top, Rudder Bottom, Stern left, Stern Right)
class holoocean.fossen_dynamics.torpedo.TAUV(scenario=None, vehicle_name=None, controlSystem='stepInput', r_z=0, r_psi=0, r_rpm=0, r_surge=0, V_current=0, beta_current=0)

Parent class of the torpedo-shaped vehicle. General parameters and calculations for all torpedo vehicles. Actuator parameters and calculations are implemented in subclasses.

Parameters:

  • scenario (dict) – scenario dictionary for holoocean

  • vehicle_name (str) – name of vehicle to initialize that matches agent in scenario dictionary

  • controlSystem (str) – autopilot method for controlling the actuators

  • r_z (float) – desired depth (m), positive downwards

  • r_psi (float) – desired yaw angle (deg)

  • r_rpm (float) – desired propeller revolution (rpm)

  • r_rpm – desired surge speed (m/s)

  • V_current (float) – current speed (m/s)

  • beta_current (float) – current direction (deg)

Methods:

actuator_dynamics(u_control)

Vehicle-specific calculations for dynamics of the actuators (fins and thruster).

calculate_additional_parameters()

After updating the vehicle parameters, calculations will be run to update other variables related to these parameters.

configure_from_scenario(scenario, vehicle_name)

Dynamics Parameters:

depthHeadingAutopilot(eta, nu[, imu])

Simultaneously control the heading and depth of the AUV using control laws of PID type.

dynamics(eta, nu, u_control)

Calculates vehicle and actuator accelerations based on the current state and control inputs.

fin_forces(nu_r, U)

Vehicle-specific calculations for forces of the actuators (fins and thruster).

set_actuator_parameters(actuator_parameters)

Set fin area limits, time constants, and lift coefficients for control surfaces.

set_autopilot_parameters(autopilot)

Set depth and heading parameters from a configuration dictionary.

set_control_mode(controlSystem[, init_depth])

Sets the control mode for the vehicle.

set_depth_goal(depth)

Set the depth goals for the autopilot.

set_goal([depth, heading, rpm, surge])

Set the goals for the autopilot.

set_heading_goal(heading)

Set the heading goals for the autopilot.

set_rpm_goal(rpm)

Set the rpm goals for the autopilot.

set_surge_goal(surge)

Set the surge goals for the autopilot.

set_vehicle_parameters(dynamics)

Set vehicle dynamics parameters.

stepInput(t)

Generates a pre-defined step input to the actuators for testing purposes.
actuator_dynamics(u_control)

Vehicle-specific calculations for dynamics of the actuators (fins and thruster).

Note: For Torpedo Vehicles, positive fin deflection will pitch the vehicle up and yaw to the starboard side.

Parameters:

  • self.u_actual (array-like) – Current control surface position.

  • nu_r (array-like) – Reference velocity of the vehicle in the body frame.

Returns:

array of actuator velocities/accelerations

calculate_additional_parameters()

After updating the vehicle parameters, calculations will be run to update other variables related to these parameters.

configure_from_scenario(scenario, vehicle_name)

Dynamics Parameters:

  • Environment Parameters

Parameters:

  • rho (float) – Density of water in kg/m^3

  • sampleTime (float) – Sample time for the dynamics manager (s)

  • Vehicle Physical Parameters

Parameters:

  • mass (float) – Mass of vehicle in kilograms

  • length (float) – Length of vehicle in meters

  • diam (float) – Diameter of vehicle in m

  • r_bg (float) – Center of gravity of the vehicle (x, y, z) in body frame x forward, y right, z down

  • r_bb (float) – Center of boyancy of the vehicle (x, y, z) in body frame x forward, y right, z down

  • area_fraction (float) – relates vehicle effective area to length and width. pi/4 for a spheroid

  • Low-Speed Linear Damping Matrix Parameters:

Parameters:

  • T_surge (float) – Time constant in surge (s)

  • T_sway (float) – Time constant in sway (s)

  • T_yaw (float) – Time constant in yaw (s)

  • zeta_roll (float) – Relative damping ratio in roll

  • zeta_ptich (float) – Relative damping ratio in pitch

  • K_nomoto (float) –

  • Other Damping Parameters

Parameters:

  • r44 (float) – Added moment of inertia in roll: A44 = r44 * Ix

  • Cd (float) – Coefficient of drag

  • e (float) – Oswald efficiency factor for vehicle drag

Autopilot Paramters:

  • Depth

Parameters:

  • wn_d_z (float) – Damped natural frequency for low pass filter for depth commands

  • Kp_z (float) – Portional gain for depth controller

  • T_z (float) –

  • Kp_theta (float) – Porportional gain for pitch angle for depth controller

  • Ki_theta (float) – Integral gain for pitch angle for depth controller

  • Kd_theta (float) – Derivative gain for pitch angle for depth controller

  • K_w (float) –

  • theta_max_deg (float) – Max output of pitch controller inner loop

  • Heading

Parameters:

  • wn_d (float) – Damped natural frequency of input commands for low pass filter

  • zeta_d – Damping coefficient

  • r_max – (?) Maximum yaw rate

  • lam –

  • phi_b –

  • K_d – (?) Derivative gain

  • K_sigma – (?) SMC switching gain

  • Surge

Parameters:

  • kp_surge – Porportional gain for surge

  • ki_surge – Integral gain for surge

  • kd_surge – Derivative gain for surge

Actuator parameters:

  • Fins

Parameters:

  • fin_area – Surface area of one side of a fin

  • x_fin – Positive x distance from center of mass to actuation distance

  • z_fin – Positive Z distance from center of mass to fin center of pressure

  • CL_delta_r – Coefficient of lift for rudder

  • CL_delta_s – Coefficient of lift for stern

  • deltaMax_fin_deg – Max deflection of the fin (degrees)

  • fin_speed_max – Max angular speed of the fin (rad/s)

  • T_delta – Time constant for fin actuation. (s)

  • Propellor

Parameters:

  • D_prop – Propeller diameter

  • t_prop – Propeller pitch

  • KT_0 – Thrust coefficient at zero rpm

  • KQ_0 – Torque coefficient at zero rpm

  • KT_max – Max thrust coefficient

  • KQ_max – Max torque coefficient

  • w – Wake fraction number

  • Ja_max – Max advance ratio

  • nMax – Max rpm of the thruster

  • T_n – Time constant for thruster actuation. (s)

depthHeadingAutopilot(eta, nu, imu=True)

Simultaneously control the heading and depth of the AUV using control laws of PID type. Propeller rpm is given as a step command.

Parameters:

  • eta (array-like) – State/pose of the vehicle in the world frame. (RPY - Euler angle order zyx in radians)

  • nu (array-like) – Velocity of the vehicle in the body frame.

Returns:

The control input u_control.

dynamics(eta, nu, u_control)

Calculates vehicle and actuator accelerations based on the current state and control inputs. Acelerations are integrated in the step function to update the state.

Inputs: :param array-like eta: Current state/pose of the vehicle in the world frame. :param array-like nu: Current velocity of the vehicle in the body frame. :param array-like u_control: Commanded control surface position.

Outputs: :param array-like nu_dot: Acceleration of the vehicle in the body frame. :param array-like u_actual_dot: Acceleration of the control surfaces.

Returns:

Two arrays: nu_dot, u_actual_dot.

fin_forces(nu_r, U)

Vehicle-specific calculations for forces of the actuators (fins and thruster). Placeholder funtion, to be overridden by subclasses.

NOTE: - For rudder, positive command turns the rudder CCW. Yaws vehicle right, negative roll. - For left stern, positive command turns the stern CCW. Pitches vehicle up in coordination, negative roll. - For right stern, positive command turns the stern CW. Pitches vehicle up in coordination, positive roll.

Parameters:

  • self.u_actual (array-like) – Current control surface position.

  • nu_r (array-like) – Reference velocity of the vehicle in the body frame.

  • U (float) – forward speed of the vehicle

Returns:

(6,) numpy array of forces and moments

set_actuator_parameters(actuator_parameters)

Set fin area limits, time constants, and lift coefficients for control surfaces. Placeholder funtion, to be overridden by subclasses.

set_autopilot_parameters(autopilot)

Set depth and heading parameters from a configuration dictionary.

Parameters:

cfg – Dictionary containing ‘depth’ and ‘heading’ sections with their respective parameters.

set_control_mode(controlSystem, init_depth=False)

Sets the control mode for the vehicle.

Parameters:

controlSystem (str) – The control system to use. Possible values are:

  • "depthHeadingAutopilot": Depth and heading autopilots.

  • "manualControl": Manual input control with set_u_control().

  • "stepInput": Step inputs for stern planes, rudder, and propeller

  • Any other value: controlSystem is set to “stepInput”.

Parameters:

init_depth (bool) – Whether to initialize depth (default is False).

Returns:

None

set_depth_goal(depth)

Set the depth goals for the autopilot.

Parameters:

depth (float) – Desired depth (m), positive downward in world frame.

Returns:

None

set_goal(depth=None, heading=None, rpm=None, surge=None)

Set the goals for the autopilot.

Parameters:

  • depth (float) – Desired depth (m), positive downwards.

  • heading (float) – Desired yaw angle (deg). (-180 to 180)

  • rpm (float) – Desired propeller revolution (rpm).

  • surge (float) – Desired body frame x velocity (m/s).

Returns:

None

set_heading_goal(heading)

Set the heading goals for the autopilot.

Parameters:

depth (float) – Desired heading (deg), -180 to 180 in NED frame

Returns:

None

set_rpm_goal(rpm)

Set the rpm goals for the autopilot.

Parameters:

depth (float) – Desired rpm for thruster

Returns:

None

set_surge_goal(surge)

Set the surge goals for the autopilot.

Parameters:

depth (float) – Desired surge (m/s), positive forward in body frame.

Returns:

None

set_vehicle_parameters(dynamics)

Set vehicle dynamics parameters. If not provided, will default to previous value

stepInput(t)

Generates a pre-defined step input to the actuators for testing purposes. Placeholder funtion, to be overridden by subclasses.

Parameters:

t (float) – Time parameter.

Returns:

The control input u_control.

class holoocean.fossen_dynamics.torpedo.fourFinDep(scenario=None, vehicle_name=None, controlSystem='stepInput', r_z=0, r_psi=0, r_rpm=0, V_current=0, beta_current=0)

Torpedo Vehicle with four fins where two fins move together on same plane (Rudder, Stern)

Methods:

depthHeadingAutopilot(eta, nu[, imu])

returns:

The control input u_control as a list: [delta_r, delta_s, n], where:

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster).

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.
depthHeadingAutopilot(eta, nu, imu=True)
Returns:

The control input u_control as a list: [delta_r, delta_s, n], where:

  • delta_r (float): Rudder angle (rad)

  • delta_s (float): Stern plane angle (rad)

  • n (float): Propeller revolution (rpm)

Return type:

list

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster).

Parameters:

  • self.u_actual (array-like) – Current control surface position.

  • nu_r (array-like) – Reference velocity of the vehicle in the body frame.

  • U (float) – forward speed of the vehicle

Returns:

(6,) numpy array of forces and moments

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.

Returns:

The control input u_control as a list: [delta_r, delta_s, n], where:

  • delta_r (float): Rudder angle (rad)

  • delta_s (float): Stern plane angle (rad)

  • n (float): Propeller revolution (rpm)

Return type:

list

class holoocean.fossen_dynamics.torpedo.fourFinInd(scenario=None, vehicle_name=None, controlSystem='stepInput', r_z=0, r_psi=0, r_rpm=0, V_current=0, beta_current=0)

Torpedo vehicle with four independetly controlled fins (Rudder Top, Rudder Bottom, Stern left, Stern Right)

Methods:

depthHeadingAutopilot(eta, nu[, imu])

returns:

The control input u_control as a list: [delta_rt, delta_rb, delta_sl, delta_sr, n], where:

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster).

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.
depthHeadingAutopilot(eta, nu, imu=True)
Returns:

The control input u_control as a list: [delta_rt, delta_rb, delta_sl, delta_sr, n], where:

  • delta_rt: Rudder top angle (rad).

  • delta_rb: Rudder bottom angle (rad).

  • delta_sl: Stern left angle (rad).

  • delta_sr: Stern right angle (rad).

  • n: Propeller revolution (rpm).

Return type:

list

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster). The implementation below is for a three-fin vehicle. NOTE: - For rudder, positive command turns the rudder CCW. Yaws vehicle starboard, negative roll. - For left stern, positive command turns the stern CCW. Pitches vehicle up in coordination, negative roll. - For right stern, positive command turns the stern CW. Pitches vehicle up in coordination, positive roll.

Parameters:

  • nu_r (array-like) – Reference velocity of the vehicle in the body frame.

  • U (float) – forward speed of the vehicle

Returns:

(6,) numpy array of forces and moments

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.

Returns:

The control input u_control as a list: [delta_rt, delta_rb, delta_sl, delta_sr, n], where:

  • delta_rt: Rudder top angle (rad).

  • delta_rb: Rudder bottom angle (rad).

  • delta_sl: Stern left angle (rad).

  • delta_sr: Stern right angle (rad).

  • n: Propeller revolution (rpm).

Return type:

list

class holoocean.fossen_dynamics.torpedo.threeFinInd(scenario=None, vehicle_name=None, controlSystem='stepInput', r_z=0, r_psi=0, r_rpm=0, V_current=0, beta_current=0)

Torpedo vehicle with four independently controlled fins (Rudder Top, Rudder Bottom, Stern left, Stern Right)

Methods:

depthHeadingAutopilot(eta, nu[, imu])

returns:

[delta_r, delta_rb, delta_sl, delta_sr, n], where:

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster).

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.
depthHeadingAutopilot(eta, nu, imu=True)
Returns:

[delta_r, delta_rb, delta_sl, delta_sr, n], where:

  • delta_r: Rudder top angle (rad).

  • delta_re: right stern angle (rad).

  • delta_le: left stern angle (rad).

  • n: Propeller revolution (rpm).

Return type:

list

fin_forces(nu_r)

Vehicle-specific calculations for forces of the actuators (fins and thruster). The implementation below is for a three-fin vehicle. NOTE: - For rudder, positive command turns the rudder CCW. Yaws vehicle starboard, negative roll. - For left stern, positive command turns the stern CCW. Pitches vehicle up in coordination, negative roll. - For right stern, positive command turns the stern CW. Pitches vehicle up in coordination, positive roll.

Parameters:

  • nu_r (array-like) – Reference velocity of the vehicle in the body frame.

  • U (float) – forward speed of the vehicle

Returns:

(6,) numpy array of forces and moments

stepInput(t)

A pre-defined step input to the vehicle controls for testing purposes.

Returns:

[delta_r, delta_rb, delta_sl, delta_sr, n], where:

  • delta_r: Rudder top angle (rad).

  • delta_re: right stern angle (rad).

  • delta_le: left stern angle (rad).

  • n: Propeller revolution (rpm).

Return type:

list