// 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_support.c. #include #include #include #include #include "test/fixture.h" namespace mujoco { namespace { using ::testing::DoubleNear; using ::testing::ContainsRegex; using ::testing::MatchesRegex; using JacobianTest = MujocoTest; static const mjtNum max_abs_err = std::numeric_limits::epsilon(); static constexpr char kJacobianTestingModel[] = R"( )"; // compare analytic and finite-differenced subtree-com Jacobian TEST_F(JacobianTest, SubtreeJac) { mjModel* model = LoadModelFromString(kJacobianTestingModel); int nv = model->nv; int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); mjData* data = mj_makeData(model); mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq); mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); // all we need for Jacobians are kinematics and CoM-related quantitites mj_kinematics(model, data); mj_comPos(model, data); // get subtree CoM Jacobian of free body mj_jacSubtreeCom(model, data, jac_subtree, bodyid); // save current subtree-com and qpos, clear nudge mjtNum subtree_com[3]; mju_copy3(subtree_com, data->subtree_com+3*bodyid); mju_copy(qpos, data->qpos, model->nq); mju_zero(nudge, nv); // compare analytic Jacobian to finite-difference approximation static const mjtNum eps = 1e-6; for (int i=0; iqpos, reset nudge mju_copy(data->qpos, qpos, model->nq); nudge[i] = 1; mj_integratePos(model, data->qpos, nudge, eps); nudge[i] = 0; // kinematics and comPos to get nudged com mj_kinematics(model, data); mj_comPos(model, data); // compare finite-differenced and analytic Jacobian for (int j=0; j<3; j++) { mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; EXPECT_THAT(jac_subtree[nv*j+i], DoubleNear(findiff, eps)); } } mju_free(nudge); mju_free(qpos); mju_free(jac_subtree); mj_deleteData(data); mj_deleteModel(model); } // confirm that applying linear forces via the subtree-com Jacobian only creates // the expected linear accelerations (no accelerations of internal joints) TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { mjModel* model = LoadModelFromString(kJacobianTestingModel); int nv = model->nv; int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); mjData* data = mj_makeData(model); mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); // all we need for Jacobians are kinematics and CoM-related quantitites mj_kinematics(model, data); mj_comPos(model, data); // get subtree CoM Jacobian of free body mj_jacSubtreeCom(model, data, jac_subtree, bodyid); // uncomment for debugging // mju_printMat(jac_subtree, 3, nv); // call fwdPosition since we'll need the factorised mass matrix in the test mj_fwdPosition(model, data); // treating the subtree Jacobian as the projection of 3 axis-aligned unit // forces into joint space, solve for the resulting accelerations in-place mj_solveM(model, data, jac_subtree, jac_subtree, 3); // expect to find accelerations of magnitude 1/subtreemass in the first 3 // coordinates of the free joint and 0s elsewhere, since applying forces to // the CoM should accelerate the whole mechanism without any internal motion int body_dofadr = model->body_dofadr[bodyid]; mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid]; for (int r = 0; r < 3; r++) { for (int c = 0; c < nv; c++) { mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0; EXPECT_THAT(jac_subtree[nv*r+c], DoubleNear(expected, max_abs_err)); } } mju_free(jac_subtree); mj_deleteData(data); mj_deleteModel(model); } using Name2idTest = MujocoTest; static constexpr char name2idTestingModel[] = R"( )"; TEST_F(Name2idTest, FindIds) { mjModel* model = LoadModelFromString(name2idTestingModel); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0); mj_deleteModel(model); } TEST_F(Name2idTest, MissingIds) { mjModel* model = LoadModelFromString(name2idTestingModel); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1); mj_deleteModel(model); } TEST_F(Name2idTest, EmptyIds) { mjModel* model = LoadModelFromString(name2idTestingModel); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1); mj_deleteModel(model); } TEST_F(Name2idTest, Namespaces) { mjModel* model = LoadModelFromString(name2idTestingModel); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3); mj_deleteModel(model); } using VersionTest = MujocoTest; TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); } TEST_F(VersionTest, MjVersionString) { #if GTEST_USES_SIMPLE_RE == 1 auto regex_matcher = ContainsRegex("^\\d+\\.\\d+\\.\\d+"); #else auto regex_matcher = MatchesRegex("^[0-9]+\\.[0-9]+\\.[0-9]+(-[0-9a-z]+)?$"); #endif EXPECT_THAT(std::string(mj_versionString()), regex_matcher); } } // namespace } // namespace mujoco