Add control values to keyframes.

- Add basic test for keyframes.
- Add missing documentation for keyframe mocap positions and quaternions.

PiperOrigin-RevId: 459021649
Change-Id: I91cf7ecbddc6262e8c72a868eeb82d627f389fb3

src/engine/engine_io.c

@@ -1016,6 +1016,7 @@ void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key) {
     mju_copy(d->act,  m->key_act+ key*m->na, m->na);
     mju_copy(d->mocap_pos,  m->key_mpos+key*3*m->nmocap, 3*m->nmocap);
     mju_copy(d->mocap_quat, m->key_mquat+key*4*m->nmocap, 4*m->nmocap);
+    mju_copy(d->ctrl, m->key_ctrl+key*m->nu, m->nu);
   }
 }
 

src/engine/engine_print.c

@@ -592,7 +592,7 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
         k = 2;
       }
 
-    // print if nozero
+    // print if nonzero
     if (k==2) {
       fprintf(fp, "key_qvel%d   ", i);
       for (int j=0; j<m->nv; j++) {
@@ -665,6 +665,24 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
       fprintf(fp, "\n");
     }
 
+    // check ctrl for nonzero
+    for (int j=0; j<m->nu; j++) {
+      if (m->key_ctrl[i*m->nu + j]) {
+        k = 6;
+        break;
+      }
+    }
+
+    // print if nonzero
+    if (k==6) {
+      fprintf(fp, "key_ctrl%d   ", i);
+      for (int j=0; j<m->nu; j++) {
+        fprintf(fp, float_format, m->key_ctrl[i*m->nu + j]);
+      }
+      fprintf(fp, "\n");
+    }
+
+
     // new line if any data was written
     if (k) {
       fprintf(fp, "\n");

src/user/user_model.cc

@@ -1935,6 +1935,8 @@ void mjCModel::CopyObjects(mjModel* m) {
     for (j=0; j<nmocap; j++) {
       mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
     }
+
+    copyvec(m->key_ctrl+i*nu, keys[i]->ctrl.data(), nu);
   }
 
   // save qpos0 in user model (to recognize changed key_qpos in write)
@@ -2878,6 +2880,9 @@ bool mjCModel::CopyBack(const mjModel* m) {
       copyvec(pk->mpos.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
       copyvec(pk->mquat.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
     }
+    if (nu) {
+      copyvec(pk->ctrl.data(), m->key_ctrl + i*nu, nu);
+    }
   }
 
   return true;

src/user/user_objects.cc

@@ -3980,6 +3980,7 @@ mjCKey::mjCKey(mjCModel* _model) {
   act.clear();
   mpos.clear();
   mquat.clear();
+  ctrl.clear();
 }
 
 
@@ -3991,6 +3992,7 @@ mjCKey::~mjCKey() {
   act.clear();
   mpos.clear();
   mquat.clear();
+  ctrl.clear();
 }
 
 
@@ -4063,4 +4065,15 @@ void mjCKey::Compile(const mjModel* m) {
   } else if (mquat.size()!=4*m->nmocap) {
     throw mjCError(this, "key %d: invalid mquat size", 0, id);
   }
+
+  // ctrl: allocate or check size
+  if (ctrl.empty()) {
+    ctrl.resize(m->nu);
+    for (i=0; i<m->nu; i++) {
+      ctrl[i] = 0;
+    }
+  } else if (ctrl.size()!=m->nu) {
+    throw mjCError(this, "key %d: invalid ctrl size", 0, id);
+  }
+
 }

src/user/user_objects.h

@@ -951,6 +951,7 @@ class mjCKey : public mjCBase {
   std::vector<double> act;        // act
   std::vector<double> mpos;       // mocap pos
   std::vector<double> mquat;      // mocap quat
+  std::vector<double> ctrl;       // ctrl
 
  private:
   mjCKey(mjCModel*);              // constructor

src/xml/xml_native_reader.cc

@@ -348,7 +348,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
 
     {"keyframe", "*", "0"},
     {"<"},
-        {"key", "*", "7", "name", "time", "qpos", "qvel", "act", "mpos", "mquat"},
+        {"key", "*", "8", "name", "time", "qpos", "qvel", "act", "mpos", "mquat", "ctrl"},
     {">"},
 {">"}
 };
@@ -2788,7 +2788,7 @@ void mjXReader::Keyframe(XMLElement* section) {
       mjuu_copyvec(pk->qvel.data(), data, n);
     }
 
-    // read qvel
+    // read act
     n = ReadAttr(elem, "act", 1000, data, text, false, false);
     if (n) {
       pk->act.resize(n);
@@ -2809,6 +2809,13 @@ void mjXReader::Keyframe(XMLElement* section) {
       mjuu_copyvec(pk->mquat.data(), data, n);
     }
 
+    // read ctrl
+    n = ReadAttr(elem, "ctrl", 1000, data, text, false, false);
+    if (n) {
+      pk->ctrl.resize(n);
+      mjuu_copyvec(pk->ctrl.data(), data, n);
+    }
+
     // advance to next element
     elem = elem->NextSiblingElement();
   }

src/xml/xml_native_writer.cc

@@ -1662,6 +1662,15 @@ void mjXWriter::Keyframe(XMLElement* root) {
       }
     }
 
+    // check ctrl and write
+    for (int j=0; j<model->nu; j++) {
+      if (pk->ctrl[j]!=0) {
+        WriteAttr(elem, "ctrl", model->nu, pk->ctrl.data());
+        change = true;
+        break;
+      }
+    }
+
     // remove elem if empty
     if (!change) {
       section->DeleteChild(elem);