Skip to content

Commit 5dced16

Browse files
author
Lui
committed
introduce a small negative offset in params_yaw_rate, switch ilqr to attitude interface
1 parent f929a31 commit 5dced16

File tree

3 files changed

+29
-1337
lines changed

3 files changed

+29
-1337
lines changed

‎crazyflow/sim/symbolic.py

+4-1
Original file line numberDiff line numberDiff line change
@@ -178,7 +178,10 @@ def symbolic(mass: float, J: NDArray, dt: float) -> SymbolicModel:
178178
params_acc = [20.907574256269616, 3.653687545690674]
179179
params_roll_rate = [-130.3, -16.33, 119.3]
180180
params_pitch_rate = [-99.94, -13.3, 84.73]
181-
params_yaw_rate = [0, 0, 0]
181+
# params_yaw_rate = [0, 0, 0], because we always keep yaw as 0 when we identified the parameters.
182+
# We introduce a small negative offset here to make sure that we could get result of LQR ect..
183+
# TODO: identify params_yaw_rate
184+
params_yaw_rate = [-0.01, 0, 0]
182185

183186
# Define dynamics equations.
184187
# TODO: create a parameter for the new quad model

‎tutorials/LQR_ILQR.ipynb

+14-86
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
},
1919
{
2020
"cell_type": "code",
21-
"execution_count": 1,
21+
"execution_count": null,
2222
"metadata": {},
2323
"outputs": [],
2424
"source": [
@@ -48,27 +48,15 @@
4848
},
4949
{
5050
"cell_type": "code",
51-
"execution_count": 2,
52-
"metadata": {},
53-
"outputs": [
54-
{
55-
"name": "stdout",
56-
"output_type": "stream",
57-
"text": [
58-
"observation space: \n",
59-
" Dict('difference_to_goal': Box(-inf, inf, (1, 3), float32), 'pos': Box(-inf, inf, (1, 3), float32), 'quat': Box(-inf, inf, (1, 4), float32), 'rpy_rates': Box(-inf, inf, (1, 3), float32), 'vel': Box(-inf, inf, (1, 3), float32))\n",
60-
"action space: \n",
61-
" Box([[ 0.11264675 -3.1415927 -3.1415927 -3.1415927 ]], [[0.5933658 3.1415927 3.1415927 3.1415927]], (1, 4), float32)\n",
62-
"time step: 0.002\n"
63-
]
64-
}
65-
],
51+
"execution_count": null,
52+
"metadata": {},
53+
"outputs": [],
6654
"source": [
6755
"# set config for simulation\n",
6856
"sim_config = config_dict.ConfigDict()\n",
6957
"sim_config.physics = Physics.sys_id\n",
7058
"sim_config.control = Control.attitude\n",
71-
"sim_config.thrust_freq = 500\n",
59+
"sim_config.thrust_freq = 50\n",
7260
"sim_config.device = \"cpu\"\n",
7361
"sim_config.n_drones = 1\n",
7462
"sim_config.n_worlds = 1 #20\n",
@@ -121,7 +109,7 @@
121109
},
122110
{
123111
"cell_type": "code",
124-
"execution_count": 3,
112+
"execution_count": null,
125113
"metadata": {},
126114
"outputs": [],
127115
"source": [
@@ -169,68 +157,9 @@
169157
},
170158
{
171159
"cell_type": "code",
172-
"execution_count": 4,
173-
"metadata": {},
174-
"outputs": [
175-
{
176-
"name": "stdout",
177-
"output_type": "stream",
178-
"text": [
179-
"A shape: (12, 12)\n",
180-
"B shape: (12, 4)\n",
181-
"A :\n",
182-
" [[ 0. 1. 0. 0. 0.\n",
183-
" 0. 0. 0. 0. 0.\n",
184-
" 0. 0. ]\n",
185-
" [ 0. 0. 0. 0. 0.\n",
186-
" 0. 0. 9.19147649 0. 0.\n",
187-
" 0. 0. ]\n",
188-
" [ 0. 0. 0. 1. 0.\n",
189-
" 0. 0. 0. 0. 0.\n",
190-
" 0. 0. ]\n",
191-
" [ 0. 0. 0. 0. 0.\n",
192-
" 0. -9.19147649 0. 0. 0.\n",
193-
" 0. 0. ]\n",
194-
" [ 0. 0. 0. 0. 0.\n",
195-
" 1. 0. 0. 0. 0.\n",
196-
" 0. 0. ]\n",
197-
" [ 0. 0. 0. 0. 0.\n",
198-
" 0. -0. -0. 0. 0.\n",
199-
" 0. 0. ]\n",
200-
" [ 0. 0. 0. 0. 0.\n",
201-
" 0. 0. 0. 0. 1.\n",
202-
" 0. 0. ]\n",
203-
" [ 0. 0. 0. 0. 0.\n",
204-
" 0. 0. 0. 0. 0.\n",
205-
" 1. 0. ]\n",
206-
" [ 0. 0. 0. 0. 0.\n",
207-
" 0. 0. 0. 0. 0.\n",
208-
" 0. 1. ]\n",
209-
" [ 0. 0. 0. 0. 0.\n",
210-
" 0. -130.3 0. 0. -16.33\n",
211-
" 0. 0. ]\n",
212-
" [ 0. 0. 0. 0. 0.\n",
213-
" 0. 0. -99.94 0. 0.\n",
214-
" -13.3 0. ]\n",
215-
" [ 0. 0. 0. 0. 0.\n",
216-
" 0. 0. 0. 0. 0.\n",
217-
" 0. 0. ]]\n",
218-
"B :\n",
219-
" [[ 0. 0. 0. 0. ]\n",
220-
" [ 0. 0. 0. 0. ]\n",
221-
" [ 0. 0. 0. 0. ]\n",
222-
" [ 0. 0. 0. 0. ]\n",
223-
" [ 0. 0. 0. 0. ]\n",
224-
" [ 20.90757426 0. 0. 0. ]\n",
225-
" [ 0. 0. 0. 0. ]\n",
226-
" [ 0. 0. 0. 0. ]\n",
227-
" [ 0. 0. 0. 0. ]\n",
228-
" [ 0. 119.3 0. 0. ]\n",
229-
" [ 0. 0. 84.73 0. ]\n",
230-
" [ 0. 0. 0. 0. ]]\n"
231-
]
232-
}
233-
],
160+
"execution_count": null,
161+
"metadata": {},
162+
"outputs": [],
234163
"source": [
235164
"dt = 1 / envs.sim.freq\n",
236165
"symbolic_model = symbolic(MASS, J, dt)\n",
@@ -343,7 +272,8 @@
343272
"outputs": [],
344273
"source": [
345274
"Ad, Bd = discretize_linear_system(A, B, dt) #, exact=True)\n",
346-
"\n",
275+
"# print(\"A :\\n\", Ad)\n",
276+
"# print(\"B :\\n\", Bd)\n",
347277
"P = scipy.linalg.solve_discrete_are(Ad, Bd, Q_lqr, R_lqr)\n",
348278
"\n",
349279
"btp = np.dot(Bd.T, P)\n",
@@ -353,9 +283,7 @@
353283
"\n",
354284
"# We can also comment out the above two lines of code \n",
355285
"# and use the following line instead to compute for the continuous-time case\n",
356-
"\n",
357286
"# P = scipy.linalg.solve_continuous_are(A, B, Q_lqr, R_lqr)\n",
358-
"\n",
359287
"# gain_lqr = np.dot(np.linalg.inv(R_lqr), np.dot(B.T, P))\n",
360288
"\n",
361289
"# print(\"A (discretized):\\n\", Ad)\n",
@@ -523,7 +451,7 @@
523451
"outputs": [],
524452
"source": [
525453
"x_d = np.array([0, 0, 5, 0, 1, 0, 0, 0, 0, 0, 0, 0]) # State equilibrium \n",
526-
"u_d = np.ones(4, dtype=np.float32) * 0.25 * MASS * GRAVITY # Control equilibrium"
454+
"u_d = np.array([MASS * GRAVITY, 0.0, 0.0, 0.0], dtype=np.float32) # Control equilibrium"
527455
]
528456
},
529457
{
@@ -621,10 +549,10 @@
621549
" control_input = input_ff[:, step] + gains_fb[step].dot(state) \n",
622550
" \n",
623551
" # Clip the control input to the specified range\n",
624-
" control_input = np.clip(control_input, 0.028161688, 0.14834145) \n",
552+
" control_input = np.clip(control_input, envs.action_space.low, envs.action_space.high) \n",
625553
" \n",
626554
" # Convert to np.ndarray\n",
627-
" action = np.array([control_input], dtype=np.float32) # (1, 4)\n",
555+
" action = control_input.reshape(1,4).astype(np.float32) # (1, 4)\n",
628556
"\n",
629557
" # Save rollout data.\n",
630558
" if step == 0:\n",

0 commit comments

Comments
 (0)