Initial open sourcing of MuJoCo.

PiperOrigin-RevId: 450374687
Change-Id: Ie3225a46ce095fc28ae8e63c326a640261f562bb

test/engine/CMakeLists.txt

@@ -0,0 +1,54 @@
+# 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
+#
+#     https://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.
+
+mujoco_test(engine_collision_convex_test)
+target_link_libraries(engine_collision_convex_test fixture gmock)
+
+mujoco_test(engine_collision_driver_test)
+target_link_libraries(engine_collision_driver_test fixture gmock)
+
+mujoco_test(engine_core_smooth_test)
+target_link_libraries(engine_core_smooth_test fixture gmock)
+
+mujoco_test(engine_forward_test)
+target_link_libraries(engine_forward_test fixture gmock)
+
+mujoco_test(engine_io_test)
+target_link_libraries(
+  engine_io_test
+  fixture
+  gmock
+  absl::str_format
+)
+
+mujoco_test(engine_ray_test)
+target_link_libraries(engine_ray_test fixture gmock)
+
+mujoco_test(engine_sensor_test)
+target_link_libraries(engine_sensor_test fixture gmock)
+
+mujoco_test(engine_support_test)
+target_link_libraries(engine_support_test fixture gmock)
+
+mujoco_test(engine_util_blas_test)
+target_link_libraries(engine_util_blas_test fixture gmock)
+
+mujoco_test(engine_util_errmem_test)
+target_link_libraries(engine_util_errmem_test fixture gmock)
+
+mujoco_test(engine_util_solve_test)
+target_link_libraries(engine_util_solve_test fixture gmock)
+
+mujoco_test(engine_util_spatial_test)
+target_link_libraries(engine_util_spatial_test fixture gmock)

test/engine/engine_collision_convex_test.cc

@@ -0,0 +1,80 @@
+// 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_collision_convex.c.
+
+#include <cstddef>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::NotNull;
+
+static const char* const kFramelessContactPath =
+    "engine/testdata/collision_convex/frameless_contact.xml";
+static const char* const kFramelessContactHfieldPath =
+    "engine/testdata/collision_convex/frameless_contact_hfield.xml";
+static const char* const kCylinderBoxPath =
+    "engine/testdata/collision_convex/cylinder_box.xml";
+
+using MjcConvexTest = MujocoTest;
+
+TEST_F(MjcConvexTest, FramelessContact) {
+  const std::string xml_path = GetTestDataFilePath(kFramelessContactPath);
+  char error[1024];
+  const std::size_t error_sz = 1024;
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, error_sz);
+  // Loading used to fail with "engine error: xaxis of contact frame undefined".
+  EXPECT_THAT(model, NotNull()) << "Failed to load model: " << error;
+  mj_deleteModel(model);
+}
+
+TEST_F(MjcConvexTest, FramelessContactHfield) {
+  const std::string xml_path = GetTestDataFilePath(kFramelessContactHfieldPath);
+  char error[1024];
+  const std::size_t error_sz = 1024;
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, error_sz);
+  // Loading used to fail with "engine error: xaxis of contact frame undefined".
+  EXPECT_THAT(model, NotNull()) << "Failed to load model: " << error;
+  mj_deleteModel(model);
+}
+
+TEST_F(MjcConvexTest, CylinderBox) {
+  const std::string xml_path = GetTestDataFilePath(kCylinderBoxPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+
+  // with multiCCD enabled, should find 5 contacts
+  mj_forward(model, data);
+  ASSERT_EQ(data->ncon, 5);
+
+  // with multiCCD disabled, should find 1 contact
+  model->opt.enableflags &= ~mjENBL_MULTICCD;
+  mj_forward(model, data);
+  ASSERT_EQ(data->ncon, 1);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_collision_driver_test.cc

@@ -0,0 +1,112 @@
+// 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_collision_driver.c.
+
+#include <cstddef>
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using MjCollisionTest = MujocoTest;
+using GeomPair = std::pair<std::string, std::string>;
+using ::testing::IsEmpty;
+using ::testing::ElementsAre;
+
+// Returns a sorted list of pairs of colliding geom names, where each pair of
+// geom names is sorted.
+static std::vector<GeomPair> colliding_pairs(
+    const mjModel* model, const mjData* data) {
+  std::vector<GeomPair> result;
+  for (int i = 0; i < data->ncon; i++) {
+    std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom1);
+    std::string geom2 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom2);
+    result.push_back(GeomPair(std::min(geom1, geom2), std::max(geom1, geom2)));
+  }
+  std::sort(result.begin(), result.end());
+  return result;
+}
+
+TEST_F(MjCollisionTest, PredefinedPairsOnly) {
+  static const char* const kModelFilePath =
+      "engine/testdata/collisions.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+
+  model->opt.collision = mjCOL_PAIR;
+  mjData* data = mj_makeData(model);
+  mj_fwdPosition(model, data);
+  EXPECT_THAT(colliding_pairs(model, data), ElementsAre(
+      GeomPair("box", "sphere_predefined")));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjCollisionTest, AllCollisions) {
+  static const char* const kModelFilePath =
+      "engine/testdata/collisions.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+
+  // mjCOL_ALL is the default
+  mj_fwdPosition(model, data);
+  EXPECT_THAT(colliding_pairs(model, data), ElementsAre(
+      GeomPair("box", "sphere_collides"),
+      GeomPair("box", "sphere_predefined")
+  ));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjCollisionTest, EmptyModel) {
+  mjModel* model = LoadModelFromString("<mujoco/>");
+  mjData* data = mj_makeData(model);
+
+  mj_fwdPosition(model, data);
+  EXPECT_THAT(colliding_pairs(model, data), IsEmpty());
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjCollisionTest, ZeroedHessian) {
+  static const char* const kModelFilePath =
+      "engine/testdata/collisions.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+
+  mj_fwdPosition(model, data);
+  for (int i = 0; i < data->ncon; i++) {
+    for (int j = 0; j < 36; j++) {
+      EXPECT_FALSE(isnan(data->contact[i].H[j]))
+          << "NaN in contact[" << i << "].H[" << j << "]";
+    }
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+}  // namespace
+}  // namespace mujoco

test/engine/engine_core_smooth_test.cc

