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

Implement plugin mechanism for actuators and sensors.

Browse Source 
PiperOrigin-RevId: 474874088
Change-Id: I65a8ffdf845f4fa0f8266c165883a747ab5812d8
This commit is contained in:
Saran Tunyasuvunakool
2022-09-16 12:20:56 -07:00
committed by Copybara-Service
parent f556d4d94f
commit 1e2a9a53bc
29 changed files with 2121 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/CMakeLists.txt
+2
View File
@@ -39,6 +39,8 @@ set(MUJOCO_ENGINE_SRCS
engine_io.c
engine_io.h
engine_macro.h
engine_plugin.cc
engine_plugin.h
engine_print.c
engine_print.h
engine_ray.c
src/engine/engine_forward.c
+44
View File
@@ -19,6 +19,7 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_callback.h"
#include "engine/engine_collision_driver.h"
#include "engine/engine_core_constraint.h"
@@ -27,6 +28,7 @@
#include "engine/engine_inverse.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_sensor.h"
#include "engine/engine_solver.h"
#include "engine/engine_support.h"
@@ -194,6 +196,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// force = gain .* [ctrl/act] + bias
for (int i=0; i<nu; i++) {
// skip actuator plugins -- these are handled after builtin actuator types
if (m->actuator_plugin[i] >= 0) {
continue;
}
// extract gain info
prm = m->actuator_gainprm + mjNGAIN*i;
@@ -262,6 +269,24 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
force[i] += bias;
}
// handle actuator plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if (plugin->type & mjPLUGIN_ACTUATOR) {
if (!plugin->compute) {
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
}
plugin->compute(m, d, i, mjPLUGIN_ACTUATOR);
}
}
}
// clamp actuator_force
for (int i=0; i<nu; i++) {
if (m->actuator_forcelimited[i]) {
@@ -275,6 +300,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// act_dot for stateful actuators
for (int i=nu-na; i<nu; i++) {
if (m->actuator_plugin[i] >= 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
int j = i-(nu-na);
@@ -481,6 +510,21 @@ static void mj_advance(const mjModel* m, mjData* d,
// advance time
d->time += m->opt.timestep;
// advance plugin states
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i = 0; i < m->nplugin; ++i) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if (plugin->advance) {
plugin->advance(m, d, i);
}
}
}
}
// Euler integrator, semi-implicit in velocity, possibly skipping factorisation
src/engine/engine_io.c
+101 -9
View File
@@ -22,8 +22,10 @@
#include <string.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_vfs.h"
@@ -396,9 +398,10 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap,
int nuser_body, int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam,
int nuser_tendon, int nuser_actuator, int nuser_sensor, int nnames) {
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames) {
intptr_t offset = 0;
// allocate mjModel
@@ -449,6 +452,8 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
m->ntupledata = ntupledata;
m->nkey = nkey;
m->nmocap = nmocap;
m->nplugin = nplugin;
m->npluginattr = npluginattr;
m->nuser_body = nuser_body;
m->nuser_jnt = nuser_jnt;
m->nuser_geom = nuser_geom;
@@ -533,10 +538,10 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor,
src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata,
src->ntuple, src->ntupledata, src->nkey, src->nmocap,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator, src->nuser_sensor,
src->nnames);
src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin,
src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom,
src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames);
}
if (!dest) {
mju_error("Failed to make mjModel. Invalid sizes.");
@@ -705,7 +710,8 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
info[21], info[22], info[23], info[24], info[25], info[26], info[27],
info[28], info[29], info[30], info[31], info[32], info[33], info[34],
info[35], info[36], info[37], info[38], info[39], info[40], info[41],
info[42], info[43], info[44], info[45], info[46], info[47], info[48]);
info[42], info[43], info[44], info[45], info[46], info[47], info[48],
info[49], info[50]);
if (!m || m->nbuffer!=info[getnint()-1]) {
if (fp) {
fclose(fp);
@@ -884,6 +890,17 @@ static mjData* _makeData(const mjModel* m) {
// set pointers into buffer, reset data
mj_setPtrData(m, d);
// copy plugins into d, required for deletion
d->nplugin = m->nplugin;
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->init) {
mju_error_i("`init` is a null function pointer for plugin at slot %d", m->plugin[i]);
}
plugin->init(m, d, i);
}
return d;
}
@@ -927,6 +944,17 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
dest->stack = save_stack;
mj_setPtrData(m, dest);
// save plugin_data, since the X macro copying block below will override it
const size_t plugin_data_size = sizeof(*dest->plugin_data) * dest->nplugin;
uintptr_t* save_plugin_data = NULL;
if (plugin_data_size) {
save_plugin_data = (uintptr_t*)mju_malloc(plugin_data_size);
if (!save_plugin_data) {
mju_error("failed to allocate temporary memory for plugin_data");
}
memcpy(save_plugin_data, dest->plugin_data, plugin_data_size);
}
// copy buffer
{
MJDATA_POINTERS_PREAMBLE(m)
@@ -936,6 +964,22 @@ mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src) {
#undef X
}
// restore plugin_data
if (plugin_data_size) {
memcpy(dest->plugin_data, save_plugin_data, plugin_data_size);
free(save_plugin_data);
save_plugin_data = NULL;
}
// copy plugin instances
dest->nplugin = m->nplugin;
for (int i = 0; i < m->nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (plugin->copy) {
plugin->copy(dest, m, src, i);
}
}
return dest;
}
@@ -966,6 +1010,12 @@ mjtNum* mj_stackAlloc(mjData* d, int size) {
// clear data, set defaults
static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
//------------------------------ save plugin state and data
mjtNum* plugin_state = mju_malloc(sizeof(mjtNum) * m->npluginstate);
memcpy(plugin_state, d->plugin_state, sizeof(mjtNum) * m->npluginstate);
uintptr_t* plugindata = mju_malloc(sizeof(uintptr_t) * m->nplugin);
memcpy(plugindata, d->plugin_data, sizeof(uintptr_t) * m->nplugin);
//------------------------------ clear header
// clear stack pointer
@@ -1048,6 +1098,22 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
d->mocap_quat[4*i] = 1.0;
}
}
// restore pluginstate and plugindata
memcpy(d->plugin_state, plugin_state, sizeof(mjtNum) * m->npluginstate);
mju_free(plugin_state);
memcpy(d->plugin_data, plugindata, sizeof(uintptr_t) * m->nplugin);
mju_free(plugindata);
// restore the plugin array back into d and reset the instances
for (int i = 0; i < m->nplugin; ++i) {
d->plugin[i] = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->reset) {
mju_error_i("`reset` is a null function pointer for plugin at slot %d", m->plugin[i]);
}
plugin->reset(m, d, i);
}
}
@@ -1087,6 +1153,13 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
// de-allocate mjData
void mj_deleteData(mjData* d) {
if (d) {
// destroy plugin instances
for (int i = 0; i < d->nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(d->plugin[i]);
if (plugin->destroy) {
plugin->destroy(d, i);
}
}
mju_free(d->buffer);
mju_free(d->stack);
mju_free(d);
@@ -1145,6 +1218,9 @@ static int sensorSize(mjtSensor sensor_type, int nuser_sensor) {
case mjSENS_USER:
return nuser_sensor;
case mjSENS_PLUGIN:
return -1;
// don't use a 'default' case, so compiler warns about missing values
}
return -1;
@@ -1202,6 +1278,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
return m->ntuple;
case mjOBJ_KEY:
return m->nkey;
case mjOBJ_PLUGIN:
return m->nplugin;
}
return -2;
}
@@ -1251,6 +1329,10 @@ const char* mj_validateReferences(const mjModel* m) {
X(skin_bonevertid, nskinbonevert, nskinvert , 0 ) \
X(pair_geom1, npair, ngeom , 0 ) \
X(pair_geom2, npair, ngeom , 0 ) \
X(actuator_plugin, nu, nplugin , 0 ) \
X(sensor_plugin, nsensor, nplugin , 0 ) \
X(plugin_stateadr, nplugin, npluginstate , 0 ) \
X(plugin_attradr, nplugin, npluginattr , 0 ) \
X(tendon_adr, ntendon, nwrap , m->tendon_num ) \
X(tendon_matid, ntendon, nmat , 0 ) \
X(numeric_adr, nnumeric, nnumericdata , m->numeric_size ) \
@@ -1459,7 +1541,17 @@ const char* mj_validateReferences(const mjModel* m) {
}
for (int i=0; i<m->nsensor; i++) {
mjtSensor sensor_type = m->sensor_type[i];
int sensor_size = sensorSize(sensor_type, m->nuser_sensor);
int sensor_size;
if (sensor_type == mjSENS_PLUGIN) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[m->sensor_plugin[i]]);
if (!plugin->nsensordata) {
mju_error_i("`nsensordata` is a null function pointer for plugin at slot %d",
m->plugin[m->sensor_plugin[i]]);
}
sensor_size = plugin->nsensordata(m, m->sensor_plugin[i], i);
} else {
sensor_size = sensorSize(sensor_type, m->nuser_sensor);
}
if (sensor_size < 0) {
return "Invalid model: Bad sensor_type.";
}
src/engine/engine_io.h
+4 -3
View File
@@ -52,9 +52,10 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap,
int nuser_body, int nuser_jnt, int nuser_geom, int nuser_site, int nuser_cam,
int nuser_tendon, int nuser_actuator, int nuser_sensor, int nnames);
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames);
// copy mjModel; allocate new if dest is NULL
MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src);
src/engine/engine_plugin.cc
+431
View File
@@ -0,0 +1,431 @@
// 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.
// Plugin registration is implemented in C++, unlike the rest of the engine code which is in C.
// This is because C++ provides a standard cross-platform mutex, which we use to guard the global
// plugin table in order to make the API thread-safe. We only expose a C API externally, and this
// entire file can in principle be re-implemented in C if necessary, without breaking any external
// or internal MuJoCo code elsewhere.
#include "engine/engine_plugin.h"
#include <atomic>
#include <cstddef>
#include <cstdlib>
#include <cstring>
#include <memory>
#include <mutex>
#include <new>
#include <shared_mutex>
#include <type_traits>
#include <utility>
#include <vector>
#ifdef __APPLE__
#include <Availability.h>
#if !defined(MAC_OS_X_VERSION_MIN_REQUIRED) && defined(__MAC_OS_X_VERSION_MIN_REQUIRED)
#define MAC_OS_X_VERSION_MIN_REQUIRED __MAC_OS_X_VERSION_MIN_REQUIRED
#endif
#endif
#include <mujoco/mjplugin.h>
#include "engine/engine_util_errmem.h"
// set default plugin definition
void mjp_defaultPlugin(mjpPlugin* plugin) {
std::memset(plugin, 0, sizeof(*plugin));
}
namespace {
constexpr int kMaxNameLength = 1024;
constexpr int kMaxAttributes = 255;
constexpr int kCacheLine = 64;
// A table of registered plugins, implemented as a linked list of array "blocks".
// This is a compromise that maintains a good degree of memory locality while not invalidating
// existing pointers when growing the table. It is expected that for most users, the number of
// plugins loaded into a program will be small enough to fit in the initial block, and so the global
// table will behave like an array. Since pointers are never invalidated, we do not need to apply a
// read lock on the global table when resolving a plugin.
struct alignas(kCacheLine) PluginTable {
static constexpr int kBlockSize = 15;
PluginTable() {
for (int i = 0; i < kBlockSize; ++i) {
mjp_defaultPlugin(&plugins[i]);
}
}
mjpPlugin plugins[kBlockSize];
PluginTable* next = nullptr;
};
static_assert(
sizeof(PluginTable) / kCacheLine ==
sizeof(PluginTable::plugins) / kCacheLine + (sizeof(PluginTable::plugins) % kCacheLine > 0),
"PluginTable::next doesn't fit in the same cache line as the end of PluginTable::plugins");
using Mutex = std::shared_mutex;
class Global {
public:
Global() {
new(mutex_) Mutex;
}
PluginTable& table() {
return table_;
}
std::atomic_int& count() {
return count_;
}
Mutex& mutex() {
return *std::launder(reinterpret_cast<Mutex*>(&mutex_));
}
private:
PluginTable table_;
std::atomic_int count_;
// A mutex whose destructor is never run.
// When a C++ program terminates, the destructors for function static objects and globals will be
// executed by whichever thread started that termination but there is no guarantee that other
// threads have terminated. In other words, a static object may be accessed by another thread
// after it is deleted. We avoid destruction issues by never running the destructor.
alignas(Mutex) unsigned char mutex_[sizeof(Mutex)];
};
Global& GetGlobal() {
static Global global;
static_assert(std::is_trivially_destructible_v<decltype(global)>);
return global;
}
// return the length of a null-terminated string, or -1 if it is not terminated after kMaxNameLength
int strnlen(const char* s) {
for (int i = 0; i < kMaxNameLength; ++i) {
if (!s[i]) {
return i;
}
}
return -1;
}
// copy a null-terminated string into a new heap-allocated char array managed by a unique_ptr
std::unique_ptr<char[]> CopyName(const char* s) {
int len = strnlen(s);
if (len == -1) {
return nullptr;
}
std::unique_ptr<char[]> out(new(std::nothrow) char[len + 1]);
if (!out) {
return nullptr;
}
std::strncpy(out.get(), s, len);
out.get()[len] = '\0';
return out;
}
// check if two plugins are identical
bool PluginsAreIdentical(const mjpPlugin& plugin1, const mjpPlugin& plugin2) {
if (plugin1.name && !plugin2.name) {
return false;
}
if (plugin2.name && !plugin1.name) {
return false;
}
if (plugin1.name && plugin2.name &&
std::strncmp(plugin1.name, plugin2.name, kMaxNameLength)) {
return false;
}
if (plugin1.nattribute != plugin2.nattribute) {
return false;
}
for (int i = 0; i < plugin1.nattribute; ++i) {
if (plugin1.attributes[i] && !plugin2.attributes[i]) {
return false;
}
if (plugin2.attributes[i] && !plugin1.attributes[i]) {
return false;
}
if (plugin1.attributes[i] && plugin2.attributes[i] &&
std::strncmp(plugin1.attributes[i], plugin2.attributes[i],
kMaxNameLength)) {
return false;
}
}
const char* ptr1 = reinterpret_cast<const char*>(&plugin1.attributes) +
sizeof(plugin1.attributes);
const char* ptr2 = reinterpret_cast<const char*>(&plugin2.attributes) +
sizeof(plugin2.attributes);
std::size_t remaining_size =
sizeof(mjpPlugin) - (ptr1 - reinterpret_cast<const char*>(&plugin1));
return !std::memcmp(ptr1, ptr2, remaining_size);
}
} // namespace
// globally register a plugin (thread-safe), return new slot id
int mjp_registerPlugin(const mjpPlugin* plugin) {
if (!plugin->name) {
mju_error("plugin->name is a null pointer");
} else if (plugin->name[0] == '\0') {
mju_error("plugin->name is an empty string");
} else if (plugin->nattribute < 0) {
mju_error("plugin->nattribute is negative");
} else if (plugin->nattribute > kMaxAttributes) {
mju_error_i("plugin->nattribute exceeds the maximum limit of ",
kMaxAttributes);
}
char err[512];
err[0] = '\0';
// ========= ATTENTION! ==========================================================================
// Do not handle objects with nontrivial destructors outside of this lambda.
// Do not call mju_error inside this lambda.
int slot = [&]() -> int {
// check and copy the plugin name
std::unique_ptr<char[]> name = CopyName(plugin->name);
if (!name) {
if (strnlen(plugin->name) == -1) {
std::snprintf(err, sizeof(err),
"plugin->name length exceeds the maximum limit of %d", kMaxNameLength);
} else {
std::snprintf(err, sizeof(err), "failed to allocate memory for plugin name");
}
return -1;
}
// check and copy plugin attributes
std::vector<std::unique_ptr<char[]>> attributes_vec;
if (plugin->nattribute) {
attributes_vec.reserve(plugin->nattribute);
for (int i = 0; i < plugin->nattribute; ++i) {
std::unique_ptr<char[]> attr = CopyName(plugin->attributes[i]);
if (!attr) {
if (strnlen(plugin->attributes[i]) == -1) {
std::snprintf(
err, sizeof(err),
"plugin->attributes[%d] exceeds the maximum limit of %d", i, kMaxAttributes);
} else {
std::snprintf(err, sizeof(err), "failed to allocate memory for plugin attribute");
}
return -1;
}
attributes_vec.emplace_back(std::move(attr));
}
}
// exclusively lock the global plugin table
Global& global = GetGlobal();
std::unique_lock lock(global.mutex());
int count = global.count().load(std::memory_order_acquire);
int local_idx = 0;
PluginTable* table = &global.table();
// check if a non-identical plugin with the same name has already been registered
for (int i = 0; i < count; ++i, ++local_idx) {
if (local_idx == PluginTable::kBlockSize) {
local_idx = 0;
table = table->next;
}
mjpPlugin& existing = table->plugins[local_idx];
if (std::strcmp(plugin->name, existing.name) == 0) {
if (PluginsAreIdentical(*plugin, existing)) {
return i;
} else {
std::snprintf(err, sizeof(err), "plugin '%s' is already registered", plugin->name);
return -1;
}
}
}
// allocate a new block of PluginTable if the last allocated block is full
if (local_idx == PluginTable::kBlockSize) {
local_idx = 0;
#if defined(MAC_OS_X_VERSION_MIN_REQUIRED) && MAC_OS_X_VERSION_MIN_REQUIRED < MAC_OS_X_VERSION_10_14
// aligned nothrow new is not available until macOS 10.14
posix_memalign(reinterpret_cast<void**>(&table->next),
alignof(PluginTable), sizeof(PluginTable));
if (table->next) new(table->next) PluginTable;
#else
table->next = new(std::nothrow) PluginTable;
#endif
if (!table->next) {
std::snprintf(err, sizeof(err), "failed to allocate memory for the global plugin table");
return -1;
}
table = table->next;
}
// release the attribute names from unique_ptr into a plain array
const char** attributes = nullptr;
if (plugin->nattribute) {
attributes = new(std::nothrow) const char*[plugin->nattribute];
if (!attributes) {
std::snprintf(err, sizeof(err), "failed to allocate memory for plugin attribute array");
return -1;
}
for (int i = 0; i < plugin->nattribute; ++i) {
attributes[i] = attributes_vec[i].release();
}
}
// all checked passed, actually register the plugin into the global table
mjpPlugin& registered_plugin = table->plugins[local_idx];
registered_plugin = *plugin;
registered_plugin.name = name.release();
registered_plugin.attributes = attributes;
// increment the global plugin count with a release memory barrier
global.count().store(count + 1, std::memory_order_release);
return count;
}();
// ========= ATTENTION! ==========================================================================
// End of safe lambda, do not handle objects with non-trivial destructors beyond this point.
// plugin registration failed, throw an mju_error
if (slot < 0) {
err[sizeof(err) - 1] = '\0';
mju_error(err);
}
return slot;
}
// look up plugin by slot number, assuming that mjp_pluginCount has already been called
const mjpPlugin* mjp_getPluginAtSlotUnsafe(int slot, int nslot) {
if (slot < 0 || slot >= nslot) {
return nullptr;
}
Global& global = GetGlobal();
PluginTable* table = &global.table();
// iterate over blocks in the global table until the local index is less the block size
int local_idx = slot;
while (local_idx >= PluginTable::kBlockSize) {
local_idx -= PluginTable::kBlockSize;
table = table->next;
if (!table) {
return nullptr;
}
}
// local_idx is now a valid index into the current block
const mjpPlugin& plugin = table->plugins[local_idx];
if (!plugin.name) {
return nullptr;
}
return &plugin;
}
// look up plugin by name, assuming that mjp_pluginCount has already been called
const mjpPlugin* mjp_getPluginUnsafe(const char* name, int* slot, int nslot) {
if (slot) *slot = -1;
if (!name || !name[0]) {
return nullptr;
}
Global& plugin = GetGlobal();
PluginTable* table = &plugin.table();
int found_slot = 0;
while (table) {
for (int i = 0;
i < PluginTable::kBlockSize && found_slot < nslot;
++i, ++found_slot) {
const mjpPlugin& plugin = table->plugins[i];
// reached an uninitialized plugin, which means that iterated beyond the plugin count
// this should never happen if `count` was actually returned by mjp_pluginCount
if (!plugin.name) {
return nullptr;
}
if (std::strcmp(plugin.name, name) == 0) {
if (slot) *slot = found_slot;
return &plugin;
}
}
table = table->next;
}
return nullptr;
}
// return the number of globally registered plugins
int mjp_pluginCount() {
return GetGlobal().count().load(std::memory_order_acquire);
}
// look up a plugin by slot number
const mjpPlugin* mjp_getPluginAtSlot(int slot) {
const int count = mjp_pluginCount();
// mjp_pluginCount uses memory_order_acquire which acts as a barrier that guarantees that all
// plugins up to `count` have been completely inserted
return mjp_getPluginAtSlotUnsafe(slot, count);
}
// look up a plugin by name, optionally also get its registered slot number
const mjpPlugin* mjp_getPlugin(const char* name, int* slot) {
const int count = mjp_pluginCount();
int found_slot = -1;
const mjpPlugin* plugin = mjp_getPluginUnsafe(name, &found_slot, count);
if (slot) *slot = found_slot;
return plugin;
}
namespace {
// seek the nth config attrib of a plugin instance by counting null terminators
const char* PluginAttrSeek(const mjModel* m, int plugin_id, int attrib_id) {
const char* ptr = m->plugin_attr + m->plugin_attradr[plugin_id];
for (int i = 0; i < attrib_id; ++i) {
while (*ptr) {
++ptr;
}
++ptr;
}
return ptr;
}
} // namespace
// return a config attribute of a plugin instance
// NULL: invalid plugin instance ID or attribute name
const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attrib) {
if (plugin_id < 0 || plugin_id >= m->nplugin || attrib == nullptr) {
return nullptr;
}
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[plugin_id]);
if (!plugin) {
return nullptr;
}
for (int i = 0; i < plugin->nattribute; ++i) {
if (std::strcmp(plugin->attributes[i], attrib) == 0) {
return PluginAttrSeek(m, plugin_id, i);
}
}
return nullptr;
}
src/engine/engine_plugin.h
+62
View File
@@ -0,0 +1,62 @@
// 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_PLUGIN_H_
#define MUJOCO_SRC_ENGINE_ENGINE_PLUGIN_H_
#include <mujoco/mjexport.h>
#include <mujoco/mjplugin.h>
#ifdef __cplusplus
extern "C" {
#endif
// set default plugin definition
MJAPI void mjp_defaultPlugin(mjpPlugin* plugin);
// globally register a plugin (thread-safe), return new slot id
MJAPI int mjp_registerPlugin(const mjpPlugin* plugin);
// return the number of globally registered plugins
MJAPI int mjp_pluginCount();
// look up a plugin by name, optionally also get its registered slot number
MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot);
// look up a plugin by slot number
MJAPI const mjpPlugin* mjp_getPluginAtSlot(int slot);
// return a config attribute of a plugin instance
// NULL: invalid plugin instance ID or attribute name
MJAPI const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attrib);
// =================================================================================================
// MuJoCo-internal functions beyond this point.
// "Unsafe" suffix indicates that improper use of these functions may result in data races.
//
// The unsafe functions assume that called mjp_pluginCount has already been called, and that it is
// safe to assume that all plugins up to `count` have been completely written into the global table.
// =================================================================================================
// look up a plugin by name, assuming that mjp_pluginCount has already been called
const mjpPlugin* mjp_getPluginUnsafe(const char* name, int* slot, int nslot);
// look up a plugin by slot number, assuming that mjp_pluginCount has already been called
const mjpPlugin* mjp_getPluginAtSlotUnsafe(int slot, int nslot);
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_PLUGIN_H_
src/engine/engine_sensor.c
+88
View File
@@ -18,10 +18,12 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_callback.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_ray.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -200,6 +202,11 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// process sensors matching stage
for (int i=0; i<m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_POS) {
// get sensor info
objtype = m->sensor_objtype[i];
@@ -342,6 +349,25 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
add_noise(m, d, mjSTAGE_POS);
}
// compute plugin sensor values
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if ((plugin->type & mjPLUGIN_SENSOR) &&
(plugin->needstage==mjSTAGE_POS || plugin->needstage==mjSTAGE_NONE)) {
if (!plugin->compute) {
mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
}
plugin->compute(m, d, i, mjPLUGIN_SENSOR);
}
}
}
// cutoff
apply_cutoff(m, d, mjSTAGE_POS);
}
@@ -362,6 +388,11 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// process sensors matching stage
int subtreeVel = 0;
for (int i=0; i<m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_VEL) {
// get sensor info
type = m->sensor_type[i];
@@ -499,6 +530,32 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
add_noise(m, d, mjSTAGE_VEL);
}
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if ((plugin->type & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_VEL) {
if (!plugin->compute) {
mju_error_i("`compute` is null for plugin at slot %d", slot);
}
if (subtreeVel == 0) {
// compute subtree_linvel, subtree_angmom
// TODO(b/247107630): add a flag to allow plugin to specify whether it actually needs this
mj_subtreeVel(m, d);
// mark computed
subtreeVel = 1;
}
plugin->compute(m, d, i, mjPLUGIN_SENSOR);
}
}
}
// cutoff
apply_cutoff(m, d, mjSTAGE_VEL);
}
@@ -520,6 +577,11 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// process sensors matching stage
int rnePost = 0;
for (int i=0; i<m->nsensor; i++) {
// skip sensor plugins -- these are handled after builtin sensor types
if (m->sensor_type[i] == mjSENS_PLUGIN) {
continue;
}
if (m->sensor_needstage[i]==mjSTAGE_ACC) {
// get sensor info
type = m->sensor_type[i];
@@ -677,6 +739,32 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
add_noise(m, d, mjSTAGE_ACC);
}
// trigger computation of plugins
if (m->nplugin) {
const int nslot = mjp_pluginCount();
for (int i=0; i<m->nplugin; i++) {
const int slot = m->plugin[i];
const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!plugin) {
mju_error_i("invalid plugin slot: %d", slot);
}
if ((plugin->type & mjPLUGIN_SENSOR) && plugin->needstage==mjSTAGE_ACC) {
if (!plugin->compute) {
mju_error_i("`compute` is null for plugin at slot %d", slot);
}
if (rnePost == 0) {
// compute cacc, cfrc_int, cfrc_ext
// TODO(b/247107630): add a flag to allow plugin to specify whether it actually needs this
mj_rnePostConstraint(m, d);
// mark computed
rnePost = 1;
}
plugin->compute(m, d, i, mjPLUGIN_SENSOR);
}
}
}
// cutoff
apply_cutoff(m, d, mjSTAGE_ACC);
}
src/engine/engine_support.c
+4 -21
View File
@@ -440,107 +440,90 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr) {
case mjOBJ_XBODY:
*padr = m->name_bodyadr;
return m->nbody;
break;
case mjOBJ_JOINT:
*padr = m->name_jntadr;
return m->njnt;
break;
case mjOBJ_GEOM:
*padr = m->name_geomadr;
return m->ngeom;
break;
case mjOBJ_SITE:
*padr = m->name_siteadr;
return m->nsite;
break;
case mjOBJ_CAMERA:
*padr = m->name_camadr;
return m->ncam;
break;
case mjOBJ_LIGHT:
*padr = m->name_lightadr;
return m->nlight;
break;
case mjOBJ_MESH:
*padr = m->name_meshadr;
return m->nmesh;
break;
case mjOBJ_SKIN:
*padr = m->name_skinadr;
return m->nskin;
break;
case mjOBJ_HFIELD:
*padr = m->name_hfieldadr;
return m->nhfield;
break;
case mjOBJ_TEXTURE:
*padr = m->name_texadr;
return m->ntex;
break;
case mjOBJ_MATERIAL:
*padr = m->name_matadr;
return m->nmat;
break;
case mjOBJ_PAIR:
*padr = m->name_pairadr;
return m->npair;
break;
case mjOBJ_EXCLUDE:
*padr = m->name_excludeadr;
return m->nexclude;
break;
case mjOBJ_EQUALITY:
*padr = m->name_eqadr;
return m->neq;
break;
case mjOBJ_TENDON:
*padr = m->name_tendonadr;
return m->ntendon;
break;
case mjOBJ_ACTUATOR:
*padr = m->name_actuatoradr;
return m->nu;
break;
case mjOBJ_SENSOR:
*padr = m->name_sensoradr;
return m->nsensor;
break;
case mjOBJ_NUMERIC:
*padr = m->name_numericadr;
return m->nnumeric;
break;
case mjOBJ_TEXT:
*padr = m->name_textadr;
return m->ntext;
break;
case mjOBJ_TUPLE:
*padr = m->name_tupleadr;
return m->ntuple;
break;
case mjOBJ_KEY:
*padr = m->name_keyadr;
return m->nkey;
break;
case mjOBJ_PLUGIN:
*padr = m->name_pluginadr;
return m->nplugin;
default:
*padr = 0;
src/engine/engine_util_misc.c
+7
View File
@@ -831,6 +831,9 @@ const char* mju_type2Str(int type) {
case mjOBJ_KEY:
return "key";
case mjOBJ_PLUGIN:
return "plugin";
default:
return 0;
}
@@ -932,6 +935,10 @@ int mju_str2Type(const char* str) {
return mjOBJ_KEY;
}
else if (!strcmp(str, "plugin")) {
return mjOBJ_PLUGIN;
}
else {
return mjOBJ_UNKNOWN;
}
src/user/user_model.cc
+131 -2
View File
@@ -24,11 +24,13 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjvisualize.h>
#include "cc/array_safety.h"
#include "engine/engine_forward.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_setconst.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -123,6 +125,7 @@ mjCModel::mjCModel() {
nuserdata = 0;
nkey = 0;
nmocap = 0;
nplugin = 0;
nuser_body = -1;
nuser_jnt = -1;
nuser_geom = -1;
@@ -198,6 +201,7 @@ mjCModel::~mjCModel() {
for (i=0; i<texts.size(); i++) delete texts[i];
for (i=0; i<tuples.size(); i++) delete tuples[i];
for (i=0; i<keys.size(); i++) delete keys[i];
for (i=0; i<plugins.size(); i++) delete plugins[i];
for (i=0; i<defaults.size(); i++) delete defaults[i];
// clear pointer lists created in model construction
@@ -216,6 +220,7 @@ mjCModel::~mjCModel() {
texts.clear();
tuples.clear();
keys.clear();
plugins.clear();
defaults.clear();
// clear sizes and pointer lists created in Compile
@@ -267,6 +272,7 @@ void mjCModel::Clear(void) {
nnumericdata = 0;
ntextdata = 0;
ntupledata = 0;
npluginattr = 0;
nnames = 0;
nemax = 0;
nM = 0;
@@ -283,6 +289,7 @@ void mjCModel::Clear(void) {
lights.clear();
// internal variables
hasImplicitPluginElem = false;
compiled = false;
errInfo = mjCError();
fixCount = 0;
@@ -405,6 +412,12 @@ mjCKey* mjCModel::AddKey(void) {
}
// add plugin instance
mjCPlugin* mjCModel::AddPlugin(void) {
return AddObject(plugins, "plugin");
}
//------------------------ API FOR ACCESS TO MODEL ELEMENTS ---------------------------------------
@@ -455,6 +468,8 @@ int mjCModel::NumObjects(mjtObj type) {
return (int)tuples.size();
case mjOBJ_KEY:
return (int)keys.size();
case mjOBJ_PLUGIN:
return (int)plugins.size();
default:
return 0;
}
@@ -509,6 +524,8 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) {
return tuples[id];
case mjOBJ_KEY:
return keys[id];
case mjOBJ_PLUGIN:
return plugins[id];
default:
return 0;
}
@@ -646,6 +663,8 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) {
return findobject(name, texts);
case mjOBJ_TUPLE:
return findobject(name, tuples);
case mjOBJ_PLUGIN:
return findobject(name, plugins);
default:
return 0;
}
@@ -893,6 +912,7 @@ void mjCModel::SetSizes(void) {
ntext = (int)texts.size();
ntuple = (int)tuples.size();
nkey = (int)keys.size();
nplugin = (int)plugins.size();
// nq, nv
for (i=0; i<njnt; i++) {
@@ -955,6 +975,9 @@ void mjCModel::SetSizes(void) {
// ntupledata
for (i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
// npluginattr
for (i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
// nnames
nnames = (int)modelname.size() + 1;
for (i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
@@ -978,6 +1001,7 @@ void mjCModel::SetSizes(void) {
for (i=0; i<ntext; i++) nnames += (int)texts[i]->name.length() + 1;
for (i=0; i<ntuple; i++) nnames += (int)tuples[i]->name.length() + 1;
for (i=0; i<nkey; i++) nnames += (int)keys[i]->name.length() + 1;
for (i=0; i<nplugin; i++) nnames += (int)plugins[i]->name.length() + 1;
// nemax
for (i=0; i<neq; i++)
@@ -1234,6 +1258,7 @@ void mjCModel::CopyNames(mjModel* m) {
adr = namelist(texts, adr, m->name_textadr, m->names);
adr = namelist(tuples, adr, m->name_tupleadr, m->names);
adr = namelist(keys, adr, m->name_keyadr, m->names);
adr = namelist(plugins, adr, m->name_pluginadr, m->names);
// check size, SHOULD NOT OCCUR
if (adr != nnames) {
@@ -1776,7 +1801,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_adr[i] = adr;
m->tendon_num[i] = (int)pte->path.size();
m->tendon_matid[i] = pte->matid;
m->tendon_group[i] = pte->group;;
m->tendon_group[i] = pte->group;
m->tendon_limited[i] = pte->limited;
m->tendon_width[i] = (mjtNum)pte->width;
copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
@@ -2390,6 +2415,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
processlist(texts, "text");
processlist(tuples, "tuple");
processlist(keys, "key");
processlist(plugins, "plugin");
// set default names, convert names into indices
SetDefaultNames();
@@ -2477,6 +2503,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
for (int i=0; i<numerics.size(); i++) numerics[i]->Compile();
for (int i=0; i<texts.size(); i++) texts[i]->Compile();
for (int i=0; i<tuples.size(); i++) tuples[i]->Compile();
for (int i=0; i<plugins.size(); i++) plugins[i]->Compile();
// compile defaults: to enforce userdata length for writer
for (int i=0; i<defaults.size(); i++) {
@@ -2545,7 +2572,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
neq, ntendon, nwrap, nsensor,
nnumeric, nnumericdata, ntext, ntextdata,
ntuple, ntupledata, nkey, nmocap,
ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr,
nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam,
nuser_tendon, nuser_actuator, nuser_sensor, nnames);
if (!m) {
@@ -2558,6 +2585,69 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
CopyNames(m);
CopyTree(m);
// assign plugin slots and copy plugin config attributes
{
int adr = 0;
for (int i = 0; i < nplugin; ++i) {
m->plugin[i] = plugins[i]->plugin_slot;
const int size = plugins[i]->flattened_attributes.size();
std::memcpy(m->plugin_attr + adr,
plugins[i]->flattened_attributes.data(), size);
m->plugin_attradr[i] = adr;
adr += size;
}
}
// query and set plugin-related information
{
// set actuator_plugin to the plugin instance ID
for (int i = 0; i < nu; ++i) {
if (actuators[i]->is_plugin) {
m->actuator_plugin[i] = actuators[i]->plugin_instance->id;
} else {
m->actuator_plugin[i] = -1;
}
}
// set sensor_plugin to the plugin instance ID
std::vector<std::vector<int>> plugin_to_sensors(nplugin);
for (int i = 0; i < nsensor; ++i) {
if (sensors[i]->type == mjSENS_PLUGIN) {
int sensor_plugin = sensors[i]->plugin_instance->id;
m->sensor_plugin[i] = sensor_plugin;
plugin_to_sensors[sensor_plugin].push_back(i);
} else {
m->sensor_plugin[i] = -1;
}
}
// query plugin->nstate, compute and set plugin_state and plugin_stateadr
// for sensor plugins, also query plugin->nsensordata and set nsensordata
int stateadr = 0;
for (int i = 0; i < nplugin; ++i) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
if (!plugin->nstate) {
mju_error_i("`nstate` is null for plugin at slot %d", m->plugin[i]);
}
int nstate = plugin->nstate(m, i);
m->plugin_stateadr[i] = stateadr;
stateadr += nstate;
if (plugin->type & mjPLUGIN_SENSOR) {
for (int sensor_id : plugin_to_sensors[i]) {
if (!plugin->nsensordata) {
mju_error_i("`reset` is null for plugin at slot %d", m->plugin[i]);
}
int nsensordata = plugin->nsensordata(m, i, sensor_id);
sensors[sensor_id]->dim = nsensordata;
sensors[sensor_id]->needstage =
static_cast<mjtStage>(plugin->needstage);
this->nsensordata += nsensordata;
}
}
}
m->npluginstate = stateadr;
}
// keyframe compilation needs access to nq, nv, na, nmocap, qpos0
for (int i=0; i<keys.size(); i++) {
keys[i]->Compile(m);
@@ -2899,3 +2989,42 @@ bool mjCModel::CopyBack(const mjModel* m) {
return true;
}
void mjCModel::ResolvePlugin(mjCBase* obj, const std::string& plugin_name,
const std::string& plugin_instance_name, mjCPlugin** plugin_instance) {
// if plugin_name is specified, check if it is in the list of active plugins
// (in XML, active plugins are those declared as <required>)
int plugin_slot = -1;
if (!plugin_name.empty()) {
for (int i = 0; i < active_plugins.size(); ++i) {
if (active_plugins[i].first->name == plugin_name) {
plugin_slot = active_plugins[i].second;
break;
}
}
if (plugin_slot == -1) {
throw mjCError(obj, "unrecognized plugin '%s'", plugin_name.c_str());
}
}
// implicit plugin instance
if (*plugin_instance && (*plugin_instance)->plugin_slot == -1) {
(*plugin_instance)->plugin_slot = plugin_slot;
(*plugin_instance)->parent = obj;
}
// explicit plugin instance, look up existing mjCPlugin by instance name
else if (!*plugin_instance) {
*plugin_instance =
static_cast<mjCPlugin*>(FindObject(mjOBJ_PLUGIN, plugin_instance_name));
if (!*plugin_instance) {
throw mjCError(
obj, "unrecognized name '%s' for plugin instance", plugin_instance_name.c_str());
}
if (plugin_slot != -1 && plugin_slot != (*plugin_instance)->plugin_slot) {
throw mjCError(
obj, "'plugin' attribute does not match that of the instance");
}
plugin_slot = (*plugin_instance)->plugin_slot;
}
}
src/user/user_model.h
+15
View File
@@ -16,10 +16,12 @@
#define MUJOCO_SRC_USER_USER_MODEL_H_
#include <string>
#include <utility>
#include <vector>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "user/user_objects.h"
typedef enum _mjtInertiaFromGeom {
@@ -85,6 +87,7 @@ class mjCModel {
mjCText* AddText(void); // custom text
mjCTuple* AddTuple(void); // custom tuple
mjCKey* AddKey(void); // keyframe
mjCPlugin* AddPlugin(void); // plugin instance
//------------------------ API for access to model elements (outside tree)
int NumObjects(mjtObj type); // number of objects in specified list
@@ -100,6 +103,12 @@ class mjCModel {
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
//------------------------ API for plugins
void ResolvePlugin(mjCBase* obj, // resolve plugin instance, create a new one if needed
const std::string& plugin_name,
const std::string& plugin_instance_name,
mjCPlugin** plugin_instance);
//------------------------ global data
std::string comment; // comment at top of XML
@@ -198,6 +207,7 @@ class mjCModel {
int ntuple; // number of tuple fields
int nkey; // number of keyframes
int nmocap; // number of mocap bodies
int nplugin; // number of plugin instances
// sizes computed by Compile
int nq; // number of generalized coordinates = dim(qpos)
@@ -220,6 +230,7 @@ class mjCModel {
int nnumericdata; // number of mjtNums in all custom fields
int ntextdata; // number of chars in all text fields, including 0
int ntupledata; // number of objects in all tuple fields
int npluginattr; // number of chars in all plugin config attributes
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
@@ -242,6 +253,9 @@ class mjCModel {
std::vector<mjCTuple*> tuples; // list of tuple fields
std::vector<mjCKey*> keys; // list of keyframe fields
std::vector<std::pair<const mjpPlugin*, int>> active_plugins; // list of active plugins
std::vector<mjCPlugin*> plugins; // list of plugin instances
// pointers to objects created inside kinematic tree
std::vector<mjCBody*> bodies; // list of bodies
std::vector<mjCJoint*> joints; // list of joints allowing motion relative to parent
@@ -251,6 +265,7 @@ class mjCModel {
std::vector<mjCLight*> lights; // list of lights
//------------------------ internal variables
bool hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator
bool compiled; // already compiled flag (cannot be compiled again)
mjCError errInfo; // last error info
int fixCount; // how many bodies have been fixed
src/user/user_objects.cc
+93 -1
View File
@@ -25,12 +25,14 @@
#include "lodepng.h"
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "cc/array_safety.h"
#include "engine/engine_core_smooth.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_file.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
@@ -3391,6 +3393,11 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
}
@@ -3553,6 +3560,21 @@ void mjCActuator::Compile(void) {
} else {
trnid[0] = ptarget->id;
}
// plugin
if (is_plugin) {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for actuator '%s', (id = %d)",
name.c_str(), id);
}
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->type & mjPLUGIN_ACTUATOR)) {
throw mjCError(this, "plugin '%s' does not support actuators", plugin->name);
}
}
}
@@ -3580,6 +3602,10 @@ mjCSensor::mjCSensor(mjCModel* _model) {
// clear private variables
objid = -1;
refid = -1;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
}
@@ -3624,7 +3650,7 @@ void mjCSensor::Compile(void) {
// get sensorized object id
objid = pobj->id;
} else if (type != mjSENS_CLOCK) {
} else if (type != mjSENS_CLOCK && type != mjSENS_PLUGIN) {
throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id);
}
@@ -3919,6 +3945,28 @@ void mjCSensor::Compile(void) {
}
break;
case mjSENS_PLUGIN:
dim = 0; // to be filled in by the plugin later
datatype = mjDATATYPE_REAL; // no noise added to plugin sensors, this attribute is unused
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for sensor '%s', (id = %d)",
name.c_str(), id);
}
// resolve plugin instance, or create one if using the "plugin" attribute shortcut
{
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->type & mjPLUGIN_SENSOR)) {
throw mjCError(this, "plugin '%s' does not support sensors", plugin->name);
}
needstage = static_cast<mjtStage>(plugin->needstage);
}
break;
default:
throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id);
}
@@ -4174,3 +4222,47 @@ void mjCKey::Compile(const mjModel* m) {
}
}
//------------------ class mjCPlugin implementation ------------------------------------------------
// initialize defaults
mjCPlugin::mjCPlugin(mjCModel* _model) {
name = "";
plugin_slot = -1;
nstate = 0;
parent = this;
model = _model;
}
// compiler
void mjCPlugin::Compile(void) {
const mjpPlugin* plugin = mjp_getPluginAtSlot(this->plugin_slot);
// concatenate all of the plugin's attribute values (as null-terminated strings) into
// flattened_attributes, in the order declared in the mjpPlugin
// each valid attribute found is appended to flattened_attributes and removed from xml_attributes
for (int i = 0; i < plugin->nattribute; ++i) {
std::string_view attr(plugin->attributes[i]);
auto it = config_attribs.find(attr);
if (it == config_attribs.end()) {
flattened_attributes.push_back('\0');
} else {
auto original_size = flattened_attributes.size();
flattened_attributes.resize(original_size + it->second.size() + 1);
std::memcpy(&flattened_attributes[original_size], it->second.c_str(),
it->second.size() + 1);
config_attribs.erase(it);
}
}
// anything left in xml_attributes at this stage is not a valid attribute
if (!config_attribs.empty()) {
std::string error =
"unrecognized attribute 'plugin:" + config_attribs.begin()->first +
"' for plugin " + std::string(plugin->name) + "'";
throw mjCError(parent, error.c_str());
}
}
src/user/user_objects.h
+33
View File
@@ -15,6 +15,7 @@
#ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
#define MUJOCO_SRC_USER_USER_OBJECTS_H_
#include <map>
#include <string>
#include <vector>
@@ -833,6 +834,27 @@ class mjCWrap : public mjCBase {
//------------------------- class mjCPlugin --------------------------------------------------------
// Describes an instance of a plugin
class mjCPlugin : public mjCBase {
friend class mjCModel;
friend class mjXWriter;
public:
int plugin_slot; // global registered slot number of the plugin
int nstate; // state size for the plugin instance
mjCBase* parent; // parent object (only used when generating error message)
std::map<std::string, std::string, std::less<>> config_attribs; // raw config attributes from XML
std::vector<char> flattened_attributes; // config attributes flattened in plugin-declared order
private:
mjCPlugin(mjCModel*); // constructor
void Compile(void); // compiler
};
//------------------------- class mjCActuator ------------------------------------------------------
// Describes an actuator
@@ -865,6 +887,12 @@ class mjCActuator : public mjCBase {
std::string slidersite; // site defining cylinder, for slider-crank only
std::string refsite; // reference site, for site transmission only
// plugin support
bool is_plugin;
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
private:
mjCActuator(mjCModel* = 0, mjCDef* = 0);// constructor
void Compile(void); // compiler
@@ -896,6 +924,11 @@ class mjCSensor : public mjCBase {
double noise; // noise stdev
std::vector<double> userdata; // user data
// plugin support
std::string plugin_name;
std::string plugin_instance_name;
mjCPlugin* plugin_instance;
private:
mjCSensor(mjCModel*); // constructor
void Compile(void); // compiler
src/xml/xml_native_reader.cc
+162 -2
View File
@@ -17,32 +17,65 @@
#include <cfloat>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <mujoco/mjmodel.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_macro.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_composite.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
namespace {
using std::string;
using std::vector;
using tinyxml2::XMLElement;
void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
std::map<std::string, std::string, std::less<>> config_attribs;
XMLElement* child = elem->FirstChildElement();
while (child) {
std::string_view name = child->Value();
if (name == "config") {
std::string key, value;
mjXUtil::ReadAttrTxt(child, "key", key, /* required = */ true);
if (config_attribs.find(key) != config_attribs.end()) {
std::string err = "duplicate config key: " + key;
throw mjXError(child, err.c_str());
}
mjXUtil::ReadAttrTxt(child, "value", value, /* required = */ true);
config_attribs[key] = value;
}
child = child->NextSiblingElement();
}
if (!pp && !config_attribs.empty()) {
throw mjXError(elem,
"plugin configuration attributes cannot be used in an "
"element that references a predefined plugin instance");
} else if (pp) {
pp->config_attribs = std::move(config_attribs);
}
}
} // namespace
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 165;
static const int nMJCF = 183;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -150,6 +183,17 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"gain", "user", "group"},
{">"},
{"extension", "*", "0"},
{"<"},
{"required", "*", "1", "plugin"},
{"<"},
{"instance", "*", "1", "name"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{">"},
{"custom", "*", "0"},
{"<"},
{"numeric", "*", "3", "name", "size", "data"},
@@ -308,6 +352,13 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "9", "name", "class", "group",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{"plugin", "*", "19", "name", "class", "plugin", "instance", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
"site", "tendon", "cranksite", "slidersite", "user"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"sensor", "*", "0"},
@@ -350,6 +401,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"clock", "*", "4", "name", "cutoff", "noise", "user"},
{"user", "*", "9", "name", "objtype", "objname", "datatype", "needstage",
"dim", "cutoff", "noise", "user"},
{"plugin", "*", "9", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname",
"user"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"keyframe", "*", "0"},
@@ -711,6 +767,11 @@ void mjXReader::Parse(XMLElement* root) {
}
readingdefaults = false;
for (section = root->FirstChildElement("extension"); section;
section = section->NextSiblingElement("extension")) {
Extension(section);
}
for (section = root->FirstChildElement("custom"); section;
section = section->NextSiblingElement("custom")) {
Custom(section);
@@ -1627,6 +1688,18 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->biastype = mjBIAS_NONE;
}
else if (type == "plugin") {
pact->is_plugin = true;
ReadAttrTxt(elem, "plugin", pact->plugin_name);
ReadAttrTxt(elem, "instance", pact->plugin_instance_name);
if (pact->plugin_instance_name.empty()) {
pact->plugin_instance = model->AddPlugin();
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, pact->plugin_instance);
}
else { // SHOULD NOT OCCUR
throw mjXError(elem, "unrecognized actuator type: %s", type.c_str());
}
@@ -1908,6 +1981,61 @@ void mjXReader::Default(XMLElement* section, int parentid) {
// extension section parser
void mjXReader::Extension(XMLElement* section) {
XMLElement* elem = section->FirstChildElement();
while (elem) {
// get sub-element name
std::string_view name = elem->Value();
if (name == "required") {
std::string plugin_name;
int plugin_slot = -1;
ReadAttrTxt(elem, "plugin", plugin_name, /* required = */ true);
const mjpPlugin* plugin = mjp_getPlugin(plugin_name.c_str(), &plugin_slot);
if (!plugin) {
throw mjXError(elem, "unknown plugin '%s'", plugin_name.c_str());
}
bool already_declared = false;
for (const auto& [existing_plugin, existing_slot] : model->active_plugins) {
if (plugin == existing_plugin) {
already_declared = true;
break;
}
}
if (!already_declared) {
model->active_plugins.emplace_back(std::make_pair(plugin, plugin_slot));
}
XMLElement* child = elem->FirstChildElement();
while (child) {
if (std::string(child->Value())=="instance") {
if (model->hasImplicitPluginElem) {
throw mjXError(
child, "explicit plugin instance must appear before implicit plugin elements");
}
mjCPlugin* pp = model->AddPlugin();
GetXMLPos(child, pp);
ReadAttrTxt(child, "name", pp->name, /* required = */ true);
if (pp->name.empty()) {
throw mjXError(child, "plugin instance must have a name");
}
ReadPluginConfigs(child, pp);
pp->plugin_slot = plugin_slot;
pp->nstate = -1; // actual value to be filled in by the plugin later
}
child = child->NextSiblingElement();
}
}
// advance to next element
elem = elem->NextSiblingElement();
}
}
// custom section parser
void mjXReader::Custom(XMLElement* section) {
string text, name;
@@ -2800,6 +2928,38 @@ void mjXReader::Sensor(XMLElement* section) {
psen->datatype = (mjtDataType)n;
}
else if (type=="plugin") {
psen->type = mjSENS_PLUGIN;
ReadAttrTxt(elem, "plugin", psen->plugin_name);
ReadAttrTxt(elem, "instance", psen->plugin_instance_name);
if (psen->plugin_instance_name.empty()) {
psen->plugin_instance = model->AddPlugin();
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, psen->plugin_instance);
ReadAttrTxt(elem, "objtype", text);
psen->objtype = (mjtObj)mju_str2Type(text.c_str());
ReadAttrTxt(elem, "objname", psen->objname);
if (psen->objtype != mjOBJ_UNKNOWN && psen->objname.empty()) {
throw mjXError(elem, "objtype is specified but objname is not");
}
if (psen->objtype == mjOBJ_UNKNOWN && !psen->objname.empty()) {
throw mjXError(elem, "objname is specified but objtype is not");
}
ReadAttrTxt(elem, "reftype", text);
psen->reftype = (mjtObj)mju_str2Type(text.c_str());
ReadAttrTxt(elem, "refname", psen->refname);
if (psen->reftype != mjOBJ_UNKNOWN && psen->refname.empty()) {
throw mjXError(elem, "reftype is specified but refname is not");
}
if (psen->reftype == mjOBJ_UNKNOWN && !psen->refname.empty()) {
throw mjXError(elem, "refname is specified but reftype is not");
}
}
GetXMLPos(elem, psen);
// advance to next element
elem = elem->NextSiblingElement();
}
src/xml/xml_native_reader.h
+1
View File
@@ -37,6 +37,7 @@ class mjXReader : public mjXBase {
private:
// XML section specific to MJCF
void Default(tinyxml2::XMLElement* section, int parentid); // default section
void Extension(tinyxml2::XMLElement* section); // extension section
void Custom(tinyxml2::XMLElement* section); // custom section
void Visual(tinyxml2::XMLElement* section); // visual section
void Statistic(tinyxml2::XMLElement* section); // statistic section
src/xml/xml_native_writer.cc
+139 -9
View File
@@ -18,10 +18,15 @@
#include <cstddef>
#include <cstdio>
#include <string>
#include <unordered_set>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
@@ -604,12 +609,38 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange);
WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear);
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm);
// plugins: write config attributes
if (pact->is_plugin) {
if (!pact->plugin_instance_name.empty()) {
WriteAttrTxt(elem, "instance", pact->plugin_instance_name);
} else {
WriteAttrTxt(elem, "plugin", pact->plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
pact->plugin_instance->plugin_slot);
const char* c = &pact->plugin_instance->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
XMLElement* config_elem = InsertEnd(elem, "config");
WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
WriteAttrTxt(config_elem, "value", value);
c += value.size();
}
++c;
}
}
}
// non-plugins: write actuator parameters
else {
WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->actuator.dyntype);
WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->actuator.gaintype);
WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->actuator.biastype);
WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->actuator.dynprm);
WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->actuator.gainprm);
WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->actuator.biasprm);
}
// userdata
if (writingdefaults) {
@@ -659,6 +690,7 @@ void mjXWriter::Write(FILE* fp) {
writingdefaults = true;
Default(root, model->defaults[0]);
writingdefaults = false;
Extension(root);
Custom(root);
Asset(root);
Body(InsertEnd(root, "worldbody"), model->GetWorld());
@@ -1020,6 +1052,69 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) {
// extension section
void mjXWriter::Extension(XMLElement* root) {
// skip section if there is no required plugin
if (model->active_plugins.empty()) {
return;
}
// create section
XMLElement* section = InsertEnd(root, "extension");
// keep track of plugins whose <required> section have been created
std::unordered_set<const mjpPlugin*> seen_plugins;
// write all plugins
const mjpPlugin* last_plugin = nullptr;
XMLElement* required_elem = nullptr;
for (int i = 0; i < model->plugins.size(); ++i) {
mjCPlugin* pp = static_cast<mjCPlugin*>(model->GetObject(mjOBJ_PLUGIN, i));
if (pp->name.empty()) {
// reached the first unnamed plugin instance, meaning that it was created through an
// "implicit" plugin element, e.g. sensor or actuator
break;
}
// check if we need to open a new <required> section
const mjpPlugin* plugin = mjp_getPluginAtSlot(pp->plugin_slot);
if (plugin != last_plugin) {
required_elem = InsertEnd(section, "required");
WriteAttrTxt(required_elem, "plugin", plugin->name);
seen_plugins.insert(plugin);
last_plugin = plugin;
}
// write instance element
XMLElement* elem = InsertEnd(required_elem, "instance");
WriteAttrTxt(elem, "name", pp->name);
// write plugin config attributes
const char* c = &pp->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
XMLElement* config_elem = InsertEnd(elem, "config");
WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
WriteAttrTxt(config_elem, "value", value);
c += value.size();
}
++c;
}
}
// write <required> elements for plugins without explicit instances
for (const auto& [plugin, slot] : model->active_plugins) {
if (seen_plugins.find(plugin) == seen_plugins.end()) {
required_elem = InsertEnd(section, "required");
WriteAttrTxt(required_elem, "plugin", plugin->name);
}
}
}
// custom section
void mjXWriter::Custom(XMLElement* root) {
XMLElement* elem;
@@ -1400,7 +1495,12 @@ void mjXWriter::Actuator(XMLElement* root) {
// write all actuators
for (int i=0; i<num; i++) {
mjCActuator* pact = (mjCActuator*)model->GetObject(mjOBJ_ACTUATOR, i);
XMLElement* elem = InsertEnd(section, "general");
XMLElement* elem;
if (pact->is_plugin) {
elem = InsertEnd(section, "plugin");
} else {
elem = InsertEnd(section, "general");
}
OneActuator(elem, pact, pact->def);
}
}
@@ -1593,6 +1693,34 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "clock");
break;
// plugin-controlled sensor
case mjSENS_PLUGIN:
elem = InsertEnd(section, "plugin");
if (psen->objtype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype));
WriteAttrTxt(elem, "objname", psen->objname);
}
if (!psen->plugin_instance_name.empty()) {
WriteAttrTxt(elem, "instance", psen->plugin_instance_name);
} else {
WriteAttrTxt(elem, "plugin", psen->plugin_name);
const mjpPlugin* plugin = mjp_getPluginAtSlot(
psen->plugin_instance->plugin_slot);
const char* c = &psen->plugin_instance->flattened_attributes[0];
for (int i = 0; i < plugin->nattribute; ++i) {
std::string value(c);
if (!value.empty()) {
XMLElement* config_elem = InsertEnd(elem, "config");
WriteAttrTxt(config_elem, "key", plugin->attributes[i]);
WriteAttrTxt(config_elem, "value", value);
c += value.size();
}
++c;
}
}
break;
// user-defined sensor
case mjSENS_USER:
elem = InsertEnd(section, "user");
@@ -1610,11 +1738,13 @@ void mjXWriter::Sensor(XMLElement* root) {
// write name, noise, userdata
WriteAttrTxt(elem, "name", psen->name);
WriteAttr(elem, "cutoff", 1, &psen->cutoff, &zero);
WriteAttr(elem, "noise", 1, &psen->noise, &zero);
if (psen->type != mjSENS_PLUGIN) {
WriteAttr(elem, "noise", 1, &psen->noise, &zero);
}
WriteVector(elem, "user", psen->userdata);
// add reference if present
if (psen->reftype > 0) {
if (psen->reftype != mjOBJ_UNKNOWN) {
WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype));
WriteAttrTxt(elem, "refname", psen->refname);
}
src/xml/xml_native_writer.h
+1
View File
@@ -37,6 +37,7 @@ class mjXWriter : public mjXBase {
void Visual(tinyxml2::XMLElement* root); // visual section
void Statistic(tinyxml2::XMLElement* root); // statistic section
void Default(tinyxml2::XMLElement* root, mjCDef* def); // default section
void Extension(tinyxml2::XMLElement* root); // extension section
void Custom(tinyxml2::XMLElement* root); // custom section
void Asset(tinyxml2::XMLElement* root); // asset section
void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section