SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added Config.py

Browse Source
This commit is contained in:
default
2026-01-02 07:07:51 +00:00
parent 924d803e71
commit 93fe686fab
6 changed files with 340 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
README.md
View File

@@ -64,6 +64,7 @@ That's it! Only 2 terminals needed.
| `simulation_host.py` | PyBullet simulator server |
| `run_bridge.py` | **PyBullet bridge + Controllers** |
| `run_gazebo.py` | **Gazebo bridge + Controllers** |
| `config.py` | **Configuration file** (edit to customize) |
| `drone_controller.py` | Drone landing logic (edit this) |
| `rover_controller.py` | Moving landing pad |

103
config.py Normal file
View File

@@ -0,0 +1,103 @@
# =============================================================================
# DRONE CONFIGURATION
# =============================================================================
DRONE = {
# Physical properties
"mass": 1.0, # kg
"size": (0.3, 0.3, 0.1), # (width, depth, height) in meters
"color": (0.8, 0.1, 0.1, 1.0), # RGBA (red)
# Starting position
"start_position": (0.0, 0.0, 5.0), # (x, y, z) in meters
# Control limits
"max_thrust": 1.0, # Maximum thrust command (-1 to 1)
"max_pitch": 0.5, # Maximum pitch command
"max_roll": 0.5, # Maximum roll command
"max_yaw": 1.0, # Maximum yaw command
# Thrust/torque scaling
"thrust_scale": 15.0, # Force multiplier for thrust
"pitch_torque_scale": 2.0, # Torque multiplier for pitch
"roll_torque_scale": 2.0, # Torque multiplier for roll
"yaw_torque_scale": 1.0, # Torque multiplier for yaw
}
# =============================================================================
# ROVER (LANDING PAD) CONFIGURATION
# =============================================================================
ROVER = {
# Physical properties
"size": (1.0, 1.0, 0.3), # (width, depth, height) in meters
"color": (0.2, 0.6, 0.2, 1.0), # RGBA (green)
# Starting position
"start_position": (0.0, 0.0, 0.15), # (x, y, z) in meters
# Movement patterns (for RoverController)
"default_pattern": "stationary", # stationary, linear, circular, square, random
"default_speed": 0.5, # m/s
"default_amplitude": 2.0, # meters (radius for circular, half-width for others)
}
# =============================================================================
# CAMERA CONFIGURATION
# =============================================================================
CAMERA = {
"width": 320,
"height": 240,
"fov": 60.0, # Field of view in degrees
"near_clip": 0.1, # Near clipping plane
"far_clip": 100.0, # Far clipping plane
"jpeg_quality": 70, # JPEG compression quality (1-100)
}
# =============================================================================
# PHYSICS CONFIGURATION
# =============================================================================
PHYSICS = {
"gravity": -9.81, # m/s² (negative = downward)
"timestep": 1.0 / 240.0, # Physics timestep (240 Hz)
"telemetry_rate": 24, # Telemetry updates per second
}
# =============================================================================
# NETWORK CONFIGURATION
# =============================================================================
NETWORK = {
"host": "0.0.0.0", # Listen on all interfaces
"command_port": 5555, # Port for receiving commands
"telemetry_port": 5556,# Port for sending telemetry
}
# =============================================================================
# LANDING DETECTION
# =============================================================================
LANDING = {
"success_distance": 0.5, # Maximum horizontal distance from pad center (m)
"success_velocity": 0.3, # Maximum landing velocity (m/s)
"height_threshold": 0.5, # Height considered "landed" (m)
}
# =============================================================================
# CONTROLLER GAINS (PD Controller)
# =============================================================================
CONTROLLER = {
# Altitude control
"Kp_z": 0.5, # Proportional gain for altitude
"Kd_z": 0.3, # Derivative gain for altitude
# Horizontal control
"Kp_xy": 0.3, # Proportional gain for horizontal position
"Kd_xy": 0.2, # Derivative gain for horizontal position
# Control rate
"rate": 50, # Control loop frequency (Hz)
}

46
drone_controller.py
View File

