Fossen Dynamics

Classes:

FossenInterface(vehicle_names, scenario[, ...])

Class for handling the connection between a holocean agent and the fossen dynamic model to control the motion of the vehicle

class holoocean.fossen_dynamics.fossen_interface.FossenInterface(vehicle_names, scenario, multi_agent=False)

Class for handling the connection between a holocean agent and the fossen dynamic model to control the motion of the vehicle

Parameters:
  • vehicle_names (list (str)) – list of name of vehicles to initialize that matches agent in scenario dictionary

  • scenario (dict) – scenario dictionary for holoocean

  • multi_agent (bool) – If there are multiple agents within the scenario

Methods:

get_control_mode(vehicle_name)

Getys the control mode for the vehicle.

get_u_control(vehicle_name)

Get the control surface positions for the vehicle (angles in radians)

set_control_mode(vehicle_name, controlSystem)

Sets the control mode for the vehicle.

set_depth_goal(vehicle_name, depth)

Set the depth goals for the autopilot.

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

Set the goals for the autopilot.

set_heading_goal(vehicle_name, heading)

Set the heading goals for the autopilot.

set_rpm_goal(vehicle_name, rpm)

Set the rpm goals for the torpedo autopilot.

set_u_control(vehicle_name, u_control)

Set the control surface commands for the vehicle (angles in radians)

update(vehicle_name, state)

Given the state of the vehicle and control inputs return the accelerations of the vehicle in holoocean global frame

get_control_mode(vehicle_name)

Getys the control mode for the vehicle.

Parameters:

name (str vehicle) – The vehicle control system is asked for

Returns:

str vehicle control mode

get_u_control(vehicle_name)

Get the control surface positions for the vehicle (angles in radians)

set_control_mode(vehicle_name, controlSystem)

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

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

Returns:

None

set_depth_goal(vehicle_name, depth)

Set the depth goals for the autopilot.

Parameters:

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

Returns:

None

set_goal(vehicle_name, depth=None, heading=None, rpm=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).

Returns:

None

set_heading_goal(vehicle_name, heading)

Set the heading goals for the autopilot.

Parameters:

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

Returns:

None

set_rpm_goal(vehicle_name, rpm)

Set the rpm goals for the torpedo autopilot.

Parameters:

depth (float) – Desired rpm for thruster

Returns:

None

set_u_control(vehicle_name, u_control)

Set the control surface commands for the vehicle (angles in radians)

update(vehicle_name, state)

Given the state of the vehicle and control inputs return the accelerations of the vehicle in holoocean global frame

Torpedo Dynamics

Classes:

Torpedo()

Rudder angle, stern plane and propeller revolution step inputs

Functions:

configure_torpedo_from_scenario(scenario, ...)

Dynamics Parameters:

set_torpedo_actuator_parameters(...)

Set fin area limits, time constants, and lift coefficients for control surfaces Create fin ojects attached to vehicle Fins will start on the starboard side then be placed equally distanced apart.

set_torpedo_autopilot_parameters(autopilot, ...)

Set depth and heading parameters from a configuration dictionary.

set_torpedo_parameters(dynamics, vehicle)

Set vehicle dynamics parameters.

class holoocean.fossen_dynamics.torpedo.Torpedo

Rudder angle, stern plane and propeller revolution step inputs

Torpedo(‘depthHeadingAutopilot’,z_d,psi_d,n_d,V_c,beta_c)

Depth and heading autopilots

Inputs:

z_d: desired depth, positive downwards (m) psi_d: desired heading angle (deg) n_d: desired propeller revolution (rpm) V_c: current speed (m/s) beta_c: current direction (deg)

Parameters:
  • 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)

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

  • beta_current (float) – water current direction (deg)

Methods:

depthHeadingAutopilot(eta, nu, sampleTime)

[delta_r, delta_s, n] = depthHeadingAutopilot(eta,nu,sampleTime) simultaneously control the heading and depth of the AUV using control laws of PID type.

dynamics(eta, nu, u_actual, u_control, ...)

