Mujoco编程开发-官方文档

https://mujoco.readthedocs.io/en/latest/programming.html

复制一份到这里，后面需要做笔记

---

Introduction

This chapter is the MuJoCo programming guide. A separate chapter contains the API Reference documentation. MuJoCo is a dynamic library compatible with Windows, Linux and macOS, which requires a process with AVX instructions. The library exposes the full functionality of the simulator through a compiler-independent shared-memory C API. It can also be used in C++ programs.

MuJoCo is a free product currently distributed as a pre-built dynamic library and will soon be made available as an open-source project. The software distribution contains a compiled version of GLFW which is used in the code samples to create an OpenGL window and direct user input to it. The Linux distribution also contains compiled versions of GLEW which is used for OpenGL symbol loading and allow the user to choose between X11, EGL or MESA. For projects where rendering is not needed, there is a smaller “nogl” library which does not have a dependence on OpenGL. This is suitable for running on headless servers without video drivers installed. The distribution for each platform contains the following dynamic libraries:

Windows:  mujoco210.dll     (stub library: mujoco210.lib)
          mujoco210nogl.dll (stub library: mujoco210nogl.lib)
          glfw3.dll         (stub library: glfw3.lib)

Linux:    mujoco210.so
          mujoco210nogl.so
          glewXXX.so
          glfw.so.3

macOS:    mujoco210.dylib
          mujoco210nogl.dylib
          glfw.3.dylib

Note that the software version number is contained in the library name (and obviously changes with every release), so for example mujoco210 means version 2.1.

Even though MuJoCo is a single dynamic library with unified C API, it contains several modules, some of which are implemented in C++. We have taken advantage of the convenience of C++ for functionality that is used before the simulation starts (namely the parser and compiler), and have gone to the trouble of writing carefully-tuned C code for all runtime functionality. The modules are:

Parser
The XML parser is written in C++. It can parse MJCF models and URDF models, converting them into an internal mjCModel C++ object which is not directly exposed to the user.

Compiler
The compiler is written in C++. It takes an mjCModel C++ object constructed by the parser, and converts it into an mjModel C structure used at runtime.

Simulator
The simulator (or physics engine) is written in C. It is responsible for all runtime computations.

Abstract visualizer
The abstract visualizer is written in C. It generates a list of abstract geometric entities representing the simulation state, with all information needed for actual rendering. It also provides abstract mouse hooks for camera and perturbation control.

OpenGL renderer
The renderer is written in C and is based on fixed-function OpenGL. It does not have all the features of state-of-the-art rendering engines (and can be replaced with such an engine if desired) but nevertheless it provides efficient and informative 3D rendering.

UI framework
The UI framework (new in MuJoCo 2.0) is written in C. UI elements are rendered in OpenGL. It has its own event mechanism and abstract hooks for keyboard and mouse input. The code samples use it with GLFW, but it can also be used with other window libraries.

Getting started

The software distribution is a single .zip (Windows) or .tar.gz (Mac and Linux) archive whose name contains the platform and software version, e.g. mujoco210_windows.zip. There is no installer. Simply unzip this archive in a directory of your choice (where you have write access). You may need to use chmod to set execute permissions or otherwise give permissions to run the libraries. From the bin subdirectory, you can now run the precompiled code samples, for example:

Windows:           simulate ..\model\humanoid.xml
Linux and macOS:   ./simulate ../model/humanoid.xml

Prior to MuJoCo 2.0, running the code samples needed LD_LIBRARY_PATH on Linux. As of MuJoCo 2.0, they are compiled with “rpath $ORIGIN” so the library is found in the executable directory (if it is not in the path).

The directory structure is shown below. Users can re-organize it if needed, as well as install the dynamic libraries in other directories and set the path accordingly. The only file created automatically is MUJOCO_LOG.TXT in the executable directory; it contains error and warning messages, and can be deleted at any time.

mujoco210
  bin     - dynamic libraries, executables, MUJOCO_LOG.TXT
  doc     - README.txt and REFERENCE.txt
  include - header files needed to develop with MuJoCo
  model   - model collection (extra models available on the Forum)
  sample  - code samples and makefile need to build them

