PD Controller

A number of built in agents have simple PD controllers built in to allow for easy navigating when data is being generated, namely the HoveringAUV and SurfaceVessel. When the the PD control scheme is selected, the control passed to holoocean will be the desired state to navigate to. Below is an example of this for the Surface Vessel where it navigates to a number of predefined waypoint on the surface.

import holoocean
import numpy as np

config = {
    "name": "SurfaceNavigator",
    "world": "SimpleUnderwater",
    "package_name": "Ocean",
    "main_agent": "sv",
    "agents":[
        {
            "agent_name": "sv",
            "agent_type": "SurfaceVessel",
            "sensors": [
            {
                "sensor_type": "GPSSensor",
            }
            ],
            "control_scheme": 1, # PD Control Scheme
            "location": [0,0,2],
            "rotation": [0, 0, 0]
        }
    ],
}

# Define waypoints
idx = 0
locations = np.array([[25,25],
                    [-25,25],
                    [-25,-25],
                    [25,-25]])

# Start simulation
with holoocean.make(scenario_cfg=config) as env:
    # Draw waypoints
    for l in locations:
        env.draw_point([l[0], l[1], 0], lifetime=0)

    print("Going to waypoint ", idx)

    while True:
        #send waypoint to holoocean
        state = env.step(locations[idx])

        # Check if we're close to the waypoint
        p = state["GPSSensor"][0:2]
        if np.linalg.norm(p-locations[idx]) < 1e-1:
            idx = (idx+1) % 4
            print("Going to waypoint ", idx)