@@ -0,0 +1,334 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Tests for engine/engine_core_smooth.c.
+
+#include <cstddef>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::ElementsAre;
+using CoreSmoothTest = MujocoTest;
+
+static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
+  return std::vector<mjtNum>(array, array + length);
+}
+
+// --------------------------- connect constraint ------------------------------
+
+TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int i=0; i < 3; i++) {
+    EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int i=0; i < 3; i++) {
+    EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/connect/force_free.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int i=0; i < 3; i++) {
+    EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostConnectTorque) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int i=0; i < 3; i++) {
+    EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int i=0; i < 3; i++) {
+    EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+// --------------------------- weld constraint ---------------------------------
+
+TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/weld/force_free.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
+    for (int i=0; i < 3; i++) {
+      EXPECT_NEAR(
+          data->sensordata[model->sensor_adr[sensor_index] + i],
+          model->sensor_user[model->nuser_sensor*sensor_index + i],
+          1e-6);
+    }
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotatoed) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
+    for (int i=0; i < 3; i++) {
+      EXPECT_NEAR(
+          data->sensordata[model->sensor_adr[sensor_index] + i],
+          model->sensor_user[model->nuser_sensor*sensor_index + i],
+          1e-6);
+    }
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
+    for (int i=0; i < 3; i++) {
+      EXPECT_NEAR(
+          data->sensordata[model->sensor_adr[sensor_index] + i],
+          model->sensor_user[model->nuser_sensor*sensor_index + i],
+          1e-6);
+    }
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
+  static const char* const kModelFilePath =
+      "engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
+  const std::string xml_path = GetTestDataFilePath(kModelFilePath);
+  mjModel* model =
+      mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  mjData* data = mj_makeData(model);
+  // settle physics:
+  for (int i=0; i < 1000; i++) {
+    mj_step(model, data);
+  }
+  for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
+    for (int i=0; i < 3; i++) {
+      EXPECT_NEAR(
+          data->sensordata[model->sensor_adr[sensor_index] + i],
+          model->sensor_user[model->nuser_sensor*sensor_index + i],
+          1e-6);
+    }
+  }
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+// ----------------------------- fluidshape --------------------------------
+
+using EllipsoidFluidTest = MujocoTest;
+
+TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
+  static constexpr char two_bodies_xml[] = R"(
+  <mujoco>
+    <option wind="5 5 0" density="10"/>
+    <worldbody>
+      <body>
+        <freejoint/>
+        <body>
+          <geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
+        </body>
+        <body>
+          <geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
+        </body>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  mjModel* m2 = LoadModelFromString(two_bodies_xml);
+  mjData* d2 = mj_makeData(m2);
+  for (int i = 0; i < 6; i++) {
+    d2->qvel[i] = (mjtNum) i+1;
+  }
+  d2->qpos[3] = 0.5;
+  d2->qpos[4] = 0.5;
+  d2->qpos[5] = 0.5;
+  d2->qpos[6] = 0.5;
+
+  static constexpr char one_body_xml[] = R"(
+  <mujoco>
+    <option wind="5 5 0" density="10"/>
+    <worldbody>
+      <body pos="1 2 3">
+        <freejoint/>
+        <geom type="box" size=".1 .01 0.01" pos="0.1 0 0" euler="40 0 0" fluidshape="ellipsoid"/>
+        <geom type="box" size=".1 .01 0.01" pos="-.1 0 0" euler="0 20 0" fluidshape="ellipsoid"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  mjModel* m1 = LoadModelFromString(one_body_xml);
+  mjData* d1 = mj_makeData(m1);
+  for (int i = 0; i < 6; i++) {
+    d1->qvel[i] = (mjtNum) i+1;
+  }
+  d1->qpos[3] = 0.5;
+  d1->qpos[4] = 0.5;
+  d1->qpos[5] = 0.5;
+  d1->qpos[6] = 0.5;
+
+  const mjtNum tol = 1e-14;  // tolerance for floating point numbers
+
+  EXPECT_EQ(m1->nv, m2->nv);
+  for (int i = 0; i < m1->nv; i++) {
+    EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
+  }
+
+  mj_forward(m2, d2);
+  mj_forward(m1, d1);
+  for (int i = 0; i < m1->nv; i++) {
+    EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
+  }
+
+  mj_deleteData(d1);
+  mj_deleteModel(m1);
+  mj_deleteData(d2);
+  mj_deleteModel(m2);
+}
+
+TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option wind="5 5 0" density="10"/>
+    <default>
+      <geom fluidshape="ellipsoid" fluidcoef="2 3 4 5 6"/>
+      <default class="test_class">
+        <geom fluidshape="none" fluidcoef="5 4 3 2 1"/>
+      </default>
+    </default>
+    <worldbody>
+      <body>
+        <freejoint/>
+        <geom type="box" size=".1 .01 0.01" pos="0.1 0 0" class="test_class"/>
+        <geom type="box" size=".1 .01 0.01" pos="-0.1 0 0"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  EXPECT_THAT(GetVector(model->geom_fluid, 6),
+              ElementsAre(0, 0, 0, 0, 0, 0));
+  EXPECT_THAT(GetVector(model->geom_fluid + mjNFLUID, 6),
+              ElementsAre(1, 2, 3, 4, 5, 6));
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_forward_test.cc

