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

Delete derivative code sample.

Browse Source 
PiperOrigin-RevId: 573477509
Change-Id: I0e619e36dd3fe677a27bbbdac87a15f04670cc13
This commit is contained in:
Yuval Tassa
2023-10-14 08:37:46 -07:00
committed by Copybara-Service
parent c0357ef3d0
commit a1b6026b8c
12 changed files with 20 additions and 537 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+3
View File
@@ -3275,6 +3275,9 @@ These matrices and their dimensions are:
- All outputs are optional (can be NULL).
- ``eps`` is the finite-differencing epsilon.
- ``flg_centered`` denotes whether to use forward (0) or centered (1) differences.
- Accuracy can be somewhat improved if solver :ref:`iterations<option-iterations>` are set to a
fixed (small) value and solver :ref:`tolerance<option-tolerance>` is set to 0. This insures that
all calls to the solver will perform exactly the same number of iterations.
.. _mjd_inverseFD:
doc/APIreference/functions_override.rst
+3
View File
@@ -463,6 +463,9 @@ These matrices and their dimensions are:
- All outputs are optional (can be NULL).
- ``eps`` is the finite-differencing epsilon.
- ``flg_centered`` denotes whether to use forward (0) or centered (1) differences.
- Accuracy can be somewhat improved if solver :ref:`iterations<option-iterations>` are set to a
fixed (small) value and solver :ref:`tolerance<option-tolerance>` is set to 0. This insures that
all calls to the solver will perform exactly the same number of iterations.
.. _mjd_inverseFD:
doc/changelog.rst
+8 -7
View File
@@ -142,15 +142,16 @@ General
`Levi Burner <https://github.com/aftersomemath>`__.
24. Deleted the code sample ``testxml``. The functionality provided by this utility is implemented in the
`WriteReadCompare <https://github.com/google-deepmind/mujoco/blob/main/test/xml/xml_native_writer_test.cc>`__ test.
25. Deleted the code sample ``derivative``. Functionality provided by :ref:`mjd_transitionFD`.
Python bindings
^^^^^^^^^^^^^^^
25. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
26. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
26. Added ``user_scn`` to the :ref:`passive viewer<PyViewerPassive>` handle, which allows users to add custom
27. Added ``user_scn`` to the :ref:`passive viewer<PyViewerPassive>` handle, which allows users to add custom
visualization geoms (`#1023 <https://github.com/google-deepmind/mujoco/issues/870>`__).
27. Added optional boolean keyword arguments ``show_left_ui`` and ``show_right_ui`` to the functions ``viewer.launch``
28. Added optional boolean keyword arguments ``show_left_ui`` and ``show_right_ui`` to the functions ``viewer.launch``
and ``viewer.launch_passive``, which allow users to launch a viewer with UI panels hidden.
Simulate
@@ -160,21 +161,21 @@ Simulate
:align: right
:width: 240px
28. Added **state history** mechanism to :ref:`simulate<saSimulate>` and the managed
29. Added **state history** mechanism to :ref:`simulate<saSimulate>` and the managed
:ref:`Python viewer<PyViewerManaged>`. State history can be viewed by scrubbing the History slider and (more
precisely) with the left and right arrow keys. See screen capture:
29. The ``LOADING...`` label is now shown correctly.
30. The ``LOADING...`` label is now shown correctly.
`Contribution <https://github.com/google-deepmind/mujoco/pull/1070>`__ by
`Levi Burner <https://github.com/aftersomemath>`__.
Bug fixes
^^^^^^^^^
30. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
31. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
midphase collision trees.
31. Fixed a bug that was generating incorrect values in ``efc_diagApprox`` for weld equality constraints.
32. Fixed a bug that was generating incorrect values in ``efc_diagApprox`` for weld equality constraints.
Version 2.3.7 (July 20, 2023)
doc/overview.rst
+1 -1
View File
@@ -98,7 +98,7 @@ Separation of model and data
:ref:`mjModel` is constructed by the compiler. :ref:`mjData` is constructed at runtime, given
:ref:`mjModel`. This separation makes it easy to simulate multiple models as well as multiple states and controls for
each model, in turn facilitating :ref:`multi-threading <siMultithread>` for sampling and :ref:`finite
differences <saDerivative>`. The top-level API functions reflect this basic separation, and have
differences <mjd_transitionFD>`. The top-level API functions reflect this basic separation, and have
the format:
.. code:: C
doc/programming/samples.rst
-66
View File
@@ -177,69 +177,3 @@ proliferation of overlapping technologies, which differ not only between platfor
case of Linux. The addition of a couple of extra functions (such as those provided by OSMesa for example) could have
avoided a lot of confusion. EGL is a newer standard from Khronos which aims to do this, and it is gaining popularity.
But we cannot yet assume that all users have it installed.
.. _saDerivative:
`derivative <https://github.com/google-deepmind/mujoco/blob/main/sample/derivative.cc>`_
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
This code sample illustrates the numerical approximation of forward and inverse dynamics derivatives via finite
differences. The process involves a number of epochs. In each epoch the simulation is advanced for a specified number
of steps, derivatives are computed at the last state, and timing and accuracy statistics are collected. The averages
over epochs are printed at the end.
The code can be incorporated in user projects where derivatives are needed, and can also be used as a stand-alone tool
for estimating CPU time and numerical accuracy. Accuracy is estimated in the function ``checkderiv()`` using several
mathematical identities about the derivatives of inverse functions; the residuals being computed would be zero if the
derivatives were exact. Note that these identities involve matrix multiplications which may affect the accuracy
estimates. Timing tests are applied only to the parallel section, where the function ``worker()`` is executed in
multiple threads using OpenMP. There are fewer threads than forward/inverse dynamics evaluations, thus each thread
executes multiple evaluations. For a more general discussion of parallel processing in MuJoCo see
:ref:`multi-threading <siMultithread>` below.
Recall than for a differentiable function ``f(x)`` the derivative can be approximated as
.. code-block:: Text
df/dx = (f(x+eps)-f(x))/eps
where ``eps`` is a small number. One can also use the centered finite difference method, which is two times slower but
more accurate. Here ``f`` is one of the functions
.. code-block:: Text
forward dynamics: qacc(qfrc_applied, qvel, qpos)
inverse dynamics: qfrc_inverse(qacc, qvel, qpos)
The code sample computes six Jacobian matrices, containing the derivative of each function with respect to its three
arguments. The results are stored in the array ``deriv``. All six Jacobian matrices are square, with dimensionality
equal to the number of degrees of freedom ``mjModel.nv``. When the model configuration includes quaternion joints,
mjData.qpos has larger dimensionality than the other vectors, however the derivative is only defined in the tangent
space to the configuration manifold. This is why, when differentiating with respect to the elements of ``mjData.qpos``,
we do not directly add ``eps`` but instead use the function :ref:`mju_quatIntegrate` to perturb the quaternion in the
tangent space, keeping it normalized. This technique should also be used in any other situation where quaternions need
to be perturbed.
There are some important subtleties in this code that improve speed as well as accuracy. To speed up the computation,
we re-use intermediate results whenever possible. This relies on the skip mechanism described under :ref:`forward
dynamics <siForward>` and :ref:`inverse dynamics <siInverse>` below. We first perturb force dimensions, keeping
position and velocity fixed. In this way we avoid recomputing results that depend on position and velocity but not on
force. Then we perturb velocity dimensions, and avoid recomputing results that depend on position but not on velocity
or force. Finally we perturb position dimensions - which requires full computation because everything depends on
position.
Accuracy depends on the value of ``eps`` which is user-adjustable, as well as the shape of the function. In the case
of forward dynamics however, the function evaluation involves an iterative constraint solver, and this must be handled
with care. In general, the difference between ``f(x+eps)`` and ``f(x)`` is very small, thus any noise affecting the
two function evaluations differently can make the resulting derivatives meaningless. Different warm-starts or
different number of solver iterations can act as such noise here. Therefore we fix the warm-start ``mjData.qacc`` to a
value pre-computed at the center point, using ``nwarmup`` extra major iterations to obtain a more accurate warm-start.
We also fix the number of solver iterations to ``niter`` and set ``mjModel.opt.tolerance = 0``; this disables the early
termination mechanism. Note that the original simulation options are restored in the serial code which advances the
state.
We emphasize that the above subtleties are not high-order corrections that can be incorporated later. In the presence
of unilateral constraints, numerical derivatives are hard to compute and there is no shortcut around it; indeed they
would not even be defined if it wasn't for our soft-constraint model. Making the constraints softer results in more
accurate results. This effect is so strong that in some situations it makes sense to intentionally work with the wrong
model, i.e., a model that is softer than desired, so as to obtain more accurate derivatives.
doc/programming/simulation.rst
+5 -10
View File
@@ -428,9 +428,7 @@ However, MuJoCo is designed not only for simulation but also for more advanced a
optimization, machine learning etc. In such settings one often needs to sample the dynamics at a cloud of nearby
states, or approximate derivatives via finite differences - which is another form of sampling. If the samples are
arranged on a grid, where only the position or only the velocity or only the control is different from the center
point, then the above mechanism can improve performance by about a factor of 2. The code sample :ref:`derivative.cc
<saDerivative>` illustrates this approach, and also shows how :ref:`multi-threading <siMultithread>` can be used for
additional speedup.
point, then the above mechanism can improve performance by about a factor of 2.
.. _siInverse:
@@ -494,10 +492,7 @@ bodies, we may eventually implement within-step multi-threading, but for now thi
Rather than speed up a single simulation, we prefer to use multi-threading to speed up sampling operations that are
common in more advanced applications. Simulation is inherently serial over time (the output of one mj_step is the
input to the next), while in sampling many calls to either forward or inverse dynamics can be executed in parallel
since there are no dependencies among them, except perhaps for a common initial state. The code sample
:ref:`derivative.cc <saDerivative>` illustrates one important example of sampling, namely the approximation of
dynamics derivatives via finite differences. Here we will not repeat the material from that section, but will instead
explain MuJoCo's general approach to parallel processing.
since there are no dependencies among them, except perhaps for a common initial state.
MuJoCo was designed for multi-threading from its beginning. Unlike most existing simulators where the notion of
dynamical system state is difficult to map to the software state and is often distributed among multiple objects, in
@@ -515,7 +510,7 @@ management.
// allocate per-thread mjData
mjData* d[64];
for( int n=0; n<nthread; n++ )
for( int n=0; n < nthread; n++ )
d[n] = mj_makeData(m);
// ... serial code, perhaps using its own mjData* dmain
@@ -541,8 +536,8 @@ writes to its own mjData. Therefore no further synchronization among threads is
The above template reflects a particular style of parallel processing. Instead of creating a large number of threads,
one for each work item, and letting OpenMP distribute them among processors, we rely on manual scheduling. More
precisely, we create as many threads as there are processors, and then within the ``worker`` function we distribute the
work explicitly among threads (not shown here, but see :ref:`derivative.cc <saDerivative>` for an example). This
approach is more efficient because the thread-specific mjData is large compared to the processor cache.
work explicitly among threads. This approach is more efficient because the thread-specific mjData is large compared to
the processor cache.
We also use a shared mjModel for cache-efficiency. In some situations it may not be possible to use the same mjModel
for all threads. One obvious reason is that mjModel may need to be modified within the thread function. Another reason
sample/CMakeLists.txt
-9
View File
@@ -76,18 +76,12 @@ target_compile_options(compile PUBLIC ${MUJOCO_SAMPLE_COMPILE_OPTIONS})
target_link_libraries(compile Threads::Threads)
target_link_options(compile PRIVATE ${MUJOCO_SAMPLE_LINK_OPTIONS})
add_executable(derivative derivative.cc)
target_compile_options(derivative PUBLIC ${MUJOCO_SAMPLE_COMPILE_OPTIONS})
target_link_libraries(derivative Threads::Threads)
target_link_options(derivative PRIVATE ${MUJOCO_SAMPLE_LINK_OPTIONS})
add_executable(testspeed testspeed.cc)
target_compile_options(testspeed PUBLIC ${MUJOCO_SAMPLE_COMPILE_OPTIONS})
target_link_libraries(testspeed Threads::Threads)
target_link_options(testspeed PRIVATE ${MUJOCO_SAMPLE_LINK_OPTIONS})
target_link_libraries(compile mujoco::mujoco)
target_link_libraries(derivative mujoco::mujoco)
target_link_libraries(testspeed mujoco::mujoco)
# Build samples that require GLFW.
@@ -115,7 +109,6 @@ target_link_options(record PRIVATE ${MUJOCO_SAMPLE_LINK_OPTIONS})
if(APPLE AND MUJOCO_BUILD_MACOS_FRAMEWORKS)
embed_in_bundle(basic simulate)
embed_in_bundle(compile simulate)
embed_in_bundle(derivative simulate)
embed_in_bundle(record simulate)
embed_in_bundle(testspeed simulate)
endif()
@@ -134,7 +127,6 @@ if(_INSTALL_SAMPLES)
TARGETS
basic
compile
derivative
record
testspeed
INSTALL_DIRECTORY
@@ -148,7 +140,6 @@ if(_INSTALL_SAMPLES)
install(
TARGETS basic
compile
derivative
record
testspeed
EXPORT ${PROJECT_NAME}
sample/Makefile
-1
View File
@@ -8,6 +8,5 @@ COMMON=-O2 -I../include -L../lib -std=c++17 -pthread -Wl,-no-as-needed -Wl,-rpat
all:
$(CXX) $(COMMON) testspeed.cc -lmujoco -o ../bin/testspeed
$(CXX) $(COMMON) compile.cc -lmujoco -o ../bin/compile
$(CXX) $(COMMON) derivative.cc -lmujoco -fopenmp -o ../bin/derivative
$(CXX) $(COMMON) basic.cc -lmujoco -lglfw -o ../bin/basic
$(CXX) $(COMMON) record.cc -lmujoco -lglfw -o ../bin/record
sample/Makefile.macos
-1
View File
@@ -14,6 +14,5 @@ ALLFLAGS=$(CXXFLAGS) -L$(GLFWROOT)/lib -Wl,-rpath,$(MUJOCOPATH)
all:
clang++ $(ALLFLAGS) testspeed.cc -framework mujoco -o testspeed
clang++ $(ALLFLAGS) compile.cc -framework mujoco -o compile
clang++ $(ALLFLAGS) derivative.cc -framework mujoco -o derivative
clang++ $(ALLFLAGS) basic.cc -framework mujoco -lglfw -o basic
clang++ $(ALLFLAGS) record.cc -framework mujoco -lglfw -o record
sample/Makefile.windows
-1
View File
@@ -12,7 +12,6 @@ COMMON=/O2 /MT /EHsc /arch:AVX /I../include /Fe../bin/
all:
cl $(COMMON) testspeed.cc ../lib/mujoco.lib
cl $(COMMON) compile.cc ../lib/mujoco.lib
cl $(COMMON) derivative.cc /openmp ../lib/mujoco.lib
cl $(COMMON) basic.cc ../lib/glfw3dll.lib ../lib/mujoco.lib
cl $(COMMON) record.cc ../lib/glfw3dll.lib ../lib/mujoco.lib
del *.obj
sample/derivative.cc
-439
View File
@@ -1,439 +0,0 @@
// 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.
#include <cstdio>
#include <cstring>
#include <mujoco/mujoco.h>
// enable compilation with and without OpenMP support
#if defined(_OPENMP)
#include <omp.h>
#else
// omp timer replacement
#include <chrono>
double omp_get_wtime(void) {
static std::chrono::system_clock::time_point _start = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed = std::chrono::system_clock::now() - _start;
return elapsed.count();
}
// omp functions used below
void omp_set_dynamic(int) {}
void omp_set_num_threads(int) {}
int omp_get_num_procs(void) {
return 1;
}
#endif
// gloval variables: internal
const int MAXTHREAD = 64; // maximum number of threads allowed
const int MAXEPOCH = 100; // maximum number of epochs
int isforward = 0; // dynamics mode: forward or inverse
mjtNum* deriv = 0; // dynamics derivatives (6*nv*nv):
// dinv/dpos, dinv/dvel, dinv/dacc, dacc/dpos, dacc/dvel, dacc/dfrc
// global variables: user-defined, with defaults
int nthread = 0; // number of parallel threads (default set later)
int niter = 30; // fixed number of solver iterations for finite-differencing
int nwarmup = 3; // center point repetitions to improve warmstart
int nepoch = 20; // number of timing epochs
int nstep = 500; // number of simulation steps per epoch
double eps = 1e-6; // finite-difference epsilon
// worker function for parallel finite-difference computation of derivatives
void worker(const mjModel* m, const mjData* dmain, mjData* d, int id) {
int nv = m->nv;
// allocate stack space for result at center
mj_markStack(d);
mjtNum* center = mj_stackAllocNum(d, nv);
mjtNum* warmstart = mj_stackAllocNum(d, nv);
// prepare static schedule: range of derivative columns to be computed by this thread
int chunk = (m->nv + nthread-1) / nthread;
int istart = id * chunk;
int iend = mjMIN(istart + chunk, m->nv);
// copy state and control from dmain to thread-specific d
d->time = dmain->time;
mju_copy(d->qpos, dmain->qpos, m->nq);
mju_copy(d->qvel, dmain->qvel, m->nv);
mju_copy(d->qacc, dmain->qacc, m->nv);
mju_copy(d->qacc_warmstart, dmain->qacc_warmstart, m->nv);
mju_copy(d->qfrc_applied, dmain->qfrc_applied, m->nv);
mju_copy(d->xfrc_applied, dmain->xfrc_applied, 6*m->nbody);
mju_copy(d->ctrl, dmain->ctrl, m->nu);
// run full computation at center point (usually faster than copying dmain)
if (isforward) {
mj_forward(m, d);
// extra solver iterations to improve warmstart (qacc) at center point
for (int rep=1; rep<nwarmup; rep++) {
mj_forwardSkip(m, d, mjSTAGE_VEL, 1);
}
} else {
mj_inverse(m, d);
}
// select output from forward or inverse dynamics
mjtNum* output = (isforward ? d->qacc : d->qfrc_inverse);
// save output for center point and warmstart (needed in forward only)
mju_copy(center, output, nv);
mju_copy(warmstart, d->qacc_warmstart, nv);
// select target vector and original vector for force or acceleration derivative
mjtNum* target = (isforward ? d->qfrc_applied : d->qacc);
const mjtNum* original = (isforward ? dmain->qfrc_applied : dmain->qacc);
// finite-difference over force or acceleration: skip = mjSTAGE_VEL
for (int i=istart; i<iend; i++) {
// perturb selected target
target[i] += eps;
// evaluate dynamics, with center warmstart
if (isforward) {
mju_copy(d->qacc_warmstart, warmstart, m->nv);
mj_forwardSkip(m, d, mjSTAGE_VEL, 1);
} else {
mj_inverseSkip(m, d, mjSTAGE_VEL, 1);
}
// undo perturbation
target[i] = original[i];
// compute column i of derivative 2
for (int j=0; j<nv; j++) {
deriv[(3*isforward+2)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
}
}
// finite-difference over velocity: skip = mjSTAGE_POS
for (int i=istart; i<iend; i++) {
// perturb velocity
d->qvel[i] += eps;
// evaluate dynamics, with center warmstart
if (isforward) {
mju_copy(d->qacc_warmstart, warmstart, m->nv);
mj_forwardSkip(m, d, mjSTAGE_POS, 1);
} else {
mj_inverseSkip(m, d, mjSTAGE_POS, 1);
}
// undo perturbation
d->qvel[i] = dmain->qvel[i];
// compute column i of derivative 1
for (int j=0; j<nv; j++) {
deriv[(3*isforward+1)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
}
}
// finite-difference over position: skip = mjSTAGE_NONE
for (int i=istart; i<iend; i++) {
// get joint id for this dof
int jid = m->dof_jntid[i];
// get quaternion address and dof position within quaternion (-1: not in quaternion)
int quatadr = -1, dofpos = 0;
if (m->jnt_type[jid]==mjJNT_BALL) {
quatadr = m->jnt_qposadr[jid];
dofpos = i - m->jnt_dofadr[jid];
} else if (m->jnt_type[jid]==mjJNT_FREE && i>=m->jnt_dofadr[jid]+3) {
quatadr = m->jnt_qposadr[jid] + 3;
dofpos = i - m->jnt_dofadr[jid] - 3;
}
// apply quaternion or simple perturbation
if (quatadr>=0) {
mjtNum angvel[3] = {0, 0, 0};
angvel[dofpos] = eps;
mju_quatIntegrate(d->qpos+quatadr, angvel, 1);
} else {
d->qpos[m->jnt_qposadr[jid] + i - m->jnt_dofadr[jid]] += eps;
}
// evaluate dynamics, with center warmstart
if (isforward) {
mju_copy(d->qacc_warmstart, warmstart, m->nv);
mj_forwardSkip(m, d, mjSTAGE_NONE, 1);
} else {
mj_inverseSkip(m, d, mjSTAGE_NONE, 1);
}
// undo perturbation
mju_copy(d->qpos, dmain->qpos, m->nq);
// compute column i of derivative 0
for (int j=0; j<nv; j++) {
deriv[(3*isforward+0)*nv*nv + i + j*nv] = (output[j] - center[j])/eps;
}
}
mj_freeStack(d);
}
// compute relative L1 norm of residual
double relnorm(mjtNum* residual, mjtNum* base, int n) {
mjtNum L1res = 0, L1base = 0;
for (int i=0; i<n; i++) {
L1res += mju_abs(residual[i]);
L1base += mju_abs(base[i]);
}
return (double) mju_log10(mju_max(mjMINVAL, L1res/mju_max(mjMINVAL, L1base)));
}
// names of residuals for accuracy check
const char* accuracy[8] = {
"G2*F2 - I ",
"G2 - G2' ",
"G1 - G1' ",
"F2 - F2' ",
"G1 + G2*F1",
"G0 + G2*F0",
"F1 + F2*G1",
"F0 + F2*G0"
};
// check accuracy of derivatives using known mathematical identities
void checkderiv(const mjModel* m, mjData* d, mjtNum error[7]) {
int nv = m->nv;
// allocate space
mj_markStack(d);
mjtNum* mat = mj_stackAllocNum(d, nv*nv);
// get pointers to derivative matrices
mjtNum* G0 = deriv; // dinv/dpos
mjtNum* G1 = deriv + nv*nv; // dinv/dvel
mjtNum* G2 = deriv + 2*nv*nv; // dinv/dacc
mjtNum* F0 = deriv + 3*nv*nv; // dacc/dpos
mjtNum* F1 = deriv + 4*nv*nv; // dacc/dvel
mjtNum* F2 = deriv + 5*nv*nv; // dacc/dfrc
// G2*F2 - I
mju_mulMatMat(mat, G2, F2, nv, nv, nv);
for (int i=0; i<nv; i++) {
mat[i*(nv+1)] -= 1;
}
error[0] = relnorm(mat, G2, nv*nv);
// G2 - G2'
mju_transpose(mat, G2, nv, nv);
mju_sub(mat, mat, G2, nv*nv);
error[1] = relnorm(mat, G2, nv*nv);
// G1 - G1'
mju_transpose(mat, G1, nv, nv);
mju_sub(mat, mat, G1, nv*nv);
error[2] = relnorm(mat, G1, nv*nv);
// F2 - F2'
mju_transpose(mat, F2, nv, nv);
mju_sub(mat, mat, F2, nv*nv);
error[3] = relnorm(mat, F2, nv*nv);
// G1 + G2*F1
mju_mulMatMat(mat, G2, F1, nv, nv, nv);
mju_addTo(mat, G1, nv*nv);
error[4] = relnorm(mat, G1, nv*nv);
// G0 + G2*F0
mju_mulMatMat(mat, G2, F0, nv, nv, nv);
mju_addTo(mat, G0, nv*nv);
error[5] = relnorm(mat, G0, nv*nv);
// F1 + F2*G1
mju_mulMatMat(mat, F2, G1, nv, nv, nv);
mju_addTo(mat, F1, nv*nv);
error[6] = relnorm(mat, F1, nv*nv);
// F0 + F2*G0
mju_mulMatMat(mat, F2, G0, nv, nv, nv);
mju_addTo(mat, F0, nv*nv);
error[7] = relnorm(mat, F0, nv*nv);
mj_freeStack(d);
}
// main function
int main(int argc, char** argv) {
// print help if not enough arguments
if (argc<2) {
std::printf("\n Arguments: modelfile [nthread niter nwarmup nepoch nstep eps]\n\n");
return 1;
}
// default nthread = number of logical cores (usually optimal)
nthread = omp_get_num_procs();
// get numeric command-line arguments
if (argc>2) {
std::sscanf(argv[2], "%d", &nthread);
}
if (argc>3) {
std::sscanf(argv[3], "%d", &niter);
}
if (argc>4) {
std::sscanf(argv[4], "%d", &nwarmup);
}
if (argc>5) {
std::sscanf(argv[5], "%d", &nepoch);
}
if (argc>6) {
std::sscanf(argv[6], "%d", &nstep);
}
if (argc>7) {
std::sscanf(argv[7], "%lf", &eps);
}
// check number of threads
if (nthread<1 || nthread>MAXTHREAD) {
std::printf("nthread must be between 1 and %d\n", MAXTHREAD);
return 1;
}
// check number of epochs
if (nepoch<1 || nepoch>MAXEPOCH) {
std::printf("nepoch must be between 1 and %d\n", MAXEPOCH);
return 1;
}
// load model
mjModel* m = 0;
if (std::strlen(argv[1])>4 && !std::strcmp(argv[1]+std::strlen(argv[1])-4, ".mjb")) {
m = mj_loadModel(argv[1], NULL);
} else {
m = mj_loadXML(argv[1], NULL, NULL, 0);
}
if (!m) {
std::printf("Could not load modelfile '%s'\n", argv[1]);
return 1;
}
// print arguments
#if defined(_OPENMP)
std::printf("\nnthread : %d (OpenMP)\n", nthread);
#else
std::printf("\nnthread : %d (serial)\n", nthread);
#endif
std::printf("niter : %d\n", niter);
std::printf("nwarmup : %d\n", nwarmup);
std::printf("nepoch : %d\n", nepoch);
std::printf("nstep : %d\n", nstep);
std::printf("eps : %g\n\n", eps);
// make mjData: main, per-thread
mjData* dmain = mj_makeData(m);
mjData* d[MAXTHREAD];
for (int n=0; n<nthread; n++) {
d[n] = mj_makeData(m);
}
// allocate derivatives
deriv = (mjtNum*) mju_malloc(6*sizeof(mjtNum)*m->nv*m->nv);
// set up OpenMP (if not enabled, this does nothing)
omp_set_dynamic(0);
omp_set_num_threads(nthread);
// save solver options
int save_iterations = m->opt.iterations;
mjtNum save_tolerance = m->opt.tolerance;
// allocate statistics
int nefc = 0;
double cputm[MAXEPOCH][2];
mjtNum error[MAXEPOCH][8];
// run epochs, collect statistics
for (int epoch=0; epoch<nepoch; epoch++) {
// set solver options for main simulation
m->opt.iterations = save_iterations;
m->opt.tolerance = save_tolerance;
// advance main simulation for nstep
for (int i=0; i<nstep; i++) {
mj_step(m, dmain);
}
// count number of active constraints
nefc += dmain->nefc;
// set solver options for finite differences
m->opt.iterations = niter;
m->opt.tolerance = 0;
// test forward and inverse
for (isforward=0; isforward<2; isforward++) {
// start timer
double starttm = omp_get_wtime();
// run worker threads in parallel if OpenMP is enabled
#pragma omp parallel for schedule(static)
for (int n=0; n<nthread; n++) {
worker(m, dmain, d[n], n);
}
// record duration in ms
cputm[epoch][isforward] = 1000*(omp_get_wtime() - starttm);
}
// check derivatives
checkderiv(m, d[0], error[epoch]);
}
// compute statistics
double mcputm[2] = {0, 0}, merror[8] = {0, 0, 0, 0, 0, 0, 0, 0};
for (int epoch=0; epoch<nepoch; epoch++) {
mcputm[0] += cputm[epoch][0];
mcputm[1] += cputm[epoch][1];
for (int ie=0; ie<8; ie++) {
merror[ie] += error[epoch][ie];
}
}
// print sizes, timing, accuracy
std::printf("sizes : nv %d, nefc %d\n\n", m->nv, nefc/nepoch);
std::printf("inverse : %.2f ms\n", mcputm[0]/nepoch);
std::printf("forward : %.2f ms\n\n", mcputm[1]/nepoch);
std::printf("accuracy: log10(residual L1 relnorm)\n");
std::printf("------------------------------------\n");
for (int ie=0; ie<8; ie++) {
std::printf(" %s : %.2g\n", accuracy[ie], merror[ie]/nepoch);
}
std::printf("\n");
// shut down
mju_free(deriv);
mj_deleteData(dmain);
for (int n=0; n<nthread; n++) {
mj_deleteData(d[n]);
}
mj_deleteModel(m);
return 0;
}
src/engine/engine_derivative_fd.c
-2
View File
@@ -309,10 +309,8 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
int ndx = 2*nv+na; // row length of Dy Jacobians
mj_markStack(d);
// states
// state to restore after finite differencing
unsigned int restore_spec = mjSTATE_FULLPHYSICS | mjSTATE_CTRL;
restore_spec |= mjDISABLED(mjDSBL_WARMSTART) ? 0 : mjSTATE_WARMSTART;
mjtNum *fullstate = mj_stackAllocNum(d, mj_stateSize(m, restore_spec));