chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add implicit integrator.

Browse Source 
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
2022-05-23 01:21:24 -07:00
committed by Copybara-Service
parent 1913a02b40
commit 64bc6d27b2
30 changed files with 1974 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+19 -9
View File
@@ -29,7 +29,18 @@ Open Sourcing
General
^^^^^^^
3. Added :at:`actlimited` and :at:`actrange` attributes to :ref:`general actuators<general>`, for clamping actuator
3. 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.
#. Added ``implicit`` integrator. Using the analytic derivatives above, a new implicit-in-velocity integrator was added.
This integrator lies between the Euler and Runge Kutta integrators in terms of both stability and computational
cost. It is most useful for models which use fluid drag (e.g. for flying or swimming) and for models which use
:ref:`velocity actuators<velocity>`. For more details, see the :ref:`Numerical Integration<geIntegration>` section.
#. Added :at:`actlimited` and :at:`actrange` attributes to :ref:`general actuators<general>`, for clamping actuator
internal states (activations). This clamping is useful for integrated-velocity actuators, see the :ref:`Activation
clamping <CActRange>` section for details.
@@ -47,16 +58,15 @@ General
Bug fixes
^^^^^^^^^
10. Antialiasing was disabled for segmentation rendering. Before this change, if the :ref:`offsamples<quality>`
attribute was greater than 0 (the default value is 4), pixels that overlapped with multiple geoms would receive
averaged segmentation IDs, leading to incorrect or non-existant IDs. After this change :at:`offsamples` is ignored
during segmentation rendering.
8. Antialiasing was disabled for segmentation rendering. Before this change, if the :ref:`offsamples<quality>`
attribute was greater than 0 (the default value is 4), pixels that overlapped with multiple geoms would receive
averaged segmentation IDs, leading to incorrect or non-existant IDs. After this change :at:`offsamples` is ignored
during segmentation rendering.
#. The value of the enable flag for the experimental multiCCD feature was made sequential with other enable flags.
Sequentiality is assumed in the ``simulate`` UI and elsewhere.
#. The value of the enable flag for the experimental multiCCD feature was made sequential with other enable flags.
Sequentiality is assumed in the ``simulate`` UI and elsewhere.
#. Fix issue of duplicated meshes when saving models with OBJ meshes using mj_saveLastXML.
#. Fix issue of duplicated meshes when saving models with OBJ meshes using mj_saveLastXML.
Version 2.1.5 (Apr. 13, 2022)
doc/computation.rst
+100 -26
View File
@@ -385,53 +385,127 @@ Numerical integration
~~~~~~~~~~~~~~~~~~~~~
MuJoCo computes forward and inverse dynamics in continuous time. The end result of forward dynamics is the joint
acceleration :math:`\dot{v}` as well as the actuator activations :math:`\dot{w}` when present in the model. These are
acceleration :math:`a=\dot{v}` as well as the actuator activations :math:`\dot{w}` when present in the model. These are
used to advance the simulation time from :math:`t` to :math:`t+h`, and to update the state variables :math:`q, v, w`.
Two numerical integrators are currently available:
Three numerical integrators are available:
Semi-implicit Euler method (Euler)
This method updates the activation and velocity with the usual Euler method, however the position is updated using
the new velocity:
the *new* velocity; this is known as a "semi-implicit" update:
.. math::
\begin{aligned}
w(t+h) &= w(t) + h \dot{w}(t) \\
v(t+h) &= v(t) + h \dot{v}(t) \\
q(t+h) &= q(t) + h v(t+h)
\textrm{activation: }w_{t+h} &= w_t + h \dot{w}_t \\
\textrm{velocity: }v_{t+h} &= v_t + h a_t \\
\textrm{position: }q_{t+h} &= q_t + h v_{t+h}
\end{aligned}
Using the new velocity in the position update improves stability, and is standard in physics engines. The summation
in the position update generally involves vectors with different dimensionality, and is done by taking into account
the properties of quaternions. When joint damping is defined in the model, the Euler method automatically uses
implicit damping integration as follows. Consider the first-order Taylor expansion
Using the new velocity in the position update improves stability, and is standard in physics simulation. The
summation in the position update generally involves vectors with different dimensionality, and is done by taking into
account the properties of quaternions.
When joint damping is defined in the model, the Euler method applies a correction to the inertia matrix that
corresponds to implicit integration of damping forces. Let :math:`B` be the diagonal matrix of negative joint damping
coefficients. Letting :math:`\widehat{M} = M-h B`, we then update the velocity using a corrected acceleration as
follows:
.. math::
\tau(v(t+h)) = \tau(v(t)) + h {\partial\tau \over \partial v} \dot{v} + o \left( h^2 \right)
\begin{aligned}
v_{t+h} &= v_t + h \widehat{M}^{-1} M a_t
\end{aligned}
Moving the acceleration term to the left hand side of the equations of motion :eq:`eq:motion`, the effective inertia
matrix becomes
This correction is temporary, and the original acceleration computed by forward dynamics and saved in ``mjData.qacc``
is not modified. We explain below how this correction is derived, and why it is a special case of implicit
integration.
Implicit-in-velocity Euler method (implicit)
This method approximates the following discrete-time update:
.. math::
\hat{M} = M - h {\partial\tau \over \partial v}
\begin{aligned}
w_{t+h} &= w_t + h \dot{w}_t \\
v_{t+h} &= v_t + h a_{t+h} \\
q_{t+h} &= q_t + h v_{t+h}
\end{aligned}
We use this matrix to correct the acceleration term in the Euler update. The correction however is temporary, and the
original acceleration computed by forward dynamics and saved in ``mjData.qacc`` is not modified. In principle this
approach could be applied to any velocity-dependent force, however joint damping has the advantage that it does not
affect the sparsity structure of the inertia matrix and is trivial to compute - which is why it is the only
velocity-dependent force that we currently integrate implicitly.
Note the acceleration :math:`a_{t+h}=\dot{v}_{t+h}` on the right hand side of the velocity update is evaluated at
:math:`t+h`, making the integrator fully implicit in velocity. When applied to Hamiltonian systems, such integrators
are called symplectic, and there is a large mathematical literature on them. Hamiltonian systems have the property
that certain quantities (symplectic forms) remain constant over time in the true continuous-time system. Symplectic
integrators have the unique property that they preserve the same quantities exactly, despite using a discrete-time
approximation. Few MuJoCo models used in practice correspond to Hamiltonian systems; these are systems without
activation dynamics, contacts, friction, driving forces etc. Nevertheless this type of integrator has appealing
properties in terms of accuracy and stability, achieved at the expense of added computation. It is particularly
effective in systems where instabilities (of the regular Euler integrator) are caused by velocity-dependent forces:
multi-joint pendulums, bodies tumbling through space, systems with substantial lift and drag forces, systems with
substantial damping in tendons and actuators (as well as joints, but joint damping is already handled implicitly in
the regular Euler integrator).
Writing the acceleration (i.e. the forward dynamics) more explicitly as a function of velocity: :math:`a_t=\dot
{v}_t = a(v_t)`, the velocity update we aim to approximate is
.. math:: v_{t+h} = v_t + h a(v_{t+h})
This is a non-linear equation in the unknown vector :math:`v_{t+h}`. It must be solved numerically at each time step
in order to implement an implicit integrator. We approximate the solution using a single step of Newton's method,
based on the first-order Taylor series expansion of :math:`a(v_{t+h})` around :math:`v_t`. Recall that the forward
dynamics are
.. math:: a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big)
Here we will ignore the velocity dependence of the constraint forces :math:`J^T f(v)`, because differentiating them
is complicated, and furthermore MuJoCo uses soft constraints whose integration is very stable even without implicit
integration. Thus we define the approximate derivative
.. math::
\begin{aligned}
{\partial a(v) \over \partial v} &\approx M^{-1} D(v) \\
D(v) &= {\partial\big(\tau(v) - c (v)\big) \over \partial v}
\end{aligned}
MuJoCo computes :math:`D(v)` analytically, by differentating the Recursive Newton-Euler algorithm as well as the code
that computes applied and bias forces. A further approximation is that we restrict :math:`D` to have the same
sparsity pattern as :math:`M`, for computational efficiency (see below). This restriction will exclude damping in
tendons which connect bodies that are on different branches of the kinematic tree. The velocity update corresponding
to Newton's method is as follows. First, we expand the right hand side to first order
.. math::
\begin{aligned}
v_{t+h} &= v_t + h a(v_{t+h}) \\
&= v_t + h a(v_t + v_{t+h}-v_t) \\
&\approx v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t)
\end{aligned}
Premultiplying by :math:`M` and rearranging yields
.. math:: (M-h D) v_{t+h} = (M-h D) v_t + h M a(v_t)
Now letting :math:`\widehat{M} = M-h D`, we obtain the implicit update
.. math:: v_{t+h} = v_t + h \widehat{M}^{-1} M a(v_t)
Comparing to the regular Euler method with damping correction, we see that the matrix :math:`B` in the regular Euler
method corresponds to :math:`D` used here, but restricted to joint damping. Since :math:`D` and :math:`M` have the
same sparsity pattern corresponding to the topology of the kinematic tree, reverse-order LU factorization of
:math:`\widehat{M}` is guaranteed to have no fill-in, which is the computational speed-up mentioned above. This
factorization is stored ``mjData.qLU``. It is now also clear why the Euler method uses :math:`B` rather than
:math:`D`: since :math:`B` is diagonal, :math:`\widehat{M}` remains symmetric and can be factorized with Cholesky
rather than LU.
4th-order Runge-Kutta method (RK4)
One advantage of our continuous-time formulation is that we can use more advanced integrators such as Runge-Kutta or
multistep methods. The only such integrator currently implemented is the fixed-step 4th-order Runge-Kutta method. We
have observed that for energy-conserving systems it is qualitatively better than the Euler method, both in terms of
stability and accuracy, despite the fact that it performs 4 mini-updates per step. In the presence of contacts we
have not observed significant benefits, although a more systematic investigation remains to be performed.
stability and accuracy, even when the timestep of the Euler method is decreased by a factor of 4 (so the
computational effort is identical). In the presence of contacts we have not observed significant benefits, although a
more systematic investigation remains to be performed.
The accuracy and stability of both integrators can be improved by reducing the time step :math:`h` which is stored in
``mjModel.opt.timestep``. Of course this also slows down the simulation. The time step is perhaps the most important
parameter that the user can adjust. If it is too large, the simulation will become unstable. If it is too small, CPU
time will be wasted without meaningful improvement in accuracy. There is always a comfortable range where the time step
is "just right", but that range is model-dependent.
.. note::
The accuracy and stability of all integrators can be improved by reducing the time step :math:`h` which is stored in
``mjModel.opt.timestep``. Of course this also slows down the simulation. The time step is perhaps the most important
parameter that the user can adjust. If it is too large, the simulation will become unstable. If it is too small, CPU
time will be wasted without meaningful improvement in accuracy. There is always a comfortable range where the time
step is "just right", but that range is model-dependent.
.. _Constraint:
include/mujoco/mjdata.h
+13 -2
View File
@@ -225,10 +225,10 @@ struct mjData_ {
// computed by mj_fwdPosition/mj_crb
mjtNum* crb; // com-based composite inertia and mass (nbody x 10)
mjtNum* qM; // total inertia (nM x 1)
mjtNum* qM; // total inertia (sparse) (nM x 1)
// computed by mj_fwdPosition/mj_factorM
mjtNum* qLD; // L'*D*L factorization of M (nM x 1)
mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1)
mjtNum* qLDiagInv; // 1/diag(D) (nv x 1)
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
@@ -286,6 +286,17 @@ struct mjData_ {
mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3)
mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3)
// computed by mj_implicit
int* D_rownnz; // non-zeros in each row (nv x 1)
int* D_rowadr; // address of each row in D_colind (nv x 1)
int* D_colind; // column indices of non-zeros (nD x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
// computed by mj_implicit/mju_factorLUSparse
mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1)
//-------------------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent
// computed by mj_fwdActuation
include/mujoco/mjmodel.h
+3 -1
View File
@@ -126,7 +126,8 @@ typedef enum mjtTexture_ { // type of texture
typedef enum mjtIntegrator_ { // integrator mode
mjINT_EULER = 0, // semi-implicit Euler
mjINT_RK4 // 4th-order Runge Kutta
mjINT_RK4, // 4th-order Runge Kutta
mjINT_IMPLICIT // implicit in velocity
} mjtIntegrator;
@@ -568,6 +569,7 @@ struct mjModel_ {
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse derivative matrix
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
include/mujoco/mjxmacro.h
+6
View File
@@ -112,6 +112,7 @@
X( nuser_sensor ) \
X( nnames ) \
X( nM ) \
X( nD ) \
X( nemax ) \
X( njmax ) \
X( nconmax ) \
@@ -508,6 +509,11 @@
X( mjtNum, efc_aref, njmax, 1 ) \
X( mjtNum, subtree_linvel, nbody, 3 ) \
X( mjtNum, subtree_angmom, nbody, 3 ) \
X( int, D_rownnz, nv, 1 ) \
X( int, D_rowadr, nv, 1 ) \
X( int, D_colind, nD, 1 ) \
X( mjtNum, qDeriv, nD, 1 ) \
X( mjtNum, qLU, nD, 1 ) \
X( mjtNum, actuator_force, nu, 1 ) \
X( mjtNum, qfrc_actuator, nv, 1 ) \
X( mjtNum, qfrc_smooth, nv, 1 ) \
introspect/enums.py
+1
View File
@@ -118,6 +118,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
values=dict([
('mjINT_EULER', 0),
('mjINT_RK4', 1),
('mjINT_IMPLICIT', 2),
]),
)),
('mjtCollision',
sample/simulate.cc
+1 -1
View File
@@ -668,7 +668,7 @@ void makephysics(int oldstate) {
mjuiDef defPhysics[] = {
{mjITEM_SECTION, "Physics", oldstate, NULL, "AP"},
{mjITEM_SELECT, "Integrator", 2, &(m->opt.integrator), "Euler\nRK4"},
{mjITEM_SELECT, "Integrator", 2, &(m->opt.integrator), "Euler\nRK4\nimplicit"},
{mjITEM_SELECT, "Collision", 2, &(m->opt.collision), "All\nPair\nDynamic"},
{mjITEM_SELECT, "Cone", 2, &(m->opt.cone), "Pyramidal\nElliptic"},
{mjITEM_SELECT, "Jacobian", 2, &(m->opt.jacobian), "Dense\nSparse\nAuto"},
src/engine/CMakeLists.txt
+2
View File
@@ -28,6 +28,8 @@ set(MUJOCO_ENGINE_SRCS
engine_core_smooth.c
engine_core_smooth.h
engine_crossplatform.h
engine_derivative.c
engine_derivative.h
engine_file.c
engine_file.h
engine_forward.c
src/engine/engine_derivative.c
+864
View File
@@ -0,0 +1,864 @@
// 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.
#include "engine/engine_derivative.h"
#include <stddef.h>
#include <string.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_core_smooth.h"
#include "engine/engine_forward.h"
#include "engine/engine_callback.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#include "engine/engine_util_spatial.h"
//------------------------- derivatives of spatial algebra -----------------------------------------
// derivative of mju_crossMotion w.r.t velocity
static void mjd_crossMotion_vel(mjtNum D[36], const mjtNum v[6])
{
mju_zero(D, 36);
// res[0] = -vel[2]*v[1] + vel[1]*v[2]
D[0 + 2] = -v[1];
D[0 + 1] = v[2];
// res[1] = vel[2]*v[0] - vel[0]*v[2]
D[6 + 2] = v[0];
D[6 + 0] = -v[2];
// res[2] = -vel[1]*v[0] + vel[0]*v[1]
D[12 + 1] = -v[0];
D[12 + 0] = v[1];
// res[3] = -vel[2]*v[4] + vel[1]*v[5] - vel[5]*v[1] + vel[4]*v[2]
D[18 + 2] = -v[4];
D[18 + 1] = v[5];
D[18 + 5] = -v[1];
D[18 + 4] = v[2];
// res[4] = vel[2]*v[3] - vel[0]*v[5] + vel[5]*v[0] - vel[3]*v[2]
D[24 + 2] = v[3];
D[24 + 0] = -v[5];
D[24 + 5] = v[0];
D[24 + 3] = -v[2];
// res[5] = -vel[1]*v[3] + vel[0]*v[4] - vel[4]*v[0] + vel[3]*v[1]
D[30 + 1] = -v[3];
D[30 + 0] = v[4];
D[30 + 4] = -v[0];
D[30 + 3] = v[1];
}
// derivative of mju_crossForce w.r.t. velocity
static void mjd_crossForce_vel(mjtNum D[36], const mjtNum f[6])
{
mju_zero(D, 36);
// res[0] = -vel[2]*f[1] + vel[1]*f[2] - vel[5]*f[4] + vel[4]*f[5]
D[0 + 2] = -f[1];
D[0 + 1] = f[2];
D[0 + 5] = -f[4];
D[0 + 4] = f[5];
// res[1] = vel[2]*f[0] - vel[0]*f[2] + vel[5]*f[3] - vel[3]*f[5]
D[6 + 2] = f[0];
D[6 + 0] = -f[2];
D[6 + 5] = f[3];
D[6 + 3] = -f[5];
// res[2] = -vel[1]*f[0] + vel[0]*f[1] - vel[4]*f[3] + vel[3]*f[4]
D[12 + 1] = -f[0];
D[12 + 0] = f[1];
D[12 + 4] = -f[3];
D[12 + 3] = f[4];
// res[3] = -vel[2]*f[4] + vel[1]*f[5]
D[18 + 2] = -f[4];
D[18 + 1] = f[5];
// res[4] = vel[2]*f[3] - vel[0]*f[5]
D[24 + 2] = f[3];
D[24 + 0] = -f[5];
// res[5] = -vel[1]*f[3] + vel[0]*f[4]
D[30 + 1] = -f[3];
D[30 + 0] = f[4];
}
// derivative of mju_crossForce w.r.t. force
static void mjd_crossForce_frc(mjtNum D[36], const mjtNum vel[6])
{
mju_zero(D, 36);
// res[0] = -vel[2]*f[1] + vel[1]*f[2] - vel[5]*f[4] + vel[4]*f[5]
D[0 + 1] = -vel[2];
D[0 + 2] = vel[1];
D[0 + 4] = -vel[5];
D[0 + 5] = vel[4];
// res[1] = vel[2]*f[0] - vel[0]*f[2] + vel[5]*f[3] - vel[3]*f[5]
D[6 + 0] = vel[2];
D[6 + 2] = -vel[0];
D[6 + 3] = vel[5];
D[6 + 5] = -vel[3];
// res[2] = -vel[1]*f[0] + vel[0]*f[1] - vel[4]*f[3] + vel[3]*f[4]
D[12 + 0] = -vel[1];
D[12 + 1] = vel[0];
D[12 + 3] = -vel[4];
D[12 + 4] = vel[3];
// res[3] = -vel[2]*f[4] + vel[1]*f[5]
D[18 + 4] = -vel[2];
D[18 + 5] = vel[1];
// res[4] = vel[2]*f[3] - vel[0]*f[5]
D[24 + 3] = vel[2];
D[24 + 5] = -vel[0];
// res[5] = -vel[1]*f[3] + vel[0]*f[4]
D[30 + 3] = -vel[1];
D[30 + 4] = vel[0];
}
// derivative of mju_mulInertVec w.r.t vel
static void mjd_mulInertVec_vel(mjtNum D[36], const mjtNum i[10])
{
mju_zero(D, 36);
// res[0] = i[0]*v[0] + i[3]*v[1] + i[4]*v[2] - i[8]*v[4] + i[7]*v[5]
D[0 + 0] = i[0];
D[0 + 1] = i[3];
D[0 + 2] = i[4];
D[0 + 4] = -i[8];
D[0 + 5] = i[7];
// res[1] = i[3]*v[0] + i[1]*v[1] + i[5]*v[2] + i[8]*v[3] - i[6]*v[5]
D[6 + 0] = i[3];
D[6 + 1] = i[1];
D[6 + 2] = i[5];
D[6 + 3] = i[8];
D[6 + 5] = -i[6];
// res[2] = i[4]*v[0] + i[5]*v[1] + i[2]*v[2] - i[7]*v[3] + i[6]*v[4]
D[12 + 0] = i[4];
D[12 + 1] = i[5];
D[12 + 2] = i[2];
D[12 + 3] = -i[7];
D[12 + 4] = i[6];
// res[3] = i[8]*v[1] - i[7]*v[2] + i[9]*v[3]
D[18 + 1] = i[8];
D[18 + 2] = -i[7];
D[18 + 3] = i[9];
// res[4] = i[6]*v[2] - i[8]*v[0] + i[9]*v[4]
D[24 + 2] = i[6];
D[24 + 0] = -i[8];
D[24 + 4] = i[9];
// res[5] = i[7]*v[0] - i[6]*v[1] + i[9]*v[5]
D[30 + 0] = i[7];
D[30 + 1] = -i[6];
D[30 + 5] = i[9];
}
//------------------------- derivatives of component functions -------------------------------------
// derivative of cvel, cdof_dot w.r.t qvel
static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot)
{
int nv = m->nv, nbody = m->nbody;
mjtNum mat[36];
// clear Dcvel
mju_zero(Dcvel, nbody*6*nv);
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
for (int i=1; i<m->nbody; i++) {
// Dcvel = Dcvel_parent
mju_copy(Dcvel+i*6*nv, Dcvel+m->body_parentid[i]*6*nv, 6*nv);
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
for (int j=m->body_dofadr[i]; j<m->body_dofadr[i]+m->body_dofnum[i]; j++) {
switch (m->jnt_type[m->dof_jntid[j]])
{
case mjJNT_FREE:
// Dcdofdot = 0
mju_zero(Dcdofdot+j*6*nv, 18*nv);
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k];
Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k];
Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k];
}
// continue with rotations
j += 3;
case mjJNT_BALL:
// Dcdofdot = D crossMotion(cvel, cdof)
for (int k=0; k<3; k++) {
mjd_crossMotion_vel(mat, d->cdof+6*(j+k));
mju_mulMatMat(Dcdofdot+(j+k)*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv);
}
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
Dcvel[i*6*nv + k*nv + j+0] += d->cdof[(j+0)*6 + k];
Dcvel[i*6*nv + k*nv + j+1] += d->cdof[(j+1)*6 + k];
Dcvel[i*6*nv + k*nv + j+2] += d->cdof[(j+2)*6 + k];
}
// adjust for 3-dof joint
j += 2;
break;
default:
// Dcdofdot = D crossMotion(cvel, cdof) * Dcvel
mjd_crossMotion_vel(mat, d->cdof+6*j);
mju_mulMatMat(Dcdofdot+j*6*nv, mat, Dcvel+i*6*nv, 6, 6, nv);
// Dcvel += cdof * (D qvel)
for (int k=0; k<6; k++) {
Dcvel[i*6*nv + k*nv + j] += d->cdof[j*6 + k];
}
}
}
}
}
// subtract (d qfrc_bias / d qvel) from DfDv
static void mjd_rne_vel(const mjModel* m, mjData* d, mjtNum* DfDv) {
int nv = m->nv, nbody = m->nbody;
mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
mjMARKSTACK;
mjtNum* Dcvel = mj_stackAlloc(d, nbody*6*nv);
mjtNum* Dcdofdot = mj_stackAlloc(d, nv*6*nv);
mjtNum* Dcacc = mj_stackAlloc(d, nbody*6*nv);
mjtNum* Dcfrcbody = mj_stackAlloc(d, nbody*6*nv);
// compute Dcdofdot and Dcvel
mjd_comVel_vel(m, d, Dcvel, Dcdofdot);
// clear Dcacc
mju_zero(Dcacc, nbody*6*nv);
// forward pass over bodies: accumulate Dcacc, set Dcfrcbody
for (int i=1; i<nbody; i++) {
// Dcacc = Dcacc_parent
mju_copy(Dcacc + i*6*nv, Dcacc + m->body_parentid[i]*6*nv, 6*nv);
// Dcacc += D(cdofdot * qvel)
for (int j=m->body_dofadr[i]; j<m->body_dofadr[i]+m->body_dofnum[i]; j++) {
// Dcacc += cdofdot * (D qvel)
for (int k=0; k<6; k++) {
Dcacc[i*6*nv + k*nv + j] += d->cdof_dot[j*6 + k];
}
// Dcacc += (D cdofdot) * qvel
mju_addToScl(Dcacc+i*6*nv, Dcdofdot+j*6*nv, d->qvel[j], 6*nv);
}
//---------- Dcfrcbody = D(cinert * cacc + cvel x (cinert * cvel))
// Dcfrcbody = (D mul / D cacc) * Dcacc
mjd_mulInertVec_vel(dmul, d->cinert+10*i);
mju_mulMatMat(Dcfrcbody+i*6*nv, dmul, Dcacc+i*6*nv, 6, 6, nv);
// mat = (D cross / D cvel) + (D cross / D mul) * (D mul / D cvel)
mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+i*6);
mjd_crossForce_vel(mat, tmp);
mjd_crossForce_frc(mat1, d->cvel+i*6);
mju_mulMatMat(mat2, mat1, dmul, 6, 6, 6);
mju_addTo(mat, mat2, 36);
// Dcfrcbody += mat * Dcvel (use body 0 as temp)
mju_mulMatMat(Dcfrcbody, mat, Dcvel+i*6*nv, 6, 6, nv);
mju_addTo(Dcfrcbody+i*6*nv, Dcfrcbody, 6*nv);
}
// clear world Dcfrcbody, for style
mju_zero(Dcfrcbody, 6*nv);
// backward pass over bodies: accumulate Dcfrcbody
for (int i=m->nbody-1; i>0; i--) {
if (m->body_parentid[i]) {
mju_addTo(Dcfrcbody+m->body_parentid[i]*6*nv, Dcfrcbody+i*6*nv, 6*nv);
}
}
// DfDv -= D(cdof * cfrc_body)
for (int i=0; i<nv; i++) {
for (int k=0; k<6; k++) {
mju_addToScl(DfDv+i*nv, Dcfrcbody+(m->dof_bodyid[i]*6+k)*nv, -d->cdof[i*6+k], nv);
}
}
mjFREESTACK;
}
// construct sparse Jacobian structure of body; return nnz
static int bodyJacSparse(const mjModel* m, int body, int* ind) {
// skip fixed bodies
while (body>0 && m->body_dofnum[body]==0) {
body = m->body_parentid[body];
}
// body is not movable: empty chain
if (body==0) {
return 0;
}
// count dofs
int nnz = 0;
int dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof>=0) {
nnz++;
dof = m->dof_parentid[dof];
}
// fill array in reverse (increasing dof)
int cnt = 0;
dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof>=0) {
ind[nnz-cnt-1] = dof;
cnt++;
dof = m->dof_parentid[dof];
}
return nnz;
}
// add J'*B*J to DfDv
static void addJTBJ(mjtNum* DfDv, const mjtNum* J, const mjtNum* B, int n, int nv) {
// process non-zero elements of B
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
if (B[i*n+j]) {
// process non-zero elements of J(i,:)
for (int k=0; k<nv; k++) {
if (J[i*nv+k]) {
// add J(i,k)*B(i,j)*J(j,:) to DfDv(k,:)
mju_addToScl(DfDv+k*nv, J+j*nv, J[i*nv+k]*B[i*n+j], nv);
}
}
}
}
}
}
// add J'*B*J to DfDv, sparse version
static void addJTBJSparse(mjtNum* DfDv, const mjtNum* J, const mjtNum* B,
int n, int nv, int offset,
const int* rownnz, const int* rowadr, const int* colind) {
// process non-zero elements of B
for (int i=0; i<n; i++) {
for (int j=0; j<n; j++) {
if (B[i*n+j]) {
// process non-zero elements of J(i,k)
for (int k=0; k<rownnz[offset+i]; k++) {
int ik = rowadr[offset+i] + k;
int col_ik = colind[ik]*nv;
mjtNum scl = J[ik]*B[i*n+j];
// process non-zero elements of J(j,p)
for (int p=0; p<rownnz[offset+j]; p++) {
int jp = rowadr[offset+j] + p;
// add J(i,k)*B(i,j)*J(j,p) to DfDv(k,p)
DfDv[col_ik + colind[jp]] += scl * J[jp];
}
}
}
}
}
}
// add (d qfrc_passive / d qvel) to DfDv
void mjd_passive_vel(const mjModel* m, mjData* d, mjtNum* DfDv) {
mjMARKSTACK;
int nv = m->nv;
// disabled: nothing to add
if (mjDISABLED(mjDSBL_PASSIVE)) {
return;
}
// dof damping
for (int i=0; i<nv; i++) {
DfDv[i*(nv+1)] -= m->dof_damping[i];
}
// tendon damping
for (int i=0; i<m->ntendon; i++) {
if (m->tendon_damping[i]>0) {
mjtNum B = -m->tendon_damping[i];
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, d->ten_J, &B, 1, nv, i,
d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind);
} else {
addJTBJ(DfDv, d->ten_J+i*nv, &B, 1, nv);
}
}
}
// body viscosity, lift and drag
if (m->opt.viscosity>0 || m->opt.density>0) {
int rownnz[6], rowadr[6];
mjtNum* J = mj_stackAlloc(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 3*nv);
int* colind = (int*) mj_stackAlloc(d, 6*nv);
for (int i=1; i<m->nbody; i++) {
if (m->body_mass[i]>mjMINVAL) {
mjtNum lvel[6], wind[6], lwind[6], box[3], B;
mjtNum* inertia = m->body_inertia + 3*i;
// equivalent inertia box
box[0] = mju_sqrt(mju_max(mjMINVAL,
(inertia[1] + inertia[2] - inertia[0])) / m->body_mass[i] * 6.0);
box[1] = mju_sqrt(mju_max(mjMINVAL,
(inertia[0] + inertia[2] - inertia[1])) / m->body_mass[i] * 6.0);
box[2] = mju_sqrt(mju_max(mjMINVAL,
(inertia[0] + inertia[1] - inertia[2])) / m->body_mass[i] * 6.0);
// map from CoM-centered to local body-centered 6D velocity
mj_objectVelocity(m, d, mjOBJ_BODY, i, lvel, 1);
// compute wind in local coordinates
mju_zero(wind, 6);
mju_copy3(wind+3, m->opt.wind);
mju_transformSpatial(lwind, wind, 0, d->xipos+3*i,
d->subtree_com+3*m->body_rootid[i], d->ximat+9*i);
// subtract translational component from body velocity
mju_subFrom3(lvel+3, lwind+3);
// get body global Jacobian: rotation then translation
mj_jacBodyCom(m, d, J+3*nv, J, i);
// init with dense
int nnz = nv;
// prepare for sparse
if (mj_isSparse(m)) {
// get sparse body Jacobian structure
nnz = bodyJacSparse(m, i, colind);
// compress body Jacobian in-place
for (int j=0; j<6; j++) {
for (int k=0; k<nnz; k++) {
J[j*nnz+k] = J[j*nv+colind[k]];
}
}
// prepare rownnz, rowadr, colind for all 6 rows
rownnz[0] = nnz;
rowadr[0] = 0;
for (int j=1; j<6; j++) {
rownnz[j] = nnz;
rowadr[j] = rowadr[j-1] + nnz;
for (int k=0; k<nnz; k++) {
colind[j*nnz+k] = colind[k];
}
}
}
// rotate (compressed) Jacobian to local frame
mju_mulMatTMat(tmp, d->ximat+9*i, J, 3, 3, nnz);
mju_copy(J, tmp, 3*nnz);
mju_mulMatTMat(tmp, d->ximat+9*i, J+3*nnz, 3, 3, nnz);
mju_copy(J+3*nnz, tmp, 3*nnz);
// add viscous force and torque
if (m->opt.viscosity>0) {
// diameter of sphere approximation
mjtNum diam = (box[0] + box[1] + box[2])/3.0;
// mju_scl3(lfrc, lvel, -mjPI*diam*diam*diam*m->opt.viscosity)
B = -mjPI*diam*diam*diam*m->opt.viscosity;
for (int j=0; j<3; j++) {
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, j,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+j*nv, &B, 1, nv);
}
}
// mju_scl3(lfrc+3, lvel+3, -3.0*mjPI*diam*m->opt.viscosity);
B = -3.0*mjPI*diam*m->opt.viscosity;
for (int j=0; j<3; j++) {
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 3+j,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+3*nv+j*nv, &B, 1, nv);
}
}
}
// add lift and drag force and torque
if (m->opt.density>0) {
// lfrc[0] -= m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])*
// mju_abs(lvel[0])*lvel[0]/64.0;
B = -m->opt.density*box[0]*(box[1]*box[1]*box[1]*box[1]+box[2]*box[2]*box[2]*box[2])*
2*mju_abs(lvel[0])/64.0;
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 0,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J, &B, 1, nv);
}
// lfrc[1] -= m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])*
// mju_abs(lvel[1])*lvel[1]/64.0;
B = -m->opt.density*box[1]*(box[0]*box[0]*box[0]*box[0]+box[2]*box[2]*box[2]*box[2])*
2*mju_abs(lvel[1])/64.0;
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 1,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+nv, &B, 1, nv);
}
// lfrc[2] -= m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])*
// mju_abs(lvel[2])*lvel[2]/64.0;
B = -m->opt.density*box[2]*(box[0]*box[0]*box[0]*box[0]+box[1]*box[1]*box[1]*box[1])*
2*mju_abs(lvel[2])/64.0;
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 2,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+2*nv, &B, 1, nv);
}
// lfrc[3] -= 0.5*m->opt.density*box[1]*box[2]*mju_abs(lvel[3])*lvel[3];
B = -0.5*m->opt.density*box[1]*box[2]*2*mju_abs(lvel[3]);
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 3,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+3*nv, &B, 1, nv);
}
// lfrc[4] -= 0.5*m->opt.density*box[0]*box[2]*mju_abs(lvel[4])*lvel[4];
B = -0.5*m->opt.density*box[0]*box[2]*2*mju_abs(lvel[4]);
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 4,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+4*nv, &B, 1, nv);
}
// lfrc[5] -= 0.5*m->opt.density*box[0]*box[1]*mju_abs(lvel[5])*lvel[5];
B = -0.5*m->opt.density*box[0]*box[1]*2*mju_abs(lvel[5]);
if (mj_isSparse(m)) {
addJTBJSparse(DfDv, J, &B, 1, nv, 5,
rownnz, rowadr, colind);
} else {
addJTBJ(DfDv, J+5*nv, &B, 1, nv);
}
}
}
}
}
mjFREESTACK;
}
// add forward fin-diff approximation of (d qfrc_passive / d qvel) to DfDv
void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps, mjtNum* DfDv) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* qfrc_passive = mj_stackAlloc(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv);
// save qfrc_passive, assume mj_fwdVelocity was called
mju_copy(qfrc_passive, d->qfrc_passive, nv);
// loop over dofs
for (int i=0; i<nv; i++) {
// save qvel[i]
mjtNum saveqvel = d->qvel[i];
// eval at qvel[i]+eps
d->qvel[i] = saveqvel + eps;
mj_fwdVelocity(m, d);
// restore qvel[i]
d->qvel[i] = saveqvel;
// finite difference result in fd
mju_sub(fd, d->qfrc_passive, qfrc_passive, nv);
mju_scl(fd, fd, 1/eps, nv);
// copy to i-th column of DfDv
for (int j=0; j<nv; j++) {
DfDv[j*nv+i] += fd[j];
}
}
// restore
mj_fwdVelocity(m, d);
mjFREESTACK;
}
// derivative of mju_muscleGain w.r.t velocity
static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], mjtNum acc0,
const mjtNum prm[9]) {
// unpack parameters
mjtNum range[2] = {prm[0], prm[1]};
mjtNum force = prm[2];
mjtNum scale = prm[3];
mjtNum lmin = prm[4];
mjtNum lmax = prm[5];
mjtNum vmax = prm[6];
mjtNum fvmax = prm[8];
// scale force if negative
if (force<0) {
force = scale / mjMAX(mjMINVAL, acc0);
}
// mid-ranges
mjtNum a = 0.5*(lmin+1);
mjtNum b = 0.5*(1+lmax);
mjtNum x;
// optimum length
mjtNum L0 = (lengthrange[1]-lengthrange[0]) / mjMAX(mjMINVAL, range[1]-range[0]);
// normalized length and velocity
mjtNum L = range[0] + (len-lengthrange[0]) / mjMAX(mjMINVAL, L0);
mjtNum V = vel / mjMAX(mjMINVAL, L0*vmax);
// length curve
mjtNum FL = 0;
if (L>=lmin && L<=a) {
x = (L-lmin) / mjMAX(mjMINVAL, a-lmin);
FL = 0.5*x*x;
} else if (L<=1) {
x = (1-L) / mjMAX(mjMINVAL, 1-a);
FL = 1 - 0.5*x*x;
} else if (L<=b) {
x = (L-1) / mjMAX(mjMINVAL, b-1);
FL = 1 - 0.5*x*x;
} else if (L<=lmax) {
x = (lmax-L) / mjMAX(mjMINVAL, lmax-b);
FL = 0.5*x*x;
}
// velocity curve
mjtNum dFV;
mjtNum y = fvmax-1;
if (V<=-1) {
// FV = 0
dFV = 0;
} else if (V<=0) {
// FV = (V+1)*(V+1)
dFV = 2*V + 2;
} else if (V<=y) {
// FV = fvmax - (y-V)*(y-V) / mjMAX(mjMINVAL, y)
dFV = (-2*V + 2*y) / mjMAX(mjMINVAL, y);
} else {
// FV = fvmax
dFV = 0;
}
// compute FVL and scale, make it negative
return -force*FL*dFV/mjMAX(mjMINVAL,L0*vmax);
}
// add (d qfrc_actuator / d qvel) to DfDv
static void mjd_actuator_vel(const mjModel* m, mjData* d, mjtNum* DfDv) {
int nv = m->nv;
// disabled: nothing to add
if (mjDISABLED(mjDSBL_ACTUATION)) {
return;
}
// process actuators
for (int i=0; i<m->nu; i++) {
// affine bias
if (m->actuator_biastype[i]==mjBIAS_AFFINE) {
// extract bias info: prm = [const, kp, kv]
mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
// add
mjtNum B = prm[2];
addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv);
}
// muscle gain
else if (m->actuator_gaintype[i]==mjGAIN_MUSCLE) {
mjtNum B = mjd_muscleGain_vel(d->actuator_length[i],
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
m->actuator_gainprm + mjNGAIN*i);
// force = gain .* [ctrl/act]
if (m->actuator_dyntype[i]==mjDYN_NONE) {
B *= d->ctrl[i];
} else {
B *= d->act[i-(m->nu - m->na)];
}
// add
addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv);
}
}
}
//------------------------- main entry points ------------------------------------------------------
// Analytical derivative:
// d->qDeriv = d (qfrc_actuator + qfrc_passive - qfrc_bias) / d qvel.
void mjd_smooth_vel(const mjModel *m, mjData *d) {
int nv = m->nv;
// allocate space
mjMARKSTACK;
mjtNum *DfDv = mj_stackAlloc(d, nv*nv);
// clear DfDv
mju_zero(DfDv, nv*nv);
// DfDv = d (qfrc_actuator + qfrc_passive - qfrc_bias) / d qvel
mjd_actuator_vel(m, d, DfDv);
mjd_passive_vel(m, d, DfDv);
mjd_rne_vel(m, d, DfDv);
// copy dense DfDv to sparse qDeriv
for (int i=0; i<nv; i++) {
for (int j=0; j<d->D_rownnz[i]; j++) {
int adr = d->D_rowadr[i] + j;
d->qDeriv[adr] = DfDv[i*nv + d->D_colind[adr]];
}
}
mjFREESTACK;
}
// Centered finite difference approximation to mj_derivative.
void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* plus = mj_stackAlloc(d, nv);
mjtNum* minus = mj_stackAlloc(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv);
int* cnt = (int*) mj_stackAlloc(d, nv);
// clear row counters
memset(cnt, 0, nv*sizeof(int));
// loop over dofs
for (int i=0; i<nv; i++) {
// save qvel[i]
mjtNum saveqvel = d->qvel[i];
// eval at qvel[i]+eps
d->qvel[i] = saveqvel + eps;
mj_fwdVelocity(m, d);
mj_fwdActuation(m, d);
mju_add(plus, d->qfrc_actuator, d->qfrc_passive, nv);
mju_subFrom(plus, d->qfrc_bias, nv);
// eval at qvel[i]-eps
d->qvel[i] = saveqvel - eps;
mj_fwdVelocity(m, d);
mj_fwdActuation(m, d);
mju_add(minus, d->qfrc_actuator, d->qfrc_passive, nv);
mju_subFrom(minus, d->qfrc_bias, nv);
// restore qvel[i]
d->qvel[i] = saveqvel;
// finite difference result in fd
mju_sub(fd, plus, minus, nv);
mju_scl(fd, fd, 0.5/eps, nv);
// copy to sparse qDeriv
for (int j=0; j<nv; j++) {
if (cnt[j]<d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]]==i) {
d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j];
cnt[j]++;
}
}
}
// make sure final row counters equal rownnz
for (int i=0; i<nv; i++) {
if (cnt[i]!=d->D_rownnz[i]) {
mju_error("error in constructing FD sparse derivative");
}
}
// restore
mj_fwdVelocity(m, d);
mj_fwdActuation(m, d);
mjFREESTACK;
}
src/engine/engine_derivative.h
+44
View File
@@ -0,0 +1,44 @@
// 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.
#ifndef MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
#define MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#ifdef __cplusplus
extern "C" {
#endif
// analytical derivative of smooth forces w.r.t velocities:
// d->qDeriv = d (qfrc_actuator + qfrc_passive - qfrc_bias) / d qvel.
MJAPI void mjd_smooth_vel(const mjModel* m, mjData* d);
// centered finite difference approximation to mjd_smooth_vel
MJAPI void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps);
// add (d qfrc_passive / d qvel) to DfDv
MJAPI void mjd_passive_vel(const mjModel* m, mjData* d, mjtNum* DfDv);
// add forward finite difference approximation of (d qfrc_passive / d qvel) to DfDv
MJAPI void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps, mjtNum* DfDv);
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_DERIVATIVE_H_
src/engine/engine_forward.c
+73 -7
View File
@@ -15,6 +15,7 @@
#include "engine/engine_forward.h"
#include <stddef.h>
#include <stdio.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
@@ -22,6 +23,7 @@
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_derivative.h"
#include "engine/engine_inverse.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
@@ -31,8 +33,11 @@
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_sparse.h"
//--------------------------- check values ---------------------------------------------------------
// check positions, reset if bad
@@ -145,7 +150,7 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// (qpos, qvel, crtl, act) => (qfrc_actuator, actuator_force, act_dot)
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
TM_START;
int nv = m->nv, nu = m->nu, na = m->na;
@@ -649,6 +654,52 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
//-------------------------- top-level API ---------------------------------------------------------
// fully implicit in velocity
void mj_implicit(const mjModel *m, mjData *d) {
int nv = m->nv;
mjMARKSTACK;
mjtNum *qfrc = mj_stackAlloc(d, nv);
mjtNum *qacc = mj_stackAlloc(d, nv);
// construct sparse structure in d->D_xxx
mj_makeMSparse(m, d, d->D_rownnz, d->D_rowadr, d->D_colind);
// compute analytical derivative qDeriv
mjd_smooth_vel(m, d);
// set qLU = qM - dt*qDeriv
mj_setMSparse(m, d, d->qLU, d->D_rownnz, d->D_rowadr, d->D_colind);
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
// factorize qLU, use qacc as scratch space
mju_factorLUSparse(d->qLU, nv, (int*)qacc, d->D_rownnz, d->D_rowadr, d->D_colind);
// set qfrc = qfrc_smooth + qfrc_constraint
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_colind);
// update qvel
mju_addToScl(d->qvel, qacc, m->opt.timestep, nv);
// update act
if (m->na) {
mju_addToScl(d->act, d->act_dot, m->opt.timestep, m->na);
}
// update qpos using new qvel
mj_integratePos(m, d->qpos, d->qvel, m->opt.timestep);
// advance time
d->time += m->opt.timestep;
mjFREESTACK
}
// forward dynamics with skip; skipstage is mjtStage
void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) {
TM_START;
@@ -714,10 +765,21 @@ void mj_step(const mjModel* m, mjData* d) {
}
// use selected integrator
if (m->opt.integrator==mjINT_RK4) {
mj_RungeKutta(m, d, 4);
} else {
mj_Euler(m, d);
switch(m->opt.integrator) {
case mjINT_EULER:
mj_Euler(m, d);
break;
case mjINT_RK4:
mj_RungeKutta(m, d, 4);
break;
case mjINT_IMPLICIT:
mj_implicit(m, d);
break;
default:
mju_error("Invalid integrator");
}
TM_END(mjTIMER_STEP);
@@ -760,8 +822,12 @@ void mj_step2(const mjModel* m, mjData* d) {
mj_compareFwdInv(m, d);
}
// integrate with Euler; ignore integrator option
mj_Euler(m, d);
// integrate with Euler or implicit; RK4 defaults to Euler
if (m->opt.integrator==mjINT_IMPLICIT) {
mj_implicit(m, d);
} else {
mj_Euler(m, d);
}
d->timer[mjTIMER_STEP].number--;
TM_END(mjTIMER_STEP);
src/engine/engine_forward.h
+3
View File
@@ -55,6 +55,9 @@ MJAPI void mj_Euler(const mjModel* m, mjData* d);
// Runge Kutta explicit order-N integrator
MJAPI void mj_RungeKutta(const mjModel* m, mjData* d, int N);
// fully implicit in velocity
MJAPI void mj_implicit(const mjModel *m, mjData *d);
//-------------------------------- solver components -----------------------------------------------
src/engine/engine_print.c
+31
View File
@@ -28,6 +28,7 @@
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#define FLOAT_FORMAT "% -9.2g"
#define FLOAT_FORMAT_MAX_LEN 20
@@ -848,6 +849,36 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
printArray("QLDIAGSQRTINV", m->nv, 1, d->qLDiagSqrtInv, fp, float_format);
// D_rownnz
fprintf(fp, NAME_FORMAT, "D_rownnz");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, "%d ", d->D_rownnz[i]);
}
fprintf(fp, "\n\n");
// D_rowadr
fprintf(fp, NAME_FORMAT, "D_rowadr");
for (int i = 0; i < m->nv; i++) {
fprintf(fp, "%d ", d->D_rowadr[i]);
}
fprintf(fp, "\n\n");
// D_colind
fprintf(fp, NAME_FORMAT, "D_colind");
for (int i = 0; i < m->nD; i++) {
fprintf(fp, "%d ", d->D_colind[i]);
}
fprintf(fp, "\n\n");
// print qDeriv
mju_sparse2dense(M, d->qDeriv, m->nv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind);
printArray("QDERIV", m->nv, m->nv, M, fp, float_format);
// print qLU
mju_sparse2dense(M, d->qLU, m->nv, m->nv, d->D_rownnz, d->D_rowadr,
d->D_colind);
printArray("QLU", m->nv, m->nv, M, fp, float_format);
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i<d->ncon; i++) {
src/engine/engine_support.c
+92
View File
@@ -852,6 +852,98 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// construct sparse matrix representations matching qM
void mj_makeMSparse(const mjModel* m, mjData* d, int* rownnz, int* rowadr, int* colind) {
int nv = m->nv;
mjMARKSTACK;
int *remaining = (int*) mj_stackAlloc(d, nv);
// compute rownnz
memset(rownnz, 0, nv*sizeof(int));
for (int i=nv-1; i>=0; i--) {
// init at diagonal
int j = i;
rownnz[i]++;
// process below diagonal
while ((j=m->dof_parentid[j]) >= 0) {
rownnz[i]++;
rownnz[j]++;
}
}
// accumulate rowadr
rowadr[0] = 0;
for (int i=1; i<nv; i++) {
rowadr[i] = rowadr[i-1] + rownnz[i-1];
}
// populate colind
memcpy(remaining, rownnz, nv*sizeof(int));
for (int i=nv-1; i>=0; i--) {
// init at diagonal
remaining[i]--;
colind[rowadr[i] + remaining[i]] = i;
// process below diagonal
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
colind[rowadr[i] + remaining[i]] = j;
remaining[j]--;
colind[rowadr[j] + remaining[j]] = i;
}
}
// sanity check; SHOULD NOT OCCUR
for (int i=0; i<nv; i++) {
if (remaining[i]!=0) {
mju_error("Error in mj_makeMSparse: unexpected remaining");
}
}
mjFREESTACK
}
// set dst = qM, handle different sparsity representations
void mj_setMSparse(const mjModel* m, mjData* d, mjtNum* dst,
const int *rownnz, const int *rowadr, const int *colind) {
int nv = m->nv;
mjMARKSTACK;
int *remaining = (int*) mj_stackAlloc(d, nv);
// copy data
memcpy(remaining, rownnz, nv*sizeof(int));
for (int i=nv-1; i>=0; i--) {
// init at diagonal
int adr = m->dof_Madr[i];
remaining[i]--;
dst[rowadr[i] + remaining[i]] = d->qM[adr];
adr++;
// process below diagonal
int j = i;
while ((j = m->dof_parentid[j]) >= 0) {
remaining[i]--;
dst[rowadr[i] + remaining[i]] = d->qM[adr];
remaining[j]--;
dst[rowadr[j] + remaining[j]] = d->qM[adr];
adr++;
}
}
mjFREESTACK
}
//-------------------------- perturbations ---------------------------------------------------------
// add cartesian force and torque to qfrc_target
src/engine/engine_support.h
+7
View File
@@ -98,6 +98,13 @@ MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum
MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int* rownnz, int* rowadr, int* colind);
// construct sparse matrix representations matching qM
MJAPI void mj_makeMSparse(const mjModel* m, mjData* d, int *rownnz, int *rowadr, int *colind);
// set dst = qM, handle different sparsity representations
MJAPI void mj_setMSparse(const mjModel* m, mjData* d, mjtNum* dst,
const int *rownnz, const int *rowadr, const int *colind);
//-------------------------- perturbations ---------------------------------------------------------
src/engine/engine_util_misc.c
+1 -1
View File
@@ -657,7 +657,7 @@ void mju_printMatSparse(const mjtNum* mat, int nr,
const int* colind) {
for (int r=0; r<nr; r++) {
for (int adr=rowadr[r]; adr<rowadr[r]+rownnz[r]; adr++) {
printf("(%d %d): %.6f ", r, colind[adr], mat[adr]);
printf("(%d %d): %9.6f ", r, colind[adr], mat[adr]);
}
printf("\n");
}
src/engine/engine_util_solve.c
+122
View File
@@ -299,6 +299,128 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
//------------------------------ LU factorization --------------------------------------------------
// sparse reverse-order LU factorization, no fill-in (assuming tree topology)
// result: LU = L + U; original = (U+I) * L; scratch size is n
void mju_factorLUSparse(mjtNum* LU, int n, int* scratch,
const int* rownnz, const int* rowadr, const int* colind) {
int* remaining = scratch;
// set remaining = rownnz
memcpy(remaining, rownnz, n*sizeof(int));
// diagonal elements (i,i)
for (int i=n-1; i>=0; i--) {
// get address of last remaining element of row i, adjust remaining counter
int ii = rowadr[i] + remaining[i] - 1;
remaining[i]--;
// make sure ii is on diagonal
if (colind[ii]!=i) {
mju_error("missing diagonal element in mju_factorLUSparse");
}
// make sure diagonal is not too small
if (mju_abs(LU[ii])<mjMINVAL) {
mju_error("diagonal element too small in mju_factorLUSparse");
}
// rows j above i
for (int j=i-1; j>=0; j--) {
// get address of last remaining element of row j
int ji = rowadr[j] + remaining[j] - 1;
// process row j if (j,i) is non-zero
if (colind[ji]==i) {
// adjust remaining counter
remaining[j]--;
// (j,i) = (j,i) / (i,i)
LU[ji] = LU[ji] / LU[ii];
mjtNum LUji = LU[ji];
// (j,k) = (j,k) - (i,k) * (j,i) for k<i; handle incompatible sparsity
int icnt = rowadr[i], jcnt = rowadr[j];
while (jcnt<rowadr[j]+remaining[j]) {
// both non-zero
if (colind[icnt]==colind[jcnt]) {
// update LU, advance counters
LU[jcnt++] -= LU[icnt++] * LUji;
}
// only (j,k) non-zero
else if (colind[icnt]>colind[jcnt]) {
// advance j counter
jcnt++;
}
// only (i,k) non-zero
else {
mju_error("mju_factorLUSparse requires fill-in");
}
}
// make sure both rows fully processed
if (icnt!=rowadr[i]+remaining[i] || jcnt!=rowadr[j]+remaining[j]) {
mju_error("row processing incomplete in mju_factorLUSparse");
}
}
}
}
// make sure remaining points to diagonal
for (int i=0; i<n; i++) {
if (remaining[i]<0 || colind[rowadr[i]+remaining[i]]!=i) {
mju_error("unexpected sparse matrix structure in mju_factorLUSparse");
}
}
}
// solve mat*res=vec given LU factorization of mat
void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n,
const int* rownnz, const int* rowadr, const int* colind) {
//------------------ solve (U+I)*res = vec
for (int i=n-1; i>=0; i--) {
// init: diagonal of (U+I) is 1
res[i] = vec[i];
// res[i] -= sum_k>i res[k]*LU(i,k)
int j = rownnz[i] - 1;
while (colind[rowadr[i]+j]>i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j--;
}
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
mju_error("diagonal of U not reached in mju_factorLUSparse");
}
}
//------------------ solve L*res(new) = res
for (int i=0; i<n; i++) {
// res[i] -= sum_k<i res[k]*LU(i,k)
int j = 0;
while (colind[rowadr[i]+j]<i) {
res[i] -= res[colind[rowadr[i]+j]] * LU[rowadr[i]+j];
j++;
}
// divide by diagonal element of L
res[i] /= LU[rowadr[i]+j];
// make sure j points to diagonal
if (colind[rowadr[i]+j]!=i) {
mju_error("diagonal of L not reached in mju_factorLUSparse");
}
}
}
//--------------------------- eigen decomposition --------------------------------------------------
// eigenvalue decomposition of symmetric 3x3 matrix
src/engine/engine_util_solve.h
+9
View File
@@ -48,6 +48,15 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
int* rownnz, int* rowadr, int* colind, int x_nnz, int* x_ind,
mjData* d);
// sparse reverse-order LU factorization, no fill-in (assuming tree topology)
// LU = L + U; original = (U+I) * L; scratch is size n
void mju_factorLUSparse(mjtNum *LU, int n, int* scratch,
const int *rownnz, const int *rowadr, const int *colind);
// solve mat*res=vec given LU factorization of mat
void mju_solveLUSparse(mjtNum *res, const mjtNum *LU, const mjtNum* vec, int n,
const int *rownnz, const int *rowadr, const int *colind);
// eigenvalue decomposition of symmetric 3x3 matrix
MJAPI int mju_eig3(mjtNum* eigval, mjtNum* eigvec, mjtNum quat[4], const mjtNum mat[9]);
src/user/user_model.cc
+7 -2
View File
@@ -267,6 +267,7 @@ void mjCModel::Clear(void) {
nnames = 0;
nemax = 0;
nM = 0;
nD = 0;
njmax = -1;
nconmax = -1;
@@ -1543,6 +1544,10 @@ void mjCModel::CopyTree(mjModel* m) {
}
m->nM = nM;
// set nD
nD = 2*nM - nv;
m->nD = nD;
// set dof_simplenum
int scnt = 0;
for (i=nv-1; i>=0; i--) {
@@ -2637,8 +2642,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
nmat != m->nmat || ntex != m->ntex || npair!=m->npair || nexclude!=m->nexclude ||
neq!=m->neq || ntendon!=m->ntendon || nwrap!=m->nwrap || nsensor!=m->nsensor ||
nnumeric!=m->nnumeric || nnumericdata!=m->nnumericdata || ntext!=m->ntext ||
ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nemax!=m->nemax ||
nconmax!=m->nconmax || njmax!=m->njmax) {
ntextdata!=m->ntextdata || nnames!=m->nnames || nM!=m->nM || nD!=m->nD ||
nemax!=m->nemax || nconmax!=m->nconmax || njmax!=m->njmax) {
errInfo = mjCError(0, "incompatible models in CopyBack");
return false;
}
src/user/user_model.h
+1
View File
@@ -218,6 +218,7 @@ class mjCModel {
int ntupledata; // number of objects in all tuple fields
int nnames; // number of chars in all names
int nM; // number of non-zeros in sparse inertia matrix
int nD; // number of non-zeros in sparse derivative matrix
//------------------------ object lists
// objects created here
src/xml/xml_native_reader.cc
+3 -2
View File
@@ -416,10 +416,11 @@ const mjMap camlight_map[camlight_sz] = {
// integrator type
const int integrator_sz = 2;
const int integrator_sz = 3;
const mjMap integrator_map[integrator_sz] = {
{"Euler", mjINT_EULER},
{"RK4", mjINT_RK4}
{"RK4", mjINT_RK4},
{"implicit", mjINT_IMPLICIT}
};
test/CMakeLists.txt
+3
View File
@@ -57,6 +57,9 @@ target_link_libraries(fixture_test fixture gmock)
mujoco_test(header_test)
target_link_libraries(header_test fixture gmock)
mujoco_test(pipeline_test)
target_link_libraries(pipeline_test fixture gmock)
add_subdirectory(benchmark)
add_subdirectory(engine)
add_subdirectory(sample)
test/engine/CMakeLists.txt
+3
View File
@@ -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)
test/engine/engine_derivative_test.cc
+164
View File
@@ -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
test/engine/engine_forward_test.cc
+137
View File
@@ -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
test/engine/testdata/derivative/damped_actuators.xml
Vendored
+24
View File
@@ -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>
test/engine/testdata/derivative/energy_conserving_pendulum.xml
Vendored
+25
View File
@@ -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>
test/engine/testdata/derivative/tumbling_thin_object.xml
Vendored
+17
View File
@@ -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>
test/pipeline_test.cc
+65
View File
@@ -0,0 +1,65 @@
// 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 of the entire pipeline that are not easily associated with one file.
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_io.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
return std::vector<mjtNum>(array, array + n);
}
static const char* const kDefaultModel = "testdata/model.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
using PipelineTest = MujocoTest;
// Joint and actuator damping should integrate identically under implicit
TEST_F(PipelineTest, SparseDenseEqivalent) {
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
// set dense jacobian, call mj_forward, save accelerations
model->opt.jacobian = mjJAC_DENSE;
mj_forward(model, data);
std::vector<mjtNum> qacc_dense = AsVector(data->qacc, model->nv);
// set sparse jacobian, call mj_forward, save accelerations
model->opt.jacobian = mjJAC_SPARSE;
mj_forward(model, data);
std::vector<mjtNum> qacc_sparse = AsVector(data->qacc, model->nv);
// expect accelerations to be insignificantly different
mjtNum tol = 1e-12;
EXPECT_THAT(qacc_dense, Pointwise(DoubleNear(tol), qacc_sparse));
// TODO: is 1e-12 larger than we expect?
// investigate sources of discrepancy, eliminate if possible
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/testdata/model.xml
Vendored
+134
View File
@@ -0,0 +1,134 @@
<mujoco>
<option iterations="10" tolerance="0" gravity="-1 0 -10" jacobian="dense">
<flag fwdinv="enable" energy="enable"/>
</option>
<asset>
<mesh name="icosahedron" scale=".05 .05 .05"
vertex="0 1 1.618
0 -1 1.618
0 1 -1.618
0 -1 -1.618
1 1.618 0
-1 1.618 0
1 -1.618 0
-1 -1.618 0
1.618 0 1
1.618 0 -1
-1.618 0 1
-1.618 0 -1"/>
<hfield name="hfield" nrow="3" ncol="3" size=".2 .2 .1 .03"/>
</asset>
<default>
<site rgba=".5 .5 .5 .5"/>
<joint armature="1" damping="10"/>
<general ctrllimited="true" ctrlrange="-1 1"/>
<default class="hip0">
<joint springref="30" stiffness="60"/>
</default>
<default class="hip1">
<joint limited="true" range="-60 60" stiffness="10"/>
</default>
<default class="wheel">
<joint damping=".1" armature=".1"/>
</default>
</default>
<worldbody>
<light pos="0 0 3"/>
<geom type="plane" size="4 4 .1"/>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05" rgba="0 0 1 1"/>
<body name="head" pos="0 0 .7">
<geom type="ellipsoid" size=".2 .2 .4" rgba="1 1 0 1" density="100"/>
<site name="head" pos="0 0 .4" size=".1 .1 .05" type="box"/>
<site name="anchor0" pos="0 .2 0"/>
<site name="rf" pos="0 0 -0.41" zaxis="0 0 -1"/>
<freejoint/>
<body euler="0 0 0" pos=".2 0 -.2">
<joint name="hipz_0" class="hip1" axis="0 0 1"/>
<joint name="hipy_0" class="hip0" axis="0 1 0"/>
<geom type="capsule" size=".05" rgba="1 0 0 1" fromto="0 0 0 .3 0 0"/>
<body pos=".3 0 0">
<site name="knee"/>
<geom type="capsule" size=".05" rgba="1 0 0 1" fromto="0 0 0 .1 0 -.3"/>
<body pos=".1 0 -.3">
<joint name="wheel_0" type="ball" class="wheel"/>
<site name="wheel_0" type="box" size=".1 .1 .1"/>
<geom size=".1 .2 .1" rgba="0 1 0 1" type="ellipsoid"/>
</body>
</body>
</body>
<body euler="0 0 120" pos="-.15 .2 -.2">
<joint name="hipz_1" class="hip1" axis="0 0 1"/>
<joint name="hipy_1" class="hip0" axis="0 1 0"/>
<geom type="box" size=".05" rgba="1 0 0 1" fromto="0 0 0 .3 0 0"/>
<geom type="box" size=".05" rgba="1 0 0 1" fromto=".3 0 0 .4 0 -.3"/>
<body pos=".45 0 -.3">
<joint name="wheel_1" axis="1 0 0" class="wheel"/>
<geom size=".1" rgba="0 1 0 1" type="cylinder" fromto="0 0 0 .03 0 0"/>
<site name="wheel_1" type="box" size=".02 .11 .11" pos=".015 0 0"/>
</body>
</body>
<body euler="0 0 240" pos="-.15 -.2 -.2">
<joint name="hipz_2" class="hip1" axis="0 0 1"/>
<joint name="hipy_2" class="hip0" axis="0 1 0"/>
<geom type="capsule" size=".05" rgba="1 0 0 1" fromto="0 0 0 .3 0 0"/>
<geom type="capsule" size=".05" rgba="1 0 0 1" fromto=".3 0 0 .4 0 -.3"/>
<body pos=".45 0 -.3">
<joint name="wheel_2" axis="1 0 0" class="wheel"/>
<geom size=".1" rgba="0 1 0 1" type="cylinder" fromto="0 0 0 .03 0 0"/>
<site name="wheel_2" size=".13" rgba="0 0 0 0.1" type="cylinder" fromto="-.01 0 0 .04 0 0"/>
</body>
</body>
</body>
<body pos="-.33 0 1">
<joint name="slider" type="slide" axis="0 0 1" limited="true" range="-.2 .5"/>
<joint type="hinge" axis="0 1 0"/>
<geom type="mesh" mesh="icosahedron" size=".1" rgba="0 0 1 1"/>
</body>
<geom name="wrapping" type="cylinder" size=".04" fromto="-.6 .1 .7 -.4 .7 .7"/>
<site name="sidesite" pos="-.5 .4 1"/>
<body pos="-.3 .6 .8">
<freejoint/>
<geom type="box" size=".05 .05 .05" rgba="0 0 1 1"/>
<site name="anchor1" pos=".05 .05 .05"/>
</body>
<body pos="-.6 .4 .8">
<freejoint/>
<geom type="box" size=".05 .05 .05" rgba="0 0 1 1"/>
<site name="anchor2" pos=".05 .05 .05"/>
</body>
</worldbody>
<tendon>
<spatial name="spatial" limited="true" range="0 .7" rgba="1 0 1 1">
<site site="anchor0"/>
<site site="anchor1"/>
<geom geom="wrapping" sidesite="sidesite"/>
<site site="anchor2"/>
</spatial>
<fixed name="fixed">
<joint joint="hipy_0" coef="1"/>
<joint joint="hipy_1" coef="1"/>
</fixed>
</tendon>
<actuator>
<motor tendon="fixed" gear="100"/>
<motor tendon="spatial" gear="10"/>
<motor joint="hipy_2" gear="100"/>
<position joint="hipz_0" kp="100"/>
<position joint="hipz_1" kp="100"/>
<position joint="hipz_2" kp="100"/>
<velocity joint="wheel_2" kv="1"/>
<general site="wheel_0" gear="0 0 0 0 10 0" dyntype="filter" dynprm="1"/>
<general joint="wheel_1" biastype="affine" dyntype="integrator" dynprm="1" biasprm="0 -1"/>
</actuator>
<sensor>
<framepos objtype="site" objname="wheel_0"/>
<rangefinder site="rf"/>
<gyro site="wheel_2"/>
<touch site="wheel_1"/>
<force site="knee"/>
<torque site="knee"/>
<jointlimitfrc joint="slider"/>
<accelerometer site="head"/>
<subtreeangmom body="head"/>
</sensor>
</mujoco>