Fossen-Based Dynamics

Dynamics for underwater and surface vehicles can be modeled using equations of motion derived in Thor Fossen’s Handbook of Marine Craft Hydrodynamics and Motion Control. Implementations of these dynamics are conveniently provided by Fossen at his GitHub repository. We have ported some of Fossen’s implementations into HoloOcean to enable accurate vehicle dynamics simulations.

Using Fossen-based dynamics in HoloOcean is an extension of the Custom Dynamics control scheme. It requires defining two objects in python when creating a simulation:

1. A vehicle controller of one of the classes described below. This implements the Fossen dynamics for that vehicle.

2. A dynamics manager of the class FossenDynamics. This handles the connection between the HoloOcean agent and the dynamics controller by associating the dynamics with a specific agent. It also handles coordinate frame changes from NED to NWU, as HoloOcean does not currently have a defined North or West direction world coordinate frame (see Coordinate System).

These classes are implemented in client/src/vehicle_dynamics. The dynamics manager is located in client/src/dynamics.py.

Note

Fossen dynamics does not currently support multiple agents. A multi-agent implementation is planned for a future release.

Fossen Vehicle Classes

Currently, we have implemented the dynamics models for one vehicle class and several child classes:

• Torpedo Vehicle Parent class (TAUV)

  • Three Independent Fins - Torpedo-shaped vehicle (threeFinInd)

  • Four Dependent Fins - Torpedo-shaped vehicle (fourFinDep)

  • Four Independent Fins - Torpedo-shaped vehicle (fourFinInd)

Default Fossen vehicle parameters are currently tuned for the REMUS100 vehicle provided by Thor Fossen.

Note

The Unreal Engine graphics asset is the same across all three classes of torpedo dynamics. The number of fins shown on the vehicle will not change in Unreal Engine when changing between child classes.

Implementing Fossen dynamics does not change the asset, so the fins will not visibly move when the simulation is running.

Note

Mass and other default vehicle parameters set in the HoloOcean engine are ignored when using the “Custom Dynamics” control scheme. The Fossen models account for these separately.

Adding Custom Fossen Dynamics

Users can add their own custom Fossen dynamics models for different types of vehicles by creating a new class.

Dynamics model classes must have the following methods and attributes:

• __init__: This method should initialize the vehicle parameters and set up the dynamics model.

• dynamics: This method should implement the dynamics equations for the vehicle. It should take in the current state of the vehicle (eta, nu) and the control input, and return the accelerations in the NWU frame.

The model can also implement other functions as desired, such as controllers and plotting functions. We recommend using a controlMode attribute to define the control scheme for the vehicle.

See the TAUV class for an example of how to implement a custom dynamics model.

Importing Models from PythonVehicleSimulator

If a user has developed a dynamics model class using Thor Fossen’s PythonVehicleSimulator and would like to use it in HoloOcean, they must make the following changes:

• The dynamics method must be changed to only take in the state of the vehicle (eta, nu) and control input vector as inputs and return only NWU accelerations.

• sampleTime must be made a class attribute instead of a passed-in parameter.

• u_actual must be made a class attribute tracked by the vehicle class instead of a passed-in parameter. The FossenDynamics manager relies on the vehicle class to track its own current actuator positions.

Be sure that all other methods and attributes are implemented as expected by the FossenDynamics manager.

Fossen Dynamics Example

In this section we will step through the key elements of our Fossen dynamics implementation and give an example of using Fossen dynamics in HoloOcean. The full code can be found in client/example.py.

Configuration

Setting up a HoloOcean simulation that uses Fossen dynamics begins with the scenario configuration. The scenario configuration is similar to other HoloOcean examples, but with some key differences:

1. When configuring the main agent, you must include the Dynamics Sensor in the list of sensors. The configuration block should include UseRPY: False (default is True; this forces the dynamics to use quaternions instead of Euler angles) and UseCOM: True (default is True).

