From 64bc6d27b242d5c5683b4a03dd36f1844b910742 Mon Sep 17 00:00:00 2001 From: DeepMind Date: Mon, 23 May 2022 01:21:24 -0700 Subject: [PATCH] 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 --- doc/changelog.rst | 28 +- doc/computation.rst | 126 ++- include/mujoco/mjdata.h | 15 +- include/mujoco/mjmodel.h | 4 +- include/mujoco/mjxmacro.h | 6 + introspect/enums.py | 1 + sample/simulate.cc | 2 +- src/engine/CMakeLists.txt | 2 + src/engine/engine_derivative.c | 864 ++++++++++++++++++ src/engine/engine_derivative.h | 44 + src/engine/engine_forward.c | 80 +- src/engine/engine_forward.h | 3 + src/engine/engine_print.c | 31 + src/engine/engine_support.c | 92 ++ src/engine/engine_support.h | 7 + src/engine/engine_util_misc.c | 2 +- src/engine/engine_util_solve.c | 122 +++ src/engine/engine_util_solve.h | 9 + src/user/user_model.cc | 9 +- src/user/user_model.h | 1 + src/xml/xml_native_reader.cc | 5 +- test/CMakeLists.txt | 3 + test/engine/CMakeLists.txt | 3 + test/engine/engine_derivative_test.cc | 164 ++++ test/engine/engine_forward_test.cc | 137 +++ .../testdata/derivative/damped_actuators.xml | 24 + .../derivative/energy_conserving_pendulum.xml | 25 + .../derivative/tumbling_thin_object.xml | 17 + test/pipeline_test.cc | 65 ++ test/testdata/model.xml | 134 +++ 30 files changed, 1974 insertions(+), 51 deletions(-) create mode 100644 src/engine/engine_derivative.c create mode 100644 src/engine/engine_derivative.h create mode 100644 test/engine/engine_derivative_test.cc create mode 100644 test/engine/testdata/derivative/damped_actuators.xml create mode 100644 test/engine/testdata/derivative/energy_conserving_pendulum.xml create mode 100644 test/engine/testdata/derivative/tumbling_thin_object.xml create mode 100644 test/pipeline_test.cc create mode 100644 test/testdata/model.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index 3df06ed3..8f0aa56e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -29,7 +29,18 @@ Open Sourcing General ^^^^^^^ -3. Added :at:`actlimited` and :at:`actrange` attributes to :ref:`general actuators`, 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`. For more details, see the :ref:`Numerical Integration` section. + +#. Added :at:`actlimited` and :at:`actrange` attributes to :ref:`general actuators`, for clamping actuator internal states (activations). This clamping is useful for integrated-velocity actuators, see the :ref:`Activation clamping ` section for details. @@ -47,16 +58,15 @@ General Bug fixes ^^^^^^^^^ +10. Antialiasing was disabled for segmentation rendering. Before this change, if the :ref:`offsamples` + 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` - 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) diff --git a/doc/computation.rst b/doc/computation.rst index 5ee6e4cd..a5a6c723 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -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: diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 45ec2f21..5118a90a 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -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 diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index a4062a3e..ed4315ba 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -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 diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 1f0bb83f..617b79e8 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -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 ) \ diff --git a/introspect/enums.py b/introspect/enums.py index 5fce4049..d57bff1a 100755 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -118,6 +118,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([ values=dict([ ('mjINT_EULER', 0), ('mjINT_RK4', 1), + ('mjINT_IMPLICIT', 2), ]), )), ('mjtCollision', diff --git a/sample/simulate.cc b/sample/simulate.cc index a007af03..5a54ca4d 100644 --- a/sample/simulate.cc +++ b/sample/simulate.cc @@ -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"}, diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index a2452634..2103f657 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -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 diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c new file mode 100644 index 00000000..e89960b6 --- /dev/null +++ b/src/engine/engine_derivative.c @@ -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 +#include + +#include +#include +#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; inbody; 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]; jbody_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; ibody_parentid[i]*6*nv, 6*nv); + + // Dcacc += D(cdofdot * qvel) + for (int j=m->body_dofadr[i]; jbody_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; idof_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; inv; + + // disabled: nothing to add + if (mjDISABLED(mjDSBL_PASSIVE)) { + return; + } + + // dof damping + for (int i=0; idof_damping[i]; + } + + // tendon damping + for (int i=0; intendon; 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; inbody; 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; kximat+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; iqvel[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=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; inu; 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; iD_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; iqvel[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; jD_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; iD_rownnz[i]) { + mju_error("error in constructing FD sparse derivative"); + } + } + + // restore + mj_fwdVelocity(m, d); + mj_fwdActuation(m, d); + + mjFREESTACK; +} + diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h new file mode 100644 index 00000000..b7c6cd01 --- /dev/null +++ b/src/engine/engine_derivative.h @@ -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 +#include +#include + +#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_ diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 1298a1d7..7d11b127 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -15,6 +15,7 @@ #include "engine/engine_forward.h" #include +#include #include #include @@ -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); diff --git a/src/engine/engine_forward.h b/src/engine/engine_forward.h index 6512d378..d75e64e4 100644 --- a/src/engine/engine_forward.h +++ b/src/engine/engine_forward.h @@ -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 ----------------------------------------------- diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 5a7eb139..e6e40402 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -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; incon; i++) { diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 553ab115..23355fdd 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -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=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; inv; + + 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 diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 2cea42b5..7c87a2d3 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -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 --------------------------------------------------------- diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index af89cea4..d1c41512 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -657,7 +657,7 @@ void mju_printMatSparse(const mjtNum* mat, int nr, const int* colind) { for (int r=0; r=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])=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 kcolind[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=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; inM = 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; } diff --git a/src/user/user_model.h b/src/user/user_model.h index 2fc07bb1..0ae132ca 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -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 diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 6f9f3cdd..71116e3f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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} }; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ffb62d89..1386df67 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -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) diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index cb1fb21c..ce423dab 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -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) diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc new file mode 100644 index 00000000..e10e50f1 --- /dev/null +++ b/test/engine/engine_derivative_test.cc @@ -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 +#include + +#include +#include +#include +#include +#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; iopt.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 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 diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index d25d2bae..519a5437 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -26,8 +26,22 @@ namespace mujoco { namespace { +std::vector AsVector(const mjtNum* array, int n) { + return std::vector(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"( @@ -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"( + + + + + + + + + + + + + )"; + + 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 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; ienergy[0] + data->energy[1]; + + // take nstep steps with implicit, measure energy + model->opt.integrator = mjINT_IMPLICIT; + mj_resetData(model, data); + for (int i=0; ienergy[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; ienergy[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 diff --git a/test/engine/testdata/derivative/damped_actuators.xml b/test/engine/testdata/derivative/damped_actuators.xml new file mode 100644 index 00000000..a5b96110 --- /dev/null +++ b/test/engine/testdata/derivative/damped_actuators.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/derivative/energy_conserving_pendulum.xml b/test/engine/testdata/derivative/energy_conserving_pendulum.xml new file mode 100644 index 00000000..deed99b2 --- /dev/null +++ b/test/engine/testdata/derivative/energy_conserving_pendulum.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/derivative/tumbling_thin_object.xml b/test/engine/testdata/derivative/tumbling_thin_object.xml new file mode 100644 index 00000000..5ce2d95d --- /dev/null +++ b/test/engine/testdata/derivative/tumbling_thin_object.xml @@ -0,0 +1,17 @@ + + diff --git a/test/pipeline_test.cc b/test/pipeline_test.cc new file mode 100644 index 00000000..8ac963f4 --- /dev/null +++ b/test/pipeline_test.cc @@ -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 +#include +#include +#include +#include "src/engine/engine_io.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +std::vector AsVector(const mjtNum* array, int n) { + return std::vector(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 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 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 diff --git a/test/testdata/model.xml b/test/testdata/model.xml new file mode 100644 index 00000000..fbf9d079 --- /dev/null +++ b/test/testdata/model.xml @@ -0,0 +1,134 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +