Skip to content

Commit 32d3f1d

Browse files
MingxuanCheMingxuanChe
committed
add L350 model
1 parent 9721168 commit 32d3f1d

3 files changed

Lines changed: 133 additions & 2 deletions

File tree

‎lsy_models/data/cf2x_L250.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@
6262
<!-- Physical Parameters-->
6363
<numeric name="gravity" data="9.807" /> <!-- Munich, Germany-->
6464
<!-- From IROS 2022 -->
65-
<numeric name="mass" data="0.03454" />
65+
<numeric name="mass" data="0.033" />
6666
<!-- <numeric name="J" data="1.4e-5 0.0 0.0
6767
0.0 1.4e-5 0.0
6868
0.0 0.0 2.17e-5"/> -->

‎lsy_models/data/cf2x_L350.xml‎

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
<mujoco model="cf2">
2+
<compiler inertiafromgeom="false" meshdir="assets" autolimits="true" />
3+
<include file="assets.xml" />
4+
5+
<default>
6+
<default class="cf2">
7+
<default class="visual">
8+
<geom group="2" type="mesh" contype="0" conaffinity="0" />
9+
</default>
10+
<default class="collision">
11+
<geom group="3" condim="1" />
12+
</default>
13+
<site group="5" />
14+
</default>
15+
</default>
16+
17+
<worldbody>
18+
<body name="drone" pos="0 0 0.05" childclass="cf2">
19+
<inertial pos="0.0 0.0 0.0" mass="0.027" diaginertia="2.3951e-5 2.3951e-5 3.2347e-5" />
20+
<camera name="track" pos="-1 0 .5" xyaxes="0 -1 0 1 0 2" mode="trackcom" />
21+
<geom mesh="cf2_0" material="propeller_plastic" class="visual" />
22+
<geom mesh="cf2_1" material="medium_gloss_plastic" class="visual" />
23+
<geom mesh="cf2_2" material="polished_gold" class="visual" />
24+
<geom mesh="cf2_3" material="polished_plastic" class="visual" />
25+
<geom mesh="cf2_4" material="burnished_chrome" class="visual" />
26+
<geom mesh="cf2_5" material="body_frame_plastic" class="visual" />
27+
<geom mesh="cf2_6" material="white" class="visual" />
28+
<!-- Collision geometry is estimated based on the visual mesh. We use a sphere for maximum
29+
computational efficiency with larger swarms. -->
30+
<geom type="sphere" size="0.055" class="collision" name="col" />
31+
<site name="CoM" />
32+
<site name="motor0" pos="0.0325 -0.0325 0.0" />
33+
<site name="motor1" pos="-0.0325 -0.0325 0.0" />
34+
<site name="motor2" pos="-0.0325 0.0325 0.0" />
35+
<site name="motor3" pos="0.0325 0.0325 0.0" />
36+
</body>
37+
</worldbody>
38+
39+
<!-- Actuators
40+
We do not limit the actuation range of the motors here, because we would need to synchronize the
41+
control limits from our constants module with the ones in the MuJoCo model. Instead, we rely on
42+
the controller simulation for proper clipping.
43+
-->
44+
<actuator>
45+
<motor gear="0 0 1 0 0 0" site="motor0" name="motor0_force" />
46+
<motor gear="0 0 1 0 0 0" site="motor1" name="motor1_force" />
47+
<motor gear="0 0 1 0 0 0" site="motor2" name="motor2_force" />
48+
<motor gear="0 0 1 0 0 0" site="motor3" name="motor3_force" />
49+
<motor gear="0 0 0 0 0 1" site="motor0" name="motor0_torque" />
50+
<motor gear="0 0 0 0 0 1" site="motor1" name="motor1_torque" />
51+
<motor gear="0 0 0 0 0 1" site="motor2" name="motor2_torque" />
52+
<motor gear="0 0 0 0 0 1" site="motor3" name="motor3_torque" />
53+
</actuator>
54+
55+
<!-- Model Parameters
56+
Below are all the parameters unique to the drone setup
57+
Base is the Crazyflie 2.X, file name encodes the specific setup
58+
- Propellers/Motors options: L(egacy), P(lus), T(hrust upgrade kit), B(rushless)
59+
- Battery (250mAh / 350mAh)
60+
-->
61+
<custom>
62+
<!-- Physical Parameters-->
63+
<numeric name="gravity" data="9.807" /> <!-- Munich, Germany-->
64+
<!-- From IROS 2022 -->
65+
<!-- <numeric name="mass" data="0.037" /> -->
66+
<numeric name="mass" data="0.037" />
67+
<!-- <numeric name="mass" data="0.037" /> TODO This is the wrong mass (cf with battery weighs more!) -->
68+
<!-- <numeric name="J" data="1.4e-5 0.0 0.0
69+
0.0 1.4e-5 0.0
70+
0.0 0.0 2.17e-5"/> -->
71+
<!-- From Förster B.S. thesis -->
72+
<numeric name="J" data="1.68e-5 0.0 0.0
73+
0.0 1.68e-5 0.0
74+
0.0 0.0 2.98e-5" />
75+
<!-- Distance of motor from axis
76+
in x configuration, it's 1/sqrt(2) smaller
77+
arm "+" = 0.046 (from CAD); arm "x" = 0.03253 (rounded)-->
78+
<numeric name="arm" data="0.03253" />
79+
<!-- Matrix on how to mix the forces into torques, different for "x" and "+" config-->
80+
<numeric name="sign_matrix" data="-1 -1 -1
81+
-1 1 1
82+
1 1 -1
83+
1 -1 1" />
84+
<numeric name="prop_radius" data="2.31348e-2" />
85+
86+
<!-- Motors and Thrust-->
87+
<numeric name="PWM_MIN" data="20000.0" />
88+
<numeric name="PWM_MAX" data="65535.0" />
89+
<numeric name="THRUST_MIN" data="0.02" /> <!-- in N, per motor-->
90+
<numeric name="THRUST_MAX" data="0.1125" /> <!-- in N, per motor-->
91+
<numeric name="THRUST_TAU" data="6.9235846332510365" />
92+
<numeric name="VMOTOR_MIN" data="1.003506695043662" /> <!-- in V-->
93+
<numeric name="VMOTOR_MAX" data="2.903351097106251" /> <!-- in V-->
94+
<numeric name="p_vmotor2thrust" data="-0.014830744918356092 0.04724465241828281 -0.01847364358025878 0.005960923942142" /> <!--Index is order-->
95+
<numeric name="p_vmotor2rpm" data="2828.6593993157803 6686.5508078905705" /> <!--Index is order-->
96+
<numeric name="kf" data="8.701227710666256e-10" />
97+
<numeric name="km" data="7.94e-12" /> <!-- From old identification! TODO: Replace if possible-->
98+
99+
<!-- System Idenfication: Simple Model-->
100+
<numeric name="SI_roll" data="-12.7 10.15" />
101+
<numeric name="SI_pitch" data="-12.7 10.15" />
102+
<numeric name="SI_yaw" data="-8.117 14.36" />
103+
<numeric name="SI_acc" data="0.1906 0.4903" /> <!--Index is order-->
104+
105+
<!-- System Identification: Double Integrator Model-->
106+
<numeric name="DI_roll" data="-238.1 -21.35 179.65" />
107+
<numeric name="DI_pitch" data="-238.1 -21.35 179.65" />
108+
<numeric name="DI_yaw" data="-170.4 -22.22 280" />
109+
<!-- <numeric name="DI_acc" data="0.187 0.47" /> -->
110+
<numeric name="DI_acc" data="0.0 1.0633" />
111+
<!--Index is order-->
112+
113+
<!-- System Identification: Double Integrator Model with delay-->
114+
<numeric name="DI_D_roll" data="-238.1 -21.35 179.65" />
115+
<numeric name="DI_D_pitch" data="-238.1 -21.35 179.65" />
116+
<numeric name="DI_D_yaw" data="-170.4 -22.22 280" />
117+
<!--Index is order, bias, scale, tau-->
118+
<!-- <numeric name="DI_D_acc" data="-0.04 0.776 0.092" /> -->
119+
<numeric name="DI_D_acc" data="0.1704 0.5210 0.0923" />
120+
121+
<!-- System Identification: Double Integrator Model with drag and delay-->
122+
<numeric name="DI_DD_roll" data="-257.2 -20.2 207.1" />
123+
<numeric name="DI_DD_pitch" data="-257.2 -20.2 207.1" />
124+
<numeric name="DI_DD_yaw" data="-225.3 -28.61 392.7" />
125+
<numeric name="DI_DD_acc" data="1.0627 0.1891 -0.0177 0.0010" /> <!--c1, tau, c2, c3 -->
126+
127+
</custom>
128+
129+
</mujoco>

‎lsy_models/utils/constants.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ class Constants:
6565
DI_DD_ACC: Array
6666

6767
# Configs (used in testing)
68-
available_configs: tuple[str] = ("cf2x_L250", "cf2x_P250", "cf2x_T350")
68+
available_configs: tuple[str] = ("cf2x_L250", "cf2x_P250", "cf2x_T350", "cf2x_L350")
6969

7070
@classmethod
7171
def from_file(cls, path: str) -> Constants:
@@ -175,5 +175,7 @@ def from_config(cls, config: str) -> Constants:
175175
return Constants.from_file("data/cf2x_P250.xml")
176176
case "cf2x_T350":
177177
return Constants.from_file("data/cf2x_T350.xml")
178+
case "cf2x_L350":
179+
return Constants.from_file("data/cf2x_L350.xml")
178180
case _:
179181
raise ValueError(f"Drone config '{config}' is not supported")

0 commit comments

Comments
 (0)