Isaac Sim 14 物理学习（车辆动力学）

阿兹卡班首席

已于 2024-11-14 14:17:54 修改

阅读量1.4k

点赞数 6

分类专栏： IsaacSim 文章标签：学习

于 2024-11-05 11:08:35 首次发布

本文链接：https://blog.csdn.net/weixin_61044335/article/details/143502081

版权

IsaacSim 专栏收录该内容

19 篇文章

订阅专栏

本文内容均来源于网络：博客文章、官方文档手册、官方论坛等

参考内容

Nvidia Isaac Sim代码编程入门教程 2024（7）_isaac sim franka-CSDN博客

Python Bindings API — omni_physics 106.1 documentation

Physics — Omniverse IsaacSim latest documentation

Vehicle Dynamics — Omniverse Extensions latest documentation

ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境_ros gazebo 阿克曼转向-CSDN博客

Wheeled Robots [omni.isaac.wheeled_robots] — isaac_sim 4.2.0-rc.17 documentation

Isaac Sim 主要是为模拟“光滑表面上的结构化、受控环境中运行的室内机器人”。没有办法完全满足模拟自动驾驶汽车。尤其是在道路上和越野时所需的动态和真实世界效果。如果需要进行室外车辆的仿真测试，要学的东西略多。

Isaac Sim 仿真环境目前遇到的糟心事儿：

无法提供逼真和多样化的户外场景，例如道路状况、不断变化的天气和照明或不同的道路类型。考虑过使用粒子发射器来模拟天气，雨，雾，结果一点点水珠就卡的要死，试都没试几分钟就闪退了。太吃硬件。基本原理：水可以看作是成千上万个小水珠（球状模型），每一个都给予一个物理效果（液体），加一个贴图（蓝色透明），再加一个物理效果（重力，可能有碰撞）
场景过于单一化，无法制作出随机性高的道路、植物等大型户外场景。可以人为建立复杂化场景，但时间成本和学习成本不可以忽视。
车辆的可以进行设置修改的物理参数过于简单，太少了。
传感器限制，传感器类型并不多，还有一些数据格式、传输速率等不可控因素（isaacsim中甚至不能指定帧率）
地形的改变比想象中要困难，目前还没有成功做出地面随机区域凹凸，只能人工去修改建模，还不是在isaacsim中，是去blender。模型完全贴合地面凹凸也是个难题，会穿模。（可以减少模型大小，并且计算当前生成位置的切线，来进行模型生成，但这种做法对于硬件挑战不小）
太吃硬件，更新速度太快，硬件跟不上

单位

在讲解之前，先说一下Isaac Sim的逆天单位。

在软件中，是不会显示参数的单位的。

在官网中，可以看到使用的usd模型文件的参数如下所示。

但当我在差分控制器中对小车参数进行设置时，发现无论如何都不正确。在场景中用measure工具测量的结果、在官网中的官方参数...什么都不对。直到我在Action Graph中看到了对差分控制器的标注：

这些个cms是什么东西？？？跟所有官网中模型参数单位m有什么关系？跟在仿真环境中测量的轮半径轮间距有什么关系？就算它是cm单位，不是m单位，输入进去车辆动都动不了。输入m的单位，动的非常非常慢，在阻力仅有0.01的情况下，0.2m/s的速度，走1米要15秒。必须在m为单位的数值基础上再将轮半径除3。这是什么逆天事情？这怎么算？

注意：

高刚度使关节更快、更难地卡住所需的目标，更高的阻尼可以平滑，但也会减慢关节对目标的移动

对于位置控制，设置相对较高的刚度和较低的阻尼（以减少振动）
对于速度控制，刚度必须设置为零，阻尼为非零
对于力控制，刚度和阻尼必须设置为零

Odom相关

Isaac Compute Odometry Node

Angular Acceleration（角加速度），vectord[3]， rad/s^2
Angular Velocity（角速度），vectord[3]， rad/s
Linear Acceleration（线性加速度），vectord[3]，m/s^2
Linear Velocity（线速度），vectord[3]，m/s
旋转方向四元数，quatd[4]，（IJKR）
位置矢量，vectord[3]，m

