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

Implement hash map for mj_name2id using djb2 and linear probing.

Browse Source 
PiperOrigin-RevId: 499485127
Change-Id: I5aed6549db18b92d7c8a3b7590e50a00ce13a439
This commit is contained in:
Kyle Bayes
2023-01-04 07:57:11 -08:00
committed by Copybara-Service
parent 5261cceb30
commit f1007df013
11 changed files with 404 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+4
View File
@@ -2,6 +2,10 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
- The function :ref:`mj_name2id` is now implemented using a hash function instead of a linear search for better
performance.
Version 2.3.1 (December 6, 2022)
-----------------------------------
doc/includes/references.h
+2
View File
@@ -785,6 +785,7 @@ struct mjModel_ {
int nuser_actuator; // number of mjtNums in actuator_user
int nuser_sensor; // number of mjtNums in sensor_user
int nnames; // number of chars in all names
int nnames_map; // number of slots in the names hash map
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
@@ -1146,6 +1147,7 @@ struct mjModel_ {
int* name_keyadr; // keyframe name pointers (nkey x 1)
int* name_pluginadr; // plugin instance name pointers (nplugin x 1)
char* names; // names of all objects, 0-terminated (nnames x 1)
int* names_map; // internal hash map of names (nnames_map x 1)
};
typedef struct mjModel_ mjModel;
typedef enum mjtPluginCapabilityBit_ {
include/mujoco/mjmodel.h
+2
View File
@@ -581,6 +581,7 @@ struct mjModel_ {
int nuser_actuator; // number of mjtNums in actuator_user
int nuser_sensor; // number of mjtNums in sensor_user
int nnames; // number of chars in all names
int nnames_map; // number of slots in the names hash map
// sizes set after mjModel construction (only affect mjData)
int nM; // number of non-zeros in sparse inertia matrix
@@ -942,6 +943,7 @@ struct mjModel_ {
int* name_keyadr; // keyframe name pointers (nkey x 1)
int* name_pluginadr; // plugin instance name pointers (nplugin x 1)
char* names; // names of all objects, 0-terminated (nnames x 1)
int* names_map; // internal hash map of names (nnames_map x 1)
};
typedef struct mjModel_ mjModel;
include/mujoco/mjxmacro.h
+3 -2
View File
@@ -113,6 +113,7 @@
X( nuser_actuator ) \
X( nuser_sensor ) \
X( nnames ) \
X( nnames_map ) \
X( nM ) \
X( nD ) \
X( nemax ) \
@@ -432,8 +433,8 @@
X( int, name_tupleadr, ntuple, 1 ) \
X( int, name_keyadr, nkey, 1 ) \
X( int, name_pluginadr, nplugin, 1 ) \
X( char, names, nnames, 1 )
X( char, names, nnames, 1 ) \
X( int, names_map, nnames_map, 1 ) \
//-------------------------------- mjData ----------------------------------------------------------
src/engine/engine_crossplatform.h
+6
View File
@@ -46,4 +46,10 @@
#endif
#endif
#if defined(__GNUC__) && __GNUC__ >= 7
#define mjFALLTHROUGH __attribute__ ((fallthrough))
#else
#define mjFALLTHROUGH ((void) 0)
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_
src/engine/engine_io.c
+3
View File
@@ -438,6 +438,9 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
m->nuser_actuator = nuser_actuator;
m->nuser_sensor = nuser_sensor;
m->nnames = nnames;
m->nnames_map = nbody + njnt + ngeom + nsite + ncam + nlight + nmesh + nskin
+ nhfield + ntex + nmat + npair + nexclude + neq + ntendon
+ nu + nsensor + nnumeric + ntext + ntuple + nkey + nplugin;
#define X(name) \
if ((m->name) < 0) { \
src/engine/engine_support.c
+190 -81
View File
@@ -20,6 +20,7 @@
#include <mujoco/mjmodel.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_util_blas.h"
@@ -433,123 +434,231 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
//-------------------------- name functions --------------------------------------------------------
// get number of objects and name addresses for given object type
static int _getnumadr(const mjModel* m, mjtObj type, int** padr) {
static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
int num = -1;
// map address starts at the end, subtract with explicit switch fallthrough below
*mapadr = m->nnames_map;
// get address list and size for object type
switch (type) {
case mjOBJ_BODY:
case mjOBJ_XBODY:
*padr = m->name_bodyadr;
return m->nbody;
case mjOBJ_BODY:
case mjOBJ_XBODY:
*mapadr -= m->nbody;
*padr = m->name_bodyadr;
num = m->nbody;
mjFALLTHROUGH;
case mjOBJ_JOINT:
*padr = m->name_jntadr;
return m->njnt;
case mjOBJ_JOINT:
*mapadr -= m->njnt;
if (num < 0) {
*padr = m->name_jntadr;
num = m->njnt;
}
mjFALLTHROUGH;
case mjOBJ_GEOM:
*padr = m->name_geomadr;
return m->ngeom;
case mjOBJ_GEOM:
*mapadr -= m->ngeom;
if (num < 0) {
*padr = m->name_geomadr;
num = m->ngeom;
}
mjFALLTHROUGH;
case mjOBJ_SITE:
*padr = m->name_siteadr;
return m->nsite;
case mjOBJ_SITE:
*mapadr -= m->nsite;
if (num < 0) {
*padr = m->name_siteadr;
num = m->nsite;
}
mjFALLTHROUGH;
case mjOBJ_CAMERA:
*padr = m->name_camadr;
return m->ncam;
case mjOBJ_CAMERA:
*mapadr -= m->ncam;
if (num < 0) {
*padr = m->name_camadr;
num = m->ncam;
}
mjFALLTHROUGH;
case mjOBJ_LIGHT:
*padr = m->name_lightadr;
return m->nlight;
case mjOBJ_LIGHT:
*mapadr -= m->nlight;
if (num < 0) {
*padr = m->name_lightadr;
num = m->nlight;
}
mjFALLTHROUGH;
case mjOBJ_MESH:
*padr = m->name_meshadr;
return m->nmesh;
case mjOBJ_MESH:
*mapadr -= m->nmesh;
if (num < 0) {
*padr = m->name_meshadr;
num = m->nmesh;
}
mjFALLTHROUGH;
case mjOBJ_SKIN:
*padr = m->name_skinadr;
return m->nskin;
case mjOBJ_SKIN:
*mapadr -= m->nskin;
if (num < 0) {
*padr = m->name_skinadr;
num = m->nskin;
}
mjFALLTHROUGH;
case mjOBJ_HFIELD:
*padr = m->name_hfieldadr;
return m->nhfield;
case mjOBJ_HFIELD:
*mapadr -= m->nhfield;
if (num < 0) {
*padr = m->name_hfieldadr;
num = m->nhfield;
}
mjFALLTHROUGH;
case mjOBJ_TEXTURE:
*padr = m->name_texadr;
return m->ntex;
case mjOBJ_TEXTURE:
*mapadr -= m->ntex;
if (num < 0) {
*padr = m->name_texadr;
num = m->ntex;
}
mjFALLTHROUGH;
case mjOBJ_MATERIAL:
*padr = m->name_matadr;
return m->nmat;
case mjOBJ_MATERIAL:
*mapadr -= m->nmat;
if (num < 0) {
*padr = m->name_matadr;
num = m->nmat;
}
mjFALLTHROUGH;
case mjOBJ_PAIR:
*padr = m->name_pairadr;
return m->npair;
case mjOBJ_PAIR:
*mapadr -= m->npair;
if (num < 0) {
*padr = m->name_pairadr;
num = m->npair;
}
mjFALLTHROUGH;
case mjOBJ_EXCLUDE:
*padr = m->name_excludeadr;
return m->nexclude;
case mjOBJ_EXCLUDE:
*mapadr -= m->nexclude;
if (num < 0) {
*padr = m->name_excludeadr;
num = m->nexclude;
}
mjFALLTHROUGH;
case mjOBJ_EQUALITY:
*padr = m->name_eqadr;
return m->neq;
case mjOBJ_EQUALITY:
*mapadr -= m->neq;
if (num < 0) {
*padr = m->name_eqadr;
num = m->neq;
}
mjFALLTHROUGH;
case mjOBJ_TENDON:
*padr = m->name_tendonadr;
return m->ntendon;
case mjOBJ_TENDON:
*mapadr -= m->ntendon;
if (num < 0) {
*padr = m->name_tendonadr;
num = m->ntendon;
}
mjFALLTHROUGH;
case mjOBJ_ACTUATOR:
*padr = m->name_actuatoradr;
return m->nu;
case mjOBJ_ACTUATOR:
*mapadr -= m->nu;
if (num < 0) {
*padr = m->name_actuatoradr;
num = m->nu;
}
mjFALLTHROUGH;
case mjOBJ_SENSOR:
*padr = m->name_sensoradr;
return m->nsensor;
case mjOBJ_SENSOR:
*mapadr -= m->nsensor;
if (num < 0) {
*padr = m->name_sensoradr;
num = m->nsensor;
}
mjFALLTHROUGH;
case mjOBJ_NUMERIC:
*padr = m->name_numericadr;
return m->nnumeric;
case mjOBJ_NUMERIC:
*mapadr -= m->nnumeric;
if (num < 0) {
*padr = m->name_numericadr;
num = m->nnumeric;
}
mjFALLTHROUGH;
case mjOBJ_TEXT:
*padr = m->name_textadr;
return m->ntext;
case mjOBJ_TEXT:
*mapadr -= m->ntext;
if (num < 0) {
*padr = m->name_textadr;
num = m->ntext;
}
mjFALLTHROUGH;
case mjOBJ_TUPLE:
*padr = m->name_tupleadr;
return m->ntuple;
case mjOBJ_TUPLE:
*mapadr -= m->ntuple;
if (num < 0) {
*padr = m->name_tupleadr;
num = m->ntuple;
}
mjFALLTHROUGH;
case mjOBJ_KEY:
*padr = m->name_keyadr;
return m->nkey;
case mjOBJ_KEY:
*mapadr -= m->nkey;
if (num < 0) {
*padr = m->name_keyadr;
num = m->nkey;
}
mjFALLTHROUGH;
case mjOBJ_PLUGIN:
*padr = m->name_pluginadr;
return m->nplugin;
case mjOBJ_PLUGIN:
*mapadr -= m->nplugin;
if (num < 0) {
*padr = m->name_pluginadr;
num = m->nplugin;
}
mjFALLTHROUGH;
default:
*padr = 0;
return 0;
default:
if (num < 0) {
*padr = 0;
num = 0;
}
}
return num;
}
// get string hash, see http://www.cse.yorku.ca/~oz/hash.html
uint64_t mj_hashdjb2(const char* s, uint64_t n) {
uint64_t h = 5381;
int c;
while ((c = *s++)) {
h = ((h << 5) + h) + c;
}
return h % n;
}
// get id of object with specified name; -1: not found
int mj_name2id(const mjModel* m, int type, const char* name) {
int num = 0;
int mapadr;
int* adr = 0;
// get number of objects and name addresses
num = _getnumadr(m, type, &adr);
int num = _getnumadr(m, type, &adr, &mapadr);
// search
if (num) {
for (int i=0; i<num; i++) {
if (!strncmp(name, m->names+adr[i], m->nnames-adr[i])) {
return i;
}
}
}
// look up at hash address
uint64_t hash = mj_hashdjb2(name, num);
uint64_t i = hash;
do {
int j = m->names_map[mapadr + i];
if (j < 0) return -1;
if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) {
return j;
}
if (++i == num) i = 0;
} while(i != hash);
}
return -1;
}
@@ -557,11 +666,11 @@ int mj_name2id(const mjModel* m, int type, const char* name) {
// get name of object with specified id; 0: invalid type or id, or null name
const char* mj_id2name(const mjModel* m, int type, int id) {
int num = 0;
int mapadr;
int* adr = 0;
// get number of objects and name addresses
num = _getnumadr(m, type, &adr);
int num = _getnumadr(m, type, &adr, &mapadr);
if (id>=0 && id<num) {
if (m->names[adr[id]]) {
src/engine/engine_support.h
+3
View File
@@ -78,6 +78,9 @@ MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
//-------------------------- name functions --------------------------------------------------------
// get string hash, see http://www.cse.yorku.ca/~oz/hash.html
uint64_t mj_hashdjb2(const char* s, uint64_t n);
// get id of object with specified name; -1: not found; type is mjtObj
MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
src/user/user_model.cc
+81 -23
View File
@@ -1201,7 +1201,21 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
// process names from one list: concatenate, compute addresses
template <class T>
static int namelist(vector<T*>& list, int adr, int* name_adr, char* names) {
static int namelist(vector<T*>& list, int adr, int* name_adr, char* names, int* map) {
// compute hash map addresses
for (unsigned int i=0; i<list.size(); i++) {
// ignore empty strings
if (list[i]->name.empty()) {
continue;
}
uint64_t j = mj_hashdjb2(list[i]->name.c_str(), list.size());
// find first empty slot using linear probing
for (; map[j]!=-1; j=(j+1) % list.size()) {}
map[j] = i;
}
for (unsigned int i=0; i<list.size(); i++) {
name_adr[i] = adr;
@@ -1222,31 +1236,75 @@ static int namelist(vector<T*>& list, int adr, int* name_adr, char* names) {
void mjCModel::CopyNames(mjModel* m) {
// start with model name
int adr = (int)modelname.size()+1;
int* map_adr = m->names_map;
mju_strncpy(m->names, modelname.c_str(), m->nnames);
memset(m->names_map, -1, sizeof(int) * m->nnames_map);
// process all lists
adr = namelist(bodies, adr, m->name_bodyadr, m->names);
adr = namelist(joints, adr, m->name_jntadr, m->names);
adr = namelist(geoms, adr, m->name_geomadr, m->names);
adr = namelist(sites, adr, m->name_siteadr, m->names);
adr = namelist(cameras, adr, m->name_camadr, m->names);
adr = namelist(lights, adr, m->name_lightadr, m->names);
adr = namelist(meshes, adr, m->name_meshadr, m->names);
adr = namelist(skins, adr, m->name_skinadr, m->names);
adr = namelist(hfields, adr, m->name_hfieldadr, m->names);
adr = namelist(textures, adr, m->name_texadr, m->names);
adr = namelist(materials, adr, m->name_matadr, m->names);
adr = namelist(pairs, adr, m->name_pairadr, m->names);
adr = namelist(excludes, adr, m->name_excludeadr, m->names);
adr = namelist(equalities, adr, m->name_eqadr, m->names);
adr = namelist(tendons, adr, m->name_tendonadr, m->names);
adr = namelist(actuators, adr, m->name_actuatoradr, m->names);
adr = namelist(sensors, adr, m->name_sensoradr, m->names);
adr = namelist(numerics, adr, m->name_numericadr, m->names);
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);
adr = namelist(bodies, adr, m->name_bodyadr, m->names, map_adr);
map_adr += bodies.size();
adr = namelist(joints, adr, m->name_jntadr, m->names, map_adr);
map_adr += joints.size();
adr = namelist(geoms, adr, m->name_geomadr, m->names, map_adr);
map_adr += geoms.size();
adr = namelist(sites, adr, m->name_siteadr, m->names, map_adr);
map_adr += sites.size();
adr = namelist(cameras, adr, m->name_camadr, m->names, map_adr);
map_adr += cameras.size();
adr = namelist(lights, adr, m->name_lightadr, m->names, map_adr);
map_adr += lights.size();
adr = namelist(meshes, adr, m->name_meshadr, m->names, map_adr);
map_adr += meshes.size();
adr = namelist(skins, adr, m->name_skinadr, m->names, map_adr);
map_adr += skins.size();
adr = namelist(hfields, adr, m->name_hfieldadr, m->names, map_adr);
map_adr += hfields.size();
adr = namelist(textures, adr, m->name_texadr, m->names, map_adr);
map_adr += textures.size();
adr = namelist(materials, adr, m->name_matadr, m->names, map_adr);
map_adr += materials.size();
adr = namelist(pairs, adr, m->name_pairadr, m->names, map_adr);
map_adr += pairs.size();
adr = namelist(excludes, adr, m->name_excludeadr, m->names, map_adr);
map_adr += excludes.size();
adr = namelist(equalities, adr, m->name_eqadr, m->names, map_adr);
map_adr += equalities.size();
adr = namelist(tendons, adr, m->name_tendonadr, m->names, map_adr);
map_adr += tendons.size();
adr = namelist(actuators, adr, m->name_actuatoradr, m->names, map_adr);
map_adr += actuators.size();
adr = namelist(sensors, adr, m->name_sensoradr, m->names, map_adr);
map_adr += sensors.size();
adr = namelist(numerics, adr, m->name_numericadr, m->names, map_adr);
map_adr += numerics.size();
adr = namelist(texts, adr, m->name_textadr, m->names, map_adr);
map_adr += texts.size();
adr = namelist(tuples, adr, m->name_tupleadr, m->names, map_adr);
map_adr += tuples.size();
adr = namelist(keys, adr, m->name_keyadr, m->names, map_adr);
map_adr += keys.size();
adr = namelist(plugins, adr, m->name_pluginadr, m->names, map_adr);
// check size, SHOULD NOT OCCUR
if (adr != nnames) {
test/engine/engine_support_test.cc
+108
View File
@@ -155,6 +155,114 @@ TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
mj_deleteModel(model);
}
using Name2idTest = MujocoTest;
static constexpr char name2idTestingModel[] = R"(
<mujoco>
<asset>
<texture name="texture1" type="2d" builtin="checker" rgb1="1 1 1"
rgb2="1 1 1" width="300" height="300" mark="none"/>
<material name="material1" texture="texture1" texrepeat="1 1"
texuniform="true" reflectance=".2"/>
</asset>
<asset>
<mesh name="mesh1" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
</asset>
<worldbody>
<light name="light1" pos="0 0 1"/>
<site name="site1" pos="0 0 .3" size=".01"/>
<site name="site2" pos="-.1 -.1 -.1" size=".01"/>
<camera name="camera1" pos="0 -1.3 .5" xyaxes="1 0 0 0 1 2"/>
<body name="body1">
<joint axis="0 1 0" name="joint1"/>
<geom size="1" name="body1_geom1"/>
<geom size="1" name="body1_geom2"/>
</body>
<body name="body2">
<joint axis="0 1 0" name="joint2"/>
<geom size="1" name="body2_geom1"/>
<geom size="1" name="camera1"/>
</body>
<body name="">
</body>
</worldbody>
<tendon>
<spatial name="tendon1" limited="true" range="0 0.35" width="0.003">
<site site="site1"/>
<site site="site2"/>
</spatial>
</tendon>
<actuator>
<motor name="actuator1" joint="joint1" gear="1"/>
</actuator>
<sensor>
<accelerometer name="sensor1" site="site1"/>
</sensor>
</mujoco>
)";
TEST_F(Name2idTest, FindIds) {
mjModel* model = LoadModelFromString(name2idTestingModel);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0);
mj_deleteModel(model);
}
TEST_F(Name2idTest, MissingIds) {
mjModel* model = LoadModelFromString(name2idTestingModel);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1);
mj_deleteModel(model);
}
TEST_F(Name2idTest, EmptyIds) {
mjModel* model = LoadModelFromString(name2idTestingModel);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1);
mj_deleteModel(model);
}
TEST_F(Name2idTest, Namespaces) {
mjModel* model = LoadModelFromString(name2idTestingModel);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3);
mj_deleteModel(model);
}
using VersionTest = MujocoTest;
TEST_F(VersionTest, MjVersion) {
unity/Runtime/Bindings/MjBindings.cs
+2
View File
@@ -1911,6 +1911,7 @@ public unsafe struct mjModel_ {
public int nuser_actuator;
public int nuser_sensor;
public int nnames;
public int nnames_map;
public int nM;
public int nD;
public int nemax;
@@ -2210,6 +2211,7 @@ public unsafe struct mjModel_ {
public int* name_keyadr;
public int* name_pluginadr;
public char* names;
public int* names_map;
}
[StructLayout(LayoutKind.Sequential)]