chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ea230a950c4fcaefb22abcac3ff8b4c6685abec9
Mujoco_WASM/test/engine/engine_forward_test.cc
T
Alessio Quaglino ea230a950c Implicit flex elasticity in the CG constraint solver via an effective metric 
This CL replaces the post-hoc implicit flex correction (`flexInterp_cgsolve`) with a **linearly-implicit effective metric** `M̃ = M + (h² + h·damping)·K` carried by the CG constraint solver itself. Contact/friction forces and implicit flex elasticity are now computed against one consistent metric, instead of the solver seeing `M` and a post-solve correction changing `qacc` behind its back.

Gate (unchanged semantics): `solver="CG"` + implicit/implicitfast integrator + pyramidal cones + flex stiffness present. Newton and PGS are untouched. `solver="CG"` remains the user-facing contract — the factorization is an implementation detail of the preconditioner.

### What's in the metric

- **mjData `efm_*`** (arena, efc-like lifetime/skip semantics; built in `mj_fwdPosition`, value-refreshed in `mj_fwdVelocity`): the per-step stiffness CSR `efm_B_*`, its reverse-Cholesky factor `efm_dofid` + `efm_L_*` (nested-dissection ordered, separators-first for the reverse factorization), and the smooth-force shift `efm_c = h·K·qvel`.
- **`mjd_flexStiff_assemble`** now assembles stretch (Gauss–Newton), standard dim-2 bending, and — via the cached corotated stiffness `d->flexelem_krot` — interp stiffness (all node bodies on simple sliders: point Jacobian is I₃, `flex_centered` not required; fixed nodes drop like pins) into one dof-level CSR. `mjd_effMulAdd`/`mjd_effSolve` apply the metric, with matrix-free operator fallbacks where assembly does not apply.
- **mjModel `efm0_*`** (`nefm0dof`/`nefm0L`): the constant part of the metric factor — currently the dim-2 bending factor, computed once in `mj_setConst` — so bending-only models pay zero per-step factorization cost. Naming mirrors mjData's `efm_*` with the standard `0`-suffix (reference/constant) idiom, and is deliberately not bending-specific: future constant contributors extend it without renames.
- The solver consumes the metric through pre-shifted `qfrc_smooth` and the metric products `Ma`/`Mv`/`Mgrad`; `qacc_smooth` becomes the unconstrained minimizer of the implicit dynamics, which makes the no-constraint shortcut and the warmstart choice consistent by construction.
- **`mj_inverse` adds `B·qacc − c`**, making inverse dynamics discrete-consistent with the gated forward dynamics — exact, since the gated path has no qDeriv term (new test `ForwardTest.GatedFlexInverseConsistency`).

### Performance

All numbers: ms/step over the same 2000-step window, models as shipped on each side (old code with the old model settings vs this CL with the new ones).

The new solver path activates on exactly two shipped models — the ponchos, the only flex models that need an implicit integrator (poncho on Euler degenerates to >200 ms/step). For them, this CL trades speed for consistency: the implicit bending solve now runs inside every solver iteration, where the contact solve can see the stiffness, instead of once after the solve. Solver iterations drop because the curvature is visible, but each iteration pays for the implicit solve:

| model | before | after | solver iters/step |
|---|---|---|---|
| poncho | 2.47 | 3.30 (1.33×) | 16.8 → 11.8 |
| poncho_edgeequality | 1.96 | 2.72 (1.39×) | 13.2 → 10.0 |

What that price buys: contact forces consistent with the implicit elasticity (previously the post-hoc correction changed `qacc` after the constraint solve), discrete-consistent inverse dynamics, and the removal of the post-hoc special case from the integration path. Raising poncho's timestep from 2 to 5 ms leaves its per-step cost nearly flat, so the consistency price can be recovered by taking fewer steps where accuracy allows.

Every other flex model was measured stable on Euler at its shipped timestep and switches to it (these models predate the post-hoc integrator; implicit was never load-bearing for them). They end up equal or faster than before: bunny_multicell 0.47 → 0.40, trampoline 0.28 → 0.25, plate 1.02 → 0.99, pancake 0.34 → 0.33.

Finally, the per-step factorization makes configurations practical that the old code could only integrate explicitly: implicit stretch elasticity (`elastic2d="stretch"`/`"both"`, dim-3 solids) and factorized interp stiffness. No before/after exists for these — stock has no implicit treatment of stretch at all.

### Behavior changes

- With the post-hoc correction deleted, interp/bending models running `solver="Newton"` (or elliptic cones, or islands) now integrate flex elasticity **explicitly** (previously: post-hoc implicit). Affects e.g. `gripper_trilinear` (stable, and faster, but different semantics). Follow-up options: Newton-side metric support, or a documented fallback.
- With the gate on, `mj_forward` outputs are timestep-dependent for gated models (they answer the linearly-implicit discrete problem); `qacc_smooth` and `mj_inverse` change accordingly. Non-gated models are bit-identical (full suite green throughout).

### Validation

- 1737/1737 tests, including new: `FlexStretchDerivatives` (FD-validated GN operator), `FlexStiffAssemble`/`FlexStiffAssembleInterp` (CSR ≡ operators), `GatedFlexInverseConsistency` (fails pre-change), equivalence tests vs the old post-hoc treatment (bending matches to 2e-11).
- Fingerprint discipline throughout: bending-only models bit-exact across every refactor; permutation/kernel changes verified iteration-identical.

### Known follow-ups (not in this CL)

3×3-block sparse Cholesky kernel (the numeric factorization is index-bound; projected ~3× on the factor); mjModel persistence of the factor's symbolic pattern (rest-pose ND makes sizes compile-time); the general effective-metric mode (all solvers, all PSD-safe force classes, behind an enable flag).

PiperOrigin-RevId: 948561856
Change-Id: I8b8e32ebd0428042af71647d0470d10773bf6daf
2026-07-15 14:57:42 -07:00

