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

Add attach tag to MJCF.

Browse Source 
PiperOrigin-RevId: 651117953
Change-Id: I57b0e53ed44607d479b3785d908e0837c0804cf3
This commit is contained in:
Alessio Quaglino
2024-07-10 12:40:50 -07:00
committed by Copybara-Service
parent 5b080bacc4
commit 27b9ddda5b
19 changed files with 419 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+18
View File
@@ -101,6 +101,7 @@ int mjs_attachBody(mjsFrame* parent, const mjsBody* child,
try {
*frame_parent += std::string(prefix) + *child_body + std::string(suffix);
} catch (mjCError& e) {
frame_parent->model->SetError(e);
return -1;
}
return 0;
@@ -116,6 +117,7 @@ int mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
try {
*body_parent += std::string(prefix) + *child_frame + std::string(suffix);
} catch (mjCError& e) {
body_parent->model->SetError(e);
return -1;
}
return 0;
@@ -158,6 +160,14 @@ void mj_deleteSpec(mjSpec* s) {
// add spec (model asset) to spec
void mjs_addSpec(mjSpec* s, mjSpec* child) {
mjCModel* model = static_cast<mjCModel*>(s->element);
model->AppendSpec(child);
}
// delete object, it will call the appropriate destructor since ~mjCBase is virtual
void mjs_delete(mjsElement* element) {
mjCBase* object = static_cast<mjCBase*>(element);
@@ -466,6 +476,14 @@ mjSpec* mjs_getSpec(mjsBody* body) {
// find spec (model asset) by name
mjSpec* mjs_findSpec(mjSpec* s, const char* name) {
mjCModel* model = static_cast<mjCModel*>(s->element);
return model->FindSpec(name);
}
// get default
mjsDefault* mjs_getDefault(mjsElement* element) {
mjCModel* model = static_cast<mjCBase*>(element)->model;
src/user/user_api.h
+6
View File
@@ -60,6 +60,9 @@ MJAPI void mj_copyBack(mjSpec* s, const mjModel* m);
// Delete spec.
MJAPI void mj_deleteSpec(mjSpec* s);
// Add spec (model asset) to spec.
MJAPI void mjs_addSpec(mjSpec* s, mjSpec* child);
//---------------------------------- Attachment ----------------------------------------------------
@@ -182,6 +185,9 @@ MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, mjsDefault* def);
// Get spec from body.
MJAPI mjSpec* mjs_getSpec(mjsBody* body);
// Find spec (model asset) by name.
MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name);
// Find body in model by name.
MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name);
src/user/user_model.cc
+25 -1
View File
@@ -16,6 +16,7 @@
#include <algorithm>
#include <cmath>
#include <array>
#include <csetjmp>
#include <cstddef>
#include <cstdint>
@@ -33,7 +34,6 @@
#include <mujoco/mjspec.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjvisualize.h>
#include "cc/array_safety.h"
#include "engine/engine_forward.h"
#include "engine/engine_io.h"
@@ -338,6 +338,7 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
// add default tree to this model
mjCModel_& mjCModel::operator+=(mjCDef& subtree) {
defaults_.push_back(&subtree);
def_map[subtree.name] = &subtree;
// set parent to the main default if this is not the only default in the model
if (!subtree.parent && &subtree != defaults_[0]) {
@@ -660,6 +661,11 @@ mjCPlugin* mjCModel::AddPlugin() {
}
// append spec to spec
void mjCModel::AppendSpec(mjSpec* spec) {
specs_.push_back(spec);
}
//------------------------ API FOR ACCESS TO MODEL ELEMENTS ---------------------------------------
@@ -886,6 +892,18 @@ mjCFrame* mjCModel::FindFrame(mjCBody* body, std::string name) const{
// find spec by name
mjSpec* mjCModel::FindSpec(std::string name) const {
for (auto spec : specs_) {
if (mjs_getString(spec->modelname) == name) {
return spec;
}
}
return nullptr;
}
// detect null pose
bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) const {
bool result = true;
@@ -3247,6 +3265,12 @@ mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) {
return nullptr;
}
// destroy attached specs
for (auto spec : specs_) {
mj_deleteSpec(spec);
}
specs_.clear();
// restore error handler, mark as compiled, return mjModel
_mjPRIVATE__set_tls_error_fn(save_error);
_mjPRIVATE__set_tls_warning_fn(save_warning);
src/user/user_model.h
+4 -1
View File
@@ -192,7 +192,7 @@ class mjCModel : public mjCModel_, private mjSpec {
mjCTuple* AddTuple();
mjCKey* AddKey();
mjCPlugin* AddPlugin();
void AppendSpec(mjSpec* spec);
// delete elements marked as discard=true
template <class T> void Delete(std::vector<T*>& elements,
@@ -209,12 +209,14 @@ class mjCModel : public mjCModel_, private mjSpec {
// API for access to other variables
bool IsCompiled() const; // is model already compiled
const mjCError& GetError() const; // get reference of error object
void SetError(const mjCError& error) { errInfo = error; } // set value of error object
mjCBody* GetWorld(); // pointer to world body
mjCDef* FindDefault(std::string name); // find defaults class name
mjCDef* AddDefault(std::string name, mjCDef* parent = nullptr); // add defaults class to array
mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name
mjCBody* FindBody(mjCBody* body, std::string name); // find body given name
mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name
mjSpec* FindSpec(std::string name) const; // find spec given name
bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose
void SetActivePlugins(const std::vector<std::pair<const mjpPlugin*, int>>&& active_plugins) {
active_plugins_ = std::move(active_plugins);
@@ -312,6 +314,7 @@ class mjCModel : public mjCModel_, private mjSpec {
std::vector<mjCTuple*> tuples_; // list of tuple fields
std::vector<mjCKey*> keys_; // list of keyframe fields
std::vector<mjCPlugin*> plugins_; // list of plugin instances
std::vector<mjSpec*> specs_; // list of specs
// pointers to objects created inside kinematic tree
std::vector<mjCBody*> bodies_; // list of bodies
src/user/user_objects.cc
+15
View File
@@ -697,6 +697,9 @@ void mjCBase::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
if (!classname.empty() && m != model) {
classname = m->prefix + classname + m->suffix;
}
}
@@ -1000,6 +1003,9 @@ void mjCBody::NameSpace_(const mjCModel* m, bool propagate) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
if (!classname.empty() && m != model) {
classname = m->prefix + classname + m->suffix;
}
for (auto& body : bodies) {
body->prefix = m->prefix;
@@ -2004,6 +2010,9 @@ void mjCGeom::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
if (!classname.empty() && m != model) {
classname = m->prefix + classname + m->suffix;
}
if (!spec_material_.empty() && model != m) {
spec_material_ = m->prefix + spec_material_ + m->suffix;
}
@@ -2768,6 +2777,9 @@ void mjCCamera::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
if (!classname.empty() && m != model) {
classname = m->prefix + classname + m->suffix;
}
if (!spec_targetbody_.empty()) {
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
}
@@ -2917,6 +2929,9 @@ void mjCLight::NameSpace(const mjCModel* m) {
if (!name.empty()) {
name = m->prefix + name + m->suffix;
}
if (!classname.empty() && m != model) {
classname = m->prefix + classname + m->suffix;
}
if (!spec_targetbody_.empty()) {
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
}
src/xml/xml.cc
+1 -1
View File
@@ -369,7 +369,7 @@ mjSpec* mjParseXML(const char* filename, const mjVFS* vfs,
// parse MuJoCo model
parser.SetModel(spec);
parser.Parse(root);
parser.Parse(root, vfs);
}
else if (!strcasecmp(root->Value(), "robot")) {
src/xml/xml_base.h
+2 -1
View File
@@ -19,6 +19,7 @@
#include <string>
#include "tinyxml2.h"
#include <mujoco/mjmodel.h>
#include <mujoco/mjspec.h>
#include "xml/xml_util.h"
@@ -79,7 +80,7 @@ class mjXBase : public mjXUtil {
virtual ~mjXBase() = default;
// parse: implemented in derived parser classes
virtual void Parse(tinyxml2::XMLElement* root) {};
virtual void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr) {};
// write: implemented in derived writer class
virtual std::string Write(char *error, std::size_t error_sz) {
src/xml/xml_native_reader.cc
+59 -5
View File
@@ -14,6 +14,7 @@
#include "xml/xml_native_reader.h"
#include <array>
#include <cstddef>
#include <cstdio>
#include <cstring>
@@ -38,6 +39,7 @@
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include <mujoco/mjspec.h>
#include "user/user_api.h"
#include "user/user_composite.h"
#include "user/user_flexcomp.h"
#include "user/user_util.h"
@@ -243,6 +245,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
"hflip", "vflip"},
{"material", "*", "12", "name", "class", "texture", "texrepeat", "texuniform",
"emission", "specular", "shininess", "reflectance", "metallic", "roughness", "rgba"},
{"model", "*", "2", "name", "file"},
{">"},
{"body", "R", "11", "name", "childclass", "pos", "quat", "mocap",
@@ -267,6 +270,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"attach", "*", "3", "model", "body", "prefix"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
"material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
{"camera", "*", "20", "name", "class", "orthographic", "fovy", "ipd", "resolution", "pos",
@@ -805,7 +809,7 @@ void mjXReader::PrintSchema(std::stringstream& str, bool html, bool pad) {
// main entry point for XML parser
// mjCModel is allocated here; caller is responsible for deallocation
void mjXReader::Parse(XMLElement* root) {
void mjXReader::Parse(XMLElement* root, const mjVFS* vfs) {
// check schema
if (!schema.GetError().empty()) {
throw mjXError(0, "XML Schema Construction Error: %s\n",
@@ -880,7 +884,7 @@ void mjXReader::Parse(XMLElement* root) {
for (XMLElement* section = FirstChildElement(root, "asset"); section;
section = NextSiblingElement(section, "asset")) {
Asset(section);
Asset(section, vfs);
}
for (XMLElement* section = FirstChildElement(root, "contact"); section;
@@ -3045,7 +3049,7 @@ void mjXReader::Visual(XMLElement* section) {
// asset section parser
void mjXReader::Asset(XMLElement* section) {
void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) {
int n;
string text, name, texname, content_type;
XMLElement* elem;
@@ -3209,6 +3213,27 @@ void mjXReader::Asset(XMLElement* section) {
}
}
// model sub-element
else if (name=="model") {
auto filename = modelfiledir_ + ReadAttrFile(elem, "file", "").value();
// parse the child
std::array<char, 1024> error;
mjSpec* child = mj_parseXML(filename.c_str(), vfs, error.data(), error.size());
if (!child) {
throw mjXError(elem, "could not parse model file with error: %s", error.data());
}
// overwrite model name if given
std::string modelname = "";
if (ReadAttrTxt(elem, "name", modelname)) {
mjs_setString(child->modelname, modelname.c_str());
}
// store child spec in model
mjs_addSpec(model, child);
}
// advance to next element
elem = NextSiblingElement(elem);
}
@@ -3437,8 +3462,8 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) {
Body(elem, subtree, pframe);
// attach to parent
if (mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str()) < 0) {
throw mjXError(elem, "failed to attach frame");
if (mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str()) != 0) {
throw mjXError(elem, mjs_getError(model));
}
}
@@ -3496,6 +3521,35 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) {
Body(elem, pchild, nullptr);
}
// attachment
else if (name=="attach") {
std::string model_name, body_name, prefix;
ReadAttrTxt(elem, "model", model_name);
ReadAttrTxt(elem, "body", body_name);
ReadAttrTxt(elem, "prefix", prefix);
mjsBody* child = mjs_findBody(model, (prefix+body_name).c_str());
mjsFrame* pframe = frame ? frame : mjs_addFrame(pbody, nullptr);
if (!child) {
mjSpec* asset = mjs_findSpec(model, model_name.c_str());
if (!asset) {
throw mjXError(0, "could not find model '%s'", model_name.c_str());
}
child = mjs_findBody(asset, body_name.c_str());
if (!child) {
throw mjXError(0, "could not find body '%s''%s'", body_name.c_str());
}
if (mjs_attachBody(pframe, child, prefix.c_str(), "") != 0) {
mj_deleteSpec(asset);
throw mjXError(elem, mjs_getError(model));
}
} else {
// only set frame to existing body
mjs_setFrame(child->element, pframe);
}
}
// no match
else {
throw mjXError(elem, "unrecognized model element '%s'", name.c_str());
src/xml/xml_native_reader.h
+3 -3
View File
@@ -30,7 +30,7 @@ class mjXReader : public mjXBase {
mjXReader(); // constructor
virtual ~mjXReader() = default; // destructor
void Parse(tinyxml2::XMLElement* root); // parse XML document
void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr); // parse XML document
void PrintSchema(std::stringstream& str, bool html, bool pad); // print text or HTML schema
void SetModelFileDir(std::string modelfiledir);
@@ -53,7 +53,7 @@ class mjXReader : public mjXBase {
void Custom(tinyxml2::XMLElement* section); // custom section
void Visual(tinyxml2::XMLElement* section); // visual section
void Statistic(tinyxml2::XMLElement* section); // statistic section
void Asset(tinyxml2::XMLElement* section); // asset section
void Asset(tinyxml2::XMLElement* section, const mjVFS* vfs); // asset section
void Body(tinyxml2::XMLElement* section, mjsBody* pbody,
mjsFrame* pframe); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
@@ -99,7 +99,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 230
#define nMJCF 232
extern const char* MJCF[nMJCF][mjXATTRNUM];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_urdf.cc
+1 -1
View File
@@ -364,7 +364,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
}
}
void mjXURDF::Parse(XMLElement* root) {
void mjXURDF::Parse(XMLElement* root, const mjVFS* vfs) {
double pos[3] = {0};
mjuu_setvec(pos, 0, 0, 0);
double quat[4] = {1, 0, 0, 0};
src/xml/xml_urdf.h
+2 -1
View File
@@ -19,6 +19,7 @@
#include <unordered_set>
#include <vector>
#include <mujoco/mjmodel.h>
#include <mujoco/mjspec.h>
#include "xml/xml_base.h"
#include "tinyxml2.h"
@@ -41,7 +42,7 @@ class mjXURDF : public mjXBase {
double* pos,
double* quat,
bool static_body);
void Parse(tinyxml2::XMLElement* root); // main parser
void Parse(tinyxml2::XMLElement* root, const mjVFS* vfs = nullptr); // main parser
private:
std::string GetPrefixedName(const std::string& name); // get prefix/name of element