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

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