Wheel Odometry（轮式里程计）：
- 这是最常见的里程计形式，适用于有轮子的机器人。通过测量轮子转过的角度或距离，结合轮子的周长，可以计算出机器人的位移。这种方法简单直接，但容易受到滑动、轮径变化等因素的影响。
Visual Odometry（视觉里程计）：
- 视觉里程计使用摄像头作为传感器，通过分析连续图像之间的差异来估计机器人的运动。这种方法不受地面条件限制，但需要足够的环境特征以供匹配，并且在光照变化或快速运动时可能表现不佳。
Laser Odometry（激光里程计）：
- 利用激光雷达（LiDAR）获取周围环境的点云数据，通过比较不同时间点的数据集来估算机器人的位移。激光里程计具有较高的精度和稳定性，特别适合于结构化环境中使用。
Inertial Measurement Unit (IMU) Odometry（惯性测量单元里程计）：
- IMU包含加速度计和陀螺仪等传感器，能够提供关于物体线性和角加速度的信息。通过积分这些信号，可以估算出物体的速度和位移。虽然IMU可以提供高频率的更新，但由于积分误差累积的问题，长期使用时其准确性会逐渐下降。
Magnetic Odometry（磁力里程计）：
- 使用磁力计检测地球磁场的变化来确定方向。这种方法主要用于辅助其他类型的里程计，尤其是在无法使用GPS的情况下提供航向信息。
Ultrasonic Odometry（超声波里程计）：
- 利用超声波传感器发射和接收超声波脉冲的时间差来测量与障碍物的距离，进而推算机器人的位移。这种方法通常用于近距离避障和定位。

车辆物理设置方式及参数意义

/World/jetbot/left_wheel/Collision

type：Sphere

Collider（碰撞器）

Collision Enabled（碰撞启用）：已勾选，表示该碰撞器启用了碰撞检测功能。
Simulation Owner（模拟拥有者）：未选择目标，表示尚未指定该碰撞器所属的模拟对象。

Advanced（高级）

Contact Offset（接触偏移）：设置为 -inf，表示接触偏移量无限大。
Rest Offset（休息偏移）：设置为 -inf，表示休息偏移量无限大。
Torsional Patch Radius（扭转变形补丁半径）：设置为 0.0，表示没有扭转变形补丁。

——————————————————————————————————

/World/jetbot/wheel_material

type：Material

Rigidbody Material（刚体材质）

Dynamic Friction（动态摩擦系数）

设置值：1.0
动态摩擦系数决定了两个表面相对滑动时产生的阻力大小。

Static Friction（静态摩擦系数）

设置值：1.0
静态摩擦系数决定了两个表面刚开始滑动时所需的最小力。

Restitution（弹性恢复系数）

设置值：0.5
弹性恢复系数决定了碰撞后的反弹程度，范围从0（完全非弹性的）到1（完全弹性的）。

Density（密度）

设置值：0.0012
密度影响物体的质量，进而影响其物理行为。

Friction Combine Mode（摩擦组合模式）

设置值：max
决定如何结合两个相交物体的摩擦系数。

Restitution Combine Mode（弹性恢复组合模式）

设置值：average
决定如何结合两个相交物体的弹性恢复系数。

Improved Patch Friction（改进的贴片摩擦）

已勾选
启用此选项可以提高摩擦模型的精度。

Advanced（高级）

Damping Combine Mode（阻尼组合模式）

设置值：average
决定如何结合两个相交物体的阻尼系数。

Compliant Contact Acceleration Spring（顺从接触加速弹簧）

已关闭
如果启用，将应用一个弹簧来模拟接触面的柔顺性。

Compliant Contact Stiffness（顺从接触刚度）

已关闭
如果启用，将设置接触面的刚度。

Compliant Contact Damping（顺从接触阻尼）

设置值：0.0
设置接触面的阻尼系数，影响接触面的粘滞性。

——————————————————————————————————

/World/car/jetbot/chassis

type：Xfrom

Advanced（高级）

Stabilization Threshold（稳定化阈值）：设置用于防止数值不稳定的小数阈值。
Max Depenetration Velocity（最大穿透速度）：限制物体穿透另一个物体的最大速度。
Max Contact Impulse（最大接触冲量）：限制单次接触事件中施加的最大冲量。
Solver Position Iteration Count（求解器位置迭代次数）：设定求解器在更新位置时的迭代次数。
Solver Velocity Iteration Count（求解器速度迭代次数）：设定求解器在更新速度时的迭代次数。
Enable Speculative CCD（启用推测式连续碰撞检测）：如果启用，则会使用一种快速的方法来预测并避免未来的碰撞。
Constraint-force-mixing Scale（约束力混合比例）：调节约束力与其他力之间的混合比例。
Contact Slop Coefficient（接触松弛系数）：定义接触面之间的容许误差。
Enable Gyroscopic Forces（启用陀螺效应力）：如果启用，则考虑旋转物体的陀螺效应。
Solve Contact Retain Accelerations（解决接触保留加速度）：决定是否在解决接触问题时保留物体的加速度。
Solve Contact（解决接触）：启动或禁用接触求解过程。

车辆动力学模型

在网络上，其实分的不是太清晰，错了再改。

轮式机器人控制器模型大约可以分为四种：差分控制器Differential Controller、麦克纳姆轮Holonomic Controller、履带Tank Drive、阿克曼Ackermann Drive。