@@ -13,23 +13,39 @@ from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
# Load configuration
try:
from config import CONTROLLER, DRONE, LANDING
CONFIG_LOADED = True
except ImportError:
CONFIG_LOADED = False
CONTROLLER = {"Kp_z": 0.5, "Kd_z": 0.3, "Kp_xy": 0.3, "Kd_xy": 0.2, "rate": 50}
DRONE = {"max_thrust": 1.0, "max_pitch": 0.5, "max_roll": 0.5}
LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
class DroneController(Node):
"""Drone controller for GPS-denied landing."""
CONTROL_RATE = 50.0
LANDING_HEIGHT_THRESHOLD = 0.1
LANDING_VELOCITY_THRESHOLD = 0.1
MAX_THRUST = 1.0
MAX_PITCH = 0.5
MAX_ROLL = 0.5
MAX_YAW = 0.5
def __init__(self):
super().__init__('drone_controller')
# Load from config
self._control_rate = CONTROLLER.get("rate", 50.0)
self._Kp_z = CONTROLLER.get("Kp_z", 0.5)
self._Kd_z = CONTROLLER.get("Kd_z", 0.3)
self._Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
self._Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
self._max_thrust = DRONE.get("max_thrust", 1.0)
self._max_pitch = DRONE.get("max_pitch", 0.5)
self._max_roll = DRONE.get("max_roll", 0.5)
self._landing_height = LANDING.get("height_threshold", 0.1)
self._landing_velocity = LANDING.get("success_velocity", 0.1)
self.get_logger().info('=' * 50)
self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
if CONFIG_LOADED:
self.get_logger().info(' Configuration loaded from config.py')
self.get_logger().info('=' * 50)
self._latest_telemetry: Optional[Dict[str, Any]] = None
@@ -50,9 +66,9 @@ class DroneController(Node):
self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info(' Publishing to: /cmd_vel')
control_period = 1.0 / self.CONTROL_RATE
control_period = 1.0 / self._control_rate
self._control_timer = self.create_timer(control_period, self._control_loop)
self.get_logger().info(f' Control loop: {self.CONTROL_RATE} Hz')
self.get_logger().info(f' Control loop: {self._control_rate} Hz')
self.get_logger().info('Drone Controller Ready!')
self.get_logger().info('Sensors: IMU, Altimeter, Camera, Landing Pad Detection')
@@ -92,10 +108,10 @@ class DroneController(Node):
self._rover_telemetry
)
thrust = max(min(thrust, self.MAX_THRUST), -self.MAX_THRUST)
pitch = max(min(pitch, self.MAX_PITCH), -self.MAX_PITCH)
roll = max(min(roll, self.MAX_ROLL), -self.MAX_ROLL)
yaw = max(min(yaw, self.MAX_YAW), -self.MAX_YAW)
thrust = max(min(thrust, self._max_thrust), -self._max_thrust)
pitch = max(min(pitch, self._max_pitch), -self._max_pitch)
roll = max(min(roll, self._max_roll), -self._max_roll)
yaw = max(min(yaw, 0.5), -0.5)
self._publish_command(thrust, pitch, roll, yaw)
@@ -106,7 +122,7 @@ class DroneController(Node):
altimeter = self._latest_telemetry.get('altimeter', {})
altitude = altimeter.get('altitude', float('inf'))
vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf')))
return altitude < self.LANDING_HEIGHT_THRESHOLD and vertical_velocity < self.LANDING_VELOCITY_THRESHOLD
return altitude < self._landing_height and vertical_velocity < self._landing_velocity
except (KeyError, TypeError):
return False

42
rover_controller.py
View File

