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

Add contact sensor.

Browse Source 
PiperOrigin-RevId: 783011982
Change-Id: Ica56fe9d520fa1d1ee7338e09148b1a55a049912
This commit is contained in:
Yuval Tassa
2025-07-14 13:01:55 -07:00
committed by Copybara-Service
parent e441868dad
commit d0e4771c8c
25 changed files with 1343 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_base.h
+3
View File
@@ -43,6 +43,7 @@ extern const int gain_sz;
extern const int bias_sz;
extern const int stage_sz;
extern const int datatype_sz;
extern const int reduce_sz;
extern const mjMap angle_map[];
extern const mjMap enable_map[];
extern const mjMap bool_map[];
@@ -70,6 +71,8 @@ extern const mjMap gain_map[];
extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap condata_map[];
extern const mjMap reduce_map[];
extern const mjMap meshtype_map[];
extern const mjMap meshinertia_map[];
extern const mjMap flexself_map[];
src/xml/xml_native_reader.cc
+95
View File
@@ -34,6 +34,7 @@
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include <mujoco/mjspec.h>
@@ -481,6 +482,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"distance", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"normal", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"fromto", "*", "8", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"contact", "*", "12", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site",
"num", "data", "reduce", "cutoff", "noise", "user"},
{"e_potential", "*", "4", "name", "cutoff", "noise", "user"},
{"e_kinetic", "*", "4", "name", "cutoff", "noise", "user"},
{"clock", "*", "4", "name", "cutoff", "noise", "user"},
@@ -606,6 +609,7 @@ const mjMap texrole_map[texrole_sz] = {
{"orm", mjTEXROLE_ORM},
};
// integrator type
const int integrator_sz = 4;
const mjMap integrator_map[integrator_sz] = {
@@ -615,6 +619,7 @@ const mjMap integrator_map[integrator_sz] = {
{"implicitfast", mjINT_IMPLICITFAST}
};
// cone type
const int cone_sz = 2;
const mjMap cone_map[cone_sz] = {
@@ -743,6 +748,28 @@ const mjMap datatype_map[datatype_sz] = {
};
// contact data type
const mjMap condata_map[mjNCONDATA] = {
{"found", mjCONDATA_FOUND},
{"force", mjCONDATA_FORCE},
{"torque", mjCONDATA_TORQUE},
{"dist", mjCONDATA_DIST},
{"pos", mjCONDATA_POS},
{"normal", mjCONDATA_NORMAL},
{"tangent", mjCONDATA_TANGENT}
};
// contact reduction type
const int reduce_sz = 4;
const mjMap reduce_map[reduce_sz] = {
{"none", 0},
{"mindist", 1},
{"maxforce", 2},
{"netforce", 3}
};
// LR mode
const int lrmode_sz = 4;
const mjMap lrmode_map[lrmode_sz] = {
@@ -4132,6 +4159,74 @@ void mjXReader::Sensor(XMLElement* section) {
}
}
// sensor for contacts; attached to geoms or bodies or a site
else if (type == "contact") {
// first matching criterion
bool has_site = ReadAttrTxt(elem, "site", objname);
bool has_body1 = ReadAttrTxt(elem, "body1", objname);
bool has_subtree1 = ReadAttrTxt(elem, "subtree1", objname);
bool has_geom1 = ReadAttrTxt(elem, "geom1", objname);
if (has_site + has_body1 + has_subtree1 + has_geom1 > 1) {
throw mjXError(elem, "at most one of (geom1, body1, subtree1, site) can be specified");
}
if (has_site) { sensor->objtype = mjOBJ_SITE; }
else if (has_body1) { sensor->objtype = mjOBJ_BODY; }
else if (has_subtree1) { sensor->objtype = mjOBJ_XBODY; }
else if (has_geom1) { sensor->objtype = mjOBJ_GEOM; }
else { sensor->objtype = mjOBJ_UNKNOWN; }
// second matching criterion
bool has_body2 = ReadAttrTxt(elem, "body2", refname);
bool has_subtree2 = ReadAttrTxt(elem, "subtree2", refname);
bool has_geom2 = ReadAttrTxt(elem, "geom2", refname);
if (has_body2 + has_subtree2 + has_geom2 > 1) {
throw mjXError(elem, "at most one of (geom2, body2, subtree2) can be specified");
}
if (has_body2) { sensor->reftype = mjOBJ_BODY; }
else if (has_subtree2) { sensor->reftype = mjOBJ_XBODY; }
else if (has_geom2) { sensor->reftype = mjOBJ_GEOM; }
else { sensor->reftype = mjOBJ_UNKNOWN; }
// process data specification (intprm[0])
int dataspec = 1 << mjCONDATA_FOUND;
std::vector<int> condata(mjNCONDATA);
int nkeys = MapValues(elem, "data", condata.data(), condata_map, mjNCONDATA);
if (nkeys) {
dataspec = 1 << condata[0];
// check ordering while adding bits to dataspec
for (int i = 1; i < nkeys; ++i) {
if (condata[i] <= condata[i-1]) {
std::string correct_order;
for (int j = 0; j < mjNCONDATA; ++j) {
correct_order += condata_map[j].key;
if (j < mjNCONDATA - 1) correct_order += ", ";
}
throw mjXError(elem, "data attributes must be in order: %s", correct_order.c_str());
}
dataspec |= 1 << condata[i];
}
}
sensor->intprm[0] = dataspec;
// number of contacts, sensor dim
sensor->dim = 1;
ReadAttrInt(elem, "num", &sensor->dim);
if (sensor->dim <= 0) {
throw mjXError(elem, "'num' must be positive in sensor");
}
sensor->dim *= mju_condataSize(dataspec);
// reduction type (intprm[1])
sensor->intprm[1] = 0;
if (MapValue(elem, "reduce", &n, reduce_map, reduce_sz)) {
sensor->intprm[1] = n;
}
// sensor type
sensor->type = mjSENS_CONTACT;
}
// global sensors
else if (type == "e_potential") {
sensor->type = mjSENS_E_POTENTIAL;
src/xml/xml_native_reader.h
+1 -1
View File
@@ -101,7 +101,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 239
#define nMJCF 240
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+34 -2
View File
@@ -28,6 +28,7 @@
#include <mujoco/mujoco.h>
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
@@ -76,7 +77,7 @@ static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) {
// top level sections
std::array<string, 17> sections = {
"<actuator", "<asset", "<compiler", "<contact", "<custom",
"<actuator", "<asset", "<compiler", "<contact>", "<custom",
"<default>", "<deformable", "<equality", "<extension", "<keyframe",
"<option", "<sensor", "<size", "<statistic", "<tendon",
"<visual", "<worldbody"};
@@ -2205,7 +2206,38 @@ void mjXWriter::Sensor(XMLElement* root) {
WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname());
WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname());
break;
case mjSENS_CONTACT:
{
elem = InsertEnd(section, "contact");
if (sensor->objtype == mjOBJ_BODY) {
WriteAttrTxt(elem, "body1", sensor->get_objname());
} else if (sensor->objtype == mjOBJ_XBODY) {
WriteAttrTxt(elem, "subtree1", sensor->get_objname());
} else if (sensor->objtype == mjOBJ_GEOM) {
WriteAttrTxt(elem, "geom1", sensor->get_objname());
} else if (sensor->objtype == mjOBJ_SITE) {
WriteAttrTxt(elem, "site", sensor->get_objname());
}
if (sensor->reftype == mjOBJ_BODY) {
WriteAttrTxt(elem, "body2", sensor->get_refname());
} else if (sensor->reftype == mjOBJ_XBODY) {
WriteAttrTxt(elem, "subtree2", sensor->get_refname());
} else if (sensor->reftype == mjOBJ_GEOM) {
WriteAttrTxt(elem, "geom2", sensor->get_refname());
}
int dataspec = sensor->intprm[0];
WriteAttrInt(elem, "num", sensor->dim / mju_condataSize(dataspec), 1);
int data[mjNCONDATA];
int ndata = 0;
for (int i=0; i < mjNCONDATA; i++) {
if (dataspec & (1 << i)) {
data[ndata++] = i;
}
}
WriteAttrKeys(elem, "data", condata_map, mjNCONDATA, data, ndata, 0);
WriteAttrKey(elem, "reduce", reduce_map, reduce_sz, sensor->intprm[1], 0);
}
break;
// global sensors
case mjSENS_E_POTENTIAL:
elem = InsertEnd(section, "potential");
src/xml/xml_util.cc
+54 -1
View File
@@ -792,7 +792,7 @@ XMLElement* mjXUtil::FindSubElem(XMLElement* elem, std::string name, bool requir
// find attribute, translate key, return int value
// find attribute, translate key into data, return true if found
bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required) {
// get attribute text
@@ -814,6 +814,42 @@ bool mjXUtil::MapValue(XMLElement* elem, const char* attr, int* data,
// find attribute, translate unique space-separated keys to data, return number of keys found
int mjXUtil::MapValues(XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required) {
// get attribute text
auto maybe_text = ReadAttrStr(elem, attr, required);
if (!maybe_text.has_value()) {
return 0;
}
std::string text = maybe_text.value();
std::istringstream strm(text);
std::string key;
std::set<std::string> found_keys;
int count = 0;
while (strm >> key) {
if (found_keys.count(key)) {
throw mjXError(elem, "duplicate keyword: '%s'");
return 0;
}
int value = FindKey(map, mapSz, key);
if (value == -1) {
throw mjXError(elem, "invalid keyword: '%s'");
return 0;
}
found_keys.insert(key);
data[count++] = value;
}
return count;
}
//---------------------------------- write functions -----------------------------------------------
// check if double is int
@@ -970,3 +1006,20 @@ void mjXUtil::WriteAttrKey(XMLElement* elem, std::string name,
WriteAttrTxt(elem, name, FindValue(map, mapsz, data));
}
// write attribute- space-separated keywords
void mjXUtil::WriteAttrKeys(XMLElement* elem, std::string name, const mjMap* map,
int mapsz, int* data, int ndata, int def) {
// skip default
if (ndata == 1 && data[0] == def) {
return;
}
std::string text = FindValue(map, mapsz, data[0]);
for (int i = 1; i < ndata; ++i) {
text += " " + FindValue(map, mapsz, data[i]);
}
WriteAttrTxt(elem, name, text);
}
src/xml/xml_util.h
+8
View File
@@ -183,6 +183,10 @@ class mjXUtil {
static bool MapValue(tinyxml2::XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required = false);
// find attribute, translate unique space-separated keys to data, return number of keys found
static int MapValues(tinyxml2::XMLElement* elem, const char* attr, int* data,
const mjMap* map, int mapSz, bool required = false);
// write attribute- any type
template<typename T>
static void WriteAttr(tinyxml2::XMLElement* elem, std::string name, int n, const T* data,
@@ -204,6 +208,10 @@ class mjXUtil {
static void WriteAttrKey(tinyxml2::XMLElement* elem, std::string name,
const mjMap* map, int mapsz, int data, int def = -12345);
// write attribute- space-separated keywords
static void WriteAttrKeys(XMLElement* elem, std::string name, const mjMap* map,
int mapsz, int* data, int ndata, int def = -12345);
private:
template<typename T>
static bool ReadAttrValues(tinyxml2::XMLElement* elem, const char* attr,