其他三种转向模型可以实现原地旋转或通过轮胎、履带差速的方式控制车辆的转向和速度，阿克曼转向模型具有后轮驱动、前轮转向的特点。当今民用汽车所使用的都是阿克曼转向模型，适用于四驱车辆实验。

在Isaac Sim 的 Vehicle Dynamics 扩展中提供了用于创建车辆仿真的工具，其中包括轮胎、发动机、离合器、变速箱和悬架模型。

车辆动力学示例场景：打开 Physics Demo Scenes： Window > Simulation > Demo Scenes。单击 Load scene （加载场景）

如果您已将游戏手柄连接到 PC，并且想要使用它驾驶车辆，请单击 Viewport 窗口左上角的设置图标。然后，取消选中 Gamepad Camera Control 设置。这允许游戏手柄控制车辆，而不是相机。如果您希望使用键盘箭头键控制车辆，则可以跳过此步骤。

如果启用了该扩展，则可以添加一个跟随车辆的特殊摄像头。在 Stage 窗口中选择 Vehicle 基元。右键单击 Vehicle ，将光标光标置于 Add 菜单上，然后将鼠标悬停在 Cameras 菜单上，然后选择 Follow Look 菜单项。(omni.physx.camera)

摄像机将出现在 Viewport 中。要通过该摄像机查看场景，请单击 Viewport 窗口左上角的摄像机图标，单击 Cameras 按钮并选择 VehicleLookFollowCamera0。有关摄像头扩展以及如何调整所有摄像头设置的其他信息，请参阅 PhysX 摄像头文档。

VehicleAudio.py：可以以交互的方式播放发动机和轮胎的声音。请确保您的 PC 音频未静音并设置为合理的音量。按下 Omniverse USD Composer 左侧的 Play 按钮开始模拟。

如果使用的是游戏手柄，请使用右扳机加速，使用左扳机制动，使用左模拟操纵杆左右驾驶车辆。如果您使用的是键盘，请使用向上箭头键进行加速，使用向下箭头键进行制动，使用向左和向右箭头键进行车辆转向。按 Stop 按钮结束模拟。

使用 Vehicle Wizard 创建车辆

Vehicle Wizard 和 Vehicle Demo 用原始的几何形状创建一辆车，一个盒子用于底盘，四个圆柱体用于车轮。然后，可以将用于渲染的车辆网格链接到这些变换并设置动画。

Omniverse USD Composer 创建的物理表示和渲染的车辆网格应使用相同的坐标系和单位进行创作。这样就无需添加额外的旋转或缩放来使两种表示匹配。Vehicle Demo 是硬编码的，使用 Y 轴作为向上轴。但是，可以将 Vehicle Wizard 配置为匹配任何坐标系。在使用 Vehicle Wizard 之前，打开 Edit > Preferences 窗口，选择 Stage 并选择 Default Up Axis 以匹配用于创作渲染车辆的坐标系。创建一个新阶段（File > New）以确保更改的默认值生效。

车辆向导描述

Vehicle Wizard 可以通过 Create > Physics > Vehicle 菜单命令打开。

Vehicle Wizard 包含三个页面。第一个要求车辆的基本尺寸和配置，第二个设置轮胎的尺寸以及其他车轮和悬架相关设置。最后一页提供了完成无人机及其物理设置的可选后续步骤的检查列表。

向导创建的车辆将根据 Create 的坐标系进行定向。如果选择了 Z 轴向上，则 X 轴或 Y 轴可以是向前方向。如果选择了 Y 轴向上，则 X 轴或 Z 轴可以向前移动。Vehicle Wizard 将创建一个应用了 PhysxVehicleContextAPI 的 PhysicsScene 基元，它将反映所选配置。但是，如果舞台中已经存在应用了 PhysxVehicleContextAPI 的物理场景基元，则其垂直轴和纵轴信息将用于确定新车辆的方向。

Vehicle Prim 面板允许指定应用作车辆 prim 的 prim 的路径。车辆图元是车辆层次结构的根，其中包含随车辆移动的所有部分。如果有可用的车辆渲染资产，并且目标是直接将车辆相关属性应用于该资产的图元以将其转换为车辆模拟资产，则可以考虑此选项。必须注意，指定的车辆图元需要是 UsdGeomXform。此外，请参阅编写车辆的渲染网格，了解有关舞台设置以及如何排列资产图元的一些建议。如果未定义 Vehicle prim，向导将创建一个 prim （以及车轮等的子 prims）。Selected （选定）和 Clear （清除） 按钮是允许分别拾取当前所选图元的路径或清除路径的辅助对象。

在 Chassis Box 面板中，输入 Length， Width 和 Height ，其单位与 Omniverse USD Composer 中用于对世界进行建模的单位相同。这些维度将用于计算车辆的质量分布或惯性矩。此外，尺寸将影响车轮的放置位置（除非扫描车轮几何形状或明确指定车轮附件图元）。如果未定义 Vehicle Prim，则尺寸也将用于创建应包含大部分 Vehicle Mesh 几何体的碰撞盒。如果需要，可以删除向导创建的碰撞箱，并在以后将其替换为更形状拟合的表示。但是，计算车辆的基本质量属性仍然需要这些输入。

如果要偏离自动计算的值，请在 Mass 编辑框中输入车辆的重量。使用与阶段中定义的相同的权重单位。

可以从 纵向轴 下拉列表中选择车辆的前进（纵向）方向。如果舞台上存在应用了 PhysxVehicleContextAPI 的图元，则 Longitudinal Axis 下拉列表将被禁用，并显示 PhysxVehicleContextAPI 中指定的纵轴。如果之前未选择纵向，则将显示可用选项。

通过从舞台中选择一组代表机箱的图元，然后按 Scan 按钮，可以使用自动填充大多数设置的选项。确保在运行扫描之前根据需要定义 Longitudinal Axis。Vehicle Wizard 将使轴对齐的边界框适合选定的图元，并填写尺寸设置。质量也会被调整，除非之前覆盖了该字段。边界框还将用于定义车辆位置（如果未指定车辆图元）和计算车辆的质心。图元具有描述其用途的用途，例如 render、guide、default 和 proxy。仅扫描 render 和 default prims。此外，只能使用几何网格和形状来拟合边界框。请注意，选择车辆图元和扫描底盘箱是分开的，因为车辆图元不必靠近底盘箱，并且并非车辆图元下的所有渲染网格或几何体都适合包含在底盘箱的边界框计算中。

“驱动”面板要求使用以下三种方法之一来推动车辆：Standard（标准）、Basic（基本）或 None（无）。

Number of Axles 编辑框用于设置车辆上的轮胎对数。轮胎设置将在下一页进行调整。

按 Next 按钮访问这些设置。或者，可以按下 Create 按钮以使用 Axle Page 的默认设置来创建车辆。随时按 Reset 将向导设置重置为其默认值（这也将清除用户覆盖的字段并返回自动计算这些值）。

标准驱动器

标准驱动类型利用发动机和变速器将扭矩传递到驱动轮胎。使用此驱动类型时，请输入最大发动机马力、最大发动机转速和变速器中的齿轮数。

车辆发动机将配置为遵循由以下参考点定义的马力与发动机 RPM 曲线：

当发动机达到其最大 RPM 的三分之一时，此功率曲线输出峰值马力，并在发动机怠速和最大 RPM 时下降到 80%。这条曲线不是很现实，但它在怠速时产生更多动力，以获得更好的静止加速。

Number of Gears （齿轮数）指定变速器中的齿轮数。最高档位始终为 1：1,1 档传动比设置为与档位数相同的值，例如，在具有 5 个档位的变速器中为 5：1。其余档位均匀地降低 1 档和最高档之间的传动比。最终传动比设置为 4：1。

齿轮在将发动机产生的扭矩施加到轮胎之前对其进行缩放。然而，传动比越高，变速器必须在发动机达到最大转速之前越早升至下一个档位。较重的车辆需要更高的传动比来产生更大的轮胎扭矩来加速，但需要额外的档位和更多的换档时间来补偿。

车辆的最高速度将由 RPM 决定，并在一定程度上由 Horsepower 设置决定，而车辆的加速曲线将由 Number of Gears 和 Horsepower 设置控制。如果增加马力没有增加车辆加速度，则很可能是驱动轮胎打滑或烧坏。为了进一步提高车辆的加速度，需要增加轮胎的纵向刚度或摩擦力。

基本驱动器

Basic 驱动类型的工作原理是简单地设置施加到驱动轮胎的最大扭矩。不会创建任何引擎或变速箱。扭矩由施加的节气门量成比例。输入设备还用于控制转向轮胎的转向角。此 Drive Type 可用于模拟电动汽车。最大扭矩是根据 Horsepower 设置计算的（以及选择此驱动类型时保持锁定的 RPM 值：Horsepower * 7120 / RPM）。

无驱动器（无）

当选择 None 驱动类型时，车辆不会将轮胎扭矩传递到轮胎。相反，轮胎扭矩和转向角是手动设置的。这允许用户独立地对每个轮胎进行自定义控制。这对于机器人应用非常有用，例如，轮胎可以沿相反方向旋转以使机器人就地旋转。

官方代码示例

WheelController.py

~/.local/share/ov/pkg/isaac-sim-4.1.0/extsPhysics/omni.physx.vehicle/omni/physxvehicle/scripts/samples/WheelController.py

