diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index 9c95e7c529..eb65b6e75e 100644 --- a/doc/programming/modeledit.rst +++ b/doc/programming/modeledit.rst @@ -117,6 +117,8 @@ to :ref:`attach a body to a site`: mjSpec* parent = mj_makeSpec(); mjSpec* child = mj_makeSpec(); + parent->compiler.degree = 0; + child->compiler.degree = 1; mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), NULL); mjsSite* site = mjs_addSite(mjs_findBody(parent, "world"), NULL); mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), NULL); @@ -133,6 +135,11 @@ or :ref:`attach a frame to a body`: mjsFrame* frame = mjs_addFrame(mjs_findBody(child, "world"), NULL); mjsFrame* attached_frame = mjs_attachFrame(body, frame, "attached-", "-1"); +Note that in the above examples, the parent and child models have different values for ``compiler.degree``, +corresponding to the :ref:`compiler/angle` attribute, specifying the units in which angles are +interperted. Compiler options are carried over during attachment, so the child model will be compiled using X, while the +parent will be compiled using Y. + .. _meDefault: Default classes diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7359c1f007..57b6a7dafe 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -146,6 +146,9 @@ mjCModel::mjCModel() { defaults_.push_back(new mjCDef); defaults_.back()->name = "main"; + // point to model from spec + PointToLocal(); + // world body mjCBody* world = new mjCBody(this); mjuu_zerovec(world->pos, 3); @@ -163,14 +166,15 @@ mjCModel::mjCModel() { // create mjCBase lists from children lists CreateObjectLists(); - // point to model from spec - PointToLocal(); + // the source spec is the model itself, overwritten in the copy constructor + source_spec_ = &spec; } mjCModel::mjCModel(const mjCModel& other) { CreateObjectLists(); + source_spec_ = (mjSpec*)&other.spec; *this = other; } @@ -1222,6 +1226,25 @@ mjSpec* mjCModel::FindSpec(std::string name) const { +// find spec by mjsCompiler pointer +mjSpec* mjCModel::FindSpec(const mjsCompiler* compiler_) const { + for (auto spec : specs_) { + if (&(static_cast(spec->element)->GetSourceSpec()->compiler) == compiler_) { + return spec; + } + } + return nullptr; +} + + + +// get the spec from which this model was created +mjSpec* mjCModel::GetSourceSpec() const { + return source_spec_; +} + + + //------------------------------- COMPILER PHASES -------------------------------------------------- // make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights diff --git a/src/user/user_model.h b/src/user/user_model.h index 1b248ea243..9109018c5a 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -236,6 +236,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name mjCBase* FindTree(mjCBody* body, mjtObj type, std::string name); // find tree object given name mjSpec* FindSpec(std::string name) const; // find spec given name + mjSpec* FindSpec(const mjsCompiler* compiler_) const; // find spec given mjsCompiler void ActivatePlugin(const mjpPlugin* plugin, int slot); // activate plugin // accessors @@ -303,10 +304,16 @@ class mjCModel : public mjCModel_, private mjSpec { // map from default class name to default class pointer std::unordered_map def_map; + // get the spec from which this model was created + mjSpec* GetSourceSpec() const; + private: // settings for each defaults class std::vector defaults_; + // spec from which this model was created in copy constructor + mjSpec* source_spec_; + // list of active plugins std::vector> active_plugins_; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 9654182c59..c96872f569 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -798,7 +798,7 @@ mjCBody::mjCBody(mjCModel* _model) { mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) { model = _model; - mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); + mjSpec* origin = model->FindSpec(other.compiler); compiler = origin ? &origin->compiler : &model->spec.compiler; *this = other; CopyPlugin(); @@ -881,7 +881,7 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { } // copy input frame - mjSpec* origin = model->FindSpec(mjs_getString(other.model->spec.modelname)); + mjSpec* origin = model->FindSpec(other.compiler); frames.push_back(new mjCFrame(other)); frames.back()->body = this; frames.back()->model = model; @@ -947,7 +947,7 @@ void mjCBody::CopyList(std::vector& dst, const std::vector& src, if (pframe && !pframe->IsAncestor(src[i]->frame)) { continue; // skip if the element is not inside pframe } - mjSpec* origin = model->FindSpec(mjs_getString(src[i]->model->spec.modelname)); + mjSpec* origin = model->FindSpec(src[i]->compiler); dst.push_back(new T(*src[i])); dst.back()->body = this; dst.back()->model = model; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e641db5823..9a81a9b9a7 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -2033,52 +2033,71 @@ TEST_F(MujocoTest, ResizeParentKeyframe) { } TEST_F(MujocoTest, DifferentUnitsAllowed) { - mjSpec* child = mj_makeSpec(); - child->compiler.degree = 0; - mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0); - body->alt.type = mjORIENTATION_EULER; - body->alt.euler[0] = -mjPI / 2; - mjsGeom* geom = mjs_addGeom(body, 0); - geom->size[0] = 1; - mjsJoint* joint = mjs_addJoint(body, 0); - joint->type = mjJNT_HINGE; - joint->range[0] = -mjPI / 4; - joint->range[1] = mjPI / 4; + static constexpr char child_xml[] = R"( + + - mjSpec* parent = mj_makeSpec(); - parent->compiler.degree = 1; - mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0); - frame->alt.type = mjORIENTATION_EULER; - frame->alt.euler[0] = 90; + + + + + + + + )"; - EXPECT_THAT(mjs_attachBody(frame, body, "child-", ""), NotNull()); - mjModel* model = mj_compile(parent, 0); + static constexpr char parent_xml[] = R"( + + + + + + + + + + )"; + + std::array error; + mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size()); + mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size()); + ASSERT_THAT(spec, NotNull()) << error.data(); + mjs_attachBody(mjs_findFrame(spec, "frame"), mjs_findBody(child, "child"), + "child_", ""); + + mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->njnt, 2); + EXPECT_NEAR(model->jnt_range[0], -mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[1], mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[2], -mjPI, 1e-6); + EXPECT_NEAR(model->jnt_range[3], mjPI, 1e-6); EXPECT_NEAR(model->body_quat[4], 1, 1e-12); EXPECT_NEAR(model->body_quat[5], 0, 1e-12); EXPECT_NEAR(model->body_quat[6], 0, 1e-12); EXPECT_NEAR(model->body_quat[7], 0, 1e-12); - EXPECT_NEAR(model->jnt_range[0], -mjPI / 4, 1e-7); - EXPECT_NEAR(model->jnt_range[1], mjPI / 4, 1e-7); - mjSpec* copy = mj_copySpec(parent); - EXPECT_THAT(copy, NotNull()); - mj_deleteModel(model); + mjSpec* copied_spec = mj_copySpec(spec); + ASSERT_THAT(copied_spec, NotNull()); mj_deleteSpec(child); - mj_deleteSpec(parent); + mj_deleteSpec(spec); + mj_deleteModel(model); // check that deleting `parent` or `child` does not invalidate the copy - mjModel* copy_model = mj_compile(copy, 0); - EXPECT_THAT(copy_model, NotNull()); - EXPECT_NEAR(copy_model->body_quat[0], 1, 1e-12); - EXPECT_NEAR(copy_model->body_quat[1], 0, 1e-12); - EXPECT_NEAR(copy_model->body_quat[2], 0, 1e-12); - EXPECT_NEAR(copy_model->body_quat[3], 0, 1e-12); - EXPECT_NEAR(copy_model->jnt_range[0], -mjPI / 4, 1e-7); - EXPECT_NEAR(copy_model->jnt_range[1], mjPI / 4, 1e-7); - - mj_deleteModel(copy_model); - mj_deleteSpec(copy); + mjModel* copied_model = mj_compile(copied_spec, 0); + EXPECT_THAT(copied_model, NotNull()); + EXPECT_THAT(copied_model->njnt, 2); + EXPECT_NEAR(copied_model->jnt_range[0], -mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[1], mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[2], -mjPI, 1e-6); + EXPECT_NEAR(copied_model->jnt_range[3], mjPI, 1e-6); + EXPECT_NEAR(copied_model->body_quat[4], 1, 1e-12); + EXPECT_NEAR(copied_model->body_quat[5], 0, 1e-12); + EXPECT_NEAR(copied_model->body_quat[6], 0, 1e-12); + EXPECT_NEAR(copied_model->body_quat[7], 0, 1e-12); + + mj_deleteSpec(copied_spec); + mj_deleteModel(copied_model); } TEST_F(MujocoTest, CopyAttachedSpec) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 219ef5e116..c75091831d 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1579,6 +1579,72 @@ TEST_F(XMLReaderTest, InvalidAttach) { mj_deleteVFS(vfs.get()); } +TEST_F(XMLReaderTest, LookupCompilerOptionWithoutSpecCopy) { + static constexpr char child_xml[] = R"( + + + + + + + + + + + )"; + + static constexpr char parent_xml[] = R"( + + + + + + + + + + + + + + )"; + + auto vfs = std::make_unique(); + mj_defaultVFS(vfs.get()); + mj_addBufferVFS(vfs.get(), "child.xml", child_xml, sizeof(child_xml)); + mj_addBufferVFS(vfs.get(), "parent.xml", parent_xml, sizeof(parent_xml)); + + std::array error; + auto* spec = mj_parseXMLString(parent_xml, vfs.get(), error.data(), + error.size()); + ASSERT_THAT(spec, NotNull()) << error.data(); + + mjModel* model = mj_compile(spec, vfs.get()); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->njnt, 2); + EXPECT_NEAR(model->jnt_range[0], -3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[1], 3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[2], -3.14159, 1e-5); + EXPECT_NEAR(model->jnt_range[3], 3.14159, 1e-5); + + mjSpec* copied_spec = mj_copySpec(spec); + ASSERT_THAT(copied_spec, NotNull()); + mjModel* copied_model = mj_compile(copied_spec, vfs.get()); + EXPECT_THAT(copied_model, NotNull()); + EXPECT_THAT(copied_model->njnt, 2); + EXPECT_NEAR(copied_model->jnt_range[0], -3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[1], 3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[2], -3.14159, 1e-5); + EXPECT_NEAR(copied_model->jnt_range[3], 3.14159, 1e-5); + + mj_deleteSpec(spec); + mj_deleteSpec(copied_spec); + mj_deleteModel(model); + mj_deleteModel(copied_model); + + mj_deleteVFS(vfs.get()); +} + // ----------------------- test camera parsing --------------------------------- TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {