Skip to content

Commit 9637fc6

Browse files
ratheronratheron
committed
Add rpm2thrust and torque parameters to first_principles model
1 parent 86f1ef9 commit 9637fc6

4 files changed

Lines changed: 84 additions & 131 deletions

File tree

‎drone_models/data/cf21B_500.xml‎

Lines changed: 10 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22
<compiler inertiafromgeom="false" meshdir="assets" autolimits="true" />
33

44
<asset>
5-
65
<material name="black" rgba="0.102 0.102 0.102 1"/>
76
<material name="gray" rgba="0.3 0.3 0.3 1"/>
87
<material name="light-gray" rgba="0.7 0.7 0.7 1"/>
@@ -11,21 +10,17 @@
1110
<material name="gold" rgba="0.969 0.878 0.6 1"/>
1211
<material name="green" rgba="0.522 0.902 0.145 1"/>
1312

14-
<material name="polished_plastic" rgba="0.631 0.659 0.678 1"/>
15-
<material name="medium_gloss_plastic" rgba="0.109 0.184 0.0 1"/>
16-
17-
<mesh name="pcb" file="cf21B/cf21B_pcb.stl" scale="0.001 0.001 0.001"/>
18-
<mesh name="motors" file="cf21B/cf21B_motors.stl" scale="0.001 0.001 0.001"/>
19-
<mesh name="propL" file="cf21B/cf21B_PropL.stl" scale="0.001 0.001 0.001"/>
20-
<mesh name="propR" file="cf21B/cf21B_PropR.stl" scale="0.001 0.001 0.001"/>
21-
<mesh name="prop-guards" file="cf21B/cf21B_prop-guards.stl" scale="0.001 0.001 0.001"/>
22-
<mesh name="connectors" file="cf21B/cf21B_connectors.stl" scale="0.001 0.001 0.001"/>
23-
<mesh name="connector-pins" file="cf21B/cf21B_connector-pins.stl" scale="0.001 0.001 0.001"/>
24-
<mesh name="battery" file="cf21B/cf21B_battery.stl" scale="0.001 0.001 0.001"/>
25-
<mesh name="battery-holder" file="cf21B/cf21B_battery-holder.stl" scale="0.001 0.001 0.001"/>
13+
<mesh name="pcb" file="cf21B/cf21B_pcb.stl" scale="1e-3 1e-3 1e-3"/>
14+
<mesh name="motors" file="cf21B/cf21B_motors.stl" scale="1e-3 1e-3 1e-3"/>
15+
<mesh name="propL" file="cf21B/cf21B_PropL.stl" scale="1e-3 1e-3 1e-3"/>
16+
<mesh name="propR" file="cf21B/cf21B_PropR.stl" scale="1e-3 1e-3 1e-3"/>
17+
<mesh name="prop-guards" file="cf21B/cf21B_prop-guards.stl" scale="1e-3 1e-3 1e-3"/>
18+
<mesh name="connectors" file="cf21B/cf21B_connectors.stl" scale="1e-3 1e-3 1e-3"/>
19+
<mesh name="connector-pins" file="cf21B/cf21B_connector-pins.stl" scale="1e-3 1e-3 1e-3"/>
20+
<mesh name="battery" file="cf21B/cf21B_battery.stl" scale="1e-3 1e-3 1e-3"/>
21+
<mesh name="battery-holder" file="cf21B/cf21B_battery-holder.stl" scale="1e-3 1e-3 1e-3"/>
2622
</asset>
2723

28-
2924
<default>
3025
<default class="cf2">
3126
<default class="visual">
@@ -40,7 +35,7 @@
4035

4136
<worldbody>
4237
<body name="drone" pos="0 0 0.05" childclass="cf2">
43-
<inertial pos="0.0 0.0 0.0" mass="0.027" diaginertia="2.3951e-5 2.3951e-5 3.2347e-5" />
38+
<inertial pos="0.0 0.0 0.0" mass="0.04338" diaginertia="2.3951e-5 2.3951e-5 3.2347e-5" />
4439
<camera name="track" pos="-1 0 .5" xyaxes="0 -1 0 1 0 2" mode="trackcom" />
4540
<geom mesh="pcb" material="gray" class="visual" />
4641
<geom mesh="motors" material="black" class="visual" />
@@ -90,45 +85,4 @@
9085
<motor gear="0 0 0 0 0 1" site="motor3" name="motor3_torque" />
9186
</actuator>
9287

93-
<!-- Model Parameters
94-
Below are all the parameters unique to the drone setup
95-
Base is the Crazyflie 2.X, file name encodes the specific setup
96-
- Propellers/Motors options: L(egacy), P(lus), T(hrust upgrade kit), B(rushless)
97-
- Battery (250mAh / 350mAh / 500mAh)
98-
-->
99-
<custom>
100-
<!-- Physical Parameters-->
101-
<numeric name="gravity" data="9.807" /> <!-- Munich, Germany-->
102-
<numeric name="mass" data="0.03454" /> <!-- From identification for L250! TODO: Redo -->
103-
<numeric name="J" data="1.4e-5 0.0 0.0
104-
0.0 1.4e-5 0.0
105-
0.0 0.0 2.17e-5"/> <!-- From IROS 2022 -->
106-
<!-- <numeric name="J" data="1.68e-5 0.0 0.0
107-
0.0 1.68e-5 0.0
108-
0.0 0.0 2.98e-5" /> --> <!-- From Förster B.S. thesis -->
109-
<!-- Distance of motor from axis
110-
in x configuration, it's 1/sqrt(2) smaller
111-
arm "+" = 0.05 (from CAD); arm "x" = 0.035355 (rounded)-->
112-
<numeric name="arm" data="0.035355" />
113-
<!-- Matrix on how to mix the forces into torques, different for "x" and "+" config-->
114-
<numeric name="sign_matrix" data="-1 -1 -1
115-
-1 1 1
116-
1 1 -1
117-
1 -1 1" />
118-
<numeric name="prop_radius" data="2.31348e-2" />
119-
120-
<!-- Motors and Thrust-->
121-
<numeric name="PWM_MIN" data="20000.0" />
122-
<numeric name="PWM_MAX" data="65535.0" />
123-
<numeric name="THRUST_MIN" data="0.03" /> <!-- in N, per motor-->
124-
<numeric name="THRUST_MAX" data="0.1625" /> <!-- in N, per motor-->
125-
<numeric name="THRUST_TAU" data="8.719306881898989" />
126-
<numeric name="VMOTOR_MIN" data="0.983691398141343" /> <!-- in V-->
127-
<numeric name="VMOTOR_MAX" data="2.643523702759344" /> <!-- in V-->
128-
<numeric name="p_vmotor2thrust" data="-0.03978221591250353 0.10979738851226176 -0.05545304285403245 0.016215002062640885" /> <!--Index is order-->
129-
<numeric name="p_vmotor2rpm" data="2738.439005850152 8054.983767001532" /> <!--Index is order-->
130-
<numeric name="kf" data="1.068340683985525e-09" />
131-
<numeric name="km" data="7.94e-12" /> <!-- From old identification! TODO: Replace if possible-->
132-
</custom>
133-
13488
</mujoco>

‎drone_models/data/params.toml‎

Lines changed: 28 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@ J = [
88
[0.0, 1.68e-5, 0.0],
99
[0.0, 0.0, 2.98e-5]
1010
]
11-
KF = 3.16e-10 # TODO: Update with new fit
12-
KM = 7.94e-12
11+
rpm2thrust = [0.0, 0.0, 3.16e-10] # TODO
12+
rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
1313
L = 0.03253
1414
mixing_matrix = [
1515
[-1.0, -1.0, 1.0, 1.0],
@@ -20,6 +20,7 @@ mixing_matrix = [
2020
# updated as soon as possible.
2121
thrust_tau = 0.03
2222

23+
2324
[cf2x_P250]
2425
mass = 0.03454
2526
gravity_vec = [0.0, 0.0, -9.81]
@@ -29,8 +30,8 @@ J = [
2930
[0.0, 1.4e-5, 0.0],
3031
[0.0, 0.0, 2.17e-5]
3132
]
32-
KF = 5.792654323957372e-10
33-
KM = 7.94e-12
33+
rpm2thrust = [0.0, 0.0, 5.792654323957372e-10] # TODO
34+
rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
3435
L = 0.03253
3536
mixing_matrix = [
3637
[-1.0, -1.0, 1.0, 1.0],
@@ -39,6 +40,7 @@ mixing_matrix = [
3940
]
4041
thrust_tau = 7.2333515424212695
4142

43+
4244
[cf2x_T350]
4345
mass = 0.03454
4446
gravity_vec = [0.0, 0.0, -9.81]
@@ -47,12 +49,31 @@ J = [
4749
[0.0, 1.4e-5, 0.0],
4850
[0.0, 0.0, 2.17e-5]
4951
]
50-
KF = 1.068340683985525e-09
51-
KM = 7.94e-12
52+
rpm2thrust = [0.0, 0.0, 1.068340683985525e-09] # TODO
53+
rpm2torque = [0.0, 0.0, 7.94e-12] # TODO
5254
L = 0.03253
5355
mixing_matrix = [
5456
[-1.0, -1.0, 1.0, 1.0],
5557
[-1.0, 1.0, 1.0, -1.0],
5658
[-1.0, 1.0, -1.0, 1.0]
5759
]
58-
thrust_tau = 8.719306881898989
60+
thrust_tau = 8.719306881898989
61+
62+
63+
[cf21B_500]
64+
mass = 0.04338
65+
gravity_vec = [0.0, 0.0, -9.81]
66+
J = [
67+
[2.5e-5, 0.0, 0.0],
68+
[0.0, 2.8e-5, 0.0],
69+
[0.0, 0.0, 4.9e-5]
70+
]
71+
rpm2thrust = [0.0, -8.357406697944084e-07, 4.716909960828207e-10]
72+
rpm2torque = [0.0, 2.435151264350409e-09, 2.424629050055297e-12]
73+
L = 0.035355
74+
mixing_matrix = [
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+
]
79+
thrust_tau = 0.03 # TODO

‎drone_models/first_principles/model.py‎

Lines changed: 41 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ def dynamics(
3131
gravity_vec: Array,
3232
J: Array,
3333
J_inv: Array,
34-
KF: float,
35-
KM: float,
34+
rpm2thrust: Array,
35+
rpm2torque: Array,
3636
L: float,
3737
mixing_matrix: Array,
3838
thrust_tau: float,
@@ -59,8 +59,8 @@ def dynamics(
5959
[0, 0, -9.81].
6060
J: Inertia matrix (kg m^2).
6161
J_inv: Inverse inertia matrix (1/kg m^2).
62-
KF: Motor force constant (N/rad^2).
63-
KM: Motor torque constant (Nm/rad^2).
62+
rpm2thrust: Propeller force constant (N min^2).
63+
rpm2torque: Propeller torque constant (Nm min^2).
6464
L: Distance from the CoM to the motor (m).
6565
mixing_matrix: Mixing matrix denoting the turn direction of the motors (4x3).
6666
thrust_tau: Thrust time constant (s).
@@ -74,8 +74,18 @@ def dynamics(
7474
More information https://ahrs.readthedocs.io/en/latest/filters/angular.html
7575
"""
7676
xp = array_namespace(pos)
77-
mass, gravity_vec, J, J_inv, KF, KM, L, mixing_matrix, thrust_tau = to_xp(
78-
mass, gravity_vec, J, J_inv, KF, KM, L, mixing_matrix, thrust_tau, xp=xp, device=device(pos)
77+
mass, gravity_vec, J, J_inv, rpm2thrust, rpm2torque, L, mixing_matrix, thrust_tau = to_xp(
78+
mass,
79+
gravity_vec,
80+
J,
81+
J_inv,
82+
rpm2thrust,
83+
rpm2torque,
84+
L,
85+
mixing_matrix,
86+
thrust_tau,
87+
xp=xp,
88+
device=device(pos),
7989
)
8090
rot = R.from_quat(quat)
8191
# Rotor dynamics
@@ -84,11 +94,17 @@ def dynamics(
8494
else:
8595
rotor_vel_dot = 1 / thrust_tau * (cmd - rotor_vel) # - 1 / KM * rotor_vel**2
8696
# Creating force and torque vector
87-
forces_motor = KF * rotor_vel**2
97+
forces_motor = rpm2thrust[0] + rpm2thrust[1] * rotor_vel + rpm2thrust[2] * rotor_vel**2
98+
torques_motor = rpm2torque[0] + rpm2torque[1] * rotor_vel + rpm2torque[2] * rotor_vel**2
8899
forces_motor_tot = xp.sum(forces_motor, axis=-1)
89100
zeros = xp.zeros_like(forces_motor_tot)
90101
forces_motor_vec = xp.stack((zeros, zeros, forces_motor_tot), axis=-1)
91-
torque = (mixing_matrix @ (rotor_vel**2)[..., None])[..., 0] * xp.stack([KF * L, KF * L, KM])
102+
103+
torque_vec = (mixing_matrix @ (forces_motor)[..., None])[..., 0] * xp.stack(
104+
[L, L, xp.asarray(0.0)]
105+
) + (mixing_matrix @ (torques_motor)[..., None])[..., 0] * xp.stack(
106+
[xp.asarray(0.0), xp.asarray(0.0), xp.asarray(1.0)]
107+
)
92108

93109
# Linear equation of motion
94110
forces_motor_vec_world = rot.apply(forces_motor_vec)
@@ -101,10 +117,10 @@ def dynamics(
101117

102118
# Rotational equation of motion
103119
if dist_t is not None:
104-
torque = torque + rot.apply(dist_t, inverse=True)
120+
torque_vec = torque_vec + rot.apply(dist_t, inverse=True)
105121
quat_dot = rotation.ang_vel2quat_dot(quat, ang_vel)
106-
torque = torque - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
107-
ang_vel_dot = (J_inv @ torque[..., None])[..., 0]
122+
torque_vec = torque_vec - xp.linalg.cross(ang_vel, (J @ ang_vel[..., None])[..., 0])
123+
ang_vel_dot = (J_inv @ torque_vec[..., None])[..., 0]
108124
return pos_dot, quat_dot, vel_dot, ang_vel_dot, rotor_vel_dot
109125

110126

@@ -117,8 +133,8 @@ def symbolic_dynamics(
117133
gravity_vec: Array,
118134
J: Array,
119135
J_inv: Array,
120-
KF: float,
121-
KM: float,
136+
rpm2thrust: Array,
137+
rpm2torque: Array,
122138
L: float,
123139
mixing_matrix: Array,
124140
thrust_tau: float,
@@ -138,13 +154,21 @@ def symbolic_dynamics(
138154
if model_rotor_vel:
139155
# Thrust dynamics
140156
rotor_vel_dot = 1 / thrust_tau * (U - symbols.rotor_vel) # - 1 / KM * symbols.rotor_vel**2
141-
forces_motor = KF * symbols.rotor_vel**2
157+
forces_motor = (
158+
rpm2thrust[0] + rpm2thrust[1] * symbols.rotor_vel + rpm2thrust[2] * symbols.rotor_vel**2
159+
)
160+
torques_motor = (
161+
rpm2torque[0] + rpm2torque[1] * symbols.rotor_vel + rpm2torque[2] * symbols.rotor_vel**2
162+
)
142163
else:
143-
forces_motor = KF * U**2
164+
forces_motor = rpm2thrust[0] + rpm2thrust[1] * U + rpm2thrust[2] * U**2
165+
torques_motor = rpm2torque[0] + rpm2torque[1] * U + rpm2torque[2] * U**2
144166

145167
# Creating force and torque vector
146-
forces_motor_vec = cs.vertcat(0, 0, cs.sum1(forces_motor))
147-
torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(L, L, KM / KF)
168+
forces_motor_vec = cs.vertcat(0.0, 0.0, cs.sum1(forces_motor))
169+
torques_motor_vec = mixing_matrix @ forces_motor * cs.vertcat(
170+
L, L, 0.0
171+
) + mixing_matrix @ torques_motor * cs.vertcat(0.0, 0.0, 1.0)
148172

149173
# Linear equation of motion
150174
forces_motor_vec_world = symbols.rot @ forces_motor_vec

‎drone_models/first_principles/params.toml‎

Lines changed: 5 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,56 +1,10 @@
11
[cf2x_L250]
2-
mass = 0.033
3-
gravity_vec = [0.0, 0.0, -9.81]
4-
J = [
5-
[1.68e-5, 0.0, 0.0],
6-
[0.0, 1.68e-5, 0.0],
7-
[0.0, 0.0, 2.98e-5]
8-
]
9-
KF = 3.16e-10 # TODO: Update with new fit
10-
KM = 7.94e-12
11-
L = 0.03253
12-
mixing_matrix = [
13-
[-1.0, -1.0, 1.0, 1.0],
14-
[-1.0, 1.0, 1.0, -1.0],
15-
[-1.0, 1.0, -1.0, 1.0]
16-
]
17-
# TODO: This is an arbitrary number that leads to a working simulation. It MUST be fitted and
18-
# updated as soon as possible.
19-
thrust_tau = 0.03
2+
203

214
[cf2x_P250]
22-
mass = 0.03454
23-
gravity_vec = [0.0, 0.0, -9.81]
24-
# J from IROS 2022
25-
J = [
26-
[1.4e-5, 0.0, 0.0],
27-
[0.0, 1.4e-5, 0.0],
28-
[0.0, 0.0, 2.17e-5]
29-
]
30-
KF = 5.792654323957372e-10
31-
KM = 7.94e-12
32-
L = 0.03253
33-
mixing_matrix = [
34-
[-1.0, -1.0, 1.0, 1.0],
35-
[-1.0, 1.0, 1.0, -1.0],
36-
[-1.0, 1.0, -1.0, 1.0]
37-
]
38-
thrust_tau = 7.2333515424212695
5+
396

407
[cf2x_T350]
41-
mass = 0.03454
42-
gravity_vec = [0.0, 0.0, -9.81]
43-
J = [
44-
[1.4e-5, 0.0, 0.0],
45-
[0.0, 1.4e-5, 0.0],
46-
[0.0, 0.0, 2.17e-5]
47-
]
48-
KF = 1.068340683985525e-09
49-
KM = 7.94e-12
50-
L = 0.03253
51-
mixing_matrix = [
52-
[-1.0, -1.0, 1.0, 1.0],
53-
[-1.0, 1.0, 1.0, -1.0],
54-
[-1.0, 1.0, -1.0, 1.0]
55-
]
56-
thrust_tau = 8.719306881898989
8+
9+
10+
[cf21B_500]

0 commit comments

Comments
 (0)