Skip to content

Commit 6bf7c95

Browse files
ratheronratheron
committed
[WIP] Continue work on symbolic models
1 parent 58ced20 commit 6bf7c95

6 files changed

Lines changed: 303 additions & 34 deletions

File tree

‎drone_models/models/__init__.py‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,33 @@
33
from typing import Callable
44

55
from drone_models.models.first_principles import dynamics as _first_principles_dynamics
6+
7+
# from drone_models.models.first_principles import (
8+
# dynamics_sybolic as _first_principles_dynamics_symbolic,
9+
# )
610
from drone_models.models.identified.so_rpy import dynamics as _so_rpy_dynamics
11+
12+
# from drone_models.models.identified.so_rpy import dynamics_symbolic as _so_rpy_dynamics_symbolic
713
from drone_models.models.identified.so_rpy_rotor import dynamics as _so_rpy_rotor_dynamics
14+
15+
# from drone_models.models.identified.so_rpy_rotor import (
16+
# dynamics_symbolic as _so_rpy_rotor_dynamics_symbolic,
17+
# )
818
from drone_models.models.identified.so_rpy_rotor_drag import dynamics as _so_rpy_rotor_drag_dynamics
919

20+
# from drone_models.models.identified.so_rpy_rotor_drag import (
21+
# dynamics_symbolic as _so_rpy_rotor_drag_dynamics_symbolic,
22+
# )
23+
1024
available_models: dict[str, Callable] = {
1125
"first_principles": _first_principles_dynamics,
1226
"so_rpy": _so_rpy_dynamics,
1327
"so_rpy_rotor": _so_rpy_rotor_dynamics,
1428
"so_rpy_rotor_drag": _so_rpy_rotor_drag_dynamics,
29+
# "first_principles_symbolic": _first_principles_dynamics_symbolic,
30+
# "so_rpy_symbolic": _so_rpy_dynamics_symbolic,
31+
# "so_rpy_rotor_symbolic": _so_rpy_rotor_dynamics_symbolic,
32+
# "so_rpy_rotor_drag_symbolic": _so_rpy_rotor_drag_dynamics_symbolic,
1533
}
1634

1735

