diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 324b9759..c1e9e548 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -70,10 +70,10 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d, int body, int flg_second, int NV, int start); // dense or sparse Jacobian difference for two body points: pos2 - pos1, global -int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, - int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3], - mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp, - mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr); +MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, + int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3], + mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp, + mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr); //-------------------------- name functions -------------------------------------------------------- diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index d8248b73..2cc20693 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -18,6 +18,9 @@ target_link_libraries(engine_collision_convex_test fixture gmock) mujoco_test(engine_collision_driver_test) target_link_libraries(engine_collision_driver_test fixture gmock) +mujoco_test(engine_core_constraint_test) +target_link_libraries(engine_core_constraint_test fixture gmock) + mujoco_test(engine_core_smooth_test) target_link_libraries(engine_core_smooth_test fixture gmock) diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc new file mode 100644 index 00000000..f5623f29 --- /dev/null +++ b/test/engine/engine_core_constraint_test.cc @@ -0,0 +1,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 +#include + +#include +#include +#include +#include +#include "src/engine/engine_support.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::Pointwise; +using ::testing::DoubleNear; +using CoreConstraintTest = MujocoTest; + +std::vector AsVector(const mjtNum* array, int n) { + return std::vector(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"( + + + )"; + 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