基于福森模型的动力学

scenario = {
    "name": "torpedo_dynamics",
    "package_name": "Ocean",
    "world": "OpenWater",
    "main_agent": 'auv0',
    "agents": [
        {
            "agent_name": 'auv0',
            "agent_type": "CougUV",
            "sensors": [
                {
                    "sensor_type": "DynamicsSensor",
                    "configuration": {
                        "UseCOM": True,
                        "UseRPY": False  # 使用四元数描述动力学
                    }
                },
            ],
            "control_scheme": 1,  # 控制方案 1 展示了如何将自定义动态特性应用于 CougUV 代理
            "location": [0, 20, -280],
            "rotation": [0, 0, 0],
            "fossen_model": "torpedo",  # 要使用的福森动力学模型类
            "control_mode": "depthHeadingAutopilot",  # 初始控制模式
            "dynamics": {
                "rho":          1026,   # 水的密度（单位：千克/立方米）
                # 载具物理参数：
                "mass":         31.03,     # 载具质量（千克）
                "length":       1.6,    # 载具长度（米）
                "diam":         0.19,   # 载具直径（米）
                "r_bg": [0, 0, 0.02],   # 载具重心 (x, y, z) 在车身坐标系中的位置，x 轴向前，y 轴向右，z 轴向下（body gravity）。
                "r_bb": [0, 0, 0],      # 载具浮心坐标 (x, y, z)（body boyancy），其中 x 轴向前，y 轴向右，z 轴向下。
                "area_fraction": 0.7,   # 将车辆有效面积与长度和宽度联系起来。对于椭球体，有效面积为π/4。

                # 低速线性阻尼矩阵参数：
                "T_surge":      20,     # 浪涌（surge）时间常数（秒）
                "T_sway":       20,     # 摆动（sway）时间常数（秒）
                "zeta_roll":    0.3,    # 横滚阻尼比
                "zeta_pitch":   0.8,    # 俯仰阻尼比
                "T_yaw":        1,      # 偏航时间常数（秒）
                "K_nomoto":     0.25,   # Nomoto gain

                # 其他阻尼参数：
                "r44":          0.3,    # 横滚附加转动惯量： A44 = r44 * Ix
                "Cd":           0.42,   # 阻力系数
                "e":            0.7,    # 载具阻力的奥斯瓦尔德效率因子
            },
            "actuator":{
                # 鳍片：
                "fin_count": 4,         # 载具上等距分布的鳍片数量
                "fin_offset_deg": 0,    # 第一个鳍片绕 x 轴的角度偏移量（度），从 y 轴正方向开始，z 轴向下
                                        # 0 度: 端口一侧的鳍片
                                        # 90 度: 底部的鳍片
                "fin_center":   0.1,    # 从鳍片上的压力中心 (COP) 到 YZ 平面内质心 (COM) 的半径 (m)
                "fin_area":     0.00697, # 鳍片一侧的表面积
                "x_fin":       -0.8,    # 机身框架 x 重心到鳍状物压力中心的距离（x 向前）
                "CL_delta":     0.5,    # 鳍片升力系数
                "deltaMax_fin_deg": 15, # 鳍片最大偏转角度（度）
                "T_delta":      0.1,    # 鳍片驱动时间常数（秒）

                # 螺旋桨：
                "nMax":         1525,   # 推进器最大转速
                "T_n":          1.0,    # 推进器执行时间常数（秒）
                "D_prop":       0.14,   # 螺旋桨（Propeller）直径
                "t_prop":       0.1,    # 螺旋桨螺距（Propeller pitch）
                "KT_0":         0.4566, # 零转速时的推力系数
                "KQ_0":         0.0700, # 零转速时的扭矩系数
                "KT_max":       0.1798, # 最大推力系数
                "KQ_max":       0.0312, # 最大扭矩系数
                "w":            0.056,  # 伴流分数（wake fraction number）
                "Ja_max":       0.6632, # 最大前进比
            },
            "autopilot": {
                'depth': {
                    'wn_d_z':   0.12,    # 深度命令低通滤波器的阻尼固有频率
                    'Kp_z':     0.153,    # 深度控制器的比例增益
                    'T_z':      100,    # 深度控制器的时间常数
                    'Kp_theta': 39.78,    # 深度控制器俯仰角比例（Proportional）增益
                    'Kd_theta': 17.1,    # 深度控制器俯仰角的微分（Derivative）增益
                    'Ki_theta': 0.5,    # 深度控制器俯仰角积分（Integral）增益
                    'wn_d_theta': 0.25,    # 深度命令低通滤波器的阻尼固有频率
                    'K_w':      0.0,    # 可选的垂荡速度反馈增益
                    'theta_max_deg': 15, # 俯仰控制器内环最大输出
                    'outer_loop_threshold': 2.91, # 外环切换到浪涌控制的阈值
                    'surge_threshold': 0.6, # 运行深度控制器的浪涌阈值
                },
                'heading': {
                    'wn_d':     0.4,    # 低通滤波器的输入指令的阻尼（Damped）固有频率
                    'zeta_d':   1.0,    # 阻尼（damping）系数
                    'r_max':    0.87,
                    'lam':      0.1,
                    'phi_b':    0.1,
                    'K_d':      0.5,
                    'K_sigma':  0.05,
                }
            }
        }
    ]
}

