example.py

"""This file contains multiple examples of how you might use BiguaSim."""
import numpy as np

import biguasim
from biguasim.environments import *
from biguasim.vehicle_dynamics import *
from biguasim.dynamics import *

def hovering_example():
    """A basic example of how to use the HoveringAUV agent."""
    env = biguasim.make("SimpleUnderwater-Hovering")

    # This command tells the AUV go forward with a power of "10"
    # The last four elements correspond to the horizontal thrusters (see docs for more info)
    command = np.array([0, 0, 0, 0, 10, 10, 10, 10])
    for _ in range(1000):
        state = env.step(command)
        # To access specific sensor data:
        if "PoseSensor" in state:
            pose = state["PoseSensor"]
        # Some sensors don't tick every timestep, so we check if it's received.
        if "DVLSensor" in state:
            dvl = state["DVLSensor"]

    # This command tells the AUV to go down with a power of "10"
    # The first four elements correspond to the vertical thrusters
    command = np.array([-10, -10, -10, -10, 0, 0, 0, 0])
    for _ in range(1000):
        # We alternatively use the act function
        env.act("auv0", command)
        state = env.tick()

    # You can control the AgentFollower camera (what you see) by pressing v to toggle spectator
    # mode. This detaches the camera and allows you to move freely about the world.
    # Press h to view the agents x-y-z location
    # You can also press c to snap to the location of the camera to see the world from the perspective of the
    # agent. See the Controls section of the ReadMe for more details.


def torpedo_example():
    """A basic example of how to use the TorpedoAUV agent."""
    env = biguasim.make("SimpleUnderwater-Torpedo")

    # This command tells the AUV go forward with a power of "50"
    # The last four elements correspond to 
    command = np.array([0, 0, 0, 0, 50])
    for _ in range(1000):
        state = env.step(command)

    # Now turn the top and bottom fins to turn left
    command = np.array([0, -45, 0, 45, 50])
    for _ in range(1000):
        state = env.step(command)


def editor_example():
    """This editor example shows how to interact with holodeck worlds while they are being built
    in the Unreal Engine Editor. Most people that use holodeck will not need this.

    This example uses a custom scenario, see 
    https://biguasim.readthedocs.io/en/latest/usage/examples/custom-scenarios.html

    Note: When launching Holodeck from the editor, press the down arrow next to "Play" and select
    "Standalone Game", otherwise the editor will lock up when the client stops ticking it.
    """

    config = {
        "name": "test",
        "world": "ExampleLevel",
        "main_agent": "auv0",
        "agents": [
            {
                "agent_name": "auv0",
                "agent_type": "HoveringAUV",
                "sensors": [
                    {
                        "sensor_type": "LocationSensor",
                    },
                    {
                        "sensor_type": "VelocitySensor"
                    },
                    {
                        "sensor_type": "RGBCamera"
                    }
                ],
                "control_scheme": 1,
                "location": [0, 0, 1]
            }
        ]
    }

    env = BiguaSimEnvironment(scenario=config, start_world=False)
    command = [0, 0, 10, 50]

    for i in range(10):
        env.reset()
        for _ in range(1000):
            state = env.step(command)


def editor_multi_agent_example():
    """This editor example shows how to interact with holodeck worlds that have multiple agents.
    This is specifically for when working with UE4 directly and not a prebuilt binary.

    Note: When launching Holodeck from the editor, press the down arrow next to "Play" and select
    "Standalone Game", otherwise the editor will lock up when the client stops ticking it.
    """
    config = {
        "name": "test_handagent",
        "world": "ExampleLevel",
        "main_agent": "auv0",
        "agents": [
            {
                "agent_name": "auv0",
                "agent_type": "HoveringAUV",
                "sensors": [
                ],
                "control_scheme": 1,
                "location": [0, 0, 1]
            },
            {
                "agent_name": "auv1",
                "agent_type": "TorpedoAUV",
                "sensors": [
                ],
                "control_scheme": 1,
                "location": [0, 0, 5]
            }
        ]
    }

    env = BiguaSimEnvironment(scenario=config, start_world=False)

    cmd0 = np.array([0, 0, -2, 10])
    cmd1 = np.array([0, 0, 5, 10])

    for i in range(10):
        env.reset()
        env.act("uav0", cmd0)
        env.act("uav1", cmd1)
        for _ in range(1000):
            states = env.tick()



def fossen_dynamics():
    """
    Example of how Thor Fossen models can be used in the BiguaSim simulator to model 
    the motion of the vehicle based on control surface commands. Uses the FossenDyanmics
    class that is is the dynamics.py file. This vehicle has 4 fins controlled by two 
    angle inputs of the stern and rudder fins. Torpedo vehicles with 3 or 4 independently 
    controlled fins can also be used with the current model. 
    
    NOTE: Vehicle parameters are currently only tuned for the REMUS100 vehicle.
    Mass and other parameters set in engine are ignored with this control scheme as they are taken into 
    account witht the Fossen Models. 
    """

    ticks_per_sec = 50
    print("Change Additional Launch Parameters to match ticks_per_sec if running live")
    numSteps = 600
    print("Total Simulation Time:", (numSteps/ticks_per_sec))

    initial_location = [0,0,-10] #Translation in NWU coordinate system
    initial_rotation = [0,0,0] #Roll, pitch, Yaw in Euler angle order ZYX and in degrees NWU coordinate system

    scenario = {
    "name": "torpedo_dynamics",
    "package_name": "Ocean",
    "world": "OpenWater",
    "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": initial_location,
            "rotation": initial_rotation,
            "dynamics": 
                {
                    "mass": 16,
                    "length": 1.6,
                    "rho": 1026,
                    "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": 5.0 / 20.0
                },
            "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,
                    }
                },
            
            }
        ]
    }

    env = biguasim.make(scenario_cfg=scenario)

    #Create vehicle object attached to biguasim agent with dynamic parameters 
    vehicle = fourFinDep(scenario, 'auv0','manualControl')    

    period = 1.0/ticks_per_sec
    #Create dynamics object passing in the vehicle created
    torpedo_dynamics = FossenDynamics(vehicle,period)  

    accel = np.array(np.zeros(6),float)  #BiguaSim parameter input 


    pos_list = []
    time_list = []

    ############## MANUAL CONTROL EXAMPLE: ###########
    #Set control surfaces command
    fins_degrees = np.array([5 , 5]) #Rudder and Stern Fin Deflection (degrees)
    fin_radians = np.radians(fins_degrees)
    thruster_rpm = 800
    u_control = np.append(fin_radians,thruster_rpm)  #[RudderAngle, SternAngle,Thruster]
    vehicle.set_control_mode('manualControl')
    
    for i in range(numSteps):
        state = env.step(accel)
        torpedo_dynamics.set_u_control_rad(u_control) #If desired you can change control command here
        accel = torpedo_dynamics.update(state) #Calculate accelerations to be applied to BiguaSim agent

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


    ############ Depth Heading Control Example:  ############ 
    env.reset()
    numSteps = 800

    depth = 13
    heading = 50    
    vehicle.set_goal(depth,heading,1525)     #Changes depth (positive depth), heading, thruster RPM goals for controller 
    vehicle.set_control_mode('depthHeadingAutopilot') #In this mode PID controller calculates control commands (u_control)
    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)
    

    ################ Plot vehicle State ###################
    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]   #Depth

        # 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()