Fossen Dynamics
Classes:
|
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 inputsAny 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:
|
Rudder angle, stern plane and propeller revolution step inputs |
Functions:
|
Dynamics 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 depth and heading parameters from a configuration dictionary. |
|
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() 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.