chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e1440902732ccce33fc280a21ab3d58cedd153a6
Mujoco_WASM/test/engine/engine_derivative_test.cc
T
Alessio 2a3554c8a3 Integrate passive flex contact implicitly 
Contact of a flex with `passive` collisions enabled was applied as an
explicit spring of fixed stiffness 1e4, which the timestep bounds: any
stiffness worth having oscillates faster than the step can resolve, so
the force was too soft to keep sheets apart and interpenetration was
routine.

Carry its curvature in the effective metric M + K instead, alongside the
flex's own stretch and bending stiffness. The contact block k*J^T*J is
appended to the per-vertex candidate list already assembled for the flex
stencils, so it costs additional entries in an existing matrix rather
than a new one, and the accompanying shift -h*K*v is what damps the
stiff modes. At a 2 ms timestep this holds roughly 50x the stiffness an
explicit force of the same step could.

With the timestep no longer setting the bound, the stiffness is chosen
as a natural frequency scaled by the participating vertex mass rather
than left at a fixed 1e4, so one value suits models of any scale.

Passive handling is scoped to contacts whose every dof is a flex vertex
carried by the metric: flex against flex, flex against itself, and flex
against static geometry, which contributes no dofs of its own. For those
the Hessian is assembled in full. Contact with a body that can move
would have that body's dofs dropped from it, and is left on the
constraint solver.

The feature now requires an integrator whose constraint solve runs in
that metric, and is rejected with an error otherwise.

Add model/flex/drape.xml as the example model, replacing sphere_passive,
whose contacts no longer demonstrated the feature.
2026-08-06 15:38:05 +01:00

2429 lines
81 KiB
C++
Raw Blame History

// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for engine/engine_derivative.c.
#include "src/engine/engine_derivative.h"
#include <cstddef>
#include <random>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_core_smooth.h"
#include "src/engine/engine_derivative_fd.h"
#include "src/engine/engine_forward.h"
#include "src/engine/engine_io.h"
#include "src/engine/engine_util_blas.h"
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::std::vector;
using ::testing::DoubleNear;
using ::testing::Each;
using ::testing::Eq;
using ::testing::NotNull;
using ::testing::Pointwise;
using DerivativeTest = MujocoTest;
// errors smaller than this are ignored
#ifdef mjUSESINGLE
static const mjtNum absolute_tolerance = 1e-3;
#else
static const mjtNum absolute_tolerance = 1e-9;
#endif
// corrected relative error
static mjtNum RelativeError(mjtNum a, mjtNum b) {
mjtNum nominator = mjMAX(0, mju_abs(a - b) - absolute_tolerance);
mjtNum denominator = (mju_abs(a) + mju_abs(b) + absolute_tolerance);
return nominator / denominator;
}
// expect two 2D arrays to have elementwise relative error smaller than eps
// return maximum absolute error
static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, int nrow,
int ncol, mjtNum eps) {
mjtNum max_error = 0;
for (int i = 0; i < nrow; i++) {
for (int j = 0; j < ncol; j++) {
mjtNum actual = Actual[i * ncol + j];
mjtNum expected = Expected[i * ncol + j];
EXPECT_LT(RelativeError(actual, expected), eps)
<< "error at position (" << i << ", " << j << ")"
<< "\nexpected = " << expected << "\nactual = " << actual
<< "\ndiff = " << expected - actual;
max_error = mjMAX(mju_abs(actual - expected), max_error);
}
}
return max_error;
}
static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
static const char* const kTumblingThinObjectPath =
"engine/testdata/derivative/tumbling_thin_object.xml";
static const char* const kTumblingThinObjectEllipsoidPath =
"engine/testdata/derivative/tumbling_thin_object_ellipsoid.xml";
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kDamperActuatorsPath =
"engine/testdata/actuation/damper.xml";
static const char* const kDampedPendulumPath =
"engine/testdata/derivative/damped_pendulum.xml";
static const char* const kLinearPath = "engine/testdata/derivative/linear.xml";
static const char* const kDCMotorPath =
"engine/testdata/derivative/dcmotor.xml";
static const char* const kModelPath = "testdata/model.xml";
// compare analytic and finite-difference d_smooth/d_qvel
TEST_F(DerivativeTest, SmoothDvel) {
// run test on all models
for (const char* local_path :
{kEnergyConservingPendulumPath, kTumblingThinObjectPath,
kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024] = "";
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD;
mjData* data = mj_makeData(model);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
// set sparsity
model->opt.jacobian = sparsity;
// take 100 steps so we have some velocities, then call forward
mj_resetData(model, data);
if (model->nu) {
data->ctrl[0] = 0.1;
}
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
mj_forward(model, data);
// construct sparse structure in d->D_xxx, compute analytical qDeriv
mju_zero(data->qDeriv, nD);
mjd_smooth_vel(model, data, /*flg_bias=*/true);
// expect derivatives to be non-zero, make copy of qDeriv as a vector
EXPECT_GT(mju_norm(data->qDeriv, nD), 0);
vector<mjtNum> qDerivAnalytic = AsVector(data->qDeriv, nD);
// compute finite-difference derivatives
mjtNum eps = MjTol(1e-7, 1e-3);
mju_zero(data->qDeriv, nD);
mjd_smooth_velFD(model, data, eps);
// expect FD and analytic derivatives to be numerically different
EXPECT_NE(mju_norm(data->qDeriv, nD),
mju_norm(qDerivAnalytic.data(), nD));
// expect FD and analytic derivatives to be similar to eps precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-7, 3e-3), qDerivAnalytic));
}
mj_deleteData(data);
mj_deleteModel(model);
}
}
// mjd_freeBias_vel: 6x6 bias-derivative block for a standalone free body
// validated against mjd_rne_vel and against finite-differenced mj_rne
TEST_F(DerivativeTest, FreeBiasVel) {
// free body with offset CoM, rotated inertia, non-identity orientation
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="0.1 -0.2 0.3" euler="20 -30 40">
<freejoint/>
<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 data = MakeData(model);
mjModel* m = model.get();
mjData* d = data.get();
// set fast, fully populated velocity
mjtNum qvel[6] = {0.4, -0.3, 0.2, 5, -3, 2};
mju_copy(d->qvel, qvel, 6);
mj_forward(m, d);
// analytic block
mjtNum B[36];
mjd_freeBias_vel(m, d, /*jnt=*/0, B);
// linear columns are zero by construction
for (int r = 0; r < 6; r++) {
for (int c = 0; c < 3; c++) {
EXPECT_EQ(B[6 * r + c], 0);
}
}
// compare with mjd_rne_vel: B == -(qDeriv(flg_bias=1) - qDeriv(flg_bias=0))
mju_zero(d->qDeriv, m->nD);
mjd_smooth_vel(m, d, /*flg_bias=*/1);
vector<mjtNum> qDeriv_bias = AsVector(d->qDeriv, m->nD);
mju_zero(d->qDeriv, m->nD);
mjd_smooth_vel(m, d, /*flg_bias=*/0);
for (int r = 0; r < 6; r++) {
int rowadr = m->D_rowadr[r];
ASSERT_EQ(m->D_rownnz[r], 6);
for (int k = 0; k < 6; k++) {
int c = m->D_colind[rowadr + k];
mjtNum rne_val = -(qDeriv_bias[rowadr + k] - d->qDeriv[rowadr + k]);
EXPECT_NEAR(B[6 * r + c], rne_val, MjTol(1e-14, 1e-6))
<< "mismatch at (" << r << ", " << c << ")";
}
}
// compare with central finite differences of mj_rne
mjtNum eps = MjTol(1e-6, 1e-3);
for (int c = 0; c < 6; c++) {
mjtNum bias_plus[6], bias_minus[6];
d->qvel[c] = qvel[c] + eps;
mj_comVel(m, d);
mj_rne(m, d, /*flg_acc=*/0, bias_plus);
d->qvel[c] = qvel[c] - eps;
mj_comVel(m, d);
mj_rne(m, d, /*flg_acc=*/0, bias_minus);
d->qvel[c] = qvel[c];
for (int r = 0; r < 6; r++) {
mjtNum fd = (bias_plus[r] - bias_minus[r]) / (2 * eps);
EXPECT_NEAR(B[6 * r + c], fd, MjTol(1e-7, 1e-2))
<< "FD mismatch at (" << r << ", " << c << ")";
}
}
}
// disabled actuators do not contribute to d_qfrc_actuator/d_qvel
TEST_F(DerivativeTest, DisabledActuators) {
// model with only a position actuator
static constexpr char xml1[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<position joint="joint" group="1" kp="2000" kv="200"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
d1->ctrl[0] = 6;
while (d1->time < 1) mj_step(m1.get(), d1.get());
// model with a position actuator and an intvelocity actuator
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast" actuatorgroupdisable="2"/>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<position joint="joint" group="1" kp="2000" kv="200"/>
<intvelocity joint="joint" group="2" kp="2000" kv="200" actrange="-6 6"/>
</actuator>
</mujoco>
)";
MjModelPtr m2 = LoadModelFromString(xml2);
MjDataPtr d2 = MakeData(m2);
d2->ctrl[0] = 6;
d2->ctrl[1] = 6;
while (d2->time < 1) mj_step(m2.get(), d2.get());
// expect same qvel in both models
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
}
// actuator order has no effect
TEST_F(DerivativeTest, ActuatorOrder) {
// model with stateful actuator first
static constexpr char xml1[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="0" type="slide" range="-1 1"/>
<geom size=".1"/>
</body>
<body pos="1 0 0">
<joint name="1" type="slide" range="-1 1"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<muscle joint="0" ctrlrange="0 6"/>
<damper joint="1" kv="200" ctrlrange="0 6"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << "Failed to load model: " << error;
MjDataPtr d1 = MakeData(m1);
d1->ctrl[0] = 6;
d1->ctrl[1] = 6;
while (d1->time < 1) mj_step(m1.get(), d1.get());
// model with stateful actuator second
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="0" type="slide" range="-1 1"/>
<geom size=".1"/>
</body>
<body pos="1 0 0">
<joint name="1" type="slide" range="-1 1"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<damper joint="1" kv="200" ctrlrange="0 6"/>
<muscle joint="0" ctrlrange="0 6"/>
</actuator>
</mujoco>
)";
MjModelPtr m2 = LoadModelFromString(xml2, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << "Failed to load model: " << error;
MjDataPtr d2 = MakeData(m2);
d2->ctrl[0] = 6;
d2->ctrl[1] = 6;
while (d2->time < 1) mj_step(m2.get(), d2.get());
// expect same qvel in both models
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
EXPECT_EQ(d1->qvel[1], d2->qvel[1]);
}
// compare analytic and fin-diff d_qfrc_passive/d_qvel
TEST_F(DerivativeTest, PassiveDvel) {
for (const char* local_path :
{kTumblingThinObjectPath, kTumblingThinObjectEllipsoidPath}) {
// load model
const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
int nD = model->nD;
mjData* data = mj_makeData(model);
// allocate Jacobians
mjtNum* qDerivAnalytic = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
mjtNum* qDerivFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
for (mjtJacobian sparsity : {mjJAC_DENSE, mjJAC_SPARSE}) {
// set sparsity
model->opt.jacobian = sparsity;
// take 100 steps so we have some velocities, then call forward
mj_resetData(model, data);
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
mj_forward(model, data);
// get analytic derivatives
mju_zero(data->qDeriv, model->nD);
mjd_passive_vel(model, data);
mju_copy(qDerivAnalytic, data->qDeriv, nD);
// clear qDeriv, get finite-difference derivatives
mju_zero(data->qDeriv, nD);
mju_zero(qDerivFD, nD);
mjtNum eps = MjTol(1e-6, 1e-4);
mjd_passive_velFD(model, data, eps);
// expect FD and analytic derivatives to be similar to tol precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-6, 1e-4), AsVector(qDerivAnalytic, nD)));
}
mju_free(qDerivFD);
mju_free(qDerivAnalytic);
mj_deleteData(data);
mj_deleteModel(model);
}
}
// ----------------------- derivatives of mj_step() ----------------------------
// mj_stepSkip computes the same next state as mj_step
TEST_F(DerivativeTest, StepSkip) {
const std::string xml_path = GetTestDataFilePath(kDampedPendulumPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nq = model->nq;
int nv = model->nv;
// disable warm-starts so we don't need to save qacc_warmstart
model->opt.disableflags |= mjDSBL_WARMSTART;
for (const mjtIntegrator integrator :
{mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) {
model->opt.integrator = integrator;
// reset, take 20 steps
mj_resetData(model, data);
for (int i = 0; i < 20; i++) {
mj_step(model, data);
}
// denormalize the quat, just to see that it doesn't make a difference
for (int j = 0; j < model->njnt; j++) {
if (model->jnt_type[j] == mjJNT_BALL) {
int adr = model->jnt_qposadr[j];
for (int k = 0; k < 4; k++) {
data->qpos[adr + k] *= 8;
}
}
}
// save state
vector<mjtNum> qpos = AsVector(data->qpos, nq);
vector<mjtNum> qvel = AsVector(data->qvel, nv);
// take one more step, save next state
mj_step(model, data);
vector<mjtNum> qpos_next = AsVector(data->qpos, nq);
vector<mjtNum> qvel_next = AsVector(data->qvel, nv);
// reset state, take step again, compare (assert mj_step is deterministic)
mju_copy(data->qpos, qpos.data(), nq);
mju_copy(data->qvel, qvel.data(), nv);
mj_step(model, data);
EXPECT_THAT(AsVector(data->qpos, nq), Pointwise(Eq(), qpos_next));
EXPECT_THAT(AsVector(data->qvel, nv), Pointwise(Eq(), qvel_next));
// reset state, change ctrl, call mj_stepSkip, save next state
mju_copy(data->qpos, qpos.data(), nq);
mju_copy(data->qvel, qvel.data(), nv);
data->ctrl[0] = 1;
mj_stepSkip(model, data, mjSTAGE_VEL, 0); // skipping both POS and VEL
vector<mjtNum> qpos_next_dctrl = AsVector(data->qpos, nq);
vector<mjtNum> qvel_next_dctrl = AsVector(data->qvel, nv);
// reset state (ctrl remains unchanged), call full mj_step, compare
mju_copy(data->qpos, qpos.data(), nq);
mju_copy(data->qvel, qvel.data(), nv);
mj_step(model, data);
EXPECT_THAT(AsVector(data->qpos, nq), Pointwise(Eq(), qpos_next_dctrl));
EXPECT_THAT(AsVector(data->qvel, nv), Pointwise(Eq(), qvel_next_dctrl));
// reset state, change velocity, call mj_stepSkip, save next state
mju_copy(data->qpos, qpos.data(), nq);
mju_copy(data->qvel, qvel.data(), nv);
data->qvel[0] += 1;
mj_stepSkip(model, data, mjSTAGE_POS, 0); // skipping POS
vector<mjtNum> qpos_next_dvel = AsVector(data->qpos, nq);
vector<mjtNum> qvel_next_dvel = AsVector(data->qvel, nv);
// reset state, change velocity, call full mj_step, compare
mju_copy(data->qpos, qpos.data(), nq);
mju_copy(data->qvel, qvel.data(), nv);
data->qvel[0] += 1;
mj_step(model, data);
EXPECT_THAT(AsVector(data->qpos, nq), Pointwise(Eq(), qpos_next_dvel));
EXPECT_THAT(AsVector(data->qvel, nv), Pointwise(Eq(), qvel_next_dvel));
}
mj_deleteData(data);
mj_deleteModel(model);
}
// Analytic transition matrices for linear dynamical system xn = A*x + B*u
// given modified mass matrix H (`data->qH`) and
// Ac = H^-1 [diag(-stiffness) diag(-damping)]
// we have
// A = eye(2*nv) + dt [dt*Ac + [zeros(3) eye(3)]; Ac]
// given the moment arm matrix K (`data->actuator_moment`) and Bc = H^-1 K
// B = dt*[Bc*dt; Bc]
static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
int nv = m->nv, nu = m->nu;
mjtNum dt = m->opt.timestep;
mj_markStack(d);
// === state-transition matrix A
if (A) {
mjtNum* Ac = mj_stackAllocNum(d, 2 * nv * nv);
// Ac = H^-1 [diag(-stiffness) diag(-damping)]
mju_zero(Ac, 2 * nv * nv);
for (int i = 0; i < nv; i++) {
Ac[i * nv + i] = -m->jnt_stiffness[i];
Ac[nv * nv + i * nv + i] = -m->dof_damping[i];
}
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2 * nv, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
// A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2 * nv, nv);
mju_scl(A, A, dt, nv * 2 * nv);
mju_transpose(A + 2 * nv * nv, Ac, 2 * nv, nv);
// Add eye(nv) to top right quadrant of A
for (int i = 0; i < nv; i++) {
A[i * 2 * nv + nv + i] += 1;
}
// A *= dt
mju_scl(A, A, dt, 2 * nv * 2 * nv);
// A += eye(2*nv)
for (int i = 0; i < 2 * nv; i++) {
A[i * 2 * nv + i] += 1;
}
}
// === control-transition matrix B
if (B) {
mjtNum* Bc = mj_stackAllocNum(d, nu * nv);
mjtNum* BcT = mj_stackAllocNum(d, nv * nu);
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind);
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt * dt, nu * nv);
mju_scl(B + nu * nv, BcT, dt, nu * nv);
}
mj_freeStack(d);
}
// compare FD derivatives to analytic derivatives of linear dynamical system
TEST_F(DerivativeTest, LinearSystem) {
const std::string xml_path = GetTestDataFilePath(kLinearPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv, nu = model->nu;
// set ctrl, integrate for 20 steps
data->ctrl[0] = .1;
data->ctrl[1] = -.1;
for (int i = 0; i < 20; i++) {
mj_step(model, data);
}
// analytic A and B
mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
LinearSystem(model, data, A, B);
// uncomment for debugging:
// PrintMatrix(A, 2*nv, 2*nv);
// PrintMatrix(B, 2*nv, nu);
// forward differenced A and B
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD, nullptr,
nullptr);
// uncomment for debugging:
// PrintMatrix(AFD, 2*nv, 2*nv);
// PrintMatrix(BFD, 2*nv, nu);
// expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(A, AFD, 2 * nv, 2 * nv, eps);
CompareMatrices(B, BFD, 2 * nv, nu, eps);
// central differenced A and B
mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model, data, eps, /*centered=*/1, AFDc, BFDc, nullptr,
nullptr);
// expect central derivatives to be equal to forward differences
CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps);
CompareMatrices(BFD, BFDc, 2 * nv, nu, eps);
mju_free(BFDc);
mju_free(AFDc);
mju_free(BFD);
mju_free(AFD);
mju_free(B);
mju_free(A);
mj_deleteData(data);
mj_deleteModel(model);
}
// check ctrl derivatives at the range limit
TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
const std::string xml_path = GetTestDataFilePath(kLinearPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv, nu = model->nu;
// set ctrl, integrate for 20 steps
data->ctrl[0] = .1;
data->ctrl[1] = -.1;
for (int i = 0; i < 20; i++) {
mj_step(model, data);
}
// analytic B
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
LinearSystem(model, data, nullptr, B);
// forward differenced A and B
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
// set ctrl to the limits, request forward differences
data->ctrl[0] = 1;
data->ctrl[1] = -1;
mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
nullptr);
// expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(B, BFD, 2 * nv, nu, eps);
// ctrl remains at limits, request central differences
mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
nullptr);
// expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(B, BFD, 2 * nv, nu, eps);
// set ctrl beyond limits, request forward differences
data->ctrl[0] = 2;
data->ctrl[1] = -2;
mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
nullptr);
// expect derivatives to be 0
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
// expect ctrl to remain unchanged (despite internal clamping)
EXPECT_EQ(data->ctrl[0], 2.0);
EXPECT_EQ(data->ctrl[1], -2.0);
// ctrl remains beyond limits, request centered differences
mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
nullptr);
// expect derivatives to be 0
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
mju_free(BFD);
mju_free(B);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare FD sensor derivatives to analytic derivatives
TEST_F(DerivativeTest, SensorDerivatives) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<general name="actuator" joint="joint" gainprm="3"/>
</actuator>
<sensor>
<jointpos joint="joint"/>
<jointvel joint="joint"/>
<actuatorfrc actuator="actuator"/>
</sensor>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
int nv = model->nv, nu = model->nu, ns = model->nsensordata;
MjDataPtr data = MakeData(model);
// finite differenced C and D
mjtNum eps = 1e-6;
mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv);
mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu);
mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr,
nullptr, CFD, DFD);
// expected analytic C and D
mjtNum C[6] = {1, 0, 0, 1, 0, 0};
mjtNum D[3] = {
0,
0,
3,
};
// compare expected and actual values
CompareMatrices(CFD, C, ns, 2 * nv, eps);
CompareMatrices(DFD, D, ns, nu, eps);
mju_free(DFD);
mju_free(CFD);
}
// if sensor derivatives aren't requested, don't compute sensors
TEST_F(DerivativeTest, SensorSkip) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<general name="actuator" joint="joint" gainprm="3"/>
</actuator>
<sensor>
<jointpos joint="joint"/>
</sensor>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
int nv = model->nv, nu = model->nu;
MjDataPtr data = MakeData(model);
// set a sentinel value in the sensor
data->sensordata[0] = 1337;
// finite differenced B
mjtNum eps = 1e-6;
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr, BFD,
nullptr, nullptr);
EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed";
mju_free(BFD);
}
// derivatives don't mutate the state
TEST_F(DerivativeTest, NoStateMutation) {
const std::string xml_path = GetTestDataFilePath(kModelPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data0 = mj_makeData(model);
mjData* data = mj_makeData(model);
int nv = model->nv, nu = model->nu, na = model->na, ns = model->nsensordata;
// set time
data->time = data0->time = 0.5;
for (int i = 0; i < nv; i++) {
data->qpos[i] = data0->qpos[i] = (mjtNum)i + 1;
data->qvel[i] = data0->qvel[i] = (mjtNum)i + 2;
}
// set ctrl
for (int i = 0; i < nu; i++) {
data->ctrl[i] = data0->ctrl[i] = (mjtNum)i + 1;
}
// set act
for (int i = 0; i < na; i++) {
data->act[i] = data0->act[i] = (mjtNum)i + 1;
}
// allocate Jacobians, call derivatives
int ndx = nv + nv + na;
mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx);
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * nu);
mjtNum* C = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * ndx);
mjtNum* D = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu);
mjtNum eps = 1e-6;
mjd_transitionFD(model, data, eps, /*centered=*/0, A, B, C, D);
// compare states in data and data0
EXPECT_EQ(data->time, data0->time);
EXPECT_EQ(AsVector(data->qpos, model->nq), AsVector(data0->qpos, model->nq));
EXPECT_EQ(AsVector(data->qvel, nv), AsVector(data0->qvel, nv));
EXPECT_EQ(AsVector(data->act, na), AsVector(data0->act, na));
EXPECT_EQ(AsVector(data->ctrl, nu), AsVector(data0->ctrl, nu));
mju_free(D);
mju_free(C);
mju_free(B);
mju_free(A);
mj_deleteData(data0);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare dense and sparse derivatives of qfrc_bias (RNE)
TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
// run test on all models
for (const char* local_path :
{kEnergyConservingPendulumPath, kTumblingThinObjectPath,
kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024] = "";
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD;
mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
mjData* data = mj_makeData(model);
// take 100 steps so we have some velocities, then call forward
mj_resetData(model, data);
if (model->nu) {
data->ctrl[0] = 0.1;
}
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
mj_forward(model, data);
// compute qDeriv with sparse function, make local copy
mjd_smooth_vel(model, data, /*flg_bias=*/1);
mju_copy(qDeriv, data->qDeriv, nD);
// re-compute with dense function
mju_zero(data->qDeriv, model->nD);
mjd_actuator_vel(model, data);
mjd_passive_vel(model, data);
mjd_rne_vel_dense(model, data);
// expect dense and sparse derivatives to be similar to precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD)));
mju_free(qDeriv);
mj_deleteData(data);
mj_deleteModel(model);
}
}
// compare FD inverse derivatives to analytic derivatives of linear system
TEST_F(DerivativeTest, LinearSystemInverse) {
const std::string xml_path = GetTestDataFilePath(kLinearPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
int nv = model->nv;
int ns = model->nsensordata;
int nC = model->nC;
vector<mjtNum> DfDq(nv * nv);
vector<mjtNum> DfDv(nv * nv);
vector<mjtNum> DfDa(nv * nv);
vector<mjtNum> DsDq(nv * ns);
vector<mjtNum> DsDv(nv * ns);
vector<mjtNum> DsDa(nv * ns);
vector<mjtNum> DmDq(nv * nC);
// call mj_forward to get accelerations at initial state
mj_forward(model, data);
// get derivatives
mjtNum eps = 1e-6;
mjtByte flg_actuation = 0;
mjd_inverseFD(model, data, eps, flg_actuation, DfDq.data(), DfDv.data(),
DfDa.data(), DsDq.data(), DsDv.data(), DsDa.data(),
DmDq.data());
// expect that position derivatives are the stiffnesses
vector<mjtNum> DfDq_expect = {model->jnt_stiffness[0], 0, 0, 0,
model->jnt_stiffness[1], 0, 0, 0,
model->jnt_stiffness[2]};
EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect));
// expect that velocity derivatives are the dampings
vector<mjtNum> DfDv_expect = {model->dof_damping[0], 0, 0, 0,
model->dof_damping[1], 0, 0, 0,
model->dof_damping[2]};
EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect));
// expect that acceleration derivatives are the mass matrix
vector<mjtNum> DfDa_expect(nv * nv, 0);
mj_fullM(model, data, DfDa_expect.data());
EXPECT_THAT(DfDa, Pointwise(DoubleNear(eps), DfDa_expect));
// expect that sensor derivatives w.r.t position only see sensor 1 at dof 0
vector<mjtNum> DsDq_expect(nv * ns, 0);
int dof_index = 0;
int sensordata_index = model->sensor_adr[1];
DsDq_expect[dof_index * ns + sensordata_index] = 1;
EXPECT_THAT(DsDq, Pointwise(DoubleNear(eps), DsDq_expect));
// expect that sensor derivatives w.r.t velocity only see sensor 0 at dof 1
vector<mjtNum> DsDv_expect(nv * ns, 0);
dof_index = 1;
sensordata_index = model->sensor_adr[0];
DsDv_expect[dof_index * ns + sensordata_index] = 1;
EXPECT_THAT(DsDv, Pointwise(DoubleNear(eps), DsDv_expect));
// expect that sensor derivatives w.r.t acceleration see the accelerometer
// in the y-axis, affected by both dof 0 and dof 1
vector<mjtNum> DsDa_expect(nv * ns, 0);
dof_index = 0;
sensordata_index = model->sensor_adr[2] + 1;
DsDa_expect[dof_index * ns + sensordata_index] = 1;
dof_index = 1;
DsDa_expect[dof_index * ns + sensordata_index] = 1;
EXPECT_THAT(DsDa, Pointwise(DoubleNear(eps), DsDa_expect));
// expect that mass matrix derivatives are zero
vector<mjtNum> DmDq_expect(nv * nC, 0);
EXPECT_THAT(DmDq, Pointwise(DoubleNear(eps), DmDq_expect));
mj_deleteData(data);
mj_deleteModel(model);
}
// utility: generate two random quaternions with a given angle difference
void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
// make distribution using seed
std::mt19937_64 rng;
rng.seed(seed);
std::normal_distribution<double> dist(0, 1);
// sample qa = qb
for (int i = 0; i < 4; i++) {
qa[i] = qb[i] = dist(rng);
}
mju_normalize4(qa);
mju_normalize4(qb);
// integrate qb in random direction by angle
mjtNum dir[3];
for (int i = 0; i < 3; i++) {
dir[i] = dist(rng);
}
mju_normalize3(dir);
mju_quatIntegrate(qb, dir, angle);
}
// utility: finite-difference Jacobians of mju_subQuat
static void subQuatFD(mjtNum Da[9], mjtNum Db[9], const mjtNum qa[4],
const mjtNum qb[4], mjtNum eps) {
// subQuat
mjtNum y[3];
mju_subQuat(y, qa, qb);
mjtNum dq[3]; // nudge input direction
mjtNum dqa[4]; // nudged qa input
mjtNum dqb[4]; // nudged qb input
mjtNum dy[3]; // nudged output
mjtNum DaT[9]; // Da transposed
mjtNum DbT[9]; // Db transposed
for (int i = 0; i < 3; i++) {
// perturbation
mju_zero3(dq);
dq[i] = 1.0;
// Jacobian: d_y / d_qa
mju_copy4(dqa, qa);
mju_quatIntegrate(dqa, dq, eps);
mju_subQuat(dy, dqa, qb);
mju_sub3(DaT + i * 3, dy, y);
mju_scl3(DaT + i * 3, DaT + i * 3, 1.0 / eps);
// Jacobian: d_y / d_qb
mju_copy4(dqb, qb);
mju_quatIntegrate(dqb, dq, eps);
mju_subQuat(dy, qa, dqb);
mju_sub3(DbT + i * 3, dy, y);
mju_scl3(DbT + i * 3, DbT + i * 3, 1.0 / eps);
}
// transpose result
mju_transpose(Da, DaT, 3, 3);
mju_transpose(Db, DbT, 3, 3);
}
TEST_F(DerivativeTest, SubQuat) {
const int nrepeats = 10; // number of repeats
const mjtNum eps =
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1;
for (int i = 0; i < nrepeats; i++) {
for (mjtNum angle : {0.0, 1e-9, 1e-5, 1e-2, 1.0, 4.0}) {
// random quaternions
mjtNum qa[4];
mjtNum qb[4];
// make random quaternion pair with given relative angle
randomQuatPair(qa, qb, angle, seed++);
// analytic Jacobians
mjtNum Da[9]; // d_subQuat(qa, qb) / d_qa
mjtNum Db[9]; // d_subQuat(qa, qb) / d_qb
mjd_subQuat(qa, qb, Da, Db);
// finite-differenced Jacobians
mjtNum DaFD[9];
mjtNum DbFD[9];
subQuatFD(DaFD, DbFD, qa, qb, eps);
// expect numerical equality
EXPECT_THAT(AsVector(DaFD, 9),
Pointwise(MjNear(1e-7, 1e-3), AsVector(Da, 9)));
EXPECT_THAT(AsVector(DbFD, 9),
Pointwise(MjNear(1e-7, 1e-3), AsVector(Db, 9)));
}
}
}
// utility: random quaternion, 3D velocity
static void randomQuatVel(mjtNum quat[4], mjtNum vel[3], int seed) {
// make distribution using seed
std::mt19937_64 rng;
rng.seed(seed);
std::normal_distribution<double> dist(0, 1);
// sample quat
for (int i = 0; i < 4; i++) {
quat[i] = dist(rng);
}
mju_normalize4(quat);
// sample vel
for (int i = 0; i < 3; i++) {
vel[i] = dist(rng);
}
}
// utility: finite-difference Jacobians of mju_quatIntegrate
void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], mjtNum Dvel[9],
mjtNum Dh[3], const mjtNum quat[4],
const mjtNum vel[3], mjtNum h, mjtNum eps) {
// compute y, output of mju_quatIntegrate(quat, vel, h)
mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]};
mju_quatIntegrate(y, vel, h);
mjtNum dx[3]; // nudged tangent-space input
mjtNum dq[4]; // quat output
mjtNum dy[3]; // nudged tangent-space output
mjtNum DquatT[9]; // Dquat transposed
mjtNum DsT[9]; // Ds transposed
mjtNum DvelT[9]; // Dvel transposed
for (int i = 0; i < 3; i++) {
// perturbation
mju_zero3(dx);
dx[i] = 1.0;
// d_y / d_quat
mju_copy4(dq, quat);
mju_quatIntegrate(dq, dx, eps); // nudge dq
mju_quatIntegrate(dq, vel, h); // compute nudged
mju_subQuat(dy, dq, y); // subtract
mju_scl3(DquatT + i * 3, dy, 1.0 / eps);
// d_y / d_sv (scaled velocity)
mju_copy4(dq, quat);
mjtNum dsv[3] = {vel[0] * h, vel[1] * h, vel[2] * h};
mju_addToScl3(dsv, dx, eps); // nudge dsv
mju_quatIntegrate(dq, dsv, 1.0); // compute nudged
mju_subQuat(dy, dq, y); // subtract
mju_scl3(DsT + i * 3, dy, 1.0 / eps);
// d_y / d_v (unscaled velocity)
mju_copy4(dq, quat);
mjtNum dv[3] = {vel[0], vel[1], vel[2]};
mju_addToScl3(dv, dx, eps); // nudge dv
mju_quatIntegrate(dq, dv, h); // compute nudged
mju_subQuat(dy, dq, y); // subtract
mju_scl3(DvelT + i * 3, dy, 1.0 / eps);
}
// d_y / d_h (unscaled velocity)
mju_copy4(dq, quat);
mju_quatIntegrate(dq, vel, h + eps); // compute nudged
mju_subQuat(dy, dq, y); // subtract
mju_scl3(Dh, dy, 1.0 / eps);
// transpose
mju_transpose(Dquat, DquatT, 3, 3);
mju_transpose(Ds, DsT, 3, 3);
mju_transpose(Dvel, DsT, 3, 3);
}
TEST_F(DerivativeTest, quatIntegrate) {
const int nrepeats = 10; // number of repeats
const mjtNum eps =
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1;
for (int i = 0; i < nrepeats; i++) {
for (mjtNum h : {0.0, 1e-9, 1e-5, 1e-2, 1.0, 4.0}) {
// make random quaternion and velocity
mjtNum quat[4];
mjtNum vel[3];
randomQuatVel(quat, vel, seed++);
// analytic Jacobians
mjtNum Dquat[9]; // d_quatIntegrate(quat, vel, h) / d_quat
mjtNum Dvel[9]; // d_quatIntegrate(quat, vel, h) / d_vel
mjtNum Dh[3]; // d_quatIntegrate(quat, vel, h) / d_h
mjd_quatIntegrate(vel, h, Dquat, Dvel, Dh);
// finite-differenced Jacobians
mjtNum DquatFD[9];
mjtNum DsFD[9];
mjtNum DvelFD[9];
mjtNum DhFD[3];
mjd_quatIntegrateFD(DquatFD, DsFD, DvelFD, DhFD, quat, vel, h, eps);
// expect numerical equality of un/scaled velocity derivatives
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(MjNear(1e-7, 1e-2), DsFD));
// expect numerical equality of analytic and FD derivatives
EXPECT_THAT(AsVector(DquatFD, 9), Pointwise(MjNear(1e-7, 1e-2), Dquat));
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(MjNear(1e-7, 1e-2), Dvel));
EXPECT_THAT(AsVector(DhFD, 3), Pointwise(MjNear(1e-7, 1e-2), Dh));
}
}
}
// implicit integration is better than Euler with active forcerange clamping
TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" integrator="implicitfast"/>
<worldbody>
<geom name="plane" type="plane" size="2 2 0.1"/>
<light pos="0 0 3"/>
<body name="1" pos="0 0 1">
<joint name="1" type="slide" axis="1 0 0"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<position joint="1" kp="10000" kv="1000" forcerange="-10 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
mjtNum dt_small = 1e-4;
mjtNum dt_large = 1e-2;
mjtNum duration = 1.0;
MjDataPtr d_gt = MakeData(m);
MjDataPtr d_implicit = MakeData(m);
MjDataPtr d_euler = MakeData(m);
mj_resetData(m.get(), d_gt.get());
mj_resetData(m.get(), d_implicit.get());
mj_resetData(m.get(), d_euler.get());
d_gt.get()->ctrl[0] = 0.5;
d_implicit->ctrl[0] = 0.5;
d_euler->ctrl[0] = 0.5;
mjtNum error_implicit = 0;
mjtNum error_euler = 0;
int nsteps_large = static_cast<int>(duration / dt_large);
int substeps = static_cast<int>(dt_large / dt_small);
m->opt.timestep = dt_large;
m->opt.integrator = mjINT_IMPLICITFAST;
mj_resetData(m.get(), d_gt.get());
d_gt.get()->ctrl[0] = 0.5;
m->opt.timestep = dt_small;
m->opt.integrator = mjINT_EULER;
for (int i = 0; i < nsteps_large; i++) {
// ground truth: small steps with Euler
m->opt.integrator = mjINT_EULER;
m->opt.timestep = dt_small;
for (int j = 0; j < substeps; j++) {
mj_step(m.get(), d_gt.get());
}
// euler at large timestep
m->opt.timestep = dt_large;
mj_step(m.get(), d_euler.get());
// implicitfast at large timestep
m->opt.integrator = mjINT_IMPLICITFAST;
mj_step(m.get(), d_implicit.get());
// accumulate errors
mjtNum diff_implicit = d_gt.get()->qpos[0] - d_implicit->qpos[0];
mjtNum diff_euler = d_gt.get()->qpos[0] - d_euler->qpos[0];
error_implicit += diff_implicit * diff_implicit;
error_euler += diff_euler * diff_euler;
}
// expect implicitfast to be more accurate than Euler
EXPECT_LT(error_implicit, error_euler)
<< "implicitfast should be more accurate than Euler at large timestep "
<< "when forcerange derivatives are correctly handled";
}
// forcelimited actuator following a multi-output SO3 actuator: the derivative
// skip for saturated actuators must index forcerange per actuator, not per
// output.
TEST_F(DerivativeTest, ForcerangeClampedAfterSO3) {
static constexpr char xml[] = R"(
<mujoco>
<option>
<flag contact="disable" gravity="disable"/>
</option>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom type="box" size=".05 .07 .03"/>
</body>
<body pos="0 0 .3">
<joint name="hinge"/>
<geom size=".05"/>
</body>
</worldbody>
<actuator>
<orientation joint="ball" kp="1" kv="1"/>
<velocity joint="hinge" kv="10" forcerange="-1 1"/>
</actuator>
</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();
// spin the hinge so the velocity actuator saturates: force -50, clamped -1
mjtNum qvel[4] = {0.1, 0.2, 0.3, 5};
mju_copy(d->qvel, qvel, 4);
mj_forward(m, d);
ASSERT_EQ(d->actuator_force[3], -1);
// analytic qDeriv
mju_zero(d->qDeriv, m->nD);
mjd_smooth_vel(m, d, /*flg_bias=*/1);
vector<mjtNum> qDerivAnalytic = AsVector(d->qDeriv, m->nD);
EXPECT_GT(mju_norm(qDerivAnalytic.data(), m->nD), 0);
// expect match with finite differences: the saturated actuator contributes
// nothing, the SO3 actuator's damping is unaffected by its neighbor
mjtNum eps = MjTol(1e-7, 1e-3);
mju_zero(d->qDeriv, m->nD);
mjd_smooth_velFD(m, d, eps);
EXPECT_THAT(AsVector(d->qDeriv, m->nD),
Pointwise(MjNear(1e-7, 3e-3), qDerivAnalytic));
}
TEST_F(DerivativeTest, NonlinearDampingDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide" damping="2 3 4"/>
<geom size="1" mass="1"/>
</body>
</worldbody>
<keyframe>
<key qvel="3"/>
</keyframe>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
mjtNum dt_small = 1e-4;
mjtNum dt_large = 1e-2;
mjtNum duration = 1.0;
MjDataPtr d_gt = MakeData(m);
MjDataPtr d_enabled = MakeData(m);
MjDataPtr d_disabled = MakeData(m);
mj_resetDataKeyframe(m.get(), d_gt.get(), 0);
mj_resetDataKeyframe(m.get(), d_enabled.get(), 0);
mj_resetDataKeyframe(m.get(), d_disabled.get(), 0);
m->opt.integrator = mjINT_EULER;
mjtNum error_enabled = 0;
mjtNum error_disabled = 0;
int nsteps_large = static_cast<int>(duration / dt_large);
int substeps = static_cast<int>(dt_large / dt_small);
for (int i = 0; i < nsteps_large; i++) {
m->opt.timestep = dt_small;
m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping
for (int j = 0; j < substeps; j++) {
mj_step(m.get(), d_gt.get());
}
m->opt.timestep = dt_large;
mj_step(m.get(), d_disabled.get());
m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping
mj_step(m.get(), d_enabled.get());
mjtNum diff_enabled = d_gt.get()->qvel[0] - d_enabled.get()->qvel[0];
mjtNum diff_disabled = d_gt.get()->qvel[0] - d_disabled.get()->qvel[0];
error_enabled += diff_enabled * diff_enabled;
error_disabled += diff_disabled * diff_disabled;
}
EXPECT_LT(error_enabled, error_disabled)
<< "Euler with implicit damping should be more accurate than without "
<< "when nonlinear damping derivatives are correctly handled";
}
// implicit derivatives should use next activation when actearly is set
TEST_F(DerivativeTest, ActearlyDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="1" integrator="implicitfast"/>
<worldbody>
<body>
<joint name="early" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
<body pos="1 0 0">
<joint name="late" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="early" dyntype="integrator" gaintype="affine"
gainprm="1 0 1" actearly="true"/>
<general joint="late" dyntype="integrator" gaintype="affine"
gainprm="1 0 1" actearly="false"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// set identical ctrl with zero initial activation
d->ctrl[0] = 1.0;
d->ctrl[1] = 1.0;
d->act[0] = 0.0;
d->act[1] = 0.0;
// step computes derivatives during implicit integration
mj_step(m.get(), d.get());
// both should have same act_dot
EXPECT_EQ(d->act_dot[0], d->act_dot[1]);
// with actearly=true and nonzero act_dot, derivative should differ
// because actearly uses next activation: act + act_dot*dt
// for our model: next_act = 0 + 1*1 = 1, current_act = 0
// derivative adds gain_vel * act to qDeriv diagonal
// for independent bodies, D is diagonal, so diag[i] is at D_rowadr[i]
int diag0 = m->D_rowadr[0]; // first joint's diagonal
int diag1 = m->D_rowadr[1]; // second joint's diagonal
EXPECT_NE(d->qDeriv[diag0], d->qDeriv[diag1])
<< "actearly=true should use next activation in derivative";
// verify specific values: gain_vel=1, next_act=1, current_act=0
EXPECT_NEAR(d->qDeriv[diag0], 1.0, 1e-10)
<< "actearly=true should use next_act=1";
EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10)
<< "actearly=false should use current_act=0";
}
// verify stateful DC motor derivative matches analytical formula
TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5"
inductance="0 0.001" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// set nonzero velocity and ctrl
d->qvel[0] = 1.0;
d->ctrl[0] = 0.5;
// forward to compute act_dot, etc.
mj_forward(m.get(), d.get());
// compute analytical derivatives
mjd_smooth_vel(m.get(), d.get(), /* flg_bias = */ 1);
// extract diagonal of qDeriv
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
// expected: K*(dVdw - K)*(1 - exp(-h/te))/R
// with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5
mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0;
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
<< "stateful DC motor derivative should match analytical formula";
}
// verify that stateful DC motor derivative converges to stateless as te -> 0
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
// stateless DC motor with position controller
static constexpr char xml_stateless[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
// stateful DC motor with very small te
static constexpr char xml_stateful[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
inductance="0 1e-8" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
ASSERT_THAT(m_sl.get(), NotNull()) << error;
MjDataPtr d_sl = MakeData(m_sl);
MjModelPtr m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
ASSERT_THAT(m_sf.get(), NotNull()) << error;
MjDataPtr d_sf = MakeData(m_sf);
// set identical state
d_sl.get()->qvel[0] = d_sf.get()->qvel[0] = 1.0;
d_sl.get()->ctrl[0] = d_sf.get()->ctrl[0] = 0.5;
// forward and compute derivatives
mj_forward(m_sl.get(), d_sl.get());
mj_forward(m_sf.get(), d_sf.get());
mjd_smooth_vel(m_sl.get(), d_sl.get(), 1);
mjd_smooth_vel(m_sf.get(), d_sf.get(), 1);
// extract diagonals
mjtNum diag_sl =
d_sl.get()->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
mjtNum diag_sf =
d_sf.get()->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
<< "stateful derivative should converge to stateless as te -> 0";
}
// Utility: Rotate flex grid
void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
double angle) {
int flex_id = mj_name2id(model, mjOBJ_FLEX, flex_name);
ASSERT_NE(flex_id, -1);
int node_adr = model->flex_nodeadr[flex_id];
int* node_bodies = model->flex_nodebodyid + node_adr;
int nodenum = model->flex_nodenum[flex_id];
// Make deterministic quaternion for rotation inside helper
mjtNum quat[4] = {1, 0, 0, 0};
if (angle != 0) {
mjtNum vel[3] = {1, 1, 1};
mju_normalize3(vel);
mju_quatIntegrate(quat, vel, angle);
}
// reset first to get initial positions
mj_resetData(model, data);
mj_forward(model, data); // Compute initial xpos
// Update qpos
for (int i = 0; i < nodenum; i++) {
int bodyid = node_bodies[i];
// Only process nodes with valid bodies (FlexInterpDamping assumes this)
if (bodyid >= 0) {
mjtNum xpos0[3];
mju_copy3(xpos0, data->xpos + 3 * bodyid); // Initial absolute position
mjtNum xpos_new[3];
mju_rotVecQuat(xpos_new, xpos0, quat); // Rotate absolute position
mjtNum delta[3];
mju_sub3(delta, xpos_new, xpos0);
// Find the qpos address for this node/body
int jnt = model->body_jntadr[bodyid];
if (jnt >= 0) {
int qadr = model->jnt_qposadr[jnt];
mju_addTo3(data->qpos + qadr, delta);
}
}
}
}
// Helper: assemble flex stiffness into dense matrix via matrix-vector products.
// Builds K column-by-column using mjd_flexInterp_mul.
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
// convention where stiffness is subtracted from the system matrix).
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, int nv,
mjtNum h) {
std::vector<mjtNum> e_i(nv, 0);
std::vector<mjtNum> col(nv, 0);
for (int i = 0; i < nv; i++) {
mju_zero(e_i.data(), nv);
mju_zero(col.data(), nv);
e_i[i] = 1.0;
mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h, NULL);
// col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K)
for (int j = 0; j < nv; j++) {
H_dense[j * nv + i] = -col[j];
}
}
}
// compare analytic and fin-diff d_qfrc_passive/d_qvel for flex interp
// Combined test for verify mjd_flexInterp_mulK (stiffness) and damping
TEST_F(DerivativeTest, FlexInterpDerivatives) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="flex" type="grid" count="3 3 3" spacing="0.1 0.2 0.3"
radius=".01" dim="3" mass="1" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1e4" poisson="0.3" damping="50"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nD = model->nD;
int nv = model->nv;
ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs
MjDataPtr data = MakeData(model);
// iterate over rotations
for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) {
RotateFlexGrid(model.get(), data.get(), "flex", angle);
mj_forward(model.get(), data.get());
// part 1: stiffness verification
{
std::vector<mjtNum> vec(nv);
std::vector<mjtNum> res(nv);
mju_zero(vec.data(), nv);
// use deterministic random perturbation to verify full stiffness matrix
// behavior
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 2) - 0.5;
}
// use mulKD to compute K * vec
// mulKD adds (h^2*K + h*D)*vec to res
// if we set h=1, damping=0, we get K*vec
mjtNum save_damping = model->flex_damping[0];
model->flex_damping[0] = 0;
std::vector<mjtNum> H(nv * nv, 0);
// assemble K into H column-by-column
mulKD_dense(model.get(), data.get(), H.data(), nv, 1.0);
// restore damping
model->flex_damping[0] = save_damping;
// compute res = K * vec
mju_mulMatVec(res.data(), H.data(), vec.data(), nv, nv);
// finite difference of mj_passive for stiffness
mjtNum eps = MjTol(1e-6, 1e-3);
mjData* data_perturbed = mj_copyData(NULL, model.get(), data.get());
// apply perturbation
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
// recompute geometry/passive
mj_forward(model.get(), data_perturbed);
// compute FD estimate of K * vec
// qfrc_passive = -dV/dq => d(qfrc)/dq = -K
// (qfrc_new - qfrc)/eps ~= -K * vec
std::vector<mjtNum> fd_res(nv);
for (int i = 0; i < nv; ++i) {
fd_res[i] =
-(data_perturbed->qfrc_passive[i] - data->qfrc_passive[i]) / eps;
}
// compare analytical result (H*vec) with FD result
for (int i = 0; i < nv; ++i) {
EXPECT_THAT(res[i], MjNear(fd_res[i], 5e-3, 5.0))
<< "Stiffness Mismatch at DOF " << i;
}
mj_deleteData(data_perturbed);
// check symmetry: K[i,j] == K[j,i]
std::vector<mjtNum>& K_full = H;
mjtNum max_asymmetry = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < i; j++) {
mjtNum diff = mju_abs(K_full[i * nv + j] - K_full[j * nv + i]);
max_asymmetry = mju_max(max_asymmetry, diff);
}
}
EXPECT_THAT(max_asymmetry, MjNear(0, 1e-10, 5e-4))
<< "K matrix is not symmetric at angle " << angle;
// check positive semi-definiteness: v^T * K * v >= 0
for (int trial = 0; trial < 5; trial++) {
std::vector<mjtNum> v(nv);
for (int i = 0; i < nv; i++) {
v[i] = mju_Halton(i + trial * nv, 3) - 0.5;
}
mjtNum vKv = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) {
vKv += v[i] * K_full[i * nv + j] * v[j];
}
}
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5))
<< "K matrix is not PSD at angle " << angle;
}
}
// part 2: damping verification
{
// set velocity non-zero to test damping
data->qvel[0] = 1.0;
mj_forward(model.get(), data.get());
// get analytic derivatives (without Flex Damping currently)
std::vector<mjtNum> qDerivAnalytic(nD);
mju_zero(data->qDeriv, nD);
mjd_passive_vel(model.get(), data.get());
mju_copy(qDerivAnalytic.data(), data->qDeriv, nD);
// finite-difference derivatives
std::vector<mjtNum> qDerivFD(nD);
mju_zero(data->qDeriv, nD);
mjtNum eps = MjTol(1e-6, 1e-3);
mjd_passive_velFD(model.get(), data.get(), eps);
mju_copy(qDerivFD.data(), data->qDeriv, nD);
// check that we have non-zero damping (FD should find it)
EXPECT_GT(mju_norm(qDerivFD.data(), nD), 1e-3);
// compute expected flex damping using mulKD_dense
// D = 4*H(0.5) - H(1)
vector<mjtNum> H1(nv * nv, 0);
mulKD_dense(model.get(), data.get(), H1.data(), nv, 1.0);
vector<mjtNum> H2(nv * nv, 0);
mulKD_dense(model.get(), data.get(), H2.data(), nv, 0.5);
vector<mjtNum> D(nv * nv);
for (int i = 0; i < nv * nv; i++) {
D[i] = 4.0 * H2[i] - H1[i];
}
// subtract D from qDerivAnalytic using sparse indexing
// d(force)/d(vel) = -D
for (int i = 0; i < nv; i++) {
int rownnz = model->D_rownnz[i];
int rowadr = model->D_rowadr[i];
for (int k = 0; k < rownnz; k++) {
int index = rowadr + k;
int j = model->D_colind[index];
qDerivAnalytic[index] -= D[i * nv + j];
}
}
// expect FD and corrected analytic derivatives to match
EXPECT_THAT(qDerivAnalytic, Pointwise(MjNear(1e-4, 1e4), qDerivFD))
<< "Damping Mismatch at angle: " << angle;
}
}
}
// Test Jacobian under deformation to highlight approximation error
TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="flex" type="grid" count="3 3 3" spacing="0.1 0.2 0.3"
radius=".01" dim="3" mass="1" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1e4" poisson="0.3" damping="0"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
MjDataPtr data = MakeData(model);
// Apply rotation
RotateFlexGrid(model.get(), data.get(), "flex", 1.0); // 1 radian rotation
// Apply deformation (stretch along X)
// qpos is initialized by RotateFlexGrid.
// Add a random perturbation to qpos that represents deformation.
// We use a deterministic sequence to ensure reproducibility.
std::vector<mjtNum> deformation(nv);
for (int i = 0; i < nv; i++) {
// Large deformation to make sure terms are significant
deformation[i] = (mju_Halton(i, 3) - 0.5) * 0.2;
}
mju_addTo(data->qpos, deformation.data(), nv);
mj_forward(model.get(), data.get());
// 1. Compute Analytic Jacobian (Approximate)
// We use mulKD_dense to get K_approx
std::vector<mjtNum> H_approx(nv * nv, 0);
// h=1, damping=0 => gives K
mulKD_dense(model.get(), data.get(), H_approx.data(), nv, 1.0);
// 2. Compute Finite Difference Jacobian (Ground Truth)
// qfrc_passive = -dV/dq
// d(qfrc)/dq = -K_true
std::vector<mjtNum> K_true(nv * nv, 0);
mjtNum eps = 1e-6;
for (int i = 0; i < nv; i++) {
mjData* data_p = mj_copyData(NULL, model.get(), data.get());
data_p->qpos[i] += eps;
mj_forward(model.get(), data_p);
for (int j = 0; j < nv; j++) {
// d(force_j)/d(q_i)
mjtNum df = data_p->qfrc_passive[j] - data->qfrc_passive[j];
// K_true[j, i] = -df/eps
K_true[j * nv + i] = -df / eps;
}
mj_deleteData(data_p);
}
// 3. Compare and check for significant mismatch
mjtNum max_error = 0;
for (int i = 0; i < nv * nv; i++) {
max_error = mju_max(max_error, mju_abs(H_approx[i] - K_true[i]));
}
// We expect significant error because of deformation + rotation.
// The missing term (geometric stiffness) is proportional to stress.
// We assert that the error is relatively large to confirm the approximation
// exists.
EXPECT_GT(max_error, 1e-3)
<< "Jacobian approximation should differ from FD when deformed";
}
// Helper: assemble the standard-flex stretch stiffness into a dense matrix,
// column-by-column using mjd_flexStretch_mul with scale (s1 + s2*damping).
static void stretchK_dense(mjModel* m, mjData* d, mjtNum* K, int nv,
mjtNum s1, mjtNum s2) {
std::vector<mjtNum> e_i(nv, 0);
std::vector<mjtNum> col(nv, 0);
for (int i = 0; i < nv; i++) {
mju_zero(e_i.data(), nv);
mju_zero(col.data(), nv);
e_i[i] = 1.0;
mjd_flexStretch_mul(m, d, col.data(), e_i.data(), s1, s2);
for (int j = 0; j < nv; j++) {
K[j * nv + i] = col[j];
}
}
}
// The same frame mismatch seen through the derivative: mjd_flexBend_mul is the
// Jacobian of the bending force only if operator and force agree on the frame.
// An unrotated flex cannot catch it, because the stencil is one scalar per
// vertex pair applied coordinate-wise and so commutes with a rotation shared by
// every vertex. The pin also covers the zero-dof vertex path, where body_dofadr
// is negative and an unguarded stencil indexes out of bounds.
TEST_F(DerivativeTest, FlexBendDerivativesRotated) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicitfast" solver="CG"/>
<worldbody>
<body name="turned" euler="90 35 20">
<flexcomp name="rot" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1">
<pin id="0"/>
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="bend" damping="0"/>
</flexcomp>
</body>
<flexcomp name="flat" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
radius=".01" dim="2" mass="1" pos="1 0 0">
<pin id="0"/>
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" thickness="0.01"
elastic2d="bend" damping="0"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
ASSERT_EQ(model->nq, nv); // all slide dofs
ASSERT_EQ(model->nflex, 2);
MjDataPtr data = MakeData(model);
// deform both flexes out of plane so the bending stencil carries real force
for (int i = 0; i < nv; i++) {
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
std::vector<mjtNum> vec(nv), res(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 3) - 0.5;
}
mjd_flexBend_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
mjtNum eps = MjTol(1e-7, 1e-4);
mjData* perturbed = mj_copyData(NULL, model.get(), data.get());
mju_addToScl(perturbed->qpos, vec.data(), eps, nv);
mj_forward(model.get(), perturbed);
// check each flex on its own: the unrotated one is the control that isolates
// the rotation
for (int f = 0; f < model->nflex; f++) {
SCOPED_TRACE(model->names + model->name_flexadr[f]);
mjtNum max_err = 0, scale = 0;
for (int k = 0; k < model->flex_vertnum[f]; k++) {
int body = model->flex_vertbodyid[model->flex_vertadr[f] + k];
int adr = model->body_dofadr[body];
for (int x = 0; x < model->body_dofnum[body]; x++) {
mjtNum fd = -(perturbed->qfrc_passive[adr+x] -
data->qfrc_passive[adr+x]) / eps;
max_err = mju_max(max_err, mju_abs(res[adr+x] - fd));
scale = mju_max(scale, mju_abs(fd));
}
}
// bending is linear in position, so the difference is exact up to roundoff
EXPECT_GT(scale, 1e-3)
<< "test should exercise nontrivial bending stiffness";
EXPECT_LT(max_err, MjTol(1e-4, 1e-2) * scale)
<< "mjd_flexBend_mul is not the Jacobian of the flex bending force";
}
mj_deleteData(perturbed);
}
// K_stretch must be the full Hessian of the stretch force, not just its Gauss-Newton part: the
// geometric (stress-proportional) term is what makes it the Jacobian at finite strain. Uniformly
// dilating the mesh puts every edge in tension, so the tensile clamp is inactive and the operator
// is exact -- with only the Gauss-Newton term the finite-difference error is a large fraction of
// the force. FlexStretchDerivatives covers the near-rest limit, where the two agree anyway.
TEST_F(DerivativeTest, FlexStretchDerivativesTensile) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 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="stretch" damping="0"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
ASSERT_EQ(model->nq, nv); // all slide dofs
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
// dilate about the flex centroid: every edge stretches by 5%, so every Me is strictly positive
mjtNum centroid[3] = {0, 0, 0};
int nvert = model->flex_vertnum[0];
for (int v = 0; v < nvert; v++) {
mju_addTo3(centroid, data->flexvert_xpos + 3*v);
}
mju_scl3(centroid, centroid, 1.0/nvert);
for (int v = 0; v < nvert; v++) {
int body = model->flex_vertbodyid[model->flex_vertadr[0] + v];
int adr = model->body_dofadr[body];
for (int x = 0; x < 3; x++) {
data->qpos[adr+x] += 0.05*(data->flexvert_xpos[3*v+x] - centroid[x]);
}
}
mj_forward(model.get(), data.get());
std::vector<mjtNum> vec(nv), res(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 2) - 0.5;
}
mjd_flexStretch_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
mjtNum eps = MjTol(1e-7, 1e-4);
mjData* perturbed = mj_copyData(NULL, model.get(), data.get());
mju_addToScl(perturbed->qpos, vec.data(), eps, nv);
mj_forward(model.get(), perturbed);
mjtNum max_err = 0, scale = 0;
for (int i = 0; i < nv; ++i) {
mjtNum fd = -(perturbed->qfrc_passive[i] - data->qfrc_passive[i]) / eps;
max_err = mju_max(max_err, mju_abs(res[i] - fd));
scale = mju_max(scale, mju_abs(fd));
}
mj_deleteData(perturbed);
EXPECT_GT(scale, 1e-3) << "test should exercise nontrivial stretch stiffness";
EXPECT_LT(max_err, MjTol(1e-4, 1e-3) * scale)
<< "mjd_flexStretch_mul is not the Jacobian of the flex stretch force";
}
// verify mjd_flexStretch_mul (Gauss-Newton Hessian of the standard-flex
// stretch force) against finite differences of qfrc_passive, plus symmetry,
// positive semi-definiteness and (s1, s2) scale linearity. The model covers
// both element edge tables (dim=2 triangles and dim=3 tets) and a pinned
// vertex (zero-dof body guard).
TEST_F(DerivativeTest, FlexStretchDerivatives) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 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="stretch" damping="50"/>
<pin id="0"/>
</flexcomp>
<flexcomp name="solid" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="1 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="10"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
ASSERT_EQ(model->nq, nv); // all slide dofs
MjDataPtr data = MakeData(model);
// deform both flexes deterministically, at small strain. FlexStretchDerivativesTensile
// covers finite strain, where the geometric term of K_stretch is what carries the accuracy;
// the solid (dim=3) flex below has no such term, so the tolerance stays loose here.
for (int i = 0; i < nv; i++) {
data->qpos[i] += 5e-4 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
// part 1: FD verification of K*vec against qfrc_passive (qvel = 0, so the
// kD elongation term vanishes and qfrc_passive is the pure stretch spring)
{
std::vector<mjtNum> vec(nv), res(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 2) - 0.5;
}
mjd_flexStretch_mul(model.get(), data.get(), res.data(), vec.data(), 1, 0);
mjtNum eps = MjTol(1e-7, 1e-4);
mjData* data_perturbed = mj_copyData(NULL, model.get(), data.get());
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
mj_forward(model.get(), data_perturbed);
// qfrc_passive = -dV/dq => -(qfrc_new - qfrc)/eps ~= K * vec.
// Compare max error against the force scale rather than entrywise: individual near-zero
// entries are not meaningful, and the dim=3 flex still carries a Gauss-Newton residual.
mjtNum max_err = 0, scale = 0;
for (int i = 0; i < nv; ++i) {
mjtNum fd =
-(data_perturbed->qfrc_passive[i] - data->qfrc_passive[i]) / eps;
max_err = mju_max(max_err, mju_abs(res[i] - fd));
scale = mju_max(scale, mju_abs(fd));
}
EXPECT_GT(scale, 1.0) << "test should exercise nontrivial stiffness";
EXPECT_LT(max_err, MjTol(5e-3, 5e-2) * scale)
<< "stretch stiffness mismatch: max_err " << max_err
<< " at force scale " << scale;
mj_deleteData(data_perturbed);
}
// part 2: symmetry and positive semi-definiteness of the assembled K
{
std::vector<mjtNum> K(nv * nv, 0);
stretchK_dense(model.get(), data.get(), K.data(), nv, 1, 0);
mjtNum max_asymmetry = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < i; j++) {
max_asymmetry =
mju_max(max_asymmetry, mju_abs(K[i * nv + j] - K[j * nv + i]));
}
}
EXPECT_THAT(max_asymmetry, MjNear(0, 1e-10, 5e-4))
<< "K_stretch is not symmetric";
for (int trial = 0; trial < 5; trial++) {
std::vector<mjtNum> v(nv);
for (int i = 0; i < nv; i++) {
v[i] = mju_Halton(i + trial * nv, 3) - 0.5;
}
mjtNum vKv = 0;
for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) {
vKv += v[i] * K[i * nv + j] * v[j];
}
}
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5)) << "K_stretch is not PSD";
}
}
// part 3: (s1, s2) scale linearity across flexes with different damping:
// mul(s1, s2) == s1*mul(1, 0) + s2*mul(0, 1)
{
std::vector<mjtNum> vec(nv), a(nv, 0), b(nv, 0), c(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i, 5) - 0.5;
}
mjtNum h = 1e-3;
mjd_flexStretch_mul(model.get(), data.get(), a.data(), vec.data(),
h * h, h);
mjd_flexStretch_mul(model.get(), data.get(), b.data(), vec.data(), 1, 0);
mjd_flexStretch_mul(model.get(), data.get(), c.data(), vec.data(), 0, 1);
for (int i = 0; i < nv; i++) {
EXPECT_THAT(a[i], MjNear(h * h * b[i] + h * c[i], 1e-12, 1e-5))
<< "scale linearity mismatch at DOF " << i;
}
}
}
// verify mjd_flexStiff_assemble against the matrix-free operators: the
// assembled CSR applied to test vectors must reproduce mjd_flexBend_mul +
// mjd_flexStretch_mul at the same state
TEST_F(DerivativeTest, FlexStiffAssemble) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="4 4 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="7"/>
</flexcomp>
<flexcomp name="solid" type="grid" count="3 3 3" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="1 0 1">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="10"/>
<pin id="0"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
MjDataPtr data = MakeData(model);
// deform deterministically
for (int i = 0; i < nv; i++) {
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
// assemble both terms with a mixed (s1, s2) scale
mjtNum s1 = 4e-6, s2 = 2e-3;
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(),
rowadr.data(), NULL, NULL, s1, s2,
/*flg_bend=*/1, /*flg_stretch=*/1, /*flg_contact=*/0, NULL);
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/1,
/*flg_stretch=*/1, /*flg_contact=*/0, NULL);
// compare CSR apply vs operators on test vectors
for (int trial = 0; trial < 3; trial++) {
std::vector<mjtNum> vec(nv), res_op(nv, 0), res_csr(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i + trial*nv, 3) - 0.5;
}
mjd_flexBend_mul(model.get(), data.get(), res_op.data(), vec.data(), s1,
s2);
mjd_flexStretch_mul(model.get(), data.get(), res_op.data(), vec.data(), s1,
s2);
for (int i = 0; i < nv; i++) {
mjtNum sum = 0;
for (int k = 0; k < rownnz[i]; k++) {
sum += val[rowadr[i] + k]*vec[colind[rowadr[i] + k]];
}
res_csr[i] = sum;
}
for (int i = 0; i < nv; i++) {
EXPECT_THAT(res_csr[i], MjNear(res_op[i], 1e-12, 2e-5))
<< "assembly/operator mismatch at DOF " << i << " trial " << trial;
}
}
}
// verify the interp assembly mode: with the K_rot cache supplied, the assembled CSR applied
// to test vectors must reproduce mjd_flexInterp_mul, whose sign convention is negated
TEST_F(DerivativeTest, FlexStiffAssembleInterp) {
static const char* const kXml = R"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<flexcomp name="soft" type="grid" count="4 4 4" spacing="0.1 0.1 0.1"
radius=".01" dim="3" mass="1" pos="0 0 1" dof="trilinear">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e4" poisson="0.3" damping="2"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
MjDataPtr data = MakeData(model);
ASSERT_EQ(mjd_flexInterpAssemblable(model.get()), 1);
// deform deterministically, refresh kinematics, cache the corotated stiffness
for (int i = 0; i < nv; i++) {
data->qpos[i] += 2e-3 * (mju_Halton(i, 2) - 0.5);
}
mj_forward(model.get(), data.get());
std::vector<mjtNum> krot(model->nflexstiffness, 0);
mjd_flexInterp_cacheKrot(model.get(), data.get(), krot.data());
// assemble interp only
mjtNum s1 = 4e-6, s2 = 2e-3;
std::vector<int> rownnz(nv), rowadr(nv);
int nnz = mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
NULL, NULL, s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0,
krot.data());
ASSERT_GT(nnz, 0);
std::vector<int> colind(nnz);
std::vector<mjtNum> val(nnz);
mjd_flexStiff_assemble(model.get(), data.get(), rownnz.data(), rowadr.data(),
colind.data(), val.data(), s1, s2, /*flg_bend=*/0, /*flg_stretch=*/0, /*flg_contact=*/0,
krot.data());
// compare CSR apply vs the operator called with negated scales (its convention)
for (int trial = 0; trial < 3; trial++) {
std::vector<mjtNum> vec(nv), res_op(nv, 0), res_csr(nv, 0);
for (int i = 0; i < nv; i++) {
vec[i] = mju_Halton(i + trial*nv, 3) - 0.5;
}
mjd_flexInterp_mul(model.get(), data.get(), res_op.data(), vec.data(), -s1, -s2,
krot.data());
for (int i = 0; i < nv; i++) {
mjtNum sum = 0;
for (int k = 0; k < rownnz[i]; k++) {
sum += val[rowadr[i] + k]*vec[colind[rowadr[i] + k]];
}
res_csr[i] = sum;
}
for (int i = 0; i < nv; i++) {
EXPECT_THAT(res_csr[i], MjNear(res_op[i], 1e-12, 2e-5))
<< "interp assembly/operator mismatch at DOF " << i << " trial " << trial;
}
}
}
// mjd_effSolve drives (M+K)x = b to opt.tolerance on the relative residual,
// for every metric coverage case. It is a PCG whose preconditioner
// (mjd_effPrec) is only approximate, so the accuracy comes from the iteration
// and not from the preconditioner being exact.
TEST_F(DerivativeTest, EffSolve) {
// relative residual of (M+K)x - b after mjd_effSolve
auto solve_residual = [](const mjModel* m, mjData* d) {
int nv = m->nv;
std::vector<mjtNum> b(nv), x(nv), r(nv);
for (int i = 0; i < nv; i++) {
b[i] = mju_Halton(i, 3) - 0.5;
}
mjd_effSolve(m, d, x.data(), b.data());
mju_mulSymVecSparse(r.data(), d->M, x.data(), nv, m->M_rownnz, m->M_rowadr, m->M_colind);
mjd_effMulAdd(m, d, r.data(), x.data());
mju_subFrom(r.data(), b.data(), nv);
return mju_norm(r.data(), nv) / mju_norm(b.data(), nv);
};
// stretch + bending cloth on world: per-step factor, exact
static const char* const kXmlBoth = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.05 0.05 0.05"
radius=".005" dim="2" mass="0.5" pos="0 0 1" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both" thickness="0.01"/>
</flexcomp>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kXmlBoth, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
EXPECT_GT(data->nefmK, 0);
EXPECT_GT(data->nefmdof, 0);
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-8, 1e-4));
// bending-only cloth: no CSR or per-step factor, constant factor covers, exact
static const char* const kXmlBend = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.05 0.05 0.05"
radius=".005" dim="2" mass="0.5" pos="0 0 1" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="bend" thickness="0.01"/>
</flexcomp>
</worldbody>
</mujoco>
)";
model = LoadModelFromString(kXmlBend, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
EXPECT_EQ(data->nefmK, 0);
EXPECT_EQ(data->nefmdof, 0);
EXPECT_GT(model->nefm0dof, 0);
EXPECT_LT(solve_residual(model.get(), data.get()), MjTol(1e-8, 1e-4));
// cloth under a jointed parent: M couples across the covered block, not exact,
// the refinement path must still meet its tolerance
static const char* const kXmlMoving = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<body name="base" pos="0 0 1">
<joint type="slide" axis="0 0 1"/>
<geom type="sphere" size=".01" mass="1" contype="0" conaffinity="0"/>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.05 0.05 0.05"
radius=".005" dim="2" mass="0.5" pos="0 0 0" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both" thickness="0.01"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
model = LoadModelFromString(kXmlMoving, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
EXPECT_LT(solve_residual(model.get(), data.get()), 1e-4);
}
// A cloth with per-step stretch stiffness, used by the two tests below.
static const char* const kStretchCloth = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<body name="base" pos="0 0 1">
<joint type="slide" axis="0 0 1"/>
<geom type="sphere" size=".01" mass="1" contype="0" conaffinity="0"/>
<flexcomp name="cloth" type="grid" count="6 6 1" spacing="0.05 0.05 0.05"
radius=".005" dim="2" mass="0.5" pos="0 0 0" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e3" poisson="0.2" damping="0.1" elastic2d="both"
thickness="0.01"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
// A metric too ill-conditioned for the 3x3 blocks exhausts mjd_effSolve's
// iteration budget; it must report that rather than return an under-converged
// qacc_smooth silently. Forced by conditioning rather than by an unreachable
// opt.tolerance, which cannot be expressed in single precision: there the
// residual reaches exactly zero and CG breaks down on a converged solve.
TEST_F(DerivativeTest, EffSolveCapWarns) {
static const char* const kStiffCloth = R"(
<mujoco>
<option solver="CG" integrator="implicitfast"/>
<worldbody>
<body name="base" pos="0 0 1">
<joint type="slide" axis="0 0 1"/>
<geom type="sphere" size=".01" mass="1" contype="0" conaffinity="0"/>
<flexcomp name="cloth" type="grid" count="24 24 1" dim="2"
spacing="0.02 0.02 0.02" radius=".002" mass="0.02" dof="full">
<contact selfcollide="none" contype="0" conaffinity="0"/>
<elasticity young="1e9" poisson="0.45" damping="0" elastic2d="both"
thickness="0.02"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(kStiffCloth, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
MockWarningHandler warning_handler;
// both: the specific cause, then the mjWARN_INERTIA it is reported through
warning_handler.ExpectWarnings("Flex stiffness is too ill-conditioned");
warning_handler.ExpectWarnings("Inertia matrix is too close to singular");
mj_forward(model.get(), data.get());
testing::Mock::VerifyAndClearExpectations(&warning_handler);
}
// PCG requires a symmetric preconditioner. mjd_effPrec must satisfy
// u.P(v) == v.P(u); it did not when the covered and uncovered dofs shared a
// kinematic tree, which is what the flex-under-a-slider model here exercises.
TEST_F(DerivativeTest, EffPrecIsSymmetric) {
char error[1024];
MjModelPtr model = LoadModelFromString(kStretchCloth, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_GE(data->efm_active, 1);
int nv = model->nv;
std::vector<mjtNum> u(nv), v(nv), Pu(nv), Pv(nv);
for (int trial = 0; trial < 5; trial++) {
for (int i = 0; i < nv; i++) {
u[i] = mju_Halton(i + trial*nv, 2) - 0.5;
v[i] = mju_Halton(i + trial*nv, 5) - 0.5;
}
mjd_effPrec(model.get(), data.get(), Pu.data(), u.data());
mjd_effPrec(model.get(), data.get(), Pv.data(), v.data());
mjtNum a = mju_dot(v.data(), Pu.data(), nv);
mjtNum b = mju_dot(u.data(), Pv.data(), nv);
EXPECT_THAT(a, MjNear(b, 1e-10, 1e-4))
<< "preconditioner is not symmetric, trial " << trial;
}
}
} // namespace
} // namespace mujoco
Reference in New Issue View Git Blame Copy Permalink