直接控制车辆车轮，而不使用驱动模型

import math

from pxr import Gf, PhysxSchema

from ..helpers import Factory
from .VehicleSampleBase import VehicleSampleBase
from . import Stepper

import omni.physxdemos as demo


class WheelControllerDemo(VehicleSampleBase):
    title = "Wheel controller"
    category = demo.Categories.VEHICLES
    short_description = "Usage of wheel controller"
    description = "Demo showing how to control wheels directly when not using a drive model."

    def create(self, stage):
        super().create(stage)

        create(stage)

        self.autofocus = True # autofocus on the scene at first update
        self.autofocus_zoom = 0.28 # Get a bit closer


class WheelControllerScenario(Stepper.Scenario):
    def __init__(
        self, stage, vehiclePaths, wheelAttachmentPaths, wheelDriveTorques, wheelBrakeTorques, wheelSteerAngles,
        timeStepsPerSecond
    ):
        secondsToRun = 6.0
        super().__init__(secondsToRun, 1.0 / timeStepsPerSecond)
        self._stage = stage
        self._vehicleCount = len(vehiclePaths)
        self._vehiclePaths = vehiclePaths
        self._wheelAttachmentPaths = wheelAttachmentPaths
        self._wheelControllers = []
        for i in range(self._vehicleCount):
            self._wheelControllers.append([])
            for j in range(len(self._wheelAttachmentPaths[i])):
                prim = self._stage.GetPrimAtPath(self._wheelAttachmentPaths[i][j])
                self._wheelControllers[i].append(PhysxSchema.PhysxVehicleWheelControllerAPI(prim))
        self._wheelDriveTorques = wheelDriveTorques
        self._wheelBrakeTorques = wheelBrakeTorques
        self._wheelSteerAngles = wheelSteerAngles

        self._steerStop = 0.3 * secondsToRun
        self._accelStop = 0.5 * secondsToRun
        self._brakeStart = self._accelStop

    def on_start(self):
        for i in range(self._vehicleCount):
            for j in range(len(self._wheelAttachmentPaths[i])):
                self._wheelControllers[i][j].GetBrakeTorqueAttr().Set(0)
                self._wheelControllers[i][j].GetSteerAngleAttr().Set(0)

            self._wheelControllers[i][Factory.WHEEL_FRONT_LEFT].GetDriveTorqueAttr().Set(self._wheelDriveTorques[i])
            self._wheelControllers[i][Factory.WHEEL_FRONT_RIGHT].GetDriveTorqueAttr().Set(self._wheelDriveTorques[i])

    def on_end(self):
        return

    def on_step(self, deltaTime, totalTime):
        if totalTime < self._steerStop:
            for i in range(self._vehicleCount):
                steerAngle = (totalTime / self._steerStop) * self._wheelSteerAngles[i]

                self._wheelControllers[i][Factory.WHEEL_FRONT_LEFT].GetSteerAngleAttr().Set(steerAngle)
                self._wheelControllers[i][Factory.WHEEL_FRONT_RIGHT].GetSteerAngleAttr().Set(steerAngle)
        elif (totalTime - self._steerStop) <= deltaTime:
            for i in range(self._vehicleCount):
                self._wheelControllers[i][Factory.WHEEL_FRONT_LEFT].GetSteerAngleAttr().Set(0)
                self._wheelControllers[i][Factory.WHEEL_FRONT_RIGHT].GetSteerAngleAttr().Set(0)

        if (totalTime > self._accelStop) and ((totalTime - self._accelStop) <= deltaTime):
            for i in range(self._vehicleCount):
                self._wheelControllers[i][Factory.WHEEL_FRONT_LEFT].GetDriveTorqueAttr().Set(0)
                self._wheelControllers[i][Factory.WHEEL_FRONT_RIGHT].GetDriveTorqueAttr().Set(0)

        if (totalTime >= self._brakeStart) and ((totalTime - self._brakeStart) <= deltaTime):
            for i in range(self._vehicleCount):
                self._wheelControllers[i][Factory.WHEEL_REAR_LEFT].GetBrakeTorqueAttr().Set(self._wheelBrakeTorques[i])
                self._wheelControllers[i][Factory.WHEEL_REAR_RIGHT].GetBrakeTorqueAttr().Set(self._wheelBrakeTorques[i])


