Add implicit integrator.
Added analytic derivatives of smooth (unconstrained) dynamics forces, with respect to velocities: - Centripetal and Coriolis forces computed by the Recursive Newton-Euler algorithm. - Damping and fluid-drag passive forces. - Actuation forces. A new implicit-in-velocity integrator is implemented using the analytic derivatives. This integrator lies between the Euler and Runge Kutta integrators in terms of both stability and computational cost. PiperOrigin-RevId: 450377010 Change-Id: Ie192b441876c22e732fb749333926f296e0a09cc
This commit is contained in:
DeepMind
committed by
Copybara-Service
Copybara-Service
parent
1913a02b40
commit
64bc6d27b2
@@ -21,6 +21,9 @@ target_link_libraries(engine_collision_driver_test fixture gmock)
|
||||
mujoco_test(engine_core_smooth_test)
|
||||
target_link_libraries(engine_core_smooth_test fixture gmock)
|
||||
|
||||
mujoco_test(engine_derivative_test)
|
||||
target_link_libraries(engine_derivative_test fixture gmock)
|
||||
|
||||
mujoco_test(engine_forward_test)
|
||||
target_link_libraries(engine_forward_test fixture gmock)
|
||||
|
||||
|
||||
@@ -0,0 +1,164 @@
|
||||
// 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 <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_derivative.h"
|
||||
#include "src/engine/engine_io.h"
|
||||
#include "src/engine/engine_support.h"
|
||||
#include "src/engine/engine_util_blas.h"
|
||||
#include "src/engine/engine_util_errmem.h"
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::DoubleNear;
|
||||
using DerivativeTest = MujocoTest;
|
||||
|
||||
// errors smaller than this are ignored
|
||||
static const mjtNum absolute_tolerance = 1e-7;
|
||||
|
||||
// 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
|
||||
static void CompareMatrices(mjtNum* Actual, mjtNum* Expected,
|
||||
int nrow, int ncol, mjtNum eps) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
|
||||
return std::vector<mjtNum>(array, array + n);
|
||||
}
|
||||
|
||||
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 kDampedActuatorsPath =
|
||||
"engine/testdata/derivative/damped_actuators.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}) {
|
||||
const std::string xml_path = GetTestDataFilePath(local_path);
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
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);
|
||||
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
|
||||
mj_makeMSparse(model, data,
|
||||
data->D_rownnz, data->D_rowadr, data->D_colind);
|
||||
mjd_smooth_vel(model, data);
|
||||
|
||||
// expect derivatives to be non-zero, make copy of qDeriv as a vector
|
||||
EXPECT_GT(mju_norm(data->qDeriv, model->nD), 0);
|
||||
std::vector<mjtNum> qDerivAnalytic = AsVector(data->qDeriv, model->nD);
|
||||
|
||||
// compute finite-difference derivatives
|
||||
mjtNum eps = 1e-7;
|
||||
mjd_smooth_velFD(model, data, eps);
|
||||
|
||||
// expect FD and analytic derivatives to be numerically different
|
||||
EXPECT_NE(mju_norm(data->qDeriv, model->nD),
|
||||
mju_norm(qDerivAnalytic.data(), model->nD));
|
||||
|
||||
// expect FD and analytic derivatives to be similar to eps precision
|
||||
EXPECT_THAT(AsVector(data->qDeriv, model->nD),
|
||||
Pointwise(DoubleNear(eps), qDerivAnalytic));
|
||||
}
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
}
|
||||
|
||||
// compare analytic and fin-diff d_qfrc_passive/d_qvel
|
||||
TEST_F(DerivativeTest, PassiveDvel) {
|
||||
const std::string xml_path = GetTestDataFilePath(kTumblingThinObjectPath);
|
||||
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
|
||||
int nv = model->nv;
|
||||
mjData* data = mj_makeData(model);
|
||||
// allocate d_qfrc_passive/d_qvel Jacobians
|
||||
mjtNum* DfDv_analytic = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
|
||||
mjtNum* DfDv_FD = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv*nv);
|
||||
|
||||
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);
|
||||
|
||||
// clear DfDv, get analytic derivatives
|
||||
mju_zero(DfDv_analytic, nv*nv);
|
||||
mjd_passive_vel(model, data, DfDv_analytic);
|
||||
|
||||
// clear DfDv, get finite-difference derivatives
|
||||
mju_zero(DfDv_FD, nv*nv);
|
||||
mjtNum eps = 1e-6;
|
||||
mjd_passive_velFD(model, data, eps, DfDv_FD);
|
||||
|
||||
// expect FD and analytic derivatives to be similar to eps precision
|
||||
CompareMatrices(DfDv_analytic, DfDv_FD, nv, nv, eps);
|
||||
}
|
||||
|
||||
mju_free(DfDv_FD);
|
||||
mju_free(DfDv_analytic);
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
@@ -26,8 +26,22 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
|
||||
return std::vector<mjtNum>(array, array + n);
|
||||
}
|
||||
|
||||
static const char* const kEnergyConservingPendulumPath =
|
||||
"engine/testdata/derivative/energy_conserving_pendulum.xml";
|
||||
static const char* const kDampedActuatorsPath =
|
||||
"engine/testdata/derivative/damped_actuators.xml";
|
||||
|
||||
using ::testing::Pointwise;
|
||||
using ::testing::DoubleNear;
|
||||
using ::testing::Ne;
|
||||
using ForwardTest = MujocoTest;
|
||||
|
||||
// --------------------------- activation limits -------------------------------
|
||||
|
||||
TEST_F(ForwardTest, ActLimited) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
@@ -75,6 +89,129 @@ TEST_F(ForwardTest, ActLimited) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// --------------------------- implicit integrator -----------------------------
|
||||
|
||||
using ImplicitIntegratorTest = MujocoTest;
|
||||
|
||||
// Euler and implicit should be equivalent if there is only joint damping
|
||||
TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
|
||||
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>
|
||||
)";
|
||||
|
||||
mjModel* model = LoadModelFromString(xml);
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
// step 10 times with Euler, save copy of qpos as vector
|
||||
for (int i=0; i<10; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
|
||||
|
||||
// reset, step 10 times with implicit
|
||||
mj_resetData(model, data);
|
||||
model->opt.integrator = mjINT_IMPLICIT;
|
||||
for (int i=0; i<10; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// expect qpos vectors to be numerically different
|
||||
EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(Ne(), qposEuler));
|
||||
|
||||
// expect qpos vectors to be similar to high precision
|
||||
EXPECT_THAT(AsVector(data->qpos, model->nq),
|
||||
Pointwise(DoubleNear(1e-14), qposEuler));
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// Joint and actuator damping should integrate identically under implicit
|
||||
TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
|
||||
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
|
||||
EXPECT_GT(fabs(data->qpos[0]-data->qpos[2]), 1e-4);
|
||||
EXPECT_GT(fabs(data->qpos[1]-data->qpos[3]), 1e-4);
|
||||
|
||||
// reset, take 1000 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]), 1e-16);
|
||||
EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), 1e-16);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
<body name="damping in the joints">
|
||||
<joint type="slide" axis="0 0 1" damping="10"/>
|
||||
<geom size=".03"/>
|
||||
<body>
|
||||
<joint axis="0 1 0" damping=".1"/>
|
||||
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
<body name="damping in the actuators" pos="0 0.1 0">
|
||||
<joint name="slide" type="slide" axis="0 0 1"/>
|
||||
<geom size=".03"/>
|
||||
<body>
|
||||
<joint name="hinge" axis="0 1 0"/>
|
||||
<geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<general joint="slide" biastype="affine" biasprm="0 0 -10"/>
|
||||
<general joint="hinge" biastype="affine" biasprm="0 0 -0.1"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,25 @@
|
||||
<mujoco>
|
||||
<option integrator="implicit">
|
||||
<flag constraint="disable" energy="enable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<light pos="0 0 1"/>
|
||||
<geom type="plane" size="1 1 .01" pos="0 0 -1"/>
|
||||
<body pos="0.15 0 0">
|
||||
<joint type="hinge" axis="0 1 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 .1 0 0"/>
|
||||
<body pos="0.1 0 0">
|
||||
<joint type="slide" axis="1 0 0" stiffness="200"/>
|
||||
<geom type="capsule" size="0.015" fromto="-.1 0 0 .1 0 0"/>
|
||||
<body pos=".1 0 0">
|
||||
<joint type="ball"/>
|
||||
<geom type="box" size=".02" fromto="0 0 0 0 .1 0"/>
|
||||
<body pos="0 .1 0">
|
||||
<joint axis="1 0 0"/>
|
||||
<geom type="capsule" size="0.02" fromto="0 0 0 0 .1 0"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,17 @@
|
||||
<mujoco>
|
||||
<option density="1.225" viscosity="1.8e-5" wind="0 0 1" integrator="implicit"/>
|
||||
|
||||
<worldbody>
|
||||
<light pos="0 0 1"/>
|
||||
<geom type="plane" size="1 1 .01" pos="0 0 -1"/>
|
||||
<body>
|
||||
<freejoint/>
|
||||
<body>
|
||||
<geom type="box" size=".025 .01 0.0001" pos=".025 0 0" euler="20 0 0" mass="1e-4"/>
|
||||
</body>
|
||||
<body>
|
||||
<geom type="box" size=".025 .01 0.0001" pos="-.025 0 0" euler="-19 0 0" mass="1e-4"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
Reference in New Issue
Block a user