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