[nu_dot,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the AUV equations of motion using Euler's method.

stepInput(t)

u_c = stepInput(t) generates step inputs.

depthHeadingAutopilot(eta, nu, sampleTime)

[delta_r, delta_s, n] = depthHeadingAutopilot(eta,nu,sampleTime) simultaneously control the heading and depth of the AUV using control laws of PID type. Propeller rpm is given as a step command.

Returns:

u_control = [ delta_r rudder angle (rad)

delta_s stern plane angle (rad) n propeller revolution (rpm) ]

dynamics(eta, nu, u_actual, u_control, sampleTime)

[nu_dot,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the AUV equations of motion using Euler’s method.

stepInput(t)

u_c = stepInput(t) generates step inputs.

Returns:

u_control = [ delta_r rudder angle (rad)

delta_s stern plane angle (rad) n propeller revolution (rpm) ]

holoocean.fossen_dynamics.torpedo.configure_torpedo_from_scenario(scenario, vehicle_name, vehicle)

Dynamics Parameters:

  • Environment Parameters

Parameters:

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

  • 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) – Time constant for depth controller

  • 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

  • wn_d_theta (float) – Damped natural frequency for low pass filter for pitch controller when only running inner loop

  • K_w (float) – Optional heave velocity feedback gain

  • 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

Actuator parameters:

  • Fins

Parameters:
  • fin_count – Number of equally spaced fins on the vehicle

  • fin_offset_deg – Angle offset of first fin around x axis (deg) starting from positive y with z down

  • fin_center – radius (m) from COP on the fin to the COM in the YZ plane

  • fin_area – Surface area of one side of a fin

  • x_fin – Body frame x distance (x forward) from center of mass to fin COP

  • CL_delta – Coefficient of lift for fin

  • 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:
  • nMax – Max rpm of the thruster

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

  • 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

holoocean.fossen_dynamics.torpedo.set_torpedo_actuator_parameters(actuator_parameters, vehicle)

Set fin area limits, time constants, and lift coefficients for control surfaces Create fin ojects attached to vehicle Fins will start on the starboard side then be placed equally distanced apart. Typical configurations will be: Fin Count - 4, Fin Offset - 0 Deg and Fin Count - 3, Fin Offset - 30 Deg

holoocean.fossen_dynamics.torpedo.set_torpedo_autopilot_parameters(autopilot, vehicle)

Set depth and heading parameters from a configuration dictionary.

Parameters:

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

holoocean.fossen_dynamics.torpedo.set_torpedo_parameters(dynamics, vehicle)

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

Otter Dynamics

Classes:

Otter([controlSystem, r, V_current, ...])

Otter() Propeller step inputs Otter('headingAutopilot',psi_d,V_c,beta_c,tau_X) Heading autopilot

class holoocean.fossen_dynamics.otter.Otter(controlSystem='stepInput', r=0, V_current=0, beta_current=0, tau_X=120)

Otter() Propeller step inputs Otter(‘headingAutopilot’,psi_d,V_c,beta_c,tau_X) Heading autopilot

Inputs:

psi_d: desired heading angle (deg) V_c: current speed (m/s) beta_c: current direction (deg) tau_X: surge force, pilot input (N)

Methods:

controlAllocation(tau_X, tau_N)

[n1, n2] = controlAllocation(tau_X, tau_N)

dynamics(eta, nu, u_actual, u_control, ...)

[nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the Otter USV equations of motion using Euler's method.

headingAutopilot(eta, nu, sampleTime)

u = headingAutopilot(eta,nu,sampleTime) is a PID controller for automatic heading control based on pole placement.

stepInput(t)

u = stepInput(t) generates propeller step inputs.

controlAllocation(tau_X, tau_N)

[n1, n2] = controlAllocation(tau_X, tau_N)

dynamics(eta, nu, u_actual, u_control, sampleTime)

[nu,u_actual] = dynamics(eta,nu,u_actual,u_control,sampleTime) integrates the Otter USV equations of motion using Euler’s method.

headingAutopilot(eta, nu, sampleTime)

u = headingAutopilot(eta,nu,sampleTime) is a PID controller for automatic heading control based on pole placement.

tau_N = (T/K) * a_d + (1/K) * rd
  • Kp * ( ssa( psi-psi_d ) + Td * (r - r_d) + (1/Ti) * z )

stepInput(t)

u = stepInput(t) generates propeller step inputs.