File size: 12,102 Bytes
26fa66a | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 | """
Tether Dynamics Module
======================
Physics engine for cable tension, constraints, and snap mechanics.
This module handles:
- Spring-damper cable model (Kelvin-Voigt viscoelastic)
- Tension calculations under aerodynamic load
- Cable release/snap mechanics
- Whip prevention algorithms
"""
import numpy as np
from dataclasses import dataclass
from typing import Tuple, Optional
from enum import Enum
class CableState(Enum):
"""Cable operational states"""
ATTACHED = "attached" # Normal towing operation
RELEASING = "releasing" # In process of mechanical release
SEVERED = "severed" # Fully disconnected
SLACK = "slack" # Tension below minimum (dangerous)
@dataclass
class CableProperties:
"""Physical properties of tether cable"""
length: float # Nominal length (m)
diameter: float # Cable diameter (m)
breaking_strength: float # Maximum tension before snap (N)
stiffness: float # Young's modulus (N/m²)
damping: float # Damping ratio (dimensionless)
linear_density: float # Mass per unit length (kg/m)
max_tension: float # Operating limit (N)
min_tension: float # Slack threshold (N)
class TetherConstraint:
"""
Models a single tether cable between mother drone and TAB.
Uses spring-damper (Kelvin-Voigt) model for realistic stretch behavior:
F_tension = k * (L - L0) + c * (dL/dt)
Where:
k = stiffness coefficient
c = damping coefficient
L = current length
L0 = nominal length
dL/dt = rate of length change
"""
def __init__(self,
cable_id: str,
properties: CableProperties,
anchor_point_mother: np.ndarray,
anchor_point_tab: np.ndarray):
self.cable_id = cable_id
self.props = properties
self.state = CableState.ATTACHED
# Anchor points in local coordinates
self.anchor_mother = np.array(anchor_point_mother, dtype=np.float64)
self.anchor_tab = np.array(anchor_point_tab, dtype=np.float64)
# State variables
self.current_tension = 0.0
self.current_length = properties.length
self.stretch_rate = 0.0
# Calculate spring/damper coefficients from material properties
cross_section = np.pi * (properties.diameter / 2) ** 2
self.spring_k = (properties.stiffness * cross_section) / properties.length
self.damper_c = properties.damping * 2 * np.sqrt(self.spring_k * properties.linear_density * properties.length)
# History for rate calculation
self._prev_length = properties.length
self._prev_time = 0.0
def compute_tension_force(self,
pos_mother: np.ndarray,
pos_tab: np.ndarray,
vel_mother: np.ndarray,
vel_tab: np.ndarray,
dt: float) -> Tuple[np.ndarray, np.ndarray, float]:
"""
Calculate tension forces on both ends of the cable.
Returns:
force_on_mother: Force vector applied to mother drone (N)
force_on_tab: Force vector applied to TAB (N)
tension_magnitude: Scalar tension value (N)
"""
if self.state == CableState.SEVERED:
return np.zeros(3), np.zeros(3), 0.0
# Vector from mother to TAB
displacement = pos_tab - pos_mother
self.current_length = np.linalg.norm(displacement)
if self.current_length < 1e-6:
return np.zeros(3), np.zeros(3), 0.0
# Unit vector along cable
cable_direction = displacement / self.current_length
# Calculate stretch (extension beyond nominal length)
stretch = self.current_length - self.props.length
# Calculate stretch rate (for damping)
if dt > 0:
self.stretch_rate = (self.current_length - self._prev_length) / dt
self._prev_length = self.current_length
# Spring-damper force (Kelvin-Voigt model)
# Only apply tension when stretched (cables can't push)
if stretch > 0:
spring_force = self.spring_k * stretch
damper_force = self.damper_c * self.stretch_rate
tension_magnitude = max(0, spring_force + damper_force)
else:
# Cable is slack
tension_magnitude = 0.0
self.state = CableState.SLACK
# Clamp to operating limits
if tension_magnitude > self.props.breaking_strength:
# SNAP! Cable breaks under load
self._trigger_snap()
return np.zeros(3), np.zeros(3), 0.0
tension_magnitude = min(tension_magnitude, self.props.max_tension)
self.current_tension = tension_magnitude
# Check for slack state
if tension_magnitude < self.props.min_tension:
self.state = CableState.SLACK
elif self.state == CableState.SLACK:
self.state = CableState.ATTACHED
# Force vectors (equal and opposite)
force_on_mother = cable_direction * tension_magnitude # Pulls mother backward
force_on_tab = -cable_direction * tension_magnitude # Pulls TAB forward
return force_on_mother, force_on_tab, tension_magnitude
def release(self, whip_prevention: bool = True) -> np.ndarray:
"""
Execute cable release sequence.
Args:
whip_prevention: If True, applies damping to prevent cable striking mother
Returns:
final_momentum: Momentum vector transferred to TAB at release
"""
if self.state == CableState.SEVERED:
return np.zeros(3)
self.state = CableState.RELEASING
# Store the tension energy at moment of release
# This converts to kinetic energy in the TAB
stored_energy = 0.5 * self.spring_k * max(0, self.current_length - self.props.length) ** 2
if whip_prevention:
# Apply braking to cable retraction
# Mother drone retracts cable with controlled damping
pass # Would trigger servo/winch control
self.state = CableState.SEVERED
self.current_tension = 0.0
return stored_energy
def _trigger_snap(self):
"""Handle catastrophic cable failure"""
self.state = CableState.SEVERED
self.current_tension = 0.0
# In real sim, would spawn cable debris particles
def get_cable_strain(self) -> float:
"""Get current strain (stretch / original length)"""
return (self.current_length - self.props.length) / self.props.length
def get_cable_stress(self) -> float:
"""Get current stress (tension / cross-sectional area)"""
cross_section = np.pi * (self.props.diameter / 2) ** 2
return self.current_tension / cross_section
class TetherArray:
"""
Manages the complete array of tether cables for the KAPS system.
Handles:
- Multi-cable coordination
- Tangle detection/prevention
- Synchronized release sequences
- Formation constraint forces
"""
def __init__(self, cable_properties: CableProperties, num_cables: int = 4):
self.cables: dict[str, TetherConstraint] = {}
self.cable_props = cable_properties
# Default cross-formation anchor points on mother drone
# UP, DOWN, LEFT, RIGHT relative to drone body
anchor_offsets = {
"UP": np.array([0.0, 0.0, 0.5]),
"DOWN": np.array([0.0, 0.0, -0.5]),
"LEFT": np.array([0.0, -0.5, 0.0]),
"RIGHT": np.array([0.0, 0.5, 0.0]),
}
for i, (name, offset) in enumerate(anchor_offsets.items()):
if i >= num_cables:
break
self.cables[name] = TetherConstraint(
cable_id=name,
properties=cable_properties,
anchor_point_mother=offset,
anchor_point_tab=np.array([0.0, 0.0, 0.0]) # Center of TAB
)
def compute_all_forces(self,
mother_pos: np.ndarray,
mother_vel: np.ndarray,
tab_positions: dict[str, np.ndarray],
tab_velocities: dict[str, np.ndarray],
dt: float) -> dict:
"""
Compute tension forces for all cables simultaneously.
Returns dict with force data for each cable.
"""
results = {}
total_force_on_mother = np.zeros(3)
for cable_id, cable in self.cables.items():
if cable_id not in tab_positions:
continue
f_mother, f_tab, tension = cable.compute_tension_force(
mother_pos,
tab_positions[cable_id],
mother_vel,
tab_velocities.get(cable_id, np.zeros(3)),
dt
)
total_force_on_mother += f_mother
results[cable_id] = {
"force_on_mother": f_mother,
"force_on_tab": f_tab,
"tension": tension,
"state": cable.state,
"length": cable.current_length,
"strain": cable.get_cable_strain()
}
results["total_mother_force"] = total_force_on_mother
return results
def release_cable(self, cable_id: str) -> bool:
"""Release a specific cable by ID"""
if cable_id in self.cables:
self.cables[cable_id].release()
return True
return False
def release_all(self) -> dict[str, float]:
"""
Emergency release of all cables.
Used for the "speed burst" maneuver.
Returns energy released per cable.
"""
energies = {}
for cable_id, cable in self.cables.items():
energies[cable_id] = cable.release()
return energies
def check_tangle_risk(self, tab_positions: dict[str, np.ndarray]) -> list[Tuple[str, str]]:
"""
Detect if any cables are at risk of tangling.
Returns list of (cable1, cable2) pairs that are too close.
"""
tangle_pairs = []
cable_ids = list(tab_positions.keys())
for i, id1 in enumerate(cable_ids):
for id2 in cable_ids[i+1:]:
pos1 = tab_positions[id1]
pos2 = tab_positions[id2]
# Check if TABs are closer than safe distance
distance = np.linalg.norm(pos1 - pos2)
safe_distance = self.cable_props.length * 0.1 # 10% of cable length
if distance < safe_distance:
tangle_pairs.append((id1, id2))
return tangle_pairs
def get_total_drag_contribution(self) -> float:
"""
Estimate additional drag on mother drone from cable windage.
Returns approximate drag force in Newtons.
"""
total_drag = 0.0
cable_drag_coeff = 1.2 # Cylinder in crossflow
for cable in self.cables.values():
if cable.state == CableState.ATTACHED:
# Approximate cable as cylinder in airflow
# D = 0.5 * rho * V^2 * Cd * A
# Assuming 50 m/s airspeed, sea level density
airspeed = 50.0
rho = 1.225
area = cable.props.diameter * cable.current_length
drag = 0.5 * rho * airspeed**2 * cable_drag_coeff * area
total_drag += drag
return total_drag
|