2. The extra parameters dynamics, actuator, and autopilot` must be added to the agent configuration. Each of these is a dictionary that defines further vehicle parameters. See holoocean.fossen_dynamics.torpedo.configure_from_scenario() for definitions of each dynamics parameter.

Below is an example scenario configuration that fully defines all the parameters for the Fossen dynamics. A full description of each parameter can be found in the API documentation for the Fossen dynamics classes.

scenario = {
"name": "torpedo_dynamics",
"package_name": "Oceans",
"world": "SimpleUnderwater",
"main_agent": "auv0",
"ticks_per_sec": ticks_per_sec,
"agents": [
        {
        "agent_name": "auv0",
        "agent_type": "TorpedoAUV",
        "sensors": [
            {
                "sensor_type": "DynamicsSensor",
                "configuration": {
                    "UseCOM": True,
                    "UseRPY": False  # Use quaternion for dynamics
                }
            },
        ],
        "control_scheme": 1,  # Control scheme 1 is how custom dynamics are applied to TAUV
        "location": [0, 0, -10],  # Represented in NWU coordinate system
        "rotation": [0, 0, 0], # Roll, pitch, yaw in Euler angle order ZYX and in degrees NWU coordinate system
        "dynamics":
            {
                "sampleTime": 1/60,
                "rho": 1026,
                "mass": 16,
                "length": 1.6,
                "diam": 0.19,
                "r_bg": [0, 0, 0.02],
                "r_bb": [0, 0, 0],
                "r44": 0.3,
                "Cd": 0.42,
                "T_surge": 20,
                "T_sway": 20,
                "zeta_roll": 0.3,
                "zeta_pitch": 0.8,
                "T_yaw": 1,
                "K_nomoto": 0.25
            },
        "actuator":
            {
                "fin_area": 0.00665,
                "deltaMax_fin_deg": 15,
                "nMax": 1525,
                "T_delta": 0.1,
                "T_n": 0.1,
                "CL_delta_r": 0.5,
                "CL_delta_s": 0.7
            },
        "autopilot":
            {
                'depth': {
                    'wn_d_z': 0.2,
                    'Kp_z': 0.08,
                    'T_z': 100,
                    'Kp_theta': 4.0,
                    'Kd_theta': 2.3,
                    'Ki_theta': 0.3,
                    'K_w': 5.0,
                },
                'heading': {
                    'wn_d': 1.2,
                    'zeta_d': 0.8,
                    'r_max': 0.9,
                    'lam': 0.1,
                    'phi_b': 0.1,
                    'K_d': 0.5,
                    'K_sigma': 0.05,
                }
            },
        }
    ]
}

Simulation Setup

The setup for the simulation is similar to other HoloOcean Examples.

Simulation setup proceeds first by setting up an environment. We pass the vehicle parameters defined in the scenario configuration to a Fossen vehicle object, which specifies a target HoloOcean agent (e.g., ‘auv0’). Next we create a dynamics manager, passing the vehicle and simulation time period (1/ticks_per_sec), to attach the parameters to the agent. Finally we initialize a numpy array with a length of 6 for accelerations in x, y, z (global frame) and angular accelerations around the x, y, and z axes (global frame).

In this example we select the fourFinDep vehicle, which has 3 control inputs (Rudder Fins, Stern Fins, Thruster).

import holoocean
import holoocean.fossen_dynamics as fd

numSteps = 600   # The total simulation time can be calculated by (numSteps/ticks_per_sec).
period = 1.0/ticks_per_sec

scenario = {...} # See above for scenario configuration

env = holoocean.make(scenario_cfg=scenario)

fossen_vehicle = fd.torpedo.fourFinDep(env, 'auv0')
fossen_manager = fd.dynamics.FossenDynamics(fossen_vehicle)
accel = np.array(np.zeros(6), float)  # Initialize HoloOcean parameter input

Note

If you are running the simulation live with the Unreal Editor in Standalone mode, be sure to change the additional launch parameters as described in Getting Started. Add the launch parameter -TicksPerSec to match what is in the Python script for consistent timing.

Manual Control Example

To use the manual control method, set the vehicle object to “manualControl” mode. The fin angles can be passed directly to the vehicle dynamics, and an acceleration array will be returned to the HoloOcean agent given the state of the agent in the HoloOcean world.

The length of the u_control array is defined in the dynamic model class. It represents the number of command inputs.

• Fin angles should be given in radians. The example below shows how to convert fin angles from degrees to radians using numpy.

• A positive fin deflection of a rudder fin will result in a yaw moment to the starboard side.

• A positive fin deflection of a stern/elevator fin will result in a pitch moment to pitch the vehicle up.

A custom controller can be used with this manual control method to input specific fin commands.

fossen_vehicle.set_control_mode('manualControl')
fins_degrees = np.array([5, -5])  # Rudder and Stern Fin Deflection (degrees)
fin_radians = np.radians(fins_degrees)
thruster_rpm = 800 # not a percent: check the vehicle dynamics for the max RPM
u_control = np.append(fin_radians, thruster_rpm)  # [RudderAngle, SternAngle, Thruster]

To tick the environment, call the step function and pass it a list of accelerations. Next, send a command to the control surfaces with the set_u_control_rad function on the FossenDynamics object. It is not required to set the u_control every tick. If you want to change the control surfaces every tick, set the control command before updating the dynamics.

The FossenDynamics.update function takes in the state returned from HoloOcean and parses the data from the Dynamics Sensor. Given the state of the vehicle and control surface inputs, it calculates an output of accelerations in the HoloOcean frame (NWU).

for i in range(numSteps):
    state = env.step(accel)
    torpedo_dynamics.set_u_control_rad(u_control)  # If desired, you can change the control command here
    accel = torpedo_dynamics.update(state)  # Calculate accelerations to be applied to HoloOcean agent

    # For Plotting the state of the vehicle
    pos = state['DynamicsSensor'][6:9]  # [x, y, z]
    pos_list.append(pos)
    time_list.append(state['t'])

Depth and Heading Control Example

A common control strategy for underwater vehicles is to control depth and heading separately. This can be done by setting the control mode to depthHeadingAutopilot and setting the goal depth, heading, and thruster RPM.

The depth goal is defined in meters. A positive depth corresponds to a negative z location, with depth increasing the further the vehicle descends.

The heading goal is given in the global frame. It ranges from -180 to 180 degrees, with 0 degrees being north (along the positive x-axis) and 90 degress being East (along the negative y axis). This makes heading = -yaw for yaw values in NWU coordinate systems.

The thruster goal is given directly as RPM, not as a percentage of the maximum RPM. Be sure to check the max RPM in the Fossen vehicle configuration to ensur ethe command is below the maximum.

Surge control of the vehicle is also supported when specified. This is not enabled by default.

Warning

Every time set_control_mode is called, the controller integral values and Low Pass filter is reset. Calling set_control_mode should only be done when switching control modes and should not be placed in the for loop.

depth_goal = 15 # meters
heading_goal = 50 # degrees
thruster_goal = 1525 # RPM

vehicle.set_control_mode('depthHeadingAutopilot') # In this mode, PID controller calculates control commands (u_control)
vehicle.set_goal(depth_goal, heading_goal, thruster_goal) # Changes depth (positive), heading, thruster RPM goals for controller

# Run simulation
for i in range(numSteps):
    state = env.step(accel)
    accel = torpedo_dynamics.update(state)

    # For plotting and arrows
    pos = state['DynamicsSensor'][6:9]  # [x, y, z]
    x_end = pos[0] + 3 * np.cos(np.deg2rad(heading))
    y_end = pos[1] - 3 * np.sin(np.deg2rad(heading))
    pos_list.append(pos)
    time_list.append(state['t'])

    # Change color if within 2 meters
    if abs(depth + pos[2]) <= 2.0:
        color = [0,255,0]
    else:
        color = [255,0,0]

    env.draw_arrow(pos.tolist(), end=[x_end, y_end, -depth], color=color, thickness=5, lifetime=0.03)

Plotting Vehicle State

The following code can be used to plot the vehicle’s state over time. This code is placed after the simulation loop.

plot = True

if plot:
    import matplotlib.pyplot as plt

    # Convert position list to a numpy array for easier slicing
    pos_array = np.array(pos_list)

    # Extract x, y, and z positions
    x_positions = pos_array[:, 0] # North Position
    y_positions = pos_array[:, 1] # West Position
    east_positions = [-y for y in y_positions] # Convert from west to east
    z_positions = pos_array[:, 2] # Z positions (Z up)

    # Plot x and y positions
    plt.figure()
    plt.plot( east_positions,x_positions, marker='o')
    plt.title('X and Y Positions')
    plt.xlabel('East (meters)')
    plt.ylabel('North (meters)')
    plt.grid(True)

    # Plot z positions over time
    plt.figure()
    plt.plot(time_list, z_positions, marker='o')
    plt.title('Z Position over Time')
    plt.xlabel('Time Step')
    plt.ylabel('Z Position')
    plt.grid(True)

    # Show the plots
    plt.show()