import math
class RigidBodyDynamics:
def __init__(self, mass=6.2, Ixx=0.05, Iyy=0.1, Izz=0.1):
self.mass = mass
self.Ixx = Ixx
self.Iyy = Iyy
self.Izz = Izz
def euler_equations(self, omega, torque):
wx, wy, wz = omega
tx, ty, tz = torque
dwx = (tx - (self.Izz - self.Iyy)*wy*wz) / self.Ixx
dwy = (ty - (self.Ixx - self.Izz)*wx*wz) / self.Iyy
dwz = (tz - (self.Iyy - self.Ixx)*wx*wy) / self.Izz
return (dwx, dwy, dwz)
def newton_equations(self, force):
ax = force[0] / self.mass
ay = force[1] / self.mass
az = force[2] / self.mass - 9.81
return (ax, ay, az)
def simulate_free_fall(self, dt=0.001, duration=0.5):
pos = [0, 0, 0.3]
vel = [0, 0, 0]
omega = [0, 0, 0]
trajectory = []
t = 0
while t < duration:
trajectory.append((t, pos[:], vel[:], omega[:]))
force = [0, 0, 0]
torque = [0, 0, 0]
acc = self.newton_equations(force)
for i in range(3):
vel[i] += acc[i] * dt
pos[i] += vel[i] * dt
domega = self.euler_equations(omega, torque)
for i in range(3):
omega[i] += domega[i] * dt
t += dt
if pos[2] <= 0:
break
return trajectory
def simulate_spin(self, dt=0.001, duration=1.0):
pos = [0, 0, 0]
vel = [0, 0, 0]
omega = [0, 0, 5.0] # initial spin
torque = [0.1, 0, 0] # small perturbation torque
trajectory = []
t = 0
while t < duration:
trajectory.append((t, omega[:]))
domega = self.euler_equations(omega, torque)
for i in range(3):
omega[i] += domega[i] * dt
t += dt
return trajectory
rbd = RigidBodyDynamics()
print("=" * 55)
print(" Rigid Body Dynamics Simulation")
print("=" * 55)
print(f" Mass: {rbd.mass} kg")
print(f" Ixx={rbd.Ixx}, Iyy={rbd.Iyy}, Izz={rbd.Izz} kg*m^2")
print()
traj = rbd.simulate_free_fall()
print(" [Free Fall from 0.3m]")
for i in range(0, len(traj), max(1, len(traj)//8)):
t, pos, vel, omega = traj[i]
print(f" t={t:.3f}s pos=({pos[0]:.4f},{pos[1]:.4f},{pos[2]:.4f}) vel=({vel[2]:.3f},0,0)")
print()
spin = rbd.simulate_spin()
print(" [Gyroscopic Precession - initial wz=5, torque tx=0.1]")
for i in range(0, len(spin), max(1, len(spin)//10)):
t, omega = spin[i]
print(f" t={t:.3f}s omega=({omega[0]:.4f},{omega[1]:.4f},{omega[2]:.4f})")
print()
# Lagrangian vs Newton-Euler comparison
print(" [Energy Conservation Check]")
ke = 0.5 * rbd.mass * 9.81**2 * 0.3**2 / (2*9.81*0.3)
print(f" Drop height 0.3m -> impact speed = {math.sqrt(2*9.81*0.3):.3f} m/s")
print(f" KE at impact = {0.5*rbd.mass*2*9.81*0.3:.2f} J")
print(f" PE at top = {rbd.mass*9.81*0.3:.2f} J")
print()
print(" OK - Rigid body dynamics simulation complete")
仿真结果：
=======================================================
Rigid Body Dynamics Simulation
=======================================================
Mass: 6.2 kg
Ixx=0.05, Iyy=0.1, Izz=0.1 kg*m^2
[Free Fall from 0.3m]
t=0.000s pos=(0.0000,0.0000,0.3000) vel=(0.000,0,0)
t=0.030s pos=(0.0000,0.0000,0.2954) vel=(-0.294,0,0)
t=0.060s pos=(0.0000,0.0000,0.2820) vel=(-0.589,0,0)
t=0.090s pos=(0.0000,0.0000,0.2598) vel=(-0.883,0,0)
t=0.120s pos=(0.0000,0.0000,0.2288) vel=(-1.177,0,0)
t=0.150s pos=(0.0000,0.0000,0.1889) vel=(-1.472,0,0)
t=0.180s pos=(0.0000,0.0000,0.1402) vel=(-1.766,0,0)
t=0.210s pos=(0.0000,0.0000,0.0827) vel=(-2.060,0,0)
t=0.240s pos=(0.0000,0.0000,0.0163) vel=(-2.354,0,0)
[Gyroscopic Precession - initial wz=5, torque tx=0.1]
t=0.000s omega=(0.0000,0.0000,5.0000)
t=0.100s omega=(0.2000,0.0247,4.9999)
t=0.200s omega=(0.4000,0.0995,4.9990)
t=0.300s omega=(0.6000,0.2242,4.9950)
t=0.400s omega=(0.8000,0.3986,4.9841)
t=0.500s omega=(1.0000,0.6221,4.9612)
t=0.600s omega=(1.2000,0.8937,4.9197)
t=0.700s omega=(1.4000,1.2112,4.8514)
t=0.800s omega=(1.6000,1.5711,4.7472)
t=0.900s omega=(1.8000,1.9683,4.5970)
[Energy Conservation Check]
Drop height 0.3m -> impact speed = 2.426 m/s
KE at impact = 18.25 J
PE at top = 18.25 J
OK - Rigid body dynamics simulation complete