Skip to content

Commit 43b11df

Browse files
YilingQiaoYilingQiao
authored
Merge pull request #211 from PieterBecking/feature/interactive-drone-control
Interactive drone controlled - cross-platform
2 parents 1294ea7 + f173f61 commit 43b11df

File tree

4 files changed

+397
-5
lines changed

4 files changed

+397
-5
lines changed

‎examples/drone/README.md

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
# Drone Examples
2+
3+
This directory contains examples of drone simulations using the Genesis framework.
4+
5+
## Available Examples
6+
7+
### 1. Interactive Drone (`interactive_drone.py`)
8+
A real-time interactive drone simulation where you can control the drone using keyboard inputs:
9+
- ↑ (Up Arrow): Move Forward (North)
10+
- ↓ (Down Arrow): Move Backward (South)
11+
- ← (Left Arrow): Move Left (West)
12+
- → (Right Arrow): Move Right (East)
13+
- Space: Increase Thrust (Accelerate)
14+
- Left Shift: Decrease Thrust (Decelerate)
15+
- ESC: Quit
16+
17+
Run with:
18+
```bash
19+
python interactive_drone.py -v -m
20+
```
21+
22+
### 2. Automated Flight (`fly.py`)
23+
A pre-programmed drone flight simulation that follows a predefined trajectory stored in `fly_traj.pkl`.
24+
25+
Run with:
26+
```bash
27+
python fly.py -v -m
28+
```
29+
30+
## Technical Details
31+
32+
- The drone model used is the Crazyflie 2.X (`urdf/drones/cf2x.urdf`)
33+
- Base hover RPM is approximately 14468
34+
- Movement is achieved by varying individual rotor RPMs to create directional thrust
35+
- The simulation uses realistic physics including gravity and aerodynamics
36+
- Visualization is optimized for macOS using threaded rendering when run with `-m` flag
37+
38+
## Controls Implementation
39+
40+
The interactive drone uses differential RPM control:
41+
- Forward/Backward movement: Adjusts front/back rotor pairs
42+
- Left/Right movement: Adjusts left/right rotor pairs
43+
- All movements maintain a stable hover while creating directional thrust
44+
- RPM changes are automatically clipped to safe ranges (0-25000 RPM)

‎examples/drone/fly.py

+172-5
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,13 @@
11
import argparse
2-
import os
3-
import pickle as pkl
4-
52
import numpy as np
63

74
import genesis as gs
85

96

107
def main():
11-
128
parser = argparse.ArgumentParser()
139
parser.add_argument("-v", "--vis", action="store_true", default=False)
10+
parser.add_argument("-m", "--mac", action="store_true", default=False)
1411
args = parser.parse_args()
1512

1613
########################## init ##########################
@@ -45,12 +42,182 @@ def main():
4542

