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

PiperOrigin-RevId: 602374498
Change-Id: I2d3364d68876a35799b24aca1051e2f5aebdda75

src/user/user_model.cc

@@ -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

@@ -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

@@ -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