chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
8aa111fe0f4cf448b4fe57f5e79110c93967c133
Mujoco_WASM/test/engine/engine_sensor_test.cc
T
Adrian Collister 6f0246baf3 Add test for Flex contact and touch sensors. 
The test verifies that contact sensors on subtrees and touch sensors on sites correctly register contacts involving Flex components.

PiperOrigin-RevId: 929109418
Change-Id: I1865311a6da838d75ef33989ac7f3c2d975cdbff
2026-06-09 04:29:40 -07:00

1872 lines
59 KiB
C++
Raw Blame History

// 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 <algorithm>
#include <cstddef>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include <mujoco/mujoco.h>
#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<mjtNum> GetSensor(const mjModel* model,
const mjData* data, int id) {
return vector<mjtNum>(
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<mjtNum> GetSensor(const mjModel* model,
const mjData* data, const char* name) {
int id = mj_name2id(model, mjOBJ_SENSOR, name);
return vector<mjtNum>(
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"(
<mujoco>
<sensor>
<clock/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body name="reference" pos="3 -4 0" xyaxes="4 3 0 -3 4 0"/>
<site name="object" pos="4 3 0" xyaxes="3 -4 0 4 3 0"/>
</worldbody>
<sensor>
<framepos objtype="site" objname="object"
reftype="xbody" refname="reference"/>
<framexaxis objtype="site" objname="object"
reftype="xbody" refname="reference"/>
<frameyaxis objtype="site" objname="object"
reftype="xbody" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), {5, 5, 0}));
vector xaxis = GetSensor(model, data, 1);
EXPECT_THAT(xaxis, Pointwise(MjNear(tol, 1e-6), {0, -1, 0}));
vector yaxis = GetSensor(model, data, 2);
EXPECT_THAT(yaxis, Pointwise(MjNear(tol, 1e-6), {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"(
<mujoco>
<worldbody>
<site name="reference" euler="10 20 30"/>
<site name="object" euler="20 40 60"/>
</worldbody>
<sensor>
<framexaxis objtype="site" objname="object"
reftype="site" refname="reference"/>
<frameyaxis objtype="site" objname="object"
reftype="site" refname="reference"/>
<framezaxis objtype="site" objname="object"
reftype="site" refname="reference"/>
<framequat objtype="site" objname="object"
reftype="site" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), 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"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<site name="reference"/>
<geom name="object" euler="20 40 60" pos="1 2 3" size="1"/>
</body>
</worldbody>
<sensor>
<framepos objtype="geom" objname="object"/>
<framexaxis objtype="geom" objname="object"/>
<frameyaxis objtype="geom" objname="object"/>
<framezaxis objtype="geom" objname="object"/>
<framequat objtype="geom" objname="object"/>
<framepos objtype="geom" objname="object"
reftype="site" refname="reference"/>
<framexaxis objtype="geom" objname="object"
reftype="site" refname="reference"/>
<frameyaxis objtype="geom" objname="object"
reftype="site" refname="reference"/>
<framezaxis objtype="geom" objname="object"
reftype="site" refname="reference"/>
<framequat objtype="geom" objname="object"
reftype="site" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), 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"(
<mujoco>
<worldbody>
<body xyaxes="1 -1 0 1 1 0">
<joint type="slide" axis="1 0 0"/>
<geom name="reference" size="1"/>
</body>
<body>
<joint type="slide" axis="1 0 0"/>
<geom name="object" size="1"/>
</body>
</worldbody>
<sensor>
<framelinvel objtype="geom" objname="object"
reftype="geom" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), 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"(
<mujoco>
<worldbody>
<body>
<joint type="hinge" axis="1 2 3"/>
<geom name="reference" size="1" pos="1 2 3"/>
<geom name="object" size="1" pos="-3 -2 -1"/>
</body>
</worldbody>
<sensor>
<frameangvel objtype="geom" objname="object"
reftype="geom" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), {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"(
<mujoco>
<worldbody>
<body xyaxes="0 -1 0 1 0 0">
<joint type="hinge" axis="0 1 0"/>
<geom name="reference" size="1"/>
</body>
<body>
<joint type="hinge" axis="1 0 0"/>
<geom name="object" size="1" pos="-3 -2 -1"/>
</body>
</worldbody>
<sensor>
<frameangvel objtype="geom" objname="object"
reftype="geom" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(tol, 1e-6), 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"(
<mujoco>
<worldbody>
<body pos="1 2 3" euler="10 20 30">
<joint type="hinge" axis="2 3 4"/>
<geom name="reference" size="1" pos="0 1 2"/>
</body>
<body pos="-3 -2 -1" euler="20 40 60">
<joint type="hinge" axis="2 3 4"/>
<geom name="object" size="1" pos="1 2 3"/>
</body>
</worldbody>
<sensor>
<framepos objtype="geom" objname="object"
reftype="geom" refname="reference"/>
<framequat objtype="geom" objname="object"
reftype="geom" refname="reference"/>
<framelinvel objtype="geom" objname="object"
reftype="geom" refname="reference"/>
<frameangvel objtype="geom" objname="object"
reftype="geom" refname="reference"/>
</sensor>
</mujoco>
)";
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(MjNear(10 * dt, 0.15),
linvel_findiff));
EXPECT_THAT(angvel,
Pointwise(MjNear(10 * dt, 0.2), angvel_findiff));
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------------------- general sensor tests -----------------------------
using SensorTest = MujocoTest;
TEST_F(SensorTest, EnableEnergy) {
constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -5">
<flag energy="enable"/>
</option>
<worldbody>
<body pos="0 0 2">
<geom size="1" mass="3"/>
<freejoint/>
</body>
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_NEAR(data->energy[0], 2*3*5, MjTol(1e-12, 1e-5));
model->opt.enableflags &= ~mjENBL_ENERGY;
mj_forward(model, data);
EXPECT_NEAR(data->energy[0], 0, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, PotentialEnergy) {
constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -5"/>
<worldbody>
<body pos="0 0 2">
<geom size="1" mass="3"/>
<freejoint/>
</body>
</worldbody>
<sensor>
<e_potential/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_NEAR(data->sensordata[0], 2*3*5, MjTol(1e-12, 1e-5));
data->qpos[2] = 7;
mj_forward(model, data);
EXPECT_NEAR(data->sensordata[0], 7*3*5, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, PotentialEnergyFreeJointSpring) {
constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<geom size="1" mass="3"/>
<joint type="free" stiffness="2"/>
</body>
</worldbody>
<sensor>
<e_potential/>
</sensor>
</mujoco>
)";
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_NEAR(data->sensordata[0], 0.5*2*14, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, KineticEnergy) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="0 0 2">
<geom size="1" mass="3"/>
<freejoint/>
</body>
</worldbody>
<sensor>
<e_kinetic/>
</sensor>
</mujoco>
)";
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_NEAR(data->sensordata[0], 0.5 * mass * speed * speed,
MjTol(1e-7, 1e-2));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, PolyStiffnessEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.0001">
<flag energy="enable"/>
</option>
<worldbody>
<body>
<joint type="slide" stiffness="10 5 1"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qpos="2"/>
</keyframe>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
mjtNum total_energy = d->energy[0] + d->energy[1];
for (int i = 0; i < 100; i++) {
mj_step(m, d);
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.003);
}
mj_deleteData(d);
mj_deleteModel(m);
}
// test clock sensor
TEST_F(SensorTest, Clock) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="1e-3"/>
<sensor>
<clock/>
<clock name="clampedclock" cutoff="3e-3"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<sensor>
<clock name="dummy"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<geom name="plane" type="plane" size="1 1 1"/>
<geom name="sphere1" pos="0 0 1" size="0.2"/>
<geom name="sphere2" pos="1 0 1" size="0.3"/>
</worldbody>
<sensor>
<distance name="0" geom1="plane" geom2="sphere1" cutoff="1"/>
<distance name="1" geom1="sphere2" geom2="plane" cutoff="1"/>
<distance name="2" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<normal name="3" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="4" geom1="sphere2" geom2="plane" cutoff="1"/>
<normal name="5" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="6" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="7" geom1="sphere2" geom2="plane" cutoff="1"/>
<fromto name="8" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<!-- sequential sensors with identical signature -->
<distance name="9" geom1="plane" geom2="sphere1" cutoff="1"/>
<fromto name="10" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="11" geom1="plane" geom2="sphere1" cutoff="1"/>
<normal name="12" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<fromto name="13" geom1="sphere1" geom2="sphere2" cutoff="1"/>
<distance name="14" geom1="sphere1" geom2="sphere2" cutoff="1"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
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<mjtNum>{0, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 4),
Pointwise(MjNear(eps, 1e-7), vector<mjtNum>{0, 0, -1}));
EXPECT_THAT(GetSensor(model, data, 5),
Pointwise(MjNear(eps, 1e-7), vector<mjtNum>{1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 6),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{0, 0, 0, 0, 0, .8}));
EXPECT_THAT(GetSensor(model, data, 7),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{1, 0, .7, 1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 8),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{.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)));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, BadContact) {
string xml_template = R"(
<mujoco>
<worldbody>
<geom name="sphere1" pos="0 0 1" size="0.2"/>
<body name="body">
<freejoint/>
<geom name="sphere2" pos="1 0 1" size="0.3"/>
<site name="site" pos="1 0 1" size="0.3"/>
<body name="non_root">
<geom name="sphere3" pos="1 0 1" size="0.3"/>
</body>
</body>
</worldbody>
<sensor>
<contact BAD_ATTR/>
</sensor>
</mujoco>
)";
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<mjtNum>{4});
vector world = GetSensor(model, data, "world");
EXPECT_EQ(world, vector<mjtNum>{3});
vector b1 = GetSensor(model, data, "b1");
EXPECT_EQ(b1, vector<mjtNum>{3});
vector g1 = GetSensor(model, data, "g1");
EXPECT_EQ(g1, vector<mjtNum>{3});
vector b1g2 = GetSensor(model, data, "b1:g2");
EXPECT_EQ(b1g2, vector<mjtNum>{1});
vector b1world = GetSensor(model, data, "b1:world");
EXPECT_EQ(b1world, vector<mjtNum>{2});
vector site = GetSensor(model, data, "site");
EXPECT_EQ(site, vector<mjtNum>{2});
vector sitewall = GetSensor(model, data, "site:wall");
EXPECT_EQ(sitewall, vector<mjtNum>{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<mjtNum> 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<mjtNum> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<body pos="1.1 0 1">
<geom type="box" size=".1 .6 .375"/>
<site name="frontorigin" pos="-.1 .6 .375"/>
<site name="frontcorner" pos="-.1 -.6 -.375"/>
</body>
<body pos="-1.1 0 1">
<geom type="box" size=".1 .6 .375"/>
<site name="backcenter" pos="-.1 0 0"/>
</body>
<camera pos="0 0 1" xyaxes="0 -1 0 0 0 1" fovy="41.11209"
resolution="1920 1200" name="fixedcamera"/>
</worldbody>
<sensor>
<camprojection site="frontorigin" camera="fixedcamera"/>
<camprojection site="frontcorner" camera="fixedcamera"/>
<camprojection site="backcenter" camera="fixedcamera"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body pos="0 0 1">
<joint type="slide" axis="1 0 0" range="-.75 .75"/>
<geom name="query" type="sphere" size=".01" rgba="1 0 0 1"/>
</body>
<site name="sphere" type="sphere" size=".11" pos="-.5 0 1"/>
<site name="capsule" type="capsule" size=".08 .15" euler="20 -40 60" pos="-.25 0 1"/>
<site name="ellipsoid" type="ellipsoid" size=".11 .15 .09" euler="20 40 -60" pos="0 0 1"/>
<site name="cylinder" type="cylinder" size=".09 .12" euler="-20 40 60" pos=".25 0 1"/>
<site name="box" type="box" size=".08 .1 .14" euler="20 -40 60" pos=".5 0 1"/>
</worldbody>
<sensor>
<insidesite name="sphere" site="sphere" objtype="geom" objname="query"/>
<insidesite name="capsule" site="capsule" objtype="geom" objname="query"/>
<insidesite name="ellipsoid" site="ellipsoid" objtype="geom" objname="query"/>
<insidesite name="cylinder" site="cylinder" objtype="geom" objname="query"/>
<insidesite name="box" site="box" objtype="geom" objname="query"/>
</sensor>
</mujoco>
)";
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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom type="plane" size="10 10 .1"/>
<body pos="0 0 2">
<camera name="persp" xyaxes="1 0 0 0 1 0" resolution="3 3" fovy="90"/>
<camera name="ortho" euler="0 45 0" resolution="3 3"
projection="orthographic" fovy="2"/>
</body>
</worldbody>
<sensor>
<rangefinder camera="persp" data="dist depth"/>
<rangefinder camera="ortho" data="dist dir origin point"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" nsample="3"/>
</sensor>
</mujoco>
)";
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, 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, 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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.015" nsample="3" interp="linear"/>
</sensor>
</mujoco>
)";
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, 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, 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"(
<mujoco>
<option timestep="1" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos name="default_phase" joint="slide" interval="2.5 0" nsample="10"/>
<jointpos name="offset_phase" joint="slide" interval="2.5 -1.5" nsample="10"/>
</sensor>
</mujoco>
)";
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, MjTol(1e-10, 1e-7))
<< "sensor0 at t=" << t;
EXPECT_NEAR(data->sensordata[adr1], value1, MjTol(1e-10, 1e-7))
<< "sensor1 at t=" << t;
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorDelayInterval) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" interval="0.03 0" nsample="5"/>
</sensor>
</mujoco>
)";
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, 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, data);
// 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, data);
EXPECT_NEAR(data->sensordata[0], 5.0, MjTol(1e-10, 1e-7));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorHistoryOnly) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
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, 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, 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"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<ballangvel joint="ball" delay="0.02" nsample="2"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<geom type="plane" size="10 10 0.1"/>
<body pos="0 0 0.1">
<freejoint/>
<geom type="box" size="0.1 0.1 0.1" mass="10"/>
<site name="imu"/>
</body>
</worldbody>
<sensor>
<accelerometer site="imu"/>
</sensor>
</mujoco>
)";
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);
}
// Test tactile sensor reads non-zero values when contacts occur
TEST_F(SensorTest, TactileSkipTangents) {
constexpr char xml[] = R"(
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<mesh name="sensor_mesh" builtin="sphere" params="0"/>
</asset>
<worldbody>
<body pos="0 0 1">
<freejoint/>
<geom name="sensor_geom" type="mesh" mesh="sensor_mesh"/>
</body>
<body>
<geom type="box" size=".7 .7 .3"/>
</body>
</worldbody>
<sensor>
<tactile geom="sensor_geom" mesh="sensor_mesh"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_GT(model->nsensordata, 0) << "No sensor data allocated";
mjData* data = mj_makeData(model);
// Use mj_forward to compute collisions and sensors at t=0
mj_forward(model, data);
// 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";
mj_deleteData(data);
mj_deleteModel(model);
}
// insidesite uses subtree_com for massless flex parent bodies
TEST_F(SensorTest, InsideSiteFlexBody) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body name="parent">
<flexcomp name="soft" type="grid" count="3 3 3"
radius="0.01" dim="3" mass="1">
<elasticity young="5e4" poisson="0.2"/>
</flexcomp>
</body>
<!-- large site centered at origin, should contain the flex -->
<site name="container" type="box" size="2 2 2"/>
</worldbody>
<sensor>
<insidesite name="inside" site="container"
objtype="body" objname="parent"/>
</sensor>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// flex is at origin, site is a large box at origin — should be inside
mj_forward(m, d);
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, 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, d);
// 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";
mj_deleteData(d);
mj_deleteModel(m);
}
// 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"(
<mujoco>
<asset>
<mesh name="sensor_mesh" builtin="sphere" params="0"/>
</asset>
<worldbody>
<body>
<geom size="0.1" contype="0" conaffinity="0"/>
</body>
<body>
<geom name="sensor_geom" type="mesh" mesh="sensor_mesh"/>
</body>
</worldbody>
<sensor>
<tactile geom="sensor_geom" mesh="sensor_mesh"/>
</sensor>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mj_deleteModel(m);
}
// Test that contact and touch sensors work with Flex contacts.
TEST_F(SensorTest, FlexContactSensors) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -9.81"/>
<worldbody>
<geom name="floor" type="plane" size="1 1 10"/>
<body name="parent" pos="0 0 0.005">
<flexcomp name="soft" type="grid" dof="trilinear" cellcount="2 2 2"
count="7 8 9" radius="0.01" dim="3" mass="1"
spacing="0.05 0.05 0.05">
<elasticity young="5e4" poisson="0.2"/>
<contact selfcollide="none"/>
</flexcomp>
</body>
<site name="floor_site" type="box" size="1 1 0.01" pos="0 0 0"/>
</worldbody>
<sensor>
<contact name="flex_contact_subtree" subtree1="parent"/>
<contact name="flex_contact_body" body1="parent"/>
<touch name="floor_touch" site="floor_site"/>
</sensor>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_forward(m, d);
// 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, d, "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, d, "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, d, "floor_touch");
EXPECT_GT(floor_touch[0], 0.0)
<< "Floor touch sensor did not detect any force";
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco
Reference in New Issue View Git Blame Copy Permalink