drones-sim 0.2.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- drones_sim/__init__.py +15 -0
- drones_sim/control/__init__.py +15 -0
- drones_sim/control/allocation.py +76 -0
- drones_sim/control/cascaded.py +204 -0
- drones_sim/control/geometric.py +170 -0
- drones_sim/control/lqr.py +174 -0
- drones_sim/control/pid.py +50 -0
- drones_sim/dynamics/__init__.py +13 -0
- drones_sim/dynamics/config.py +117 -0
- drones_sim/dynamics/disturbances.py +262 -0
- drones_sim/dynamics/quadcopter.py +317 -0
- drones_sim/estimation/__init__.py +2 -0
- drones_sim/estimation/ahrs.py +79 -0
- drones_sim/estimation/ekf.py +594 -0
- drones_sim/logging/__init__.py +13 -0
- drones_sim/logging/csv_logger.py +76 -0
- drones_sim/logging/json_logger.py +53 -0
- drones_sim/math_utils.py +119 -0
- drones_sim/models/__init__.py +21 -0
- drones_sim/models/quadcopter.urdf +296 -0
- drones_sim/models/urdf_loader.py +444 -0
- drones_sim/rl/__init__.py +16 -0
- drones_sim/rl/actions.py +219 -0
- drones_sim/rl/env.py +195 -0
- drones_sim/rl/observations.py +40 -0
- drones_sim/rl/reward.py +69 -0
- drones_sim/rl/tasks.py +73 -0
- drones_sim/sensors/__init__.py +3 -0
- drones_sim/sensors/gps.py +158 -0
- drones_sim/sensors/imu.py +196 -0
- drones_sim/sensors/models.py +113 -0
- drones_sim/simulation.py +283 -0
- drones_sim/state.py +133 -0
- drones_sim/trajectory.py +391 -0
- drones_sim/visualization/__init__.py +23 -0
- drones_sim/visualization/api.py +50 -0
- drones_sim/visualization/dashboard.py +74 -0
- drones_sim/visualization/plots.py +183 -0
- drones_sim/visualization/rerun_viewer.py +411 -0
- drones_sim/visualization/viewer.py +452 -0
- drones_sim-0.2.0.dist-info/METADATA +323 -0
- drones_sim-0.2.0.dist-info/RECORD +45 -0
- drones_sim-0.2.0.dist-info/WHEEL +5 -0
- drones_sim-0.2.0.dist-info/licenses/LICENSE +21 -0
- drones_sim-0.2.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,53 @@
|
|
|
1
|
+
"""JSON Lines telemetry logger."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import json
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
|
|
9
|
+
|
|
10
|
+
class JsonLogger:
|
|
11
|
+
"""Write simulation state as JSON Lines (one JSON object per line).
|
|
12
|
+
|
|
13
|
+
Usage identical to ``CsvLogger``::
|
|
14
|
+
|
|
15
|
+
logger = JsonLogger("flight.jsonl")
|
|
16
|
+
for ...:
|
|
17
|
+
logger.log(t, quad.get_position(), motors=motors)
|
|
18
|
+
logger.close()
|
|
19
|
+
"""
|
|
20
|
+
|
|
21
|
+
def __init__(self, path: str) -> None:
|
|
22
|
+
self._file = open(path, "w", encoding="utf-8") # noqa: SIM115
|
|
23
|
+
|
|
24
|
+
def log(
|
|
25
|
+
self,
|
|
26
|
+
t: float,
|
|
27
|
+
state: np.ndarray,
|
|
28
|
+
estimate: np.ndarray | None = None,
|
|
29
|
+
motors: np.ndarray | None = None,
|
|
30
|
+
) -> None:
|
|
31
|
+
qw, qx, qy, qz = state[6:10].tolist()
|
|
32
|
+
record = {
|
|
33
|
+
"t": float(t),
|
|
34
|
+
"position": [float(v) for v in state[:3]],
|
|
35
|
+
"velocity": [float(v) for v in state[3:6]],
|
|
36
|
+
"quaternion": [float(qw), float(qx), float(qy), float(qz)],
|
|
37
|
+
"angular_velocity": [float(v) for v in state[10:13]],
|
|
38
|
+
"motors": [float(v) for v in motors] if motors is not None else [],
|
|
39
|
+
"estimate": (
|
|
40
|
+
[float(v) for v in estimate] if estimate is not None else None
|
|
41
|
+
),
|
|
42
|
+
}
|
|
43
|
+
self._file.write(json.dumps(record) + "\n")
|
|
44
|
+
|
|
45
|
+
def __enter__(self):
|
|
46
|
+
return self
|
|
47
|
+
|
|
48
|
+
def __exit__(self, *args):
|
|
49
|
+
self.close()
|
|
50
|
+
|
|
51
|
+
def close(self) -> None:
|
|
52
|
+
if not self._file.closed:
|
|
53
|
+
self._file.close()
|
drones_sim/math_utils.py
ADDED
|
@@ -0,0 +1,119 @@
|
|
|
1
|
+
"""Quaternion and rotation matrix utilities.
|
|
2
|
+
|
|
3
|
+
Convention: quaternions are stored as [w, x, y, z] throughout this package.
|
|
4
|
+
scipy.spatial.transform.Rotation uses [x, y, z, w], so conversions are needed
|
|
5
|
+
at the boundary.
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from __future__ import annotations
|
|
9
|
+
|
|
10
|
+
import numpy as np
|
|
11
|
+
from numpy.typing import NDArray
|
|
12
|
+
|
|
13
|
+
# ---------------------------------------------------------------------------
|
|
14
|
+
# Quaternion operations
|
|
15
|
+
# ---------------------------------------------------------------------------
|
|
16
|
+
|
|
17
|
+
def quat_normalize(q: NDArray) -> NDArray:
|
|
18
|
+
"""Normalize a quaternion to unit magnitude."""
|
|
19
|
+
mag = np.linalg.norm(q)
|
|
20
|
+
return q / mag if mag > 0 else q
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
def quat_multiply(q1: NDArray, q2: NDArray) -> NDArray:
|
|
24
|
+
"""Hamilton product of two quaternions [w,x,y,z]."""
|
|
25
|
+
w1, x1, y1, z1 = q1
|
|
26
|
+
w2, x2, y2, z2 = q2
|
|
27
|
+
return np.array([
|
|
28
|
+
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
|
|
29
|
+
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
|
|
30
|
+
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
|
|
31
|
+
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
|
|
32
|
+
])
|
|
33
|
+
|
|
34
|
+
|
|
35
|
+
def quat_conjugate(q: NDArray) -> NDArray:
|
|
36
|
+
"""Conjugate (inverse for unit quaternion)."""
|
|
37
|
+
return np.array([q[0], -q[1], -q[2], -q[3]])
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
def quat_to_rotation_matrix(q: NDArray) -> NDArray:
|
|
41
|
+
"""Convert unit quaternion [w,x,y,z] to 3x3 rotation matrix."""
|
|
42
|
+
q = quat_normalize(q)
|
|
43
|
+
w, x, y, z = q
|
|
44
|
+
return np.array([
|
|
45
|
+
[1 - 2 * (y**2 + z**2), 2 * (x * y - w * z), 2 * (x * z + w * y)],
|
|
46
|
+
[2 * (x * y + w * z), 1 - 2 * (x**2 + z**2), 2 * (y * z - w * x)],
|
|
47
|
+
[2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x**2 + y**2)],
|
|
48
|
+
])
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
def quat_from_euler(roll: float, pitch: float, yaw: float) -> NDArray:
|
|
52
|
+
"""Convert Euler angles (xyz intrinsic) to quaternion [w,x,y,z].
|
|
53
|
+
|
|
54
|
+
Uses scipy internally for correctness, then converts to our convention.
|
|
55
|
+
"""
|
|
56
|
+
from scipy.spatial.transform import Rotation as R
|
|
57
|
+
q_xyzw = R.from_euler("xyz", [roll, pitch, yaw]).as_quat()
|
|
58
|
+
return np.roll(q_xyzw, 1) # xyzw -> wxyz
|
|
59
|
+
|
|
60
|
+
|
|
61
|
+
def quat_to_euler(q: NDArray) -> NDArray:
|
|
62
|
+
"""Convert quaternion [w,x,y,z] to Euler angles [roll, pitch, yaw]."""
|
|
63
|
+
from scipy.spatial.transform import Rotation as R
|
|
64
|
+
q_xyzw = np.roll(q, -1) # wxyz -> xyzw
|
|
65
|
+
return R.from_quat(q_xyzw).as_euler("xyz")
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
def quat_derivative(q: NDArray, omega: NDArray) -> NDArray:
|
|
69
|
+
"""Quaternion time-derivative given body angular velocity omega."""
|
|
70
|
+
omega_quat = np.array([0.0, omega[0], omega[1], omega[2]])
|
|
71
|
+
return 0.5 * quat_multiply(q, omega_quat)
|
|
72
|
+
|
|
73
|
+
|
|
74
|
+
def quat_angular_velocity_jacobian(omega: NDArray) -> NDArray:
|
|
75
|
+
"""4x4 Jacobian of quaternion derivative w.r.t. quaternion components.
|
|
76
|
+
|
|
77
|
+
Returns the Omega matrix such that q_dot = 0.5 * Omega @ q.
|
|
78
|
+
"""
|
|
79
|
+
wx, wy, wz = omega
|
|
80
|
+
return 0.5 * np.array([
|
|
81
|
+
[0, -wx, -wy, -wz],
|
|
82
|
+
[wx, 0, wz, -wy],
|
|
83
|
+
[wy, -wz, 0, wx],
|
|
84
|
+
[wz, wy, -wx, 0],
|
|
85
|
+
])
|
|
86
|
+
|
|
87
|
+
|
|
88
|
+
# ---------------------------------------------------------------------------
|
|
89
|
+
# Rotation matrix helpers (Euler-based, used by dynamics module)
|
|
90
|
+
# ---------------------------------------------------------------------------
|
|
91
|
+
|
|
92
|
+
def euler_to_rotation_matrix(phi: float, theta: float, psi: float) -> NDArray:
|
|
93
|
+
"""ZYX rotation matrix from Euler angles (roll=phi, pitch=theta, yaw=psi)."""
|
|
94
|
+
R_x = np.array([
|
|
95
|
+
[1, 0, 0],
|
|
96
|
+
[0, np.cos(phi), -np.sin(phi)],
|
|
97
|
+
[0, np.sin(phi), np.cos(phi)],
|
|
98
|
+
])
|
|
99
|
+
R_y = np.array([
|
|
100
|
+
[np.cos(theta), 0, np.sin(theta)],
|
|
101
|
+
[0, 1, 0],
|
|
102
|
+
[-np.sin(theta), 0, np.cos(theta)],
|
|
103
|
+
])
|
|
104
|
+
R_z = np.array([
|
|
105
|
+
[np.cos(psi), -np.sin(psi), 0],
|
|
106
|
+
[np.sin(psi), np.cos(psi), 0],
|
|
107
|
+
[0, 0, 1],
|
|
108
|
+
])
|
|
109
|
+
return R_z @ R_y @ R_x
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
def angular_vel_to_euler_rates(phi: float, theta: float, omega_body: NDArray) -> NDArray:
|
|
113
|
+
"""Convert body angular velocity [p,q,r] to Euler angle rates."""
|
|
114
|
+
T = np.array([
|
|
115
|
+
[1, 0, -np.sin(theta)],
|
|
116
|
+
[0, np.cos(phi), np.cos(theta) * np.sin(phi)],
|
|
117
|
+
[0, -np.sin(phi), np.cos(theta) * np.cos(phi)],
|
|
118
|
+
])
|
|
119
|
+
return np.linalg.solve(T, omega_body)
|
|
@@ -0,0 +1,21 @@
|
|
|
1
|
+
"""Drone model assets — URDF and mesh utilities."""
|
|
2
|
+
|
|
3
|
+
from .urdf_loader import (
|
|
4
|
+
DroneURDFModel,
|
|
5
|
+
URDFGeometry,
|
|
6
|
+
URDFJoint,
|
|
7
|
+
URDFLink,
|
|
8
|
+
geometry_to_mesh,
|
|
9
|
+
get_urdf_path,
|
|
10
|
+
load_drone_urdf,
|
|
11
|
+
)
|
|
12
|
+
|
|
13
|
+
__all__ = [
|
|
14
|
+
"DroneURDFModel",
|
|
15
|
+
"URDFGeometry",
|
|
16
|
+
"URDFJoint",
|
|
17
|
+
"URDFLink",
|
|
18
|
+
"geometry_to_mesh",
|
|
19
|
+
"get_urdf_path",
|
|
20
|
+
"load_drone_urdf",
|
|
21
|
+
]
|
|
@@ -0,0 +1,296 @@
|
|
|
1
|
+
<?xml version="1.0" encoding="UTF-8"?>
|
|
2
|
+
<robot name="quadcopter">
|
|
3
|
+
|
|
4
|
+
<!-- ============================================================
|
|
5
|
+
Quadcopter URDF — X-configuration
|
|
6
|
+
Physical params match QuadcopterDynamics defaults:
|
|
7
|
+
mass=1.0 kg, arm_length=0.2 m,
|
|
8
|
+
inertia=diag(0.01, 0.01, 0.018) kg·m²
|
|
9
|
+
Motor layout (top view):
|
|
10
|
+
1 (front, +X)
|
|
11
|
+
4 (left, -Y) 2 (right, +Y)
|
|
12
|
+
3 (back, -X)
|
|
13
|
+
============================================================ -->
|
|
14
|
+
|
|
15
|
+
<!-- ======================== Materials ======================== -->
|
|
16
|
+
<material name="blue">
|
|
17
|
+
<color rgba="0.196 0.196 0.784 1.0"/>
|
|
18
|
+
</material>
|
|
19
|
+
<material name="red">
|
|
20
|
+
<color rgba="1.0 0.196 0.196 1.0"/>
|
|
21
|
+
</material>
|
|
22
|
+
<material name="green">
|
|
23
|
+
<color rgba="0.196 0.784 0.196 1.0"/>
|
|
24
|
+
</material>
|
|
25
|
+
<material name="grey">
|
|
26
|
+
<color rgba="0.392 0.392 0.392 1.0"/>
|
|
27
|
+
</material>
|
|
28
|
+
<material name="dark_grey">
|
|
29
|
+
<color rgba="0.25 0.25 0.25 1.0"/>
|
|
30
|
+
</material>
|
|
31
|
+
|
|
32
|
+
<!-- ======================== Base Link ======================== -->
|
|
33
|
+
<link name="base_link">
|
|
34
|
+
<inertial>
|
|
35
|
+
<mass value="0.6"/>
|
|
36
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
37
|
+
<inertia ixx="0.0006" ixy="0" ixz="0"
|
|
38
|
+
iyy="0.0006" iyz="0"
|
|
39
|
+
izz="0.001"/>
|
|
40
|
+
</inertial>
|
|
41
|
+
<visual>
|
|
42
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
43
|
+
<geometry>
|
|
44
|
+
<box size="0.1 0.1 0.04"/>
|
|
45
|
+
</geometry>
|
|
46
|
+
<material name="blue"/>
|
|
47
|
+
</visual>
|
|
48
|
+
<collision>
|
|
49
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
50
|
+
<geometry>
|
|
51
|
+
<box size="0.1 0.1 0.04"/>
|
|
52
|
+
</geometry>
|
|
53
|
+
</collision>
|
|
54
|
+
</link>
|
|
55
|
+
|
|
56
|
+
<!-- ======================== Arm 1 (Front, +X) ================ -->
|
|
57
|
+
<link name="arm_1">
|
|
58
|
+
<inertial>
|
|
59
|
+
<mass value="0.05"/>
|
|
60
|
+
<origin xyz="0.05 0 0" rpy="0 0 0"/>
|
|
61
|
+
<inertia ixx="0.000001" ixy="0" ixz="0"
|
|
62
|
+
iyy="0.00005" iyz="0"
|
|
63
|
+
izz="0.00005"/>
|
|
64
|
+
</inertial>
|
|
65
|
+
<visual>
|
|
66
|
+
<origin xyz="0.05 0 0" rpy="0 1.5708 0"/>
|
|
67
|
+
<geometry>
|
|
68
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
69
|
+
</geometry>
|
|
70
|
+
<material name="red"/>
|
|
71
|
+
</visual>
|
|
72
|
+
<collision>
|
|
73
|
+
<origin xyz="0.05 0 0" rpy="0 1.5708 0"/>
|
|
74
|
+
<geometry>
|
|
75
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
76
|
+
</geometry>
|
|
77
|
+
</collision>
|
|
78
|
+
</link>
|
|
79
|
+
|
|
80
|
+
<joint name="base_to_arm_1" type="fixed">
|
|
81
|
+
<parent link="base_link"/>
|
|
82
|
+
<child link="arm_1"/>
|
|
83
|
+
<origin xyz="0.05 0 0" rpy="0 0 0"/>
|
|
84
|
+
</joint>
|
|
85
|
+
|
|
86
|
+
<!-- ======================== Arm 2 (Right, +Y) ================ -->
|
|
87
|
+
<link name="arm_2">
|
|
88
|
+
<inertial>
|
|
89
|
+
<mass value="0.05"/>
|
|
90
|
+
<origin xyz="0 0.05 0" rpy="0 0 0"/>
|
|
91
|
+
<inertia ixx="0.00005" ixy="0" ixz="0"
|
|
92
|
+
iyy="0.000001" iyz="0"
|
|
93
|
+
izz="0.00005"/>
|
|
94
|
+
</inertial>
|
|
95
|
+
<visual>
|
|
96
|
+
<origin xyz="0 0.05 0" rpy="1.5708 0 0"/>
|
|
97
|
+
<geometry>
|
|
98
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
99
|
+
</geometry>
|
|
100
|
+
<material name="green"/>
|
|
101
|
+
</visual>
|
|
102
|
+
<collision>
|
|
103
|
+
<origin xyz="0 0.05 0" rpy="1.5708 0 0"/>
|
|
104
|
+
<geometry>
|
|
105
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
106
|
+
</geometry>
|
|
107
|
+
</collision>
|
|
108
|
+
</link>
|
|
109
|
+
|
|
110
|
+
<joint name="base_to_arm_2" type="fixed">
|
|
111
|
+
<parent link="base_link"/>
|
|
112
|
+
<child link="arm_2"/>
|
|
113
|
+
<origin xyz="0 0.05 0" rpy="0 0 0"/>
|
|
114
|
+
</joint>
|
|
115
|
+
|
|
116
|
+
<!-- ======================== Arm 3 (Back, -X) ================= -->
|
|
117
|
+
<link name="arm_3">
|
|
118
|
+
<inertial>
|
|
119
|
+
<mass value="0.05"/>
|
|
120
|
+
<origin xyz="-0.05 0 0" rpy="0 0 0"/>
|
|
121
|
+
<inertia ixx="0.000001" ixy="0" ixz="0"
|
|
122
|
+
iyy="0.00005" iyz="0"
|
|
123
|
+
izz="0.00005"/>
|
|
124
|
+
</inertial>
|
|
125
|
+
<visual>
|
|
126
|
+
<origin xyz="-0.05 0 0" rpy="0 1.5708 0"/>
|
|
127
|
+
<geometry>
|
|
128
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
129
|
+
</geometry>
|
|
130
|
+
<material name="grey"/>
|
|
131
|
+
</visual>
|
|
132
|
+
<collision>
|
|
133
|
+
<origin xyz="-0.05 0 0" rpy="0 1.5708 0"/>
|
|
134
|
+
<geometry>
|
|
135
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
136
|
+
</geometry>
|
|
137
|
+
</collision>
|
|
138
|
+
</link>
|
|
139
|
+
|
|
140
|
+
<joint name="base_to_arm_3" type="fixed">
|
|
141
|
+
<parent link="base_link"/>
|
|
142
|
+
<child link="arm_3"/>
|
|
143
|
+
<origin xyz="-0.05 0 0" rpy="0 0 0"/>
|
|
144
|
+
</joint>
|
|
145
|
+
|
|
146
|
+
<!-- ======================== Arm 4 (Left, -Y) ================= -->
|
|
147
|
+
<link name="arm_4">
|
|
148
|
+
<inertial>
|
|
149
|
+
<mass value="0.05"/>
|
|
150
|
+
<origin xyz="0 -0.05 0" rpy="0 0 0"/>
|
|
151
|
+
<inertia ixx="0.00005" ixy="0" ixz="0"
|
|
152
|
+
iyy="0.000001" iyz="0"
|
|
153
|
+
izz="0.00005"/>
|
|
154
|
+
</inertial>
|
|
155
|
+
<visual>
|
|
156
|
+
<origin xyz="0 -0.05 0" rpy="1.5708 0 0"/>
|
|
157
|
+
<geometry>
|
|
158
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
159
|
+
</geometry>
|
|
160
|
+
<material name="grey"/>
|
|
161
|
+
</visual>
|
|
162
|
+
<collision>
|
|
163
|
+
<origin xyz="0 -0.05 0" rpy="1.5708 0 0"/>
|
|
164
|
+
<geometry>
|
|
165
|
+
<cylinder radius="0.01" length="0.1"/>
|
|
166
|
+
</geometry>
|
|
167
|
+
</collision>
|
|
168
|
+
</link>
|
|
169
|
+
|
|
170
|
+
<joint name="base_to_arm_4" type="fixed">
|
|
171
|
+
<parent link="base_link"/>
|
|
172
|
+
<child link="arm_4"/>
|
|
173
|
+
<origin xyz="0 -0.05 0" rpy="0 0 0"/>
|
|
174
|
+
</joint>
|
|
175
|
+
|
|
176
|
+
<!-- ======================== Rotor 1 (Front, +X) ============== -->
|
|
177
|
+
<link name="rotor_1">
|
|
178
|
+
<inertial>
|
|
179
|
+
<mass value="0.05"/>
|
|
180
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
181
|
+
<inertia ixx="0.000045" ixy="0" ixz="0"
|
|
182
|
+
iyy="0.000045" iyz="0"
|
|
183
|
+
izz="0.00009"/>
|
|
184
|
+
</inertial>
|
|
185
|
+
<visual>
|
|
186
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
187
|
+
<geometry>
|
|
188
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
189
|
+
</geometry>
|
|
190
|
+
<material name="red"/>
|
|
191
|
+
</visual>
|
|
192
|
+
<collision>
|
|
193
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
194
|
+
<geometry>
|
|
195
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
196
|
+
</geometry>
|
|
197
|
+
</collision>
|
|
198
|
+
</link>
|
|
199
|
+
|
|
200
|
+
<joint name="arm_1_to_rotor_1" type="fixed">
|
|
201
|
+
<parent link="arm_1"/>
|
|
202
|
+
<child link="rotor_1"/>
|
|
203
|
+
<origin xyz="0.1 0 0.01" rpy="0 0 0"/>
|
|
204
|
+
</joint>
|
|
205
|
+
|
|
206
|
+
<!-- ======================== Rotor 2 (Right, +Y) ============== -->
|
|
207
|
+
<link name="rotor_2">
|
|
208
|
+
<inertial>
|
|
209
|
+
<mass value="0.05"/>
|
|
210
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
211
|
+
<inertia ixx="0.000045" ixy="0" ixz="0"
|
|
212
|
+
iyy="0.000045" iyz="0"
|
|
213
|
+
izz="0.00009"/>
|
|
214
|
+
</inertial>
|
|
215
|
+
<visual>
|
|
216
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
217
|
+
<geometry>
|
|
218
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
219
|
+
</geometry>
|
|
220
|
+
<material name="green"/>
|
|
221
|
+
</visual>
|
|
222
|
+
<collision>
|
|
223
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
224
|
+
<geometry>
|
|
225
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
226
|
+
</geometry>
|
|
227
|
+
</collision>
|
|
228
|
+
</link>
|
|
229
|
+
|
|
230
|
+
<joint name="arm_2_to_rotor_2" type="fixed">
|
|
231
|
+
<parent link="arm_2"/>
|
|
232
|
+
<child link="rotor_2"/>
|
|
233
|
+
<origin xyz="0 0.1 0.01" rpy="0 0 0"/>
|
|
234
|
+
</joint>
|
|
235
|
+
|
|
236
|
+
<!-- ======================== Rotor 3 (Back, -X) =============== -->
|
|
237
|
+
<link name="rotor_3">
|
|
238
|
+
<inertial>
|
|
239
|
+
<mass value="0.05"/>
|
|
240
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
241
|
+
<inertia ixx="0.000045" ixy="0" ixz="0"
|
|
242
|
+
iyy="0.000045" iyz="0"
|
|
243
|
+
izz="0.00009"/>
|
|
244
|
+
</inertial>
|
|
245
|
+
<visual>
|
|
246
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
247
|
+
<geometry>
|
|
248
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
249
|
+
</geometry>
|
|
250
|
+
<material name="dark_grey"/>
|
|
251
|
+
</visual>
|
|
252
|
+
<collision>
|
|
253
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
254
|
+
<geometry>
|
|
255
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
256
|
+
</geometry>
|
|
257
|
+
</collision>
|
|
258
|
+
</link>
|
|
259
|
+
|
|
260
|
+
<joint name="arm_3_to_rotor_3" type="fixed">
|
|
261
|
+
<parent link="arm_3"/>
|
|
262
|
+
<child link="rotor_3"/>
|
|
263
|
+
<origin xyz="-0.1 0 0.01" rpy="0 0 0"/>
|
|
264
|
+
</joint>
|
|
265
|
+
|
|
266
|
+
<!-- ======================== Rotor 4 (Left, -Y) =============== -->
|
|
267
|
+
<link name="rotor_4">
|
|
268
|
+
<inertial>
|
|
269
|
+
<mass value="0.05"/>
|
|
270
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
271
|
+
<inertia ixx="0.000045" ixy="0" ixz="0"
|
|
272
|
+
iyy="0.000045" iyz="0"
|
|
273
|
+
izz="0.00009"/>
|
|
274
|
+
</inertial>
|
|
275
|
+
<visual>
|
|
276
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
277
|
+
<geometry>
|
|
278
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
279
|
+
</geometry>
|
|
280
|
+
<material name="dark_grey"/>
|
|
281
|
+
</visual>
|
|
282
|
+
<collision>
|
|
283
|
+
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
284
|
+
<geometry>
|
|
285
|
+
<cylinder radius="0.06" length="0.005"/>
|
|
286
|
+
</geometry>
|
|
287
|
+
</collision>
|
|
288
|
+
</link>
|
|
289
|
+
|
|
290
|
+
<joint name="arm_4_to_rotor_4" type="fixed">
|
|
291
|
+
<parent link="arm_4"/>
|
|
292
|
+
<child link="rotor_4"/>
|
|
293
|
+
<origin xyz="0 -0.1 0.01" rpy="0 0 0"/>
|
|
294
|
+
</joint>
|
|
295
|
+
|
|
296
|
+
</robot>
|