From e143b3dbc064f37d021c6dbe40e5801171f5761f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 29 Jan 2024 06:58:30 -0800 Subject: [PATCH] Allow height-field elevation data to be specified in XML. PiperOrigin-RevId: 602374498 Change-Id: I2d3364d68876a35799b24aca1051e2f5aebdda75 --- doc/XMLreference.rst | 14 +++-- doc/XMLschema.rst | 2 +- doc/changelog.rst | 21 ++++---- src/user/user_model.cc | 19 ++++++- src/user/user_objects.cc | 11 ++++ src/user/user_objects.h | 7 +++ src/xml/xml_native_reader.cc | 31 +++++++++-- src/xml/xml_native_writer.cc | 5 ++ src/xml/xml_util.cc | 6 ++- src/xml/xml_util.h | 2 +- test/user/testdata/hfield_png.xml | 12 +++++ test/user/testdata/hfield_xml.xml | 22 ++++++++ test/user/testdata/png_hfield.xml | 9 ---- test/user/user_objects_test.cc | 2 +- test/xml/xml_native_reader_test.cc | 86 ++++++++++++++++++++++++++++-- test/xml/xml_native_writer_test.cc | 44 +++++++++++++++ 16 files changed, 258 insertions(+), 35 deletions(-) create mode 100644 test/user/testdata/hfield_png.xml create mode 100644 test/user/testdata/hfield_xml.xml delete mode 100644 test/user/testdata/png_hfield.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a1c14c64..56f37fc4 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1427,15 +1427,23 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe .. _asset-hfield-nrow: :at:`nrow`: :at-val:`int, "0"` - This attribute and the next are used to allocate a height field in mjModel and leave the elevation data undefined - (i.e., set to 0). This attribute specifies the number of rows in the elevation data matrix. The default value of 0 - means that the data will be loaded from a file, which will be used to infer the size of the matrix. + This attribute and the next are used to allocate a height field in mjModel. If the :at:`elevation` attribute is not + set, the elevation data is set to 0. This attribute specifies the number of rows in the elevation data matrix. The + default value of 0 means that the data will be loaded from a file, which will be used to infer the size of the + matrix. .. _asset-hfield-ncol: :at:`ncol`: :at-val:`int, "0"` This attribute specifies the number of columns in the elevation data matrix. +.. _asset-hfield-elevation: + +:at:`elevation`: :at-val:`real(nrow*ncol), optional` + This attribute specifies the elevation data matrix. Values are automatically normalized to lie between 0 and 1 by + first subtracting the minimum value and then dividing by the (maximum-minimum) difference, if not 0. If not provided, + values are set to 0. + .. _asset-hfield-size: :at:`size`: :at-val:`real(4), required` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index b319f26b..143fc733 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -157,7 +157,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`content_type` | :ref:`file` | :ref:`nrow` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ncol` | :ref:`size` | | | | +| | | | :ref:`ncol` | :ref:`size` | :ref:`elevation` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| asset |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 2b1befd2..16ffb157 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,19 +19,22 @@ General - Added color of :ref:`bounding volumes` and :ref:`active bounding volumes` to :ref:`visual/rgba`. +4. Height-field elevation data can now be specified directly in XML with the :ref:`elevation` + attribute (and not only with PNG files). + See `example model `__. MJX ^^^ -4. Added :ref:`dyntype` ``filterexact``. -5. Added :at:`site` transmission. -6. Updated MJX colab tutorial with more stable quadruped environment. -7. Added ``mjx.ray`` which mirrors :ref:`mj_ray` for planes, spheres, capsules, boxes, and meshes. -8. Added ``mjx.is_sparse`` which mirrors :ref:`mj_isSparse` and ``mjx.full_m`` which mirrors :ref:`mj_fullM`. -9. Added support for specifying sparse or dense mass matrices via :ref:`option-jacobian`. +5. Added :ref:`dyntype` ``filterexact``. +6. Added :at:`site` transmission. +7. Updated MJX colab tutorial with more stable quadruped environment. +8. Added ``mjx.ray`` which mirrors :ref:`mj_ray` for planes, spheres, capsules, boxes, and meshes. +9. Added ``mjx.is_sparse`` which mirrors :ref:`mj_isSparse` and ``mjx.full_m`` which mirrors :ref:`mj_fullM`. +10. Added support for specifying sparse or dense mass matrices via :ref:`option-jacobian`. Python bindings ^^^^^^^^^^^^^^^ -10. Improved the implmentation of the :ref:`rollout` module. Note the changes below are breaking, dependent +11. Improved the implmentation of the :ref:`rollout` module. Note the changes below are breaking, dependent code will require modification. - Uses :ref:`mjSTATE_FULLPHYSICS` as state spec, enabling divergence detection by inspecting time. @@ -40,9 +43,9 @@ Python bindings Bug fixes ^^^^^^^^^ -11. Fixed a bug that prevented the use of pins with plugins if flexes are not in the worldbody. Fixes +12. Fixed a bug that prevented the use of pins with plugins if flexes are not in the worldbody. Fixes :github:issue:`1270`. -12. Fixed a bug in the :ref:`muscle model` that led to non-zero values outside the lower +13. Fixed a bug in the :ref:`muscle model` that led to non-zero values outside the lower bound of the length range. Fixes :github:issue:`1342`. diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a3407b8e..95f671a6 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3376,11 +3376,28 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCMesh* pm; for (int i=0; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); } + // heightfield + mjCHField* phf; + for (int i=0; iuserdata().size(); + if (size) { + int nrow = m->hfield_nrow[i]; + int ncol = m->hfield_ncol[i]; + float* userdata = phf->userdata().data(); + float* modeldata = m->hfield_data + m->hfield_adr[i]; + // copy back in reverse row order + for (int j=0; jsize, m->site_size + 3 * i, 3); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4797363d..6c30e7d2 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -20,9 +20,11 @@ #include #include #include +#include #include #include #include +#include #include #include "lodepng.h" @@ -2233,6 +2235,7 @@ mjCHField::mjCHField(mjCModel* _model) { nrow = 0; ncol = 0; data = 0; + userdata_.clear(); } @@ -2335,6 +2338,14 @@ void mjCHField::LoadPNG(mjResource* resource) { +// user data setter +void mjCHField::set_userdata(std::optional>&& userdata) { + if (userdata.has_value()) { + userdata_ = std::move(userdata.value()); + } +} + + // compiler void mjCHField::Compile(const mjVFS* vfs) { // check size parameters diff --git a/src/user/user_objects.h b/src/user/user_objects.h index c4839fee..7b5fb265 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -843,9 +843,16 @@ class mjCHField : public mjCBase { int ncol; // number of columns float* data; // elevation data, row-major format + // getter for user data + std::vector& userdata() { return userdata_; } + + // setter for user data + void set_userdata(std::optional>&& userdata); + private: mjCHField(mjCModel* model); // constructor ~mjCHField(); // destructor + std::vector userdata_; // user-provided elevation data void Compile(const mjVFS* vfs); // compiler void LoadCustom(mjResource* resource); // load from custom format diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index aeb074fb..79cfdbf5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -217,7 +217,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "fileright", "fileleft", "fileup", "filedown", "filefront", "fileback", "builtin", "rgb1", "rgb2", "mark", "markrgb", "random", "width", "height", "hflip", "vflip"}, - {"hfield", "*", "6", "name", "content_type", "file", "nrow", "ncol", "size"}, + {"hfield", "*", "7", "name", "content_type", "file", "nrow", "ncol", "size", "elevation"}, {"mesh", "*", "12", "name", "class", "content_type", "file", "vertex", "normal", "texcoord", "face", "refpos", "refquat", "scale", "smoothnormal"}, {"<"}, @@ -2931,10 +2931,33 @@ void mjXReader::Asset(XMLElement* section) { ReadAttrInt(elem, "ncol", &phf->ncol); ReadAttr(elem, "size", 4, phf->size, text, true); - // allocate buffer for dynamic hfield + // allocate buffer for dynamic hfield, copy user data if given if (phf->file.empty() && phf->nrow>0 && phf->ncol>0) { - phf->data = (float*) mju_malloc(phf->nrow*phf->ncol*sizeof(float)); - memset(phf->data, 0, phf->nrow*phf->ncol*sizeof(float)); + int nrow = phf->nrow; + int ncol = phf->ncol; + phf->data = (float*) mju_malloc(nrow*ncol*sizeof(float)); + + // read user data + phf->set_userdata(ReadAttrVec(elem, "elevation")); + + // user data given, copy into data + if (!phf->userdata().empty()) { + if (phf->userdata().size() != nrow*ncol) { + throw mjXError(elem, "elevation data length must match nrow*ncol"); + } + + // copy in reverse row order, so XML string is top-to-bottom + const float* userdata = phf->userdata().data(); + for (int i = 0; i < nrow; i++) { + int flip = nrow-1-i; + memcpy(phf->data + flip*ncol, userdata + i*ncol, ncol*sizeof(float)); + } + } + + // user data not given, set to 0 + else { + memset(phf->data, 0, phf->nrow*phf->ncol*sizeof(float)); + } } } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 88bc6fc6..da41f82c 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1449,6 +1449,11 @@ void mjXWriter::Asset(XMLElement* root) { } else { WriteAttrInt(elem, "nrow", phf->nrow); WriteAttrInt(elem, "ncol", phf->ncol); + if (!phf->userdata().empty()) { + string text; + Vector2String(text, phf->userdata(), phf->ncol); + WriteAttrTxt(elem, "elevation", text); + } } } } diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index d7a09afc..2d68ab6e 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -875,11 +875,13 @@ void mjXUtil::Vector2String(string& txt, const vector& vec) { // write vector to string -void mjXUtil::Vector2String(string& txt, const vector& vec) { +void mjXUtil::Vector2String(string& txt, const vector& vec, int ncol) { stringstream strm; for (size_t i=0; i0) { + if (ncol && (i % ncol) == 0) { + strm << "\n "; + } else if (i>0) { strm << " "; } strm << vec[i]; diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index acf5d288..af4f5405 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -179,7 +179,7 @@ class mjXUtil { static void Vector2String(std::string& txt, const std::vector& vec); // write vector to string - static void Vector2String(std::string& txt, const std::vector& vec); + static void Vector2String(std::string& txt, const std::vector& vec, int ncol = 0); // write vector to string static void Vector2String(std::string& txt, const std::vector& vec); diff --git a/test/user/testdata/hfield_png.xml b/test/user/testdata/hfield_png.xml new file mode 100644 index 00000000..f72333de --- /dev/null +++ b/test/user/testdata/hfield_png.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/test/user/testdata/hfield_xml.xml b/test/user/testdata/hfield_xml.xml new file mode 100644 index 00000000..3b64954b --- /dev/null +++ b/test/user/testdata/hfield_xml.xml @@ -0,0 +1,22 @@ + + + + + + + + + + diff --git a/test/user/testdata/png_hfield.xml b/test/user/testdata/png_hfield.xml deleted file mode 100644 index a8fec865..00000000 --- a/test/user/testdata/png_hfield.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index a5c2be05..ad9220c0 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -694,7 +694,7 @@ using MjCHFieldTest = MujocoTest; TEST_F(MjCHFieldTest, PngMap) { const std::string xml_path = - GetTestDataFilePath("user/testdata/png_hfield.xml"); + GetTestDataFilePath("user/testdata/hfield_png.xml"); std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size()); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 21c11a92..ef037688 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -36,6 +36,7 @@ using ::testing::HasSubstr; using ::testing::IsNan; using ::testing::IsNull; using ::testing::NotNull; +using ::testing::FloatEq; using XMLReaderTest = MujocoTest; @@ -436,7 +437,7 @@ TEST_F(XMLReaderTest, ParseFrame) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + EXPECT_THAT(m, NotNull()) << error.data(); mj_deleteModel(m); } @@ -455,7 +456,7 @@ TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(m, IsNull()); EXPECT_THAT(error.data(), HasSubstr("either 'fovy' or 'sensorsize'")); } @@ -472,7 +473,7 @@ TEST_F(XMLReaderTest, CameraPricipalRequiresSensorsize) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(m, IsNull()); EXPECT_THAT(error.data(), HasSubstr("attribute missing: 'sensorsize'")); } @@ -489,7 +490,7 @@ TEST_F(XMLReaderTest, CameraSensorsizeRequiresResolution) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::IsNull()); + EXPECT_THAT(m, IsNull()); EXPECT_THAT(error.data(), HasSubstr("attribute missing: 'resolution'")); } @@ -585,6 +586,83 @@ TEST_F(XMLReaderTest, InvalidSkinGroup) { mj_deleteModel(model); } +// ------------- test height-field parsing ------------------------------------- + +using HfieldParsingTest = MujocoTest; + +TEST_F(HfieldParsingTest, NoData) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->hfield_nrow[0], 4); + EXPECT_EQ(model->hfield_ncol[0], 3); + EXPECT_EQ(model->hfield_size[0], 0.5); + EXPECT_EQ(model->hfield_size[1], 0.5); + EXPECT_EQ(model->hfield_size[2], 1); + EXPECT_EQ(model->hfield_size[3], 0.1); + mj_deleteModel(model); +} + +TEST_F(HfieldParsingTest, HasDataBadSize) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("data length must match nrow*ncol")); +} + +TEST_F(HfieldParsingTest, HasData) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->hfield_nrow[0], 3); + EXPECT_EQ(model->hfield_ncol[0], 2); + EXPECT_EQ(model->hfield_size[0], 0.5); + EXPECT_EQ(model->hfield_size[1], 0.5); + EXPECT_EQ(model->hfield_size[2], 1); + EXPECT_EQ(model->hfield_size[3], 0.1); + + // offset (minimum) and scaling (maximum) from normalizing operation + float offset = 1.0; + float scale = 6.0 - offset; + + // compare data, note: reverse row order + EXPECT_THAT(model->hfield_data[0], FloatEq((5-offset)/scale)); + EXPECT_THAT(model->hfield_data[1], FloatEq((6-offset)/scale)); + EXPECT_THAT(model->hfield_data[2], FloatEq((3-offset)/scale)); + EXPECT_THAT(model->hfield_data[3], FloatEq((4-offset)/scale)); + EXPECT_THAT(model->hfield_data[4], FloatEq((1-offset)/scale)); + EXPECT_THAT(model->hfield_data[5], FloatEq((2-offset)/scale)); + mj_deleteModel(model); +} + + // ------------- test relative frame sensor parsing ---------------------------- using RelativeFrameSensorParsingTest = MujocoTest; diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 4b5b7f9e..378c1b8b 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -43,6 +43,7 @@ namespace { using ::testing::HasSubstr; using ::testing::Not; using ::testing::NotNull; +using ::testing::FloatEq; using XMLWriterTest = PluginTest; @@ -828,6 +829,49 @@ TEST_F(XMLWriterTest, WritesSkin) { mj_deleteModel(mtemp); } +TEST_F(XMLWriterTest, WritesHfield) { + static constexpr char xml[] = R"( + + + + + + )"; + // load model + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + int size = model->hfield_nrow[0]*model->hfield_ncol[0]; + EXPECT_EQ(size, 6); + + // save and read, compare data + mjModel* mtemp = LoadModelFromString(SaveAndReadXml(model)); + ASSERT_THAT(mtemp, NotNull()); + for (int i = 0; i < size; ++i) { + EXPECT_THAT(mtemp->hfield_data[i], FloatEq(model->hfield_data[i])); + } + mj_deleteModel(mtemp); + + // modify data, save read and compare + model->hfield_data[0] = 0.25; + model->hfield_data[1] = 0.0; + model->hfield_data[2] = 0.5; + model->hfield_data[3] = 0.0; + model->hfield_data[4] = 1.0; + model->hfield_data[5] = 0.0; + + mtemp = LoadModelFromString(SaveAndReadXml(model)); + ASSERT_THAT(mtemp, NotNull()); + for (int i = 0; i < size; ++i) { + EXPECT_THAT(mtemp->hfield_data[i], FloatEq(model->hfield_data[i])); + } + mj_deleteModel(mtemp); + + mj_deleteModel(model); +} + TEST_F(XMLWriterTest, SpringlengthOneValue) { static constexpr char xml[] = R"(