diff --git a/doc/APIreference.rst b/doc/APIreference.rst index f64fa973..7ff6c389 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -1725,6 +1725,7 @@ mjModel mjtNum* key_act; // key activation (nkey x na) mjtNum* key_mpos; // key mocap position (nkey x 3*nmocap) mjtNum* key_mquat; // key mocap quaternion (nkey x 4*nmocap) + mjtNum* key_ctrl; // key control (nkey x nu) // names int* name_bodyadr; // body name pointers (nbody x 1) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b06a0028..9b28fac8 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1411,7 +1411,9 @@ if present, the parser ignores it. The symbols in the second column of the table | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`name` | :at:`time` | :at:`qpos` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`qvel` | :at:`act` | | | +| | | | :at:`qvel` | :at:`act` | :at:`ctrl` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`mpos` | :at:`mquat` | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ @@ -5194,3 +5196,9 @@ This element sets the data for one of the keyframes. They are set in the order i Vector of joint velocities, copied into mjData.qvel when the simulation state is set to this keyframe. :at:`act`: :at-val:`real(mjModel.na), "0 0 ..."` Vector of actuator activations, copied into mjData.act when the simulation state is set to this keyframe. +:at:`ctrl`: :at-val:`real(mjModel.nu), "0 0 ..."` + Vector of controls, copied into mjData.ctrl when the simulation state is set to this keyframe. +:at:`mpos`: :at-val:`real(3*mjModel.nmocap), default = mjModel.body_pos` + Vector of mocap body positions, copied into mjData.mocap_pos when the simulation state is set to this keyframe. +:at:`mquat`: :at-val:`real(4*mjModel.nmocap), default = mjModel.body_quat` + Vector of mocap body quaternions, copied into mjData.mocap_quat when the simulation state is set to this keyframe. diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index b93fdd57..43086516 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -888,6 +888,7 @@ struct mjModel_ { mjtNum* key_act; // key activation (nkey x na) mjtNum* key_mpos; // key mocap position (nkey x 3*nmocap) mjtNum* key_mquat; // key mocap quaternion (nkey x 4*nmocap) + mjtNum* key_ctrl; // key control (nkey x nu) // names int* name_bodyadr; // body name pointers (nbody x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index b275573d..cec1ee7b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -135,6 +135,7 @@ int nq = m->nq; \ int nv = m->nv; \ int na = m->na; \ + int nu = m->nu; \ int nmocap = m->nmocap; @@ -395,6 +396,7 @@ X( mjtNum, key_act, nkey, MJ_M(na) ) \ X( mjtNum, key_mpos, nkey, MJ_M(nmocap)*3 ) \ X( mjtNum, key_mquat, nkey, MJ_M(nmocap)*4 ) \ + X( mjtNum, key_ctrl, nkey, MJ_M(nu) ) \ X( int, name_bodyadr, nbody, 1 ) \ X( int, name_jntadr, njnt, 1 ) \ X( int, name_geomadr, ngeom, 1 ) \ diff --git a/sample/simulate.cc b/sample/simulate.cc index 633582b6..ac2e308b 100644 --- a/sample/simulate.cc +++ b/sample/simulate.cc @@ -1442,6 +1442,7 @@ void uiEvent(mjuiState* state) { mju_copy(d->act, m->key_act+i*m->na, m->na); mju_copy(d->mocap_pos, m->key_mpos+i*3*m->nmocap, 3*m->nmocap); mju_copy(d->mocap_quat, m->key_mquat+i*4*m->nmocap, 4*m->nmocap); + mju_copy(d->ctrl, m->key_ctrl+i*m->nu, m->nu); mj_forward(m, d); profilerupdate(); sensorupdate(); @@ -1456,6 +1457,7 @@ void uiEvent(mjuiState* state) { mju_copy(m->key_act+i*m->na, d->act, m->na); mju_copy(m->key_mpos+i*3*m->nmocap, d->mocap_pos, 3*m->nmocap); mju_copy(m->key_mquat+i*4*m->nmocap, d->mocap_quat, 4*m->nmocap); + mju_copy(m->key_ctrl+i*m->nu, d->ctrl, m->nu); break; } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index e40a7a45..9c407b38 100644 --- a/src/engine/engine_io.c +++ b/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); } } diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index accaf7b2..b33222f5 100644 --- a/src/engine/engine_print.c +++ b/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; jnv; 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; jnu; 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; jnu; 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"); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 6611b362..73b928cb 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1935,6 +1935,8 @@ void mjCModel::CopyObjects(mjModel* m) { for (j=0; jkey_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; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index b7cc39ba..c484a92f 100644 --- a/src/user/user_objects.cc +++ b/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; inu; i++) { + ctrl[i] = 0; + } + } else if (ctrl.size()!=m->nu) { + throw mjCError(this, "key %d: invalid ctrl size", 0, id); + } + } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9a3dd6d6..383a391b 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -951,6 +951,7 @@ class mjCKey : public mjCBase { std::vector act; // act std::vector mpos; // mocap pos std::vector mquat; // mocap quat + std::vector ctrl; // ctrl private: mjCKey(mjCModel*); // constructor diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 83f3cce0..9bcce884 100644 --- a/src/xml/xml_native_reader.cc +++ b/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(); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 6ffb3230..1a41fb3f 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1662,6 +1662,15 @@ void mjXWriter::Keyframe(XMLElement* root) { } } + // check ctrl and write + for (int j=0; jnu; 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); diff --git a/test/user/testdata/keyframe.xml b/test/user/testdata/keyframe.xml new file mode 100644 index 00000000..e6d2e583 --- /dev/null +++ b/test/user/testdata/keyframe.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index d7e68e53..5fa9224d 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -28,9 +28,8 @@ namespace mujoco { namespace { -static std::vector GetRow(const mjtNum* array, int ncolumn, int row) { - return std::vector(array + ncolumn * row, - array + ncolumn * (row + 1)); +std::vector AsVector(const mjtNum* array, int n) { + return std::vector(array, array + n); } using ::testing::ElementsAre; @@ -38,6 +37,59 @@ using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; +// ------------------------ test keyframes ------------------------------------- + +static const char* const kKeyframePath = "user/testdata/keyframe.xml"; + +TEST_F(MujocoTest, KeyFrameTest) { + const std::string xml_path = GetTestDataFilePath(kKeyframePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + ASSERT_THAT(model, NotNull()); + EXPECT_EQ(model->nkey, 7); + EXPECT_EQ(model->key_time[0 * 1], 0.1); + EXPECT_EQ(model->key_qpos[1 * model->nq], 0.2); + EXPECT_EQ(model->key_qvel[2 * model->nv], 0.3); + EXPECT_EQ(model->key_act[3 * model->na], 0.4); + EXPECT_THAT(AsVector(model->key_ctrl + 4*model->nu, model->nu), + ElementsAre(0.5, 0.6)); + EXPECT_THAT(AsVector(model->key_mpos + 3*model->nmocap*5, 3), + ElementsAre(.1, .2, .3)); + EXPECT_THAT(AsVector(model->key_mquat + 4*model->nmocap*6, 4), + ElementsAre(.5, .5, .5, .5)); + mj_deleteModel(model); +} + +TEST_F(MujocoTest, ResetDataKeyframeTest) { + const std::string xml_path = GetTestDataFilePath(kKeyframePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + mj_resetDataKeyframe(model, data, 0); + EXPECT_EQ(data->time, 0.1); + + mj_resetDataKeyframe(model, data, 1); + EXPECT_EQ(data->qpos[0], 0.2); + + mj_resetDataKeyframe(model, data, 2); + EXPECT_EQ(data->qvel[0], 0.3); + + mj_resetDataKeyframe(model, data, 3); + EXPECT_EQ(data->act[0], 0.4); + + mj_resetDataKeyframe(model, data, 4); + EXPECT_EQ(data->ctrl[0], 0.5); + EXPECT_EQ(data->ctrl[1], 0.6); + + mj_resetDataKeyframe(model, data, 5); + EXPECT_THAT(AsVector(data->mocap_pos, 3), ElementsAre(.1, .2, .3)); + + mj_resetDataKeyframe(model, data, 6); + EXPECT_THAT(AsVector(data->mocap_quat, 4), ElementsAre(.5, .5, .5, .5)); + + mj_deleteData(data); + mj_deleteModel(model); +} // ------------- test relative frame sensor compilation------------------------- @@ -214,10 +266,10 @@ TEST_F(QuatNorm, QuatNotNormalized) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(GetRow(m->body_quat, 4, 1), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(GetRow(m->geom_quat, 4, 0), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(GetRow(m->site_quat, 4, 0), ElementsAre(1./5, 2./5, 2./5, 4./5)); - EXPECT_THAT(GetRow(m->cam_quat, 4, 0), ElementsAre(1./5, 2./5, 2./5, 4./5)); + EXPECT_THAT(AsVector(m->body_quat+4, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); + EXPECT_THAT(AsVector(m->geom_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); + EXPECT_THAT(AsVector(m->site_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); + EXPECT_THAT(AsVector(m->cam_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5)); mj_deleteModel(m); } diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index ad7047dd..f66597e8 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -2147,6 +2147,7 @@ public unsafe struct mjModel_ { public double* key_act; public double* key_mpos; public double* key_mquat; + public double* key_ctrl; public int* name_bodyadr; public int* name_jntadr; public int* name_geomadr;