-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathball_bouncing_quad_fancy.xml
executable file
·64 lines (59 loc) · 4.99 KB
/
ball_bouncing_quad_fancy.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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
<!-- written by Inkyu Sa, enddl22@gmail.com -->
<mujoco model="bbq">
<compiler inertiafromgeom="auto" coordinate="local"/>
<option timestep='0.001' iterations="50" solver="PGS" gravity="0 0 -9.81" density="1" viscosity="1e-5">
<flag energy="enable"/>
</option>
<visual>
<map fogstart="3" fogend="5" force="0.1" znear="0.1"/>
<quality shadowsize="2048"/>
<global offwidth="800" offheight="800"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" width="128" height="128" rgb1=".4 .6 .8"
rgb2="0 0 0"/>
<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="127" height="1278"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512"/>
<material name='MatPlane' reflectance='0.5' texture="texplane" texrepeat="1 1" texuniform="true"/>
<material name='geom' texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<light directional='true' diffuse='.8 .8 .8' specular='0.3 0.3 0.3' pos='0 0 4.0' dir='0 -.15 -1'/>
<geom name="floor" pos="0 0 -4" size="10 10 .2" type="plane" material="MatPlane" 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.12 0.12 0.025" rgba="0.3 0.3 0.8 1" mass = ".2" contype="1" conaffinity="1" condim="3"/>
<geom name="a00" type="box" pos="0.18 0.18 0.0" size="0.08 0.01 0.0025" quat = ".924 0.0 0.0 0.383" rgba="0.3 0.3 0.8 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="a10" type="box" pos="0.18 -0.18 0.0" size="0.08 0.01 0.0025" quat = ".383 0.0 0.0 0.924" rgba="0.3 0.3 0.8 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="a20" type="box" pos="-0.18 -0.18 0.0" size="0.08 0.01 0.0025" quat = "-.383 0.0 0.0 0.924" rgba="0.3 0.3 0.8 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="a30" type="box" pos="-0.18 0.18 0.0" size="0.08 0.01 0.0025" quat = ".924 0.0 0.0 -0.383" rgba="0.3 0.3 0.8 1" mass = ".025" contype="0" conaffinity="0"/>
<joint name="root" type="free" damping="0" armature="0" pos="0 0 0" />
<!-- Actuator sites to attach actuators -->
<site name="thrust" type="box" pos="0 0 0" size="0.12 0.12 0.025" quat = "1.0 0.0 0.0 0." rgba="0.3 0.8 0.3 1"/>
<site name="rateX" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.12 0.12 0.025" rgba="0.3 0.8 0.3 1" />
<site name="rateY" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.12 0.12 0.025" rgba="0.3 0.8 0.3 1" />
<site name="rateZ" type="box" pos="0 0 0" quat = "1. 0. 0. 0." size="0.12 0.12 0.025" rgba="0.3 0.8 0.3 1" />
<!-- Thruster geometries for collisions since site's are excluded from collision checking -->
<geom name="thruster0" type="cylinder" pos=" 0.25 0.25 0.01" size="0.12 0.0025" quat = "1.0 0.0 0.0 0." rgba="0.3 0.8 0.3 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="thruster1" type="cylinder" pos=" 0.25 -0.25 0.01" size="0.12 0.0025" quat = "1.0 0.0 0.0 0." rgba="0.3 0.8 0.3 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="thruster2" type="cylinder" pos="-0.25 -0.25 0.01" size="0.12 0.0025" quat = "1.0 0.0 0.0 0." rgba="0.3 0.8 0.3 1" mass = ".025" contype="0" conaffinity="0"/>
<geom name="thruster3" type="cylinder" pos="-0.25 0.25 0.01" size="0.12 0.0025" quat = "1.0 0.0 0.0 0." rgba="0.3 0.8 0.3 1" mass = ".025" contype="0" conaffinity="0"/>
<!-- 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>
<body name="ball" pos="0 0 -1">
<joint name="ball_joint" type="free" armature='0' damping="0" limited="false"/>
<!-- <geom name="ball" type="sphere" size=".05" solref="0.01 -0.4" mass = ".01"/> -->
<geom name="ball" type="sphere" size=".05" solref="-100000 0" mass = ".01" contype="1" conaffinity="1" condim="3" rgba="1 1 0 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>