// 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_sensor.c. #include #include #include #include #include #include #include #include #include #include "src/engine/engine_support.h" #include "src/engine/engine_util_blas.h" #include "src/engine/engine_util_spatial.h" #include "test/fixture.h" namespace mujoco { namespace { using ::std::string; using ::std::vector; using ::testing::ElementsAre; using ::testing::ElementsAreArray; using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::Not; using ::testing::NotNull; using ::testing::Pointwise; using ::testing::SizeIs; using ::testing::StrEq; using ::testing::WhenSorted; const mjtNum tol = 1e-14; // nearness tolerance for floating point numbers // returns as a vector the measured values from sensor with index `id` static vector GetSensor(const mjModel* model, const mjData* data, int id) { return vector( data->sensordata + model->sensor_adr[id], data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); } // returns as a vector the measured values from sensor with name `name static vector GetSensor(const mjModel* model, const mjData* data, const char* name) { int id = mj_name2id(model, mjOBJ_SENSOR, name); return vector( data->sensordata + model->sensor_adr[id], data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]); } using SensorTest = MujocoTest; // --------------------- test sensor disable flag ------------------------------ TEST_F(SensorTest, DisableSensors) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // call mj_forward, expect clock to report 0 mj_forward(model.get(), data.get()); EXPECT_EQ(data->sensordata[0], 0.0); // call mj_step, mj_step1, expect clock to be incremented by timestep mj_step(model.get(), data.get()); mj_step1(model.get(), data.get()); EXPECT_EQ(data->sensordata[0], model->opt.timestep); // disable sensors, call mj_step, mj_step1, expect clock to not increment model->opt.disableflags |= mjDSBL_SENSOR; mj_step(model.get(), data.get()); mj_step1(model.get(), data.get()); EXPECT_EQ(data->time, 2 * model->opt.timestep); EXPECT_EQ(data->sensordata[0], model->opt.timestep); // re-enable sensors, call mj_step, mj_step1, expect clock to match time model->opt.disableflags = 0; mj_step(model.get(), data.get()); mj_step1(model.get(), data.get()); EXPECT_EQ(data->time, data->sensordata[0]); } // --------------------- test relative frame sensors -------------------------- using RelativeFrameSensorTest = MujocoTest; // hand-picked positions and orientations for simple expected values TEST_F(RelativeFrameSensorTest, ReferencePosMat) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); // compare actual and expected values vector pos = GetSensor(model.get(), data.get(), 0); EXPECT_THAT(pos, Pointwise(MjNear(tol, 1e-6), {5, 5, 0})); vector xaxis = GetSensor(model.get(), data.get(), 1); EXPECT_THAT(xaxis, Pointwise(MjNear(tol, 1e-6), {0, -1, 0})); vector yaxis = GetSensor(model.get(), data.get(), 2); EXPECT_THAT(yaxis, Pointwise(MjNear(tol, 1e-6), {1, 0, 0})); } // orientations given by quaternion and by orientation matrix are identical TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // call mj_forward and convert orientation matrix to quaternion mj_forward(model.get(), data.get()); mjtNum mat[9], converted_quat[4]; mju_transpose(mat, data->sensordata, 3, 3); mju_mat2Quat(converted_quat, mat); // compare quaternion sensor and quat derived from orientation matrix vector quat = GetSensor(model.get(), data.get(), 3); EXPECT_THAT(quat, Pointwise(MjNear(tol, 1e-6), converted_quat)); } // compare global frame and initially co-located relative frame on same body TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); constexpr int nsensordata = 32; ASSERT_EQ(model->nsensordata, nsensordata); MjDataPtr data = MakeData(model); // call mj_forward, save global sensors (colocated with reference frame) mj_forward(model.get(), data.get()); vector expected_values(data->sensordata, data->sensordata + nsensordata / 2); // set qpos to arbitrary values, call mj_forward for (int i = 0; i < 7; i++) { data->qpos[i] = i + 1; } mj_forward(model.get(), data.get()); // get values from relative sensors after moving the object vector actual_values(data->sensordata + nsensordata / 2, data->sensordata + nsensordata); // object and reference have moved together, we expect values to not change EXPECT_THAT(actual_values, Pointwise(MjNear(tol, 1e-6), expected_values)); } // hand-picked velocities and orientations for simple expected values TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); data->qvel[0] = mju_sqrt(2); data->qvel[1] = 1; mj_forward(model.get(), data.get()); // compare to expected values vector linvel = GetSensor(model.get(), data.get(), 0); const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0}; EXPECT_THAT(linvel, Pointwise(MjNear(tol, 1e-6), expected_linvel)); } // object and reference in the same body, expect angular velocities to be zero TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // set joint velocities and call forward dynamics data->qvel[0] = 1; mj_forward(model.get(), data.get()); // obj and ref rotate together, relative angular velocities should be zero vector angvel = GetSensor(model.get(), data.get(), 0); EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), {0, 0, 0})); } // object and reference rotate on the same global axis TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // set joint velocities and call forward dynamics data->qvel[0] = -1; data->qvel[1] = 1; mj_forward(model.get(), data.get()); // obj and ref rotate on same axis, we can just difference the velocities vector angvel = GetSensor(model.get(), data.get(), 0); const mjtNum expected_angvel[3] = {0, data->qvel[1] - data->qvel[0], 0}; EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), expected_angvel)); } // two arbitrary frames, compare velocity sensors and fin-diffed positions TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); mjtNum dt = MjTol(1e-6, 1e-3); // timestep used for finite differencing // set (arbitrary) joint velocities and call forward dynamics data->qvel[0] = 1; data->qvel[1] = -1; mj_forward(model.get(), data.get()); // save measured linear and angular velocities as vectors vector linvel = GetSensor(model.get(), data.get(), 2); vector angvel = GetSensor(model.get(), data.get(), 3); // save current position, quaternion as arrays mjtNum pos0[3], quat0[4]; mju_copy3(pos0, data->sensordata); mju_copy4(quat0, data->sensordata + 3); // explicit Euler integration with small dt mju_addToScl(data->qpos, data->qvel, dt, 2); // call mj_forward again, save new position and quaternion mj_forward(model.get(), data.get()); mjtNum pos1[3], quat1[4]; mju_copy3(pos1, data->sensordata); mju_copy4(quat1, data->sensordata + 3); // compute expected linear velocities using finite differencing mjtNum linvel_findiff[3]; mju_sub3(linvel_findiff, pos1, pos0); mju_scl3(linvel_findiff, linvel_findiff, 1 / dt); // compute expected angular velocities using finite differencing mjtNum dquat[4], angvel_findiff[3]; mju_negQuat(quat0, quat0); mju_mulQuat(dquat, quat1, quat0); mju_quat2Vel(angvel_findiff, dquat, dt); // compare analytic and finite-differenced relative velocities EXPECT_THAT(linvel, Pointwise(MjNear(10 * dt, 1.5e-2), linvel_findiff)); EXPECT_THAT(angvel, Pointwise(MjNear(10 * dt, 2e-3), angvel_findiff)); } // ------------------------- general sensor tests ----------------------------- using SensorTest = MujocoTest; TEST_F(SensorTest, EnableEnergy) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); EXPECT_NEAR(data->energy[0], 2 * 3 * 5, MjTol(1e-12, 1e-5)); model->opt.enableflags &= ~mjENBL_ENERGY; mj_forward(model.get(), data.get()); EXPECT_NEAR(data->energy[0], 0, MjTol(1e-12, 1e-5)); } TEST_F(SensorTest, PotentialEnergy) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 2 * 3 * 5, MjTol(1e-12, 1e-5)); data->qpos[2] = 7; mj_forward(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 7 * 3 * 5, MjTol(1e-12, 1e-5)); } TEST_F(SensorTest, PotentialEnergyFreeJointSpring) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); data->qpos[0] = 1; data->qpos[1] = 2; data->qpos[2] = 3; mj_forward(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.5 * 2 * 14, MjTol(1e-12, 1e-5)); } TEST_F(SensorTest, KineticEnergy) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); while (data->time < 1.5) { mj_step(model.get(), data.get()); } mj_forward(model.get(), data.get()); mjtNum mass = 3; mjtNum speed = data->time * mju_norm3(model->opt.gravity); EXPECT_NEAR(data->sensordata[0], 0.5 * mass * speed * speed, MjTol(1e-7, 1e-2)); } TEST_F(SensorTest, PolyStiffnessEnergy) { static constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d = MakeData(m); mj_resetDataKeyframe(m.get(), d.get(), 0); mj_forward(m.get(), d.get()); mjtNum total_energy = d->energy[0] + d->energy[1]; for (int i = 0; i < 100; i++) { mj_step(m.get(), d.get()); EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.003); } } // test clock sensor TEST_F(SensorTest, Clock) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // call step 4 times, checking that clock works as expected for (int i = 0; i < 5; i++) { mj_step(model.get(), data.get()); mj_step1(model.get(), data.get()); // update values of position-based sensors EXPECT_EQ(data->sensordata[0], data->time); EXPECT_EQ(data->sensordata[1], mju_min(data->time, 3e-3)); } // check names const char* name0 = mj_id2name(model.get(), mjOBJ_SENSOR, 0); EXPECT_EQ(name0, nullptr); const char* name1 = mj_id2name(model.get(), mjOBJ_SENSOR, 1); EXPECT_THAT(name1, StrEq("clampedclock")); } // test that integer parameters pass through TEST_F(SensorTest, IntPrm) { constexpr char xml[] = R"( )"; ASSERT_EQ(mjNSENS, 3); char err[1024]; mjSpec* spec = mj_parseXMLString(xml, 0, err, sizeof(err)); ASSERT_THAT(spec, NotNull()) << err; mjModel* model = mj_compile(spec, nullptr); EXPECT_EQ(model->sensor_intprm[0], 0); EXPECT_EQ(model->sensor_intprm[1], 0); mj_deleteModel(model); mjsSensor* s = mjs_asSensor(mjs_findElement(spec, mjOBJ_SENSOR, "dummy")); s->intprm[0] = 3; s->intprm[1] = 4; s->intprm[2] = 5; model = mj_compile(spec, nullptr); EXPECT_EQ(model->sensor_intprm[0], 3); EXPECT_EQ(model->sensor_intprm[1], 4); EXPECT_EQ(model->sensor_intprm[2], 5); mj_deleteModel(model); mj_deleteSpec(spec); } // test sequential collision sensors TEST_F(SensorTest, CollisionSequential) { constexpr char xml[] = R"( )"; MjModelPtr model_ptr = LoadModelFromString(xml); MjDataPtr data_ptr = MakeData(model_ptr); mjModel* model = model_ptr.get(); mjData* data = data_ptr.get(); mj_forward(model, data); mjtNum eps = 1e-14; EXPECT_NEAR(data->sensordata[0], 0.8, MjTol(eps, 1e-7)); EXPECT_NEAR(data->sensordata[1], 0.7, MjTol(eps, 1e-7)); EXPECT_NEAR(data->sensordata[2], 0.5, MjTol(eps, 1e-7)); EXPECT_THAT(GetSensor(model, data, 3), Pointwise(MjNear(eps, 1e-7), vector{0, 0, 1})); EXPECT_THAT(GetSensor(model, data, 4), Pointwise(MjNear(eps, 1e-7), vector{0, 0, -1})); EXPECT_THAT(GetSensor(model, data, 5), Pointwise(MjNear(eps, 1e-7), vector{1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 6), Pointwise(MjNear(eps, 1e-6), vector{0, 0, 0, 0, 0, .8})); EXPECT_THAT(GetSensor(model, data, 7), Pointwise(MjNear(eps, 1e-6), vector{1, 0, .7, 1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 8), Pointwise(MjNear(eps, 1e-6), vector{.2, 0, 1, .7, 0, 1})); EXPECT_THAT(GetSensor(model, data, 9), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 0))); EXPECT_THAT(GetSensor(model, data, 10), Pointwise(MjNear(eps, 1e-6), GetSensor(model, data, 6))); EXPECT_THAT(GetSensor(model, data, 11), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 3))); EXPECT_THAT(GetSensor(model, data, 12), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 5))); EXPECT_THAT(GetSensor(model, data, 13), Pointwise(MjNear(eps, 1e-6), GetSensor(model, data, 8))); EXPECT_THAT(GetSensor(model, data, 14), Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 2))); } TEST_F(SensorTest, BadContact) { string xml_template = R"( )"; struct Case { string bad_attr; string expected_error; }; Case test_cases[] = { {"geom1='sphere1' geom2='sphere2' data='dist force normal'", "must be in order: found, force, torque, dist, pos, normal, tangent"}, {"geom1='sphere1' geom2='sphere2' num='-3'", "'num' must be positive in sensor"}, {"geom1='sphere1' geom2='sphere2' site='site'", "at most one of (geom1, body1, subtree1, site) can be specified"}, {"geom2='sphere1' body2='body'", "at most one of (geom2, body2, subtree2) can be specified"}, }; for (const auto& test : test_cases) { string xml = xml_template; size_t pos = xml.find("BAD_ATTR"); ASSERT_NE(pos, string::npos); xml.replace(pos, 8, test.bad_attr); char error[1024]; MjModelPtr model = LoadModelFromString(xml.c_str(), error, sizeof(error)); ASSERT_THAT(model.get(), IsNull()) << "Test case: " << test.bad_attr; EXPECT_THAT(error, HasSubstr(test.expected_error)) << "Test case: " << test.bad_attr; } } TEST_F(SensorTest, Contact) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { model->opt.cone = cone; mj_resetData(model, data); while (data->time < 2) { mj_step(model, data); } vector all = GetSensor(model, data, "all"); EXPECT_EQ(all, vector{4}); vector world = GetSensor(model, data, "world"); EXPECT_EQ(world, vector{3}); vector b1 = GetSensor(model, data, "b1"); EXPECT_EQ(b1, vector{3}); vector g1 = GetSensor(model, data, "g1"); EXPECT_EQ(g1, vector{3}); vector b1g2 = GetSensor(model, data, "b1:g2"); EXPECT_EQ(b1g2, vector{1}); vector b1world = GetSensor(model, data, "b1:world"); EXPECT_EQ(b1world, vector{2}); vector site = GetSensor(model, data, "site"); EXPECT_EQ(site, vector{2}); vector sitewall = GetSensor(model, data, "site:wall"); EXPECT_EQ(sitewall, vector{1}); vector wall = GetSensor(model, data, "wall"); EXPECT_THAT(wall, Pointwise(MjNear(1e-4, 0.02), {1, 8, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0})); // normals points *away* from b2 (towards floor / b1) vector b2 = GetSensor(model, data, "b2"); EXPECT_THAT(b2, Pointwise(MjNear(1e-4, 0.02), {3, 0, 0, 0, 0, -1, 4, 0, 0, 1, 0, 0})); // normal points *towards* b2 vector b2f = GetSensor(model, data, "b2_flipped"); EXPECT_THAT(b2f, Pointwise(MjNear(1e-4, 0.02), {3, 0, 0, 0, 0, 1, 4, 0, 0, -1, 0, 0})); vector b2r = GetSensor(model, data, "b2_reduced"); EXPECT_THAT(b2r, Pointwise(MjNear(1e-4, 0.02), {4, 0, 0, -1, 0, 0})); } mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, ContactSorted) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact_sorted.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); while (data->time < .5) { mj_step(model, data); } vector unsorted = GetSensor(model, data, "unsorted"); EXPECT_THAT(unsorted, SizeIs(4)); EXPECT_THAT(unsorted, Not(WhenSorted(ElementsAreArray(unsorted)))); vector sorted = GetSensor(model, data, "sorted dist"); EXPECT_THAT(sorted, SizeIs(4)); EXPECT_THAT(sorted, WhenSorted(ElementsAreArray(sorted))); vector sorted_force = GetSensor(model, data, "sorted force"); EXPECT_THAT(sorted_force, SizeIs(12)); vector nnorms; for (size_t i = 0; i < sorted_force.size(); i += 3) { nnorms.push_back(-sorted_force[i] * sorted_force[i] + -sorted_force[i + 1] * sorted_force[i + 1] + -sorted_force[i + 2] * sorted_force[i + 2]); } EXPECT_THAT(nnorms, WhenSorted(ElementsAreArray(nnorms))); vector smallest = GetSensor(model, data, "smallest dist"); EXPECT_THAT(smallest, SizeIs(1)); EXPECT_EQ(smallest[0], sorted[0]); vector largest = GetSensor(model, data, "largest force"); EXPECT_THAT(largest, SizeIs(3)); EXPECT_THAT(largest, ElementsAre(sorted_force[0], sorted_force[1], sorted_force[2])); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, ContactSubtree) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact_subtree.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); while (data->time < 0.2) { mj_step(model, data); int all = GetSensor(model, data, "all")[0]; int w_t1 = GetSensor(model, data, "w_t1")[0]; int w_t2 = GetSensor(model, data, "w_t2")[0]; int t1 = GetSensor(model, data, "t1")[0]; int t2 = GetSensor(model, data, "t2")[0]; int t1_t1 = GetSensor(model, data, "t1_t1")[0]; int t2_t2 = GetSensor(model, data, "t2_t2")[0]; int t1_t2 = GetSensor(model, data, "t1_t2")[0]; int t2_t1 = GetSensor(model, data, "t2_t1")[0]; // compute the number of first tree contacts in two different ways EXPECT_EQ(t1, w_t1 + t1_t1 + t1_t2); // compute the number of second tree contacts in two different ways EXPECT_EQ(t2, w_t2 + t2_t2 + t2_t1); // compute the number of all contacts in two different ways EXPECT_EQ(all, w_t1 + w_t2 + t1_t1 + t2_t2 + t1_t2); } mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, ContactSubtreePartial) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact_subtree_partial.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); while (data->time < 0.6) { mj_step(model, data); } EXPECT_EQ(GetSensor(model, data, "all")[0], 4); EXPECT_EQ(GetSensor(model, data, "world")[0], 4); EXPECT_EQ(GetSensor(model, data, "thigh")[0], 4); EXPECT_EQ(GetSensor(model, data, "shin")[0], 2); EXPECT_EQ(GetSensor(model, data, "foot")[0], 1); EXPECT_EQ(GetSensor(model, data, "foot_w")[0], 0); EXPECT_EQ(GetSensor(model, data, "foot_w2")[0], 1); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, ContactNet) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/contact_net.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; int b1 = mj_name2id(model, mjOBJ_BODY, "b1"); int b2 = mj_name2id(model, mjOBJ_BODY, "b2"); int nv = model->nv; mjData* data = mj_makeData(model); for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) { model->opt.cone = cone; mj_resetData(model, data); // for each timestep, compare the net force computation to qfrc_constraint // data->ncon varies in [0, 6] int nconmax = 0; while (data->time < 0.2) { mj_step(model, data); vector qfrc_expected = AsVector(data->qfrc_constraint, nv); // check net force, sensor returns body1 -> body2 vector net12 = GetSensor(model, data, "net12"); EXPECT_EQ(net12.size(), 9); mjtNum* force = net12.data(); mjtNum* torque = net12.data() + 3; mjtNum* point = net12.data() + 6; // apply wrench to b2 vector qfrc(nv, 0.0); mj_applyFT(model, data, force, torque, point, b2, qfrc.data()); // apply opposite wrench to b1 mju_scl3(force, force, -1); mju_scl3(torque, torque, -1); mj_applyFT(model, data, force, torque, point, b1, qfrc.data()); // compare EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 2e-4), qfrc_expected)); // check net force, sensor returns body2 -> body1 vector net21 = GetSensor(model, data, "net21"); EXPECT_EQ(net21.size(), 9); force = net21.data(); torque = net21.data() + 3; point = net21.data() + 6; qfrc.assign(nv, 0.0); // apply wrench to b1 mj_applyFT(model, data, force, torque, point, b1, qfrc.data()); // apply opposite wrench to b2 mju_scl3(force, force, -1); mju_scl3(torque, torque, -1); mj_applyFT(model, data, force, torque, point, b2, qfrc.data()); // compare EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 2e-4), qfrc_expected)); nconmax = std::max(nconmax, data->ncon); } // at least 5 contacts happened EXPECT_GT(nconmax, 4); } mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, CameraProjection) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // update positions and sensors mj_fwdPosition(model.get(), data.get()); mj_sensorPos(model.get(), data.get()); EXPECT_THAT(model->cam_resolution[0], 1920); EXPECT_THAT(model->cam_resolution[1], 1200); mjtNum eps = 1e-4; EXPECT_NEAR(data->sensordata[0], 0, eps); EXPECT_NEAR(data->sensordata[1], 0, eps); EXPECT_NEAR(data->sensordata[2], 1920, eps); EXPECT_NEAR(data->sensordata[3], 1200, eps); EXPECT_NEAR(data->sensordata[4], 960, eps); EXPECT_NEAR(data->sensordata[5], 600, eps); } // previous implementation of cam_project to verify the new one static void cam_project_old( mjtNum sensordata[2], const mjtNum target_xpos[3], const mjtNum cam_xpos[3], const mjtNum cam_xmat[9], const int cam_res[2], mjtNum cam_fovy, const float cam_intrinsic[4], const float cam_sensorsize[2]) { mjtNum fx, fy; // translation matrix (4x4) mjtNum translation[4][4] = {}; translation[0][0] = 1; translation[1][1] = 1; translation[2][2] = 1; translation[3][3] = 1; translation[0][3] = -cam_xpos[0]; translation[1][3] = -cam_xpos[1]; translation[2][3] = -cam_xpos[2]; // rotation matrix (4x4) mjtNum rotation[4][4] = {}; rotation[0][0] = 1; rotation[1][1] = 1; rotation[2][2] = 1; rotation[3][3] = 1; for (int i=0; i < 3; i++) { for (int j=0; j < 3; j++) { rotation[i][j] = cam_xmat[j*3+i]; } } // focal transformation matrix (3x4) if (cam_sensorsize[0] && cam_sensorsize[1]) { fx = cam_intrinsic[0] / cam_sensorsize[0] * cam_res[0]; fy = cam_intrinsic[1] / cam_sensorsize[1] * cam_res[1]; } else { fx = fy = .5 / mju_tan(cam_fovy * mjPI / 360.) * cam_res[1]; } mjtNum focal[3][4] = {}; focal[0][0] = -fx; focal[1][1] = fy; focal[2][2] = 1.0; // image matrix (3x3) mjtNum image[3][3] = {}; image[0][0] = 1; image[1][1] = 1; image[2][2] = 1; image[0][2] = (mjtNum)cam_res[0] / 2.0; image[1][2] = (mjtNum)cam_res[1] / 2.0; // projection matrix (3x4): product of all 4 matrices mjtNum proj[3][4] = {}; for (int i=0; i < 3; i++) { for (int j=0; j < 3; j++) { for (int k=0; k < 4; k++) { for (int l=0; l < 4; l++) { for (int n=0; n < 4; n++) { proj[i][n] += image[i][j] * focal[j][k] * rotation[k][l] * translation[l][n]; } } } } } // projection matrix multiplies homogenous [x, y, z, 1] vectors mjtNum pos_hom[4] = {0, 0, 0, 1}; mju_copy3(pos_hom, target_xpos); // project world coordinates into pixel space, see: // https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula mjtNum pixel_coord_hom[3] = {0}; for (int i=0; i < 3; i++) { for (int j=0; j < 4; j++) { pixel_coord_hom[i] += proj[i][j] * pos_hom[j]; } } // avoid dividing by tiny numbers mjtNum denom = pixel_coord_hom[2]; if (mju_abs(denom) < mjMINVAL) { if (denom < 0) { denom = mju_min(denom, -mjMINVAL); } else { denom = mju_max(denom, mjMINVAL); } } // compute projection sensordata[0] = pixel_coord_hom[0] / denom; sensordata[1] = pixel_coord_hom[1] / denom; } TEST_F(SensorTest, CameraProjectionComparison) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); mj_fwdPosition(model.get(), data.get()); mj_sensorPos(model.get(), data.get()); for (int i = 0; i < model->nsensor; ++i) { if (model->sensor_type[i] == mjSENS_CAMPROJECTION) { int objid = model->sensor_objid[i]; int refid = model->sensor_refid[i]; mjtNum expected[2]; cam_project_old(expected, data->site_xpos + 3*objid, data->cam_xpos + 3*refid, data->cam_xmat + 9*refid, model->cam_resolution + 2*refid, model->cam_fovy[refid], model->cam_intrinsic + 4*refid, model->cam_sensorsize + 2*refid); int adr = model->sensor_adr[i]; EXPECT_NEAR(data->sensordata[adr], expected[0], MjTol(1e-10, 1e-3)); EXPECT_NEAR(data->sensordata[adr + 1], expected[1], MjTol(1e-10, 1e-3)); } } } TEST_F(SensorTest, InsideSite) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_EQ(model->nsensordata, 5); MjDataPtr data = MakeData(model); mjtNum hpos[5] = {-.5, -.25, 0, .25, .5}; for (int i = 0; i < 5; i++) { data->qpos[0] = hpos[i]; mj_forward(model.get(), data.get()); vector expected(5, 0.0); expected[i] = 1.0; EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); } } TEST_F(SensorTest, RangefinderCamera) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; // first sensor: data="dist depth" => (1+1)*9 = 18 // second sensor: data="dist dir origin point" => (1+3+3+3)*9 = 90 EXPECT_EQ(model->nsensordata, 108); MjDataPtr data = MakeData(model); mj_forward(model.get(), data.get()); mjtNum tol = 1e-6; mjtNum height = 2.0; mjtNum fy = 1.5; mjtNum offsets[3] = {-1.0, 0.0, 1.0}; // pixel center - principal point // test 1: perspective camera - rays diverge, distance varies with angle int adr0 = model->sensor_adr[0]; constexpr int stride0 = 2; // dist(1) + depth(1) for (int row = 0; row < 3; row++) { for (int col = 0; col < 3; col++) { int idx = row * 3 + col; mjtNum dx = offsets[col] / fy; mjtNum dy = offsets[row] / fy; mjtNum expected_dist = height * mju_sqrt(1 + dx * dx + dy * dy); mjtNum dist = data->sensordata[adr0 + idx * stride0]; EXPECT_NEAR(dist, expected_dist, tol) << "perspective dist pixel (" << row << ", " << col << ")"; // depth should equal camera height (2.0) for all pixels mjtNum depth = data->sensordata[adr0 + idx * stride0 + 1]; EXPECT_NEAR(depth, height, tol) << "perspective depth pixel (" << row << ", " << col << ")"; } } // test 2: orthographic camera distance - tilted 45 degrees around Y axis mjtNum extent = 2.0; // fovy for orthographic mjtNum half_extent = extent / 2; mjtNum fx = 1.5; // width / 2 for 3x3 image mjtNum cx = 1.5; // principal point mjtNum cos45 = mju_sqrt(0.5); mjtNum sin45 = mju_sqrt(0.5); int adr1 = model->sensor_adr[1]; constexpr int stride1 = 10; // dist(1) + dir(3) + origin(3) + point(3) for (int row = 0; row < 3; row++) { for (int col = 0; col < 3; col++) { int idx = row * 3 + col; // pixel offset in camera frame: matches mju_camPixelRay formula mjtNum px_cam = (col + 0.5 - cx) / fx * half_extent; // camera tilted 45 around Y: local +X maps to world (+cos45, 0, -sin45) mjtNum origin_z = height - px_cam * sin45; // ray hits z=0 plane: distance = origin_z / cos45 mjtNum expected_dist = origin_z / cos45; mjtNum dist = data->sensordata[adr1 + idx * stride1]; EXPECT_NEAR(dist, expected_dist, tol) << "orthographic dist pixel (" << row << ", " << col << ")"; // verify point = origin + dir * dist mjtNum* dir = data->sensordata + adr1 + idx * stride1 + 1; mjtNum* origin = data->sensordata + adr1 + idx * stride1 + 4; mjtNum* point = data->sensordata + adr1 + idx * stride1 + 7; mjtNum expected_point[3]; mju_addScl3(expected_point, origin, dir, dist); EXPECT_NEAR(point[0], expected_point[0], tol) << "ortho point[0] pixel (" << row << ", " << col << ")"; EXPECT_NEAR(point[1], expected_point[1], tol) << "ortho point[1] pixel (" << row << ", " << col << ")"; EXPECT_NEAR(point[2], expected_point[2], tol) << "ortho point[2] pixel (" << row << ", " << col << ")"; } } } TEST_F(SensorTest, RFCamera) { const string xml_path = GetTestDataFilePath("engine/testdata/sensor/rfcamera.xml"); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; // both sensors have data="dist point normal" => (1+3+3)*16 = 112 ASSERT_EQ(model->nsensor, 2); EXPECT_EQ(model->sensor_dim[0], 112); EXPECT_EQ(model->sensor_dim[1], 112); mjData* data = mj_makeData(model); mj_step(model, data); // check both sensors: dist, point, normal constexpr int stride = 7; // dist(1) + point(3) + normal(3) for (int s = 0; s < 2; s++) { int adr = model->sensor_adr[s]; for (int i = 0; i < 16; i++) { mjtNum dist = data->sensordata[adr + i * stride]; mjtNum* point = data->sensordata + adr + i * stride + 1; mjtNum* normal = data->sensordata + adr + i * stride + 4; EXPECT_TRUE(dist > 0 || dist == -1) << "sensor " << s << " pixel " << i; if (dist > 0) { EXPECT_GT(mju_norm3(point), 0.0) << "sensor " << s << " point " << i; EXPECT_NEAR(mju_norm3(normal), 1.0, 1e-6) << "sensor " << s << " normal " << i; } else { EXPECT_NEAR(mju_norm3(point), 0.0, 1e-6) << "sensor " << s << " point " << i; EXPECT_NEAR(mju_norm3(normal), 0.0, 1e-6) << "sensor " << s << " normal " << i; } } } mj_deleteData(data); mj_deleteModel(model); } // ------------------------------- sensor delays ------------------------------- TEST_F(SensorTest, SensorDelay) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // delay = 0.02 seconds, timestep = 0.01 // history = 3 (more than delay/timestep=2) to ensure buffer coverage EXPECT_EQ(model->sensor_history[0], 3); EXPECT_NEAR(model->sensor_delay[0], 0.02, MjTol(1e-10, 1e-7)); // Use different values to verify exact delay timing. // With delay=0.02 and timestep=0.01, we expect 2-step delay: // - At step N, sensordata should reflect qpos from step N-2. // step 0: qpos=10, read from initial buffer data->qpos[0] = 10.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; // step 1: qpos=20, still reading initial buffer data->qpos[0] = 20.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 1"; // step 2: qpos=30, read value from step 0 (delay=2 steps) data->qpos[0] = 30.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 10.0, 1e-10) << "step 2"; // step 3: qpos=40, read value from step 1 (delay=2 steps) data->qpos[0] = 40.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 20.0, 1e-10) << "step 3"; // step 4: qpos=50, read value from step 2 (delay=2 steps) data->qpos[0] = 50.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 30.0, 1e-10) << "step 4"; } // Test sensor delay with linear interpolation (interp=1) // Uses delay = 1.5*timestep so interpolation is meaningful TEST_F(SensorTest, SensorDelayLinearInterp) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear) // With linear interpolation and 1.5*timestep delay, the read time falls // exactly between two buffer samples, so we should get the average. EXPECT_EQ(model->sensor_history[0], 3); EXPECT_EQ(model->sensor_history[1], 1); // interp=1 (linear) EXPECT_NEAR(model->sensor_delay[0], 0.015, MjTol(1e-10, 1e-7)); // Set increasing qpos values: step i -> qpos = (i+1)*10 // Buffer has samples at times: -0.02, -0.01, 0 (initialized) // After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, // 0, 10 Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 // (val=0) and t=0 (val=0) Expected: 0 * 0.5 + 0 * 0.5 = 0 data->qpos[0] = 10.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0"; // After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, // 10, 20 Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) // and t=0.01 (val=10) Expected: 0 * 0.5 + 10 * 0.5 = 5 data->qpos[0] = 20.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10) << "step 1"; // After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values // 10, 20, 30 Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 // (val=10) and t=0.02 (val=20) Expected: 10 * 0.5 + 20 * 0.5 = 15 data->qpos[0] = 30.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 15.0, 1e-10) << "step 2"; } TEST_F(SensorTest, SensorInterval) { // This test uses the exact values from the documentation for interval: // timestep=1, interval=2.5, producing times 0, 3, 5, 8, 10, 13, ... // with interval="2.5 -1.5", producing times 1, 4, 6, 9, 11, 14, ... constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); int sensor0 = mj_name2id(model.get(), mjOBJ_SENSOR, "default_phase"); int sensor1 = mj_name2id(model.get(), mjOBJ_SENSOR, "offset_phase"); int adr0 = model->sensor_adr[sensor0]; int adr1 = model->sensor_adr[sensor1]; // Verify initial buffer timestamps (after mj_makeData/mj_resetData) // With period=2.5, dt=1.0, nsample=10: // sensor0 (phase = -period = -2.5): continuous times are -2.5, -5, -7.5, ... // rounded up to dt: -2, -5, -7, -10, -12, -15, -17, -20, -22, -25 // sensor1 (phase = -1.5): continuous times are -1.5, -4, -6.5, ... // rounded up to dt: -1, -4, -6, -9, -11, -14, -16, -19, -21, -24 int n0 = model->sensor_history[2 * sensor0]; int n1 = model->sensor_history[2 * sensor1]; mjtNum* buf0 = data->history + model->sensor_historyadr[sensor0]; mjtNum* buf1 = data->history + model->sensor_historyadr[sensor1]; mjtNum* times0 = buf0 + 2; mjtNum* times1 = buf1 + 2; mjtNum expected_times0[] = {-25, -22, -20, -17, -15, -12, -10, -7, -5, -2}; mjtNum expected_times1[] = {-24, -21, -19, -16, -14, -11, -9, -6, -4, -1}; for (int i = 0; i < n0; i++) { EXPECT_NEAR(times0[i], expected_times0[i], 1e-10); } for (int i = 0; i < n1; i++) { EXPECT_NEAR(times1[i], expected_times1[i], 1e-10); } // sensor0: interval="2.5 0" -> time_prev starts at -2.5 // triggers at: 0, 3, 5, 8, 10, 13, ... (gaps: 3,2,3,2,3,...) // sensor1: interval="2.5 -1.5" -> time_prev starts at -1.5 // triggers at: 1, 4, 6, 9, 11, 14, ... (gaps: 3,2,3,2,3,...) // Arrays tracking when each sensor triggers (1=triggers, 0=holds) // Times: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 int triggers0[] = {1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0}; int triggers1[] = {0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1}; mjtNum value0 = 0, value1 = 0; for (int t = 0; t < 15; t++) { // set position to current time (so we can track when sensor was computed) data->qpos[0] = t; mj_step(model.get(), data.get()); // update expected values based on trigger pattern if (triggers0[t]) value0 = t; if (triggers1[t]) value1 = t; EXPECT_NEAR(data->sensordata[adr0], value0, MjTol(1e-10, 1e-7)) << "sensor0 at t=" << t; EXPECT_NEAR(data->sensordata[adr1], value1, MjTol(1e-10, 1e-7)) << "sensor1 at t=" << t; } } TEST_F(SensorTest, SensorDelayInterval) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // Combined delay and interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.02, MjTol(1e-10, 1e-7)); EXPECT_NEAR(model->sensor_interval[2 * 0], 0.03, MjTol(1e-10, 1e-7)); // Verify initial buffer timestamps (after mj_makeData/mj_resetData) // With period=0.03, dt=0.01, nsample=5, phase=0 (means -period=-0.03): // continuous times: -0.03, -0.06, -0.09, -0.12, -0.15 // rounded up to dt: -0.03, -0.06, -0.09, -0.12, -0.15 (multiples of dt) int n = model->sensor_history[0]; mjtNum* buf = data->history + model->sensor_historyadr[0]; mjtNum* times = buf + 2; mjtNum expected_times[] = {-0.15, -0.12, -0.09, -0.06, -0.03}; for (int i = 0; i < n; i++) { EXPECT_NEAR(times[i], expected_times[i], MjTol(1e-10, 0.015)); } // set position data->qpos[0] = 5.0; // initial steps: reading from buffer (initially 0) // With delay=0.02, interval=0.03: // - At t=0, interval satisfied: compute 5.0, insert at t=0 (current time) // - Reading happens at d->time - delay; at t=0.02, reads at t=0.00 (5.0) for (int i = 0; i < 2; i++) { mj_step(model.get(), data.get()); // sensor reads delayed value (0.0 from initial buffer) EXPECT_NEAR(data->sensordata[0], 0.0, MjTol(1e-10, 1e-7)) << "step " << i; } // step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0 mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 5.0, MjTol(1e-10, 1e-7)); } TEST_F(SensorTest, SensorHistoryOnly) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // history only, no delay or interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.0, MjTol(1e-10, 1e-7)); EXPECT_NEAR(model->sensor_interval[2 * 0], 0.0, MjTol(1e-10, 1e-7)); // set position data->qpos[0] = 3.0; // without delay, sensordata reflects current value immediately mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 3.0, 1e-10); // change position, check again data->qpos[0] = 7.0; mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 7.0, 1e-10); } TEST_F(SensorTest, SensorDelayMultiDim) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // ballangvel is 3D EXPECT_EQ(model->sensor_dim[0], 3); EXPECT_EQ(model->sensor_history[0], 2); // set angular velocity data->qvel[0] = 1.0; data->qvel[1] = 2.0; data->qvel[2] = 3.0; // step: reading delayed value (initially 0) mj_step(model.get(), data.get()); EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10); EXPECT_NEAR(data->sensordata[1], 0.0, 1e-10); EXPECT_NEAR(data->sensordata[2], 0.0, 1e-10); // after delay, values should propagate mj_step(model.get(), data.get()); mj_step(model.get(), data.get()); mj_step(model.get(), data.get()); // angular velocity is affected by dynamics, just check the buffer works EXPECT_THAT(AsVector(data->sensordata, 3), Not(ElementsAre(0.0, 0.0, 0.0))); } TEST_F(SensorTest, ReadSensor) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); // step with different qpos values to populate buffer // mj_advance inserts at current time, then time advances data->qpos[0] = 1.0; mj_step(model.get(), data.get()); // inserts 1.0 at t=0, time -> 0.01 data->qpos[0] = 2.0; mj_step(model.get(), data.get()); // inserts 2.0 at t=0.01, time -> 0.02 data->qpos[0] = 3.0; mj_step(model.get(), data.get()); // inserts 3.0 at t=0.02, time -> 0.03 // now time=0.03, buffer has: [t=0: 1.0, t=0.01: 2.0, t=0.02: 3.0] // read at different times from history mjtNum result[1]; const mjtNum* ptr; // read at t=0 -> returns 1.0 ptr = mj_readSensor(model.get(), data.get(), 0, 0.0, result, /*order=*/0); EXPECT_NEAR(*ptr, 1.0, 1e-10); // read at t=0.01 -> returns 2.0 (ZOH: exactly at insertion time) ptr = mj_readSensor(model.get(), data.get(), 0, 0.01, result, /*order=*/0); EXPECT_NEAR(*ptr, 2.0, 1e-10); // read at t=0.02 -> returns 3.0 ptr = mj_readSensor(model.get(), data.get(), 0, 0.02, result, /*order=*/0); EXPECT_NEAR(*ptr, 3.0, 1e-10); } // mj_sensorAcc returns correct accelerometer after mj_step1 (issue #3133) TEST_F(SensorTest, AccelerometerAfterStep1) { constexpr char xml[] = R"( )"; MjModelPtr model = LoadModelFromString(xml); MjDataPtr data = MakeData(model); // settle the simulation with split-step loop for (int i = 0; i < 100; i++) { mj_step1(model.get(), data.get()); mj_step2(model.get(), data.get()); } // call mj_step1 + mj_sensorAcc, expect gravity reading (~9.81 m/s^2) mj_step1(model.get(), data.get()); mj_sensorAcc(model.get(), data.get()); EXPECT_NEAR(data->sensordata[2], 9.81, 1e-2); } // Test tactile sensor reads non-zero values when contacts occur TEST_F(SensorTest, TactileSkipTangents) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_GT(model->nsensordata, 0) << "No sensor data allocated"; MjDataPtr data = MakeData(model); // Use mj_forward to compute collisions and sensors at t=0 mj_forward(model.get(), data.get()); // Verify initial state EXPECT_EQ(data->time, 0.0); EXPECT_GT(data->ncon, 0) << "No contacts generated"; // Tactile sensor layout: [depths..., tang1_vel..., tang2_vel...] int ntaxel = model->nsensordata / 3; ASSERT_EQ(model->nsensordata % 3, 0) << "Sensor dim should be divisible by 3"; // No tangents, so tangent components should be zero for (int i = ntaxel; i < model->nsensordata; i++) { EXPECT_EQ(data->sensordata[i], 0.0) << "Tangent component at index " << i << " should be 0"; } // Penetration depth components: verify count, sign, and magnitude ~0.2 int nonzero_count = 0; for (int i = 0; i < ntaxel; i++) { if (data->sensordata[i] != 0) { nonzero_count++; EXPECT_NEAR(data->sensordata[i], 0.2, 0.1) << "Penetration depth at taxel " << i; } } EXPECT_EQ(nonzero_count, 2) << "Expected 2 taxels in contact"; } // insidesite uses subtree_com for massless flex parent bodies TEST_F(SensorTest, InsideSiteFlexBody) { static constexpr char xml[] = R"( )"; char error[1024] = {0}; EXPECT_CALL(mock_warning_handler, Warn(testing::HasSubstr("is not rigid"))) .WillOnce(testing::Return()); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d = MakeData(m); // flex is at origin, site is a large box at origin — should be inside mj_forward(m.get(), d.get()); EXPECT_EQ(d->sensordata[0], 1) << "flex body should be inside the container site"; // shift all vertex/node bodies far outside the site via qpos // each body has 3 slide joints (x, y, z); shift z by +10 int parent_id = mj_name2id(m.get(), mjOBJ_BODY, "parent"); for (int b = parent_id + 1; b < m->nbody; b++) { if (m->body_parentid[b] == parent_id) { int jadr = m->body_jntadr[b]; if (jadr >= 0 && m->body_jntnum[b] == 3) { // z-slide is the 3rd joint d->qpos[m->jnt_qposadr[jadr + 2]] = 10.0; } } } mj_forward(m.get(), d.get()); // subtree_com should now be far outside; sensor should read 0 EXPECT_EQ(d->sensordata[0], 0) << "flex body should be outside the container site after displacement"; } // Test that a tactile sensor's compile-time body-collision check correctly // uses the referenced Geom ID instead of mistakenly indexing by Mesh ID. TEST_F(SensorTest, TactileMeshIdMismatchedValidator) { static constexpr char xml[] = R"( )"; char error[1024] = {0}; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; } // Test that contact and touch sensors work with Flex contacts. TEST_F(SensorTest, FlexContactSensors) { static constexpr char xml[] = R"( )"; char error[1024] = {0}; EXPECT_CALL(mock_warning_handler, Warn(testing::HasSubstr("is not rigid"))) .WillOnce(testing::Return()); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d = MakeData(m); mj_forward(m.get(), d.get()); // We expect at least one contact between the flex and the floor ASSERT_GT(d->ncon, 0) << "No contacts generated"; // Check the contact sensor "flex_contact_subtree" vector flex_contact_subtree = GetSensor(m.get(), d.get(), "flex_contact_subtree"); EXPECT_GT(flex_contact_subtree[0], 0) << "Flex contact sensor (subtree) did not detect any contacts"; // Check the contact sensor "flex_contact_body" vector flex_contact_body = GetSensor(m.get(), d.get(), "flex_contact_body"); EXPECT_EQ(flex_contact_body[0], 0) << "Flex contact sensor (body) should not match contacts on child bodies"; // Check the touch sensor "floor_touch" vector floor_touch = GetSensor(m.get(), d.get(), "floor_touch"); EXPECT_GT(floor_touch[0], 0.0) << "Floor touch sensor did not detect any force"; } } // namespace } // namespace mujoco