import holoocean
from holoocean.fossen_dynamics import *
import numpy as np

scenario = {...} # 有关场景配置，请参见上文。

env = holoocean.make(scenario_cfg=scenario)

# 初始化代理的福森动力学模型
fossen_agents = ['auv0'] # 使用福森动力学的代理名称列表
fossen_interface = FossenInterface(fossen_agents, scenario)
accel = np.array(np.zeros(6), float)  # 初始化 HoloOcean 加速度输入

fossen_interface.set_control_mode('auv0', 'manualControl')
fins_degrees = np.array([10, 10, -10, -10])  # 鳍片挠度（度）
fin_radians = np.radians(fins_degrees)
thruster_rpm = 800  # 不是百分比：请查看载具动力学数据以确定最高转速
u_control = np.append(fin_radians, thruster_rpm)

for i in range(1500):
    state = env.step(accel)
    fossen_interface.set_u_control('auv0', u_control)  # 如果需要，您可以在此处更改控制命令。
    accel = fossen_interface.update('auv0', state)  # 计算要施加于 HoloOcean 代理的加速度。

depth_goal = 279 # 以米为单位
heading_goal = -10 # 以度为单位
thruster_goal = 1525 # RPM

fossen_interface.set_control_mode('auv0', 'depthHeadingAutopilot')
fossen_interface.set_goal('auv0', depth_goal, heading_goal, thruster_goal)

# 运行模拟
for i in range(1500):
    state = env.step(accel)
    accel = fossen_interface.update('auv0', state)

    # 可选的箭头可视化效果（用于指示航向和深度目标）
    pos = state['DynamicsSensor'][6:9]  # [x, y, z]
    x_end = pos[0] + 3 * np.cos(np.deg2rad(heading_goal))
    y_end = pos[1] + 3 * np.sin(np.deg2rad(heading_goal))

    color = [0, 255, 0] if abs(depth + pos[2]) <= 2.0 else [255, 0, 0]
    env.draw_arrow(pos.tolist(), end=[x_end, y_end, -depth], color=color, thickness=5, lifetime=0.03)

scenario = {
"name": "multi_agent_fossen",
"world": "OpenWater",
"package_name": "Ocean",
"main_agent": "auv0",
"agents": [
    {
        "agent_name": "auv0",
        "agent_type": "TorpedoAUV",
        "sensors": [
            {
                "sensor_type": "DynamicsSensor",
                "configuration": {
                    "UseCOM": True,
                    "UseRPY": False  # 使用四元数描述动力学
                }
            },
        ],
        "control_scheme": 1,  # Control scheme 1 is how custom dynamics are applied to TAUV
        "location": [10,0,0],
        "rotation": [0,0,0],
        "fossen_model": "torpedo",
        "control_mode": "manualControl",
        },
        {
        "agent_name": "sv1",
        "agent_type": "SurfaceVessel",
        "sensors": [
            {
                "sensor_type": "DynamicsSensor",
                "configuration": {
                    "UseCOM": True,
                    "UseRPY": False  # 使用四元数描述动力学
                }
            },
        ],
        "control_scheme": 2,  # 控制方案 2 是将自定义动态特性应用于 SV 的方式
        "location": [10, -10, 0],
        "rotation": [0,0,0],
        "fossen_model": "otter",
        "control_mode": "manualControl",
        },
    ]
}

import holoocean
from holoocean.fossen_dynamics import *
import numpy as np

scenario = {...} # 有关场景配置，请参见上文

env = holoocean.make(scenario_cfg=scenario)
main_agent = 'auv0'
sv_agent = 'sv1'
fossen_agents = [main_agent, sv_agent]
fossen_interface = FossenInterface(fossen_agents, scenario)

accel = np.array(np.zeros(6),float)

ins_degrees = np.array([10, -10, -10, 10]) # 舵和尾鳍偏转角度（度）
fin_radians = np.radians(fins_degrees)
thruster_rpm = 1000
u_control_torpedo = np.append(fin_radians,thruster_rpm)  # [RudderAngle, SternAngle,Thruster] 以弧度为单位
fossen_interface.set_control_mode(main_agent, 'manualControl')

u_control_otter = np.array([105, 80])
fossen_interface.set_control_mode(sv_agent, 'manualControl')

states = env.tick() # 获取代理的初始阶段动力学

for i in range(1500):
    fossen_interface.set_u_control(main_agent, u_control_torpedo) # 如果需要，您可以在此处更改控制命令
    fossen_interface.set_u_control(sv_agent, u_control_otter)

    for agent in fossen_agents:
        accel = fossen_interface.update(agent, states) # 计算要施加于 HoloOcean 代理的加速度
        env.act(agent, accel)

    states = env.tick()