@@ -0,0 +1,80 @@
+// 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_forward.c.
+
+#include "src/engine/engine_forward.h"
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "src/engine/engine_io.h"
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ForwardTest = MujocoTest;
+
+TEST_F(ForwardTest, ActLimited) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option timestep="0.01"/>
+    <worldbody>
+      <body>
+        <joint name="slide" type="slide" axis="1 0 0"/>
+        <geom size=".1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <general joint="slide" gainprm="100" biasprm="0 -100" biastype="affine"
+      dynprm="10" dyntype="integrator"
+      actlimited="true" actrange="-1 1"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+
+  data->ctrl[0] = 1.0;
+  // integrating up from 0, we will hit the clamp after 99 steps
+  for (int i=0; i<200; i++) {
+    mj_step(model, data);
+    // always greater than lower bound
+    ASSERT_GT(data->act[0], -1);
+    // after 99 steps we hit the upper bound
+    if (i < 99) ASSERT_LT(data->act[0], 1);
+    if (i >= 99) ASSERT_EQ(data->act[0], 1);
+  }
+
+  data->ctrl[0] = -1.0;
+  // integrating down from 1, we will hit the clamp after 199 steps
+  for (int i=0; i<300; i++) {
+    mj_step(model, data);
+    // always smaller than upper bound
+    ASSERT_LT(data->act[0], model->actuator_actrange[1]);
+    // after 199 steps we hit the lower bound
+    if (i < 199) ASSERT_GT(data->act[0], model->actuator_actrange[0]);
+    if (i >= 199) ASSERT_EQ(data->act[0], model->actuator_actrange[0]);
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_io_test.cc

@@ -0,0 +1,588 @@
+// 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_io.c.
+
+#include "src/engine/engine_io.h"
+
+#include <array>
+#include <cstring>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <absl/strings/str_format.h>
+#include <mujoco/mjxmacro.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::HasSubstr;
+using ::testing::IsNull;
+using ::testing::NotNull;
+
+using EngineIoTest = MujocoTest;
+
+// Return an mjModel with just the ints set.
+mjModel PartialModel(const mjModel* m) {
+  mjModel partial_model = {0};
+  #define X(var) partial_model.var = m->var;
+  MJMODEL_INTS;
+  #undef X
+  partial_model.nbuffer = 0;
+  return partial_model;
+}
+
+TEST_F(EngineIoTest, MakeDataFromPartialModel) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+  mjData* data_from_model = mj_makeData(model);
+  ASSERT_THAT(data_from_model, NotNull());
+
+  mjModel partial_model = PartialModel(model);
+  mj_deleteModel(model);
+
+  mjData* data_from_partial = mj_makeData(&partial_model);
+  ASSERT_THAT(data_from_partial, NotNull());
+
+  EXPECT_EQ(data_from_partial->nbuffer, data_from_model->nbuffer);
+  int nbuffer = data_from_partial->nbuffer;
+  // If there are no mocap bodies and qpos0 is all zero, mjData should be the
+  // same whether it was made from the full model or the partial model.
+  EXPECT_EQ(
+      std::memcmp(data_from_partial->buffer, data_from_model->buffer, nbuffer),
+      0) << "mjData content differs";
+
+  mj_deleteData(data_from_model);
+  mj_deleteData(data_from_partial);
+}
+
+TEST_F(EngineIoTest, MakeDataLoadsQpos0) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+  model->qpos0[0] = 1;
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+  EXPECT_EQ(data->qpos[0], 1);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body mocap="true" pos="42 0 42">
+        <geom type="sphere" size="0.1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+  EXPECT_EQ(data->mocap_pos[0], 42);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(EngineIoTest, CopyDataWithPartialModel) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <geom size="1"/>
+      </body>
+      <body mocap="true" pos="42 0 42">
+        <geom type="sphere" size="0.1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+
+  mjModel partial_model = PartialModel(model);
+  mj_deleteModel(model);
+  mjData* copy = mj_makeData(&partial_model);
+  ASSERT_THAT(copy, NotNull());
+
+  data->qpos[0] = 1;
+  mj_copyData(copy, &partial_model, data);
+
+  EXPECT_EQ(copy->nbuffer, data->nbuffer);
+  EXPECT_EQ(copy->qpos[0], 1);
+  int nbuffer = copy->nbuffer;
+  EXPECT_EQ(
+      std::memcmp(copy->buffer, data->buffer, nbuffer),
+      0) << "mjData content differs";
+
+  mj_deleteData(data);
+  mj_deleteData(copy);
+}
+
+using ValidateReferencesTest = MujocoTest;
+
+TEST_F(ValidateReferencesTest, BodyReferences) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  model->jnt_bodyid[0] = 2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("jnt_bodyid"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, AddressRange) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <joint/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  model->body_jntnum[1] = 3;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
+  model->body_jntnum[1] = 2;
+  // Could be more strict and test for -1, but at the moment the code is a bit
+  // lenient.
+  model->body_jntadr[1] = -2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, GeomCondim) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint/>
+        <geom size="1" condim="6"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+  model->geom_condim[0] = 7;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
+  model->geom_condim[0] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, HField) {
+  static const char xml[] = R"(
+  <mujoco>
+    <asset>
+      <hfield name="h" nrow="2" ncol="3" size="1 1 1 1" />
+    </asset>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  model->hfield_adr[0] = -2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
+  model->hfield_adr[0] = 0;
+  model->hfield_ncol[0] = 4;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, Texture) {
+  static const char xml[] = R"(
+  <mujoco>
+    <asset>
+      <texture name="t" type="2d" width="2" height="3" builtin="flat" />
+    </asset>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  model->tex_adr[0] = -2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
+  model->tex_adr[0] = 0;
+  model->tex_height[0] = 4;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, GeomPairs) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom name="geom0" size="1"/>
+      </body>
+      <body>
+        <geom name="geom1" size="1"/>
+        <geom name="geom2" size="1"/>
+        <geom name="geom3" size="1"/>
+      </body>
+    </worldbody>
+    <contact>
+      <pair geom1="geom0" geom2="geom3"/>
+    </contact>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  // Invalid geomid=4
+  model->pair_signature[0] = (1 << 16) | 5;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body1"));
+
+  model->pair_signature[0] = (5 << 16) | 1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body2"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, SensorsAddress) {
+  // The test will likely only catch sensor size errors for the last sensor
+  // in the model, so iterate over possible last sensors, instead of adding
+  // them all into the same model.
+  static const char xml_template[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body1">
+        <joint name="slider" type="slide" axis="0 0 1"
+            limited="true" range="-.2 .5"/>
+        <geom name="geom1" size="1"/>
+        <site name="site1"/>
+      </body>
+    </worldbody>
+    <sensor>
+      %s
+    </sensor>
+  </mujoco>
+  )";
+  std::vector<std::string> sensor_strings{
+      "<framepos objtype='site' objname='site1'/>",
+      "<rangefinder site='site1'/>",
+      "<gyro site='site1'/>",
+      "<touch site='site1'/>",
+      "<force site='site1'/>",
+      "<torque site='site1'/>",
+      "<jointlimitfrc joint='slider'/>",
+      "<accelerometer site='site1'/>",
+      "<subtreeangmom body='body1'/>",
+  };
+
+  for (const std::string& sensor_string : sensor_strings) {
+    std::string xml = absl::StrFormat(xml_template, sensor_string);
+    std::array<char, 1024> error;
+    mjModel* model =
+        LoadModelFromString(xml.c_str(), error.data(), error.size());
+    ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+    EXPECT_THAT(mj_validateReferences(model), IsNull());
+    mj_deleteModel(model);
+  }
+}
+
+TEST_F(ValidateReferencesTest, SensorsObj) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body1">
+        <joint/>
+        <geom size="1"/>
+        <site name="site1"/>
+      </body>
+    </worldbody>
+    <sensor>
+      <framepos objtype="site" objname="site1" reftype="body" refname="body1"/>
+    </sensor>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  model->sensor_objtype[0] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objtype"));
+  model->sensor_objtype[0] = mjOBJ_SITE;
+
+  model->sensor_objid[0] = model->nsite;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objid"));
+  model->sensor_objid[0] = 0;
+
+  model->sensor_reftype[0] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_reftype"));
+  model->sensor_reftype[0] = mjOBJ_BODY;
+
+  model->sensor_refid[0] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_refid"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom name="geom0" size="1"/>
+      </body>
+      <body>
+        <geom name="geom1" size="1"/>
+      </body>
+    </worldbody>
+    <contact>
+      <pair geom1="geom1" geom2="geom0"/>
+    </contact>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, BodyExcludes) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body1" />
+      <body name="body2" />
+    </worldbody>
+    <contact>
+      <exclude body1="body1" body2="body2"/>
+    </contact>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  // Invalid bodyid=3
+  model->exclude_signature[0] = (1 << 16) | 4;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body1"));
+  model->exclude_signature[0] = (4 << 16) | 2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body2"));
+
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, EqualityConstraints) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body1">
+        <joint name="joint1"/>
+        <geom name="geom1" size="1"/>
+      </body>
+      <body name="body2">
+        <joint/>
+        <joint name="joint2"/>
+        <geom size="1"/>
+        <geom size="1"/>
+        <geom name="geom2" size="1"/>
+      </body>
+    </worldbody>
+    <tendon>
+      <fixed name="tendon1"><joint joint="joint1" coef="1"/></fixed>
+      <fixed><joint joint="joint1" coef="1"/></fixed>
+      <fixed><joint joint="joint1" coef="1"/></fixed>
+      <fixed><joint joint="joint1" coef="1"/></fixed>
+      <fixed name="tendon2"><joint joint="joint1" coef="1"/></fixed>
+    </tendon>
+    <equality>
+      <connect anchor="0 0 0" body1="body1" />
+      <weld body1="body1" body2="body2" />
+      <distance geom1="geom1" geom2="geom2" />
+      <joint joint1="joint1"/>
+      <joint joint1="joint1" joint2="joint2"/>
+      <tendon tendon1="tendon1"/>
+      <tendon tendon1="tendon1" tendon2="tendon2"/>
+    </equality>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  // connect constraint
+  model->eq_obj1id[0] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[0] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[0] = 1;
+
+  model->eq_obj2id[0] = -2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[0] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[0] = 0;
+
+  // weld constraint
+  model->eq_obj1id[1] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[1] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[1] = 1;
+
+  model->eq_obj2id[1] = -2;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[1] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[1] = model->nbody - 1;
+
+  // distance constraint
+  model->eq_obj1id[2] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[2] = model->ngeom;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
+  model->eq_obj1id[2] = 1;
+
+  model->eq_obj2id[2] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[2] = model->ngeom;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
+  model->eq_obj2id[2] = model->ngeom - 1;
+  mj_deleteModel(model);
+}
+
+TEST_F(ValidateReferencesTest, Tuples) {
+  static const char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="body1">
+        <joint name="joint1"/>
+        <geom size="1"/>
+      </body>
+      <body name="body2">
+        <joint/>
+        <joint name="joint2"/>
+        <geom size="1"/>
+      </body>
+    </worldbody>
+    <custom>
+      <tuple name="tuple">
+        <element objtype="body" objname="body1"/>
+        <element objtype="joint" objname="joint2"/>
+      </tuple>
+    </custom>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  EXPECT_THAT(mj_validateReferences(model), IsNull());
+
+  model->tuple_objtype[0] = -1;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objtype"));
+  model->tuple_objtype[0] = mjOBJ_BODY;
+
+  model->tuple_objid[0] = model->nbody;
+  EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objid"));
+  model->tuple_objid[0] = 1;
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_ray_test.cc

@@ -0,0 +1,137 @@
+// Copyright 2022 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 ray casting.
+
+#include <array>
+#include <cstddef>
+#include <cstring>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjdata.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+static constexpr char kRayCastingModel[] = R"(
+<mujoco>
+  <worldbody>
+    <geom name="static_group1" type="sphere" size=".1" pos="1 0 0"
+     group="1"/>
+    <body pos="0 0 0">
+      <body pos="0 0 0">
+        <geom name="group0" type="sphere" size=".1" pos="3 0 0"/>
+      </body>
+      <geom name="group2" type="sphere" size=".1" pos="5 0 0" group="2"/>
+    </body>
+  </worldbody>
+</mujoco>
+)";
+
+using ::testing::NotNull;
+using RayTest = MujocoTest;
+
+TEST_F(RayTest, NoExclusions) {
+  mjModel* model = LoadModelFromString(kRayCastingModel);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+
+  mjtNum pnt[] = {0.0, 0.0, 0.0};
+  mjtNum vec[] = {1.0, 0.0, 0.0};
+  mjtByte* geomgroup = nullptr;
+  mjtByte flg_static = 1;  // Include static geoms
+  int bodyexclude = -1;
+  int geomid = -1;
+
+  mj_kinematics(model, data);
+  mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
+                           bodyexclude, &geomid);
+  EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
+  EXPECT_FLOAT_EQ(distance, 0.9);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(RayTest, Exclusions) {
+  mjModel* model = LoadModelFromString(kRayCastingModel);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+
+  mjtNum pnt[] = {0.0, 0.0, 0.0};
+  mjtNum vec[] = {1.0, 0.0, 0.0};
+  mjtByte geomgroup[] = {1, 1, 1};
+  mjtByte flg_static = 1;
+  int bodyexclude = -1;
+  int geomid = -1;
+
+  mj_kinematics(model, data);
+  mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
+                           bodyexclude, &geomid);
+  EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
+  EXPECT_FLOAT_EQ(distance, 0.9);
+
+  // Exclude nearest geom
+  geomgroup[1] = 0;
+  distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
+                    &geomid);
+  EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
+  EXPECT_FLOAT_EQ(distance, 2.9);
+
+  geomgroup[0] = 0;
+  distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
+                    &geomid);
+  EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
+  EXPECT_FLOAT_EQ(distance, 4.9);
+
+  geomgroup[2] = 0;
+  distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
+                    &geomid);
+  EXPECT_EQ(geomid, -1);
+  EXPECT_FLOAT_EQ(distance, -1);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(RayTest, ExcludeStatic) {
+  mjModel* model = LoadModelFromString(kRayCastingModel);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+  ASSERT_THAT(data, NotNull());
+
+  mjtNum pnt[] = {0.0, 0.0, 0.0};
+  mjtNum vec[] = {1.0, 0.0, 0.0};
+  mjtByte geomgroup[] = {1, 1, 1};
+  mjtByte flg_static = 0;  // Exclude static geoms
+  int bodyexclude = -1;
+  int geomid = -1;
+
+  mj_kinematics(model, data);
+  mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
+                           bodyexclude, &geomid);
+  EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
+  EXPECT_FLOAT_EQ(distance, 2.9);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_sensor_test.cc

