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

Allow height-field elevation data to be specified in XML.

Browse Source 
PiperOrigin-RevId: 602374498
Change-Id: I2d3364d68876a35799b24aca1051e2f5aebdda75
This commit is contained in:
Yuval Tassa
2024-01-29 06:58:30 -08:00
committed by Copybara-Service
parent e6847f843d
commit e143b3dbc0
16 changed files with 258 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+11 -3
View File
@@ -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`
doc/XMLschema.rst
+1 -1
View File
@@ -157,7 +157,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<asset-hfield-name>` | :ref:`content_type<asset-hfield-content_type>` | :ref:`file<asset-hfield-file>` | :ref:`nrow<asset-hfield-nrow>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ncol<asset-hfield-ncol>` | :ref:`size<asset-hfield-size>` | | | |
| | | | :ref:`ncol<asset-hfield-ncol>` | :ref:`size<asset-hfield-size>` | :ref:`elevation<asset-hfield-elevation>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| asset |br| |_| |L| | | .. table:: |
doc/changelog.rst
+12 -9
View File
@@ -19,19 +19,22 @@ General
- Added color of :ref:`bounding volumes<visual-rgba-bv>` and :ref:`active bounding volumes<visual-rgba-bvactive>`
to :ref:`visual/rgba<visual-rgba>`.
4. Height-field elevation data can now be specified directly in XML with the :ref:`elevation<asset-hfield-elevation>`
attribute (and not only with PNG files).
See `example model <https://github.com/google-deepmind/mujoco/blob/main/test/user/testdata/hfield_xml.xml>`__.
MJX
^^^
4. Added :ref:`dyntype<actuator-general-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<actuator-general-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<PySample>` module. Note the changes below are breaking, dependent
11. Improved the implmentation of the :ref:`rollout<PySample>` module. Note the changes below are breaking, dependent
code will require modification.
- Uses :ref:`mjSTATE_FULLPHYSICS<geFullPhysics>` 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<CMuscle>` that led to non-zero values outside the lower
13. Fixed a bug in the :ref:`muscle model<CMuscle>` that led to non-zero values outside the lower
bound of the length range. Fixes :github:issue:`1342`.
src/user/user_model.cc
+18 -1
View File
@@ -3376,11 +3376,28 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCMesh* pm;
for (int i=0; i<nmesh; i++) {
pm = meshes[i];
copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
}
// heightfield
mjCHField* phf;
for (int i=0; i<nhfield; i++) {
phf = hfields[i];
int size = phf->userdata().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; j<nrow; j++) {
int flip = nrow-1-j;
copyvec(userdata + flip*ncol, modeldata+j*ncol, ncol);
}
}
}
// sites
for (int i=0; i<nsite; i++) {
copyvec(sites[i]->size, m->site_size + 3 * i, 3);
src/user/user_objects.cc
+11
View File
@@ -20,9 +20,11 @@
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <optional>
#include <sstream>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#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<std::vector<float>>&& userdata) {
if (userdata.has_value()) {
userdata_ = std::move(userdata.value());
}
}
// compiler
void mjCHField::Compile(const mjVFS* vfs) {
// check size parameters
src/user/user_objects.h
+7
View File
@@ -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<float>& userdata() { return userdata_; }
// setter for user data
void set_userdata(std::optional<std::vector<float>>&& userdata);
private:
mjCHField(mjCModel* model); // constructor
~mjCHField(); // destructor
std::vector<float> userdata_; // user-provided elevation data
void Compile(const mjVFS* vfs); // compiler
void LoadCustom(mjResource* resource); // load from custom format
src/xml/xml_native_reader.cc
+27 -4
View File
@@ -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<float>(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));
}
}
}
src/xml/xml_native_writer.cc
+5
View File
@@ -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);
}
}
}
}
src/xml/xml_util.cc
+4 -2
View File
@@ -875,11 +875,13 @@ void mjXUtil::Vector2String(string& txt, const vector<double>& vec) {
// write vector<float> to string
void mjXUtil::Vector2String(string& txt, const vector<float>& vec) {
void mjXUtil::Vector2String(string& txt, const vector<float>& vec, int ncol) {
stringstream strm;
for (size_t i=0; i<vec.size(); i++) {
if (i>0) {
if (ncol && (i % ncol) == 0) {
strm << "\n ";
} else if (i>0) {
strm << " ";
}
strm << vec[i];
src/xml/xml_util.h
+1 -1
View File
@@ -179,7 +179,7 @@ class mjXUtil {
static void Vector2String(std::string& txt, const std::vector<double>& vec);
// write vector<float> to string
static void Vector2String(std::string& txt, const std::vector<float>& vec);
static void Vector2String(std::string& txt, const std::vector<float>& vec, int ncol = 0);
// write vector<int> to string
static void Vector2String(std::string& txt, const std::vector<int>& vec);
test/user/testdata/hfield_png.xml
Vendored
+12
View File
@@ -0,0 +1,12 @@
<mujoco>
<asset>
<hfield name="hfield" file="hfield.png" size="0.5 0.5 0.05 0.1"/>
<texture name="hfield" type="2d" file="hfield.png"/>
<material name="hfield" texture="hfield"/>
</asset>
<worldbody>
<light pos="-.4 .6 1"/>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05" material="hfield"/>
</worldbody>
</mujoco>
test/user/testdata/hfield_xml.xml
Vendored
+22
View File
@@ -0,0 +1,22 @@
<mujoco>
<asset>
<hfield name="J" size="0.9 1.1 .2 .1" nrow="12" ncol="10" elevation="
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 1 1 0 0
0 0 1 1 0 0 1 1 0 0
0 0 1 1 1 1 1 1 0 0
0 0 1 1 1 1 1 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0"/>
</asset>
<worldbody>
<light pos="0 0 1"/>
<geom type="hfield" hfield="J"/>
</worldbody>
</mujoco>
test/user/testdata/png_hfield.xml
Vendored
-9
View File
@@ -1,9 +0,0 @@
<mujoco>
<asset>
<hfield name="hfield" file="hfield.png" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</worldbody>
</mujoco>
test/user/user_objects_test.cc
+1 -1
View File
@@ -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<char, 1024> error;
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
test/xml/xml_native_reader_test.cc
+82 -4
View File
@@ -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<char, 1024> 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<char, 1024> 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<char, 1024> 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<char, 1024> 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"(
<mujoco>
<asset>
<hfield name="hf" nrow="4" ncol="3" size="0.5 0.5 1 0.1"/>
</asset>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<asset>
<hfield name="hf" nrow="3" ncol="2" size="0.5 0.5 1 0.1"
elevation="1 2
3 4
5 6 7"/>
</asset>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<asset>
<hfield name="hf" nrow="3" ncol="2" size="0.5 0.5 1 0.1"
elevation="1 2
3 4
5 6"/>
</asset>
</mujoco>
)";
std::array<char, 1024> 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;
test/xml/xml_native_writer_test.cc
+44
View File
@@ -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"(
<mujoco>
<asset>
<hfield name="hf" nrow="3" ncol="2" size="0.5 0.5 1 0.1"
elevation="1 2
3 4
5 6"/>
</asset>
</mujoco>
)";
// 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"(
<mujoco>