def create(stage):
    vehicleCount = 3
    vehiclePaths = []
    wheelAttachmentPaths = []
    timeStepsPerSec = 60

    Factory.create4WheeledCarsScenario(
        stage,
        1.0,
        vehicleCount,
        createCollisionShapesForWheels=True,
        driveMode=Factory.DRIVE_NONE,
        vehiclePathsOut=vehiclePaths,
        wheelAttachmentPathsOut=wheelAttachmentPaths,
        vehicleDelta=[-3, 0, 0],
        timeStepsPerSecond = timeStepsPerSec
    )

    wheelDriveTorques = [300, 600, 900]

    wheelBrakeTorques = [500, 1000, 2000]

    wheelSteerAngles = [(45 * math.pi) / 180, (30 * math.pi) / 180, (10 * math.pi) / 180]

    scenario = WheelControllerScenario(
        stage, vehiclePaths, wheelAttachmentPaths, wheelDriveTorques, wheelBrakeTorques, wheelSteerAngles,
        timeStepsPerSec
    )
    Stepper.run_scenario(scenario)

小车加音效。利用物理引擎数据（如车辆速度、轮胎滑移、引擎转速等）生成动态音效

VehicleAudio.py

/home/lxy/.local/share/ov/pkg/isaac-sim-4.1.0/extsPhysics/omni.physx.vehicle/omni/physxvehicle/scripts/samples/VehicleAudio.py

import enum
import math
import os

import pxr.OmniAudioSchema as AudioSchema
from pxr import Gf, PhysxSchema, Usd, UsdPhysics

import omni.kit.app
import omni.physxdemos as demo
import omni.timeline
import omni.usd
from omni.physx.bindings._physx import (
    VEHICLE_DRIVE_STATE_ENGINE_ROTATION_SPEED,
    VEHICLE_WHEEL_STATE_ROTATION_SPEED, VEHICLE_WHEEL_STATE_TIRE_LATERAL_SLIP,
    VEHICLE_WHEEL_STATE_TIRE_LONGITUDINAL_SLIP)

from . import BasicSetup
from .VehicleSampleBase import VehicleSampleBase

CMPS_TO_MPH = 0.0223694
RPS_TO_RPM = 30.0 / 3.1415

NOTE0 = 13.6
NOTE1 = 0.0066
NOTE2 = -2.95e-7

GAUSSIAN_A = 0.8
GAUSSIAN_B = 0.0
GAUSSIAN_C = 666.0


class VehicleSoundDemo(VehicleSampleBase):
    title = "Vehicle sounds"
    category = demo.Categories.VEHICLES
    short_description = "Usage of vehicle telemetry to emit sounds"
    description = ("Demo of engine, tire roll and skidding audio and how to connect it to the vehicle telemetry. The arrow keys can be used to steer, accelerate and brake. "
        "To use a gamepad for controlling the vehicle, make sure to disable Gamepad Camera Control in the Viewport Settings.")

    def create(self, stage):
        super().create(stage)

        self._physxInterface = omni.physx.get_physx_interface()
        BasicSetup.create(stage, True)
        create(stage, self._physxInterface)

        self.autofocus = True # autofocus on the scene at first update
        self.autofocus_zoom = 0.28 # Get a bit closer

    def on_shutdown(self):
        self._physxInterface = None


class SoundType(enum.Enum):
    engine = 1
    tire = 2
    skid = 3


class VehicleSound:
    def __init__(self, vehiclePrim, soundPrim, soundType, lowLoad, rpm, speed):
        self._vehicle = vehiclePrim
        self._sound = soundPrim
        self._soundType = soundType
        self._lowLoad = lowLoad
        self._rpm = rpm
        self._speed = speed


class LowPassFilter:
    def __init__(self):
        self._timeConstant = 1.0
        self._oneOverTimeConstant = 0.0
        self._value = 0.0

    def setTimeConstant(self, timeConstant):
        if (timeConstant > 0.0):
            self._timeConstant = timeConstant
            self._oneOverTimeConstant = 1.0 / timeConstant

    def getValue(self):
        return self._value

    def filter(self, value, timeStep):
        if (timeStep < self._timeConstant):
            k = timeStep * self._oneOverTimeConstant
            self._value = k * value + (1.0 - k) * self._value
        else:
            self._value = value

        return self._value


def Lerp(x0, y0, x1, y1, x):
    t = (x - x0) / (x1 - x0)
    t = min(max(t, 0.0), 1.0)

    return y0 + t * (y1 - y0)


def gaussian(a, b, c, x):
    numerator = -(x - b) * (x - b)
    denominator = 2.0 * c * c
    gauss = a * math.exp(numerator / denominator)
    return gauss