@@ -0,0 +1,361 @@
+// 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 <array>
+#include <cstddef>
+#include <cstdio>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mjtnum.h>
+#include <mujoco/mujoco.h>
+#include "src/engine/engine_util_blas.h"
+#include "src/engine/engine_util_spatial.h"
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+// returns as a vector the measured values from sensor with index `id`
+static std::vector<mjtNum> GetSensor(const mjModel* model, const mjData* data, int id) {
+  return std::vector<mjtNum>(data->sensordata + model->sensor_adr[id],
+                             data->sensordata + model->sensor_adr[id] + model->sensor_dim[id]);
+}
+
+
+// --------------------- test relative frame sensors  --------------------------
+
+using ::testing::Pointwise;
+using ::testing::DoubleNear;
+using RelativeFrameSensorTest = MujocoTest;
+
+const mjtNum tol = 1e-14;  // nearness tolerance for floating point numbers
+
+// 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>
+  )";
+  mjModel* model = LoadModelFromString(xml, 0, 0);
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  // compare actual and expected values
+  std::vector pos = GetSensor(model, data, 0);
+  EXPECT_THAT(pos, Pointwise(DoubleNear(tol), {5, 5, 0}));
+
+  std::vector xaxis = GetSensor(model, data, 1);
+  EXPECT_THAT(xaxis, Pointwise(DoubleNear(tol), {0, -1, 0}));
+
+  std::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"(
+  <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, 0, 0);
+  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
+  std::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"(
+  <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, 0, 0);
+  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);
+  std::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);
+
+  // note that in the loop above the quat is unnormalized, but that's ok,
+  // quaternions are automatically normalized in place:
+  EXPECT_NEAR(mju_norm(data->qpos+3, 4), 1.0, tol);
+
+  // get values from relative sensors after moving the object
+  std::vector actual_values(data->sensordata+nsensordata/2,
+                            data->sensordata+nsensordata);
+
+  // object and reference have moved together, we expect values to not unchange
+  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"(
+  <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, 0, 0);
+  mjData* data = mj_makeData(model);
+  data->qvel[0] = mju_sqrt(2);
+  data->qvel[1] = 1;
+  mj_forward(model, data);
+
+  // compare to expected values
+  std::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 velocites 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, 0, 0);
+  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 velocites should be zero
+  std::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"(
+  <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, 0, 0);
+  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
+  std::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"(
+  <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, 0, 0);
+  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
+  std::vector linvel = GetSensor(model, data, 2);
+  std::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);
+}
+
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_support_test.cc