After verifying that the simulator works, the next step is to re-compile the code samples so as to ensure that the development environment is properly installed. The distribution includes a platform-specific makefile in the sample subdirectory, which assumes Visual Studio on Windows, GCC on Linux and Clang on macOS. On Windows, remember to open a Visual Studio command prompt with native x64 tools. Assuming the compilation succeeded and the resulting executables in the bin subdirectory work, you are ready to start developing with MuJoCo.

As already mentioned, MuJoCo is a compiler-independent library. In theory the user should be able to switch to any compiler of their choice. In practice we are using C++11 features as well as std:: functionality internally, and despite our efforts to statically link all necessary runtime libraries, this is not always possible - especially on Linux where licensing restrictions prevent static linking. If MuJoCo fails to start because of missing or incompatible dynamic libraries, please install the necessary libraries.

Header files

The distribution contains several header files which are identical on all platforms. They are also available from the links below, to make this documentation self-contained.
mujoco.h (source)
This is the main header file and must be included in all programs using MuJoCo. It defines all API functions and global variables, and includes the next 5 files which provide the necessary type definitions.

mjmodel.h (source)
This file defines the C structure mjModel which is the runtime representation of the model being simulated. It also defines a number of primitive types and other structures needed to define mjModel.

mjdata.h (source)
This file defines the C structure mjData which is the workspace where all computations read their inputs and write their outputs. It also defines primitive types and other structures needed to define mjData.

mjvisualize.h (source)
This file defines the primitive types and structures needed by the abstract visualizer.

mjrender.h (source)
This file defines the primitive types and structures needed by the OpenGL renderer.

mjui.h (source)
This file defines the primitive types and structures needed by the UI framework.

mjxmacro.h (source)
This file is optional and is not included by mujoco.h. It defines X Macros that can automate the mapping of mjModel and mjData into scripting languages, as well as other operations that require accessing all fields of mjModel and mjData. See code sample testxml.cc.

glfw3.h
This file is optional and is not included by mujoco.h. It is the only header file needed for the GLFW library. See code sample simulate.cc.

Naming convention

All symbols defined in the API start with the prefix “mj”. The character after “mj” in the prefix determines the family to which the symbol belongs. First we list the prefixes corresponding to type definitions.

