// 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::DoubleNear; 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]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // call mj_forward, expect clock to report 0 mj_forward(model, data); EXPECT_EQ(data->sensordata[0], 0.0); // call mj_step, mj_step1, expect clock to be incremented by timestep mj_step(model, data); mj_step1(model, data); 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, data); mj_step1(model, data); 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, data); mj_step1(model, data); EXPECT_EQ(data->time, data->sensordata[0]); mj_deleteData(data); mj_deleteModel(model); } // --------------------- 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]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); mj_forward(model, data); // compare actual and expected values vector pos = GetSensor(model, data, 0); EXPECT_THAT(pos, Pointwise(DoubleNear(tol), {5, 5, 0})); vector xaxis = GetSensor(model, data, 1); EXPECT_THAT(xaxis, Pointwise(DoubleNear(tol), {0, -1, 0})); vector yaxis = GetSensor(model, data, 2); EXPECT_THAT(yaxis, Pointwise(DoubleNear(tol), {1, 0, 0})); mj_deleteData(data); mj_deleteModel(model); } // orientations given by quaternion and by orientation matrix are identical TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // call mj_forward and convert orientation matrix to quaternion mj_forward(model, data); 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, data, 3); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), converted_quat)); mj_deleteData(data); mj_deleteModel(model); } // compare global frame and initially co-located relative frame on same body TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); constexpr int nsensordata = 32; ASSERT_EQ(model->nsensordata, nsensordata); mjData* data = mj_makeData(model); // call mj_forward, save global sensors (colocated with reference frame) mj_forward(model, data); 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, data); // 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(DoubleNear(tol), expected_values)); mj_deleteData(data); mj_deleteModel(model); } // hand-picked velocities and orientations for simple expected values TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); data->qvel[0] = mju_sqrt(2); data->qvel[1] = 1; mj_forward(model, data); // compare to expected values vector linvel = GetSensor(model, data, 0); const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0}; EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel)); mj_deleteData(data); mj_deleteModel(model); } // object and reference in the same body, expect angular velocities to be zero TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // set joint velocities and call forward dynamics data->qvel[0] = 1; mj_forward(model, data); // obj and ref rotate together, relative angular velocities should be zero vector angvel = GetSensor(model, data, 0); EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0})); mj_deleteData(data); mj_deleteModel(model); } // object and reference rotate on the same global axis TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // set joint velocities and call forward dynamics data->qvel[0] = -1; data->qvel[1] = 1; mj_forward(model, data); // obj and ref rotate on same axis, we can just difference the velocities vector angvel = GetSensor(model, data, 0); const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0}; EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel)); mj_deleteData(data); mj_deleteModel(model); } // two arbitrary frames, compare velocity sensors and fin-diffed positions TEST_F(RelativeFrameSensorTest, FrameVelGeneral) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); mjtNum dt = 1e-6; // timestep used for finite differencing // set (arbitrary) joint velocities and call forward dynamics data->qvel[0] = 1; data->qvel[1] = -1; mj_forward(model, data); // save measured linear and angular velocities as vectors vector linvel = GetSensor(model, data, 2); vector angvel = GetSensor(model, data, 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, data); 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(DoubleNear(10*dt), linvel_findiff)); EXPECT_THAT(angvel, Pointwise(DoubleNear(10*dt), angvel_findiff)); mj_deleteData(data); mj_deleteModel(model); } // ------------------------- general sensor tests ----------------------------- using SensorTest = MujocoTest; TEST_F(SensorTest, EnableEnergy) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); mj_forward(model, data); EXPECT_EQ(data->energy[0], 2*3*5); model->opt.enableflags &= ~mjENBL_ENERGY; mj_forward(model, data); EXPECT_EQ(data->energy[0], 0); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, PotentialEnergy) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); mj_forward(model, data); EXPECT_EQ(data->sensordata[0], 2*3*5); data->qpos[2] = 7; mj_forward(model, data); EXPECT_EQ(data->sensordata[0], 7*3*5); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, PotentialEnergyFreeJointSpring) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); data->qpos[0] = 1; data->qpos[1] = 2; data->qpos[2] = 3; mj_forward(model, data); EXPECT_EQ(data->sensordata[0], 0.5*2*14); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, KineticEnergy) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); while (data->time < 1.5) { mj_step(model, data); } mj_forward(model, data); mjtNum mass = 3; mjtNum speed = data->time * mju_norm3(model->opt.gravity); EXPECT_FLOAT_EQ(data->sensordata[0], 0.5 * mass * speed * speed); mj_deleteData(data); mj_deleteModel(model); } // test clock sensor TEST_F(SensorTest, Clock) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // call step 4 times, checking that clock works as expected for (int i=0; i < 5; i++) { mj_step(model, data); mj_step1(model, data); // 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, mjOBJ_SENSOR, 0); EXPECT_EQ(name0, nullptr); const char* name1 = mj_id2name(model, mjOBJ_SENSOR, 1); EXPECT_THAT(name1, StrEq("clampedclock")); mj_deleteData(data); mj_deleteModel(model); } // 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"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); mj_forward(model, data); EXPECT_DOUBLE_EQ(data->sensordata[0], 0.8); EXPECT_DOUBLE_EQ(data->sensordata[1], 0.7); EXPECT_DOUBLE_EQ(data->sensordata[2], 0.5); mjtNum eps = 1e-14; EXPECT_THAT(GetSensor(model, data, 3), Pointwise(DoubleNear(eps), vector{0, 0, 1})); EXPECT_THAT(GetSensor(model, data, 4), Pointwise(DoubleNear(eps), vector{0, 0, -1})); EXPECT_THAT(GetSensor(model, data, 5), Pointwise(DoubleNear(eps), vector{1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 6), Pointwise(DoubleNear(eps), vector{0, 0, 0, 0, 0, .8})); EXPECT_THAT(GetSensor(model, data, 7), Pointwise(DoubleNear(eps), vector{1, 0, .7, 1, 0, 0})); EXPECT_THAT(GetSensor(model, data, 8), Pointwise(DoubleNear(eps), vector{.2, 0, 1, .7, 0, 1})); EXPECT_THAT(GetSensor(model, data, 9), Pointwise(DoubleNear(eps), GetSensor(model, data, 0))); EXPECT_THAT(GetSensor(model, data, 10), Pointwise(DoubleNear(eps), GetSensor(model, data, 6))); EXPECT_THAT(GetSensor(model, data, 11), Pointwise(DoubleNear(eps), GetSensor(model, data, 3))); EXPECT_THAT(GetSensor(model, data, 12), Pointwise(DoubleNear(eps), GetSensor(model, data, 5))); EXPECT_THAT(GetSensor(model, data, 13), Pointwise(DoubleNear(eps), GetSensor(model, data, 8))); EXPECT_THAT(GetSensor(model, data, 14), Pointwise(DoubleNear(eps), GetSensor(model, data, 2))); mj_deleteData(data); mj_deleteModel(model); } 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]; mjModel* model = LoadModelFromString(xml.c_str(), error, sizeof(error)); ASSERT_THAT(model, 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}); mjtNum tol = 1e-4; vector wall = GetSensor(model, data, "wall"); EXPECT_THAT(wall, Pointwise(DoubleNear(tol), {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(DoubleNear(tol), {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(DoubleNear(tol), {3, 0, 0, 0, 0, 1, 4, 0, 0, -1, 0, 0})); vector b2r = GetSensor(model, data, "b2_reduced"); EXPECT_THAT(b2r, Pointwise(DoubleNear(tol), {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(DoubleNear(1e-6), 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(DoubleNear(1e-6), 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"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // call step to update sensors mj_step(model, data); mj_step1(model, data); // update values of position-based sensors 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); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, InsideSite) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; ASSERT_EQ(model->nsensordata, 5); mjData* data = mj_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, data); vector expected(5, 0.0); expected[i] = 1.0; EXPECT_EQ(AsVector(data->sensordata, model->nsensordata), expected); } mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, RangefinderCamera) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, 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); mjData* data = mj_makeData(model); mj_forward(model, data); 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 << ")"; } } mj_deleteData(data); mj_deleteModel(model); } 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]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_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, 1e-10); // 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, data); 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, data); 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, data); 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, data); 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, data); EXPECT_NEAR(data->sensordata[0], 30.0, 1e-10) << "step 4"; mj_deleteData(data); mj_deleteModel(model); } // 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]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_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, 1e-10); // 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, data); 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, data); 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, data); EXPECT_NEAR(data->sensordata[0], 15.0, 1e-10) << "step 2"; mj_deleteData(data); mj_deleteModel(model); } 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]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); int sensor0 = mj_name2id(model, mjOBJ_SENSOR, "default_phase"); int sensor1 = mj_name2id(model, 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, data); // update expected values based on trigger pattern if (triggers0[t]) value0 = t; if (triggers1[t]) value1 = t; EXPECT_NEAR(data->sensordata[adr0], value0, 1e-10) << "sensor0 at t=" << t; EXPECT_NEAR(data->sensordata[adr1], value1, 1e-10) << "sensor1 at t=" << t; } mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, SensorDelayInterval) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // Combined delay and interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10); EXPECT_NEAR(model->sensor_interval[2*0], 0.03, 1e-10); // 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], 1e-10); } // 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, data); // sensor reads delayed value (0.0 from initial buffer) EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step " << i; } // step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0 mj_step(model, data); EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, SensorHistoryOnly) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // history only, no delay or interval EXPECT_EQ(model->sensor_history[0], 5); EXPECT_NEAR(model->sensor_delay[0], 0.0, 1e-10); EXPECT_NEAR(model->sensor_interval[0], 0.0, 1e-10); // set position data->qpos[0] = 3.0; // without delay, sensordata reflects current value immediately mj_step(model, data); EXPECT_NEAR(data->sensordata[0], 3.0, 1e-10); // change position, check again data->qpos[0] = 7.0; mj_step(model, data); EXPECT_NEAR(data->sensordata[0], 7.0, 1e-10); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, SensorDelayMultiDim) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_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, data); 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, data); mj_step(model, data); mj_step(model, data); // 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))); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SensorTest, ReadSensor) { constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_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, data); // inserts 1.0 at t=0, time -> 0.01 data->qpos[0] = 2.0; mj_step(model, data); // inserts 2.0 at t=0.01, time -> 0.02 data->qpos[0] = 3.0; mj_step(model, data); // 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, data, 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, data, 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, data, 0, 0.02, result, /*order=*/0); EXPECT_NEAR(*ptr, 3.0, 1e-10); mj_deleteData(data); mj_deleteModel(model); } // mj_sensorAcc returns correct accelerometer after mj_step1 (issue #3133) TEST_F(SensorTest, AccelerometerAfterStep1) { constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); mjData* data = mj_makeData(model); // settle the simulation with split-step loop for (int i = 0; i < 100; i++) { mj_step1(model, data); mj_step2(model, data); } // call mj_step1 + mj_sensorAcc, expect gravity reading (~9.81 m/s^2) mj_step1(model, data); mj_sensorAcc(model, data); EXPECT_NEAR(data->sensordata[2], 9.81, 1e-2); mj_deleteData(data); mj_deleteModel(model); } } // namespace } // namespace mujoco