@@ -0,0 +1,40 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Tests for engine/engine_support.c.
+
+#include <string_view>
+
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+#include <gtest/gtest.h>
+
+namespace mujoco {
+namespace {
+
+using VersionTest = MujocoTest;
+
+const char *const kExpectedVersionString = "2.2.0";
+
+TEST_F(VersionTest, MjVersion) {
+  EXPECT_EQ(mj_version(), mjVERSION_HEADER);
+}
+
+TEST_F(VersionTest, MjVersionString) {
+  EXPECT_EQ(std::string_view(mj_versionString()), kExpectedVersionString);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_util_blas_test.cc

@@ -0,0 +1,45 @@
+// Copyright 2022 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_util_blas.c
+
+#include "src/engine/engine_util_blas.h"
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjtnum.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using EngineUtilBlasTest = MujocoTest;
+
+TEST_F(EngineUtilBlasTest, MjuDot) {
+  mjtNum a[] = {1, 2, 3, 4, 5, 6, 7};
+  mjtNum b[] = {7, 6, 5, 4, 3, 2, 1};
+
+  // test various vector lengths because mju_dot adds numbers in groups of four
+  EXPECT_EQ(mju_dot(a, b, 0), 0);
+  EXPECT_EQ(mju_dot(a, b, 1), 7);
+  EXPECT_EQ(mju_dot(a, b, 2), 7 + 2*6);
+  EXPECT_EQ(mju_dot(a, b, 3), 7 + 2*6 + 3*5);
+  EXPECT_EQ(mju_dot(a, b, 4), 7 + 2*6 + 3*5 + 4*4);
+  EXPECT_EQ(mju_dot(a, b, 5), 7 + 2*6 + 3*5 + 4*4 + 5*3);
+  EXPECT_EQ(mju_dot(a, b, 6), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2);
+  EXPECT_EQ(mju_dot(a, b, 7), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2 + 7);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_util_errmem_test.cc

@@ -0,0 +1,137 @@
+// 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_util_errmem.c.
+
+#include <cstring>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include "src/engine/engine_util_errmem.h"
+
+namespace mujoco {
+namespace {
+
+constexpr int kBufferSize = 1000;
+
+char* ErrorMessageBuffer() {
+  static char error_message[kBufferSize] = "";
+  return error_message;
+}
+
+char* WarningMessageBuffer() {
+  static char warning_message[kBufferSize] = "";
+  return warning_message;
+}
+
+void MjErrorHandler(const char* msg) {
+  if (strnlen(msg, kBufferSize) == kBufferSize) {
+    FAIL() << "mju_user_error message exceeds maximum length of "
+           << kBufferSize;
+  }
+  strncpy(ErrorMessageBuffer(), msg, kBufferSize);
+}
+
+void MjWarningHandler(const char* msg) {
+  if (strnlen(msg, kBufferSize) == kBufferSize) {
+    FAIL() << "mju_user_warning message exceeds maximum length of "
+           << kBufferSize;
+  }
+  strncpy(WarningMessageBuffer(), msg, kBufferSize);
+}
+
+void ClearErrorMessage() { ErrorMessageBuffer()[0] = '\0'; }
+
+void ClearWarningMessage() { WarningMessageBuffer()[0] = '\0'; }
+
+class MujocoErrorAndWarningTest : public ::testing::Test {
+ public:
+  MujocoErrorAndWarningTest() {
+    mju_user_error = MjErrorHandler;
+    mju_user_warning = MjWarningHandler;
+  }
+
+  ~MujocoErrorAndWarningTest() {
+    mju_user_error = nullptr;
+    mju_user_warning = nullptr;
+  }
+};
+
+TEST_F(MujocoErrorAndWarningTest, MjuErrorI) {
+  std::string format_string = "%010d";
+  while (format_string.length() < 2 * kBufferSize) {
+    format_string += 'x';
+  }
+
+  std::string expected_message = "0123456789";
+  while (expected_message.length() < kBufferSize - 1) {
+    expected_message += 'x';
+  }
+
+  ClearErrorMessage();
+  mju_error_i(format_string.c_str(), 123456789);
+  EXPECT_EQ(std::string(ErrorMessageBuffer()), expected_message);
+}
+
+TEST_F(MujocoErrorAndWarningTest, MjuWarningI) {
+  std::string format_string = "%010d";
+  while (format_string.length() < 2 * kBufferSize) {
+    format_string += 'x';
+  }
+
+  std::string expected_message = "0123456789";
+  while (expected_message.length() < kBufferSize - 1) {
+    expected_message += 'x';
+  }
+
+  ClearWarningMessage();
+  mju_warning_i(format_string.c_str(), 123456789);
+  EXPECT_EQ(std::string(WarningMessageBuffer()), expected_message);
+}
+
+TEST_F(MujocoErrorAndWarningTest, MjuErrorS) {
+  std::string format_string = "% 9s";
+  while (format_string.length() < 2 * kBufferSize) {
+    format_string += 'z';
+  }
+
+  std::string expected_message = "   foobar";
+  while (expected_message.length() < kBufferSize - 1) {
+    expected_message += 'z';
+  }
+
+  ClearErrorMessage();
+  mju_error_s(format_string.c_str(), "foobar");
+  EXPECT_EQ(std::string(ErrorMessageBuffer()), expected_message);
+}
+
+TEST_F(MujocoErrorAndWarningTest, MjuWarningS) {
+  std::string format_string = "% 9s";
+  while (format_string.length() < 2 * kBufferSize) {
+    format_string += 'z';
+  }
+
+  std::string expected_message = "   foobar";
+  while (expected_message.length() < kBufferSize - 1) {
+    expected_message += 'z';
+  }
+
+  ClearWarningMessage();
+  mju_warning_s(format_string.c_str(), "foobar");
+  EXPECT_EQ(std::string(WarningMessageBuffer()), expected_message);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_util_solve_test.cc

@@ -0,0 +1,66 @@
+// 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_util_solve.c.
+
+#include "src/engine/engine_util_solve.h"
+
+#include <gtest/gtest.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using QCQP2Test = MujocoTest;
+
+TEST_F(QCQP2Test, DegenerateAMatrix) {
+  // A 2x2 matrix with determinant zero.
+  const mjtNum Ain[9] { 6, -15, 2, -5 };
+
+  // Any values will do for these three inputs.
+  const mjtNum bin[3] { -12, 49 };
+  const mjtNum d[3] { 11, 31 };
+  const mjtNum r = 0.01;
+
+  // Make output array explicitly nonzero to simulate uninitialized memory.
+  mjtNum res[2] { 999, 999 };
+
+  EXPECT_EQ(mju_QCQP2(res, Ain, bin, d, r), 0);
+  EXPECT_EQ(res[0], 0);
+  EXPECT_EQ(res[1], 0);
+}
+
+using QCQP3Test = MujocoTest;
+
+TEST_F(QCQP3Test, DegenerateAMatrix) {
+  // A 3x3 matrix with determinant zero.
+  const mjtNum Ain[9] { 1, 4, -2, -3, -7, 5, 2, -9, 0 };
+
+  // Any values will do for these three inputs.
+  const mjtNum bin[3] { -12, 49, 8 };
+  const mjtNum d[3] { 11, 31, -23 };
+  const mjtNum r = 0.1;
+
+  // Make output array explicitly nonzero to simulate uninitialized memory.
+  mjtNum res[3] { 999, 999, 999 };
+
+  EXPECT_EQ(mju_QCQP3(res, Ain, bin, d, r), 0);
+  EXPECT_EQ(res[0], 0);
+  EXPECT_EQ(res[1], 0);
+  EXPECT_EQ(res[2], 0);
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/engine_util_spatial_test.cc

@@ -0,0 +1,106 @@
+// Copyright 2022 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_util_spatial.c
+
+#include "src/engine/engine_util_spatial.h"
+
+#include <cmath>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjtnum.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::ElementsAre;
+
+using Quat2MatTest = MujocoTest;
+
+std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
+  return std::vector<mjtNum>(array, array + n);
+}
+
+TEST_F(Quat2MatTest, NoRotation) {
+  mjtNum result[9] = {0};
+  mjtNum quat[] = {1, 0, 0, 0};
+  mju_quat2Mat(result, quat);
+  EXPECT_THAT(
+      AsVector(result, 9),
+      ElementsAre(1, 0, 0,
+                  0, 1, 0,
+                  0, 0, 1)
+  );
+}
+
+TEST_F(Quat2MatTest, TinyRotation) {
+  mjtNum result[9] = {0};
+  // An angle so small that cos(angle) == 1.0 to double accuracy
+  mjtNum angle = 1e-8;
+  mjtNum quat[] = {cos(angle/2), sin(angle/2), 0, 0};
+  mju_quat2Mat(result, quat);
+  EXPECT_THAT(
+      AsVector(result, 9),
+      ElementsAre(1, 0         ,  0         ,
+                  0, cos(angle), -sin(angle),
+                  0, sin(angle),  cos(angle))
+  );
+}
+
+using MulQuatTest = MujocoTest;
+
+TEST_F(MulQuatTest, TinyRotation) {
+  mjtNum null_quat[4] = {1, 0, 0, 0};
+  mjtNum result[4];
+  // An angle so small that cos(angle) == 1.0 to double accuracy
+  mjtNum angle = 1e-8;
+  mjtNum quat[] = {cos(angle/2), sin(angle/2), 0, 0};
+  mju_mulQuat(result, null_quat, quat);
+  EXPECT_THAT(
+      AsVector(result, 4),
+      ElementsAre(cos(angle/2), sin(angle/2), 0, 0)
+  );
+}
+
+using RotVecQuatTest = MujocoTest;
+
+TEST_F(RotVecQuatTest, NoRotation) {
+  mjtNum result[3];
+  mjtNum vec[] = {1, 2, 3};
+  mjtNum quat[] = {1, 0, 0, 0};
+  mju_rotVecQuat(result, vec, quat);
+  EXPECT_THAT(
+      AsVector(result, 3),
+      ElementsAre(1, 2, 3)
+  );
+}
+
+TEST_F(RotVecQuatTest, TinyRotation) {
+  mjtNum result[3];
+  mjtNum vec[] = {0, 1, 0};
+  // An angle so small that cos(angle) == 1.0 to double accuracy
+  mjtNum angle = 1e-8;
+  mjtNum quat[] = {cos(angle/2), sin(angle/2), 0, 0};
+  mju_rotVecQuat(result, vec, quat);
+  EXPECT_THAT(
+      AsVector(result, 3),
+      ElementsAre(0, cos(angle), sin(angle))
+  );
+}
+
+}  // namespace
+}  // namespace mujoco

test/engine/testdata/collision_convex/capsule_mesh.xml

@@ -0,0 +1,25 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <quality shadowsize="8192"/>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <asset>
+    <mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
+          scale="1 1 .3"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1"/>
+    <geom type="mesh" mesh="box" pos="0 0 -.3"/>
+    <body pos="0 0 .02" euler="90 40 0">
+      <freejoint/>
+      <geom type="capsule" size=".03 .1"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/cylinder_box.xml

@@ -0,0 +1,20 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <quality shadowsize="8192"/>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <worldbody>
+    <light pos="0 0 1"/>
+    <geom type="box" size="1 1 .3" pos="0 0 -.3" rgba=".5 .5 .5 .5"/>
+    <body pos="0 0 .02">
+      <freejoint/>
+      <geom type="cylinder" size=".4 .03"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/frameless_contact.xml

@@ -0,0 +1,9 @@
+<mujoco model="frameless contact">
+  <worldbody>
+    <geom type="box" pos="0.0 0.0 -0.46" size="0.4 0.4 0.46"/>
+    <body>
+      <joint type="slide" axis="1 0 0"/>
+      <geom pos="0.2 0 0.02" size="0.062 0.02" type="cylinder"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/frameless_contact_hfield.xml

@@ -0,0 +1,14 @@
+<mujoco model="frameless contact">
+  <asset>
+    <hfield name="hf" nrow="10" ncol="10" size="1 1 0.1 0.01"/>
+  </asset>
+
+  <worldbody>
+    <geom type="hfield" hfield="hf"/>
+    <body>
+      <joint type="slide" axis="1 0 0" damping="1"/>
+      <joint type="slide" axis="0 1 0" damping="1"/>
+      <geom pos="0 0 0.2" size="0.3 0.3 0.2" type="box"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/long_box.xml

@@ -0,0 +1,26 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <quality shadowsize="8192"/>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <asset>
+    <mesh name="long_box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
+          scale=".6 .03 .03"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1"/>
+    <geom type="box" size="1 1 .3" pos="0 0 -.3" rgba=".5 .5 .5 .5"/>
+    <body pos="0 0 .02" euler="0 0 40">
+      <freejoint/>
+      <geom type="mesh" mesh="long_box"/>
+      <!--  <geom type="box" size=".6 .03 .03"/> -->
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/multiple_objects.xml

@@ -0,0 +1,49 @@
+<mujoco>
+  <option noslip_iterations="2">
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <quality shadowsize="8192"/>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <default>
+    <geom solref=".006 1"/>
+  </default>
+
+  <asset>
+      <texture name="skybox" type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
+               width="256" height="256" mark="random" markrgb="1 1 1" random="0.003"/>
+    <mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
+          scale="1 1 .1"/>
+    <mesh name="boxoid" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 .5 -1 -1 2"
+          scale=".3 .2 .1"/>
+    <mesh name="pentaprism" vertex="1 0 0 0.309 0.951 0 -0.809 0.588 0 -0.809 -0.588 0 0.309 -0.951 0
+                                    1 0 1 0.309 0.951 1 -0.809 0.588 1 -0.809 -0.588 1 0.309 -0.951 1"
+          scale=".2 .2 .1"/>
+  </asset>
+  <worldbody>
+    <light pos="0 0 3"/>
+    <light pos="2 2 2" dir="-1 -1 -1"/>
+    <geom type="plane" pos="0 0 -.5" size="3 3 .01"/>
+    <geom type="mesh" mesh="box" pos="0 0 -.15" euler="3 7 30"/>
+    <body pos="-.3 -.3 .3">
+      <freejoint/>
+      <geom type="mesh" mesh="boxoid" rgba=".8 0 0 1" euler="3 5 -130"/>
+    </body>
+    <body pos=".3 .3 0.3">
+      <freejoint/>
+      <geom type="box" euler="3 5 -80" size=".3 .2 .1" rgba="0 .8 0 1"/>
+    </body>
+    <body pos=".3 .3 .6">
+      <freejoint/>
+      <geom type="mesh" mesh="pentaprism" rgba="0 0 .8 1"/>
+    </body>
+    <body pos=".6 -.3 .3">
+      <freejoint/>
+      <geom type="cylinder" size=".2 .05" rgba=".6 0 .6 1"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collision_convex/stacked_boxes.xml

@@ -0,0 +1,108 @@
+<mujoco>
+  <option gravity="-4 4 -10">
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <quality shadowsize="8192"/>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <size njmax="3000" nconmax="1000"/>
+
+  <asset>
+    <mesh name="box" vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"
+          scale=".05 .05 .05"/>
+  </asset>
+
+  <default>
+    <geom friction="0.4"/>
+    <default class="mesh_box">
+      <geom type="mesh" mesh="box"/>
+    </default>
+    <default class="primitive_box">
+      <geom type="box" size=".05 .05 .05" friction=".1"/>
+    </default>
+  </default>
+
+  <worldbody>
+    <light pos="1 -1 1" dir="-1 1 -1"/>
+    <geom type="box" size=".5 .5 .1" pos="-.4 .4 -.1" rgba=".5 .5 .8 1"/>
+    <geom type="box" size=".5 .1 .5" pos="-.4 .8 .4" rgba=".5 .5 .8 1"/>
+    <geom type="box" size=".1 .5 .5" pos="-.8 .4 .4" rgba=".5 .5 .8 1"/>
+    <body pos="-.4 .4 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .4 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .4 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.4 .5 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .5 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .5 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.4 .6 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .6 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .6 .1">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+
+    <body pos="-.4 .4 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .4 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .4 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.4 .5 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .5 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .5 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.4 .6 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.5 .6 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+    <body pos="-.6 .6 .2">
+      <freejoint/>
+      <geom class="mesh_box"/>
+    </body>
+  </worldbody>
+</mujoco>

test/engine/testdata/collisions.xml

@@ -0,0 +1,36 @@
+<mujoco model="collisions">
+  <worldbody>
+    <body name="box">
+      <geom name="box" type="box" size="1 1 1"/>
+    </body>
+    <body pos="1.2 1.2 0.0">
+      <!-- collides with box -->
+      <joint/>
+      <geom name="sphere_collides" type="sphere" size="1"/>
+    </body>
+    <body pos="-0.9 -0.9 0.0">
+      <!-- collides with box, and is a predefined pair -->
+      <joint/>
+      <geom name="sphere_predefined" type="sphere"  size="0.1"/>
+    </body>
+    <body pos="1.8 -1.8 0.0" >
+      <!-- doesn't collide with box, but requires narrowphase checking -->
+      <joint/>
+      <geom name="sphere_narrowphase" type="sphere" size="1"/>
+    </body>
+    <body pos="-2.1 -2.1 0.0">
+      <!-- doesn't collide with box, and can be eliminated in broadphase -->
+      <joint/>
+      <geom name="sphere_broadphase" type="sphere" size="1"/>
+    </body>
+    <body name="sphere_excluded">
+      <!-- collides with box, but is excluded -->
+      <joint/>
+      <geom name="sphere_excluded" type="sphere" pos="0.0 0.0 0.0" size="0.2"/>
+    </body>
+  </worldbody>
+  <contact>
+    <exclude body1="box" body2="sphere_excluded"/>
+    <pair geom1="box" geom2="sphere_predefined"/>
+  </contact>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/connect/force_free.xml

@@ -0,0 +1,43 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is held by body1 through the connect constraint, so the force sensor on body1
+  should register -2*gravity (for the weight of both bodies).
+  -->
+  <option gravity="1 2 3"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom density="1000" size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="1 2 3">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect body1="body1" body2="body2" anchor="0 0 0"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="-2 -4 -6"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/connect/force_slide.xml

@@ -0,0 +1,43 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is held by body1 through the connect constraint, so the force sensor on body1
+  should register 20 (for the weight of both bodies).
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="0 0 -0.1">
+      <joint type="slide" axis="0 0 1"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect body1="body1" body2="body2" anchor="0 0 0"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 0 2"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml

@@ -0,0 +1,43 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is held by body1 through the connect constraint, so the force sensor on body1
+  should register 20 (for the weight of both bodies), rotated into the y-z plane.
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1" euler="0 45 0">
+      <geom/>
+      <site name="sensor" euler="0 0 90"/>
+    </body>
+    <body name="body2" pos="0 0 -0.2">
+      <joint type="slide" axis="0 0 1"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect body1="body1" body2="body2" anchor="0 0 0"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 1.41421356237 1.41421356237"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml

@@ -0,0 +1,59 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  Same as force_free.xml, but with a distractor constraint.
+  -->
+  <option gravity="1 2 3"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body0" pos="-3 0 ">
+      <joint name="joint0" type="slide" axis="0 0 1"/>
+      <geom/>
+    </body>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="1 2 3">
+      <joint type="free"/>
+      <geom/>
+    </body>
+    <body name="body3" pos="3 0 0">
+      <geom/>
+      <body name="body3b" pos="3 0 -1">
+        <joint type="slide" axis="0 0 1" frictionloss="9"/>
+        <geom/>
+      </body>
+    </body>
+    <body name="body4" pos="4 0 0">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <joint joint1="joint0"/>
+    <weld body1="body3" body2="body4"/>
+    <connect body1="body1" body2="body2" anchor="0 0 0"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="-2 -4 -6"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/connect/torque_free.xml

@@ -0,0 +1,43 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is free, but because it's offset on y it wants to rotate about x due to gravity on z. The
+  connect prevents that, so the torque sensor should record the weight*offset of the second body.
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="0 2 0">
+      <freejoint/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect body1="body1" body2="body2" anchor="0 2 0"/>
+  </equality>
+
+  <sensor>
+    <torque site="sensor" user="2 0 0"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/weld/force_free.xml

@@ -0,0 +1,45 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is held by body1 through the weld constraint, so the force sensor on body1
+  should register -2*gravity (for the weight of both bodies).
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="0 0 -.1">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="body1" body2="body2"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 0 2"/>
+    <torque site="sensor" user="0 0 0"/>
+  </sensor>
+</mujoco>
+

test/engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml

@@ -0,0 +1,45 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  body2 is held by body1 through the weld constraint, so the force sensor on body1
+  should register -2*gravity (for the weight of both bodies).
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1" euler="0 45 0">
+      <geom/>
+      <site name="sensor" euler="0 0 90"/>
+    </body>
+    <body name="body2" pos="0 0 -.2" euler="30 30 30">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="body1" body2="body2"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 1.41421356237 1.41421356237"/>
+    <torque site="sensor" user="0 0 0"/>
+  </sensor>
+</mujoco>
+

test/engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml

@@ -0,0 +1,44 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  the force sensor on body1 should register 2N (for the weight of both bodies).
+  body2 is welded to body1 at an offset of 1m, so the torque sensor should register 1Nm.
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1">
+      <geom/>
+      <site name="sensor"/>
+    </body>
+    <body name="body2" pos="0 1 0">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="body1" body2="body2"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 0 2"/>
+    <torque site="sensor" user="1 0 0"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml

@@ -0,0 +1,44 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  The force sensor on body1 should register 2 (for the weight of both bodies).
+  body2 is welded to body1 at an offset of 1, so the torque sensor should register 1.
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1" euler="0 45 0">
+      <geom/>
+      <site name="sensor" euler="0 0 90"/>
+    </body>
+    <body name="body2" pos="0 1 0" euler="30 30 30">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="body1" body2="body2"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 1.41421356237 1.41421356237"/>
+    <torque site="sensor" user="0 -0.70710678118 0.70710678118"/>
+  </sensor>
+</mujoco>

test/engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated_tendon.xml

@@ -0,0 +1,44 @@
+<!-- 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.
+-->
+
+<mujoco>
+  <!--
+  the force sensor on body1 should register 2N (for the weight of both bodies).
+  body2 is welded to body1 at an offset of 1m, so the torque sensor should register 1Nm.
+  -->
+  <option gravity="0 0 -1"/>
+
+  <default>
+    <!-- A cube of size 0.1^3 with density of 1000 has mass of 1. -->
+    <geom size="0.05 0.05 0.05" type="box"/>
+  </default>
+
+  <worldbody>
+    <body name="body1" euler="0 45 0">
+      <geom/>
+      <site name="sensor" euler="0 0 90"/>
+    </body>
+    <body name="body2" pos="0 1 0" euler="30 30 30">
+      <joint type="free"/>
+      <geom/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="body1" body2="body2"/>
+  </equality>
+
+  <sensor>
+    <force site="sensor" user="0 1.41421356237 1.41421356237"/>
+    <torque site="sensor" user="0 -0.70710678118 0.70710678118"/>
+  </sensor>
+</mujoco>