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
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
|
// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for engine/engine_forward.c.
#include "src/engine/engine_forward.h"
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "src/cc/array_safety.h"
#include "src/engine/engine_io.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
return std::vector<mjtNum>(array, array + n);
}
static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::Ne;
using ::testing::HasSubstr;
// --------------------------- activation limits -------------------------------
struct ActLimitedTestCase {
std::string test_name;
mjtIntegrator integrator;
};
using ParametrizedForwardTest = ::testing::TestWithParam<ActLimitedTestCase>;
TEST_P(ParametrizedForwardTest, ActLimited) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" gainprm="100" biasprm="0 -100" biastype="affine"
dynprm="10" dyntype="integrator"
actlimited="true" actrange="-1 1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
model->opt.integrator = GetParam().integrator;
data->ctrl[0] = 1.0;
// integrating up from 0, we will hit the clamp after 99 steps
for (int i=0; i<200; i++) {
mj_step(model, data);
// always greater than lower bound
EXPECT_GT(data->act[0], -1);
// after 99 steps we hit the upper bound
if (i < 99) EXPECT_LT(data->act[0], 1);
if (i >= 99) EXPECT_EQ(data->act[0], 1);
}
data->ctrl[0] = -1.0;
// integrating down from 1, we will hit the clamp after 199 steps
for (int i=0; i<300; i++) {
mj_step(model, data);
// always smaller than upper bound
EXPECT_LT(data->act[0], model->actuator_actrange[1]);
// after 199 steps we hit the lower bound
if (i < 199) EXPECT_GT(data->act[0], model->actuator_actrange[0]);
if (i >= 199) EXPECT_EQ(data->act[0], model->actuator_actrange[0]);
}
mj_deleteData(data);
mj_deleteModel(model);
}
INSTANTIATE_TEST_SUITE_P(
ParametrizedForwardTest, ParametrizedForwardTest,
testing::ValuesIn<ActLimitedTestCase>({
{"Euler", mjINT_EULER},
{"Implicit", mjINT_IMPLICIT},
{"RK4", mjINT_RK4},
}),
[](const testing::TestParamInfo<ParametrizedForwardTest::ParamType>& info) {
return info.param.test_name;
});
// --------------------------- damping actuator --------------------------------
using ForwardTest = MujocoTest;
TEST_F(ForwardTest, DamperDampens) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt"/>
<damper joint="jnt" kv="1000" ctrlrange="0 100"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// move the joint
data->ctrl[0] = 100.0;
data->ctrl[1] = 0.0;
for (int i=0; i<100; i++)
mj_step(model, data);
// stop the joint with damping
data->ctrl[0] = 0.0;
data->ctrl[1] = 100.0;
for (int i=0; i<1000; i++)
mj_step(model, data);
EXPECT_LE(data->qvel[0], std::numeric_limits<double>::epsilon());
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- implicit integrator -----------------------------
using ImplicitIntegratorTest = MujocoTest;
// Euler and implicit should be equivalent if there is only joint damping
TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// step 10 times with Euler, save copy of qpos as vector
for (int i=0; i<10; i++) {
mj_step(model, data);
}
std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
// reset, step 10 times with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
mj_step(model, data);
}
// expect qpos vectors to be numerically different
EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(Ne(), qposEuler));
// expect qpos vectors to be similar to high precision
EXPECT_THAT(AsVector(data->qpos, model->nq),
Pointwise(DoubleNear(1e-14), qposEuler));
mj_deleteData(data);
mj_deleteModel(model);
}
// Joint and actuator damping should integrate identically under implicit
TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
const std::string xml_path = GetTestDataFilePath(kDampedActuatorsPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// take 1000 steps with Euler
for (int i=0; i<1000; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be significantly different
EXPECT_GT(fabs(data->qpos[0]-data->qpos[2]), 1e-4);
EXPECT_GT(fabs(data->qpos[1]-data->qpos[3]), 1e-4);
// reset, take 1000 steps with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be insignificantly different
EXPECT_LT(fabs(data->qpos[0]-data->qpos[2]), 1e-16);
EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), 1e-16);
mj_deleteData(data);
mj_deleteModel(model);
}
// Energy conservation: RungeKutta > implicit > Euler
TEST_F(ImplicitIntegratorTest, EnergyConservation) {
const std::string xml_path =
GetTestDataFilePath(kEnergyConservingPendulumPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
const int nstep = 500; // number of steps to take
// take nstep steps with Euler, measure energy (potential + kinetic)
model->opt.integrator = mjINT_EULER;
for (int i=0; i<nstep; i++) {
mj_step(model, data);
}
mjtNum energyEuler = data->energy[0] + data->energy[1];
// take nstep steps with implicit, measure energy
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
mj_step(model, data);
}
mjtNum energyImplicit = data->energy[0] + data->energy[1];
// take nstep steps with 4th order Runge-Kutta, measure energy
model->opt.integrator = mjINT_RK4;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
mj_step(model, data);
}
mjtNum energyRK4 = data->energy[0] + data->energy[1];
// energy was measured: expect all energies to be nonzero
EXPECT_NE(energyEuler, 0);
EXPECT_NE(energyImplicit, 0);
EXPECT_NE(energyRK4, 0);
// test conservation: perfectly conserved energy would remain 0.0
// expect RK4 to be better than implicit
EXPECT_LT(fabs(energyRK4), fabs(energyImplicit));
// expect implicit to be better than Euler
EXPECT_LT(fabs(energyImplicit), fabs(energyEuler));
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- control clamping --------------------------------
TEST_F(ForwardTest, ControlClamping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="slide" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<motor name="unclamped" joint="slide"/>
<motor name="clamped" joint="slide" ctrllimited="true" ctrlrange="-1 1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
// for the unclamped actuator, ctrl={1, 2} produce different accelerations
data->ctrl[0] = 1;
mj_forward(model, data);
mjtNum qacc1 = data->qacc[0];
data->ctrl[0] = 2;
mj_forward(model, data);
mjtNum qacc2 = data->qacc[0];
EXPECT_NE(qacc1, qacc2);
// for the clamped actuator, ctrl={1, 2} produce identical accelerations
data->ctrl[1] = 1;
mj_forward(model, data);
qacc1 = data->qacc[0];
data->ctrl[1] = 2;
mj_forward(model, data);
qacc2 = data->qacc[0];
EXPECT_EQ(qacc1, qacc2);
// data->ctrl[1] remains pristine
EXPECT_EQ(data->ctrl[1], 2);
// install warning handler
static char warning[1024];
warning[0] = '\0';
mju_user_warning = [](const char* msg) {
util::strcpy_arr(warning, msg);
};
// for the unclamped actuator, huge raises warning
data->ctrl[0] = 10*mjMAXVAL;
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0"));
// for the clamped actuator, huge does not raise warning
mj_resetData(model, data);
warning[0] = '\0';
data->ctrl[1] = 10*mjMAXVAL;
mj_forward(model, data);
EXPECT_EQ(warning[0], '\0');
// for the clamped actuator, NaN raises warning
mj_resetData(model, data);
data->ctrl[1] = std::numeric_limits<double>::quiet_NaN();
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1"));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
|