‎drone_models/models/first_principles.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ def dynamics(
102102
return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
103103

104104

105-
def dynamics_sybolic(
105+
def dynamics_symbolic(
106106
constants: Constants,
107107
calc_rotor_vel: bool = True,
108108
calc_dist_f: bool = False,
@@ -123,8 +123,8 @@ def dynamics_sybolic(
123123
if calc_rotor_vel:
124124
# Thrust dynamics
125125
rotor_vel_dot = (
126-
1 / constants.ROTOR_TAU * (U - symbols.rotor_vel)
127-
- 1 / constants.ROTOR_D * symbols.rotor_vel**2
126+
1 / constants.THRUST_TAU * (U - symbols.rotor_vel)
127+
- 1 / constants.KM * symbols.rotor_vel**2
128128
)
129129
forces_motor = constants.KF * symbols.rotor_vel**2
130130
else:

‎drone_models/models/identified/so_rpy.py‎

Lines changed: 71 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,11 @@
44

55
from typing import TYPE_CHECKING
66

7+
import casadi as cs
78
from array_api_compat import array_namespace
89
from scipy.spatial.transform import Rotation as R
910

11+
import drone_models.models.symbols as symbols
1012
from drone_models.models.utils import supports
1113
from drone_models.utils import rotation
1214

@@ -87,10 +89,74 @@ def dynamics(
8789
return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
8890

8991

90-
def dynamics_symbolic():
91-
"""Symbolic dynamics of the fitted double integrator (DI) model.
92+
def dynamics_symbolic(
93+
constants: Constants,
94+
calc_rotor_vel: bool = False,
95+
calc_dist_f: bool = False,
96+
calc_dist_t: bool = False,
97+
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
98+
"""The fitted double integrator (DI) model with optional motor delay (D).
9299
93-
Returns:
94-
The symbolic derivatives of all state variables.
100+
TODO.
95101
"""
96-
...
102+
# States and Inputs
103+
X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
104+
if calc_rotor_vel:
105+
X = cs.vertcat(X, symbols.rotor_vel)
106+
if calc_dist_f:
107+
X = cs.vertcat(X, symbols.dist_f)
108+
if calc_dist_t:
109+
X = cs.vertcat(X, symbols.dist_t)
110+
U = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
111+
112+
# Defining the dynamics function
113+
# Creating force vector
114+
forces_motor_vec = cs.vertcat(
115+
0, 0, constants.DI_ACC[0] + constants.DI_ACC[1] * symbols.cmd_thrust
116+
)
117+
118+
# Linear equation of motion
119+
pos_dot = symbols.vel
120+
vel_dot = symbols.rot @ forces_motor_vec / constants.MASS + constants.GRAVITY_VEC
121+
if calc_dist_f:
122+
# Adding force disturbances to the state
123+
vel_dot = vel_dot + symbols.dist_f / constants.MASS
124+
125+
# Rotational equation of motion
126+
euler_angles = rotation.cs_quat2euler(symbols.quat)
127+
128+
xi = cs.vertcat(
129+
cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
130+
)
131+
quat_dot = 0.5 * (xi @ symbols.quat)
132+
rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
133+
if calc_rotor_vel:
134+
rpy_rates_dot = (
135+
constants.DI_D_PARAMS[:, 0] * euler_angles
136+
+ constants.DI_D_PARAMS[:, 1] * rpy_rates
137+
+ constants.DI_D_PARAMS[:, 2]
138+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
139+
)
140+
else:
141+
rpy_rates_dot = (
142+
constants.DI_PARAMS[:, 0] * euler_angles
143+
+ constants.DI_PARAMS[:, 1] * rpy_rates
144+
+ constants.DI_PARAMS[:, 2]
145+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
146+
)
147+
ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
148+
if calc_dist_t:
149+
# adding torque disturbances to the state
150+
# angular acceleration can be converted to total torque
151+
torque = constants.J @ ang_vel_dot - cs.cross(
152+
symbols.ang_vel, constants.J @ symbols.ang_vel
153+
)
154+
# adding torque
155+
torque = torque + symbols.dist_t
156+
# back to angular acceleration
157+
ang_vel_dot = constants.J_INV @ torque
158+
159+
X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
160+
Y = cs.vertcat(symbols.pos, symbols.quat)
161+
162+
return X_dot, X, U, Y

‎drone_models/models/identified/so_rpy_rotor.py‎

Lines changed: 88 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,11 @@
55
import warnings
66
from typing import TYPE_CHECKING
77

8+
import casadi as cs
89
from array_api_compat import array_namespace
910
from scipy.spatial.transform import Rotation as R
1011

12+
import drone_models.models.symbols as symbols
1113
from drone_models.models.utils import supports
1214
from drone_models.transform import motor_force2rotor_speed
1315
from drone_models.utils import rotation
@@ -61,7 +63,8 @@ def dynamics(
6163
warnings.warn("Rotor velocity is not provided, using commanded rotor velocity directly.")
6264
else:
6365
rotor_vel_dot = (
64-
1 / constants.DI_D_ACC[2] * (cmd_rotor_vel - rotor_vel) - constants.KM * rotor_vel**2
66+
1 / constants.DI_D_ACC[2] * (cmd_rotor_vel[..., None] - rotor_vel)
67+
- constants.KM * rotor_vel**2
6568
)
6669
forces_motor = xp.sum(constants.KF * rotor_vel**2, axis=-1)
6770
forces_sum = xp.sum(forces_motor, axis=-1)
@@ -97,3 +100,87 @@ def dynamics(
97100
ang_vel_dot = xp.matvec(constants.J_INV, torque)
98101

99102
return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
103+
104+
105+
def dynamics_symbolic(
106+
constants: Constants,
107+
calc_rotor_vel: bool = False,
108+
calc_dist_f: bool = False,
109+
calc_dist_t: bool = False,
110+
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
111+
"""The fitted double integrator (DI) model with optional motor delay (D).
112+
113+
TODO.
114+
"""
115+
# States and Inputs
116+
X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
117+
if calc_rotor_vel:
118+
X = cs.vertcat(X, symbols.rotor_vel)
119+
if calc_dist_f:
120+
X = cs.vertcat(X, symbols.dist_f)
121+
if calc_dist_t:
122+
X = cs.vertcat(X, symbols.dist_t)
123+
U = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
124+
125+
# Defining the dynamics function
126+
if calc_rotor_vel:
127+
# Note: Due to the structure of the integrator, we split the commanded thrust into
128+
# four equal parts and later apply the sum as total thrust again. Those four forces
129+
# are not the true forces of the motors, but the sum is the true total thrust.
130+
forces_motor_dot = 1 / constants.DI_D_ACC[2] * (symbols.cmd_thrust / 4 - symbols.rotor_vel)
131+
thrust = cs.sum1(symbols.rotor_vel)
132+
# Creating force vector
133+
forces_motor_vec = cs.vertcat(0, 0, constants.DI_D_ACC[0] + constants.DI_D_ACC[1] * thrust)
134+
else:
135+
thrust = symbols.cmd_thrust
136+
# Creating force vector
137+
forces_motor_vec = cs.vertcat(0, 0, constants.DI_ACC[0] + constants.DI_ACC[1] * thrust)
138+
139+
# Linear equation of motion
140+
pos_dot = symbols.vel
141+
vel_dot = symbols.rot @ forces_motor_vec / constants.MASS + constants.GRAVITY_VEC
142+
if calc_dist_f:
143+
# Adding force disturbances to the state
144+
vel_dot = vel_dot + symbols.dist_f / constants.MASS
145+
146+
# Rotational equation of motion
147+
euler_angles = rotation.cs_quat2euler(symbols.quat)
148+
149+
xi = cs.vertcat(
150+
cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
151+
)
152+
quat_dot = 0.5 * (xi @ symbols.quat)
153+
rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
154+
if calc_rotor_vel:
155+
rpy_rates_dot = (
156+
constants.DI_D_PARAMS[:, 0] * euler_angles
157+
+ constants.DI_D_PARAMS[:, 1] * rpy_rates
158+
+ constants.DI_D_PARAMS[:, 2]
159+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
160+
)
161+
else:
162+
rpy_rates_dot = (
163+
constants.DI_PARAMS[:, 0] * euler_angles
164+
+ constants.DI_PARAMS[:, 1] * rpy_rates
165+
+ constants.DI_PARAMS[:, 2]
166+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
167+
)
168+
ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
169+
if calc_dist_t:
170+
# adding torque disturbances to the state
171+
# angular acceleration can be converted to total torque
172+
torque = constants.J @ ang_vel_dot - cs.cross(
173+
symbols.ang_vel, constants.J @ symbols.ang_vel
174+
)
175+
# adding torque
176+
torque = torque + symbols.dist_t
177+
# back to angular acceleration
178+
ang_vel_dot = constants.J_INV @ torque
179+
180+
if calc_rotor_vel:
181+
X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot, forces_motor_dot)
182+
else:
183+
X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
184+
Y = cs.vertcat(symbols.pos, symbols.quat)
185+
186+
return X_dot, X, U, Y

‎drone_models/models/identified/so_rpy_rotor_drag.py‎

Lines changed: 84 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,11 @@
55
import warnings
66
from typing import TYPE_CHECKING
77

8+
import casadi as cs
89
from array_api_compat import array_namespace
910
from scipy.spatial.transform import Rotation as R
1011

12+
import drone_models.models.symbols as symbols
1113
from drone_models.models.utils import supports
1214
from drone_models.transform import motor_force2rotor_speed
1315
from drone_models.utils import rotation
@@ -61,7 +63,8 @@ def dynamics(
6163
warnings.warn("Rotor velocity is not provided, using commanded rotor velocity directly.")
6264
else:
6365
rotor_vel_dot = (
64-
1 / constants.DI_DD_ACC[2] * (cmd_rotor_vel - rotor_vel) - constants.KM * rotor_vel**2
66+
1 / constants.DI_DD_ACC[2] * (cmd_rotor_vel[..., None] - rotor_vel)
67+
- constants.KM * rotor_vel**2
6568
)
6669
forces_motor = xp.sum(constants.KF * rotor_vel**2, axis=-1)
6770
forces_sum = xp.sum(forces_motor, axis=-1)
@@ -102,10 +105,85 @@ def dynamics(
102105
return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
103106

104107

105-
def symbolic_dynamics():
106-
"""Symbolic dynamics of the fitted double integrator (DI) model with optional motor delay (D).
108+
def dynamics_symbolic(
109+
constants: Constants,
110+
calc_rotor_vel: bool = False,
111+
calc_dist_f: bool = False,
112+
calc_dist_t: bool = False,
113+
) -> tuple[cs.MX, cs.MX, cs.MX, cs.MX]:
114+
"""The fitted double integrator (DI) model with optional motor delay (D).
107115
108-
Returns:
109-
The symbolic derivatives of all state variables.
116+
TODO.
110117
"""
111-
...
118+
# States and Inputs
119+
X = cs.vertcat(symbols.pos, symbols.quat, symbols.vel, symbols.ang_vel)
120+
if calc_rotor_vel:
121+
X = cs.vertcat(X, symbols.rotor_vel)
122+
if calc_dist_f:
123+
X = cs.vertcat(X, symbols.dist_f)
124+
if calc_dist_t:
125+
X = cs.vertcat(X, symbols.dist_t)
126+
U = cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw, symbols.cmd_thrust)
127+
128+
# Defining the dynamics function
129+
if calc_rotor_vel:
130+
# Note: Due to the structure of the integrator, we split the commanded thrust into
131+
# four equal parts and later apply the sum as total thrust again. Those four forces
132+
# are not the true forces of the motors, but the sum is the true total thrust.
133+
forces_motor_dot = 1 / constants.DI_D_ACC[2] * (symbols.cmd_thrust / 4 - symbols.rotor_vel)
134+
thrust = cs.sum1(symbols.rotor_vel)
135+
# Creating force vector
136+
forces_motor_vec = cs.vertcat(0, 0, constants.DI_D_ACC[0] + constants.DI_D_ACC[1] * thrust)
137+
else:
138+
thrust = symbols.cmd_thrust
139+
# Creating force vector
140+
forces_motor_vec = cs.vertcat(0, 0, constants.DI_ACC[0] + constants.DI_ACC[1] * thrust)
141+
142+
# Linear equation of motion
143+
pos_dot = symbols.vel
144+
vel_dot = symbols.rot @ forces_motor_vec / constants.MASS + constants.GRAVITY_VEC
145+
if calc_dist_f:
146+
# Adding force disturbances to the state
147+
vel_dot = vel_dot + symbols.dist_f / constants.MASS
148+
149+
# Rotational equation of motion
150+
euler_angles = rotation.cs_quat2euler(symbols.quat)
151+
152+
xi = cs.vertcat(
153+
cs.horzcat(0, -symbols.ang_vel.T), cs.horzcat(symbols.ang_vel, -cs.skew(symbols.ang_vel))
154+
)
155+
quat_dot = 0.5 * (xi @ symbols.quat)
156+
rpy_rates = rotation.cs_ang_vel2rpy_rates(symbols.quat, symbols.ang_vel)
157+
if calc_rotor_vel:
158+
rpy_rates_dot = (
159+
constants.DI_D_PARAMS[:, 0] * euler_angles
160+
+ constants.DI_D_PARAMS[:, 1] * rpy_rates
161+
+ constants.DI_D_PARAMS[:, 2]
162+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
163+
)
164+
else:
165+
rpy_rates_dot = (
166+
constants.DI_PARAMS[:, 0] * euler_angles
167+
+ constants.DI_PARAMS[:, 1] * rpy_rates
168+
+ constants.DI_PARAMS[:, 2]
169+
* cs.vertcat(symbols.cmd_roll, symbols.cmd_pitch, symbols.cmd_yaw)
170+
)
171+
ang_vel_dot = rotation.cs_rpy_rates_deriv2ang_vel_deriv(symbols.quat, rpy_rates, rpy_rates_dot)
172+
if calc_dist_t:
173+
# adding torque disturbances to the state
174+
# angular acceleration can be converted to total torque
175+
torque = constants.J @ ang_vel_dot - cs.cross(
176+
symbols.ang_vel, constants.J @ symbols.ang_vel
177+
)
178+
# adding torque
179+
torque = torque + symbols.dist_t
180+
# back to angular acceleration
181+
ang_vel_dot = constants.J_INV @ torque
182+
183+
if calc_rotor_vel:
184+
X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot, forces_motor_dot)
185+
else:
186+
X_dot = cs.vertcat(pos_dot, quat_dot, vel_dot, ang_vel_dot)
187+
Y = cs.vertcat(symbols.pos, symbols.quat)
188+
189+
return X_dot, X, U, Y

0 commit comments

Comments
 (0)