4643
########################## build ##########################
4744
scene.build()
48-
traj = pkl.load(open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "fly_traj.pkl"), "rb"))
45+
46+
if args.mac:
47+
gs.tools.run_in_another_thread(fn=run_sim, args=(scene, drone, args.vis))
48+
if args.vis:
49+
scene.viewer.start()
50+
else:
51+
run_sim(scene, drone, args.vis)
52+
53+
54+
def run_sim(scene, drone, enable_vis):
55+
traj = np.array(
56+
[
57+
[1.0, 1.0, 0.98824805, 1.0],
58+
[0.67815816, 1.0, 1.0, 1.0],
59+
[1.0, 0.87905186, 0.8319297, 1.0],
60+
[1.0, 0.85295373, 0.94554883, 1.0],
61+
[1.0, 1.0, 0.9663153, 1.0],
62+
[1.0, 1.0, 1.0, 1.0],
63+
[1.0, 1.0, 1.0, 1.0],
64+
[1.0, 1.0, 1.0, 1.0],
65+
[1.0, 1.0, 1.0, 1.0],
66+
[1.0, 1.0, 1.0, 1.0],
67+
[1.0, 1.0, 1.0, 1.0],
68+
[1.0, 1.0, 1.0, 1.0],
69+
[1.0, 1.0, 1.0, 1.0],
70+
[1.0, 1.0, 1.0, 1.0],
71+
[1.0, 1.0, 1.0, 1.0],
72+
[1.0, 1.0, 1.0, 1.0],
73+
[1.0, 1.0, 1.0, 1.0],
74+
[1.0, 1.0, 1.0, 1.0],
75+
[1.0, 1.0, 1.0, 1.0],
76+
[1.0, 1.0, 1.0, 1.0],
77+
[1.0, 1.0, 1.0, 1.0],
78+
[1.0, 1.0, 1.0, 1.0],
79+
[1.0, 1.0, 1.0, 1.0],
80+
[1.0, 1.0, 1.0, 1.0],
81+
[1.0, 1.0, 1.0, 1.0],
82+
[1.0, 1.0, 1.0, 1.0],
83+
[1.0, 1.0, 1.0, 1.0],
84+
[1.0, 1.0, 1.0, 1.0],
85+
[1.0, 1.0, 1.0, 1.0],
86+
[1.0, 1.0, 1.0, 1.0],
87+
[1.0, 1.0, 1.0, 1.0],
88+
[1.0, 1.0, 1.0, 1.0],
89+
[1.0, 1.0, 1.0, 1.0],
90+
[1.0, 1.0, 1.0, 1.0],
91+
[1.0, 1.0, 1.0, 1.0],
92+
[1.0, 1.0, 1.0, 1.0],
93+
[1.0, 1.0, 1.0, 1.0],
94+
[1.0, 0.9954323, 1.0, 1.0],
95+
[1.0, 0.9974212, 1.0, 1.0],
96+
[1.0, 0.99529535, 1.0, 1.0],
97+
[1.0, 0.9965133, 1.0, 1.0],
98+
[1.0, 0.99495167, 1.0, 1.0],
99+
[1.0, 0.99533206, 1.0, 1.0],
100+
[1.0, 0.9941533, 1.0, 1.0],
101+
[1.0, 0.9937679, 1.0, 1.0],
102+
[1.0, 0.9926078, 1.0, 1.0],
103+
[1.0, 0.99150425, 1.0, 1.0],
104+
[1.0, 0.9899133, 1.0, 1.0],
105+
[1.0, 0.9879518, 1.0, 1.0],
106+
[1.0, 0.985401, 1.0, 0.93023926],
107+
[1.0, 0.9757207, 1.0, 0.8165948],
108+
[1.0, 0.94503635, 1.0, 0.47085124],
109+
[1.0, 0.9973584, 1.0, 0.92234856],
110+
[1.0, 0.97841257, 1.0, 0.9260282],
111+
[1.0, 0.9749091, 1.0, 0.8766382],
112+
[1.0, 0.9661152, 1.0, 0.8508391],
113+
[1.0, 0.9588664, 1.0, 0.8184431],
114+
[1.0, 0.9513355, 1.0, 0.78656846],
115+
[1.0, 0.9438352, 1.0, 0.7555151],
116+
[1.0, 0.9364986, 1.0, 0.7264303],
117+
[1.0, 0.92944163, 1.0, 0.70034355],
118+
[1.0, 0.9227477, 1.0, 0.6780561],
119+
[1.0, 0.91646177, 1.0, 0.6601221],
120+
[1.0, 0.91059643, 1.0, 0.6468646],
121+
[1.0, 0.90513545, 1.0, 0.6384181],
122+
[1.0, 0.9000414, 1.0, 0.63476765],
123+
[1.0, 0.8952549, 1.0, 0.6357777],
124+
[1.0, 0.89069635, 1.0, 0.64121217],
125+
[1.0, 0.8862596, 1.0, 0.6507421],
126+
[1.0, 0.8818036, 1.0, 0.66393715],
127+
[1.0, 0.8771375, 1.0, 0.68025357],
128+
[1.0, 0.8720022, 1.0, 0.6990145],
129+
[1.0, 0.8660441, 1.0, 0.719399],
130+
[1.0, 0.85878307, 1.0, 0.7404561],
131+
[1.0, 0.84957653, 1.0, 0.7611621],
132+
[1.0, 0.83757895, 1.0, 0.78056324],
133+
[1.0, 0.8216941, 1.0, 0.7980228],
134+
[1.0, 0.8005113, 1.0, 0.8135467],
135+
[0.99108833, 0.77218705, 1.0, 0.828064],
136+
[0.97618765, 0.7336579, 1.0, 0.8425417],
137+
[0.9570234, 0.6814261, 1.0, 0.8581086],
138+
[0.92717046, 0.6107281, 1.0, 0.8740242],
139+
[0.87264377, 0.51361734, 1.0, 0.8831364],
140+
[0.7667494, 0.37425363, 1.0, 0.86565703],
141+
[0.58684665, 0.15105894, 1.0, 0.792903],
142+
[0.40309954, -0.22192897, 1.0, 0.6778493],
143+
[0.30307913, -0.6645406, 0.32032692, 0.5561814],
144+
[0.1432502, -1.0, -0.7834326, 0.30731094],
145+
[-0.05076139, -1.0, -1.0, -0.18850122],
146+
[-0.2028995, -1.0, -1.0, -0.47833002],
147+
[-0.33243275, -1.0, -1.0, -0.63186795],
148+
[-0.43252927, -1.0, -0.93684345, -0.7109936],
149+
[-0.50198543, -1.0, -0.8966909, -0.7451998],
150+
[-0.55477273, -1.0, -0.87718576, -0.7572431],
151+
[-0.59963316, -1.0, -0.8707306, -0.7596289],
152+
[-0.641077, -1.0, -0.8736891, -0.7593429],
153+
[-0.68137753, -1.0, -0.8838504, -0.76042706],
154+
[-0.72137207, -1.0, -0.8991675, -0.7649133],
155+
[-0.76085263, -1.0, -0.9172805, -0.77330756],
156+
[-0.7989401, -1.0, -0.93559915, -0.78509957],
157+
[-0.8345167, -1.0, -0.95170647, -0.799334],
158+
[-0.86661166, -1.0, -0.963775, -0.8150858],
159+
[-0.8946256, -1.0, -0.97078484, -0.8317086],
160+
[-0.91837806, -1.0, -0.97249895, -0.84885603],
161+
[-0.9380378, -1.0, -0.96929866, -0.8663847],
162+
[-0.9540071, -1.0, -0.96197397, -0.8842322],
163+
[-0.9668097, -1.0, -0.9515317, -0.90233094],
164+
[-0.97700363, -1.0, -0.9390431, -0.9205734],
165+
[-0.98512334, -1.0, -0.92553586, -0.93881696],
166+
[-0.99164504, -1.0, -0.91191846, -0.9569116],
167+
[-0.99697274, -1.0, -0.8989406, -0.97473806],
168+
[-1.0, -1.0, -0.8871773, -0.99223274],
169+
[-1.0, -1.0, -0.87696224, -1.0],
170+
[-1.0, -1.0, -0.86855894, -1.0],
171+
[-1.0, -1.0, -0.8622101, -1.0],
172+
[-1.0, -1.0, -0.85795015, -1.0],
173+
[-1.0, -1.0, -0.8557587, -1.0],
174+
[-1.0, -1.0, -0.8555704, -1.0],
175+
[-1.0, -1.0, -0.8572821, -1.0],
176+
[-1.0, -1.0, -0.860755, -1.0],
177+
[-1.0, -1.0, -0.8658133, -1.0],
178+
[-1.0, -1.0, -0.87224096, -1.0],
179+
[-1.0, -1.0, -0.87977535, -1.0],
180+
[-1.0, -1.0, -0.8881058, -1.0],
181+
[-1.0, -1.0, -0.89687437, -1.0],
182+
[-1.0, -1.0, -0.9056818, -1.0],
183+
[-1.0, -1.0, -0.91409653, -1.0],
184+
[-1.0, -1.0, -0.9216669, -1.0],
185+
[-1.0, -1.0, -0.9279278, -1.0],
186+
[-1.0, -1.0, -0.93239695, -1.0],
187+
[-0.9961943, -1.0, -0.9345514, -1.0],
188+
[-0.9834586, -1.0, -0.93362767, -1.0],
189+
[-0.9671113, -1.0, -0.9284645, -1.0],
190+
[-0.94588476, -1.0, -0.91775376, -1.0],
191+
[-0.91785616, -1.0, -0.8995549, -1.0],
192+
[-0.88016766, -1.0, -0.87093955, -1.0],
193+
[-0.8287441, -1.0, -0.82749766, -1.0],
194+
[-0.7582472, -1.0, -0.76269644, -1.0],
195+
[-0.66290134, -1.0, -0.66715723, -1.0],
196+
[-0.5392508, -1.0, -0.5280629, -1.0],
197+
[-0.39078623, -1.0, -0.3290425, -1.0],
198+
[-0.2295668, -1.0, -0.05206226, -1.0],
199+
[-0.06826158, -1.0, 0.30915332, -1.0],
200+
[0.08895309, -1.0, 0.7070197, -1.0],
201+
[0.2400503, -1.0, 1.0, -1.0],
202+
[0.3742329, -1.0, 1.0, -1.0],
203+
[0.48094982, -1.0, 1.0, -1.0],
204+
[0.56609666, -1.0, 1.0, -1.0],
205+
[0.63677347, -0.88508135, 1.0, -1.0],
206+
[0.7058708, -0.694147, 1.0, -1.0],
207+
[0.7992784, -0.5113944, 1.0, -1.0],
208+
[0.9422653, -0.2919022, 1.0, -1.0],
209+
],
210+
dtype=np.float32,
211+
)
212+
49213
for i in range(len(traj)):
50214
# 14468 is hover rpm
51215
drone.set_propellels_rpm((1 + 0.05 * traj[i]) * 14468.429183500699)
52216
scene.step()
53217

218+
if enable_vis:
219+
scene.viewer.stop()
220+
54221

55222
if __name__ == "__main__":
56223
main()

‎examples/drone/fly_traj.pkl

-2.52 KB
Binary file not shown.

0 commit comments

Comments
 (0)