-
Notifications
You must be signed in to change notification settings - Fork 1
/
inchworm.xml
146 lines (117 loc) · 6.23 KB
/
inchworm.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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
<!--An inchworm xml environment in MuJoCo-->
<mujoco model="2d_inchworm">
<!--
`inertiafromgeom` specifies whether body masses and inertias are inferred from their attached geoms
or defined by explicit attribute values
-->
<compiler angle="degree" inertiafromgeom="true" />
<!--
`integrator` is for numerical integration, see: https://mujoco.readthedocs.io/en/latest/computation.html#geintegration
NOTE: `timestep` must be equal to 1 / (frame_skip * metadata['render_fps'])
-->
<option integrator="RK4" timestep="0.002" />
<!-- Default attribute values for various elements -->
<default>
<!-- These joints are the ones connecting the inchworm segments -->
<default class="connector_joint">
<joint type="hinge" armature="1" damping="1" limited="true" axis="0 1 0"/>
</default>
<geom conaffinity="0" condim="3" density="5.0" friction="1 1 1" margin="0.01" rgba="0.8 0.4 0.8 1" />
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" />
</default>
<!-- Visual textures and materials -->
<asset>
<!-- The skybox texture -->
<texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100" />
<!--
The checker texture used for the floor material.
`height` and `width` determine the resolution
-->
<texture name="checker_tex" builtin="checker" type="2d" rgb1="0 0 0" rgb2="0.8 0.8 0.8" height="100" width="100" />
<!--
The checker material used for the floor geom.
`texrepeat` determines the pattern scaling
-->
<material name="checker_mat" texture="checker_tex" texrepeat="1.5 1.5" texuniform="true" reflectance="0.5" shininess="1" specular="1" />
</asset>
<!-- The physical environment -->
<worldbody>
<!-- Light source -->
<light cutoff="100" diffuse="1 1 1" dir="0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular="0.1 0.1 0.1" />
<!--
The floor.
A size of 0 means infinite in that dimension.
According to MuJoCo docs, the third size parameter for a plane represents
the spacing between square grid lines for rendering, but not sure what that means.
-->
<geom type="plane" size="0 8 1" conaffinity="1" condim="3" material="checker_mat" rgba="0.8 0.9 0.8 1" />
<!-- Segment 1: Left Foot -->
<body name="left_foot" pos="0 0 1">
<!-- Left end of foot -->
<geom type="cylinder" mass="0" size="0.225" fromto="-0.5 -0.201 0 -0.5 0.201 0" rgba="0.8 0 0 1"/>
<!-- Top of foot -->
<geom type="box" pos="0 0 0.025" size="0.5 0.2 0.2" rgba="0.8 0 0 1"/>
<!-- Bottom of foot -->
<geom type="box" pos="-0.425 0 -0.2" size="0.075 0.2 0.025" rgba="0.8 0 0 1"/>
<body name="left_gripper" pos="0 0 -0.2">
<geom name="left_gripper_geom" type="box" size="0.35 0.2 0.025" margin="0.04" gap="0.03" rgba="0 0.5 0.8 1"/>
</body>
<geom type="box" pos="0.425 0 -0.2" size="0.075 0.2 0.025" rgba="0.8 0 0 1"/>
<!-- Right end of foot -->
<geom type="cylinder" mass="0" size="0.225" fromto="0.5 -0.201 0 0.5 0.201 0" rgba="0.8 0 0 1"/>
<!--
These three joints allow the first segment, which is the root segment of the inchworm,
hierarchically speaking, to slide along the x and y axes as well as rotate within the x-z plane,
relative to its parent body (the worldbody). This effectively constrains the inchworm to two dimensions.
-->
<joint name="hc_joint" type="hinge" axis="0 1 0" />
<joint name="hsx_joint" type="slide" axis="1 0 0" />
<joint name="hsz_joint" type="slide" axis="0 0 1" />
<!-- Segment 2: Left Middle -->
<body name="left_middle" pos="1.5 0 0">
<!-- Camera to track the inchworm body (fixed on the left_middle body segment) -->
<camera name="track" mode="trackcom" pos="1 -10 0.3" xyaxes="1 0 0 0 0 1"/>
<geom type="box" size="1 0.2 0.2" rgba="0 0.8 0 1"/>
<body name="mid_point" pos="1 0 0">
<geom type="cylinder" mass="0" size="0.2" fromto="0 -0.201 0 0 0.201 0" rgba="0 0 0.8 1"/>
</body>
<!-- This joint allows the second segment to hinge off the first -->
<joint name="left_joint" class="connector_joint" pos="-1 0 0" range="-90 0" />
<!-- Segment 3: Right Middle -->
<body name="right_middle" pos="2 0 0">
<geom type="box" size="1 0.2 0.2" rgba="0 0 0.8 1"/>
<!-- This joint allows the third segment to hinge off the second -->
<joint name="middle_joint" class="connector_joint" pos="-1 0 0" range="0 150" />
<!-- Segment 4: Right Foot -->
<body name="right_foot" pos="1.5 0 0">
<!-- Left end of foot -->
<geom type="cylinder" mass="0" size="0.225" fromto="-0.5 -0.201 0 -0.5 0.201 0" rgba="0.8 0.8 0 1"/>
<!-- Top of foot -->
<geom type="box" pos="0 0 0.025" size="0.5 0.2 0.2" rgba="0.8 0.8 0 1"/>
<!-- Bottom of foot -->
<geom type="box" pos="-0.425 0 -0.2" size="0.075 0.2 0.025" rgba="0.8 0.8 0 1"/>
<body name="right_gripper" pos="0 0 -0.2">
<geom name="right_gripper_geom" type="box" size="0.35 0.2 0.025" margin="0.04" gap="0.03" rgba="0 0.5 0.8 1"/>
</body>
<!-- <geom type="box" pos="0 0 -0.2" size="0.35 0.2 0.025" rgba="0 0.5 0.8 1"/>-->
<geom type="box" pos="0.425 0 -0.2" size="0.075 0.2 0.025" rgba="0.8 0.8 0 1"/>
<!-- Right end of foot -->
<geom type="cylinder" mass="0" size="0.225" fromto="0.5 -0.201 0 0.5 0.201 0" rgba="0.8 0.8 0 1"/>
<!-- This joint allows the fourth segment to hinge off the third -->
<joint name="right_joint" class="connector_joint" pos="-0.5 0 0" range="-90 0" />
</body>
</body>
</body>
</body>
</worldbody>
<!-- The actuators (the agent's means of interacting with the environment) -->
<actuator>
<!-- Motors for the three connector joints -->
<motor joint="left_joint" />
<motor joint="middle_joint" />
<motor joint="right_joint" />
<!-- Adhesion for the two feet (`gain` is the force of the adhesion) -->
<adhesion body="left_foot" ctrlrange="0 1" forcelimited="false" gain="10000" />
<adhesion body="right_foot" ctrlrange="0 1" forcelimited="false" gain="10000" />
</actuator>
</mujoco>