@@ -16,6 +16,14 @@ from geometry_msgs.msg import Twist, Point
from std_msgs.msg import String
import json
# Load configuration
try:
from config import ROVER
CONFIG_LOADED = True
except ImportError:
CONFIG_LOADED = False
ROVER = {"default_pattern": "stationary", "default_speed": 0.5, "default_amplitude": 2.0}
class MovementPattern(Enum):
STATIONARY = "stationary"
@@ -28,21 +36,22 @@ class MovementPattern(Enum):
class RoverController(Node):
"""Controls the rover movement."""
DEFAULT_PATTERN = MovementPattern.STATIONARY
DEFAULT_SPEED = 0.5
DEFAULT_AMPLITUDE = 2.0
CONTROL_RATE = 50.0
MAX_X = 5.0
MAX_Y = 5.0
def __init__(
self,
pattern: MovementPattern = DEFAULT_PATTERN,
speed: float = DEFAULT_SPEED,
amplitude: float = DEFAULT_AMPLITUDE
pattern: MovementPattern = None,
speed: float = None,
amplitude: float = None
):
super().__init__('rover_controller')
# Use config defaults if not specified
if pattern is None:
pattern = MovementPattern(ROVER.get("default_pattern", "stationary"))
if speed is None:
speed = ROVER.get("default_speed", 0.5)
if amplitude is None:
amplitude = ROVER.get("default_amplitude", 2.0)
self._pattern = pattern
self._speed = speed
self._amplitude = amplitude
@@ -55,6 +64,9 @@ class RoverController(Node):
self._target_y = 0.0
self._random_timer = 0.0
self._square_segment = 0
self._control_rate = 50.0
self._max_x = 5.0
self._max_y = 5.0
self.get_logger().info('=' * 50)
self.get_logger().info('Rover Controller Starting...')
@@ -69,21 +81,21 @@ class RoverController(Node):
self.get_logger().info(' Publishing to: /rover/cmd_vel, /rover/position, /rover/telemetry')
control_period = 1.0 / self.CONTROL_RATE
control_period = 1.0 / self._control_rate
self._control_timer = self.create_timer(control_period, self._control_loop)
self.get_logger().info('Rover Controller Ready!')
def _control_loop(self) -> None:
dt = 1.0 / self.CONTROL_RATE
dt = 1.0 / self._control_rate
self._time += dt
vel_x, vel_y = self._calculate_velocity()
self._position.x += vel_x * dt
self._position.y += vel_y * dt
self._position.x = max(-self.MAX_X, min(self.MAX_X, self._position.x))
self._position.y = max(-self.MAX_Y, min(self.MAX_Y, self._position.y))
self._position.x = max(-self._max_x, min(self._max_x, self._position.x))
self._position.y = max(-self._max_y, min(self._max_y, self._position.y))
cmd = Twist()
cmd.linear.x = vel_x
@@ -148,7 +160,7 @@ class RoverController(Node):
return (vel_x, vel_y)
def _pattern_random(self) -> Tuple[float, float]:
dt = 1.0 / self.CONTROL_RATE
dt = 1.0 / self._control_rate
self._random_timer += dt
if self._random_timer >= 3.0:
self._random_timer = 0.0

162
simulation_host.py
View File