class VehicleAudioSample:

    def __init__(self, stage, physxInterface):
        self._physxInterface = physxInterface
        self._stage = stage

        # the update loop callback seems to get triggered before the vehicle update and thus the vehicles have not
        # been set up yet. Unfortunately, there seems no way to describe depenencies on other tasks to enforce
        # an order. Hence, just wait for one update to pass before starting.
        #
        self._firstUpdatePassed = False

        self._slipFilter = LowPassFilter()
        self._slipFilter.setTimeConstant(0.1)

        self._soundList = []

        self._timeline = omni.timeline.get_timeline_interface()

        self._appUpdate = omni.kit.app.get_app().get_update_event_stream().create_subscription_to_pop(
            self.update, name="omni.physx.vehicle update"
        )

        self._usd_context = omni.usd.get_context()
        self._stageEventSubscription = self._usd_context.get_stage_event_stream().create_subscription_to_pop(self.on_stage_event)

        vehiclePrim = None
        self._maxRPM = 0
        self._minRPM = 99999

        for prim in self._stage.Traverse():
            # print(dir(prim))

            if (prim.HasAPI(PhysxSchema.PhysxVehicleAPI)):
                vehiclePrim = prim

            elif (prim.IsA(AudioSchema.Sound)):

                # Initialize all of the sounds.
                # Silent and looping.
                prim.GetAttribute("timeScale").Set(1.0)
                prim.GetAttribute("gain").Set(0.0)
                prim.GetAttribute("loopCount").Set(-1)

                primPath = str(prim.GetPath())

                rpm = 0
                speed = 0
                lowLoad = False

                lowLoadIndex = primPath.find("loww")
                highLoadIndex = primPath.find("high")
                tireIndex = primPath.find("driveon")
                skidIndex = primPath.find("tireslip")

                if (lowLoadIndex != -1):
                    soundType = SoundType.engine
                    lowLoad = True
                    rpm = int(primPath[lowLoadIndex + 4:])
                    self._maxRPM = max(self._maxRPM, rpm)
                    self._minRPM = min(self._minRPM, rpm)

                elif (highLoadIndex != -1):
                    soundType = SoundType.engine
                    rpm = int(primPath[highLoadIndex + 4:])

                elif (tireIndex != -1):
                    soundType = SoundType.tire
                    speed = int(primPath[tireIndex + 7:])

                elif (skidIndex != -1):
                    soundType = SoundType.skid

                newSound = VehicleSound(vehiclePrim, prim, soundType, lowLoad, rpm, speed)
                self._soundList.append(newSound)

    def on_stage_event(self, event):
        if (event.type == int(omni.usd.StageEventType.CLOSING)):
            self.shutdown()

    def shutdown(self):
        self._slipFilter = None
        self._soundList = None
        self._appUpdate = None
        self._stageEventSubscription = None

    def update(self, e):
        if (self._timeline.is_playing()):
            if (self._firstUpdatePassed):
                deltaTime = e.payload["dt"]

                vehiclePrim = None
                throttle = None
                wheelState = None

                tireRadius = 35.0

                for vehicleSound in self._soundList:
                    gain = 0.0
                    timeScale = 1.0

                    if (vehiclePrim != vehicleSound._vehicle):
                        vehiclePrim = vehicleSound._vehicle

                        throttle = vehiclePrim.GetAttribute("physxVehicleController:accelerator").Get()

                        rearLeftWheelPath = vehiclePrim.GetPath().pathString + "/RearLeftWheel"

                        wheelState = self._physxInterface.get_wheel_state(rearLeftWheelPath)
                        driveState = self._physxInterface.get_vehicle_drive_state(vehiclePrim.GetPath().pathString)

                    longitudinalSlip = math.fabs(wheelState[VEHICLE_WHEEL_STATE_TIRE_LONGITUDINAL_SLIP])
                    lateralSlip = math.fabs(wheelState[VEHICLE_WHEEL_STATE_TIRE_LATERAL_SLIP])
                    slip = max(longitudinalSlip, lateralSlip)
                    slip = min(max(slip, 0.0), 1.0)

                    wheelRotationSpeed = wheelState[VEHICLE_WHEEL_STATE_ROTATION_SPEED]
                    wheelSpeedMPH = tireRadius * math.fabs(wheelRotationSpeed) * CMPS_TO_MPH

                    speedMPH = wheelSpeedMPH

                    rpm = driveState[VEHICLE_DRIVE_STATE_ENGINE_ROTATION_SPEED] * RPS_TO_RPM
                    rpm = min(max(rpm, self._minRPM), self._maxRPM)

                    currentNote = rpm * rpm * NOTE2 + rpm * NOTE1 + NOTE0

                    if (vehicleSound._soundType == SoundType.engine):
                        # Engine sounds
                        loadGain = throttle

                        if (vehicleSound._lowLoad):
                            loadGain = 1 - throttle

                        deltaRPM = rpm - vehicleSound._rpm
                        rpmGain = gaussian(GAUSSIAN_A, GAUSSIAN_B, GAUSSIAN_C, deltaRPM)

                        gain = loadGain * rpmGain

                        soundNote = vehicleSound._rpm * vehicleSound._rpm * NOTE2 + vehicleSound._rpm * NOTE1 + NOTE0
                        semitoneDelta = currentNote - soundNote
                        timeScale = math.pow(2.0, semitoneDelta / 12.0)

                    elif (vehicleSound._soundType == SoundType.tire):
                        # Tire rolling sounds
                        if (vehicleSound._speed == 10.0):
                            if (speedMPH < 10.0):
                                gain = Lerp(0.0, 0.0, 10.0, 0.7, speedMPH)
                                semitoneDelta = Lerp(0.0, -2.0, 10.0, 0.0, speedMPH)
                            else:
                                gain = Lerp(10.0, 0.7, 20.0, 0.3, speedMPH)
                                semitoneDelta = Lerp(10.0, 0.0, 20.0, 2.0, speedMPH)

                        elif (vehicleSound._speed == 20.0):
                            if (speedMPH < 20.0):
                                gain = Lerp(10.0, 0.0, 20.0, 0.7, speedMPH)
                                semitoneDelta = Lerp(10.0, -2.0, 20.0, 0.0, speedMPH)
                            else:
                                gain = Lerp(20.0, 0.7, 60.0, 0.35, speedMPH)
                                semitoneDelta = Lerp(20.0, 0.0, 60.0, 3.0, speedMPH)

                        elif (vehicleSound._speed == 60.0):
                            gain = Lerp(0.0, 0.0, 30.0, 1.0, speedMPH)

                            if (speedMPH < 60.0):
                                semitoneDelta = Lerp(10.0, -2.0, 60.0, 0.0, speedMPH)
                            else:
                                semitoneDelta = Lerp(60.0, 0.0, 150.0, 2.0, speedMPH)

                        timeScale = math.pow(2.0, semitoneDelta / 12.0)

                    elif (vehicleSound._soundType == SoundType.skid):
                        # Tire skidding sounds
                        gain = self._slipFilter.filter(slip, deltaTime)

                    vehicleSound._sound.GetAttribute("gain").Set(gain)
                    vehicleSound._sound.GetAttribute("timeScale").Set(timeScale)
            else:
                self._firstUpdatePassed = True
        else:
            self._firstUpdatePassed = False


