diff --git a/README.md b/README.md index ec3ceb9..b7e60e0 100644 --- a/README.md +++ b/README.md @@ -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 | diff --git a/config.py b/config.py new file mode 100644 index 0000000..603f8fe --- /dev/null +++ b/config.py @@ -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) +} diff --git a/drone_controller.py b/drone_controller.py index ae6a338..1cff220 100644 --- a/drone_controller.py +++ b/drone_controller.py @@ -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 diff --git a/rover_controller.py b/rover_controller.py index fc0cc4b..cde0947 100644 --- a/rover_controller.py +++ b/rover_controller.py @@ -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 diff --git a/simulation_host.py b/simulation_host.py index 3837086..44ea3db 100644 --- a/simulation_host.py +++ b/simulation_host.py @@ -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: diff --git a/standalone_simulation.py b/standalone_simulation.py index 3556e4b..f307f8a 100644 --- a/standalone_simulation.py +++ b/standalone_simulation.py @@ -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