From 6654d63438f1359975fc3e861047fcd01826e378 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 8 Jan 2025 05:42:59 -0800 Subject: [PATCH] Add spec lookup in the attached spec array using the compiler option pointer. This enables to find the spec associated with the compiler option stored in the objects, which do not necessarily belong to the model that owns them. PiperOrigin-RevId: 713259419 Change-Id: I579c770147ff07ed2a5edd20e7a795ee7cc27147 --- doc/programming/modeledit.rst | 7 +++ src/user/user_model.cc | 27 +++++++++- src/user/user_model.h | 7 +++ src/user/user_objects.cc | 6 +-- test/user/user_api_test.cc | 87 ++++++++++++++++++------------ test/xml/xml_native_reader_test.cc | 66 +++++++++++++++++++++++ 6 files changed, 161 insertions(+), 39 deletions(-) diff --git a/doc/programming/modeledit.rst b/doc/programming/modeledit.rst index 9c95e7c5..eb65b6e7 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 7359c1f0..57b6a7da 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 1b248ea2..9109018c 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 9654182c..c96872f5 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 e641db58..9a81a9b9 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); + 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_deleteModel(copy_model); - mj_deleteSpec(copy); + 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 219ef5e1..c7509183 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) {