242 lines
9.6 KiB
Python
242 lines
9.6 KiB
Python
"""
|
||
URDF Structure Visualizer for Lev Pod using PyBullet
|
||
Loads and displays the pod.urdf file in PyBullet's GUI
|
||
"""
|
||
|
||
import pybullet as p
|
||
import pybullet_data
|
||
import time
|
||
import os
|
||
|
||
# Initialize PyBullet in GUI mode
|
||
physicsClient = p.connect(p.GUI)
|
||
|
||
# Set up the simulation environment
|
||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||
p.setGravity(0, 0, 0)
|
||
|
||
# Configure camera view - looking at inverted maglev system
|
||
p.resetDebugVisualizerCamera(
|
||
cameraDistance=0.5,
|
||
cameraYaw=45,
|
||
cameraPitch=1, # Look up at the hanging pod
|
||
cameraTargetPosition=[0, 0, 0]
|
||
)
|
||
|
||
# Create the maglev track with collision physics (ABOVE, like a monorail)
|
||
# The track BOTTOM surface is at Z=0, pod hangs below
|
||
track_collision = p.createCollisionShape(
|
||
shapeType=p.GEOM_BOX,
|
||
halfExtents=[1.0, 0.2, 0.010] # 2m long × 0.4m wide × 2cm thick
|
||
)
|
||
track_visual = p.createVisualShape(
|
||
shapeType=p.GEOM_BOX,
|
||
halfExtents=[1.0, 0.2, 0.010],
|
||
rgbaColor=[0.3, 0.3, 0.3, 0.8] # Gray, semi-transparent
|
||
)
|
||
trackId = p.createMultiBody(
|
||
baseMass=0, # Static object
|
||
baseCollisionShapeIndex=track_collision,
|
||
baseVisualShapeIndex=track_visual,
|
||
basePosition=[0, 0, 0.010] # Track bottom at Z=0, center at Z=10mm
|
||
)
|
||
# Set track surface properties (steel)
|
||
p.changeDynamics(trackId, -1,
|
||
lateralFriction=0.3, # Steel-on-steel
|
||
restitution=0.1) # Minimal bounce
|
||
|
||
# Load the lev pod URDF
|
||
urdf_path = "pod.xml"
|
||
if not os.path.exists(urdf_path):
|
||
print(f"Error: Could not find {urdf_path}")
|
||
print(f"Current directory: {os.getcwd()}")
|
||
p.disconnect()
|
||
exit(1)
|
||
|
||
# INVERTED SYSTEM: Pod hangs BELOW track
|
||
# Track bottom is at Z=0
|
||
# Gap height = distance from track bottom DOWN to magnetic yoke (top of pod body)
|
||
# URDF collision spheres at +25mm from center = bolts that contact track from below
|
||
# URDF box top at +25mm from center = magnetic yoke
|
||
#
|
||
# For 10mm gap:
|
||
# - Yoke (top of pod at +25mm from center) should be at Z = 0 - 10mm = -10mm
|
||
# - Therefore: pod center = -10mm - 25mm = -35mm
|
||
# - Bolts (at +25mm from center) end up at: -35mm + 25mm = -10mm (touching track at Z=0)
|
||
start_pos = [0, 0, -0.10085] # Pod center 100.85mm BELOW track → yoke at 10mm gap
|
||
start_orientation = p.getQuaternionFromEuler([0, 0, 0])
|
||
podId = p.loadURDF(urdf_path, start_pos, start_orientation)
|
||
|
||
print("\n" + "=" * 60)
|
||
print("PyBullet URDF Visualizer")
|
||
print("=" * 60)
|
||
print(f"Loaded: {urdf_path}")
|
||
print(f"Position: {start_pos}")
|
||
print("\nControls:")
|
||
print(" • Mouse: Rotate view (left drag), Pan (right drag), Zoom (scroll)")
|
||
print(" • Ctrl+Mouse: Apply forces to the pod")
|
||
print(" • Press ESC or close window to exit")
|
||
print("=" * 60)
|
||
|
||
# Get and display URDF information
|
||
num_joints = p.getNumJoints(podId)
|
||
print(f"\nNumber of joints: {num_joints}")
|
||
|
||
# Get base info
|
||
base_mass, base_lateral_friction = p.getDynamicsInfo(podId, -1)[:2]
|
||
print(f"\nBase link:")
|
||
print(f" Mass: {base_mass} kg")
|
||
print(f" Lateral friction: {base_lateral_friction}")
|
||
|
||
# Get collision shape info
|
||
collision_shapes = p.getCollisionShapeData(podId, -1)
|
||
print(f"\nCollision shapes: {len(collision_shapes)}")
|
||
for i, shape in enumerate(collision_shapes):
|
||
shape_type = shape[2]
|
||
dimensions = shape[3]
|
||
local_pos = shape[5]
|
||
shape_names = {
|
||
p.GEOM_BOX: "Box",
|
||
p.GEOM_SPHERE: "Sphere",
|
||
p.GEOM_CAPSULE: "Capsule",
|
||
p.GEOM_CYLINDER: "Cylinder",
|
||
p.GEOM_MESH: "Mesh"
|
||
}
|
||
print(f" Shape {i}: {shape_names.get(shape_type, 'Unknown')}")
|
||
print(f" Dimensions: {dimensions}")
|
||
print(f" Position: {local_pos}")
|
||
|
||
# Enable visualization of collision shapes
|
||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
|
||
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
|
||
|
||
# Add coordinate frame visualization at the pod's origin
|
||
axis_length = 0.1
|
||
p.addUserDebugLine([0, 0, 0], [axis_length, 0, 0], [1, 0, 0], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1)
|
||
p.addUserDebugLine([0, 0, 0], [0, axis_length, 0], [0, 1, 0], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1)
|
||
p.addUserDebugLine([0, 0, 0], [0, 0, axis_length], [0, 0, 1], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1)
|
||
|
||
print("\n" + "=" * 60)
|
||
print("Visualization is running. Interact with the viewer...")
|
||
print("Close the PyBullet window to exit.")
|
||
print("=" * 60 + "\n")
|
||
|
||
# Store collision shape local positions and types for tracking
|
||
collision_local_positions = []
|
||
collision_types = []
|
||
for shape in collision_shapes:
|
||
collision_local_positions.append(shape[5]) # Local position (x, y, z)
|
||
collision_types.append(shape[2]) # Shape type (BOX, CYLINDER, etc.)
|
||
|
||
# Identify the 4 yoke top collision boxes and 4 sensor cylinders
|
||
# Both are at Z ≈ 0.08585m, but yokes are BOXES and sensors are CYLINDERS
|
||
yoke_indices = []
|
||
yoke_labels = []
|
||
sensor_indices = []
|
||
sensor_labels = []
|
||
|
||
for i, (local_pos, shape_type) in enumerate(zip(collision_local_positions, collision_types)):
|
||
# Check if at sensor/yoke height (Z ≈ 0.08585m)
|
||
if abs(local_pos[2] - 0.08585) < 0.001: # Within 1mm tolerance
|
||
if shape_type == p.GEOM_BOX:
|
||
# Yoke collision boxes (BOX shapes)
|
||
yoke_indices.append(i)
|
||
x_pos = "Front" if local_pos[0] > 0 else "Back"
|
||
y_pos = "Right" if local_pos[1] > 0 else "Left"
|
||
yoke_labels.append(f"{x_pos} {y_pos}")
|
||
elif shape_type == p.GEOM_CYLINDER or shape_type == p.GEOM_MESH:
|
||
# Sensor cylinders (may be loaded as MESH by PyBullet)
|
||
# Distinguish from yokes by X or Y position patterns
|
||
# Yokes have both X and Y non-zero, sensors have one coordinate near zero
|
||
if abs(local_pos[0]) < 0.06 or abs(local_pos[1]) < 0.02:
|
||
sensor_indices.append(i)
|
||
# Label sensors by position
|
||
if abs(local_pos[0]) < 0.001: # X ≈ 0 (center sensors)
|
||
label = "Center Right" if local_pos[1] > 0 else "Center Left"
|
||
else:
|
||
label = "Front" if local_pos[0] > 0 else "Back"
|
||
sensor_labels.append(label)
|
||
|
||
print(f"\nIdentified {len(yoke_indices)} yoke collision boxes for gap height tracking")
|
||
print(f"Identified {len(sensor_indices)} sensor cylinders for gap height tracking")
|
||
|
||
# Run the simulation with position tracking
|
||
import numpy as np
|
||
step_count = 0
|
||
print_interval = 240 # Print every second (at 240 Hz)
|
||
|
||
try:
|
||
while p.isConnected():
|
||
p.stepSimulation()
|
||
|
||
# Extract positions periodically
|
||
if step_count % print_interval == 0:
|
||
# Get pod base position and orientation
|
||
pos, orn = p.getBasePositionAndOrientation(podId)
|
||
|
||
# Convert quaternion to rotation matrix
|
||
rot_matrix = p.getMatrixFromQuaternion(orn)
|
||
rot_matrix = np.array(rot_matrix).reshape(3, 3)
|
||
|
||
# Calculate world positions of yoke tops and sensors
|
||
print(f"\n--- Time: {step_count/240:.2f}s ---")
|
||
print(f"Pod center: [{pos[0]*1000:.1f}, {pos[1]*1000:.1f}, {pos[2]*1000:.1f}] mm")
|
||
|
||
print("\nYoke Gap Heights:")
|
||
yoke_gap_heights = []
|
||
for i, yoke_idx in enumerate(yoke_indices):
|
||
local_pos = collision_local_positions[yoke_idx]
|
||
|
||
# Transform local position to world coordinates
|
||
local_vec = np.array(local_pos)
|
||
world_offset = rot_matrix @ local_vec
|
||
world_pos = np.array(pos) + world_offset
|
||
|
||
# Add 0.005m (5mm) to get top surface of yoke box (half-height of 10mm box)
|
||
yoke_top_z = world_pos[2] + 0.005
|
||
|
||
# Gap height: distance from track bottom (Z=0) down to yoke top
|
||
gap_height = 0.0 - yoke_top_z # Negative means below track
|
||
gap_height_mm = gap_height * 1000
|
||
yoke_gap_heights.append(gap_height_mm)
|
||
|
||
print(f" {yoke_labels[i]} yoke: pos=[{world_pos[0]*1000:.1f}, {world_pos[1]*1000:.1f}, {yoke_top_z*1000:.1f}] mm | Gap: {gap_height_mm:.2f} mm")
|
||
|
||
# Calculate average yoke gap height (for Ansys model input)
|
||
avg_yoke_gap = np.mean(yoke_gap_heights)
|
||
print(f" Average yoke gap: {avg_yoke_gap:.2f} mm")
|
||
|
||
print("\nSensor Gap Heights:")
|
||
sensor_gap_heights = []
|
||
for i, sensor_idx in enumerate(sensor_indices):
|
||
local_pos = collision_local_positions[sensor_idx]
|
||
|
||
# Transform local position to world coordinates
|
||
local_vec = np.array(local_pos)
|
||
world_offset = rot_matrix @ local_vec
|
||
world_pos = np.array(pos) + world_offset
|
||
|
||
# Add 0.005m (5mm) to get top surface of cylinder (half-length of 10mm cylinder)
|
||
sensor_top_z = world_pos[2] + 0.005
|
||
|
||
# Gap height: distance from track bottom (Z=0) down to sensor top
|
||
gap_height = 0.0 - sensor_top_z
|
||
gap_height_mm = gap_height * 1000
|
||
sensor_gap_heights.append(gap_height_mm)
|
||
|
||
print(f" {sensor_labels[i]} sensor: pos=[{world_pos[0]*1000:.1f}, {world_pos[1]*1000:.1f}, {sensor_top_z*1000:.1f}] mm | Gap: {gap_height_mm:.2f} mm")
|
||
|
||
# Calculate average sensor gap height
|
||
avg_sensor_gap = np.mean(sensor_gap_heights)
|
||
print(f" Average sensor gap: {avg_sensor_gap:.2f} mm")
|
||
|
||
step_count += 1
|
||
time.sleep(1./240.) # Run at 240 Hz
|
||
|
||
except KeyboardInterrupt:
|
||
print("\nExiting...")
|
||
|
||
p.disconnect()
|
||
print("PyBullet session closed.")
|