def _create_audio_prim(stage, audioFolder, rootPath, filename):
    sound = AudioSchema.Sound.Define(stage, rootPath + "/Vehicle/Audio/" + filename)
    sound.CreateFilePathAttr().Set(audioFolder + "/" + filename + ".wav")


def create(stage, physxInterface):
    # print(dir(AudioSchema.Sound))

    data_path = "../../../../../data/audio"

    audio_folder = os.path.abspath(os.path.normpath(os.path.join(__file__, data_path)))
    audio_folder = audio_folder.replace("\\", "/")
    # print(audio_folder)

    rootPath = str(stage.GetDefaultPrim().GetPath())

    # Engine sounds
    _create_audio_prim(stage, audio_folder, rootPath, "loww1000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww1500")
    _create_audio_prim(stage, audio_folder, rootPath, "loww2000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww2500")
    _create_audio_prim(stage, audio_folder, rootPath, "loww3000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww3500")
    _create_audio_prim(stage, audio_folder, rootPath, "loww4000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww4500")
    _create_audio_prim(stage, audio_folder, rootPath, "loww5000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww5500")
    _create_audio_prim(stage, audio_folder, rootPath, "loww6000")
    _create_audio_prim(stage, audio_folder, rootPath, "loww6500")

    _create_audio_prim(stage, audio_folder, rootPath, "high1000")
    _create_audio_prim(stage, audio_folder, rootPath, "high1500")
    _create_audio_prim(stage, audio_folder, rootPath, "high2000")
    _create_audio_prim(stage, audio_folder, rootPath, "high2500")
    _create_audio_prim(stage, audio_folder, rootPath, "high3000")
    _create_audio_prim(stage, audio_folder, rootPath, "high3500")
    _create_audio_prim(stage, audio_folder, rootPath, "high4000")
    _create_audio_prim(stage, audio_folder, rootPath, "high4500")
    _create_audio_prim(stage, audio_folder, rootPath, "high5000")
    _create_audio_prim(stage, audio_folder, rootPath, "high5500")
    _create_audio_prim(stage, audio_folder, rootPath, "high6000")
    _create_audio_prim(stage, audio_folder, rootPath, "high6500")

    # Skid sounds
    _create_audio_prim(stage, audio_folder, rootPath, "tireslip")

    # Drive on sounds
    _create_audio_prim(stage, audio_folder, rootPath, "driveon10")
    _create_audio_prim(stage, audio_folder, rootPath, "driveon20")
    _create_audio_prim(stage, audio_folder, rootPath, "driveon60")

    vehicleAudio = VehicleAudioSample(stage, physxInterface)