Gyro-Stabilization Platform - Digital Twin

Mechanical Engineering Senior Design Capstone Project: Autonomous GPS-denied Maritime Gimbal system that actively stabilizes for executing drone landings in rough seas on autonomous surface vehicles (USVs) & Wave Adaptive Modular Vehicles (WAM-Vs).

This repository contains a Unity-based digital twin simulation that replicates the physical control system 1:1. The control architecture mirrors the real-world system built with a Jetson Orin Nano, BNO08x IMU, and CubeMars actuators.

Control Reference: Marine_Docking_Platform

---

Architecture

The digital twin replicates the physical system's software stack 1:1 in Unity C#:

Physical System (Python)              Digital Twin (Unity C#)
─────────────────────────              ──────────────────────
main.py                        →      StabilizationController.cs
  50Hz control loop                      50Hz FixedUpdate loop
  state dict (angles, torques)           Inspector-exposed telemetry

pd_controller.py               →      PDController.cs
  Kp/Kd gains                           Kp/Kd gains (serialized)
  torque clamping (±3.0A)               torque clamping (±3.0A)
  derivative-on-error                    derivative-on-error

imu_sensor.py                  →      IMUSensor.cs
  BNO08x over I2C                       Simulated quaternion source
  quaternion→Euler (ZYX)                 quaternion→Euler (ZYX)
  I2C failure fallback                   Configurable failure rate

motor_driver.py                →      MotorActuator.cs
  CubeMars via SocketCAN                 Physics-based torque model
  arm/send_torque/stop                   arm/SendTorque/Stop
  position & current feedback            position & current feedback

System Block Diagram

┌─────────────────────────────────────────────────────────┐
│                  DigitalTwinManager                      │
│  (orchestrator, CAD import, simulation lifecycle)        │
└──────────┬──────────────┬──────────────┬────────────────┘
           │              │              │
     ┌─────▼─────┐  ┌────▼────┐  ┌──────▼──────┐
     │  Scene     │  │Stabili- │  │ TelemetryUI │
     │  Builder   │  │zation   │  │ + DataLogger│
     │            │  │Controller│  │             │
     └─────┬──┬──┘  └──┬───┬──┘  └─────────────┘
           │  │        │   │
    ┌──────▼┐ │   ┌────▼┐ ┌▼──────────┐
    │Ocean  │ │   │IMU  │ │MotorActua-│
    │Waves  │ │   │Senso│ │tor (x2)   │
    │       │ │   │r    │ │Roll+Pitch │
    └───────┘ │   └─────┘ └───────────┘
              │
        ┌─────▼─────┐
        │  Orbit     │
        │  Camera    │
        └────────────┘

Script Descriptions

Script	Location	Purpose
PDController.cs	Scripts/Core/	Pure math PD controller. Proportional + derivative with symmetric torque clamping. No hardware deps.
IMUSensor.cs	Scripts/Core/	Simulates BNO08x IMU. Quaternion-to-Euler (ZYX aerospace convention), Gaussian noise, I2C failure fallback.
MotorActuator.cs	Scripts/Core/	Simulates CubeMars motor. Torque-to-acceleration model with damping, arm/stop lifecycle, current monitoring.
StabilizationController.cs	Scripts/Core/	50Hz control loop. Reads IMU, computes PD, commands motors. Mirrors main.py exactly.
SceneBuilder.cs	Scripts/Environment/	Auto-generates ocean surface, directional sun, placeholder gimbal geometry, and orbit camera.
OceanWaveAnimator.cs	Scripts/Environment/	Superimposed sine waves creating roll/pitch/heave disturbances. Sea state 1-5 adjustable.
OrbitCamera.cs	Scripts/Environment/	Right-click orbit, scroll zoom, middle-click pan for scene inspection.
DigitalTwinManager.cs	Scripts/DigitalTwin/	Top-level orchestrator. Manages simulation lifecycle and CAD model hot-swap via inspector slot.
TelemetryUI.cs	Scripts/DigitalTwin/	On-screen HUD showing angles, torque commands, current draw, loop timing. Matches terminal output format.
DataLogger.cs	Scripts/DigitalTwin/	Exports 31-column telemetry CSV at 50Hz: wave, hull, hydro, IMU, gimbal, motor data.
CrestOceanIntegration.cs	Scripts/Environment/	Bridges Crest Ocean wave height/velocity/normal queries into the simulation. Falls back gracefully.
HydrodynamicsModel.cs	Scripts/Environment/	Computes buoyancy, drag, and wave-slope torques on the USV hull. Optional DWP2 integration.

---

Setup

Prerequisites

• Unity 2021.3+ (LTS recommended)
• 3D template (Built-in or URP)

Installation

1. Clone this repository:

   git clone https://github.com/click-b8/Gyro-Stabilization-Platform-.git

2. Open Unity Hub > Add > select the DigitalTwin folder

3. Open the project in Unity

Scene Setup

1. Create an empty scene

2. Create the following GameObjects and attach scripts:

   [DigitalTwinManager]           <- DigitalTwinManager.cs
     [SceneBuilder]               <- SceneBuilder.cs
     [StabilizationController]    <- StabilizationController.cs
     [TelemetryUI]                <- TelemetryUI.cs
     [DataLogger]                 <- DataLogger.cs
   

3. Press Play -- the scene auto-builds with ocean, lighting, and placeholder gimbal geometry

4. Wire references in the Inspector:

   • DigitalTwinManager: assign sceneBuilder, stabilizer, telemetryUI
   • StabilizationController: assign imu, rollMotor, pitchMotor
   • TelemetryUI: assign all references

5. Tune PD gains in StabilizationController inspector:

   • Roll PD: Kp=1.5, Kd=0.3, maxTorque=3.0
   • Pitch PD: Kp=1.5, Kd=0.3, maxTorque=3.0

Importing Your CAD Model

1. Export your CAD as .FBX (preferred), .OBJ, or .STEP
2. Drag the file into Assets/ in Unity
3. In the import settings, set Scale Factor (CAD in mm -> use 0.001)
4. Create a Prefab from the imported model
5. Assign it to the cadModelPrefab field on DigitalTwinManager
6. Press Play -- the CAD model auto-replaces the placeholder geometry

Tip: Structure your CAD export with separate meshes for each component (base hull, pitch frame, roll frame, landing surface) to enable per-component physics and control.

---

Controls

Key	Action
Space	Pause / Resume simulation
R	Reset simulation
T	Toggle telemetry HUD
Right-click + drag	Orbit camera
Scroll wheel	Zoom
Middle-click + drag	Pan

---

Data Output

Telemetry logs are saved to <ProjectRoot>/SimulationLogs/ as CSV with 31 columns:

TIMING:       time_s, loop_dt_s
WAVE:         sea_state, wave_height_m, wave_vel_x/y/z
HULL:         hull_roll_deg, hull_pitch_deg, hull_heave_m
HYDRO:        buoyancy_force_N, drag_force_N, hydro_torque_roll/pitch_Nm,
              disturbance_total_N, submerged_fraction
IMU:          imu_roll_deg, imu_pitch_deg, imu_yaw_deg
GIMBAL:       target_roll/pitch_deg, gimbal_roll/pitch_actual_deg,
              gimbal_roll/pitch_error_deg
MOTOR:        motor_roll/pitch_torque_cmd_A, motor_roll/pitch_current_A,
              motor_roll/pitch_position_deg

This data enables:

• Plotting motor torque vs sea state to find saturation limits
• Comparing PD gain sets across wave conditions
• Validating against physical system test data
• Finding the sea state threshold where stabilization breaks down

Ocean and Physics Packages

Crest Ocean System (Free)

Provides real wave spectrum data, wave height queries at any point, and buoyancy.

Install via Package Manager > Add Git URL:

https://github.com/wave-harmonic/crest.git?path=/crest/Assets/Crest/Crest

After installing, uncomment #define CREST_INSTALLED in CrestOceanIntegration.cs.

Dynamic Water Physics 2 (Optional, Asset Store)

Provides per-vertex buoyancy forces, submerged volume, and hydrodynamic drag/torque data.

After installing, uncomment #define DWP2_INSTALLED in HydrodynamicsModel.cs.

Standalone Mode

Both scripts work without their packages installed -- they fall back to the built-in sine-wave ocean and simplified buoyancy model. No configuration needed.

---

Tuning Parameters

Parameter	Location	Default	Description
Kp	StabilizationController	1.5	Proportional gain
Kd	StabilizationController	0.3	Derivative gain
maxTorque	StabilizationController	3.0 A	Torque saturation
controlRateHz	StabilizationController	50 Hz	Control loop rate
seaState	OceanWaveAnimator	2.0	Wave intensity (1-5)
anglNoiseSigma	IMUSensor	0.1 deg	Sensor noise level
i2cFailRate	IMUSensor	1%	Simulated comm failures

---

License

This project is for research and development purposes.