3756 lines
112 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_forward.c.
#include "src/engine/engine_forward.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_callback.h"
#include "src/engine/engine_core_util.h"
#include "src/engine/engine_derivative.h"
#include "src/engine/engine_io.h"
#include "test/fixture.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#endif
namespace mujoco {
namespace {
static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
// helper for precision-aware checks in macros (e.g. MJDATA_POINTERS)
template <typename T>
void ExpectNear(T a, T b) {
EXPECT_EQ(a, b);
}
template <>
void ExpectNear<mjtNum>(mjtNum a, mjtNum b) {
EXPECT_EQ(a, b);
}
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kJointForceClamp =
"engine/testdata/actuation/joint_force_clamp.xml";
static const char* const kTendonForceClamp =
"engine/testdata/actuation/tendon_force_clamp.xml";
using ::testing::Pointwise;
using ::testing::_;
using ::testing::Gt;
using ::testing::HasSubstr;
using ::testing::Ne;
using ::testing::NotNull;
// --------------------------- activation limits -------------------------------
struct ActLimitedTestCase {
std::string test_name;
mjtIntegrator integrator;
};
class ParametrizedForwardTest
: public MujocoTest,
public ::testing::WithParamInterface<ActLimitedTestCase> {};
TEST_P(ParametrizedForwardTest, 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>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
model->opt.integrator = GetParam().integrator;
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.get(), data.get());
// always greater than lower bound
EXPECT_GT(data->act[0], -1);
// after 99 steps we hit the upper bound
if (i < 99) EXPECT_LT(data->act[0], 1);
if (i >= 99) EXPECT_NEAR(data->act[0], 1, MjTol(0, 5e-6));
}
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.get(), data.get());
// always smaller than upper bound
EXPECT_LT(data->act[0], model->actuator_actrange[1]);
// after 199 steps we hit the lower bound
if (i < 199) EXPECT_GT(data->act[0], model->actuator_actrange[0]);
if (i >= 199) {
EXPECT_NEAR(data->act[0], model->actuator_actrange[0], MjTol(0.0, 5e-6));
}
}
}
INSTANTIATE_TEST_SUITE_P(
ParametrizedForwardTest, ParametrizedForwardTest,
testing::ValuesIn<ActLimitedTestCase>({
{"Euler", mjINT_EULER},
{"Implicit", mjINT_IMPLICIT},
{"RK4", mjINT_RK4},
}),
[](const testing::TestParamInfo<ParametrizedForwardTest::ParamType>& info) {
return info.param.test_name;
});
// --------------------------- damping actuator --------------------------------
using ForwardTest = MujocoTest;
TEST_F(ForwardTest, DamperDampens) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt"/>
<damper joint="jnt" kv="1000" ctrlrange="0 100"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// move the joint
data->ctrl[0] = 100.0;
data->ctrl[1] = 0.0;
for (int i = 0; i < 100; i++) mj_step(model.get(), data.get());
// stop the joint with damping
data->ctrl[0] = 0.0;
data->ctrl[1] = 100.0;
for (int i = 0; i < 1000; i++) mj_step(model.get(), data.get());
EXPECT_LE(data->qvel[0], std::numeric_limits<double>::epsilon());
}
static const char* const kArmatureEquivalencePath =
"engine/testdata/armature_equivalence.xml";
// test that adding joint armature is equivalent to a coupled rotating mass with
// a gear ratio enforced by an equality
TEST_F(ForwardTest, ArmatureEquivalence) {
const std::string xml_path = GetTestDataFilePath(kArmatureEquivalencePath);
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// with actuators
mjtNum qpos_mse = 0;
int nstep = 0;
while (data->time < 4) {
data->ctrl[0] = data->ctrl[1] = mju_sin(2 * data->time);
mj_step(model, data);
nstep++;
mjtNum err = data->qpos[0] - data->qpos[2];
qpos_mse += err * err;
}
EXPECT_LT(mju_sqrt(qpos_mse / nstep), 1e-3);
// no actuators
model->opt.disableflags |= mjDSBL_ACTUATION;
qpos_mse = 0;
nstep = 0;
mj_resetData(model, data);
while (data->time < 4) {
mj_step(model, data);
nstep++;
mjtNum err = data->qpos[0] - data->qpos[2];
qpos_mse += err * err;
}
EXPECT_LT(mju_sqrt(qpos_mse / nstep), 1e-3);
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- implicit integrator -----------------------------
using ImplicitIntegratorTest = MujocoTest;
// Disabling implicit joint damping works as expected
TEST_F(ImplicitIntegratorTest, EulerDampDisable) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag eulerdamp="disable"/>
</option>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// step once, call mj_forward, save qvel and qacc
mj_step(model.get(), data.get());
mj_forward(model.get(), data.get());
std::vector<mjtNum> qvel = AsVector(data->qvel, model->nv);
std::vector<mjtNum> qacc = AsVector(data->qacc, model->nv);
// second step
mj_step(model.get(), data.get());
// compute finite-difference acceleration
std::vector<mjtNum> qacc_fd(model->nv);
for (int i = 0; i < model->nv; i++) {
qacc_fd[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep;
}
// expect finite-differenced qacc to match to high precision
EXPECT_THAT(qacc_fd, Pointwise(MjNear(1e-14, 1e-6), qacc));
// reach the same initial state
mj_resetData(model.get(), data.get());
mj_step(model.get(), data.get());
// second step again, but with implicit integration of joint damping
model->opt.disableflags &= ~mjDSBL_EULERDAMP;
mj_step(model.get(), data.get());
// compute finite-difference acceleration difference
std::vector<mjtNum> dqacc(model->nv);
for (int i = 0; i < model->nv; i++) {
dqacc[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep;
}
// expect finite-differenced qacc to not match
EXPECT_GT(mju_norm(dqacc.data(), model->nv), 1);
}
// Reducing timesteps reduces the difference between implicit/explicit
TEST_F(ImplicitIntegratorTest, EulerDampLimit) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjtNum diff_norm_prev = -1;
for (const mjtNum dt : {1e-2, 1e-3, 1e-4, 1e-5, 1e-6, 1e-7, 1e-8}) {
// set timestep
model->opt.timestep = dt;
// step twice with implicit damping, save qvel
model->opt.disableflags &= ~mjDSBL_EULERDAMP;
mj_resetData(model.get(), data.get());
mj_step(model.get(), data.get());
mj_step(model.get(), data.get());
std::vector<mjtNum> qvel_imp = AsVector(data->qvel, model->nv);
// step once, step again without implicit damping, save qvel
mj_resetData(model.get(), data.get());
mj_step(model.get(), data.get());
model->opt.disableflags |= mjDSBL_EULERDAMP;
mj_step(model.get(), data.get());
std::vector<mjtNum> qvel_exp = AsVector(data->qvel, model->nv);
mjtNum diff_norm = 0;
for (int i = 0; i < model->nv; i++) {
diff_norm += (qvel_imp[i] - qvel_exp[i]) * (qvel_imp[i] - qvel_exp[i]);
}
diff_norm = mju_sqrt(diff_norm);
if (diff_norm_prev != -1) {
EXPECT_LT(diff_norm, diff_norm_prev);
}
diff_norm_prev = diff_norm;
}
}
// Euler and implicit should be equivalent if there is only joint damping
TEST_F(ImplicitIntegratorTest, EulerImplicitEquivalent) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint axis="1 0 0" damping="2"/>
<geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
<body pos="0 .1 0">
<joint axis="0 1 0" damping="1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// step 10 times with Euler, save copy of qpos as vector
for (int i = 0; i < 10; i++) {
mj_step(model.get(), data.get());
}
std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
// reset, step 10 times with implicit
mj_resetData(model.get(), data.get());
model->opt.integrator = mjINT_IMPLICIT;
for (int i = 0; i < 10; i++) {
mj_step(model.get(), data.get());
}
// expect qpos vectors to be numerically different
#ifndef mjUSESINGLE
EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(Ne(), qposEuler));
#endif
// expect qpos vectors to be similar to high precision
EXPECT_THAT(AsVector(data->qpos, model->nq),
Pointwise(MjNear(1e-14, 1e-6), qposEuler));
}
// Joint and actuator damping should integrate identically under implicit
TEST_F(ImplicitIntegratorTest, JointActuatorEquivalent) {
const std::string xml_path = GetTestDataFilePath(kDampedActuatorsPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// take 1000 steps with Euler
for (int i = 0; i < 1000; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be significantly different
#ifndef mjUSESINGLE
EXPECT_GT(fabs(data->qpos[0] - data->qpos[2]), 1e-4);
EXPECT_GT(fabs(data->qpos[1] - data->qpos[3]), 1e-4);
#endif
// reset, take 10 steps with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i = 0; i < 10; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be insignificantly different
EXPECT_LT(fabs(data->qpos[0] - data->qpos[2]), MjTol(1e-16, 1e-6));
EXPECT_LT(fabs(data->qpos[1] - data->qpos[3]), MjTol(1e-16, 1e-6));
mj_deleteData(data);
mj_deleteModel(model);
}
// Energy conservation: RungeKutta > implicit > Euler
TEST_F(ImplicitIntegratorTest, EnergyConservation) {
const std::string xml_path =
GetTestDataFilePath(kEnergyConservingPendulumPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
const int nstep = 500; // number of steps to take
// take nstep steps with Euler, measure energy (potential + kinetic)
model->opt.integrator = mjINT_EULER;
for (int i = 0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyEuler = data->energy[0] + data->energy[1];
// take nstep steps with implicit, measure energy
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model, data);
for (int i = 0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyImplicit = data->energy[0] + data->energy[1];
// take nstep steps with 4th order Runge-Kutta, measure energy
model->opt.integrator = mjINT_RK4;
mj_resetData(model, data);
for (int i = 0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyRK4 = data->energy[0] + data->energy[1];
// energy was measured: expect all energies to be nonzero
EXPECT_NE(energyEuler, 0);
EXPECT_NE(energyImplicit, 0);
EXPECT_NE(energyRK4, 0);
// test conservation: perfectly conserved energy would remain 0.0
// expect RK4 to be better than implicit
EXPECT_LT(fabs(energyRK4), fabs(energyImplicit));
// expect implicit to be better than Euler
EXPECT_LT(fabs(energyImplicit), fabs(energyEuler));
mj_deleteData(data);
mj_deleteModel(model);
}
// free-body local solve: implicitfast matches implicit exactly for a standalone
// free body
TEST_F(ImplicitIntegratorTest, FreeBodyMatchesImplicit) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.005"/>
<worldbody>
<body pos="0.1 -0.2 0.5" euler="20 -30 40">
<joint type="free" damping="0.1"/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".04 -.02 .03" euler="10 20 30"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(model);
MjDataPtr d2 = MakeData(model);
mjModel* m = model.get();
// tumbling initial velocity
mj_resetData(m, d1.get());
d1->qvel[3] = 5;
d1->qvel[4] = -3;
d1->qvel[5] = 2;
// step both integrators from identical states, re-synchronizing each step
// to avoid chaotic divergence of tumbling trajectories
int nstate = mj_stateSize(m, mjSTATE_INTEGRATION);
std::vector<mjtNum> state(nstate);
mjtNum tol = MjTol(1e-14, 1e-6);
for (int i = 0; i < 50; i++) {
mj_getState(m, d1.get(), state.data(), mjSTATE_INTEGRATION);
mj_setState(m, d2.get(), state.data(), mjSTATE_INTEGRATION);
m->opt.integrator = mjINT_IMPLICITFAST;
mj_step(m, d1.get());
m->opt.integrator = mjINT_IMPLICIT;
mj_step(m, d2.get());
for (int k = 0; k < m->nv; k++) {
EXPECT_NEAR(d1->qvel[k], d2->qvel[k], tol)
<< "step " << i << " dof " << k;
}
}
}
// free-body local solve: spinning free bodies do not gain energy in vacuum
TEST_F(ImplicitIntegratorTest, FreeBodyGyroStable) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.005">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
// middle-axis tumble and fast principal-axis spin
static constexpr mjtNum qvel0[2][3] = {{0.05, 5, 0.05}, {20, 0.05, 0.05}};
for (int c = 0; c < 2; c++) {
SCOPED_TRACE(testing::Message() << "velocity case " << c);
mj_resetData(m, d);
mju_copy3(d->qvel + 3, qvel0[c]);
mj_forward(m, d);
mjtNum initial_energy = d->energy[1];
// 100 simulated seconds
for (int i = 0; i < 20000; i++) {
mj_step(m, d);
ASSERT_LT(d->energy[1], 1.01 * initial_energy)
<< "energy gain at step " << i;
}
}
}
// free-body local solve: applies to bodies in contact
TEST_F(ImplicitIntegratorTest, FreeBodyGyroStableContact) {
// spinning ellipsoid on an inclined plane, as in gyroscopic.xml
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.002"/>
<worldbody>
<geom type="plane" size="5 5 .1" euler="0 15 0"/>
<body pos="0 0 .2">
<freejoint/>
<geom type="ellipsoid" size=".05 .1 .15" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
mj_resetData(m, d);
d->qvel[3] = 30;
mjtNum initial_speed = mju_norm(d->qvel, m->nv);
int ncon_total = 0;
for (int i = 0; i < 5000; i++) {
mj_step(m, d);
ncon_total += d->ncon;
ASSERT_LT(mju_norm(d->qvel, m->nv), 2 * initial_speed)
<< "speed gain at step " << i;
}
// the body was in contact while spinning
EXPECT_GT(ncon_total, 1000);
}
// free-body local solve: energy of a tumbling free body never increases and is
// only mildly damped; angular momentum drift is bounded
TEST_F(ImplicitIntegratorTest, FreeBodyConservation) {
// aligned: CoM at joint origin
static constexpr char xml1[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
// non-aligned: CoM offset from joint origin
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast" timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1" euler="10 20 30" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
int xml_idx = 1;
for (auto xml : {xml1, xml2}) {
SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++);
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
mj_resetData(m, d);
d->qvel[3] = 1.0;
d->qvel[4] = 2.0;
d->qvel[5] = 3.0;
mj_forward(m, d);
mjtNum initial_energy = d->energy[1];
mjtNum initial_angmom[3];
mj_subtreeVel(m, d);
mju_copy3(initial_angmom, d->subtree_angmom);
for (int i = 0; i < 500; i++) {
mj_step(m, d);
// energy never increases (small tolerance for rounding)
ASSERT_LT(d->energy[1], initial_energy * (1 + MjTol(1e-9, 1e-4)))
<< "energy gain at step " << i;
}
// implicit damping of tumbling is mild: measured E_end/E0 = 0.93
EXPECT_GT(d->energy[1], 0.7 * initial_energy);
// angular momentum drift is bounded: measured 5e-3
mj_subtreeVel(m, d);
mjtNum angmom_err[3];
mju_sub3(angmom_err, d->subtree_angmom, initial_angmom);
EXPECT_LT(mju_norm3(angmom_err), 0.05);
}
}
// gyroscopic instability: Euler gains energy where implicitfast does not
TEST_F(ImplicitIntegratorTest, FreeBodyEulerGainsImplicitfastDissipates) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01">
<flag energy="enable" gravity="disable"/>
</option>
<worldbody>
<body>
<freejoint/>
<geom type="box" size=".1 .2 .3" mass="1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
mjtNum energy_end[2];
for (int integrator : {mjINT_EULER, mjINT_IMPLICITFAST}) {
m->opt.integrator = integrator;
mj_resetData(m, d);
d->qvel[3] = 1.0;
d->qvel[4] = 2.0;
d->qvel[5] = 3.0;
mj_forward(m, d);
mjtNum initial_energy = d->energy[1];
for (int i = 0; i < 500; i++) {
mj_step(m, d);
}
energy_end[integrator == mjINT_IMPLICITFAST] =
d->energy[1] / initial_energy;
}
// Euler gains energy (measured: 1.09), implicitfast does not
EXPECT_GT(energy_end[0], 1.01);
EXPECT_LT(energy_end[1], 1.0);
}
// the invdiscrete flag has no effect on forward dynamics
TEST_F(ImplicitIntegratorTest, InvdiscreteForwardNoop) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.005"/>
<worldbody>
<geom type="plane" size="2 2 .1"/>
<body pos="0 0 .3">
<joint type="free" damping="0.1"/>
<geom type="box" size=".1 .2 .3" mass="2" pos=".03 .02 .01"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(model);
MjDataPtr d2 = MakeData(model);
mjModel* m = model.get();
for (int integrator : {mjINT_IMPLICITFAST, mjINT_IMPLICIT}) {
m->opt.integrator = integrator;
mj_resetData(m, d1.get());
d1->qvel[3] = 5;
d1->qvel[5] = 2;
mj_resetData(m, d2.get());
d2->qvel[3] = 5;
d2->qvel[5] = 2;
for (int i = 0; i < 200; i++) {
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
mj_step(m, d1.get());
m->opt.enableflags |= mjENBL_INVDISCRETE;
mj_step(m, d2.get());
}
m->opt.enableflags &= ~mjENBL_INVDISCRETE;
// trajectories are bit-identical
for (int k = 0; k < m->nq; k++) {
EXPECT_EQ(d1->qpos[k], d2->qpos[k]) << "qpos " << k;
}
for (int k = 0; k < m->nv; k++) {
EXPECT_EQ(d1->qvel[k], d2->qvel[k]) << "qvel " << k;
}
}
}
// model with degenerate translational inertia
TEST_F(ForwardTest, DegenerateInertia) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" cone="elliptic">
<flag gravity="disable" diagexact="enable"/>
</option>
<worldbody>
<body name="1" pos="0.05 0.3 0">
<joint name="1" axis="0 1 0"/>
<geom type="capsule" size="0.1 0.5"/>
</body>
<body name="2">
<joint name="2" axis="1 0 0" stiffness="1" springref="90"/>
<geom type="capsule" size="0.1 0.5"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
for (int i = 0; i < 1000; i++) {
mj_step(model.get(), data.get());
EXPECT_EQ(data->warning[mjWARN_BADQACC].number, 0)
<< "divergence at timestep " << i;
if (data->warning[mjWARN_BADQACC].number != 0) {
break;
}
}
}
TEST_F(ForwardTest, ControlClamping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="slide" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<motor name="unclamped" joint="slide"/>
<motor name="clamped" joint="slide" ctrllimited="true" ctrlrange="-1 1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// for the unclamped actuator, ctrl={1, 2} produce different accelerations
data->ctrl[0] = 1;
mj_forward(model.get(), data.get());
mjtNum qacc1 = data->qacc[0];
data->ctrl[0] = 2;
mj_forward(model.get(), data.get());
mjtNum qacc2 = data->qacc[0];
EXPECT_NE(qacc1, qacc2);
// for the clamped actuator, ctrl={1, 2} produce identical accelerations
data->ctrl[1] = 1;
mj_forward(model.get(), data.get());
qacc1 = data->qacc[0];
data->ctrl[1] = 2;
mj_forward(model.get(), data.get());
qacc2 = data->qacc[0];
EXPECT_EQ(qacc1, qacc2);
// data->ctrl[1] remains pristine
EXPECT_EQ(data->ctrl[1], 2);
MockWarningHandler warning_handler;
// for the unclamped actuator, huge raises warning
warning_handler.ExpectWarnings(
"Nan, Inf or huge value in CTRL at ACTUATOR 0");
data->ctrl[0] = 10 * mjMAXVAL;
mj_forward(model.get(), data.get());
testing::Mock::VerifyAndClearExpectations(&warning_handler);
// for the clamped actuator, huge does not raise warning
EXPECT_CALL(warning_handler, Warn(_)).Times(0);
mj_resetData(model.get(), data.get());
data->ctrl[1] = 10 * mjMAXVAL;
mj_forward(model.get(), data.get());
testing::Mock::VerifyAndClearExpectations(&warning_handler);
// for the clamped actuator, NaN raises warning
warning_handler.ExpectWarnings(
"Nan, Inf or huge value in CTRL at ACTUATOR 1");
mj_resetData(model.get(), data.get());
data->ctrl[1] = std::numeric_limits<double>::quiet_NaN();
mj_forward(model.get(), data.get());
}
void control_callback(const mjModel* m, mjData* d) { d->ctrl[0] = 2; }
TEST_F(ForwardTest, MjcbControlDisabled) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<motor joint="hinge"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// install global control callback
mjcb_control = control_callback;
// call forward
mj_forward(model.get(), data.get());
// expect that callback was used
EXPECT_EQ(data->ctrl[0], 2.0);
// reset, disable actuation, call forward
mj_resetData(model.get(), data.get());
model->opt.disableflags |= mjDSBL_ACTUATION;
mj_forward(model.get(), data.get());
// expect that callback was not used
EXPECT_EQ(data->ctrl[0], 0.0);
// remove global control callback
mjcb_control = nullptr;
}
TEST_F(ForwardTest, gravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -10" />
<worldbody>
<body>
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
<body pos="3 0 0" gravcomp="1">
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
<body pos="6 0 0" gravcomp="2">
<joint type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
while (data->time < 1) {
mj_step(model.get(), data.get());
}
mjtNum dist = 0.5 * mju_norm3(model->opt.gravity) * (data->time * data->time);
// expect that body 1 moved down, allowing some slack from our estimate
EXPECT_NEAR(data->qpos[0], -dist, 0.011);
// expect that body 2 does not move
EXPECT_EQ(data->qpos[1], 0.0);
// expect that body 3 moves up the same distance that body 0 moved down
EXPECT_EQ(data->qpos[0], -data->qpos[2]);
}
// test disabling of equality constraints
TEST_F(ForwardTest, eq_active) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="vertical" type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
<equality>
<joint joint1="vertical"/>
</equality>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// simulate for 1 second
while (data->time < 1) {
mj_step(model.get(), data.get());
}
// expect that the body has barely moved
EXPECT_LT(mju_abs(data->qpos[0]), 0.001);
// turn the equality off, simulate for another second
data->eq_active[0] = 0;
while (data->time < 2) {
mj_step(model.get(), data.get());
}
// expect that the body has fallen about 5m
EXPECT_LT(data->qpos[0], -4.5);
EXPECT_GT(data->qpos[0], -5.5);
// turn the equality back on, simulate for another second
data->eq_active[0] = 1;
while (data->time < 3) {
mj_step(model.get(), data.get());
}
// expect that the body has snapped back
EXPECT_LT(mju_abs(data->qpos[0]), 0.001);
}
// test that normalized and denormalized quats give the same result
TEST_F(ForwardTest, NormalizeQuats) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Skipping in float32: exact mjData comparison infeasible.";
#endif
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicit">
<flag warmstart="disable" energy="enable"/>
</option>
<worldbody>
<body name="free">
<freejoint/>
<geom size="1" pos=".1 .2 .3"/>
</body>
<body pos="3 0 0">
<joint name="ball" type="ball" stiffness="100" range="0 10"/>
<geom size="1" pos=".1 .2 .3"/>
</body>
</worldbody>
<sensor>
<ballquat joint="ball"/>
<framequat objtype="body" objname="free"/>
</sensor>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data_u = MakeData(model);
// we'll compare all the memory, so unpoison it first
#ifdef MEMORY_SANITIZER
__msan_unpoison(data_u->buffer, data_u->nbuffer);
__msan_unpoison(data_u->arena, data_u->narena);
#endif
// set quats to denormalized values, non-zero velocities
for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i;
for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1 * i;
// copy data and normalize quats
mjData* data_n = mj_copyData(nullptr, model.get(), data_u.get());
mj_normalizeQuat(model.get(), data_n->qpos);
// call forward, expect quats to be untouched
mj_forward(model.get(), data_u.get());
for (int i = 3; i < model->nq; i++) {
EXPECT_EQ(data_u->qpos[i], (mjtNum)i);
}
// expect that the ball joint limit is active
EXPECT_EQ(data_u->nl, 1);
// step both models
mj_step(model.get(), data_u.get());
mj_step(model.get(), data_n);
// expect everything to match
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
ExpectNear(data_n->name[i * nc + j], data_u->name[i * nc + j]);
MJDATA_POINTERS;
#undef X
// repeat the above with RK4 integrator
model->opt.integrator = mjINT_RK4;
// reset data, unpoison
mj_resetData(model.get(), data_u.get());
#ifdef MEMORY_SANITIZER
__msan_unpoison(data_u->buffer, data_u->nbuffer);
__msan_unpoison(data_u->arena, data_u->narena);
#endif
// set quats to un-normalized values, non-zero velocities
for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i;
for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1 * i;
// copy data and normalize quats
mj_copyData(data_n, model.get(), data_u.get());
mj_normalizeQuat(model.get(), data_n->qpos);
// step both models
mj_step(model.get(), data_u.get());
mj_step(model.get(), data_n);
// expect everything to match
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
ExpectNear(data_n->name[i * nc + j], data_u->name[i * nc + j]);
MJDATA_POINTERS;
#undef X
mj_deleteData(data_n);
}
// test that normalized and denormalized quats give the same result
TEST_F(ForwardTest, MocapQuats) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="mocap" mocap="true" quat="1 1 1 1">
<geom size="1"/>
</body>
</worldbody>
<sensor>
<framequat objtype="body" objname="mocap"/>
</sensor>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
// expect mocap_quat to be normalized (by the compiler)
for (int i = 0; i < 4; i++) {
EXPECT_NEAR(data->mocap_quat[i], 0.5, MjTol(0, 1e-6));
EXPECT_NEAR(data->xquat[4 + i], 0.5, MjTol(0, 1e-6));
}
// write denormalized quats to mocap_quat, call forward again
for (int i = 0; i < 4; i++) {
data->mocap_quat[i] = 1;
}
mj_forward(model.get(), data.get());
// expect mocap_quat to remain denormalized, but xquat to be normalized
for (int i = 0; i < 4; i++) {
EXPECT_NEAR(data->mocap_quat[i], 1, MjTol(0, 1e-6));
EXPECT_NEAR(data->xquat[4 + i], 0.5, MjTol(0, 1e-6));
}
}
// user defined 2nd-order activation dynamics: frequency-controlled oscillator
// note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but
// since we have a vector output we write into act_dot directly
mjtNum oscillator(const mjModel* m, const mjData* d, int id) {
// check that actnum == 2
if (m->actuator_actnum[id] != 2) {
mju_error("callback expected actnum == 2");
}
// get pointers to activations (inputs) and their derivatives (outputs)
mjtNum* act = d->act + m->actuator_actadr[id];
mjtNum* act_dot = d->act_dot + m->actuator_actadr[id];
// harmonic oscillator with controlled frequency
mjtNum frequency = 2 * mjPI * d->ctrl[id];
act_dot[0] = -act[1] * frequency;
act_dot[1] = act[0] * frequency;
return 0; // ignored by caller
}
TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="1e-4"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="user" actdim="2"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// install global dynamics callback
mjcb_act_dyn = oscillator;
// for two arbitrary frequencies, compare actuator force as output by the
// user-defined oscillator and analytical sine function
for (mjtNum frequency : {1.5, 0.7}) {
mj_resetData(model.get(), data.get());
data->ctrl[0] = frequency; // set desired oscillation frequency
data->act[0] = 1; // initialise activation
// simulate and compare to sine function
while (data->time < 1) {
mjtNum expected_force = mju_sin(2 * mjPI * data->time * frequency);
mj_step(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], expected_force, .01);
}
}
// uninstall global dynamics callback
mjcb_act_dyn = nullptr;
}
// ------------------------------ actuators -----------------------------------
using ActuatorTest = MujocoTest;
TEST_F(ActuatorTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
<worldbody>
<body name="static">
<!-- small increase to size to ensure contact -->
<geom size=".02001" pos=" .01 .01 .07"/>
<geom size=".02001" pos="-.01 .01 .07"/>
<geom size=".02001" pos=" .01 -.01 .07"/>
<geom size=".02001" pos="-.01 -.01 .07"/>
</body>
<body name="free">
<freejoint/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="static" ctrlrange="0 2"/>
<adhesion body="free" ctrlrange="0 2"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// iterate over cone type
for (mjtCone cone : {mjCONE_ELLIPTIC, mjCONE_PYRAMIDAL}) {
// set cone
model->opt.cone = cone;
// iterate over condim
for (int condim : {1, 3, 4, 6}) {
// set condim
for (int id = 0; id < model->ngeom; id++) {
model->geom_condim[id] = condim;
}
// iterate over actuators
for (int id = 0; id < 2; id++) {
// set ctrl > 1, expect free body to not fall
mj_resetData(model.get(), data.get());
data->ctrl[id] = 1.01;
for (int i = 0; i < 100; i++) {
mj_step(model.get(), data.get());
}
// moved down at most 10 microns
EXPECT_GT(data->qpos[2], -1e-5);
// set ctrl < 1, expect free body to fall below 1cm
mj_resetData(model.get(), data.get());
data->ctrl[id] = 0.99;
for (int i = 0; i < 100; i++) {
mj_step(model.get(), data.get());
}
// fell lower than 1cm
EXPECT_LT(data->qpos[2], -0.01);
}
}
}
}
// Actuator force clamping at joints
TEST_F(ActuatorTest, ActuatorForceClamping) {
const std::string xml_path = GetTestDataFilePath(kJointForceClamp);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
data->ctrl[0] = 10;
mj_forward(model, data);
// expect clamping as specified in the model
EXPECT_NEAR(data->actuator_force[0], 1, MjTol(0, 1e-6));
EXPECT_NEAR(data->qfrc_actuator[0], 0.4, MjTol(0, 1e-6));
// simulate for 2 seconds to gain velocity
while (data->time < 2) {
mj_step(model, data);
}
// activate damper, expect force to be clamped at lower bound
data->ctrl[1] = 1;
mj_forward(model, data);
EXPECT_NEAR(data->qfrc_actuator[0], -0.4, MjTol(0, 1e-6));
mj_deleteData(data);
mj_deleteModel(model);
}
// Apply gravity compensation via actuators
TEST_F(ActuatorTest, ActuatorGravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1">
<joint name="joint" type="slide" axis="0 0 1"
actuatorfrcrange="-2 2" actuatorgravcomp="true"/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor name="actuator" joint="joint"/>
</actuator>
<sensor>
<actuatorfrc actuator="actuator"/>
<jointactuatorfrc joint="joint"/>
</sensor>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
// expect force clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 2);
// reduce gravity so gravcomp is not clamped
model->opt.gravity[2] = -1;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 1);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 1);
// add control, see that it adds up
data->ctrl[0] = 0.5;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->actuator_force[0], 0.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0.5);
EXPECT_EQ(data->sensordata[1], 1.5);
// add larger control, expect clamping
data->ctrl[0] = 1.5;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 2);
// disable actgravcomp, expect gravcomp as a passive force
model->jnt_actgravcomp[0] = 0;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 1);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 1.5);
}
// Check that dampratio works as expected
TEST_F(ActuatorTest, DampRatio) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="slide1" axis="1 0 0" type="slide"/>
<geom size=".05"/>
</body>
<body pos="0 0 -.15">
<joint name="slide2" axis="1 0 0" type="slide"/>
<geom size=".05"/>
</body>
</worldbody>
<actuator>
<position name="slightly underdamped" joint="slide1" kp="10" dampratio="0.99"/>
<position name="slightly overdamped" joint="slide2" kp="10" dampratio="1.01"/>
</actuator>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
data->qpos[0] = data->qpos[1] = -0.1;
mjtNum under_damped = data->qpos[0];
mjtNum over_damped = data->qpos[1];
while (data->time < 10) {
mj_step(model.get(), data.get());
under_damped = mju_max(under_damped, data->qpos[0]);
over_damped = mju_max(over_damped, data->qpos[1]);
}
// expect slightly underdamped to slightly overshoot
EXPECT_GT(under_damped, 0);
EXPECT_LT(under_damped, 1e-6);
// expect slightly overdamped to slightly undershoot
EXPECT_LT(over_damped, 0);
EXPECT_GT(over_damped, -1e-6);
}
// Check dampratio for actuators with nontrivial transmission
TEST_F(ActuatorTest, DampRatioTendon) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/actuation/tendon_dampratio.xml");
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
data->ctrl[0] = 1;
data->ctrl[1] = 4;
while (data->time < 1) {
mj_step(model, data);
}
// expect first and second fingers to move together
double tol = 1e-10;
EXPECT_THAT(AsVector(data->qpos, 4),
Pointwise(MjNear(tol, tol), AsVector(data->qpos + 4, 4)));
EXPECT_THAT(AsVector(data->qvel, 4),
Pointwise(MjNear(tol, tol), AsVector(data->qvel + 4, 4)));
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- DC motor actuators ----------------------------------
using DCMotorTest = MujocoTest;
TEST_F(DCMotorTest, IntVelocityEquivalence) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<body pos="0 0 0">
<joint name="slide1" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
<body pos="0 1 0">
<joint name="slide2" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<!--
Equivalence mapping:
intvelocity force: F = kp * \int(ctrl - v) - kv * v
dcmotor force: F = (V*K - K^2*v) / R
where V = ki * \int(ctrl - v) (since kp=0, kd=0)
Setting K=1, R=0.2, ki=2 yields:
F = (2 * \int(ctrl - v) - v) / 0.2
= 10 * \int(ctrl - v) - 5 * v
This perfectly matches intvelocity with kp=10, kv=5.
-->
<intvelocity name="intvel" joint="slide1" kp="10" kv="5" actrange="-0.01 0.01"/>
<dcmotor name="dcmotor" joint="slide2" motorconst="1" resistance="0.2" input="velocity" controller="0 2 0 0 0.01"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Apply a time-varying velocity command
while (data->time < 1.0) {
data->ctrl[0] = mju_sin(20 * data->time);
data->ctrl[1] = mju_sin(20 * data->time);
mj_step(model.get(), data.get());
// Both actuators should integrate identical states
EXPECT_MJTNUM_EQ(data->act[0], data->act[1]);
// Both bodies should move identically
EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-7));
EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-7));
EXPECT_NEAR(data->qacc[0], data->qacc[1], MjTol(1e-14, 1e-6));
// Both actuators should produce identical force
EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1],
MjTol(1e-14, 1e-6));
}
}
TEST_F(DCMotorTest, StatelessSteadyState) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double K = 0.05;
double R = 2.0;
double V = 12.0;
double omega = 3.0;
data->ctrl[0] = V;
data->qvel[0] = omega;
mj_forward(model.get(), data.get());
double expected_force = K / R * (V - K * omega);
EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-12, 1e-5));
EXPECT_EQ(model->actuator_actnum[0], 0);
}
TEST_F(DCMotorTest, CurrentFilterConverges) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.0001"/>
<worldbody>
<body>
<joint name="joint" damping="1000"/>
<geom size="1" mass="100"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->actuator_actnum[0], 1);
double K = 0.05;
double R = 2.0;
double V = 12.0;
data->ctrl[0] = V;
for (int i = 0; i < 10000; i++) {
mj_step(model.get(), data.get());
}
double omega = data->qvel[0];
double i_ss = V / R - K / R * omega;
double expected_force = K * i_ss;
EXPECT_NEAR(data->act[0], i_ss, MjTol(1e-6, 1e-4));
EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-6, 1e-4));
}
TEST_F(DCMotorTest, CurrentFilterExactIntegration) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double R = 2.0;
double te = 0.01 / R;
double V = 12.0;
data->ctrl[0] = V;
mj_step(model.get(), data.get());
double h = model->opt.timestep;
double exact_current = V / R * (1 - mju_exp(-h / te));
EXPECT_NEAR(data->act[0], exact_current, MjTol(1e-10, 1e-4));
double euler_current = V / R * h / te;
EXPECT_GT(std::abs(data->act[0] - euler_current),
std::abs(data->act[0] - exact_current));
}
TEST_F(DCMotorTest, CoggingTorque) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
cogging="0.1 6 0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double A = 0.1, Np = 6, phi = 0;
double K = 0.05, R = 2.0;
double V = 5.0;
double pos = 1.0;
data->ctrl[0] = V;
data->qpos[0] = pos;
mj_forward(model.get(), data.get());
double electrical_force = K / R * V;
double cogging = A * mju_sin(Np * pos + phi);
EXPECT_NEAR(data->actuator_force[0], electrical_force + cogging,
MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, CoggingBypassesSaturation) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
saturation="0.001 0" cogging="0.1 6 0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double A = 0.1, Np = 6, phi = 0;
double pos = 1.0;
data->ctrl[0] = 100.0;
data->qpos[0] = pos;
mj_forward(model.get(), data.get());
double cogging = A * mju_sin(Np * pos + phi);
EXPECT_NEAR(model->actuator_forcerange[1], 0.001, MjTol(1e-12, 1e-5));
EXPECT_GT(mju_abs(data->actuator_force[0]), 0.001);
EXPECT_NEAR(data->actuator_force[0], 0.001 + cogging, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, LuGreViscousFriction) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->actuator_actnum[0], 1);
double sigma1 = 1, sigma2 = 0.01;
double K = 0.05, R = 2.0;
double omega = 2.0;
data->ctrl[0] = 0;
data->qvel[0] = omega;
mj_forward(model.get(), data.get());
EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2);
double electrical_force = K / R * (0 - K * omega);
double z = data->act[model->actuator_actadr[0]];
double z_dot = data->act_dot[model->actuator_actadr[0]];
double lugre_force = 100 * z + sigma1 * z_dot;
EXPECT_NEAR(data->actuator_force[0], electrical_force - lugre_force,
MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, ThermalRiseAndFall) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="10 5 0 0 25 25"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int adr = model->actuator_actadr[0];
ASSERT_EQ(model->actuator_actnum[0], 1);
EXPECT_EQ(data->act[adr], 0);
double R = 2.0, V = 10.0;
double RT = 10.0, C = 5.0;
double h = model->opt.timestep;
double P = V * V / R;
data->ctrl[0] = V;
mj_step(model.get(), data.get());
double dT1 = h * P / C;
EXPECT_NEAR(data->act[adr], dT1, MjTol(1e-11, 1e-4));
mj_step(model.get(), data.get());
double dT2 = dT1 + h * (P - dT1 / RT) / C;
EXPECT_NEAR(data->act[adr], dT2, MjTol(1e-11, 1e-4));
data->ctrl[0] = 0;
mj_step(model.get(), data.get());
double dT3 = dT2 + h * (0 - dT2 / RT) / C;
EXPECT_NEAR(data->act[adr], dT3, MjTol(1e-11, 1e-4));
EXPECT_LT(data->act[adr], dT2);
}
TEST_F(DCMotorTest, ThermalSteadyState) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="0.1 0.1 0 0 25 25"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double R = 2.0, V = 10.0;
double RT = 0.1;
double dT_ss = RT * V * V / R;
data->ctrl[0] = V;
for (int i = 0; i < 10000; i++) {
mj_step(model.get(), data.get());
}
int adr = model->actuator_actadr[0];
EXPECT_NEAR(data->act[adr], dT_ss, 1e-4);
}
TEST_F(DCMotorTest, ThermalAffectsForce) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="0.1 0.1 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
double K = 0.05, R = 2.0, V = 10.0;
double alpha = 0.004;
int adr = model->actuator_actadr[0];
data->ctrl[0] = V;
data->act[adr] = 0;
mj_forward(model.get(), data.get());
double force_cold = data->actuator_force[0];
EXPECT_NEAR(force_cold, K / R * V, MjTol(1e-12, 1e-5));
double dT = 50;
data->act[adr] = dT;
mj_forward(model.get(), data.get());
double R_hot = R * (1 + alpha * dT);
double force_hot = data->actuator_force[0];
EXPECT_NEAR(force_hot, K / R_hot * V, MjTol(1e-12, 1e-5));
EXPECT_LT(force_hot, force_cold);
}
// Temperature slot must be correctly offset past slew and integral states.
TEST_F(DCMotorTest, ThermalAffectsForceWithController) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
input="position" controller="1.0 1.0 0 5.0 0"
thermal="0.1 0.1 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// slot order: slew(0), integral(1), temperature(2)
ASSERT_EQ(model->actuator_actnum[0], 3);
int adr = model->actuator_actadr[0];
int temp_adr = adr + 2; // temperature is slot 2
double K = 0.05, R = 2.0, alpha = 0.004;
double dT = 50;
data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied
data->act[adr + 1] = 0.0; // integral state x_I = 0
data->act[temp_adr] = dT; // temperature rise above ambient
data->ctrl[0] = 1.0; // position setpoint = 1.0, qpos = 0, error = 1.0
mj_forward(model.get(), data.get());
// u_eff = ctrl = 1.0 (no slew applied since act[slew] == ctrl)
// V = kp*(u_eff - length) + ki*x_I - kd*omega = 1.0*1.0 + 1.0*0.0 - 0*0 = 1.0
// R(T) = 2.0 * (1 + 0.004 * 50) = 2.4
// stateless (no te): force = K/R(T) * V = 0.05/2.4 * 1.0
double R_hot = R * (1 + alpha * dT);
EXPECT_NEAR(data->actuator_force[0], K / R_hot * 1.0, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, StatelessPositionMode) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0 0.5 0 0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Position target 5.0, current pos 0.0, current vel 0.0
data->ctrl[0] = 5.0;
mj_forward(model.get(), data.get());
// V = Kp * (u - theta) = 2.0 * 5.0 = 10.0
// force = K / R * V + bias = (0.05 / 2.0) * 10.0 + 0 = 0.25
EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5));
// Velocity penalty
data->qvel[0] = 2.0;
mj_forward(model.get(), data.get());
// V = 10.0 - Kd * omega = 10.0 - (0.5 * 2.0) = 9.0
// bias = - K^2 / R * omega = -0.0025 / 2.0 * 2.0 = -0.0025
// force = K / R * V + bias = 0.225 - 0.0025 = 0.2225
EXPECT_NEAR(data->actuator_force[0], 0.2225, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, StatelessVelocityMode) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 0 0 0 0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Velocity target 4.0, current vel 1.0
data->ctrl[0] = 4.0;
data->qvel[0] = 1.0;
mj_forward(model.get(), data.get());
// V = Kp * (u - omega) = 3.0 * (4.0 - 1.0) = 9.0
// bias = - K^2 / R * omega = -0.0025 / 2.0 * 1.0 = -0.00125
// force = K / R * V + bias = (0.05 / 2.0) * 9.0 - 0.00125 = 0.22375
EXPECT_NEAR(data->actuator_force[0], 0.22375, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, StatefulPositionMode) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Controller states: 1 for slew, 1 for ki -> actnum = 2
ASSERT_EQ(model->actuator_actnum[0], 2);
int adr = model->actuator_actadr[0];
// Current states
double u_prev = 1.0;
double x_I = 2.0;
data->act[adr] = u_prev;
data->act[adr + 1] = x_I;
// target 5.0 position, current 0.0
data->ctrl[0] = 5.0;
data->qvel[0] = 0.5;
mj_forward(model.get(), data.get());
// slew bounding: s = 10.0, dt = 0.001. max_change = 0.01
// Target = 5.0. It is upper bounded by u_prev + 0.01 = 1.01
EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5));
// PI error: error = u_eff - length = 1.01 - 0.0 = 1.01
EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5));
// V = Kp(u_eff - length) + Ki * x_I - Kd * omega
// V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97
// bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 0.5 = -0.000625
// force = K/R * V + bias = 0.025 * 2.97 - 0.000625 = 0.073625
EXPECT_NEAR(data->actuator_force[0], 0.073625, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0" inductance="1.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Controller states: slew (0), ki (1), current (2). actnum = 3
ASSERT_EQ(model->actuator_actnum[0], 3);
int adr = model->actuator_actadr[0];
double u_prev = 1.0;
double x_I = 2.0;
double current = 0.5;
data->act[adr] = u_prev;
data->act[adr + 1] = x_I;
data->act[adr + 2] = current;
// Target 5.0 position, velocity 0.5
data->ctrl[0] = 5.0;
data->qvel[0] = 0.5;
mj_forward(model.get(), data.get());
// Slew bounding: max_change = 0.01, u_eff = 1.01
EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5));
// PI error: error = u_eff - length = 1.01
EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5));
// Voltage computation:
// V = Kp(u_eff - length) + Ki * x_I - Kd * omega
// V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97
// Current filter:
// t_e = L / R = 1.0 / 2.0 = 0.5
// di/dt = (V/R - K/R * omega - i) / t_e
// di/dt = (2.97/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5
// di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945
EXPECT_NEAR(data->act_dot[adr + 2], 1.945, MjTol(1e-12, 1e-5));
// Force is K * next_activation (actearly is always on for DC motors)
// Inline mj_nextActivation for te = 0.5
mjtNum te = 0.5;
mjtNum h = model->opt.timestep;
mjtNum next_i = 0.5 + data->act_dot[adr + 2] * te * (1 - mju_exp(-h / te));
EXPECT_NEAR(data->actuator_force[0], 0.05 * next_i, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, StatefulVelocityMode) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 1.0 0 0 2.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Controller states: 1 for ki (no slew)
ASSERT_EQ(model->actuator_actnum[0], 1);
int adr = model->actuator_actadr[0];
double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0)
data->act[adr] = x_I;
// target vel 4.0, current vel 1.0
data->ctrl[0] = 4.0;
data->qvel[0] = 1.0;
mj_forward(model.get(), data.get());
// integrate command directly: error = target = 4.0
// since x_I == Imax (2.0) and error (4.0) > 0, act_dot should be clamped to 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
// V = Kp * (u_eff - omega) + Ki * (x_I - length)
// V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 0.0) = 9.0 + 2.0 = 11.0
// bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 1.0 = -0.00125
// force = K/R * V + bias = 0.025 * 11.0 - 0.00125 = 0.275 - 0.00125 = 0.27375
EXPECT_NEAR(data->actuator_force[0], 0.27375, MjTol(1e-12, 1e-5));
// repeat with non-zero joint position
data->qpos[0] = 1.5;
mj_forward(model.get(), data.get());
// V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 1.5) = 9.0 + 0.5 = 9.5
// force = K/R * V + bias = 0.025 * 9.5 - 0.00125 = 0.2375 - 0.00125 = 0.23625
EXPECT_NEAR(data->actuator_force[0], 0.23625, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, CurrentPlusThermal) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0" thermal="10 5 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->actuator_actnum[0], 2);
int adr = model->actuator_actadr[0];
double K = 0.05, R = 2.0, V = 12.0;
double te = 0.01 / R;
double RT = 10.0, C = 5.0;
double current = 3.0;
double dT = 10.0;
data->act[adr] = dT;
data->act[adr + 1] = current;
data->ctrl[0] = V;
mj_forward(model.get(), data.get());
// Force uses next_activation (actearly is always on for DC motors)
// Inline mj_nextActivation for te = 0.01 / R = 0.005
mjtNum h = model->opt.timestep;
mjtNum next_i =
current + data->act_dot[adr + 1] * te * (1 - mju_exp(-h / te));
EXPECT_NEAR(data->actuator_force[0], K * next_i, MjTol(1e-12, 1e-5));
double R_hot = R * (1 + 0.004 * dT);
double T_dot = (R_hot * current * current - dT / RT) / C;
EXPECT_NEAR(data->act_dot[adr], T_dot, MjTol(1e-10, 1e-4));
double omega = data->qvel[0];
double i_dot = (V / R_hot - K / R_hot * omega - current) / te;
EXPECT_NEAR(data->act_dot[adr + 1], i_dot, MjTol(1e-10, 1e-3));
}
TEST_F(DCMotorTest, CurrentRateLimit) {
// Verifies that saturation:current_rate clamps di/dt.
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0" saturation="0 0 100"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->actuator_actnum[0], 1);
int adr = model->actuator_actadr[0];
double V = 12.0;
double dimax = 100.0; // A/s rate limit
// unclamped: i_dot = (V/R - 0 - 0) / te = 6 / 0.005 = 1200 A/s >> dimax
data->act[adr] = 0; // current = 0
data->ctrl[0] = V;
mj_forward(model.get(), data.get());
// i_dot should be clipped to +dimax
EXPECT_NEAR(data->act_dot[adr], dimax, MjTol(1e-12, 1e-5));
// reverse: large negative drive
data->ctrl[0] = -V;
mj_forward(model.get(), data.get());
// i_dot should be clipped to -dimax
EXPECT_NEAR(data->act_dot[adr], -dimax, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, VoltageLimit) {
// verifies that saturation:voltage clamps voltage
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
input="position" controller="1 0 0 0 0 10.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Vmax = 10.0, ctrl = 20.0
// force = K/R * Vmax = 0.05 / 2.0 * 10.0 = 0.25
data->ctrl[0] = 20.0;
mj_forward(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5));
// negative drive
data->ctrl[0] = -20.0;
mj_forward(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], -0.25, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, IntegralClamp) {
// verifies that controller Imax clamps integral state
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0 0 5.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Imax = 5.0
ASSERT_EQ(model->actuator_actnum[0], 1); // only ki is stateful
int adr = model->actuator_actadr[0];
// set integral state to Imax
data->act[adr] = 5.0;
// set target to generate positive error (ctrl - length)
data->ctrl[0] = 1.0; // target
data->qpos[0] = 0.0; // length = 0
mj_forward(model.get(), data.get());
// act_dot should be clamped to 0 because act >= Imax and error > 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
// set target to generate negative error
data->ctrl[0] = -1.0;
mj_forward(model.get(), data.get());
// act_dot should be negative (not clamped)
EXPECT_NEAR(data->act_dot[adr], -1.0, MjTol(1e-12, 1e-5));
// set integral state to -Imax
data->act[adr] = -5.0;
// set target to generate negative error
data->ctrl[0] = -1.0;
data->qpos[0] = 0.0;
mj_forward(model.get(), data.get());
// act_dot should be clamped to 0 because act <= -Imax and error < 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, LuGreExactIntegration) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1" mass="1e6"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->actuator_actnum[0], 1);
int adr = model->actuator_actadr[0];
double sigma0 = 100, F_C = 0.5, F_S = 0.7, v_S = 10;
double z0 = 0.002;
double v = 0.5;
double h = model->opt.timestep;
data->act[adr] = z0;
data->qvel[0] = v;
double ratio = v / v_S;
double g_v = F_C + (F_S - F_C) * mju_exp(-ratio * ratio);
double a = -sigma0 * std::abs(v) / g_v;
double exp_ah = mju_exp(a * h);
double int_h = (exp_ah - 1) / a;
double z_new = exp_ah * z0 + int_h * v;
mj_step(model.get(), data.get());
EXPECT_NEAR(data->act[adr], z_new, MjTol(1e-12, 1e-5));
}
TEST_F(DCMotorTest, LuGreSteadyState) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1" mass="1e6"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int adr = model->actuator_actadr[0];
double sigma0 = 100, sigma2 = 0.01;
double F_C = 0.5, F_S = 0.7, v_S = 10;
double K = 0.05, R = 2.0;
double v = 0.5;
data->qvel[0] = v;
data->ctrl[0] = 0;
for (int i = 0; i < 10000; i++) {
mj_step(model.get(), data.get());
}
double ratio = v / v_S;
double g_v = F_C + (F_S - F_C) * mju_exp(-ratio * ratio);
double z_ss = g_v / sigma0;
EXPECT_NEAR(data->act[adr], z_ss, 1e-4);
EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2);
double back_emf = K * K / R * data->qvel[0];
double lugre_ss = g_v;
EXPECT_NEAR(data->actuator_force[0], -back_emf - lugre_ss, 1e-3);
}
TEST_F(DCMotorTest, LuGreBristleSpring) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int adr = model->actuator_actadr[0];
double sigma0 = 100;
double X = 0.01;
data->act[adr] = X;
data->ctrl[0] = 0;
mj_forward(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], -sigma0 * X, MjTol(1e-12, 1e-5));
}
// ----------------------- filterexact actuators -------------------------------
using FilterExactTest = MujocoTest;
TEST_F(FilterExactTest, ApproximatesContinuousTime) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filter" gainprm="1.1" />
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
const mjtNum kSimulationTime = 1.0;
// compute act with a small timestep to approximate continuous integration
model->opt.timestep = 0.001;
mj_resetData(model.get(), data.get());
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model.get(), data.get());
}
mjtNum continuous_act = data->act[0];
// compute again with a larger timestep, introducing integration error
model->opt.timestep = 0.01;
mj_resetData(model.get(), data.get());
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model.get(), data.get());
}
mjtNum discrete_act = data->act[0];
// compute a third time with exact integration
model->actuator_dyntype[0] = mjDYN_FILTEREXACT;
mj_resetData(model.get(), data.get());
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model.get(), data.get());
}
mjtNum exactfilter_act = data->act[0];
// expect exact integration to be closer to the small-timestep result
EXPECT_THAT(std::abs(continuous_act - discrete_act),
Gt(5 * std::abs(continuous_act - exactfilter_act)))
<< "Using filterexact should make the error at least 5 times smaller";
}
TEST_F(FilterExactTest, TimestepIndependent) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filterexact" dynprm="0.9" gainprm="1.1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
const mjtNum kSimulationTime = 1.0;
// first, compute act based on a small timestep and exact integration
model->opt.timestep = 0.01;
mj_resetData(model.get(), data.get());
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model.get(), data.get());
}
mjtNum small_timestep_act = data->act[0];
// now change the timestep to a much larger timestep
model->opt.timestep = 0.1;
mj_resetData(model.get(), data.get());
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model.get(), data.get());
}
mjtNum large_timestep_act = data->act[0];
EXPECT_NEAR(small_timestep_act, large_timestep_act, MjTol(1e-14, 1e-6))
<< "exact integration should be independent of timestep to machine "
"precision.";
}
TEST_F(FilterExactTest, ActEqualsCtrlWhenTauIsZero) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filterexact" dynprm="0" gainprm="1.1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
data->ctrl[0] = 0.5;
data->act[0] = 0.0;
mj_step(model.get(), data.get());
EXPECT_EQ(data->act[0], data->ctrl[0]);
}
// ----------------------- actearly actuator attribute -------------------------
using ActEarlyTest = MujocoTest;
TEST_F(ActEarlyTest, RemovesOneStepDelay) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/actuation/actearly.xml");
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_EQ(model->nu % 2, 0) << "number of actuators should be even";
ASSERT_EQ(model->nu, model->na) << "all actuators should be stateful";
ASSERT_EQ(model->nq, model->nu);
EXPECT_GT(model->nu, 0);
// actuators are ordered in pairs with actearly=true and actearly=false
for (int i = 0; i < model->na / 2; i++) {
EXPECT_TRUE(model->actuator_actearly[2 * i]);
EXPECT_FALSE(model->actuator_actearly[2 * i + 1]);
}
mjData* data = mj_makeData(model);
// set all controls to the same value and make one step
mju_fill(data->ctrl, 0.5, model->nu);
mj_step(model, data);
for (int i = 0; i < model->na / 2; i++) {
EXPECT_EQ(data->act[2 * i], data->act[2 * i + 1])
<< "act should be the same after first step for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * i);
EXPECT_EQ(data->act_dot[2 * i], data->act_dot[2 * i + 1])
<< "act_dot should be the same after first step for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * i);
}
for (int i = 0; i < 100; i++) {
std::vector<mjtNum> last_qfrc(data->qfrc_actuator,
data->qfrc_actuator + model->nu);
mj_step(model, data);
for (int j = 0; j < model->nu / 2; j++) {
// this is true for torque actuators
EXPECT_NEAR(last_qfrc[2 * j], data->qfrc_actuator[2 * j + 1],
MjTol(1e-3, 1e-1))
<< "there should be a 1 step delay between qfrc for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * j);
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(ActEarlyTest, DoesntChangeStateInMjForward) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/actuation/actearly.xml");
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// set all controls to the same value and make one step
mju_fill(data->ctrl, 0.5, model->nu);
mj_forward(model, data);
for (int i = 0; i < model->na; i++) {
EXPECT_EQ(data->act[i], 0) << "act should not change with mj_forward."
<< mj_id2name(model, mjOBJ_ACTUATOR, i);
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(ActuatorTest, DisableActuator) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" gear="2" group="0"/>
<position joint="slide" kp="1" group="1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
data->ctrl[0] = 1.0;
data->ctrl[1] = 1.0;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->qfrc_actuator[0], 3.0);
model->opt.disableactuator = 1 << 0;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->qfrc_actuator[0], 1.0);
model->opt.disableactuator = 1 << 1;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->qfrc_actuator[0], 2.0);
}
TEST_F(ActuatorTest, DisableActuatorOutOfRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" gear="-1" group="-1"/>
<motor joint="slide" gear="5" group="0"/>
<motor joint="slide" gear="31" group="31"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
data->ctrl[0] = 1.0;
data->ctrl[1] = 1.0;
data->ctrl[2] = 1.0;
// all actuators active
mj_forward(model.get(), data.get());
EXPECT_EQ(data->qfrc_actuator[0], 35.0);
// set all bits of disableactuator, only group 1 is disabled
model->opt.disableactuator = ~0;
mj_forward(model.get(), data.get());
EXPECT_EQ(data->qfrc_actuator[0], 30.0);
}
TEST_F(ActuatorTest, TendonActuatorForceRange) {
const std::string xml_path = GetTestDataFilePath(kTendonForceClamp);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
EXPECT_EQ(model->tendon_actfrclimited[0], 0);
EXPECT_EQ(model->tendon_actfrcrange[0], 0);
EXPECT_EQ(model->tendon_actfrcrange[1], 0);
EXPECT_EQ(model->tendon_actfrclimited[1], 1);
EXPECT_EQ(model->tendon_actfrcrange[2], -1);
EXPECT_EQ(model->tendon_actfrcrange[3], 1);
EXPECT_EQ(model->tendon_actfrclimited[2], 1);
EXPECT_EQ(model->tendon_actfrcrange[4], -10);
EXPECT_EQ(model->tendon_actfrcrange[5], 10);
EXPECT_EQ(model->tendon_actfrclimited[3], 1);
EXPECT_EQ(model->tendon_actfrcrange[6], 0);
EXPECT_EQ(model->tendon_actfrcrange[7], 1);
data->ctrl[0] = 1;
data->ctrl[1] = 1;
data->ctrl[2] = 1;
data->ctrl[3] = -1;
data->ctrl[4] = 1;
data->ctrl[5] = -20;
data->ctrl[6] = 5;
data->ctrl[7] = -5;
mj_forward(model, data);
EXPECT_NEAR(data->actuator_force[0], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[1], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[2], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[3], -1, 1e-6);
EXPECT_NEAR(data->actuator_force[4], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[5], -10, 1e-6);
EXPECT_NEAR(data->actuator_force[6], 5, 1e-6);
EXPECT_NEAR(data->actuator_force[7], -5, 1e-6);
EXPECT_EQ(data->sensordata[0], 3);
EXPECT_EQ(data->sensordata[1], 0);
EXPECT_EQ(data->sensordata[2], -10);
EXPECT_EQ(data->sensordata[3], 0);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------------- actuator delays -------------------------------
TEST_F(ForwardTest, ActuatorDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="2"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// delay = 0.02 seconds, timestep = 0.01, so ndelay = ceil(0.02/0.01) = 2
EXPECT_EQ(model->actuator_history[0], 2);
// set ctrl to a nonzero value
data->ctrl[0] = 10.0;
// step once: the new ctrl is appended but won't be read for 2 timesteps
mj_step(model.get(), data.get());
// actuator_force should still be 0 (delayed value from buffer init)
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10);
// step again
mj_step(model.get(), data.get());
// still reading old values
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10);
// step a third time - now the delayed ctrl should arrive
mj_step(model.get(), data.get());
// actuator_force should now be 10.0
EXPECT_NEAR(data->actuator_force[0], 10.0, 1e-10);
}
// Test actuator delay with linear interpolation (interp=1)
// Uses delay = 1.5*timestep so interpolation is meaningful
TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.015" nsample="3" interp="linear"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear)
EXPECT_EQ(model->actuator_history[0], 3);
EXPECT_EQ(model->actuator_history[1], 1); // interp=1 (linear)
EXPECT_NEAR(model->actuator_delay[0], 0.015, MjTol(1e-10, 5e-6));
// Set increasing ctrl values
// Buffer has samples at times: -0.02, -0.01, 0 with values 0, 0, 0
// After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0,
// 0, ctrl[0] Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01
// and t=0 Since both values are 0, expected actuator_force = 0
data->ctrl[0] = 10.0;
mj_step(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], 0.0, MjTol(1e-10, 5e-6)) << "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->ctrl[0] = 20.0;
mj_step(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], 5.0, MjTol(1e-10, 5e-6)) << "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->ctrl[0] = 30.0;
mj_step(model.get(), data.get());
EXPECT_NEAR(data->actuator_force[0], 15.0, MjTol(1e-10, 5e-6)) << "step 2";
}
TEST_F(ForwardTest, FlexTrilinearInstability) {
// model parameters matches user's trilinear.xml
constexpr char xml[] = R"(
<mujoco model="stability_test">
<option gravity="0 0 -9.81" iterations="100" solver="CG" tolerance="1e-10"
timestep="0.002" integrator="implicitfast">
<flag warmstart="disable" island="disable"/>
</option>
<worldbody>
<geom name="floor" size="0 0 .05" type="plane" condim="3"/>
<flexcomp name="bed" type="grid" count="17 17 3" spacing="0.05 0.05 0.05"
pos="0 0 0.05" radius="0.0005" dim="3" mass="10" dof="trilinear">
<contact condim="3" solref="0.005 1" solimp=".99 .99 .001" selfcollide="none"/>
<elasticity young="865067.00" poisson="0.1" damping="1"/>
</flexcomp>
<body name="box" pos="0.05 0.05 0.5">
<freejoint/>
<geom name="box_geom" type="box" size="0.04 0.04 0.04" mass="0.5"
solref="0.001 1" solimp="0.99 0.99 0.01"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// flex stiffness sign checks
// verify correct sign of flex stiffness derivatives before simulation
int nv = model->nv;
mjtNum h = model->opt.timestep;
// create a test vector
std::vector<mjtNum> v(nv), Mv(nv), flex_Kv(nv);
for (int i = 0; i < nv; i++) v[i] = mju_Halton(i, 2) - 0.5;
mjtNum vnorm = mju_norm(v.data(), nv);
for (int i = 0; i < nv; i++) v[i] /= vnorm;
mj_forward(model.get(), data.get());
// compute M*v and stiffness contributions
mj_mulM(model.get(), data.get(), Mv.data(), v.data());
// note: we use mjd_flexInterp_mulK here (unscaled by h^2) to check raw
// stiffness logic similar to what we expect in the solver now
mjtNum* v_copy = (mjtNum*)mju_malloc(nv * sizeof(mjtNum));
mju_copy(v_copy, v.data(), nv);
mju_zero(flex_Kv.data(), nv);
// using mulKD for legacy check consistency, but we know it applies h^2+h*d
// scaling; actually, let's stick to the high-level property checks from
// FlexStiffnessSign which used mulKD
mjd_flexInterp_mul(model.get(), data.get(), flex_Kv.data(), v.data(), h * h,
h, NULL);
// compute v^T*M*v and v^T*scale*K*v
mjtNum vMv = mju_dot(v.data(), Mv.data(), nv);
// mulKD returns -scale*K*v, so -flex_Kv = +scale*K*v
mjtNum vKv = -mju_dot(v.data(), flex_Kv.data(), nv);
// assertions from FlexStiffnessSign
EXPECT_GT(vKv, 0) << "Stiffness contribution should be positive";
EXPECT_GT(vMv + vKv, vMv) << "Full Hessian should exceed M alone";
mju_free(v_copy);
// stability simulation
// run for steps to catch instability
for (int i = 0; i < 2000; ++i) {
mj_step(model.get(), data.get());
for (int j = 0; j < model->nq; ++j) {
if (mju_abs(data->qpos[j]) > 1000.0) {
ADD_FAILURE() << "Instability detected at step " << i << " dof " << j
<< " val " << data->qpos[j];
return; // Exit early
}
}
}
}
// Verify that flex damping does not affect rigid body motion
TEST_F(ForwardTest, FlexDampingRigidMotion) {
constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.01" integrator="implicitfast" solver="CG"/>
<worldbody>
<flexcomp name="flex" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
pos="0 0 0" euler="45 45 45" radius="0.01" dim="3" mass="1" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1e5" poisson="0.3" damping="10"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Set initial rigid rotation velocity about Z axis
// Center of mass is roughly at 0 0 0 because pos="0 0 0" and symmetric grid.
// v = w x r. Let w = (1, 1, 1).
mjtNum w[3] = {10.0, 10.0, 10.0};
for (int i = 0; i < model->nv / 3; ++i) {
int qpos_adr = model->jnt_qposadr[i];
int qvel_adr = model->jnt_dofadr[i];
mjtNum* pos = data->qpos + qpos_adr;
mjtNum* vel = data->qvel + qvel_adr;
mjtNum r[3] = {pos[0], pos[1], pos[2]};
mju_cross(vel, w, r);
}
mj_forward(model.get(), data.get());
mjtNum initial_energy = data->energy[0] + data->energy[1];
// Run a few steps
for (int i = 0; i < 10; ++i) {
mj_step(model.get(), data.get());
}
mj_forward(model.get(), data.get());
mjtNum final_energy = data->energy[0] + data->energy[1];
// Expect energy conservation.
// With the bug, damping force acts on rigid rotation, dissipating energy.
EXPECT_NEAR(final_energy, initial_energy, 1e-6 * initial_energy)
<< "Energy decayed significantly (" << initial_energy << " -> "
<< final_energy << ")";
}
// verify that implicit integrator respects parent-flex coupling
TEST_F(ForwardTest, FlexParentCoupling) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit" timestep="0.01" solver="CG"/>
<worldbody>
<body name="parent" pos="0 0 0">
<freejoint/>
<geom size=".1" mass="0.1"/>
<flexcomp name="flex" type="grid" count="3 3 3" cellcount="1 1 1" spacing="1 1 1"
radius=".01" dim="3" mass="100" dof="trilinear" pos="1 1 1">
<contact selfcollide="none"/>
<elasticity young="1e4" poisson="0.3" damping="50"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// set state: parent moving, flex deformed
// this ensures both H_fp (coupling) and qacc_parent are non-trivial
// Run with Euler (timestep 1e-6)
model->opt.timestep = 1e-6;
model->opt.integrator = mjINT_EULER;
mj_resetData(model.get(), data.get());
data->qvel[0] = 1.0;
data->qpos[7] += 0.01;
data->qfrc_applied[0] = 10000.0; // Apply large force to parent
mj_step(model.get(), data.get()); // Step integrates
std::vector<mjtNum> qvel_euler(model->nv);
mju_copy(qvel_euler.data(), data->qvel, model->nv);
// Run with Implicit (timestep 1e-6)
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model.get(), data.get());
data->qvel[0] = 1.0;
data->qpos[7] += 0.01;
data->qfrc_applied[0] = 10000.0;
mj_step(model.get(), data.get()); // Step integrates
std::vector<mjtNum> qvel_implicit(model->nv);
mju_copy(qvel_implicit.data(), data->qvel, model->nv);
// Check agreement
double max_diff = 0;
for (int i = 0; i < model->nv; ++i) {
double diff = mju_abs(qvel_euler[i] - qvel_implicit[i]);
if (diff > max_diff) max_diff = diff;
}
// tolerance rebaselined 2e-5 -> 5e-5 for the in-solver implicit flex treatment: implicit
// and explicit flex damping legitimately differ at O(h*damping*K/M) in this comparison, and
// the in-solver form lands at ~3e-5 where the old post-hoc operator landed just under 2e-5
EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2))
<< "Implicit integrator should match Euler at small timestep";
}
TEST_F(ForwardTest, TrilinearPinnedParentWithFreejoint) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" solver="CG"/>
<worldbody>
<body>
<joint type="free"/>
<geom type="box" size="0.13 0.18 0.036" pos="0 0 0.036"/>
<body name="parent">
<flexcomp name="test" type="grid"
count="3 3 3" spacing=".1 .02 .1" radius="0.001"
pos="0 0 0.1" dof="trilinear" xyaxes="0 1 0 0 0 1" mass="10" dim="3">
<contact selfcollide="none"/>
<elasticity young="1e5" poisson="0.3" damping="0.1"/>
<pin id="0 2 4 6"/>
</flexcomp>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
int parent_id = mj_name2id(m.get(), mjOBJ_BODY, "parent");
ASSERT_GT(parent_id, 0);
EXPECT_EQ(m->nflexnode, 8);
EXPECT_EQ(m->body_dofnum[parent_id], 0) << "parent body should have 0 DOFs";
int freejoint_body = m->body_parentid[parent_id];
EXPECT_EQ(m->body_dofnum[freejoint_body], 6) << "freejoint body has 6 DOFs";
mj_resetData(m.get(), d.get());
mj_forward(m.get(), d.get());
for (int i = 0; i < 500; i++) {
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation became unstable at step " << i;
ASSERT_FALSE(mju_isBad(d->qvel[0]))
<< "Velocity became unstable at step " << i;
for (int j = 0; j < m->nq; j++) {
ASSERT_LT(mju_abs(d->qpos[j]), 100.0)
<< "Position exploded at step " << i << ", qpos[" << j
<< "]=" << d->qpos[j];
}
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i << ", qvel[" << j
<< "]=" << d->qvel[j];
}
}
}
// -------------------- actuator damping and armature --------------------------
using ActuatorDampingTest = MujocoTest;
TEST_F(ActuatorDampingTest, SingleActuatorJointDamping) {
// actuator damping=3 with gear=2 should produce same force as
// joint damping=12 (3*2^2=12)
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="2" damping="3"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_joint[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"
damping="12"/>
<geom size="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_forward(m1.get(), d1.get());
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
mj_forward(m2.get(), d2.get());
EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]);
}
TEST_F(ActuatorDampingTest, SingleActuatorTendonDamping) {
// actuator damping through tendon transmission
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<tendon>
<fixed name="ten">
<joint joint="jnt" coef="1"/>
</fixed>
</tendon>
<actuator>
<motor tendon="ten" gear="2" damping="3"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_tendon[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<tendon>
<fixed name="ten" damping="12">
<joint joint="jnt" coef="1"/>
</fixed>
</tendon>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_tendon, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_forward(m1.get(), d1.get());
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
mj_forward(m2.get(), d2.get());
EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]);
}
TEST_F(ActuatorDampingTest, SingleActuatorArmature) {
// actuator armature=0.5 with gear=3 should equal
// joint armature=4.5 (0.5*3^2=4.5)
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="3" armature="0.5"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_joint[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"
armature="4.5"/>
<geom size="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_forward(m1.get(), d1.get());
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
mj_forward(m2.get(), d2.get());
EXPECT_EQ(d1->qacc[0], d2->qacc[0]);
}
TEST_F(ActuatorDampingTest, MultipleActuatorsAccumulate) {
// two actuators: damping=2 gear=3, damping=1 gear=4
// equivalent joint damping: 2*9 + 1*16 = 34
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="3" damping="2"/>
<motor joint="jnt" gear="4" damping="1"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_joint[] = R"(
<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"
damping="34"/>
<geom size="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_forward(m1.get(), d1.get());
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
mj_forward(m2.get(), d2.get());
EXPECT_EQ(d1->qfrc_passive[0], d2->qfrc_passive[0]);
}
TEST_F(ActuatorDampingTest, DampingSimulationEquivalence) {
// actuator damping=5 gear=2 should match joint damping=20 over time
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="2" damping="5"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_joint[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="0 0 1"
damping="20"/>
<geom size="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
for (int i = 0; i < 100; i++) {
mj_step(m1.get(), d1.get());
mj_step(m2.get(), d2.get());
}
EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]);
EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]);
}
TEST_F(ActuatorDampingTest, ArmatureSimulationEquivalence) {
// actuator armature=2 gear=3 should match joint armature=18 over time
static constexpr char xml_actuator[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="0 0 1"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="3" armature="2"/>
</actuator>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
static constexpr char xml_joint[] = R"(
<mujoco>
<option gravity="0 0 -10"/>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="0 0 1"
armature="18"/>
<geom size="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml_actuator, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
MjModelPtr m2 = LoadModelFromString(xml_joint, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
mj_resetDataKeyframe(m1.get(), d1.get(), 0);
mj_resetDataKeyframe(m2.get(), d2.get(), 0);
for (int i = 0; i < 100; i++) {
mj_step(m1.get(), d1.get());
mj_step(m2.get(), d2.get());
}
EXPECT_MJTNUM_EQ(d1->qpos[0], d2->qpos[0]);
EXPECT_MJTNUM_EQ(d1->qvel[0], d2->qvel[0]);
}
TEST_F(ActuatorDampingTest, UtilityFunctionValues) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="5" damping="7" armature="3"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
mjtNum poly[mjNPOLY] = {0};
EXPECT_EQ(mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly), 175);
EXPECT_EQ(mj_actuatorArmature(m.get(), mjOBJ_JOINT, 0), 75);
}
TEST_F(ActuatorDampingTest, NonlinearDamping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt" gear="3" damping="2 0.5 0.1"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
// linear damping: 2 * gear^2 = 18
mjtNum poly0[mjNPOLY] = {0};
EXPECT_EQ(mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly0), 18);
// poly coefficients scaled by gear^2
mjtNum poly[mjNPOLY] = {0};
mj_actuatorDamping(m.get(), mjOBJ_JOINT, 0, poly);
EXPECT_MJTNUM_EQ(poly[0], 0.5 * 9); // 4.5
EXPECT_MJTNUM_EQ(poly[1], 0.1 * 9); // 0.9
}
TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) {
// Single model with two parallel bodies: one using kv, one using damping.
// Both produce the same joint-space damping force:
// kv: qfrc_actuator contribution = -kv * gear^2 * qvel
// damping: qfrc_passive contribution = -damping * gear^2 * qvel
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" integrator="implicitfast"/>
<worldbody>
<body name="kv_body">
<joint name="jnt_kv" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
<body name="damp_body" pos="5 0 0">
<joint name="jnt_damp" type="slide" axis="1 0 0"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="jnt_kv" kp="0" kv="5" gear="3"/>
<position joint="jnt_damp" kp="0" damping="5" gear="3"/>
</actuator>
<keyframe>
<key qvel="1 1"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// check forces at initial state
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m.get(), d.get());
// kv force arrives via qfrc_actuator, damping via qfrc_passive
mjtNum frc_kv = d->qfrc_actuator[0];
mjtNum frc_damp = d->qfrc_passive[1];
EXPECT_NEAR(frc_kv, frc_damp, MjTol(1e-12, 1e-5));
// expected force = -5 * 3^2 * 1 = -45
EXPECT_NEAR(frc_damp, -45, MjTol(1e-12, 1e-5));
// simulate and check trajectory equivalence
mj_resetDataKeyframe(m.get(), d.get(), 0);
for (int i = 0; i < 100; i++) {
mj_step(m.get(), d.get());
}
EXPECT_NEAR(d->qpos[0], d->qpos[1], MjTol(1e-12, 1e-5))
<< "position trajectory mismatch";
EXPECT_NEAR(d->qvel[0], d->qvel[1], MjTol(1e-12, 1e-5))
<< "velocity trajectory mismatch";
}
// flex sheet dropping on a plane should not gain energy from implicit bending
TEST_F(ImplicitIntegratorTest, FlexContactEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -10" timestep="0.001" integrator="implicitfast"
solver="CG" tolerance="1e-6">
<flag energy="enable"/>
</option>
<default>
<geom solref="0.003 1"/>
</default>
<worldbody>
<geom type="plane" size="5 5 0.1"/>
<flexcomp type="grid" count="8 8 1" spacing=".04 .04 .04"
radius=".01" name="sheet" dim="2" pos="0 0 0.02" mass="0.1">
<edge equality="true" damping="0.1"/>
<elasticity young="3e6" poisson="0" thickness="2e-2"
elastic2d="bend" damping="0"/>
<contact solref="0.003 1" internal="false" selfcollide="none"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
ASSERT_EQ(m->nflex, 1);
MjDataPtr d = MakeData(m);
// compute initial energy
mj_forward(m.get(), d.get());
mjtNum initial_energy = d->energy[0] + d->energy[1];
ASSERT_GT(initial_energy, 0);
// simulate
mjtNum max_energy = initial_energy;
int max_energy_step = 0;
int nsteps = 500;
for (int i = 0; i < nsteps; i++) {
mj_step(m.get(), d.get());
mjtNum total_energy = d->energy[0] + d->energy[1];
if (total_energy > max_energy) {
max_energy = total_energy;
max_energy_step = i + 1;
}
}
mjtNum energy_ratio = max_energy / initial_energy;
EXPECT_LE(energy_ratio, 1.01)
<< "contact solver injected energy: max_energy/initial_energy = "
<< energy_ratio << " (max at step " << max_energy_step << ")"
<< "\n initial_energy = " << initial_energy
<< "\n max_energy = " << max_energy;
}
// bending damping on a flat flex must dissipate energy with implicit integrator
TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast" solver="CG">
<flag energy="enable"/>
</option>
<worldbody>
<flexcomp type="grid" count="6 6 1" spacing=".1 .1 .1"
radius=".005" name="sheet" dim="2" mass="0.1">
<edge equality="false" damping="0" stiffness="0"/>
<elasticity young="1e6" poisson="0" thickness="0.02"
elastic2d="bend" damping="0.1"/>
<contact solref="0.01" internal="false" selfcollide="none"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
ASSERT_EQ(m->nflex, 1);
ASSERT_GT(m->flex_damping[0], 0) << "flex_damping not set";
MjDataPtr d = MakeData(m);
// perturb a central vertex with upward velocity
// vertex layout is 6x6 grid; pick a central vertex (row=3, col=3 -> id=21)
int center_vert = 21;
int bid = m->flex_vertbodyid[m->flex_vertadr[0] + center_vert];
int dofadr = m->body_dofadr[bid];
d->qvel[dofadr + 2] = 1.0; // z-velocity
// initial forward to compute energy
mj_forward(m.get(), d.get());
mjtNum initial_energy = d->energy[0] + d->energy[1];
ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero";
// step forward and check energy decay
mjtNum max_energy = initial_energy;
int nsteps = 100;
for (int i = 0; i < nsteps; i++) {
mj_step(m.get(), d.get());
mjtNum total_energy = d->energy[0] + d->energy[1];
max_energy = mju_max(max_energy, total_energy);
}
// energy must never exceed initial (system must not go unstable)
EXPECT_LE(max_energy, initial_energy * 1.01)
<< "energy exceeded initial by more than 1%: max=" << max_energy
<< ", initial=" << initial_energy;
// after 100 steps (0.1 seconds), energy should have decayed significantly
mjtNum final_energy = d->energy[0] + d->energy[1];
EXPECT_LT(final_energy, 0.5 * initial_energy)
<< "energy did not decay by at least 50% after " << nsteps << " steps"
<< " (initial=" << initial_energy << ", final=" << final_energy << ")";
}
// interp stretch stiffness with implicitfast must preserve energy stability
TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 0" timestep="0.001" integrator="implicitfast" solver="CG">
<flag energy="enable"/>
</option>
<worldbody>
<flexcomp type="grid" count="4 4 4" cellcount="3 3 3"
spacing=".05 .05 .05" radius=".005" name="cube"
dim="3" mass="10" dof="trilinear">
<elasticity young="1e6" poisson="0.3" damping="0"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024] = {0};
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// perturb a central vertex with velocity
int center_body = m->nbody / 2;
int dofadr = m->body_dofadr[center_body];
ASSERT_GT(m->body_dofnum[center_body], 0);
d->qvel[dofadr + 2] = 1.0; // z-velocity
mj_forward(m.get(), d.get());
mjtNum initial_energy = d->energy[0] + d->energy[1];
ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero";
// step and track max energy
mjtNum max_energy = initial_energy;
int nsteps = 50;
for (int i = 0; i < nsteps; i++) {
mj_step(m.get(), d.get());
mjtNum total_energy = d->energy[0] + d->energy[1];
max_energy = mju_max(max_energy, total_energy);
}
// energy must not blow up
EXPECT_LE(max_energy, initial_energy * 1.01)
<< "energy exceeded initial by more than 1%: max=" << max_energy
<< ", initial=" << initial_energy;
}
} // namespace
// with the implicit effective metric active, inverse dynamics must recover the applied force
// (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c + J'*f and the inverse adds
// the same B*qacc - c terms. This is the fwd/inv consistency fence for the flex-CG dispatch.
TEST_F(ForwardTest, GatedFlexInverseConsistency) {
static const char* const kXml = R"(
<mujoco>
<option solver="CG" integrator="implicitfast" tolerance="1e-14"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1" pos="0 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="both" damping="0.5"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int nv = model->nv;
// deform and settle a few steps under gravity
for (int i=0; i < nv; i++) {
data->qvel[i] = 0.1 * (mju_Halton(i, 3) - 0.5);
}
for (int step=0; step < 50; step++) {
mj_step(model.get(), data.get());
}
// forward then inverse at the same state
mj_forward(model.get(), data.get());
mj_inverse(model.get(), data.get());
// no applied forces: the inverse must return ~zero, at the scale of the passive forces
mjtNum scale = 1 + mju_norm(data->qfrc_passive, nv);
EXPECT_LT(mju_norm(data->qfrc_inverse, nv), 1e-6 * scale);
}
} // namespace mujoco
Reference in New Issue View Git Blame Copy Permalink