1- import argparse
2- import numpy as np
3- import genesis as gs
41import time
52import threading
3+
64from pynput import keyboard
5+ import numpy as np
6+
7+ import genesis as gs
78
89
910class DroneController :
1011 def __init__ (self ):
11- self .thrust = 14468.429183500699 # Base hover RPM - constant hover
12- self .rotation_delta = 200 # Differential RPM for rotation
13- self .thrust_delta = 10 # Amount to change thrust by when accelerating/decelerating
12+ self .thrust = 14475.8 # Base hover RPM - constant hover
13+ self .rotation_delta = 200.0 # Differential RPM for rotation
14+ self .thrust_delta = 10.0 # Amount to change thrust by when accelerating/decelerating
1415 self .running = True
1516 self .rpms = [self .thrust ] * 4
1617 self .pressed_keys = set ()
@@ -93,46 +94,37 @@ def update_thrust(self):
9394
9495def run_sim (scene , drone , controller ):
9596 while controller .running :
96- try :
97- # Update drone with current RPMs
98- rpms = controller .update_thrust ()
99- drone .set_propellels_rpm (rpms )
97+ # Update drone with current RPMs
98+ rpms = controller .update_thrust ()
99+ drone .set_propellels_rpm (rpms )
100100
101- # Update physics
102- scene .step ()
101+ # Update physics
102+ scene .step (refresh_visualizer = False )
103103
104- time .sleep (1 / 60 ) # Limit simulation rate
105- except Exception as e :
106- print (f"Error in simulation loop: { e } " )
107-
108- if scene .viewer :
109- scene .viewer .stop ()
104+ # Limit simulation rate
105+ time .sleep (1.0 / scene .viewer .max_FPS )
110106
111107
112108def main ():
113- parser = argparse .ArgumentParser ()
114- parser .add_argument ("-v" , "--vis" , action = "store_true" , default = True , help = "Enable visualization (default: True)" )
115- parser .add_argument ("-m" , "--mac" , action = "store_true" , default = False , help = "Running on MacOS (default: False)" )
116- args = parser .parse_args ()
117-
118109 # Initialize Genesis
119110 gs .init (backend = gs .cpu )
120111
121- # Create scene with initial camera view
122- viewer_options = gs .options .ViewerOptions (
123- camera_pos = (0.0 , - 4.0 , 2.0 ), # Now behind the drone (negative Y)
124- camera_lookat = (0.0 , 0.0 , 0.5 ),
125- camera_fov = 45 ,
126- max_FPS = 60 ,
127- )
128-
112+ # Create scene
129113 scene = gs .Scene (
130114 sim_options = gs .options .SimOptions (
131115 dt = 0.01 ,
132116 gravity = (0 , 0 , - 9.81 ),
133117 ),
134- viewer_options = viewer_options ,
135- show_viewer = args .vis ,
118+ viewer_options = gs .options .ViewerOptions (
119+ camera_pos = (0.0 , - 2.0 , 1.0 ),
120+ camera_lookat = (0.0 , 0.0 , 0.3 ),
121+ camera_fov = 45 ,
122+ max_FPS = 60 ,
123+ ),
124+ vis_options = gs .options .VisOptions (
125+ show_world_frame = False ,
126+ ),
127+ show_viewer = True ,
136128 )
137129
138130 # Add entities
@@ -146,38 +138,32 @@ def main():
146138
147139 scene .viewer .follow_entity (drone )
148140
149- # Build scene
150- scene .build ()
151-
152141 # Initialize controller
153142 controller = DroneController ()
154143
144+ # Start keyboard listener.
145+ # Note that instantiating the listener after building the scene causes segfault on MacOS.
146+ listener = keyboard .Listener (on_press = controller .on_press , on_release = controller .on_release )
147+ listener .start ()
148+
149+ # Build scene
150+ scene .build ()
151+
155152 # Print control instructions
156153 print ("\n Drone Controls:" )
157154 print ("↑ - Move Forward (North)" )
158155 print ("↓ - Move Backward (South)" )
159156 print ("← - Move Left (West)" )
160157 print ("→ - Move Right (East)" )
158+ print ("space - Increase RPM" )
159+ print ("shift - Decrease RPM" )
161160 print ("ESC - Quit\n " )
162161 print ("Initial hover RPM:" , controller .thrust )
163162
164- # Start keyboard listener
165- listener = keyboard .Listener (on_press = controller .on_press , on_release = controller .on_release )
166- listener .start ()
167-
168- if args .mac :
169- # Run simulation in another thread
170- sim_thread = threading .Thread (target = run_sim , args = (scene , drone , controller ))
171- sim_thread .start ()
172-
173- if args .vis :
174- scene .viewer .start ()
163+ # Run simulation in another thread
164+ threading .Thread (target = run_sim , args = (scene , drone , controller )).start ()
165+ scene .viewer .run ()
175166
176- # Wait for threads to finish
177- sim_thread .join ()
178- else :
179- # Run simulation in main thread
180- run_sim (scene , drone , controller )
181167 listener .stop ()
182168
183169
0 commit comments