@@ -16,46 +16,62 @@ import numpy as np
import pybullet as p
import pybullet_data
# Load configuration
try:
from config import DRONE, ROVER, CAMERA, PHYSICS, NETWORK
CONFIG_LOADED = True
except ImportError:
CONFIG_LOADED = False
DRONE = {"mass": 1.0, "size": (0.3, 0.3, 0.1), "color": (0.8, 0.1, 0.1, 1.0),
"start_position": (0.0, 0.0, 5.0), "thrust_scale": 15.0,
"pitch_torque_scale": 2.0, "roll_torque_scale": 2.0, "yaw_torque_scale": 1.0}
ROVER = {"size": (1.0, 1.0, 0.3), "color": (0.2, 0.6, 0.2, 1.0),
"start_position": (0.0, 0.0, 0.15)}
CAMERA = {"width": 320, "height": 240, "fov": 60.0, "near_clip": 0.1, "far_clip": 20.0}
PHYSICS = {"gravity": -9.81, "timestep": 1/240, "telemetry_rate": 24}
NETWORK = {"host": "0.0.0.0", "command_port": 5555, "telemetry_port": 5556}
class DroneSimulator:
"""PyBullet-based drone simulation with camera."""
UDP_HOST = '0.0.0.0'
UDP_PORT = 5555
UDP_BUFFER_SIZE = 65535
SOCKET_TIMEOUT = 0.001
TELEMETRY_PORT = 5556
PHYSICS_TIMESTEP = 1.0 / 240.0
GRAVITY = -9.81
TELEMETRY_INTERVAL = 10
DRONE_MASS = 1.0
DRONE_SIZE = (0.3, 0.3, 0.1)
DRONE_START_POS = (0.0, 0.0, 5.0)
THRUST_SCALE = 15.0
PITCH_TORQUE_SCALE = 2.0
ROLL_TORQUE_SCALE = 2.0
YAW_TORQUE_SCALE = 1.0
HOVER_THRUST = DRONE_MASS * abs(GRAVITY)
ROVER_SIZE = (1.0, 1.0, 0.3)
ROVER_POS = (0.0, 0.0, 0.15)
# Camera parameters
CAMERA_WIDTH = 320
CAMERA_HEIGHT = 240
CAMERA_FOV = 60.0
CAMERA_NEAR = 0.1
CAMERA_FAR = 20.0
CAMERA_INTERVAL = 5 # Capture every N telemetry frames
def __init__(self):
print("=" * 60)
print("Drone Simulation Host (GPS-Denied + Camera)")
if CONFIG_LOADED:
print(" Configuration loaded from config.py")
print("=" * 60)
# Load from config
self._udp_host = NETWORK.get("host", "0.0.0.0")
self._udp_port = NETWORK.get("command_port", 5555)
self._telemetry_port = NETWORK.get("telemetry_port", 5556)
self._gravity = PHYSICS.get("gravity", -9.81)
self._timestep = PHYSICS.get("timestep", 1/240)
self._telemetry_interval = int(240 / PHYSICS.get("telemetry_rate", 24))
self._drone_mass = DRONE.get("mass", 1.0)
self._drone_size = DRONE.get("size", (0.3, 0.3, 0.1))
self._drone_start = DRONE.get("start_position", (0.0, 0.0, 5.0))
self._drone_color = DRONE.get("color", (0.8, 0.1, 0.1, 1.0))
self._thrust_scale = DRONE.get("thrust_scale", 15.0)
self._pitch_torque_scale = DRONE.get("pitch_torque_scale", 2.0)
self._roll_torque_scale = DRONE.get("roll_torque_scale", 2.0)
self._yaw_torque_scale = DRONE.get("yaw_torque_scale", 1.0)
self._hover_thrust = self._drone_mass * abs(self._gravity)
self._rover_size = ROVER.get("size", (1.0, 1.0, 0.3))
self._rover_start = list(ROVER.get("start_position", (0.0, 0.0, 0.15)))
self._rover_color = ROVER.get("color", (0.2, 0.6, 0.2, 1.0))
self._camera_width = CAMERA.get("width", 320)
self._camera_height = CAMERA.get("height", 240)
self._camera_fov = CAMERA.get("fov", 60.0)
self._camera_near = CAMERA.get("near_clip", 0.1)
self._camera_far = CAMERA.get("far_clip", 20.0)
self._camera_interval = 5
self._running = True
self._step_count = 0
self._camera_frame_count = 0
@@ -63,7 +79,7 @@ class DroneSimulator:
'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0
}
self._rover_pos = list(self.ROVER_POS)
self._rover_pos = list(self._rover_start)
self._last_camera_image = None
self._init_physics()
@@ -72,15 +88,15 @@ class DroneSimulator:
print("Simulation Ready! (GPS-Denied Mode)")
print(" Sensors: IMU, Altimeter, Camera")
print(f" Camera: {self.CAMERA_WIDTH}x{self.CAMERA_HEIGHT} @ {self.CAMERA_FOV}° FOV")
print(f" Listening on UDP port {self.UDP_PORT}")
print(f" Camera: {self._camera_width}x{self._camera_height} @ {self._camera_fov}° FOV")
print(f" Listening on UDP port {self._udp_port}")
print("=" * 60)
def _init_physics(self) -> None:
print("Initializing PyBullet physics engine...")
self._physics_client = p.connect(p.GUI)
p.setGravity(0, 0, self.GRAVITY)
p.setTimeStep(self.PHYSICS_TIMESTEP)
p.setGravity(0, 0, self._gravity)
p.setTimeStep(self._timestep)
p.resetDebugVisualizerCamera(
cameraDistance=8.0,
cameraYaw=45,
@@ -98,47 +114,47 @@ class DroneSimulator:
rover_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE]
halfExtents=[s/2 for s in self._rover_size]
)
rover_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE],
rgbaColor=[0.2, 0.6, 0.2, 1.0]
halfExtents=[s/2 for s in self._rover_size],
rgbaColor=list(self._rover_color)
)
self._rover_id = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=rover_collision,
baseVisualShapeIndex=rover_visual,
basePosition=self.ROVER_POS
basePosition=self._rover_start
)
print(f" Rover created at position {self.ROVER_POS}")
print(f" Rover created at position {self._rover_start}")
self._create_landing_pad_marker()
drone_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE]
halfExtents=[s/2 for s in self._drone_size]
)
drone_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE],
rgbaColor=[0.8, 0.2, 0.2, 1.0]
halfExtents=[s/2 for s in self._drone_size],
rgbaColor=list(self._drone_color)
)
self._drone_id = p.createMultiBody(
baseMass=self.DRONE_MASS,
baseMass=self._drone_mass,
baseCollisionShapeIndex=drone_collision,
baseVisualShapeIndex=drone_visual,
basePosition=self.DRONE_START_POS
basePosition=self._drone_start
)
p.changeDynamics(
self._drone_id, -1,
linearDamping=0.1,
angularDamping=0.5
)
print(f" Drone created at position {self.DRONE_START_POS}")
print(f" Drone created at position {self._drone_start}")
def _create_landing_pad_marker(self) -> None:
marker_height = self.ROVER_POS[2] + self.ROVER_SIZE[2] / 2 + 0.01
marker_height = self._rover_start[2] + self._rover_size[2] / 2 + 0.01
h_size = 0.3
line_color = [1, 1, 1]
line_width = 3
@@ -166,10 +182,10 @@ class DroneSimulator:
print("Initializing network...")
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._socket.settimeout(self.SOCKET_TIMEOUT)
self._socket.bind((self.UDP_HOST, self.UDP_PORT))
self._socket.settimeout(0.001)
self._socket.bind((self._udp_host, self._udp_port))
self._controller_addr: Optional[Tuple[str, int]] = None
print(f" UDP socket bound to {self.UDP_HOST}:{self.UDP_PORT}")
print(f" UDP socket bound to {self._udp_host}:{self._udp_port}")
def run(self) -> None:
print("\nStarting simulation loop...")
@@ -188,11 +204,11 @@ class DroneSimulator:
p.stepSimulation()
self._step_count += 1
if self._step_count % self.TELEMETRY_INTERVAL == 0:
if self._step_count % self._telemetry_interval == 0:
self._send_telemetry()
elapsed = time.time() - loop_start
sleep_time = self.PHYSICS_TIMESTEP - elapsed
sleep_time = self._timestep - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
@@ -203,8 +219,8 @@ class DroneSimulator:
def _receive_commands(self) -> None:
try:
data, addr = self._socket.recvfrom(self.UDP_BUFFER_SIZE)
self._controller_addr = (addr[0], self.TELEMETRY_PORT)
data, addr = self._socket.recvfrom(65535)
self._controller_addr = (addr[0], self._telemetry_port)
try:
command = json.loads(data.decode('utf-8'))
@@ -244,7 +260,7 @@ class DroneSimulator:
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
thrust_command = self._last_command['thrust']
total_thrust = self.HOVER_THRUST + (thrust_command * self.THRUST_SCALE)
total_thrust = self._hover_thrust + (thrust_command * self._thrust_scale)
total_thrust = max(0, total_thrust)
thrust_force = [
@@ -260,9 +276,9 @@ class DroneSimulator:
flags=p.WORLD_FRAME
)
pitch_torque = self._last_command['pitch'] * self.PITCH_TORQUE_SCALE
roll_torque = self._last_command['roll'] * self.ROLL_TORQUE_SCALE
yaw_torque = self._last_command['yaw'] * self.YAW_TORQUE_SCALE
pitch_torque = self._last_command['pitch'] * self._pitch_torque_scale
roll_torque = self._last_command['roll'] * self._roll_torque_scale
yaw_torque = self._last_command['yaw'] * self._yaw_torque_scale
p.applyExternalTorque(
self._drone_id, -1,
@@ -283,24 +299,24 @@ class DroneSimulator:
cameraUpVector=[math.cos(euler[2]), math.sin(euler[2]), 0]
)
aspect = self.CAMERA_WIDTH / self.CAMERA_HEIGHT
aspect = self._camera_width / self._camera_height
projection_matrix = p.computeProjectionMatrixFOV(
fov=self.CAMERA_FOV,
fov=self._camera_fov,
aspect=aspect,
nearVal=self.CAMERA_NEAR,
farVal=self.CAMERA_FAR
nearVal=self._camera_near,
farVal=self._camera_far
)
_, _, rgb, _, _ = p.getCameraImage(
width=self.CAMERA_WIDTH,
height=self.CAMERA_HEIGHT,
width=self._camera_width,
height=self._camera_height,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL
)
image = np.array(rgb, dtype=np.uint8).reshape(
self.CAMERA_HEIGHT, self.CAMERA_WIDTH, 4
self._camera_height, self._camera_width, 4
)[:, :, :3]
# Encode as base64 for transmission
@@ -330,7 +346,7 @@ class DroneSimulator:
if vertical_dist <= 0 or vertical_dist > 10.0:
return None
fov_rad = math.radians(self.CAMERA_FOV / 2)
fov_rad = math.radians(self._camera_fov / 2)
max_horizontal = vertical_dist * math.tan(fov_rad)
if horizontal_dist > max_horizontal:
@@ -369,7 +385,7 @@ class DroneSimulator:
self._camera_frame_count += 1
camera_image = None
if self._camera_frame_count >= self.CAMERA_INTERVAL:
if self._camera_frame_count >= self._camera_interval:
self._camera_frame_count = 0
camera_image = self._capture_camera_image()
@@ -388,7 +404,7 @@ class DroneSimulator:
"linear_acceleration": {
"x": 0.0,
"y": 0.0,
"z": round(-self.GRAVITY, 4)
"z": round(-self._gravity, 4)
}
},
"altimeter": {
@@ -402,13 +418,13 @@ class DroneSimulator:
},
"landing_pad": pad_detection,
"camera": {
"width": self.CAMERA_WIDTH,
"height": self.CAMERA_HEIGHT,
"fov": self.CAMERA_FOV,
"width": self._camera_width,
"height": self._camera_height,
"fov": self._camera_fov,
"image": camera_image # Base64 encoded JPEG (or None)
},
"landed": landed_on_rover,
"timestamp": round(self._step_count * self.PHYSICS_TIMESTEP, 4)
"timestamp": round(self._step_count * self._timestep, 4)
}
try:

146
standalone_simulation.py
View File

@@ -17,43 +17,76 @@ import numpy as np
import pybullet as p
import pybullet_data
# Load configuration
try:
from config import DRONE, ROVER, CAMERA, PHYSICS, CONTROLLER, LANDING
CONFIG_LOADED = True
except ImportError:
CONFIG_LOADED = False
# Defaults if config.py is missing
DRONE = {"mass": 1.0, "size": (0.3, 0.3, 0.1), "color": (0.8, 0.1, 0.1, 1.0),
"start_position": (0.0, 0.0, 5.0), "thrust_scale": 15.0,
"pitch_torque_scale": 2.0, "roll_torque_scale": 2.0, "yaw_torque_scale": 1.0}
ROVER = {"size": (1.0, 1.0, 0.3), "color": (0.2, 0.6, 0.2, 1.0),
"start_position": (0.0, 0.0, 0.15), "default_pattern": "stationary",
"default_speed": 0.5, "default_amplitude": 2.0}
CAMERA = {"fov": 60.0}
PHYSICS = {"gravity": -9.81, "timestep": 1/240}
CONTROLLER = {"Kp_z": 0.5, "Kd_z": 0.3, "Kp_xy": 0.3, "Kd_xy": 0.2}
LANDING = {"success_velocity": 0.5}
class StandaloneSimulation:
"""Complete simulation with built-in controller - no ROS 2 needed."""
PHYSICS_TIMESTEP = 1.0 / 240.0
GRAVITY = -9.81
CONTROL_INTERVAL = 5 # Apply control every N physics steps
DRONE_MASS = 1.0
DRONE_SIZE = (0.3, 0.3, 0.1)
DRONE_START_POS = (10.0, 0.0, 5.0)
THRUST_SCALE = 15.0
PITCH_TORQUE_SCALE = 2.0
ROLL_TORQUE_SCALE = 2.0
YAW_TORQUE_SCALE = 1.0
HOVER_THRUST = DRONE_MASS * abs(GRAVITY)
ROVER_SIZE = (1.0, 1.0, 0.3)
ROVER_START_POS = [0.0, 0.0, 0.15]
CAMERA_FOV = 60.0
def __init__(self, rover_pattern='stationary', rover_speed=0.5, rover_amplitude=2.0):
def __init__(self, rover_pattern=None, rover_speed=None, rover_amplitude=None):
print("=" * 60)
print("Standalone Drone Simulation (No ROS 2 Required)")
if CONFIG_LOADED:
print(" Configuration loaded from config.py")
print("=" * 60)
self._running = True
self._step_count = 0
self._time = 0.0
# Rover movement settings
self._rover_pattern = rover_pattern
self._rover_speed = rover_speed
self._rover_amplitude = rover_amplitude
self._rover_pos = list(self.ROVER_START_POS)
# Use config values or arguments
self._rover_pattern = rover_pattern or ROVER.get("default_pattern", "stationary")
self._rover_speed = rover_speed if rover_speed is not None else ROVER.get("default_speed", 0.5)
self._rover_amplitude = rover_amplitude if rover_amplitude is not None else ROVER.get("default_amplitude", 2.0)
self._rover_pos = list(ROVER.get("start_position", (0.0, 0.0, 0.15)))
# Physics constants from config
self._gravity = PHYSICS.get("gravity", -9.81)
self._timestep = PHYSICS.get("timestep", 1/240)
self._control_interval = 5
# Drone constants from config
self._drone_mass = DRONE.get("mass", 1.0)
self._drone_size = DRONE.get("size", (0.3, 0.3, 0.1))
self._drone_start = DRONE.get("start_position", (0.0, 0.0, 5.0))
self._drone_color = DRONE.get("color", (0.8, 0.1, 0.1, 1.0))
self._thrust_scale = DRONE.get("thrust_scale", 15.0)
self._pitch_torque_scale = DRONE.get("pitch_torque_scale", 2.0)
self._roll_torque_scale = DRONE.get("roll_torque_scale", 2.0)
self._yaw_torque_scale = DRONE.get("yaw_torque_scale", 1.0)
self._hover_thrust = self._drone_mass * abs(self._gravity)
# Rover constants from config
self._rover_size = ROVER.get("size", (1.0, 1.0, 0.3))
self._rover_color = ROVER.get("color", (0.2, 0.6, 0.2, 1.0))
# Camera
self._camera_fov = CAMERA.get("fov", 60.0)
# Controller gains
self._Kp_z = CONTROLLER.get("Kp_z", 0.5)
self._Kd_z = CONTROLLER.get("Kd_z", 0.3)
self._Kp_xy = CONTROLLER.get("Kp_xy", 0.3)
self._Kd_xy = CONTROLLER.get("Kd_xy", 0.2)
# Landing
self._landing_velocity = LANDING.get("success_velocity", 0.5)
# Control command
self._command = {'thrust': 0.0, 'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0}
@@ -61,15 +94,17 @@ class StandaloneSimulation:
self._init_physics()
self._init_objects()
print(f" Rover Pattern: {rover_pattern}")
print(f" Rover Speed: {rover_speed} m/s")
print(f" Drone Start: {self._drone_start}")
print(f" Rover Start: {self._rover_pos}")
print(f" Rover Pattern: {self._rover_pattern}")
print(f" Rover Speed: {self._rover_speed} m/s")
print(" Press Ctrl+C to exit")
print("=" * 60)
def _init_physics(self) -> None:
self._physics_client = p.connect(p.GUI)
p.setGravity(0, 0, self.GRAVITY)
p.setTimeStep(self.PHYSICS_TIMESTEP)
p.setGravity(0, 0, self._gravity)
p.setTimeStep(self._timestep)
p.resetDebugVisualizerCamera(
cameraDistance=8.0,
cameraYaw=45,
@@ -84,18 +119,18 @@ class StandaloneSimulation:
# Create rover (landing pad)
rover_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE]
halfExtents=[s/2 for s in self._rover_size]
)
rover_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.ROVER_SIZE],
rgbaColor=[0.2, 0.6, 0.2, 1.0]
halfExtents=[s/2 for s in self._rover_size],
rgbaColor=list(self._rover_color)
)
self._rover_id = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=rover_collision,
baseVisualShapeIndex=rover_visual,
basePosition=self.ROVER_START_POS
basePosition=self._rover_pos
)
# Create landing pad marker
@@ -104,18 +139,18 @@ class StandaloneSimulation:
# Create drone
drone_collision = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE]
halfExtents=[s/2 for s in self._drone_size]
)
drone_visual = p.createVisualShape(
p.GEOM_BOX,
halfExtents=[s/2 for s in self.DRONE_SIZE],
rgbaColor=[0.8, 0.2, 0.2, 1.0]
halfExtents=[s/2 for s in self._drone_size],
rgbaColor=list(self._drone_color)
)
self._drone_id = p.createMultiBody(
baseMass=self.DRONE_MASS,
baseMass=self._drone_mass,
baseCollisionShapeIndex=drone_collision,
baseVisualShapeIndex=drone_visual,
basePosition=self.DRONE_START_POS
basePosition=self._drone_start
)
p.changeDynamics(
self._drone_id, -1,
@@ -124,7 +159,7 @@ class StandaloneSimulation:
)
def _create_landing_marker(self) -> None:
marker_height = self.ROVER_START_POS[2] + self.ROVER_SIZE[2] / 2 + 0.01
marker_height = self._rover_pos[2] + self._rover_size[2] / 2 + 0.01
h_size = 0.3
line_color = [1, 1, 1]
@@ -149,14 +184,14 @@ class StandaloneSimulation:
self._update_rover()
# Run controller
if self._step_count % self.CONTROL_INTERVAL == 0:
if self._step_count % self._control_interval == 0:
self._run_controller()
# Apply physics
self._apply_controls()
p.stepSimulation()
self._step_count += 1
self._time += self.PHYSICS_TIMESTEP
self._time += self._timestep
# Check landing
if self._check_landing():
@@ -166,7 +201,7 @@ class StandaloneSimulation:
# Timing
elapsed = time.time() - loop_start
sleep_time = self.PHYSICS_TIMESTEP - elapsed
sleep_time = self._timestep - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
@@ -178,7 +213,7 @@ class StandaloneSimulation:
def _update_rover(self) -> None:
"""Move the rover based on pattern."""
dt = self.PHYSICS_TIMESTEP
dt = self._timestep
if self._rover_pattern == 'stationary':
return
@@ -230,7 +265,7 @@ class StandaloneSimulation:
landing_pad = None
if vertical_dist > 0 and vertical_dist < 10.0:
fov_rad = math.radians(self.CAMERA_FOV / 2)
fov_rad = math.radians(self._camera_fov / 2)
max_horizontal = vertical_dist * math.tan(fov_rad)
if horizontal_dist < max_horizontal:
landing_pad = {
@@ -267,20 +302,17 @@ class StandaloneSimulation:
# Altitude control
target_alt = 0.0
Kp_z, Kd_z = 0.5, 0.3
thrust = Kp_z * (target_alt - altitude) - Kd_z * vertical_vel
thrust = self._Kp_z * (target_alt - altitude) - self._Kd_z * vertical_vel
# Horizontal control
Kp_xy, Kd_xy = 0.3, 0.2
if landing_pad is not None:
target_x = landing_pad['relative_x']
target_y = landing_pad['relative_y']
pitch = Kp_xy * target_x - Kd_xy * vel_x
roll = Kp_xy * target_y - Kd_xy * vel_y
pitch = self._Kp_xy * target_x - self._Kd_xy * vel_x
roll = self._Kp_xy * target_y - self._Kd_xy * vel_y
else:
pitch = -Kd_xy * vel_x
roll = -Kd_xy * vel_y
pitch = -self._Kd_xy * vel_x
roll = -self._Kd_xy * vel_y
# Clamp
thrust = max(-1.0, min(1.0, thrust))
@@ -295,7 +327,7 @@ class StandaloneSimulation:
rot_matrix = p.getMatrixFromQuaternion(orn)
local_up = [rot_matrix[2], rot_matrix[5], rot_matrix[8]]
total_thrust = self.HOVER_THRUST + (self._command['thrust'] * self.THRUST_SCALE)
total_thrust = self._hover_thrust + (self._command['thrust'] * self._thrust_scale)
total_thrust = max(0, total_thrust)
thrust_force = [
@@ -314,9 +346,9 @@ class StandaloneSimulation:
p.applyExternalTorque(
self._drone_id, -1,
torqueObj=[
self._command['pitch'] * self.PITCH_TORQUE_SCALE,
self._command['roll'] * self.ROLL_TORQUE_SCALE,
self._command['yaw'] * self.YAW_TORQUE_SCALE
self._command['pitch'] * self._pitch_torque_scale,
self._command['roll'] * self._roll_torque_scale,
self._command['yaw'] * self._yaw_torque_scale
],
flags=p.LINK_FRAME
)
@@ -327,7 +359,7 @@ class StandaloneSimulation:
if len(contacts) > 0:
vel, _ = p.getBaseVelocity(self._drone_id)
speed = math.sqrt(vel[0]**2 + vel[1]**2 + vel[2]**2)
return speed < 0.5
return speed < self._landing_velocity
return False