-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathquadrotor_quat.xml
executable file
·43 lines (37 loc) · 3.23 KB
/
quadrotor_quat.xml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
<!-- written by Inkyu Sa, enddl22@gmail.com -->
<!-- and Dongho Kang <eastsky.kang@gmail.com> -->
<mujoco model="quadrotor0">
<compiler inertiafromgeom="true" coordinate="local"/>
<option timestep="0.001" gravity="0 0 -9.81" density="1" viscosity="1e-5" />
<worldbody>
<geom name="floor" pos="0 0 -4" size="2 2 .2" type="plane" conaffinity="1" rgba="1 1 1 1" condim="3"/>
<body name="quadrotor" pos="0 0 -2">
<geom name="core" type="box" pos="0 0 0" quat = "1. 0. 0. 0" size="0.06 0.035 0.025" rgba="0.3 0.3 0.8 1" mass = ".2"/>
<geom name="a00" type="box" pos=".071 0.071 0.0" size="0.05 0.01 0.0025" quat = ".924 0.0 0.0 0.383" rgba="1 1 1 0.7" mass = ".025"/>
<geom name="a10" type="box" pos=".071 -0.071 0.0" size="0.05 0.01 0.0025" quat = ".383 0.0 0.0 0.924" rgba="1 1 1 0.7" mass = ".025"/>
<geom name="a20" type="box" pos="-0.071 -0.071 0.0" size="0.05 0.01 0.0025" quat = "-.383 0.0 0.0 0.924" rgba="1 1 1 0.7" mass = ".025"/>
<geom name="a30" type="box" pos="-.071 0.071 0.0" size="0.05 0.01 0.0025" quat = ".924 0.0 0.0 -0.383" rgba="1 1 1 0.7" mass = ".025"/>
<joint name="root" type="free" damping="0" armature="0" pos="0 0 0" />
<!-- Actuator sites to attach actuators, make it invisible -->
<site name="thrust" type="box" pos="0 0 0" size="0.035 0.035 0.035" quat = "1.0 0.0 0.0 0." rgba="0 1 1 0" />
<site name="rateX" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.06 0.035 0.025" rgba="0 1 1 0"/>
<site name="rateY" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.06 0.035 0.025" rgba="0 1 1 0"/>
<site name="rateZ" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.06 0.035 0.025" rgba="0 1 1 0"/>
<!-- Thruster geometries for collisions since site's are excluded from collision checking -->
<geom name="thruster0" type="cylinder" pos=" 0.1 0.1 0.01" size="0.05 0.0025" quat = "1.0 0.0 0.0 0." rgba="1 0.0 0.0 1" mass = ".025"/>
<geom name="thruster1" type="cylinder" pos=" 0.1 -0.1 0.01" size="0.05 0.0025" quat = "1.0 0.0 0.0 0." rgba="1 0.0 0.0 1" mass = ".025"/>
<geom name="thruster2" type="cylinder" pos="-0.1 -0.1 0.01" size="0.05 0.0025" quat = "1.0 0.0 0.0 0." rgba="0 1 0 1" mass = ".025"/>
<geom name="thruster3" type="cylinder" pos="-0.1 0.1 0.01" size="0.05 0.0025" quat = "1.0 0.0 0.0 0." rgba="0 1 0 1" mass = ".025"/>
<!-- Visualization of the coordinate frame -->
<site name="qcX" type="box" pos="0.1 0.0 0.0" size="0.1 0.005 0.005" quat = " 1.000 0.0 0.0 0." rgba="1 0 0 1" />
<site name="qcY" type="box" pos="0.0 0.1 0.0" size="0.1 0.005 0.005" quat = " 0.707 0.0 0.0 0.707" rgba="0 1 0 1" />
<site name="qcZ" type="box" pos="0.0 0.0 0.1" size="0.1 0.005 0.005" quat = "-0.707 0.0 0.707 0." rgba="0 0 1 1" />
</body>
</worldbody>
<actuator>
<motor ctrllimited="true" ctrlrange="0.0 7.0" gear="0 0. 1. 0. 0. 0." site="thrust"/>
<velocity ctrllimited="true" ctrlrange="-1 1" gear="0. 0. 0. 1. 0. 0." kv="0.1" site="rateX"/>
<velocity ctrllimited="true" ctrlrange="-1 1" gear="0. 0. 0. 0. 1. 0." kv="0.1" site="rateY"/>
<velocity ctrllimited="true" ctrlrange="-1 1" gear="0. 0. 0. 0. 0. 1." kv="0.1" site="rateZ"/>
</actuator>
</mujoco>