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

Add childclass support to frames.

Browse Source 
PiperOrigin-RevId: 615487831
Change-Id: I0e4d2efd52211701b4286a0b0de84424470a3a47
This commit is contained in:
Alessio Quaglino
2024-03-13 11:29:50 -07:00
committed by Copybara-Service
parent 0d16587135
commit bda03af49a
17 changed files with 195 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+14 -2
View File
@@ -2174,8 +2174,8 @@ defined. Its body name is automatically defined as "world".
:at:`childclass`: :at-val:`string, optional`
If this attribute is present, all descendant elements that admit a defaults class will use the class specified here,
unless they specify their own class or another body with a childclass attribute is encountered along the chain of
nested bodies. Recall :ref:`CDefault`.
unless they specify their own class or another body or frame with a childclass attribute is encountered along the
chain of nested bodies and frames. Recall :ref:`CDefault`.
.. _body-mocap:
@@ -4029,6 +4029,18 @@ Associate this flexcomp with an :ref:`engine plugin<exPlugin>`. Either :at:`plug
Frames specify a coordinate transformation which is applied to all child elements. They disappear during compilation
and the transformation they encode is accumulated in their direct children. See :ref:`frame<frame>` for examples.
.. _frame-name:
:at:`name`: :at-val:`string, optional`
Name of the frame.
.. _frame-childclass:
:at:`childclass`: :at-val:`string, optional`
If this attribute is present, all descendant elements that admit a defaults class will use the class specified here,
unless they specify their own class or another frame or body with a childclass attribute is encountered along the
chain of nested bodies and frames. Recall :ref:`CDefault`.
.. _frame-pos:
:at:`pos`: :at-val:`real(3), "0 0 0"`
doc/includes/references.h
+4 -1
View File
@@ -572,7 +572,10 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_KEY, // keyframe
mjOBJ_PLUGIN, // plugin instance
mjNOBJECT // number of object types
mjNOBJECT, // number of object types
// meta elements, do not appear in mjModel
mjOBJ_FRAME = 100 // frame
} mjtObj;
typedef enum mjtConstraint_ { // type of constraint
mjCNSTR_EQUALITY = 0, // equality constraint
include/mujoco/mjmodel.h
+4 -1
View File
@@ -250,7 +250,10 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_KEY, // keyframe
mjOBJ_PLUGIN, // plugin instance
mjNOBJECT // number of object types
mjNOBJECT, // number of object types
// meta elements, do not appear in mjModel
mjOBJ_FRAME = 100 // frame
} mjtObj;
introspect/enums.py
+1
View File
@@ -267,6 +267,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjOBJ_KEY', 24),
('mjOBJ_PLUGIN', 25),
('mjNOBJECT', 26),
('mjOBJ_FRAME', 100),
]),
)),
('mjtConstraint',
src/engine/engine_io.c
+1
View File
@@ -1811,6 +1811,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
// -2: invalid objtype
static int numObjects(const mjModel* m, mjtObj objtype) {
switch (objtype) {
case mjOBJ_FRAME:
case mjOBJ_UNKNOWN:
return -1;
case mjOBJ_BODY:
src/engine/engine_util_misc.c
+3
View File
@@ -1060,6 +1060,9 @@ const char* mju_type2Str(int type) {
case mjOBJ_PLUGIN:
return "plugin";
case mjOBJ_FRAME:
return "frame";
default:
return 0;
}
src/user/user_api.h
+3 -1
View File
@@ -156,7 +156,7 @@ typedef struct _mjmPlugin { // plugin specification
typedef struct _mjmBody { // body specification
mjElement element; // internal, do not modify
mjString name; // name
mjString classname; // childclass name
mjString childclass; // childclass name
// body frame
double pos[3]; // frame position
@@ -183,6 +183,8 @@ typedef struct _mjmBody { // body specification
typedef struct _mjmFrame { // frame specification
mjElement element; // internal, do not modify
mjString name; // name
mjString childclass; // childclass name
double pos[3]; // position
double quat[4]; // orientation
mjmOrientation alt; // alternative orientation
src/user/user_model.cc
+17 -11
View File
@@ -636,6 +636,7 @@ void mjCModel::MakeLists(mjCBody* body) {
for (int i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
for (int i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
for (int i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
for (int i=0; i<body->frames.size(); i++) frames.push_back(body->frames[i]);
// recursive call to all child bodies
for (int i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
@@ -2679,18 +2680,20 @@ static void reassignid(vector<T*>& list) {
template <class T>
static void processlist(mjListKeyMap& ids, vector<T*>& list,
mjtObj type, bool checkrepeat = true) {
// loop over list elements
for (size_t i=0; i < list.size(); i++) {
// check for incompatible id setting; SHOULD NOT OCCUR
if (list[i]->id!=-1 && list[i]->id!=i) {
throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i);
// assign ids for regular elements
if (type < mjNOBJECT) {
for (size_t i=0; i < list.size(); i++) {
// check for incompatible id setting; SHOULD NOT OCCUR
if (list[i]->id!=-1 && list[i]->id!=i) {
throw mjCError(list[i], "incompatible id in %s array, position %d", mju_type2Str(type), i);
}
// id equals position in array
list[i]->id = i;
// add to ids map
ids[type][list[i]->name] = i;
}
// id equals position in array
list[i]->id = i;
// add to ids map
ids[type][list[i]->name] = i;
}
// check for repeated names
@@ -2855,6 +2858,9 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
}
// check repeated names in meta elements
processlist(ids, frames, mjOBJ_FRAME);
// delete visual assets
if (discardvisual) {
DeleteAll(materials);
src/user/user_model.h
+1
View File
@@ -250,6 +250,7 @@ class mjCModel : private mjSpec {
std::vector<mjCSite*> sites; // list of sites attached to this body
std::vector<mjCCamera*> cameras; // list of cameras
std::vector<mjCLight*> lights; // list of lights
std::vector<mjCFrame*> frames; // list of frames
// array of pointers to each object list (enumerated by type)
std::array<std::vector<mjCBase*>*, mjNOBJECT> object_lists;
src/user/user_objects.cc
+3 -1
View File
@@ -765,7 +765,7 @@ mjCBody& mjCBody::operator=(const mjCBody& other) {
void mjCBody::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.classname = (mjString)&classname;
spec.childclass = (mjString)&classname;
spec.userdata = (mjDoubleVec)&spec_userdata_;
spec.plugin.name = (mjString)&plugin_name;
spec.plugin.instance_name = (mjString)&plugin_instance_name;
@@ -1375,6 +1375,8 @@ mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
void mjCFrame::PointToLocal() {
spec.element = (mjElement)this;
spec.name = (mjString)&name;
spec.childclass = (mjString)&classname;
spec.info = (mjString)&info;
}
src/user/user_objects.h
+2
View File
@@ -321,6 +321,8 @@ class mjCFrame : public mjCFrame_, private mjmFrame {
public:
mjmFrame spec;
using mjCBase::name;
using mjCBase::classname;
using mjCBase::info;
void CopyFromSpec(void);
src/xml/xml_native_reader.cc
+22 -2
View File
@@ -3228,7 +3228,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
// get class if specified, otherwise use body
mjmDefault* def = GetClass(elem);
if (!def) {
def = (mjmDefault*)mjm_getDefault(pbody->element);
def = mjm_getDefault(frame ? frame->element : pbody->element);
}
// inertial sub-element
@@ -3335,9 +3335,29 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
// frame sub-element
else if (name=="frame") {
// read childdef
mjmDefault* childdef = 0;
if (ReadAttrTxt(elem, "childclass", text)) {
childdef = mjm_findDefault(model, text.c_str());
mjm_findDefault(model, text.c_str());
if (!childdef) {
throw mjXError(elem, "unknown default childclass");
}
}
// create frame
mjmFrame* pframe = mjm_addFrame(pbody, frame);
mjm_setString(pframe->info, ("line = " + std::to_string(elem->GetLineNum())).c_str());
mjm_setDefault(pframe->element, childdef ? childdef : def);
// read attributes
std::string name, childclass;
if (ReadAttrTxt(elem, "name", name)) {
mjm_setString(pframe->name, name.c_str());
}
if (ReadAttrTxt(elem, "childclass", childclass)) {
mjm_setString(pframe->childclass, childclass.c_str());
}
ReadAttr(elem, "pos", 3, pframe->pos, text);
ReadQuat(elem, "quat", pframe->quat, text);
ReadAlternative(elem, pframe->alt);
@@ -3368,7 +3388,7 @@ void mjXReader::Body(XMLElement* section, mjmBody* pbody, mjmFrame* frame) {
mjm_setString(pchild->name, name.c_str());
}
if (ReadAttrTxt(elem, "childclass", childclass)) {
mjm_setString(pchild->classname, childclass.c_str());
mjm_setString(pchild->childclass, childclass.c_str());
}
ReadAttr(elem, "pos", 3, pchild->pos, text);
ReadQuat(elem, "quat", pchild->quat, text);
src/xml/xml_native_writer.cc
+25 -3
View File
@@ -807,7 +807,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
Extension(root);
Custom(root);
Asset(root);
Body(InsertEnd(root, "worldbody"), model->GetWorld());
Body(InsertEnd(root, "worldbody"), model->GetWorld(), /*frame=*/nullptr);
Contact(root);
Deformable(root);
Equality(root);
@@ -1449,13 +1449,19 @@ void mjXWriter::Asset(XMLElement* root) {
// recursive body writer
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame) {
double unitq[4] = {1, 0, 0, 0};
if (!body) {
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
}
// write frame if classname is defined
if (frame) {
WriteAttrTxt(elem, "name", frame->name);
WriteAttrTxt(elem, "childclass", frame->classname);
}
// write body attributes and inertial
if (body!=model->GetWorld()) {
WriteAttrTxt(elem, "name", body->name);
@@ -1490,26 +1496,31 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
// write joints
for (int i=0; i<body->joints.size(); i++) {
if (body->joints[i]->frame != frame) continue;
OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def);
}
// write geoms
for (int i=0; i<body->geoms.size(); i++) {
if (body->geoms[i]->frame != frame) continue;
OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def);
}
// write sites
for (int i=0; i<body->sites.size(); i++) {
if (body->sites[i]->frame != frame) continue;
OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def);
}
// write cameras
for (int i=0; i<body->cameras.size(); i++) {
if (body->cameras[i]->frame != frame) continue;
OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def);
}
// write lights
for (int i=0; i<body->lights.size(); i++) {
if (body->lights[i]->frame != frame) continue;
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
}
@@ -1518,9 +1529,20 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
OnePlugin(InsertEnd(elem, "plugin"), &body->plugin);
}
// write frames
for (int i=0; i<body->frames.size(); i++) {
if (body->frames[i]->frame != frame) continue;
if (!body->frames[i]->name.empty() || !body->frames[i]->classname.empty()) {
Body(InsertEnd(elem, "frame"), body, body->frames[i]);
} else {
Body(elem, body, body->frames[i]);
}
}
// write child bodies recursively
for (int i=0; i<body->bodies.size(); i++) {
Body(InsertEnd(elem, "body"), body->bodies[i]);
if (body->bodies[i]->frame != frame) continue;
Body(InsertEnd(elem, "body"), body->bodies[i], nullptr);
}
}
src/xml/xml_native_writer.h
+17 -17
View File
@@ -40,23 +40,23 @@ class mjXWriter : public mjXBase {
mjCModel* model = 0;
// XML section writers
void Compiler(tinyxml2::XMLElement* root); // compiler section
void Option(tinyxml2::XMLElement* root); // option section
void Size(tinyxml2::XMLElement* root); // size section
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
void Contact(tinyxml2::XMLElement* root); // contact section
void Deformable(tinyxml2::XMLElement* root); // deformable section
void Equality(tinyxml2::XMLElement* root); // equality section
void Tendon(tinyxml2::XMLElement* root); // tendon section
void Actuator(tinyxml2::XMLElement* root); // actuator section
void Sensor(tinyxml2::XMLElement* root); // sensor section
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
void Compiler(tinyxml2::XMLElement* root); // compiler section
void Option(tinyxml2::XMLElement* root); // option section
void Size(tinyxml2::XMLElement* root); // size section
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, mjCFrame* frame); // body/world section
void Contact(tinyxml2::XMLElement* root); // contact section
void Deformable(tinyxml2::XMLElement* root); // deformable section
void Equality(tinyxml2::XMLElement* root); // equality section
void Tendon(tinyxml2::XMLElement* root); // tendon section
void Actuator(tinyxml2::XMLElement* root); // actuator section
void Sensor(tinyxml2::XMLElement* root); // sensor section
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
// single element writers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
test/xml/xml_native_reader_test.cc
+44 -7
View File
@@ -822,35 +822,72 @@ TEST_F(XMLReaderTest, IncludeAbsoluteTest) {
TEST_F(XMLReaderTest, ParseFrame) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="frame">
<geom size=".1"/>
</default>
<default class="body">
<geom size=".2"/>
</default>
<default class="geom">
<geom size=".3"/>
</default>
</default>
<worldbody>
<frame euler="0 0 30">
<geom size=".1" euler="0 0 20"/>
<geom size=".5" euler="0 0 20"/>
</frame>
<body>
<frame pos="0 1 0">
<geom size=".1" pos="0 1 0"/>
<body pos="1 0 0">
<geom size=".1" pos="0 0 1"/>
<frame pos="0 1 0" childclass="frame">
<geom pos="0 1 0"/>
<body pos="1 0 0" childclass="body">
<geom pos="0 0 1"/>
<geom pos="0 0 1" class="geom"/>
</body>
</frame>
</body>
<frame euler="0 0 30">
<frame euler="0 0 20">
<geom size=".1"/>
<geom size=".6"/>
</frame>
</frame>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_THAT(m->geom_size[ 0], .5);
EXPECT_THAT(m->geom_size[ 3], .6);
EXPECT_THAT(m->geom_size[ 6], .1);
EXPECT_THAT(m->geom_size[ 9], .2);
EXPECT_THAT(m->geom_size[12], .3);
mj_deleteModel(m);
}
TEST_F(XMLReaderTest, DuplicateFrameName) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<frame name="frame1" euler="0 0 30">
<geom size=".1"/>
</frame>
<frame name="frame1" euler="0 0 30">
<geom size=".1"/>
</frame>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("repeated name 'frame1'"));
}
// ----------------------- test camera parsing ---------------------------------
TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {
test/xml/xml_native_writer_test.cc
+33
View File
@@ -724,6 +724,39 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) {
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesFrameDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default class="dframe">
<geom size=".1"/>
</default>
<worldbody>
<frame name="f1" euler="0 0 30">
<geom size=".5" euler="0 0 20"/>
</frame>
<body>
<frame pos="0 1 0" name="f2" childclass="dframe">
<geom pos="0 1 0"/>
<body pos="1 0 0">
<geom pos="0 0 1"/>
</body>
</frame>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data();
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, HasSubstr("frame name=\"f1\""));
EXPECT_THAT(saved_xml, HasSubstr("frame name=\"f2\" childclass=\"dframe\""));
EXPECT_THAT(saved_xml, Not(HasSubstr("<frame>")));
mj_deleteModel(model);
}
TEST_F(XMLWriterTest, WritesDensity) {
static constexpr char xml[] = R"(
<mujoco>
unity/Runtime/Bindings/MjBindings.cs
+1
View File
@@ -302,6 +302,7 @@ public enum mjtObj : int{
mjOBJ_KEY = 24,
mjOBJ_PLUGIN = 25,
mjNOBJECT = 26,
mjOBJ_FRAME = 100,
}
public enum mjtConstraint : int{
mjCNSTR_EQUALITY = 0,