mj
Core simulation data structure (C struct), for example mjModel. If all characters after the prefix are capital, for example mjMIN, this is a macro or a symbol (#define).

mjt
Primitive type, for example mjtGeom. Except for mjtByte and mjtNum, all other definitions in this family are enums.

mjf
Callback function type, for example mjfGeneric.

mjv
Data structure related to abstract visualization, for example mjvCamera.

mjui
Data structure related to UI framework, for example mjuiSection.

Next we list the prefixes corresponding to function definitions. Note that function prefixes always end with underscore.

mj_
Core simulation function, for example mj_step. Almost all such functions have pointers to mjModel and mjData as their first two arguments, possibly followed by other arguments. They usually write their outputs to mjData.

mju_
Utility function, for example mju_mulMatVec. These functions are self-contained in the sense that they do not have mjModel and mjData pointers as their arguments.

mjv_
Function related to abstract visualization, for example mjv_updateScene.

mjr_
Function related to OpenGL rendering, for example mjr_render.

mjui_
Function related to UI framework, for example mjui_update.

mjcb_
Global callback function pointer, for example mjcb_control. The user can install custom callbacks by setting these global pointers to user-defined functions.

---

Code samples 官方自带例子代码介绍

MuJoCo comes with several code samples providing useful functionality. Some of them are quite elaborate (simulate.cc in particular) but nevertheless we hope that they will help users learn how to program with the library.

testspeed.cc

This code sample tests the simulation speed for a given model. The command line arguments are the model file, the number of time steps to simulate, the number of parallel threads to use, and a flag to enable internal profiling (the last two are optional). When N threads are specified with N>1, the code allocates a single mjModel and per-thread mjData, and runs N identical simulations in parallel. The idea is to test performance with all cores active, similar to Reinforcement Learning scenarios where samples are collected in parallel. The optimal N usually equals the number of logical cores. By default the simulation starts from the model reference configuration qpos0 and qvel=0. However if a keyframe named “test” is present in the model, it is used as the initial state state.

The timing code is straightforward: the simulation of the passive dynamics is advanced for the specified number of steps, while collecting statistics about the number of contacts, scalar constraints, and CPU times from internal profiling. The results are then printed in the console. To simulate controlled dynamics instead of passive dynamics one can either install the control callback mjcb_control, or set control signals explicitly as explained in the simulation loop section below.

testxml.cc

This code sample tests the parser, compiler and XML writer. The testing code does the following:

1. Parse and compile a specified XML model in MJCF or URDF. This yields an mjModel structure ready for simulation;

2. Save the model as a temporary MJCF file, using a “canonical” subset of MJCF where a number of conversions have already been performed by the compiler;

3. Parse and compile the temporary MJCF file. This yields a second mjModel structure ready for simulation;

4. Compare the two mjModel structures field by field, and print the field with the largest numerical difference. Since MJCF is a text format, the real-valued numbers saved in it have lower precision than the double precision used internally, thus we cannot expect the two models to be identical on the bit level. But we can expect the largest difference to be on the order of 1e-6. A substantially larger difference indicates a bug in the parser, compiler or XML writer - and should be reported.

The code uses the X Macros described in the Reference chapter. This is a convenient way to apply the same operation to all fields in mjModel, without explicitly typing their names. The code sample simulate.cc also uses X Macros to implement a watch, where the user can type the name of any mjData field which is resolved at runtime.

compile.cc

This code sample evokes the built-in parser and compiler. It implements all possible model conversions from (MJCF, URDF, MJB) format to (MJCF, MJB, TXT) format. Models saved as MJCF use a canonical subset of our format as described in the Modeling chapter, and therefore MJCF-to-MJCF conversion will generally result in a different file. The TXT format is a human-readable road-map to the model. It cannot be loaded by MuJoCo, but can be a very useful aid during model development. It is in one-to-one correspondence with the compiled mjModel. Note also that one can use the function mj_printData to create a text file which is in one-to-one correspondence with mjData, although this is not done by the code sample.

basic.cc

This code sample is a minimal interactive simulator. The model file must be provided as command-line argument. It opens an OpenGL window using the platform-independent GLFW library, and renders the simulation state at 60 fps while advancing the simulation in real-time. Press Backspace to reset the simulation. The mouse can be used to control the camera: left drag to rotate, right drag to translate in the vertical plane, shift right drag to translate in the horizontal plane, scroll or middle drag to zoom.

The Visualization programming guide below explains how visualization works. This code sample is a minimal illustration of the concepts in that guide.

simulate.cc

This code sample is a full-featured interactive simulator. It opens an OpenGL window using the platform-independent GLFW library, and renders the simulation state in it. There is built-in help, simulation statistics, profiler, sensor data plots. The model file can be specified as a command-line argument, or loaded at runtime using drag-and-drop functionality. As of MuJoCo 2.0, this code sample uses the native UI to render various controls, and provides an illustration of how the new UI framework is intended to be used.

Interaction is done with the mouse; see the built-in help for summary of available commands. Briefly, an object is selected by left-double-click. The user can then apply forces and torques on the selected object by holding Ctrl and dragging the mouse. Dragging the mouse alone (without Ctrl) moves the camera. There are keyboard shortcuts for pausing the simulation, resetting, and re-loading the model file. The latter functionality is very useful while editing the model in an XML editor.

The code is quite long yet reasonably commented, so it is best to just read it. Here we provide a high-level overview. The main() function initializes both MuJoCo and GLFW, opens a window, and install GLFW callbacks for mouse and keyboard handling. Note that there is no render callback; GLFW puts the user in charge, instead of running a rendering loop behind the scenes. The main loop handles UI events and rendering. The simulation is handled in a background thread, which is synchronized with the main thread.

The mouse and keyboard callbacks perform whatever action is necessary. Many of these actions invoke functionality provided by MuJoCo’s abstract visualization mechanism. Indeed this mechanism is designed to be hooked to mouse and keyboard events more or less directly, and provides camera as well as perturbation control.

The profiler and sensor data plots illustrate the use of the mjr_figure function that can plot elaborate 2D figures with grids, annotation, axis scaling etc. The information presented in the profiler is extracted from the diagnostic fields of mjData. It is a very useful tool for tuning the parameters of the constraint solver algorithms. The outputs of the sensors defined in the model are visualized as a bar graph.

Note that the profiler shows timing information collected with high-resolution timers. On Windows, depending on the power settings, the OS may reduce the CPU frequency; this is because simulate.cc sleeps most of the time in order to slow down to realtime. This results in inaccurate timings. To avoid this problem, change the Windows power plan so that the minimum processor state is 100%.

record.cc

This code sample simulates the passive dynamics of a given model, renders it offscreen, reads the color and depth pixel values, and saves them into a raw data file that can then be converted into a movie file with tools such as ffmpeg. The rendering is simplified compared to simulate.cc because there is no user interaction, visualization options or timing; instead we simply render with the default settings as fast as possible. The dimensions and number of multi-samples for the offscreen buffer are specified in the MuJoCo model, while the simulation duration, frames-per-second to be rendered (usually much less than the physics simulation rate), and output file name are specified as command-line arguments. For example, a 5 second animation at 60 frames per second is created with:

record humanoid.xml 5 60 rgb.out

The default humanoid.xml model specifies offscreen rendering with 800x800 resolution. With this information in hand, we can compress the (large) raw date file into a playable movie file:

ffmpeg -f rawvideo -pixel_format rgb24 -video_size 800x800
  -framerate 60 -i rgb.out -vf "vflip" video.mp4

This sample can be compiled in three ways which differ in how the OpenGL context is created: using GLFW with an invisible window, using OSMesa, or using EGL. The latter two options are only available on Linux and are envoked by defining the symbols MJ_OSMESA or MJ_EGL when compiling record.cc. The functions initOpenGL and closeOpenGL create and close the OpenGL context in three different ways depending on which of the above symbols is defined.

Note that the MuJoCo rendering code does not depend on how the OpenGL context was created. This is the beauty of OpenGL: it leaves context creation to the platform, and the actual rendering is then standard and works in the same way on all platforms. In retrospect, the decision to leave context creation out of the standard has led to unnecessary proliferation of overlapping technologies, which differ not only between platforms but also within a platform in the 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.

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 multi-threading below.

Recall than for a differentiable function f(x) the derivative can be approximated as

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

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 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 forward dynamics and inverse dynamics 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.

uitools

(uitools.h) (uitools.c) This is not a stand-alone code sample, but rather a small utility used to hook up the new UI to GLFW. It is used in simulate.cc and can also be used in user projects that involve the new UI. If GLFW is replaced with a different window library, this is the only file that would have to be changed in order to access the UI functionality.

---

Simulation

一 Initialization

After the version check(现在开源了，不需要了), the next step is to allocate and initialize the main data structures needed for simulation, namely mjModel and mjData. Additional initialization steps related to visualization and callbacks will be discussed later.

mjModel and mjData should never be allocated directly by the user. Instead they are allocated and initialized by the corresponding API functions. These are very elaborate data structures, containing (arrays of) other structures, pre-allocated data arrays for all intermediate results, as well as an internal stack. Our strategy is to allocate all necessary heap memory at the beginning of the simulation, and free it after the simulation is done, so that we never have to call the C memory allocation and deallocation functions during the simulation. This is done for speed, avoidance of memory fragmentation, future GPU portability, and ease of managing the state of the entire simulator during a reset. It also means however that the maximal sizes njmax, nconmax and nstack in the XML element size, which affect the allocation of mjData, must be set to sufficiently large values. If these maximal sizes are exceeded during the simulation, they are not increased dynamically, but instead errors or warnings are generated. See also diagnostics below.

1 加载模型，创建mjModel

First we must call one of the functions that allocates and initializes mjModel and returns a pointer to it. The available options are

// option 1: parse and compile XML from file
mjModel* m = mj_loadXML("mymodel.xml", NULL, errstr, errstr_sz);

// option 2: parse and compile XML from virtual file system
mjModel* m = mj_loadXML("mymodel.xml", vfs, errstr_sz);

// option 3: load precompiled model from MJB file
mjModel* m = mj_loadModel("mymodel.mjb", NULL);

// option 4: load precompiled model from virtual file system
mjModel* m = mj_loadModel("mymodel.mjb", vfs);

// option 5: deep copy from existing mjModel
mjModel* m = mj_copyModel(NULL, mexisting);

All these functions return a NULL pointer if there is an error or warning. In the case of XML parsing and model compilation, a description of the error is returned in the string provided as argument. For the remaining functions, the low-level mju_error or mju_warning is called with the error/warning message; see error handling. Once we have a pointer to the mjModel that was allocated by one of the above functions, we pass it as argument to all API functions that need model access. Note that most functions treat this pointer as const; more on this in model changes below.

The virtual file system (VFS) was introduced in MuJoCo 1.50. It allows disk resources to be loaded in memory or created programmatically by the user, and then MuJoCo’s load functions search for files in the VFS before accessing the disk. See Virtual file system in the API Reference chapter.

2 创建模拟需要的workspace对象mjModel

In addition to mjModel which holds the model description, we also need mjData which is the workspace where all computations are performed. Note that mjData is specific to a given mjModel. The API functions generally assume that users know what they are doing, and perform minimal argument checking. If the mjModel and mjData passed to any API function are incompatible (or NULL) the resulting behavior is unpredictable. mjData is created with

// option 1: create mjDada corresponding to given mjModel
mjData* d = mj_makeData(m);

// option 2: deep copy from existing mjData
mjData* d = mj_copyData(NULL, m, dexisting);

mjModel,mjData 释放

Once both mjModel and mjData are allocated and initialized, we can call the various simulation functions. When we are done, we can delete them with

// deallocate existing mjModel
mj_deleteModel(m);

// deallocate existing mjData
mj_deleteData(d);

The code samples illustrate the complete initialization and termination sequence.

MuJoCo simulations are deterministic with one exception: sensor noise can be generated when this feature is enabled. This is done by calling the C function rand() internally. To generate the same random number sequence, call srand() with a desired seed after the model is loaded and before the simulation starts. The model compiler calls srand(123) internally, so as to generate random dots for procedural textures. Therefore the noise sequence in the sensor data will change if the specification of procedural textures changes, and the user does not call srand() after model compilation.

二 Simulation loop

There are multiple ways to run a simulation loop in MuJoCo. The simplest way is to call the top-level simulation function mj_step in a loop such as

// simulate until t = 10 seconds
while( d->time<10 )
    mj_step(m, d);

This by itself will simulate the passive dynamics, because we have not provided any control signals or applied forces. The default (and recommended) way to control the system is to implement a control callback, for example

// simple controller applying damping to each dof
void mycontroller(const mjModel* m, mjData* d)
{
    if( m->nu==m->nv )
        mju_scl(d->ctrl, d->qvel, -0.1, m->nv);
}

This illustrates two concepts. First, we are checking if the number of controls mjModel.nu equals the number of dofs mjModel.nv. In general, the same callback may be used with multiple models depending on how the user code is structured, and so it is a good idea to check the model dimensions in the callback. Second, MuJoCo has a library of BLAS-like functions that are very useful; indeed a large part of the code base consists of calling such functions internally. The mju_scl function above scales the velocity vector mjData.qvel by a constant feedback gain and copies the result into the control vector mjData.ctrl. To install this callback, we simply assign it to the global control callback pointer mjcb_control:

// install control callback
mjcb_control = mycontroller;

Now if we call mj_step, our control callback will be executed whenever the control signal is needed by the simulation pipeline, and as a result we will end up simulating the controlled dynamics (except damping does not really do justice to the notion of control, and is better implemented as a passive joint property, but these are finer points).

Instead of relying on a control callback, we could set the control vector mjData.ctrl directly. Alternatively we could set applied forces as explained in state and control. If we could compute these control-related quantities before mj_step is called, then the simulation loop for the controlled dynamics (without using a control callback) would become

while( d->time<10 )
{
    // set d->ctrl or d->qfrc_applied or d->xfrc_applied
    mj_step(m, d);
}

Why would we not be able to compute the controls before mj_step is called? After all, isn’t this what causality means? The answer is subtle but important, and has to do with the fact that we are simulating in discrete time. The top-level simulation function mj_step basically does two things: compute the forward dynamics in continuous time, and then integrate over a time period specified by mjModel.opt.timestep. Forward dynamics computes the acceleration mjData.qacc at time mjData.time, given the state and control at time mjData.time. The numerical integrator then advances the state and time to mjData.time + mjModel.opt.timestep. Now, the control is required to be a function of the state at time mjData.time. However a general feedback controller can be a very complex function, depending on various features of the state - in particular all the features computed by MuJoCo as intermediate results of the simulation. These may include contacts, Jacobians, passive forces. None of these quantities are available before mj_step is called (or rather, they are available but outdated by one time step). In contrast, when mj_step calls our control callback, it does so as late in the computation as possible - namely after all the intermediate results dependent on the state but not on the control have been computed.

为什么不在mj_step之前执行控制计算？
首先我们是使用离散时间进行模拟的。mj_step主要做2件事情：
1 计算连续时间下的正动力学，然后在一个时间区间上计算动力学的积分。正动力学计算
与时间相关的加速度，状态。
2 调用控制计算
因为控制计算有可能是十分复杂的，可能会依赖状态变量，有可能包含接触点，雅可比，欠驱动力。这些量
在mj_step执行之前都是没有准备好的（当然，存在过时的上一步的数据）。
因此，mj_step会尽可能的延迟控制计算的调用

The same effect can be achieved without using a control callback. This is done by breaking mj_step in two parts: before the control is needed, and after the control is needed. The simulation loop now becomes

while( d->time<10 )
{
    mj_step1(m, d);
    // set d->ctrl or d->qfrc_applied or d->xfrc_applied
    mj_step2(m, d);
}

There is one complication however: this only works with Euler integration. The Runge-Kutta integrator (as well as other advanced integrators we plan to implement) need to evaluate the entire dynamics including the feedback control law multiple times per step, which can only be done using a control callback. But with Euler integration, the above separation of mj_step into mj_step1 and mj_step2 is sufficient to provide the control law with the intermediate results of the computation.

To make the above discussion more clear, we provide the internal implementation of mj_step, mj_step1 and mj_step2, omitting some code that computes timing diagnostics. The main simulation function is

void mj_step(const mjModel* m, mjData* d)
{
    // common to all integrators
    mj_checkPos(m, d);
    mj_checkVel(m, d);
    mj_forward(m, d);
    mj_checkAcc(m, d);

    // compare forward and inverse solutions if enabled
    if( mjENABLED(mjENBL_FWDINV) )
        mj_compareFwdInv(m, d);

    // use selected integrator
    if( m->opt.integrator==mjINT_RK4 )
        mj_RungeKutta(m, d, 4);
    else
        mj_Euler(m, d);
}

The checking functions reset the simulation automatically if any numerical values have become invalid or too large. The control callback (if any) is called from within the forward dynamics function.

Next we show the implementation of the two-part stepping approach, although the specifics will make sense only after we explain the forward dynamics later. Note that the control callback is now called directly, since we have essentially unpacked the forward dynamics function. Note also that we always call the Euler integrator in mj_step2 regardless of the setting of mjModel.opt.integrator.

void mj_step1(const mjModel* m, mjData* d)
{
    mj_checkPos(m, d);
    mj_checkVel(m, d);
    mj_fwdPosition(m, d);
    mj_sensorPos(m, d);
    mj_energyPos(m, d);
    mj_fwdVelocity(m, d);
    mj_sensorVel(m, d);
    mj_energyVel(m, d);

    // if we had a callback we would be using mj_step, but call it anyway
    if( mjcb_control )
        mjcb_control(m, d);
}

void mj_step2(const mjModel* m, mjData* d)
{
    mj_fwdActuation(m, d);
    mj_fwdAcceleration(m, d);
    mj_fwdConstraint(m, d);
    mj_sensorAcc(m, d);
    mj_checkAcc(m, d);

    // compare forward and inverse solutions if enabled
    if( mjENABLED(mjENBL_FWDINV) )
        mj_compareFwdInv(m, d);

    // integrate with Euler; ignore integrator option
    mj_Euler(m, d);
}

State and control