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
|
// 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_core_constraint.c.
#include <cstddef>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_support.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::Pointwise;
using ::testing::DoubleNear;
using CoreConstraintTest = MujocoTest;
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
return std::vector<mjtNum>(array, array + n);
}
// compute rotation residual following formula in mj_instantiateEquality
void RotationResidual(const mjModel *model, mjData *data,
const mjtNum qpos[7], const mjtNum dqpos[6],
mjtNum res[3]) {
// copy configuration, compute required quantities with mj_step1
mju_copy(data->qpos, qpos, 7);
// perturb configuration if given
if (dqpos) {
mj_integratePos(model, data->qpos, dqpos, 1);
}
// update relevant quantities
mj_step1(model, data);
// compute orientation residual
mjtNum quat1[4], quat2[4], quat3[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(quat2, data->xquat+4*2);
mju_mulQuat(quat3, quat2, quat1);
mju_copy3(res, quat3+1);
}
// validate rotational Jacobian used in welds
TEST_F(CoreConstraintTest, WeldRotJacobian) {
constexpr char xml[] = R"(
<mujoco>
<option jacobian="dense"/>
<worldbody>
<body>
<joint type="ball"/>
<geom size=".1"/>
</body>
<body pos=".5 0 0">
<joint axis="1 0 0" pos="0 0 .01"/>
<joint axis="0 1 0" pos=".02 0 0"/>
<joint axis="0 0 1" pos="0 .03 0"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, testing::NotNull());
ASSERT_EQ(model->nq, 7);
ASSERT_EQ(model->nv, 6);
static const int nv = 6; // for increased readabilty
mjData* data = mj_makeData(model);
// arbitrary initial values for the ball and hinge joints
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
// compute required quantities using mj_step1
mj_step1(model, data);
// get orientation error
mjtNum res[3];
RotationResidual(model, data, qpos0, NULL, res);
// compute Jacobian with finite-differencing
mjtNum jacFD[3*nv];
mjtNum dqpos[nv] = {0};
mjtNum dres[3];
const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
// nudge i-th dof
dqpos[i] = eps;
// get nudged residual
RotationResidual(model, data, qpos0, dqpos, dres);
// remove nudge
dqpos[i] = 0.0;
// compute Jacobian column
for (int j=0; j < 3; j++) {
jacFD[nv*j + i] = (dres[j] - res[j]) / eps;
}
}
// reset mjData to qpos0
mju_copy(data->qpos, qpos0, 7);
mj_step1(model, data);
// intermediate quaternions quat1 and quat2
mjtNum quat1[4], negQuat2[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(negQuat2, data->xquat+4*2);
// get analytical Jacobian following formula in mj_instantiateEquality
mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv];
mjtNum point[3] = {0};
// rotational Jacobian difference
mj_jacDifPair(model, data, NULL, 2, 1, point, point,
NULL, NULL, NULL, jac0, jac1, jacdif);
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
mjtNum axis[3], quat3[4], quat4[4];
for (int j=0; j < nv; j++) {
// axis = [jac1-jac2]_col(j)
axis[0] = jacdif[0*nv+j];
axis[1] = jacdif[1*nv+j];
axis[2] = jacdif[2*nv+j];
// apply formula
mju_mulQuatAxis(quat3, negQuat2, axis);
mju_mulQuat(quat4, quat3, quat1);
// correct Jacobian
jacdif[0*nv+j] = 0.5*quat4[1];
jacdif[1*nv+j] = 0.5*quat4[2];
jacdif[2*nv+j] = 0.5*quat4[3];
}
// test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3*nv),
Pointwise(DoubleNear(eps), AsVector(jacdif, 3*nv)));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
|
